Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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"
33 #include "subsystems/nav.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 void h_ctl_init( void ) {
119  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
121  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
122 
123 #ifdef USE_AOA
124  h_ctl_pitch_mode = 0;
125 #endif
126 
128 
129  h_ctl_roll_setpoint = 0.;
130 #ifdef H_CTL_ROLL_PGAIN
131  h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
132 #endif
134 #ifdef H_CTL_AILERON_OF_THROTTLE
135  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
136 #endif
137 
140  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
143  h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
144 
145 #ifdef H_CTL_RATE_LOOP
146  h_ctl_roll_rate_mode = H_CTL_ROLL_RATE_MODE_DEFAULT;
147  h_ctl_roll_rate_setpoint_pgain = H_CTL_ROLL_RATE_SETPOINT_PGAIN;
148  h_ctl_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
149  h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
150  h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
151  h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
152 #endif
153 
154 #ifdef H_CTL_ROLL_SLEW
155  h_ctl_roll_slew = H_CTL_ROLL_SLEW;
156 #endif
157 
158 #ifdef H_CTL_COURSE_SLEW_INCREMENT
159  h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
160 #endif
161 
162 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
163  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
165 #endif
166 
167 #ifdef AGR_CLIMB
168 nav_ratio=0;
169 #endif
170 }
171 
176 void h_ctl_course_loop ( void ) {
177  static float last_err;
178 
179  // Ground path error
181  NormRadAngle(err);
182 
183 #ifdef STRONG_WIND
184  // Usefull path speed
185  const float reference_advance = (NOMINAL_AIRSPEED / 2.);
186  float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance;
187 
188  if (
189  (advance < 1.) && // Path speed is small
190  ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed)
191  )
192  {
193 /*
194  // rough crabangle approximation
195  float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
196  float wind_dir = atan2(wind_east,wind_north);
197 
198  float wind_course = h_ctl_course_setpoint - wind_dir;
199  NormRadAngle(wind_course);
200 
201  estimator_hspeed_dir = estimator_psi;
202 
203  float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
204  //crab = estimator_hspeed_mod - estimator_psi;
205  NormRadAngle(crab);
206 */
207 
208  // Heading error
209  float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
210  NormRadAngle(herr);
211 
212  if (advance < -0.5) //<! moving in the wrong direction / big > 90 degree turn
213  {
214  err = herr;
215  }
216  else if (advance < 0.) //<!
217  {
218  err = (-advance)*2. * herr;
219  }
220  else
221  {
222  err = advance * err;
223  }
224 
225  // Reset differentiator when switching mode
226  //if (h_ctl_course_heading_mode == 0)
227  // last_err = err;
228  //h_ctl_course_heading_mode = 1;
229  }
230 /* else
231  {
232  // Reset differentiator when switching mode
233  if (h_ctl_course_heading_mode == 1)
234  last_err = err;
235  h_ctl_course_heading_mode = 0;
236  }
237 */
238 #endif //STRONG_WIND
239 
240  float d_err = err - last_err;
241  last_err = err;
242 
243  NormRadAngle(d_err);
244 
245 #ifdef H_CTL_COURSE_SLEW_INCREMENT
246  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
247  static float h_ctl_course_slew_rate = 0.;
248  float nav_angle_saturation = h_ctl_roll_max_setpoint/h_ctl_course_pgain; /* heading error corresponding to max_roll */
249  float half_nav_angle_saturation = nav_angle_saturation / 2.;
250  if (launch) { /* prevent accumulator run-up on the ground */
251  if (err > half_nav_angle_saturation) {
252  h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
253  err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate));
254  h_ctl_course_slew_rate +=h_ctl_course_slew_increment;
255  }
256  else if ( err < -half_nav_angle_saturation) {
257  h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
258  err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate));
259  h_ctl_course_slew_rate -=h_ctl_course_slew_increment;
260  }
261  else {
262  h_ctl_course_slew_rate = 0.;
263  }
264  }
265 #endif
266 
267  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
268  Bound(speed_depend_nav, 0.66, 1.5);
269 
270  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
271 
272 
273 
274 #if defined(AGR_CLIMB) && !USE_AIRSPEED
275 
276  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
278  BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
279  /* altitude: z-up is positive -> positive error -> too low */
280  if (v_ctl_altitude_error > 0) {
281  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));
282  Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
283  } else {
284  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));
285  Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
286  }
287  cmd *= nav_ratio;
288  }
289  }
290 #endif
291 
292  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
293 
294 #ifdef H_CTL_ROLL_SLEW
295  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
296  BoundAbs(diff_roll, h_ctl_roll_slew);
297  h_ctl_roll_setpoint += diff_roll;
298 #else
299  h_ctl_roll_setpoint = roll_setpoint;
300 #endif
301 
302  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
303 }
304 
305 void h_ctl_attitude_loop ( void ) {
306  if (!h_ctl_disabled) {
307  h_ctl_roll_loop();
309  }
310 }
311 
312 
313 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
314 inline static void h_ctl_roll_loop( void ) {
316  struct FloatRates* body_rate = stateGetBodyRates_f();
317 #ifdef SITL
318  static float last_err = 0;
319  body_rate->p = (err - last_err)/(1/60.);
320  last_err = err;
321 #endif
322  float cmd = h_ctl_roll_attitude_gain * err
323  + h_ctl_roll_rate_gain * body_rate->p
325 
327 }
328 
329 #else // H_CTL_ROLL_ATTITUDE_GAIN
330 
332 inline static void h_ctl_roll_loop( void ) {
334  float cmd = h_ctl_roll_pgain * err
337 
338 #ifdef H_CTL_RATE_LOOP
339  if (h_ctl_auto1_rate) {
341  h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
342  h_ctl_roll_rate_loop();
343  } else {
344  h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
345  BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
346 
347  float saved_aileron_setpoint = h_ctl_aileron_setpoint;
348  h_ctl_roll_rate_loop();
349  h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint, saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
350  }
351 #endif
352 }
353 
354 #ifdef H_CTL_RATE_LOOP
355 
356 static inline void h_ctl_roll_rate_loop() {
357  float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
358 
359  /* I term calculation */
360  static float roll_rate_sum_err = 0.;
361  static uint8_t roll_rate_sum_idx = 0;
362  static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
363 
364  roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
365  roll_rate_sum_values[roll_rate_sum_idx] = err;
366  roll_rate_sum_err += err;
367  roll_rate_sum_idx++;
368  if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) roll_rate_sum_idx = 0;
369 
370  /* D term calculations */
371  static float last_err = 0;
372  float d_err = err - last_err;
373  last_err = err;
374 
375  float throttle_dep_pgain =
376  Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ));
377  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);
378 
380 }
381 #endif /* H_CTL_RATE_LOOP */
382 
383 #endif /* !H_CTL_ROLL_ATTITUDE_GAIN */
384 
385 
386 
387 
388 
389 
390 #ifdef LOITER_TRIM
391 
392 float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
393 float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
394 
395 inline static float loiter(void) {
396  static float last_elevator_trim;
397  float elevator_trim;
398 
400  if (throttle_dif > 0) {
401  float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
402  elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
403  } else {
404  float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
405  elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
406  }
407 
408  float max_change = (v_ctl_auto_throttle_loiter_trim - v_ctl_auto_throttle_dash_trim) / 80.;
409  Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim + max_change);
410 
411  last_elevator_trim = elevator_trim;
412  return elevator_trim;
413 }
414 #endif
415 
416 
417 inline static void h_ctl_pitch_loop( void ) {
418  static float last_err;
419  struct FloatEulers* att = stateGetNedToBodyEulers_f();
420  /* sanity check */
421  if (h_ctl_elevator_of_roll <0.)
423 
425 
426  float err = 0;
427 
428 #ifdef USE_AOA
429  switch (h_ctl_pitch_mode){
430  case H_CTL_PITCH_MODE_THETA:
431  err = att->theta - h_ctl_pitch_loop_setpoint;
432  break;
433  case H_CTL_PITCH_MODE_AOA:
435  break;
436  default:
437  err = att->theta - h_ctl_pitch_loop_setpoint;
438  break;
439  }
440 #else //NO_AOA
441  err = att->theta - h_ctl_pitch_loop_setpoint;
442 #endif
443 
444 
445  float d_err = err - last_err;
446  last_err = err;
447  float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
448 #ifdef LOITER_TRIM
449  cmd += loiter();
450 #endif
452 }
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
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 h_ctl_elevator_of_roll
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^2
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.