Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
stabilization_attitude.c
Go to the documentation of this file.
1 /*
2  * Paparazzi $Id$
3  *
4  * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
32 #include "std.h"
33 #include "led.h"
34 #include "estimator.h"
35 #include "subsystems/nav.h"
36 #include "generated/airframe.h"
39 
40 #pragma message "CAUTION! ALL control gains have to be positive now!"
41 
42 /* outer loop parameters */
43 float h_ctl_course_setpoint; /* rad, CW/north */
49 
50 /* roll and pitch disabling */
52 
53 /* AUTO1 rate mode */
55 
56 
57 /* inner roll loop parameters */
62 
63 /* inner pitch loop parameters */
69 
70 #ifdef USE_AOA
71  uint8_t h_ctl_pitch_mode;
72 #endif
73 
74 /* inner loop pre-command */
77 
78 /* rate loop */
79 #ifdef H_CTL_RATE_LOOP
80 float h_ctl_roll_rate_setpoint;
81 float h_ctl_roll_rate_mode;
82 float h_ctl_roll_rate_setpoint_pgain;
83 float h_ctl_hi_throttle_roll_rate_pgain;
84 float h_ctl_lo_throttle_roll_rate_pgain;
85 float h_ctl_roll_rate_igain;
86 float h_ctl_roll_rate_dgain;
87 #endif
88 
89 #ifdef H_CTL_COURSE_SLEW_INCREMENT
90 float h_ctl_course_slew_increment;
91 #endif
92 
93 
94 inline static void h_ctl_roll_loop( void );
95 inline static void h_ctl_pitch_loop( void );
96 #ifdef H_CTL_RATE_LOOP
97 static inline void h_ctl_roll_rate_loop( void );
98 #endif
99 
100 #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
101 #define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
102 #endif
103 
104 #ifndef H_CTL_COURSE_DGAIN
105 #define H_CTL_COURSE_DGAIN 0.
106 #endif
107 
108 #ifndef H_CTL_ROLL_RATE_GAIN
109 #define H_CTL_ROLL_RATE_GAIN 0.
110 #endif
111 
114 
115 #ifdef AGR_CLIMB
116 static float nav_ratio;
117 #endif
118 
119 void h_ctl_init( void ) {
123  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
125  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
126 
127 #ifdef USE_AOA
128  h_ctl_pitch_mode = 0;
129 #endif
130 
132 
133  h_ctl_roll_setpoint = 0.;
134 #ifdef H_CTL_ROLL_PGAIN
135  h_ctl_roll_pgain = H_CTL_ROLL_PGAIN;
136 #endif
138 #ifdef H_CTL_AILERON_OF_THROTTLE
139  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
140 #endif
141 
144  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
147  h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
148 
149 #ifdef H_CTL_RATE_LOOP
150  h_ctl_roll_rate_mode = H_CTL_ROLL_RATE_MODE_DEFAULT;
151  h_ctl_roll_rate_setpoint_pgain = H_CTL_ROLL_RATE_SETPOINT_PGAIN;
152  h_ctl_hi_throttle_roll_rate_pgain = H_CTL_HI_THROTTLE_ROLL_RATE_PGAIN;
153  h_ctl_lo_throttle_roll_rate_pgain = H_CTL_LO_THROTTLE_ROLL_RATE_PGAIN;
154  h_ctl_roll_rate_igain = H_CTL_ROLL_RATE_IGAIN;
155  h_ctl_roll_rate_dgain = H_CTL_ROLL_RATE_DGAIN;
156 #endif
157 
158 #ifdef H_CTL_ROLL_SLEW
159  h_ctl_roll_slew = H_CTL_ROLL_SLEW;
160 #endif
161 
162 #ifdef H_CTL_COURSE_SLEW_INCREMENT
163  h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT;
164 #endif
165 
166 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
167  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
169 #endif
170 
171 #ifdef AGR_CLIMB
172 nav_ratio=0;
173 #endif
174 }
175 
180 void h_ctl_course_loop ( void ) {
181  static float last_err;
182 
183  // Ground path error
185  NormRadAngle(err);
186 
187 #ifdef STRONG_WIND
188  // Usefull path speed
189  const float reference_advance = (NOMINAL_AIRSPEED / 2.);
190  float advance = cos(err) * estimator_hspeed_mod / reference_advance;
191 
192  if (
193  (advance < 1.) && // Path speed is small
194  (estimator_hspeed_mod < reference_advance) // Small path speed is due to wind (small groundspeed)
195  )
196  {
197 /*
198  // rough crabangle approximation
199  float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
200  float wind_dir = atan2(wind_east,wind_north);
201 
202  float wind_course = h_ctl_course_setpoint - wind_dir;
203  NormRadAngle(wind_course);
204 
205  estimator_hspeed_dir = estimator_psi;
206 
207  float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
208  //crab = estimator_hspeed_mod - estimator_psi;
209  NormRadAngle(crab);
210 */
211 
212  // Heading error
213  float herr = estimator_psi - h_ctl_course_setpoint; //+crab);
214  NormRadAngle(herr);
215 
216  if (advance < -0.5) //<! moving in the wrong direction / big > 90 degree turn
217  {
218  err = herr;
219  }
220  else if (advance < 0.) //<!
221  {
222  err = (-advance)*2. * herr;
223  }
224  else
225  {
226  err = advance * err;
227  }
228 
229  // Reset differentiator when switching mode
230  //if (h_ctl_course_heading_mode == 0)
231  // last_err = err;
232  //h_ctl_course_heading_mode = 1;
233  }
234 /* else
235  {
236  // Reset differentiator when switching mode
237  if (h_ctl_course_heading_mode == 1)
238  last_err = err;
239  h_ctl_course_heading_mode = 0;
240  }
241 */
242 #endif //STRONG_WIND
243 
244  float d_err = err - last_err;
245  last_err = err;
246 
247  NormRadAngle(d_err);
248 
249 #ifdef H_CTL_COURSE_SLEW_INCREMENT
250  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
251  static float h_ctl_course_slew_rate = 0.;
252  float nav_angle_saturation = h_ctl_roll_max_setpoint/h_ctl_course_pgain; /* heading error corresponding to max_roll */
253  float half_nav_angle_saturation = nav_angle_saturation / 2.;
254  if (launch) { /* prevent accumulator run-up on the ground */
255  if (err > half_nav_angle_saturation) {
256  h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
257  err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate));
258  h_ctl_course_slew_rate +=h_ctl_course_slew_increment;
259  }
260  else if ( err < -half_nav_angle_saturation) {
261  h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
262  err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate));
263  h_ctl_course_slew_rate -=h_ctl_course_slew_increment;
264  }
265  else {
266  h_ctl_course_slew_rate = 0.;
267  }
268  }
269 #endif
270 
271  float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
272  Bound(speed_depend_nav, 0.66, 1.5);
273 
274  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
275 
276 
277 
278 #if defined(AGR_CLIMB) && !USE_AIRSPEED
279 
280  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
282  BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
283  /* altitude: z-up is positive -> positive error -> too low */
284  if (v_ctl_altitude_error > 0) {
285  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));
286  Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
287  } else {
288  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));
289  Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
290  }
291  cmd *= nav_ratio;
292  }
293  }
294 #endif
295 
296  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
297 
298 #ifdef H_CTL_ROLL_SLEW
299  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
300  BoundAbs(diff_roll, h_ctl_roll_slew);
301  h_ctl_roll_setpoint += diff_roll;
302 #else
303  h_ctl_roll_setpoint = roll_setpoint;
304 #endif
305 
306  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
307 }
308 
309 void h_ctl_attitude_loop ( void ) {
310  if (!h_ctl_disabled) {
311  h_ctl_roll_loop();
313  }
314 }
315 
316 
317 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
318 inline static void h_ctl_roll_loop( void ) {
319  float err = estimator_phi - h_ctl_roll_setpoint;
320 #ifdef SITL
321  static float last_err = 0;
322  estimator_p = (err - last_err)/(1/60.);
323  last_err = err;
324 #endif
325  float cmd = h_ctl_roll_attitude_gain * err
328 
330 }
331 
332 #else // H_CTL_ROLL_ATTITUDE_GAIN
333 
335 inline static void h_ctl_roll_loop( void ) {
336  float err = estimator_phi - h_ctl_roll_setpoint;
337  float cmd = h_ctl_roll_pgain * err
340 
341 #ifdef H_CTL_RATE_LOOP
342  if (h_ctl_auto1_rate) {
344  h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
345  h_ctl_roll_rate_loop();
346  } else {
347  h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
348  BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
349 
350  float saved_aileron_setpoint = h_ctl_aileron_setpoint;
351  h_ctl_roll_rate_loop();
352  h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint, saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
353  }
354 #endif
355 }
356 
357 #ifdef H_CTL_RATE_LOOP
358 
359 static inline void h_ctl_roll_rate_loop() {
360  float err = estimator_p - h_ctl_roll_rate_setpoint;
361 
362  /* I term calculation */
363  static float roll_rate_sum_err = 0.;
364  static uint8_t roll_rate_sum_idx = 0;
365  static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
366 
367  roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
368  roll_rate_sum_values[roll_rate_sum_idx] = err;
369  roll_rate_sum_err += err;
370  roll_rate_sum_idx++;
371  if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) roll_rate_sum_idx = 0;
372 
373  /* D term calculations */
374  static float last_err = 0;
375  float d_err = err - last_err;
376  last_err = err;
377 
378  float throttle_dep_pgain =
379  Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ));
380  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);
381 
383 }
384 #endif /* H_CTL_RATE_LOOP */
385 
386 #endif /* !H_CTL_ROLL_ATTITUDE_GAIN */
387 
388 
389 
390 
391 
392 
393 #ifdef LOITER_TRIM
394 
395 float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
396 float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
397 
398 inline static float loiter(void) {
399  static float last_elevator_trim;
400  float elevator_trim;
401 
403  if (throttle_dif > 0) {
404  float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
405  elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
406  } else {
407  float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
408  elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
409  }
410 
411  float max_change = (v_ctl_auto_throttle_loiter_trim - v_ctl_auto_throttle_dash_trim) / 80.;
412  Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim + max_change);
413 
414  last_elevator_trim = elevator_trim;
415  return elevator_trim;
416 }
417 #endif
418 
419 
420 inline static void h_ctl_pitch_loop( void ) {
421  static float last_err;
422  /* sanity check */
423  if (h_ctl_elevator_of_roll <0.)
425 
427 
428  float err = 0;
429 
430 #ifdef USE_AOA
431  switch (h_ctl_pitch_mode){
432  case H_CTL_PITCH_MODE_THETA:
434  break;
435  case H_CTL_PITCH_MODE_AOA:
437  break;
438  default:
440  break;
441  }
442 #else //NO_AOA
444 #endif
445 
446 
447  float d_err = err - last_err;
448  last_err = err;
449  float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
450 #ifdef LOITER_TRIM
451  cmd += loiter();
452 #endif
454 }
float h_ctl_roll_setpoint
float h_ctl_pitch_dgain
#define H_CTL_PITCH_DGAIN
#define V_CTL_AUTO_THROTTLE_BLENDED
Definition: guidance_v.h:59
bool_t launch
Definition: sim_ap.c:41
static void h_ctl_roll_loop(void)
Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint.
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
#define H_CTL_ROLL_RATE_GAIN
#define Min(x, y)
void h_ctl_course_loop(void)
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
pprz_t h_ctl_aileron_setpoint
int16_t pprz_t
Definition: paparazzi.h:6
float h_ctl_pitch_pgain
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: guidance_v.c:56
void h_ctl_init(void)
float h_ctl_elevator_of_roll
float estimator_p
Definition: estimator.c:54
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
#define V_CTL_AUTO_THROTTLE_AGRESSIVE
Definition: guidance_v.h:58
pprz_t h_ctl_elevator_setpoint
float h_ctl_course_setpoint
fixed wing horizontal control
#define FALSE
Definition: imu_chimu.h:141
void h_ctl_attitude_loop(void)
uint8_t v_ctl_auto_throttle_submode
Definition: guidance_v.c:48
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 estimator_AOA
angle of attack in rad
Definition: estimator.c:70
Vertical control for fixed wing vehicles.
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: guidance_v.c:41
float h_ctl_aileron_of_throttle
#define H_CTL_COURSE_PRE_BANK_CORRECTION
float h_ctl_course_pre_bank_correction
float h_ctl_pitch_loop_setpoint
float h_ctl_roll_pgain
#define H_CTL_COURSE_DGAIN
pprz_t v_ctl_throttle_setpoint
Definition: guidance_v.c:88
float h_ctl_roll_attitude_gain
float v_ctl_auto_throttle_cruise_throttle
Definition: guidance_v.c:55
unsigned char uint8_t
Definition: types.h:14
float h_ctl_roll_rate_gain
float h_ctl_course_pre_bank
float estimator_psi
heading in rad, CW, 0 = N
Definition: estimator.c:50
State estimation, fusioning sensors.
arch independent LED (Light Emitting Diodes) API
#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