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_adaptive.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009-2010 The Paparazzi Team
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 
29 #include "std.h"
30 #include "led.h"
33 #include "state.h"
35 #include "generated/airframe.h"
36 #include CTRL_TYPE_H
38 
39 /* outer loop parameters */
40 float h_ctl_course_setpoint; /* rad, CW/north */
46 
47 /* roll and pitch disabling */
49 
50 /* AUTO1 rate mode */
52 
53 struct HCtlAdaptRef {
54  float roll_angle;
55  float roll_rate;
56  float roll_accel;
57  float pitch_angle;
58  float pitch_rate;
59  float pitch_accel;
60 
61  float max_p;
62  float max_p_dot;
63  float max_q;
64  float max_q_dot;
65 };
66 
68 
69 /* Reference generator */
70 #ifndef H_CTL_REF_W_P
71 #define H_CTL_REF_W_P 5.
72 #endif
73 #ifndef H_CTL_REF_XI_P
74 #define H_CTL_REF_XI_P 0.85
75 #endif
76 #ifndef H_CTL_REF_W_Q
77 #define H_CTL_REF_W_Q 5.
78 #endif
79 #ifndef H_CTL_REF_XI_Q
80 #define H_CTL_REF_XI_Q 0.85
81 #endif
82 #ifndef H_CTL_REF_MAX_P
83 #define H_CTL_REF_MAX_P RadOfDeg(150.)
84 #endif
85 #ifndef H_CTL_REF_MAX_P_DOT
86 #define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
87 #endif
88 #ifndef H_CTL_REF_MAX_Q
89 #define H_CTL_REF_MAX_Q RadOfDeg(150.)
90 #endif
91 #ifndef H_CTL_REF_MAX_Q_DOT
92 #define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
93 #endif
94 
95 #if USE_ANGLE_REF
96 PRINT_CONFIG_MSG("USING ATTITUDE REFERENCE GENERATOR")
103 #endif
104 
105 /* inner roll loop parameters */
115 
117 
118 /* inner pitch loop parameters */
128 
129 /* inner loop pre-command */
132 float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
133 
136 #define AIRSPEED_RATIO_MIN 0.5
137 #define AIRSPEED_RATIO_MAX 2.
138 
141 
142 // Pitch trim rate limiter
143 #ifndef PITCH_TRIM_RATE_LIMITER
144 #define PITCH_TRIM_RATE_LIMITER 3.
145 #endif
146 inline static void h_ctl_roll_loop( void );
147 inline static void h_ctl_pitch_loop( void );
148 
149 // Some default course gains
150 // H_CTL_COURSE_PGAIN needs to be define in airframe
151 #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
152 #define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
153 #endif
154 #ifndef H_CTL_COURSE_DGAIN
155 #define H_CTL_COURSE_DGAIN 0.
156 #endif
157 
158 // Some default roll gains
159 // H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe
160 #ifndef H_CTL_ROLL_RATE_GAIN
161 #define H_CTL_ROLL_RATE_GAIN 0.
162 #endif
163 #ifndef H_CTL_ROLL_IGAIN
164 #define H_CTL_ROLL_IGAIN 0.
165 #endif
166 #ifndef H_CTL_ROLL_KFFA
167 #define H_CTL_ROLL_KFFA 0.
168 #endif
169 #ifndef H_CTL_ROLL_KFFD
170 #define H_CTL_ROLL_KFFD 0.
171 #endif
172 
173 // Some default pitch gains
174 // H_CTL_PITCH_PGAIN needs to be define in airframe
175 #ifndef H_CTL_PITCH_DGAIN
176 #define H_CTL_PITCH_DGAIN 0.
177 #endif
178 #ifndef H_CTL_PITCH_IGAIN
179 #define H_CTL_PITCH_IGAIN 0.
180 #endif
181 #ifndef H_CTL_PITCH_KFFA
182 #define H_CTL_PITCH_KFFA 0.
183 #endif
184 #ifndef H_CTL_PITCH_KFFD
185 #define H_CTL_PITCH_KFFD 0.
186 #endif
187 
188 #ifndef USE_GYRO_PITCH_RATE
189 #define USE_GYRO_PITCH_RATE TRUE
190 #endif
191 
192 #if PERIODIC_TELEMETRY
194 
195 static void send_calibration(void) {
197 }
198 
199 static void send_tune_roll(void) {
200  DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice,
201  &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
202 }
203 
204 static void send_ctl_a(void) {
205  DOWNLINK_SEND_H_CTL_A(DefaultChannel, DefaultDevice,
206  &h_ctl_roll_sum_err,
207  &h_ctl_roll_setpoint,
209  &(stateGetNedToBodyEulers_f()->phi),
210  &h_ctl_aileron_setpoint,
211  &h_ctl_pitch_sum_err,
212  &h_ctl_pitch_loop_setpoint,
214  &(stateGetNedToBodyEulers_f()->theta),
215  &h_ctl_elevator_setpoint);
216 }
217 #endif
218 
219 void h_ctl_init( void ) {
224 
228  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
230  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
231 
233 
234  h_ctl_roll_setpoint = 0.;
235  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
236  h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
237  h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
238  h_ctl_roll_sum_err = 0;
239  h_ctl_roll_Kffa = H_CTL_ROLL_KFFA;
240  h_ctl_roll_Kffd = H_CTL_ROLL_KFFD;
241  h_ctl_aileron_setpoint = 0;
242 #ifdef H_CTL_AILERON_OF_THROTTLE
243  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
244 #endif
245 
246  h_ctl_pitch_setpoint = 0.;
247  h_ctl_pitch_loop_setpoint = 0.;
248  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
249  h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
250  h_ctl_pitch_igain = H_CTL_PITCH_IGAIN;
251  h_ctl_pitch_sum_err = 0.;
252  h_ctl_pitch_Kffa = H_CTL_PITCH_KFFA;
253  h_ctl_pitch_Kffd = H_CTL_PITCH_KFFD;
254  h_ctl_elevator_setpoint = 0;
255  h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
256 #ifdef H_CTL_PITCH_OF_ROLL
257  h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
258 #endif
259 
260  use_airspeed_ratio = FALSE;
261  airspeed_ratio2 = 1.;
262 
263 #if USE_PITCH_TRIM
264  v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
265  v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
266 #else
267  v_ctl_pitch_loiter_trim = 0.;
268  v_ctl_pitch_dash_trim = 0.;
269 #endif
270 
271 #if PERIODIC_TELEMETRY
272  register_periodic_telemetry(DefaultPeriodic, "CALIBRATION", send_calibration);
273  register_periodic_telemetry(DefaultPeriodic, "TUNE_ROLL", send_tune_roll);
274  register_periodic_telemetry(DefaultPeriodic, "H_CTL_A", send_ctl_a);
275 #endif
276 }
277 
282 void h_ctl_course_loop ( void ) {
283  static float last_err;
284 
285  // Ground path error
287  NormRadAngle(err);
288 
289  float d_err = err - last_err;
290  last_err = err;
291  NormRadAngle(d_err);
292 
293  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
294  Bound(speed_depend_nav, 0.66, 1.5);
295 
297  + h_ctl_course_pgain * speed_depend_nav * err
298  + h_ctl_course_dgain * d_err;
299 
300  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
301 }
302 
303 #if USE_AIRSPEED
304 static inline void compute_airspeed_ratio( void ) {
305  if (use_airspeed_ratio) {
306  // low pass airspeed
307  static float airspeed = 0.;
308  airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16;
309  // compute ratio
310  float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
311  Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
312  airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
313  } else {
314  airspeed_ratio2 = 1.;
315  }
316 }
317 #endif
318 
319 void h_ctl_attitude_loop ( void ) {
320  if (!h_ctl_disabled) {
321 #if USE_AIRSPEED
322  compute_airspeed_ratio();
323 #endif
324  h_ctl_roll_loop();
326  }
327 }
328 
333 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
334 #define H_CTL_REF_DT (1./AHRS_PROPAGATE_FREQUENCY)
335 #else
336 #define H_CTL_REF_DT (1./CONTROL_FREQUENCY)
337 #endif
338 
344 #define KFFA_UPDATE 0.1
345 #define KFFD_UPDATE 0.05
346 
347 inline static void h_ctl_roll_loop( void ) {
348 
349  static float cmd_fb = 0.;
350 
351 #if USE_ANGLE_REF
352  // Update reference setpoints for roll
356  // Saturation on references
360  if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.;
361  }
362  else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {
364  if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.;
365  }
366 #else
368  h_ctl_ref.roll_rate = 0.;
369  h_ctl_ref.roll_accel = 0.;
370 #endif
371 
372 #ifdef USE_KFF_UPDATE
373  // update Kff gains
374  h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot);
375  h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p);
376 #ifdef SITL
377  printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
378 #endif
379  h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);
380  h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);
381 #endif
382 
383  // Compute errors
385  struct FloatRates* body_rate = stateGetBodyRates_f();
386  float d_err = h_ctl_ref.roll_rate - body_rate->p;
387 
388  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
389  h_ctl_roll_sum_err = 0.;
390  }
391  else {
392  if (h_ctl_roll_igain > 0.) {
393  h_ctl_roll_sum_err += err * H_CTL_REF_DT;
394  BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
395  } else {
396  h_ctl_roll_sum_err = 0.;
397  }
398  }
399 
400  cmd_fb = h_ctl_roll_attitude_gain * err;
401  float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel
402  - h_ctl_roll_Kffd * h_ctl_ref.roll_rate
403  - cmd_fb
404  - h_ctl_roll_rate_gain * d_err
405  - h_ctl_roll_igain * h_ctl_roll_sum_err
407 
408  cmd /= airspeed_ratio2;
409 
410  // Set aileron commands
411  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
412 }
413 
414 
415 #if USE_PITCH_TRIM
416 inline static void loiter(void) {
417  float pitch_trim;
418  static float last_pitch_trim;
419 
420 #if USE_AIRSPEED
421  if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) {
422  pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
423  } else {
424  pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
425  }
426 #else
428  if (throttle_diff > 0) {
429  float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
430  pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
431  } else {
432  float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
433  pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
434  }
435 #endif
436 
437  // Pitch trim rate limiter
438  float max_change = (v_ctl_pitch_loiter_trim - v_ctl_pitch_dash_trim) * H_CTL_REF_DT/ PITCH_TRIM_RATE_LIMITER;
439  Bound(pitch_trim, last_pitch_trim - max_change, last_pitch_trim + max_change);
440 
441  last_pitch_trim = pitch_trim;
442  h_ctl_pitch_loop_setpoint += pitch_trim;
443 }
444 #endif
445 
446 
447 inline static void h_ctl_pitch_loop( void ) {
448 #if !USE_GYRO_PITCH_RATE
449  static float last_err;
450 #endif
451 
452  /* sanity check */
453  if (h_ctl_pitch_of_roll <0.)
454  h_ctl_pitch_of_roll = 0.;
455 
456  h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
457 #if USE_PITCH_TRIM
458  loiter();
459 #endif
460 
461 #if USE_ANGLE_REF
462  // Update reference setpoints for pitch
466  // Saturation on references
471  }
472  else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {
475  }
476 #else
478  h_ctl_ref.pitch_rate = 0.;
479  h_ctl_ref.pitch_accel = 0.;
480 #endif
481 
482  // Compute errors
484 #if USE_GYRO_PITCH_RATE
485  float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;
486 #else // soft derivation
487  float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate;
488  last_err = err;
489 #endif
490 
491  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
492  h_ctl_pitch_sum_err = 0.;
493  }
494  else {
495  if (h_ctl_pitch_igain > 0.) {
496  h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
497  BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
498  } else {
499  h_ctl_pitch_sum_err = 0.;
500  }
501  }
502 
503  float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel
504  - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate
505  + h_ctl_pitch_pgain * err
506  + h_ctl_pitch_dgain * d_err
507  + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
508 
509  cmd /= airspeed_ratio2;
510 
511  h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
512 }
513 
#define H_CTL_REF_XI_P
#define H_CTL_REF_XI_Q
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:107
#define H_CTL_PITCH_DGAIN
void h_ctl_course_loop(void)
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
bool_t launch
Definition: sim_ap.c:40
#define H_CTL_ROLL_IGAIN
void h_ctl_attitude_loop(void)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
float v_ctl_pitch_loiter_trim
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:134
Periodic telemetry system header (includes downlink utility and generated code).
angular rates
float h_ctl_course_setpoint
#define H_CTL_REF_DT
Define reference generator time step default to control frequency and ahrs propagation freq if contro...
void h_ctl_init(void)
float h_ctl_roll_igain
float v_ctl_pitch_dash_trim
float h_ctl_roll_Kffa
uint8_t pprz_mode
Definition: autopilot.c:40
#define PITCH_TRIM_RATE_LIMITER
#define AIRSPEED_RATIO_MAX
int16_t pprz_t
Definition: paparazzi.h:6
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
#define H_CTL_REF_MAX_P_DOT
float theta
in radians
#define H_CTL_PITCH_KFFA
pprz_t h_ctl_elevator_setpoint
pprz_t h_ctl_aileron_setpoint
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:80
float h_ctl_roll_Kffd
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define H_CTL_ROLL_KFFA
#define KFFD_UPDATE
#define FALSE
Definition: imu_chimu.h:141
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
#define H_CTL_PITCH_SUM_ERR_MAX
Fixed wing horizontal control.
float p
in rad/s
#define H_CTL_REF_MAX_P
#define Max(x, y)
float h_ctl_aileron_of_throttle
float h_ctl_pitch_igain
#define H_CTL_REF_MAX_Q_DOT
float phi
in radians
#define H_CTL_REF_W_Q
#define H_CTL_ROLL_RATE_GAIN
float h_ctl_roll_slew
float h_ctl_pitch_loop_setpoint
#define H_CTL_COURSE_DGAIN
#define H_CTL_REF_MAX_Q
#define H_CTL_REF_W_P
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:79
static void h_ctl_pitch_loop(void)
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1078
float h_ctl_roll_max_setpoint
#define H_CTL_ROLL_SUM_ERR_MAX
float h_ctl_pitch_dgain
bool_t use_airspeed_ratio
float h_ctl_course_pre_bank
#define KFFA_UPDATE
Adaptive control tuning parameters activate with USE_KFF_UPDATE.
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
float h_ctl_pitch_setpoint
#define H_CTL_PITCH_IGAIN
API to get/set the generic vehicle states.
float h_ctl_pitch_of_roll
float h_ctl_roll_setpoint
float h_ctl_elevator_of_roll
float h_ctl_course_pgain
bool_t h_ctl_auto1_rate
float h_ctl_pitch_sum_err
float h_ctl_course_pre_bank_correction
float h_ctl_course_dgain
#define H_CTL_COURSE_PRE_BANK_CORRECTION
#define H_CTL_PITCH_KFFD
arch independent LED (Light Emitting Diodes) API
static float p[2][2]
float h_ctl_roll_attitude_gain
float q
in rad/s
float h_ctl_roll_pgain
#define H_CTL_ROLL_KFFD
static void h_ctl_roll_loop(void)
float airspeed_ratio2
#define PPRZ_MODE_MANUAL
AP modes.
Definition: autopilot.h:50
bool_t h_ctl_disabled
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
struct HCtlAdaptRef h_ctl_ref
float h_ctl_pitch_Kffd
float h_ctl_pitch_Kffa
float h_ctl_roll_rate_gain
Fixed wing horizontal adaptive control.
float h_ctl_roll_sum_err
#define AIRSPEED_RATIO_MIN
float h_ctl_pitch_pgain
Fixedwing autopilot modes.