Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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  */
22 
29 #include "std.h"
30 #include "led.h"
33 #include "estimator.h"
34 #include "subsystems/nav.h"
35 #include "generated/airframe.h"
36 #include CTRL_TYPE_H
38 
39 
40 /* outer loop parameters */
41 float h_ctl_course_setpoint; /* rad, CW/north */
47 
48 /* roll and pitch disabling */
50 
51 /* AUTO1 rate mode */
53 
54 
61 #define H_CTL_REF_W 5.
62 #define H_CTL_REF_XI 0.85
63 #define H_CTL_REF_MAX_P RadOfDeg(150.)
64 #define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
65 #define H_CTL_REF_MAX_Q RadOfDeg(150.)
66 #define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
67 
68 /* inner roll loop parameters */
78 
80 
81 /* inner pitch loop parameters */
91 
92 /* inner loop pre-command */
95 float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
96 
99 #define AIRSPEED_RATIO_MIN 0.5
100 #define AIRSPEED_RATIO_MAX 2.
101 
104 
105 inline static void h_ctl_roll_loop( void );
106 inline static void h_ctl_pitch_loop( void );
107 
108 // Some default course gains
109 // H_CTL_COURSE_PGAIN needs to be define in airframe
110 #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
111 #define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
112 #endif
113 #ifndef H_CTL_COURSE_DGAIN
114 #define H_CTL_COURSE_DGAIN 0.
115 #endif
116 
117 #pragma message "CAUTION! ALL control gains have to be positive now!"
118 
119 // Some default roll gains
120 // H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe
121 #ifndef H_CTL_ROLL_RATE_GAIN
122 #define H_CTL_ROLL_RATE_GAIN 0.
123 #endif
124 #ifndef H_CTL_ROLL_IGAIN
125 #define H_CTL_ROLL_IGAIN 0.
126 #endif
127 #ifndef H_CTL_ROLL_KFFA
128 #define H_CTL_ROLL_KFFA 0.
129 #endif
130 #ifndef H_CTL_ROLL_KFFD
131 #define H_CTL_ROLL_KFFD 0.
132 #endif
133 
134 // Some default pitch gains
135 // H_CTL_PITCH_PGAIN needs to be define in airframe
136 #ifndef H_CTL_PITCH_DGAIN
137 #define H_CTL_PITCH_DGAIN 0.
138 #endif
139 #ifndef H_CTL_PITCH_IGAIN
140 #define H_CTL_PITCH_IGAIN 0.
141 #endif
142 #ifndef H_CTL_PITCH_KFFA
143 #define H_CTL_PITCH_KFFA 0.
144 #endif
145 #ifndef H_CTL_PITCH_KFFD
146 #define H_CTL_PITCH_KFFD 0.
147 #endif
148 
149 void h_ctl_init( void ) {
153  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
155  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
156 
158 
159  h_ctl_roll_setpoint = 0.;
160  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
163  h_ctl_roll_sum_err = 0;
167 #ifdef H_CTL_AILERON_OF_THROTTLE
168  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
169 #endif
170 
173  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
176  h_ctl_pitch_sum_err = 0.;
180  h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
181 #ifdef H_CTL_PITCH_OF_ROLL
182  h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
183 #endif
184 
186  airspeed_ratio2 = 1.;
187 
188 #if USE_PITCH_TRIM
189  v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
190  v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
191 #else
194 #endif
195 
196 }
197 
202 void h_ctl_course_loop ( void ) {
203  static float last_err;
204 
205  // Ground path error
207  NormRadAngle(err);
208 
209  float d_err = err - last_err;
210  last_err = err;
211  NormRadAngle(d_err);
212 
213  float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
214  Bound(speed_depend_nav, 0.66, 1.5);
215 
217  + h_ctl_course_pgain * speed_depend_nav * err
218  + h_ctl_course_dgain * d_err;
219 
221 }
222 
223 #if USE_AIRSPEED
224 static inline void compute_airspeed_ratio( void ) {
225  if (use_airspeed_ratio) {
226  // low pass airspeed
227  static float airspeed = 0.;
228  airspeed = ( 15*airspeed + estimator_airspeed ) / 16;
229  // compute ratio
230  float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
231  Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
232  airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
233  } else {
234  airspeed_ratio2 = 1.;
235  }
236 }
237 #endif
238 
239 void h_ctl_attitude_loop ( void ) {
240  if (!h_ctl_disabled) {
241 #if USE_AIRSPEED
242  compute_airspeed_ratio();
243 #endif
244  h_ctl_roll_loop();
246  }
247 }
248 
249 #define H_CTL_REF_DT (1./60.)
250 #define KFFA_UPDATE 0.1
251 #define KFFD_UPDATE 0.05
252 
253 inline static void h_ctl_roll_loop( void ) {
254 
255  static float cmd_fb = 0.;
256 
257 #if USE_ANGLE_REF
258  // Update reference setpoints for roll
262  // Saturation on references
267  }
268  else if (h_ctl_ref_roll_rate < - H_CTL_REF_MAX_P) {
271  }
272 #else
274  h_ctl_ref_roll_rate = 0.;
276 #endif
277 
278 #ifdef USE_KFF_UPDATE
279  // update Kff gains
282 #ifdef SITL
283  printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
284 #endif
287 #endif
288 
289  // Compute errors
290  float err = h_ctl_ref_roll_angle - estimator_phi;
291 #ifdef SITL
292  static float last_err = 0;
293  estimator_p = (err - last_err)/(1/60.);
294  last_err = err;
295 #endif
296  float d_err = h_ctl_ref_roll_rate - estimator_p;
297 
298  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
299  h_ctl_roll_sum_err = 0.;
300  }
301  else {
302  if (h_ctl_roll_igain > 0.) {
305  } else {
306  h_ctl_roll_sum_err = 0.;
307  }
308  }
309 
310  cmd_fb = h_ctl_roll_attitude_gain * err;
311  float cmd = - h_ctl_roll_Kffa * h_ctl_ref_roll_accel
313  - cmd_fb
314  - h_ctl_roll_rate_gain * d_err
317 
318  cmd /= airspeed_ratio2;
319 
320  // Set aileron commands
322 }
323 
324 
325 #if USE_PITCH_TRIM
326 inline static void loiter(void) {
327  float pitch_trim;
328 
329 #if USE_AIRSPEED
330  if (estimator_airspeed > NOMINAL_AIRSPEED) {
332  } else {
334  }
335 #else
337  if (throttle_diff > 0) {
338  float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
339  pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
340  } else {
341  float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
342  pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
343  }
344 #endif
345 
346  h_ctl_pitch_loop_setpoint += pitch_trim;
347 }
348 #endif
349 
350 
351 inline static void h_ctl_pitch_loop( void ) {
352 #if !USE_GYRO_PITCH_RATE
353  static float last_err;
354 #endif
355 
356  /* sanity check */
357  if (h_ctl_pitch_of_roll <0.)
358  h_ctl_pitch_of_roll = 0.;
359 
361 #if USE_PITCH_TRIM
362  loiter();
363 #endif
364 
365 #if USE_ANGLE_REF
366  // Update reference setpoints for pitch
370  // Saturation on references
375  }
376  else if (h_ctl_ref_pitch_rate < - H_CTL_REF_MAX_Q) {
379  }
380 #else
384 #endif
385 
386  // Compute errors
388 #if USE_GYRO_PITCH_RATE
389  float d_err = h_ctl_ref_pitch_rate - estimator_q;
390 #else // soft derivation
391  float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
392  last_err = err;
393 #endif
394 
395  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
396  h_ctl_pitch_sum_err = 0.;
397  }
398  else {
399  if (h_ctl_pitch_igain > 0.) {
402  } else {
403  h_ctl_pitch_sum_err = 0.;
404  }
405  }
406 
409  + h_ctl_pitch_pgain * err
410  + h_ctl_pitch_dgain * d_err
412 
413  cmd /= airspeed_ratio2;
414 
416 }
417 
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:107
#define H_CTL_PITCH_DGAIN
void h_ctl_course_loop(void)
bool_t launch
Definition: sim_ap.c:41
#define H_CTL_ROLL_IGAIN
float estimator_theta
pitch angle in rad, + = up
Definition: estimator.c:51
float estimator_q
Definition: estimator.c:55
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
void h_ctl_attitude_loop(void)
#define H_CTL_REF_W
float v_ctl_pitch_loiter_trim
uint8_t pprz_mode
Definition: main_ap.c:111
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:134
float h_ctl_course_setpoint
fixed wing horizontal adaptive control
#define H_CTL_REF_DT
float estimator_phi
roll angle in rad, + = right
Definition: estimator.c:49
void h_ctl_init(void)
float h_ctl_roll_igain
float v_ctl_pitch_dash_trim
float h_ctl_roll_Kffa
#define AIRSPEED_RATIO_MAX
int16_t pprz_t
Definition: paparazzi.h:6
#define H_CTL_REF_MAX_P_DOT
#define H_CTL_PITCH_KFFA
pprz_t h_ctl_elevator_setpoint
pprz_t h_ctl_aileron_setpoint
float h_ctl_roll_Kffd
float estimator_p
Definition: estimator.c:54
#define H_CTL_ROLL_KFFA
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
#define KFFD_UPDATE
#define FALSE
Definition: imu_chimu.h:141
float h_ctl_ref_roll_angle
#define H_CTL_PITCH_SUM_ERR_MAX
#define H_CTL_REF_MAX_P
float h_ctl_ref_pitch_accel
#define Max(x, y)
float h_ctl_aileron_of_throttle
float h_ctl_pitch_igain
#define H_CTL_REF_MAX_Q_DOT
#define H_CTL_ROLL_RATE_GAIN
float estimator_airspeed
m/s
Definition: estimator.c:69
#define H_CTL_REF_XI
float h_ctl_roll_slew
float h_ctl_pitch_loop_setpoint
#define H_CTL_COURSE_DGAIN
#define H_CTL_REF_MAX_Q
static void h_ctl_pitch_loop(void)
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
float h_ctl_pitch_setpoint
#define H_CTL_PITCH_IGAIN
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_ref_pitch_angle
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
State estimation, fusioning sensors.
#define H_CTL_PITCH_KFFD
arch independent LED (Light Emitting Diodes) API
float h_ctl_ref_pitch_rate
float h_ctl_roll_attitude_gain
float h_ctl_roll_pgain
#define H_CTL_ROLL_KFFD
static void h_ctl_roll_loop(void)
#define MAX_PPRZ
Definition: paparazzi.h:8
float airspeed_ratio2
#define PPRZ_MODE_MANUAL
Definition: autopilot.h:44
float h_ctl_ref_roll_accel
float h_ctl_ref_roll_rate
bool_t h_ctl_disabled
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float h_ctl_pitch_Kffd
float h_ctl_pitch_Kffa
float h_ctl_roll_rate_gain
float h_ctl_roll_sum_err
fixed wing horizontal adaptive control
#define AIRSPEED_RATIO_MIN
float h_ctl_pitch_pgain