Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
68 #include "std.h"
69 #include "led.h"
72 #include "state.h"
74 #include "generated/airframe.h"
75 #include CTRL_TYPE_H
76 #include "autopilot.h"
77 
78 
79 /* outer loop parameters */
80 float h_ctl_course_setpoint; /* rad, CW/north */
86 
87 /* roll and pitch disabling */
89 
90 /* AUTO1 rate mode */
92 
93 struct HCtlAdaptRef {
94  float roll_angle;
95  float roll_rate;
96  float roll_accel;
97  float pitch_angle;
98  float pitch_rate;
99  float pitch_accel;
100  float yaw_angle;
101  float yaw_rate;
102  float yaw_accel;
103 
104  float max_p;
105  float max_p_dot;
106  float max_q;
107  float max_q_dot;
108  float max_r;
109  float max_r_dot;
110 };
111 
113 
114 /* Reference generator */
115 #ifndef H_CTL_REF_W_P
116 #define H_CTL_REF_W_P 5.
117 #endif
118 #ifndef H_CTL_REF_XI_P
119 #define H_CTL_REF_XI_P 0.85
120 #endif
121 #ifndef H_CTL_REF_W_Q
122 #define H_CTL_REF_W_Q 5.
123 #endif
124 #ifndef H_CTL_REF_XI_Q
125 #define H_CTL_REF_XI_Q 0.85
126 #endif
127 #ifndef H_CTL_REF_W_R
128 #define H_CTL_REF_W_R 5.
129 #endif
130 #ifndef H_CTL_REF_XI_R
131 #define H_CTL_REF_XI_R 0.85
132 #endif
133 #ifndef H_CTL_REF_MAX_P
134 #define H_CTL_REF_MAX_P RadOfDeg(150.)
135 #endif
136 #ifndef H_CTL_REF_MAX_P_DOT
137 #define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
138 #endif
139 #ifndef H_CTL_REF_MAX_Q
140 #define H_CTL_REF_MAX_Q RadOfDeg(150.)
141 #endif
142 #ifndef H_CTL_REF_MAX_Q_DOT
143 #define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
144 #endif
145 
146 #if USE_ANGLE_REF
147 PRINT_CONFIG_MSG("USING ATTITUDE REFERENCE GENERATOR")
148 PRINT_CONFIG_VAR(H_CTL_REF_W_P)
149 PRINT_CONFIG_VAR(H_CTL_REF_W_Q)
150 PRINT_CONFIG_VAR(H_CTL_REF_MAX_P)
151 PRINT_CONFIG_VAR(H_CTL_REF_MAX_P_DOT)
152 PRINT_CONFIG_VAR(H_CTL_REF_MAX_Q)
153 PRINT_CONFIG_VAR(H_CTL_REF_MAX_Q_DOT)
154 #endif
155 
156 /* inner roll loop parameters */
166 
168 
169 /* inner pitch loop parameters */
179 
180 /* inner yaw loop parameters */
181 #if H_CTL_YAW_LOOP
182 float h_ctl_yaw_rate_setpoint;
183 float h_ctl_yaw_dgain;
184 float h_ctl_yaw_ny_igain;
185 float h_ctl_yaw_ny_sum_err;
186 pprz_t h_ctl_rudder_setpoint;
187 #endif
188 
189 /* inner CL loop parameters */
190 #if H_CTL_CL_LOOP
191 pprz_t h_ctl_flaps_setpoint;
192 #endif
193 
194 /* inner loop pre-command */
197 float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
198 
201 #define AIRSPEED_RATIO_MIN 0.5
202 #define AIRSPEED_RATIO_MAX 2.
203 
206 
207 // Pitch trim rate limiter
208 #ifndef PITCH_TRIM_RATE_LIMITER
209 #define PITCH_TRIM_RATE_LIMITER 3.
210 #endif
211 inline static void h_ctl_roll_loop(void);
212 inline static void h_ctl_pitch_loop(void);
213 #if H_CTL_YAW_LOOP
214 inline static void h_ctl_yaw_loop(void);
215 #endif
216 #if H_CTL_CL_LOOP
217 inline static void h_ctl_cl_loop(void);
218 #endif
219 
220 // Some default course gains
221 // H_CTL_COURSE_PGAIN needs to be define in airframe
222 #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION
223 #define H_CTL_COURSE_PRE_BANK_CORRECTION 1.
224 #endif
225 #ifndef H_CTL_COURSE_DGAIN
226 #define H_CTL_COURSE_DGAIN 0.
227 #endif
228 
229 // Some default roll gains
230 // H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe
231 #ifndef H_CTL_ROLL_RATE_GAIN
232 #define H_CTL_ROLL_RATE_GAIN 0.
233 #endif
234 #ifndef H_CTL_ROLL_IGAIN
235 #define H_CTL_ROLL_IGAIN 0.
236 #endif
237 #ifndef H_CTL_ROLL_KFFA
238 #define H_CTL_ROLL_KFFA 0.
239 #endif
240 #ifndef H_CTL_ROLL_KFFD
241 #define H_CTL_ROLL_KFFD 0.
242 #endif
243 
244 // Some default pitch gains
245 // H_CTL_PITCH_PGAIN needs to be define in airframe
246 #ifndef H_CTL_PITCH_DGAIN
247 #define H_CTL_PITCH_DGAIN 0.
248 #endif
249 #ifndef H_CTL_PITCH_IGAIN
250 #define H_CTL_PITCH_IGAIN 0.
251 #endif
252 #ifndef H_CTL_PITCH_KFFA
253 #define H_CTL_PITCH_KFFA 0.
254 #endif
255 #ifndef H_CTL_PITCH_KFFD
256 #define H_CTL_PITCH_KFFD 0.
257 #endif
258 
259 // H_CTL_YAW_GAINS to be defined in airframe
260 #ifndef H_CTL_YAW_KFFD
261 #define H_CTL_YAW_KFFD 0.
262 #endif
263 
264 #ifndef USE_GYRO_PITCH_RATE
265 #define USE_GYRO_PITCH_RATE TRUE
266 #endif
267 
268 #if PERIODIC_TELEMETRY
270 
271 static void send_calibration(struct transport_tx *trans, struct link_device *dev)
272 {
273  pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode);
274 }
275 
276 static void send_tune_roll(struct transport_tx *trans, struct link_device *dev)
277 {
278  pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID,
280 }
281 
282 static void send_ctl_a(struct transport_tx *trans, struct link_device *dev)
283 {
284  pprz_msg_send_H_CTL_A(trans, dev, AC_ID,
288  &(stateGetNedToBodyEulers_f()->phi),
293  &(stateGetNedToBodyEulers_f()->theta),
295 }
296 #endif
297 
298 void h_ctl_init(void)
299 {
304 
308  h_ctl_course_pgain = H_CTL_COURSE_PGAIN;
310  h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT;
311 
312  h_ctl_disabled = false;
313 
314  h_ctl_roll_setpoint = 0.;
315  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
318  h_ctl_roll_sum_err = 0;
322 #ifdef H_CTL_AILERON_OF_THROTTLE
323  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
324 #endif
325 
328  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
331  h_ctl_pitch_sum_err = 0.;
335 
336 #if H_CTL_YAW_LOOP
337  h_ctl_yaw_dgain = H_CTL_YAW_DGAIN;
338  h_ctl_yaw_ny_igain = H_CTL_YAW_NY_IGAIN;
339  h_ctl_yaw_ny_sum_err = 0.;
340  h_ctl_rudder_setpoint = 0;
341 #endif
342 
343 #if H_CTL_CL_LOOP
344  h_ctl_flaps_setpoint = 0;
345 #endif
346 
347  h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
348 #ifdef H_CTL_PITCH_OF_ROLL
349  h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
350 #endif
351 
352  use_airspeed_ratio = false;
353  airspeed_ratio2 = 1.;
354 
355 #if USE_PITCH_TRIM
356  v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
357  v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
358 #else
361 #endif
362 
363 #if PERIODIC_TELEMETRY
367 #endif
368 }
369 
375 {
376  static float last_err;
377 
378  // Ground path error
380  NormRadAngle(err);
381 
382  float d_err = err - last_err;
383  last_err = err;
384  NormRadAngle(d_err);
385 
386  float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
387  Bound(speed_depend_nav, 0.66, 1.5);
388 
390  + h_ctl_course_pgain * speed_depend_nav * err
391  + h_ctl_course_dgain * d_err;
392 
394 }
395 
396 #if USE_AIRSPEED
397 static inline void compute_airspeed_ratio(void)
398 {
399  if (use_airspeed_ratio) {
400  // low pass airspeed
401  static float airspeed = 0.;
402  airspeed = (15 * airspeed + stateGetAirspeed_f()) / 16;
403  // compute ratio
404  float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
405  Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
406  airspeed_ratio2 = airspeed_ratio * airspeed_ratio;
407  } else {
408  airspeed_ratio2 = 1.;
409  }
410 }
411 #endif
412 
414 {
415  if (!h_ctl_disabled) {
416 #if USE_AIRSPEED
417  compute_airspeed_ratio();
418 #endif
419  h_ctl_roll_loop();
421 #if H_CTL_YAW_LOOP
422  h_ctl_yaw_loop();
423 #endif
424 #if H_CTL_CL_LOOP
425  h_ctl_cl_loop();
426 #endif
427  }
428 }
429 
434 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
435 #define H_CTL_REF_DT (1./AHRS_PROPAGATE_FREQUENCY)
436 #else
437 #define H_CTL_REF_DT (1./CONTROL_FREQUENCY)
438 #endif
439 
445 #define KFFA_UPDATE 0.1
446 #define KFFD_UPDATE 0.05
447 
448 inline static void h_ctl_roll_loop(void)
449 {
450 
451  static float cmd_fb = 0.;
452 
453 #if USE_ANGLE_REF
454  // Update reference setpoints for roll
459  // Saturation on references
463  if (h_ctl_ref.roll_accel > 0.) { h_ctl_ref.roll_accel = 0.; }
464  } else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {
466  if (h_ctl_ref.roll_accel < 0.) { h_ctl_ref.roll_accel = 0.; }
467  }
468 #else
470  h_ctl_ref.roll_rate = 0.;
471  h_ctl_ref.roll_accel = 0.;
472 #endif
473 
474 #ifdef USE_KFF_UPDATE_ROLL
475  // update Kff gains
478 #ifdef SITL
479  printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
480 #endif
483 #endif
484 
485  // Compute errors
487  struct FloatRates *body_rate = stateGetBodyRates_f();
488  float d_err = h_ctl_ref.roll_rate - body_rate->p;
489 
490  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
491  h_ctl_roll_sum_err = 0.;
492  } else {
493  if (h_ctl_roll_igain > 0.) {
496  } else {
497  h_ctl_roll_sum_err = 0.;
498  }
499  }
500 
501  cmd_fb = h_ctl_roll_attitude_gain * err;
502  float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel
504  - cmd_fb
505  - h_ctl_roll_rate_gain * d_err
508 
509  cmd /= airspeed_ratio2;
510 
511  // Set aileron commands
513 }
514 
515 
516 #if USE_PITCH_TRIM
517 inline static void loiter(void)
518 {
519  float pitch_trim;
520  static float last_pitch_trim;
521 
522 #if USE_AIRSPEED
523  if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) {
525  } else {
527  }
528 #else
530  if (throttle_diff > 0) {
532  pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
533  } else {
535  pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
536  }
537 #endif
538 
539  // Pitch trim rate limiter
541  Bound(pitch_trim, last_pitch_trim - max_change, last_pitch_trim + max_change);
542 
543  last_pitch_trim = pitch_trim;
544  h_ctl_pitch_loop_setpoint += pitch_trim;
545 }
546 #endif
547 
548 
549 inline static void h_ctl_pitch_loop(void)
550 {
551  static float cmd_fb = 0.;
552 #if !USE_GYRO_PITCH_RATE
553  static float last_err;
554 #endif
555 
556  /* sanity check */
557  if (h_ctl_pitch_of_roll < 0.) {
558  h_ctl_pitch_of_roll = 0.;
559  }
560 
562 #if USE_PITCH_TRIM
563  loiter();
564 #endif
565 
566 #if USE_ANGLE_REF
567  // Update reference setpoints for pitch
572  // Saturation on references
576  if (h_ctl_ref.pitch_accel > 0.) { h_ctl_ref.pitch_accel = 0.; }
577  } else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {
579  if (h_ctl_ref.pitch_accel < 0.) { h_ctl_ref.pitch_accel = 0.; }
580  }
581 #else
583  h_ctl_ref.pitch_rate = 0.;
584  h_ctl_ref.pitch_accel = 0.;
585 #endif
586 
587 #ifdef USE_KFF_UPDATE_PITCH
588  // update Kff gains
591 #ifdef SITL
592  printf("%f %f %f\n", h_ctl_pitch_Kffa, h_ctl_pitch_Kffd, cmd_fb);
593 #endif
596 #endif
597 
598  // Compute errors
600 #if USE_GYRO_PITCH_RATE
601  float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;
602 #else // soft derivation
603  float d_err = (err - last_err) / H_CTL_REF_DT - h_ctl_ref.pitch_rate;
604  last_err = err;
605 #endif
606 
608  h_ctl_pitch_sum_err = 0.;
609  } else {
610  if (h_ctl_pitch_igain > 0.) {
613  } else {
614  h_ctl_pitch_sum_err = 0.;
615  }
616  }
617 
618  cmd_fb = h_ctl_pitch_pgain * err;
619  float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel
621  + cmd_fb
622  + h_ctl_pitch_dgain * d_err
624 
625  cmd /= airspeed_ratio2;
626 
628 }
629 
630 /* yaw damper */
631 #if H_CTL_YAW_LOOP
632 inline static void h_ctl_yaw_loop(void)
633 {
634 
635 #if H_CTL_YAW_TRIM_NY
636  // Actual Acceleration from IMU:
637 #if (!defined SITL || defined USE_NPS)
638  struct Int32Vect3 accel_meas_body, accel_ned;
639  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
640  struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
641  VECT3_COPY(accel_ned, (*accel_tmp));
642  accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
643  int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
644  float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g)
645 #else
646  float ny = 0.f;
647 #endif
648 
649  if (autopilot_get_mode() == AP_MODE_MANUAL || autopilot.launch == false) {
650  h_ctl_yaw_ny_sum_err = 0.;
651  } else {
652  if (h_ctl_yaw_ny_igain > 0.) {
653  // only update when: phi<60degrees and ny<2g
654  if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) {
655  h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT;
656  // max half rudder deflection for trim
657  BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain));
658  }
659  } else {
660  h_ctl_yaw_ny_sum_err = 0.;
661  }
662  }
663 #endif
664 
665 #ifdef USE_AIRSPEED
666  float Vo = stateGetAirspeed_f();
667  Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED);
668 #else
669  float Vo = NOMINAL_AIRSPEED;
670 #endif
671 
672  h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC
673  + 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns
674  float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r;
675 
676  float cmd = + h_ctl_yaw_dgain * d_err
677 #if H_CTL_YAW_TRIM_NY
678  + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
679 #endif
680  ;
681  cmd /= airspeed_ratio2;
682  h_ctl_rudder_setpoint = TRIM_PPRZ(cmd);
683 }
684 #endif
685 
686 /* CL with Flaps - direct lift control */
687 #if H_CTL_CL_LOOP
688 inline static void h_ctl_cl_loop(void)
689 {
690 
691 #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
692 #if (!defined SITL || defined USE_NPS)
693  struct Int32Vect3 accel_meas_body, accel_ned;
694  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
695  struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
696  VECT3_COPY(accel_ned, (*accel_tmp));
697  accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
698  int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
699  float nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f;
700  // max load factor to be taken into acount
701  // to prevent negative flap movement du to negative acceleration
702  Bound(nz, 1.f, 2.f);
703 #else
704  float nz = 1.f;
705 #endif
706 #endif
707 
708  // Compute a corrected airspeed corresponding to the current load factor nz
709  // with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V,
710  // the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2,
711  // thus Vn = V / sqrt(nz)
712 #if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
713  float corrected_airspeed = v_ctl_auto_airspeed_setpoint;
714 #else
715  float corrected_airspeed = stateGetAirspeed_f();
716 #endif
717 #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
718  corrected_airspeed /= sqrtf(nz);
719 #endif
720  Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED);
721 
722  float cmd = 0.f;
723  // deadband around NOMINAL_AIRSPEED, rest linear
724  if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) {
725  cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED);
726  }
727  else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) {
728  cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED);
729  }
730 
731  // no control in manual mode
733  cmd = 0.f;
734  }
735  // bound max flap angle
736  Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL);
737  // from percent to pprz
738  cmd = cmd * MAX_PPRZ;
739  h_ctl_flaps_setpoint = TRIM_PPRZ(cmd);
740 }
741 #endif
742 
use_airspeed_ratio
bool use_airspeed_ratio
Definition: stabilization_adaptive.c:199
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
HCtlAdaptRef::roll_angle
float roll_angle
Definition: stabilization_adaptive.c:94
AIRSPEED_RATIO_MIN
#define AIRSPEED_RATIO_MIN
Definition: stabilization_adaptive.c:201
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
airspeed_ratio2
float airspeed_ratio2
Definition: stabilization_adaptive.c:200
H_CTL_PITCH_DGAIN
#define H_CTL_PITCH_DGAIN
Definition: stabilization_adaptive.c:247
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
H_CTL_REF_W_Q
#define H_CTL_REF_W_Q
Definition: stabilization_adaptive.c:122
h_ctl_roll_sum_err
float h_ctl_roll_sum_err
Definition: stabilization_adaptive.c:161
h_ctl_roll_slew
float h_ctl_roll_slew
Definition: stabilization_adaptive.c:165
H_CTL_ROLL_KFFA
#define H_CTL_ROLL_KFFA
Definition: stabilization_adaptive.c:238
H_CTL_REF_W_P
#define H_CTL_REF_W_P
Definition: stabilization_adaptive.c:116
h_ctl_course_pre_bank_correction
float h_ctl_course_pre_bank_correction
Definition: stabilization_adaptive.c:82
h_ctl_course_dgain
float h_ctl_course_dgain
Definition: stabilization_adaptive.c:84
KFFD_UPDATE
#define KFFD_UPDATE
Definition: stabilization_adaptive.c:446
H_CTL_REF_DT
#define H_CTL_REF_DT
Define reference generator time step default to control frequency and ahrs propagation freq if contro...
Definition: stabilization_adaptive.c:437
v_ctl_auto_throttle_max_cruise_throttle
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:59
H_CTL_COURSE_PRE_BANK_CORRECTION
#define H_CTL_COURSE_PRE_BANK_CORRECTION
Definition: stabilization_adaptive.c:223
h_ctl_roll_Kffa
float h_ctl_roll_Kffa
Definition: stabilization_adaptive.c:162
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
stateGetBodyRates_f
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
h_ctl_roll_attitude_gain
float h_ctl_roll_attitude_gain
Definition: stabilization_adaptive.c:158
HCtlAdaptRef::yaw_angle
float yaw_angle
Definition: stabilization_adaptive.c:100
H_CTL_PITCH_SUM_ERR_MAX
#define H_CTL_PITCH_SUM_ERR_MAX
Definition: stabilization_adaptive.h:54
h_ctl_roll_max_setpoint
float h_ctl_roll_max_setpoint
Definition: stabilization_adaptive.c:85
HCtlAdaptRef::max_q
float max_q
Definition: stabilization_adaptive.c:106
h_ctl_roll_loop
static void h_ctl_roll_loop(void)
Definition: stabilization_adaptive.c:448
stabilization_attitude.h
h_ctl_pitch_igain
float h_ctl_pitch_igain
Definition: stabilization_adaptive.c:174
h_ctl_disabled
bool h_ctl_disabled
Definition: stabilization_adaptive.c:88
v_ctl_auto_throttle_min_cruise_throttle
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:58
H_CTL_PITCH_IGAIN
#define H_CTL_PITCH_IGAIN
Definition: stabilization_adaptive.c:250
h_ctl_course_pgain
float h_ctl_course_pgain
Definition: stabilization_adaptive.c:83
h_ctl_pitch_sum_err
float h_ctl_pitch_sum_err
Definition: stabilization_adaptive.c:175
h_ctl_elevator_of_roll
float h_ctl_elevator_of_roll
Definition: stabilization_adaptive.c:196
h_ctl_roll_setpoint
float h_ctl_roll_setpoint
Definition: stabilization_adaptive.c:157
h_ctl_pitch_of_roll
float h_ctl_pitch_of_roll
Definition: stabilization_adaptive.c:197
h_ctl_ref
struct HCtlAdaptRef h_ctl_ref
Definition: stabilization_adaptive.c:112
v_ctl_auto_throttle_cruise_throttle
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
AIRSPEED_RATIO_MAX
#define AIRSPEED_RATIO_MAX
Definition: stabilization_adaptive.c:202
HCtlAdaptRef::pitch_angle
float pitch_angle
Definition: stabilization_adaptive.c:97
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
h_ctl_pitch_loop_setpoint
float h_ctl_pitch_loop_setpoint
Definition: stabilization_adaptive.c:171
H_CTL_PITCH_KFFD
#define H_CTL_PITCH_KFFD
Definition: stabilization_adaptive.c:256
telemetry.h
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
H_CTL_REF_MAX_P_DOT
#define H_CTL_REF_MAX_P_DOT
Definition: stabilization_adaptive.c:137
std.h
h_ctl_pitch_pgain
float h_ctl_pitch_pgain
Definition: stabilization_adaptive.c:172
H_CTL_REF_MAX_Q_DOT
#define H_CTL_REF_MAX_Q_DOT
Definition: stabilization_adaptive.c:143
HCtlAdaptRef::yaw_rate
float yaw_rate
Definition: stabilization_adaptive.c:101
pprz_t
int16_t pprz_t
Definition: paparazzi.h:6
ACCEL_FLOAT_OF_BFP
#define ACCEL_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:222
h_ctl_course_loop
void h_ctl_course_loop(void)
Definition: stabilization_adaptive.c:374
H_CTL_REF_XI_P
#define H_CTL_REF_XI_P
Definition: stabilization_adaptive.c:119
H_CTL_REF_XI_Q
#define H_CTL_REF_XI_Q
Definition: stabilization_adaptive.c:125
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
H_CTL_ROLL_RATE_GAIN
#define H_CTL_ROLL_RATE_GAIN
Definition: stabilization_adaptive.c:232
H_CTL_ROLL_KFFD
#define H_CTL_ROLL_KFFD
Definition: stabilization_adaptive.c:241
stabilization_adaptive.h
HCtlAdaptRef::pitch_accel
float pitch_accel
Definition: stabilization_adaptive.c:99
v_ctl_auto_throttle_nominal_cruise_throttle
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:104
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
h_ctl_aileron_of_throttle
float h_ctl_aileron_of_throttle
Definition: stabilization_adaptive.c:195
h_ctl_attitude_loop
void h_ctl_attitude_loop(void)
Definition: stabilization_adaptive.c:413
HCtlAdaptRef::max_q_dot
float max_q_dot
Definition: stabilization_adaptive.c:107
H_CTL_REF_MAX_Q
#define H_CTL_REF_MAX_Q
Definition: stabilization_adaptive.c:140
v_ctl_pitch_loiter_trim
float v_ctl_pitch_loiter_trim
Definition: stabilization_adaptive.c:204
Int32Vect3
Definition: pprz_algebra_int.h:88
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:221
h_ctl_pitch_Kffa
float h_ctl_pitch_Kffa
Definition: stabilization_adaptive.c:176
HCtlAdaptRef::pitch_rate
float pitch_rate
Definition: stabilization_adaptive.c:98
send_ctl_a
static void send_ctl_a(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_adaptive.c:282
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
v_ctl_pitch_dash_trim
float v_ctl_pitch_dash_trim
Definition: stabilization_adaptive.c:205
h_ctl_pitch_Kffd
float h_ctl_pitch_Kffd
Definition: stabilization_adaptive.c:177
HCtlAdaptRef::max_r
float max_r
Definition: stabilization_adaptive.c:108
PITCH_TRIM_RATE_LIMITER
#define PITCH_TRIM_RATE_LIMITER
Definition: stabilization_adaptive.c:209
H_CTL_COURSE_DGAIN
#define H_CTL_COURSE_DGAIN
Definition: stabilization_adaptive.c:226
h_ctl_roll_rate_gain
float h_ctl_roll_rate_gain
Definition: stabilization_adaptive.c:159
if
if(GpsFixValid() &&e_identification_started)
Definition: e_identification_fr.c:159
autopilot.h
H_CTL_REF_MAX_P
#define H_CTL_REF_MAX_P
Definition: stabilization_adaptive.c:134
H_CTL_ROLL_SUM_ERR_MAX
#define H_CTL_ROLL_SUM_ERR_MAX
Definition: stabilization_adaptive.h:53
led.h
arch independent LED (Light Emitting Diodes) API
HCtlAdaptRef::roll_rate
float roll_rate
Definition: stabilization_adaptive.c:95
HCtlAdaptRef::yaw_accel
float yaw_accel
Definition: stabilization_adaptive.c:102
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
H_CTL_ROLL_IGAIN
#define H_CTL_ROLL_IGAIN
Definition: stabilization_adaptive.c:235
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
HCtlAdaptRef::roll_accel
float roll_accel
Definition: stabilization_adaptive.c:96
h_ctl_course_pre_bank
float h_ctl_course_pre_bank
Definition: stabilization_adaptive.c:81
KFFA_UPDATE
#define KFFA_UPDATE
Adaptive control tuning parameters activate with USE_KFF_UPDATE.
Definition: stabilization_adaptive.c:445
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
int32_rmat_vmult
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_int.c:107
v_ctl_throttle_setpoint
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
nav.h
H_CTL_PITCH_KFFA
#define H_CTL_PITCH_KFFA
Definition: stabilization_adaptive.c:253
h_ctl_roll_Kffd
float h_ctl_roll_Kffd
Definition: stabilization_adaptive.c:163
h_ctl_pitch_dgain
float h_ctl_pitch_dgain
Definition: stabilization_adaptive.c:173
h_ctl_elevator_setpoint
pprz_t h_ctl_elevator_setpoint
Definition: stabilization_adaptive.c:178
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
HCtlAdaptRef::max_r_dot
float max_r_dot
Definition: stabilization_adaptive.c:109
send_calibration
static void send_calibration(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_adaptive.c:271
h_ctl_roll_igain
float h_ctl_roll_igain
Definition: stabilization_adaptive.c:160
h_ctl_init
void h_ctl_init(void)
Definition: stabilization_adaptive.c:298
h_ctl_pitch_loop
static void h_ctl_pitch_loop(void)
Definition: stabilization_adaptive.c:549
HCtlAdaptRef
Definition: stabilization_adaptive.c:93
send_tune_roll
static void send_tune_roll(struct transport_tx *trans, struct link_device *dev)
Definition: stabilization_adaptive.c:276
v_ctl_auto_airspeed_setpoint
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
stateGetAccelNed_i
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:1020
HCtlAdaptRef::max_p_dot
float max_p_dot
Definition: stabilization_adaptive.c:105
AP_MODE_MANUAL
#define AP_MODE_MANUAL
AP modes.
Definition: autopilot_static.h:36
h_ctl_pitch_setpoint
float h_ctl_pitch_setpoint
Definition: stabilization_adaptive.c:170
h_ctl_aileron_setpoint
pprz_t h_ctl_aileron_setpoint
Definition: stabilization_adaptive.c:164
TRIM_PPRZ
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:216
state.h
h_ctl_roll_pgain
float h_ctl_roll_pgain
Definition: stabilization_adaptive.c:167
v_ctl_auto_throttle_submode
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
v_ctl_auto_throttle_sum_err
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
p
static float p[2][2]
Definition: ins_alt_float.c:257
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
HCtlAdaptRef::max_p
float max_p
Definition: stabilization_adaptive.c:104
STALL_AIRSPEED
#define STALL_AIRSPEED
Definition: energy_ctrl.c:148
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
h_ctl_course_setpoint
float h_ctl_course_setpoint
Definition: stabilization_adaptive.c:80
h_ctl_auto1_rate
bool h_ctl_auto1_rate
Definition: stabilization_adaptive.c:91