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_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"
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 // Some default roll gains
118 // H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe
119 #ifndef H_CTL_ROLL_RATE_GAIN
120 #define H_CTL_ROLL_RATE_GAIN 0.
121 #endif
122 #ifndef H_CTL_ROLL_IGAIN
123 #define H_CTL_ROLL_IGAIN 0.
124 #endif
125 #ifndef H_CTL_ROLL_KFFA
126 #define H_CTL_ROLL_KFFA 0.
127 #endif
128 #ifndef H_CTL_ROLL_KFFD
129 #define H_CTL_ROLL_KFFD 0.
130 #endif
131 
132 // Some default pitch gains
133 // H_CTL_PITCH_PGAIN needs to be define in airframe
134 #ifndef H_CTL_PITCH_DGAIN
135 #define H_CTL_PITCH_DGAIN 0.
136 #endif
137 #ifndef H_CTL_PITCH_IGAIN
138 #define H_CTL_PITCH_IGAIN 0.
139 #endif
140 #ifndef H_CTL_PITCH_KFFA
141 #define H_CTL_PITCH_KFFA 0.
142 #endif
143 #ifndef H_CTL_PITCH_KFFD
144 #define H_CTL_PITCH_KFFD 0.
145 #endif
146 
147 void h_ctl_init( void ) {
151  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
153  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
154 
156 
157  h_ctl_roll_setpoint = 0.;
158  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
161  h_ctl_roll_sum_err = 0;
165 #ifdef H_CTL_AILERON_OF_THROTTLE
166  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
167 #endif
168 
171  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
174  h_ctl_pitch_sum_err = 0.;
178  h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
179 #ifdef H_CTL_PITCH_OF_ROLL
180  h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
181 #endif
182 
184  airspeed_ratio2 = 1.;
185 
186 #if USE_PITCH_TRIM
187  v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
188  v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
189 #else
192 #endif
193 
194 }
195 
200 void h_ctl_course_loop ( void ) {
201  static float last_err;
202 
203  // Ground path error
205  NormRadAngle(err);
206 
207  float d_err = err - last_err;
208  last_err = err;
209  NormRadAngle(d_err);
210 
211  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
212  Bound(speed_depend_nav, 0.66, 1.5);
213 
215  + h_ctl_course_pgain * speed_depend_nav * err
216  + h_ctl_course_dgain * d_err;
217 
219 }
220 
221 #if USE_AIRSPEED
222 static inline void compute_airspeed_ratio( void ) {
223  if (use_airspeed_ratio) {
224  // low pass airspeed
225  static float airspeed = 0.;
226  airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16;
227  // compute ratio
228  float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
229  Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
230  airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
231  } else {
232  airspeed_ratio2 = 1.;
233  }
234 }
235 #endif
236 
237 void h_ctl_attitude_loop ( void ) {
238  if (!h_ctl_disabled) {
239 #if USE_AIRSPEED
240  compute_airspeed_ratio();
241 #endif
242  h_ctl_roll_loop();
244  }
245 }
246 
247 #define H_CTL_REF_DT (1./60.)
248 #define KFFA_UPDATE 0.1
249 #define KFFD_UPDATE 0.05
250 
251 inline static void h_ctl_roll_loop( void ) {
252 
253  static float cmd_fb = 0.;
254 
255 #if USE_ANGLE_REF
256  // Update reference setpoints for roll
260  // Saturation on references
265  }
266  else if (h_ctl_ref_roll_rate < - H_CTL_REF_MAX_P) {
269  }
270 #else
272  h_ctl_ref_roll_rate = 0.;
274 #endif
275 
276 #ifdef USE_KFF_UPDATE
277  // update Kff gains
280 #ifdef SITL
281  printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
282 #endif
285 #endif
286 
287  // Compute errors
289  struct FloatRates* body_rate = stateGetBodyRates_f();
290 #ifdef SITL
291  static float last_err = 0;
292  body_rate->p = (err - last_err)/(1/60.); // FIXME should be done in ahrs sim
293  last_err = err;
294 #endif
295  float d_err = h_ctl_ref_roll_rate - body_rate->p;
296 
297  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
298  h_ctl_roll_sum_err = 0.;
299  }
300  else {
301  if (h_ctl_roll_igain > 0.) {
304  } else {
305  h_ctl_roll_sum_err = 0.;
306  }
307  }
308 
309  cmd_fb = h_ctl_roll_attitude_gain * err;
310  float cmd = - h_ctl_roll_Kffa * h_ctl_ref_roll_accel
312  - cmd_fb
313  - h_ctl_roll_rate_gain * d_err
316 
317  cmd /= airspeed_ratio2;
318 
319  // Set aileron commands
321 }
322 
323 
324 #if USE_PITCH_TRIM
325 inline static void loiter(void) {
326  float pitch_trim;
327 
328 #if USE_AIRSPEED
329  if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) {
331  } else {
333  }
334 #else
336  if (throttle_diff > 0) {
337  float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
338  pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
339  } else {
340  float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
341  pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
342  }
343 #endif
344 
345  h_ctl_pitch_loop_setpoint += pitch_trim;
346 }
347 #endif
348 
349 
350 inline static void h_ctl_pitch_loop( void ) {
351 #if !USE_GYRO_PITCH_RATE
352  static float last_err;
353 #endif
354 
355  /* sanity check */
356  if (h_ctl_pitch_of_roll <0.)
357  h_ctl_pitch_of_roll = 0.;
358 
360 #if USE_PITCH_TRIM
361  loiter();
362 #endif
363 
364 #if USE_ANGLE_REF
365  // Update reference setpoints for pitch
369  // Saturation on references
374  }
375  else if (h_ctl_ref_pitch_rate < - H_CTL_REF_MAX_Q) {
378  }
379 #else
383 #endif
384 
385  // Compute errors
387 #if USE_GYRO_PITCH_RATE
388  float d_err = h_ctl_ref_pitch_rate - stateGetBodyRates_f()->q;
389 #else // soft derivation
390  float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
391  last_err = err;
392 #endif
393 
394  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
395  h_ctl_pitch_sum_err = 0.;
396  }
397  else {
398  if (h_ctl_pitch_igain > 0.) {
401  } else {
402  h_ctl_pitch_sum_err = 0.;
403  }
404  }
405 
408  + h_ctl_pitch_pgain * err
409  + h_ctl_pitch_dgain * d_err
411 
412  cmd /= airspeed_ratio2;
413 
415 }
416 
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)
#define H_CTL_REF_W
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
angular rates
float h_ctl_course_setpoint
#define H_CTL_REF_DT
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:31
#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 h_ctl_roll_Kffd
#define H_CTL_ROLL_KFFA
#define KFFD_UPDATE
#define FALSE
Definition: imu_chimu.h:141
float h_ctl_ref_roll_angle
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^2
#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
float phi
in radians
#define H_CTL_ROLL_RATE_GAIN
#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)
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
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_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
#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 q
in rad/s^2
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
AP modes.
Definition: autopilot.h:51
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
Fixed wing horizontal adaptive control.
float h_ctl_roll_sum_err
#define AIRSPEED_RATIO_MIN
float h_ctl_pitch_pgain
Fixedwing autopilot modes.