Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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
36 #include "autopilot.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 
148  h_ctl_disabled = false;
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
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 (autopilot.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
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 
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) {
430  elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
431  } else {
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 
456  }
457  else {
459  }
460 
461  float err = 0;
462 
463 #ifdef USE_AOA
464  switch (h_ctl_pitch_mode) {
465  case H_CTL_PITCH_MODE_THETA:
466  err = att->theta - h_ctl_pitch_loop_setpoint;
467  break;
468  case H_CTL_PITCH_MODE_AOA:
470  break;
471  default:
472  err = att->theta - h_ctl_pitch_loop_setpoint;
473  break;
474  }
475 #else //NO_AOA
476  err = att->theta - h_ctl_pitch_loop_setpoint;
477 #endif
478 
479 
480  float d_err = err - last_err;
481  last_err = err;
482  float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
483 #ifdef LOITER_TRIM
484  cmd += loiter();
485 #endif
487 }
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:104
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: energy_ctrl.c:92
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define Min(x, y)
Definition: esc_dshot.c:109
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:58
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:59
Fixed wing horizontal control.
float phi
in radians
float p
in rad/s
float theta
in radians
float psi
in radians
euler angles
angular rates
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static float stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1416
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
#define V_CTL_MODE_LANDING
#define V_CTL_AUTO_THROTTLE_BLENDED
arch independent LED (Light Emitting Diodes) API
Fixedwing Navigation library.
int16_t pprz_t
Definition: paparazzi.h:6
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
#define H_CTL_PITCH_DGAIN
void h_ctl_course_loop(void)
static void h_ctl_pitch_loop(void)
float h_ctl_pitch_setpoint
static void send_calibration(struct transport_tx *trans, struct link_device *dev)
pprz_t h_ctl_elevator_setpoint
#define H_CTL_COURSE_PRE_BANK_CORRECTION
float h_ctl_course_dgain
float h_ctl_roll_pgain
float h_ctl_roll_max_setpoint
bool h_ctl_disabled
float h_ctl_elevator_of_roll
float h_ctl_roll_setpoint
static void h_ctl_roll_loop(void)
Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint.
float h_ctl_pitch_dgain
pprz_t h_ctl_aileron_setpoint
float h_ctl_pitch_pgain
bool h_ctl_auto1_rate
float h_ctl_roll_rate_gain
void h_ctl_init(void)
void h_ctl_attitude_loop(void)
float h_ctl_roll_attitude_gain
float h_ctl_aileron_of_throttle
float h_ctl_course_pgain
float h_ctl_pitch_loop_setpoint
#define H_CTL_COURSE_DGAIN
float h_ctl_roll_slew
float h_ctl_course_pre_bank_correction
float h_ctl_course_pre_bank
float h_ctl_course_setpoint
#define H_CTL_ROLL_RATE_GAIN
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98