Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
68 #include "std.h"
69 #include "led.h"
72 #include "state.h"
74 #include "generated/airframe.h"
75 #include CTRL_TYPE_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,
279  &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
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,
285  &h_ctl_roll_sum_err,
286  &h_ctl_roll_setpoint,
288  &(stateGetNedToBodyEulers_f()->phi),
289  &h_ctl_aileron_setpoint,
290  &h_ctl_pitch_sum_err,
291  &h_ctl_pitch_loop_setpoint,
293  &(stateGetNedToBodyEulers_f()->theta),
294  &h_ctl_elevator_setpoint);
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 
313 
314  h_ctl_roll_setpoint = 0.;
315  h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
316  h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
317  h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
318  h_ctl_roll_sum_err = 0;
319  h_ctl_roll_Kffa = H_CTL_ROLL_KFFA;
320  h_ctl_roll_Kffd = H_CTL_ROLL_KFFD;
321  h_ctl_aileron_setpoint = 0;
322 #ifdef H_CTL_AILERON_OF_THROTTLE
323  h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
324 #endif
325 
326  h_ctl_pitch_setpoint = 0.;
327  h_ctl_pitch_loop_setpoint = 0.;
328  h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
329  h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
330  h_ctl_pitch_igain = H_CTL_PITCH_IGAIN;
331  h_ctl_pitch_sum_err = 0.;
332  h_ctl_pitch_Kffa = H_CTL_PITCH_KFFA;
333  h_ctl_pitch_Kffd = H_CTL_PITCH_KFFD;
334  h_ctl_elevator_setpoint = 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
359  v_ctl_pitch_loiter_trim = 0.;
360  v_ctl_pitch_dash_trim = 0.;
361 #endif
362 
363 #if PERIODIC_TELEMETRY
364  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CALIBRATION, send_calibration);
365  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_TUNE_ROLL, send_tune_roll);
366  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_H_CTL_A, send_ctl_a);
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 
393  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
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
476  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);
477  h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p * h_ctl_ref.max_p);
478 #ifdef SITL
479  printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
480 #endif
481  h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);
482  h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);
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 (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
491  h_ctl_roll_sum_err = 0.;
492  } else {
493  if (h_ctl_roll_igain > 0.) {
494  h_ctl_roll_sum_err += err * H_CTL_REF_DT;
495  BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
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
503  - h_ctl_roll_Kffd * h_ctl_ref.roll_rate
504  - cmd_fb
505  - h_ctl_roll_rate_gain * d_err
506  - h_ctl_roll_igain * h_ctl_roll_sum_err
508 
509  cmd /= airspeed_ratio2;
510 
511  // Set aileron commands
512  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
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) {
524  pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2 - 1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
525  } else {
526  pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2 - 1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
527  }
528 #else
530  if (throttle_diff > 0) {
531  float max_diff = Max(v_ctl_auto_throttle_max_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
532  pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
533  } else {
534  float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - v_ctl_auto_throttle_min_cruise_throttle, 0.1);
535  pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
536  }
537 #endif
538 
539  // Pitch trim rate limiter
540  float max_change = (v_ctl_pitch_loiter_trim - v_ctl_pitch_dash_trim) * H_CTL_REF_DT / 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 
561  h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabsf(stateGetNedToBodyEulers_f()->phi);
562 #if USE_PITCH_TRIM
563  loiter();
564 #endif
565 
566 #if USE_ANGLE_REF
567  // Update reference setpoints for pitch
570  h_ctl_ref.pitch_accel = H_CTL_REF_W_Q * H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2 *
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
589  h_ctl_pitch_Kffa += KFFA_UPDATE * h_ctl_ref.pitch_accel * cmd_fb / (h_ctl_ref.max_q_dot * h_ctl_ref.max_q_dot);
590  h_ctl_pitch_Kffd += KFFD_UPDATE * h_ctl_ref.pitch_rate * cmd_fb / (h_ctl_ref.max_q * h_ctl_ref.max_q);
591 #ifdef SITL
592  printf("%f %f %f\n", h_ctl_pitch_Kffa, h_ctl_pitch_Kffd, cmd_fb);
593 #endif
594  h_ctl_pitch_Kffa = Max(h_ctl_pitch_Kffa, 0);
595  h_ctl_pitch_Kffd = Max(h_ctl_pitch_Kffd, 0);
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 
607  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
608  h_ctl_pitch_sum_err = 0.;
609  } else {
610  if (h_ctl_pitch_igain > 0.) {
611  h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
612  BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
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
620  - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate
621  + cmd_fb
622  + h_ctl_pitch_dgain * d_err
623  + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
624 
625  cmd /= airspeed_ratio2;
626 
627  h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
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 (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
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
732  if (pprz_mode == PPRZ_MODE_MANUAL) {
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 
#define H_CTL_REF_XI_P
#define H_CTL_REF_XI_Q
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:104
#define H_CTL_PITCH_DGAIN
void h_ctl_course_loop(void)
bool_t launch
Definition: sim_ap.c:39
#define H_CTL_ROLL_IGAIN
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
float phi
in radians
Generic transmission transport header.
Definition: transport.h:89
void h_ctl_attitude_loop(void)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
float v_ctl_pitch_loiter_trim
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1090
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:41
#define PITCH_TRIM_RATE_LIMITER
#define AIRSPEED_RATIO_MAX
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
float r
in rad/s
int16_t pprz_t
Definition: paparazzi.h:6
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1310
#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 q
in rad/s
float p
in rad/s
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
float h_ctl_roll_Kffd
#define H_CTL_ROLL_KFFA
float v_ctl_auto_throttle_max_cruise_throttle
Definition: guidance_v.c:58
#define KFFD_UPDATE
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
#define FALSE
Definition: std.h:5
#define H_CTL_PITCH_SUM_ERR_MAX
Fixed wing horizontal control.
float theta
in radians
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
#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
#define H_CTL_REF_W_Q
#define H_CTL_ROLL_RATE_GAIN
#define ACCEL_FLOAT_OF_BFP(_ai)
#define STALL_AIRSPEED
Definition: energy_ctrl.c:150
float h_ctl_roll_slew
float h_ctl_pitch_loop_setpoint
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define H_CTL_COURSE_DGAIN
#define H_CTL_REF_MAX_Q
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
#define H_CTL_REF_W_P
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
float v_ctl_auto_throttle_min_cruise_throttle
Definition: guidance_v.c:57
static void h_ctl_pitch_loop(void)
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1171
float h_ctl_roll_max_setpoint
#define H_CTL_ROLL_SUM_ERR_MAX
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
float h_ctl_pitch_dgain
bool_t use_airspeed_ratio
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:921
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:103
float h_ctl_pitch_setpoint
#define H_CTL_PITCH_IGAIN
API to get/set the generic vehicle states.
vector in North East Down coordinates
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]
rotation matrix
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
#define ACCEL_BFP_OF_REAL(_af)
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
angular rates
float h_ctl_pitch_Kffd
float h_ctl_pitch_Kffa
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
Definition: state.h:991
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
if(PrimarySpektrumState.SpektrumTimer)
Fixedwing autopilot modes.