Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 #ifdef USE_AOA
67 uint8_t h_ctl_pitch_mode;
68 #endif
69 
70 /* inner loop pre-command */
73 
74 /* rate loop */
75 #ifdef H_CTL_RATE_LOOP
76 float h_ctl_roll_rate_setpoint;
77 float h_ctl_roll_rate_mode;
78 float h_ctl_roll_rate_setpoint_pgain;
79 float h_ctl_hi_throttle_roll_rate_pgain;
80 float h_ctl_lo_throttle_roll_rate_pgain;
81 float h_ctl_roll_rate_igain;
82 float h_ctl_roll_rate_dgain;
83 #endif
84 
85 #ifdef H_CTL_COURSE_SLEW_INCREMENT
86 float h_ctl_course_slew_increment;
87 #endif
88 
89 
90 inline static void h_ctl_roll_loop( void );
91 inline static void h_ctl_pitch_loop( void );
92 #ifdef H_CTL_RATE_LOOP
93 static inline void h_ctl_roll_rate_loop( void );
94 #endif
95 
96 #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
97 #define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
98 #endif
99 
100 #ifndef H_CTL_COURSE_DGAIN
101 #define H_CTL_COURSE_DGAIN 0.
102 #endif
103 
104 #ifndef H_CTL_ROLL_RATE_GAIN
105 #define H_CTL_ROLL_RATE_GAIN 0.
106 #endif
107 
110 
111 #ifdef AGR_CLIMB
112 static float nav_ratio;
113 #endif
114 
115 #if PERIODIC_TELEMETRY
117 
118 static void send_calibration(void) {
120 }
121 #endif
122 
123 void h_ctl_init( void ) {
127  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
129  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
130 
131 #ifdef USE_AOA
132  h_ctl_pitch_mode = 0;
133 #endif
134 
136 
137  h_ctl_roll_setpoint = 0.;
138 #ifdef H_CTL_ROLL_PGAIN
139  h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
140 #endif
142 #ifdef H_CTL_AILERON_OF_THROTTLE
143  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
144 #endif
145 
148  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
151  h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
152 
153 #ifdef H_CTL_RATE_LOOP
154  h_ctl_roll_rate_mode = H_CTL_ROLL_RATE_MODE_DEFAULT;
155  h_ctl_roll_rate_setpoint_pgain = H_CTL_ROLL_RATE_SETPOINT_PGAIN;
156  h_ctl_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
157  h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
158  h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
159  h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
160 #endif
161 
162 #ifdef H_CTL_ROLL_SLEW
163  h_ctl_roll_slew = H_CTL_ROLL_SLEW;
164 #endif
165 
166 #ifdef H_CTL_COURSE_SLEW_INCREMENT
167  h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
168 #endif
169 
170 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
171  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
173 #endif
174 
175 #ifdef AGR_CLIMB
176  nav_ratio=0;
177 #endif
178 
179 #if PERIODIC_TELEMETRY
180  register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration);
181 #endif
182 }
183 
188 void h_ctl_course_loop ( void ) {
189  static float last_err;
190 
191  // Ground path error
193  NormRadAngle(err);
194 
195 #ifdef STRONG_WIND
196  // Usefull path speed
197  const float reference_advance = (NOMINAL_AIRSPEED / 2.);
198  float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance;
199 
200  if (
201  (advance < 1.) && // Path speed is small
202  ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed)
203  )
204  {
205  /*
206  // rough crabangle approximation
207  float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
208  float wind_dir = atan2(wind_east,wind_north);
209 
210  float wind_course = h_ctl_course_setpoint - wind_dir;
211  NormRadAngle(wind_course);
212 
213  estimator_hspeed_dir = estimator_psi;
214 
215  float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
216  //crab = estimator_hspeed_mod - estimator_psi;
217  NormRadAngle(crab);
218  */
219 
220  // Heading error
221  float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
222  NormRadAngle(herr);
223 
224  if (advance < -0.5) //<! moving in the wrong direction / big > 90 degree turn
225  {
226  err = herr;
227  }
228  else if (advance < 0.) //<!
229  {
230  err = (-advance)*2. * herr;
231  }
232  else
233  {
234  err = advance * err;
235  }
236 
237  // Reset differentiator when switching mode
238  //if (h_ctl_course_heading_mode == 0)
239  // last_err = err;
240  //h_ctl_course_heading_mode = 1;
241  }
242  /* else
243  {
244  // Reset differentiator when switching mode
245  if (h_ctl_course_heading_mode == 1)
246  last_err = err;
247  h_ctl_course_heading_mode = 0;
248  }
249  */
250 #endif //STRONG_WIND
251 
252  float d_err = err - last_err;
253  last_err = err;
254 
255  NormRadAngle(d_err);
256 
257 #ifdef H_CTL_COURSE_SLEW_INCREMENT
258  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
259  static float h_ctl_course_slew_rate = 0.;
260  float nav_angle_saturation = h_ctl_roll_max_setpoint/h_ctl_course_pgain; /* heading error corresponding to max_roll */
261  float half_nav_angle_saturation = nav_angle_saturation / 2.;
262  if (launch) { /* prevent accumulator run-up on the ground */
263  if (err > half_nav_angle_saturation) {
264  h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
265  err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate));
266  h_ctl_course_slew_rate +=h_ctl_course_slew_increment;
267  }
268  else if ( err < -half_nav_angle_saturation) {
269  h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
270  err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate));
271  h_ctl_course_slew_rate -=h_ctl_course_slew_increment;
272  }
273  else {
274  h_ctl_course_slew_rate = 0.;
275  }
276  }
277 #endif
278 
279  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
280  Bound(speed_depend_nav, 0.66, 1.5);
281 
282  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
283 
284 
285 
286 #if defined(AGR_CLIMB) && !USE_AIRSPEED
287 
288  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
290  BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
291  /* altitude: z-up is positive -> positive error -> too low */
292  if (v_ctl_altitude_error > 0) {
293  nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END));
294  Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
295  } else {
296  nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END));
297  Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
298  }
299  cmd *= nav_ratio;
300  }
301  }
302 #endif
303 
304  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
305 
306 #ifdef H_CTL_ROLL_SLEW
307  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
308  BoundAbs(diff_roll, h_ctl_roll_slew);
309  h_ctl_roll_setpoint += diff_roll;
310 #else
311  h_ctl_roll_setpoint = roll_setpoint;
312 #endif
313 
314  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
315 }
316 
317 void h_ctl_attitude_loop ( void ) {
318  if (!h_ctl_disabled) {
319  h_ctl_roll_loop();
321  }
322 }
323 
324 
325 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
326 inline static void h_ctl_roll_loop( void ) {
328  struct FloatRates* body_rate = stateGetBodyRates_f();
329 #ifdef SITL
330  static float last_err = 0;
331  body_rate->p = (err - last_err)/(1/60.);
332  last_err = err;
333 #endif
334  float cmd = h_ctl_roll_attitude_gain * err
335  + h_ctl_roll_rate_gain * body_rate->p
337 
339 }
340 
341 #else // H_CTL_ROLL_ATTITUDE_GAIN
342 
344 inline static void h_ctl_roll_loop( void ) {
346  float cmd = h_ctl_roll_pgain * err
349 
350 #ifdef H_CTL_RATE_LOOP
351  if (h_ctl_auto1_rate) {
353  h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
354  h_ctl_roll_rate_loop();
355  } else {
356  h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
357  BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
358 
359  float saved_aileron_setpoint = h_ctl_aileron_setpoint;
360  h_ctl_roll_rate_loop();
361  h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint, saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
362  }
363 #endif
364 }
365 
366 #ifdef H_CTL_RATE_LOOP
367 
368 static inline void h_ctl_roll_rate_loop() {
369  float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
370 
371  /* I term calculation */
372  static float roll_rate_sum_err = 0.;
373  static uint8_t roll_rate_sum_idx = 0;
374  static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
375 
376  roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
377  roll_rate_sum_values[roll_rate_sum_idx] = err;
378  roll_rate_sum_err += err;
379  roll_rate_sum_idx++;
380  if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) roll_rate_sum_idx = 0;
381 
382  /* D term calculations */
383  static float last_err = 0;
384  float d_err = err - last_err;
385  last_err = err;
386 
387  float throttle_dep_pgain =
388  Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ));
389  float cmd = throttle_dep_pgain * ( err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err);
390 
392 }
393 #endif /* H_CTL_RATE_LOOP */
394 
395 #endif /* !H_CTL_ROLL_ATTITUDE_GAIN */
396 
397 
398 
399 
400 
401 
402 #ifdef LOITER_TRIM
403 
404 float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
405 float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
406 
407 inline static float loiter(void) {
408  static float last_elevator_trim;
409  float elevator_trim;
410 
412  if (throttle_dif > 0) {
413  float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
414  elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
415  } else {
416  float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
417  elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
418  }
419 
420  float max_change = (v_ctl_auto_throttle_loiter_trim - v_ctl_auto_throttle_dash_trim) / 80.;
421  Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim + max_change);
422 
423  last_elevator_trim = elevator_trim;
424  return elevator_trim;
425 }
426 #endif
427 
428 
429 inline static void h_ctl_pitch_loop( void ) {
430  static float last_err;
431  struct FloatEulers* att = stateGetNedToBodyEulers_f();
432  /* sanity check */
433  if (h_ctl_elevator_of_roll <0.)
435 
437 
438  float err = 0;
439 
440 #ifdef USE_AOA
441  switch (h_ctl_pitch_mode){
442  case H_CTL_PITCH_MODE_THETA:
443  err = att->theta - h_ctl_pitch_loop_setpoint;
444  break;
445  case H_CTL_PITCH_MODE_AOA:
447  break;
448  default:
449  err = att->theta - h_ctl_pitch_loop_setpoint;
450  break;
451  }
452 #else //NO_AOA
453  err = att->theta - h_ctl_pitch_loop_setpoint;
454 #endif
455 
456 
457  float d_err = err - last_err;
458  last_err = err;
459  float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
460 #ifdef LOITER_TRIM
461  cmd += loiter();
462 #endif
464 }
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: energy_ctrl.c:95
float h_ctl_roll_setpoint
float h_ctl_pitch_dgain
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:107
#define H_CTL_PITCH_DGAIN
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
bool_t launch
Definition: sim_ap.c:40
static void h_ctl_roll_loop(void)
Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint.
#define H_CTL_ROLL_RATE_GAIN
#define Min(x, y)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:134
Periodic telemetry system header (includes downlink utility and generated code).
void h_ctl_course_loop(void)
angular rates
float psi
in radians
pprz_t h_ctl_aileron_setpoint
int16_t pprz_t
Definition: paparazzi.h:6
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
float h_ctl_pitch_pgain
float theta
in radians
void h_ctl_init(void)
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:80
float h_ctl_elevator_of_roll
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
pprz_t h_ctl_elevator_setpoint
float h_ctl_course_setpoint
#define FALSE
Definition: imu_chimu.h:141
void h_ctl_attitude_loop(void)
Fixed wing horizontal control.
float p
in rad/s
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 phi
in radians
float h_ctl_aileron_of_throttle
#define H_CTL_COURSE_PRE_BANK_CORRECTION
float h_ctl_course_pre_bank_correction
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:79
float h_ctl_pitch_loop_setpoint
float h_ctl_roll_pgain
#define H_CTL_COURSE_DGAIN
static float * stateGetAngleOfAttack_f(void)
Get angle of attack (float).
Definition: state.h:1206
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1078
float h_ctl_roll_attitude_gain
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
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
float h_ctl_course_pgain
float h_ctl_course_dgain
float h_ctl_roll_max_setpoint
Fixedwing autopilot modes.