Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include "std.h"
31 #include "led.h"
32 #include "state.h"
34 #include "generated/airframe.h"
35 #include CTRL_TYPE_H
37 
38 /* outer loop parameters */
39 float h_ctl_course_setpoint; /* rad, CW/north */
45 
46 /* roll and pitch disabling */
48 
49 /* AUTO1 rate mode */
51 
52 
53 /* inner roll loop parameters */
58 
59 /* inner pitch loop parameters */
65 
66 /* inner yaw loop parameters */
67 #if H_CTL_YAW_LOOP
68 float h_ctl_yaw_rate_setpoint;
69 pprz_t h_ctl_rudder_setpoint;
70 #endif
71 
72 /* inner CL loop parameters */
73 #if H_CTL_CL_LOOP
74 pprz_t h_ctl_flaps_setpoint;
75 #endif
76 
77 #ifdef USE_AOA
78 uint8_t h_ctl_pitch_mode;
79 #endif
80 
81 /* inner loop pre-command */
84 
85 /* rate loop */
86 #ifdef H_CTL_RATE_LOOP
87 float h_ctl_roll_rate_setpoint;
88 float h_ctl_roll_rate_mode;
89 float h_ctl_roll_rate_setpoint_pgain;
90 float h_ctl_hi_throttle_roll_rate_pgain;
91 float h_ctl_lo_throttle_roll_rate_pgain;
92 float h_ctl_roll_rate_igain;
93 float h_ctl_roll_rate_dgain;
94 #endif
95 
96 #ifdef H_CTL_COURSE_SLEW_INCREMENT
97 float h_ctl_course_slew_increment;
98 #endif
99 
100 
101 inline static void h_ctl_roll_loop(void);
102 inline static void h_ctl_pitch_loop(void);
103 #ifdef H_CTL_RATE_LOOP
104 static inline void h_ctl_roll_rate_loop(void);
105 #endif
106 
107 #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
108 #define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
109 #endif
110 
111 #ifndef H_CTL_COURSE_DGAIN
112 #define H_CTL_COURSE_DGAIN 0.
113 #endif
114 
115 #ifndef H_CTL_ROLL_RATE_GAIN
116 #define H_CTL_ROLL_RATE_GAIN 0.
117 #endif
118 
121 
122 #ifdef AGR_CLIMB
123 static float nav_ratio;
124 #endif
125 
126 #if PERIODIC_TELEMETRY
128 
129 static void send_calibration(struct transport_tx *trans, struct link_device *dev)
130 {
131  pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode);
132 }
133 #endif
134 
135 void h_ctl_init(void)
136 {
140  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
142  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
143 
144 #ifdef USE_AOA
145  h_ctl_pitch_mode = 0;
146 #endif
147 
149 
150  h_ctl_roll_setpoint = 0.;
151 #ifdef H_CTL_ROLL_PGAIN
152  h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
153 #endif
155 #ifdef H_CTL_AILERON_OF_THROTTLE
156  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
157 #endif
158 
161  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
164  h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
165 
166 #ifdef H_CTL_RATE_LOOP
167  h_ctl_roll_rate_mode = H_CTL_ROLL_RATE_MODE_DEFAULT;
168  h_ctl_roll_rate_setpoint_pgain = H_CTL_ROLL_RATE_SETPOINT_PGAIN;
169  h_ctl_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
170  h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
171  h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
172  h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
173 #endif
174 
175 #ifdef H_CTL_ROLL_SLEW
176  h_ctl_roll_slew = H_CTL_ROLL_SLEW;
177 #endif
178 
179 #ifdef H_CTL_COURSE_SLEW_INCREMENT
180  h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
181 #endif
182 
183 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
184  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
186 #endif
187 
188 #ifdef AGR_CLIMB
189  nav_ratio = 0;
190 #endif
191 
192 #if PERIODIC_TELEMETRY
193  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CALIBRATION, send_calibration);
194 #endif
195 }
196 
202 {
203  static float last_err;
204 
205  // Ground path error
207  NormRadAngle(err);
208 
209 #ifdef STRONG_WIND
210  // Usefull path speed
211  const float reference_advance = (NOMINAL_AIRSPEED / 2.);
212  float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;
213 
214  if (
215  (advance < 1.) && // Path speed is small
216  (stateGetHorizontalSpeedNorm_f() < reference_advance) // Small path speed is due to wind (small groundspeed)
217  ) {
218  /*
219  // rough crabangle approximation
220  float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
221  float wind_dir = atan2(wind_east,wind_north);
222 
223  float wind_course = h_ctl_course_setpoint - wind_dir;
224  NormRadAngle(wind_course);
225 
226  estimator_hspeed_dir = estimator_psi;
227 
228  float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
229  //crab = estimator_hspeed_mod - estimator_psi;
230  NormRadAngle(crab);
231  */
232 
233  // Heading error
234  float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
235  NormRadAngle(herr);
236 
237  if (advance < -0.5) { //<! moving in the wrong direction / big > 90 degree turn
238  err = herr;
239  } else if (advance < 0.) { //<!
240  err = (-advance) * 2. * herr;
241  } else {
242  err = advance * err;
243  }
244 
245  // Reset differentiator when switching mode
246  //if (h_ctl_course_heading_mode == 0)
247  // last_err = err;
248  //h_ctl_course_heading_mode = 1;
249  }
250  /* else
251  {
252  // Reset differentiator when switching mode
253  if (h_ctl_course_heading_mode == 1)
254  last_err = err;
255  h_ctl_course_heading_mode = 0;
256  }
257  */
258 #endif //STRONG_WIND
259 
260  float d_err = err - last_err;
261  last_err = err;
262 
263  NormRadAngle(d_err);
264 
265 #ifdef H_CTL_COURSE_SLEW_INCREMENT
266  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
267  static float h_ctl_course_slew_rate = 0.;
268  float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
269  float half_nav_angle_saturation = nav_angle_saturation / 2.;
270  if (launch) { /* prevent accumulator run-up on the ground */
271  if (err > half_nav_angle_saturation) {
272  h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
273  err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
274  h_ctl_course_slew_rate += h_ctl_course_slew_increment;
275  } else if (err < -half_nav_angle_saturation) {
276  h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
277  err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));
278  h_ctl_course_slew_rate -= h_ctl_course_slew_increment;
279  } else {
280  h_ctl_course_slew_rate = 0.;
281  }
282  }
283 #endif
284 
285  float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
286  Bound(speed_depend_nav, 0.66, 1.5);
287 
288  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
289 
290 
291 
292 #if defined(AGR_CLIMB) && !USE_AIRSPEED
293 
294  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
297  BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
298  /* altitude: z-up is positive -> positive error -> too low */
299  if (v_ctl_altitude_error > 0) {
300  nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
301  (AGR_BLEND_START - AGR_BLEND_END));
302  Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
303  } else {
304  nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
305  (AGR_BLEND_START - AGR_BLEND_END));
306  Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
307  }
308  cmd *= nav_ratio;
309  }
310  }
311 #endif
312 
313  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
314 
315 #ifdef H_CTL_ROLL_SLEW
316  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
317  BoundAbs(diff_roll, h_ctl_roll_slew);
318  h_ctl_roll_setpoint += diff_roll;
319 #else
320  h_ctl_roll_setpoint = roll_setpoint;
321 #endif
322 
323  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
324 }
325 
327 {
328  if (!h_ctl_disabled) {
329  h_ctl_roll_loop();
331  }
332 }
333 
334 
335 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
336 inline static void h_ctl_roll_loop(void)
337 {
339  struct FloatRates *body_rate = stateGetBodyRates_f();
340 #ifdef SITL
341  static float last_err = 0;
342  body_rate->p = (err - last_err) / (1 / 60.);
343  last_err = err;
344 #endif
345  float cmd = h_ctl_roll_attitude_gain * err
346  + h_ctl_roll_rate_gain * body_rate->p
348 
350 }
351 
352 #else // H_CTL_ROLL_ATTITUDE_GAIN
353 
355 inline static void h_ctl_roll_loop(void)
356 {
358  float cmd = h_ctl_roll_pgain * err
361 
362 #ifdef H_CTL_RATE_LOOP
363  if (h_ctl_auto1_rate) {
365  h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
366  h_ctl_roll_rate_loop();
367  } else {
368  h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
369  BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
370 
371  float saved_aileron_setpoint = h_ctl_aileron_setpoint;
372  h_ctl_roll_rate_loop();
373  h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint, saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
374  }
375 #endif
376 }
377 
378 #ifdef H_CTL_RATE_LOOP
379 
380 static inline void h_ctl_roll_rate_loop()
381 {
382  float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
383 
384  /* I term calculation */
385  static float roll_rate_sum_err = 0.;
386  static uint8_t roll_rate_sum_idx = 0;
387  static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
388 
389  roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
390  roll_rate_sum_values[roll_rate_sum_idx] = err;
391  roll_rate_sum_err += err;
392  roll_rate_sum_idx++;
393  if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) { roll_rate_sum_idx = 0; }
394 
395  /* D term calculations */
396  static float last_err = 0;
397  float d_err = err - last_err;
398  last_err = err;
399 
400  float throttle_dep_pgain =
401  Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain,
402  v_ctl_throttle_setpoint / ((float)MAX_PPRZ));
403  float cmd = throttle_dep_pgain * (err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES +
404  h_ctl_roll_rate_dgain * d_err);
405 
407 }
408 #endif /* H_CTL_RATE_LOOP */
409 
410 #endif /* !H_CTL_ROLL_ATTITUDE_GAIN */
411 
412 
413 
414 
415 
416 
417 #ifdef LOITER_TRIM
418 
419 float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
420 float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
421 
422 inline static float loiter(void)
423 {
424  static float last_elevator_trim;
425  float elevator_trim;
426 
428  if (throttle_dif > 0) {
429  float max_dif = Max(v_ctl_auto_throttle_max_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
430  elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
431  } else {
432  float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle - v_ctl_auto_throttle_min_cruise_throttle, 0.1);
433  elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
434  }
435 
436  float max_change = (v_ctl_auto_throttle_loiter_trim - v_ctl_auto_throttle_dash_trim) / 80.;
437  Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim + max_change);
438 
439  last_elevator_trim = elevator_trim;
440  return elevator_trim;
441 }
442 #endif
443 
444 
445 inline static void h_ctl_pitch_loop(void)
446 {
447  static float last_err;
448  struct FloatEulers *att = stateGetNedToBodyEulers_f();
449  /* sanity check */
450  if (h_ctl_elevator_of_roll < 0.) {
452  }
453 
455 
456  float err = 0;
457 
458 #ifdef USE_AOA
459  switch (h_ctl_pitch_mode) {
460  case H_CTL_PITCH_MODE_THETA:
461  err = att->theta - h_ctl_pitch_loop_setpoint;
462  break;
463  case H_CTL_PITCH_MODE_AOA:
465  break;
466  default:
467  err = att->theta - h_ctl_pitch_loop_setpoint;
468  break;
469  }
470 #else //NO_AOA
471  err = att->theta - h_ctl_pitch_loop_setpoint;
472 #endif
473 
474 
475  float d_err = err - last_err;
476  last_err = err;
477  float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
478 #ifdef LOITER_TRIM
479  cmd += loiter();
480 #endif
482 }
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: energy_ctrl.c:92
float h_ctl_roll_setpoint
float h_ctl_pitch_dgain
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:104
#define H_CTL_PITCH_DGAIN
bool_t launch
Definition: sim_ap.c:39
static void h_ctl_roll_loop(void)
Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint.
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
#define H_CTL_ROLL_RATE_GAIN
#define Min(x, y)
float phi
in radians
Generic transmission transport header.
Definition: transport.h:89
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
Periodic telemetry system header (includes downlink utility and generated code).
void h_ctl_course_loop(void)
pprz_t h_ctl_aileron_setpoint
int16_t pprz_t
Definition: paparazzi.h:6
float psi
in radians
float h_ctl_pitch_pgain
void h_ctl_init(void)
float p
in rad/s
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
float h_ctl_elevator_of_roll
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:58
pprz_t h_ctl_elevator_setpoint
float h_ctl_course_setpoint
#define FALSE
Definition: std.h:5
void h_ctl_attitude_loop(void)
euler angles
Fixed wing horizontal control.
float theta
in radians
float h_ctl_roll_slew
static void h_ctl_pitch_loop(void)
bool_t h_ctl_disabled
#define Max(x, y)
bool_t h_ctl_auto1_rate
float h_ctl_pitch_setpoint
float h_ctl_aileron_of_throttle
#define H_CTL_COURSE_PRE_BANK_CORRECTION
float h_ctl_course_pre_bank_correction
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
float h_ctl_pitch_loop_setpoint
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:57
float h_ctl_roll_pgain
#define H_CTL_COURSE_DGAIN
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1319
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1171
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
float h_ctl_roll_attitude_gain
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float h_ctl_roll_rate_gain
float h_ctl_course_pre_bank
arch independent LED (Light Emitting Diodes) API
#define V_CTL_AUTO_THROTTLE_BLENDED
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
angular rates
float h_ctl_course_pgain
float h_ctl_course_dgain
float h_ctl_roll_max_setpoint
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Fixedwing autopilot modes.