Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_indi_hybrid.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
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 
30 #include "generated/airframe.h"
33 #include "state.h"
35 #include "mcu_periph/sys_time.h"
36 #include "autopilot.h"
38 #include "stdio.h"
40 #include "modules/core/abi.h"
42 
43 
44 // The acceleration reference is calculated with these gains. If you use GPS,
45 // they are probably limited by the update rate of your GPS. The default
46 // values are tuned for 4 Hz GPS updates. If you have high speed position updates, the
47 // gains can be higher, depending on the speed of the inner loop.
48 #ifndef GUIDANCE_INDI_SPEED_GAIN
49 #define GUIDANCE_INDI_SPEED_GAIN 1.8
50 #define GUIDANCE_INDI_SPEED_GAINZ 1.8
51 #endif
52 
53 #ifndef GUIDANCE_INDI_POS_GAIN
54 #define GUIDANCE_INDI_POS_GAIN 0.5
55 #define GUIDANCE_INDI_POS_GAINZ 0.5
56 #endif
57 
58 #ifndef GUIDANCE_INDI_LIFTD_ASQ
59 #define GUIDANCE_INDI_LIFTD_ASQ 0.20
60 #endif
61 
62 /* If lift effectiveness at low airspeed not defined,
63  * just make one interpolation segment that connects to
64  * the quadratic part from 12 m/s onward
65  */
66 #ifndef GUIDANCE_INDI_LIFTD_P50
67 #define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12)
68 #define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
69 #endif
70 
73  .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
74 
75  .speed_gain = GUIDANCE_INDI_SPEED_GAIN,
76  .speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
77 
78  .heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
79  .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared
80  .liftd_p80 = GUIDANCE_INDI_LIFTD_P80,
81  .liftd_p50 = GUIDANCE_INDI_LIFTD_P50,
82 };
83 
84 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
85 #error "You must have an airspeed sensor to use this guidance"
86 #endif
87 float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
88 
89 // Quadplanes can hover at various pref pitch
91 
92 
93 // If using WLS, check that the matrix size is sufficient
94 #if GUIDANCE_INDI_HYBRID_USE_WLS
95 #if GUIDANCE_INDI_HYBRID_U > WLS_N_U
96 #error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file
97 #endif
98 
99 #if GUIDANCE_INDI_HYBRID_V > WLS_N_V
100 #error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file
101 #endif
102 #endif
103 
104 
105 // Tell the guidance that the airspeed needs to be zeroed.
106 // Recomended to also put GUIDANCE_INDI_NAV_SPEED_MARGIN low in this case.
107 #ifndef GUIDANCE_INDI_ZERO_AIRSPEED
108 #define GUIDANCE_INDI_ZERO_AIRSPEED FALSE
109 #endif
110 
111 /*Airspeed threshold where making a turn is "worth it"*/
112 #ifndef TURN_AIRSPEED_TH
113 #define TURN_AIRSPEED_TH 10.0
114 #endif
115 
116 /*Boolean to force the heading to a static value (only use for specific experiments)*/
117 bool take_heading_control = false;
118 
119 bool force_forward = false;
120 
121 struct FloatVect3 sp_accel = {0.0,0.0,0.0};
122 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
123 float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
124 static void guidance_indi_filter_thrust(void);
125 
126 #ifdef GUIDANCE_INDI_THRUST_DYNAMICS
127 #warning GUIDANCE_INDI_THRUST_DYNAMICS is deprecated, use GUIDANCE_INDI_THRUST_DYNAMICS_FREQ instead.
128 #warning "The thrust dynamics are now specified in continuous time with the corner frequency of the first order model!"
129 #warning "define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ in rad/s"
130 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
131 #endif
132 
133 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
134 #ifndef STABILIZATION_INDI_ACT_FREQ_P
135 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
136 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
137 #define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
138 #endif
139 #endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
140 
141 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
142 
143 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
144 #ifdef STABILIZATION_INDI_FILT_CUTOFF
145 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
146 #else
147 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
148 #endif
149 #endif
150 
151 #ifndef GUIDANCE_INDI_CLIMB_SPEED_FWD
152 #define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0
153 #endif
154 
155 #ifndef GUIDANCE_INDI_DESCEND_SPEED_FWD
156 #define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0
157 #endif
158 
161 
162 float inv_eff[4];
163 
164 // Max bank angle in radians
167 
169 struct FloatEulers eulers_zxy;
170 
171 float thrust_dyn = 0.f;
172 float thrust_act = 0;
178 
180 
181 float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
182 struct FloatVect3 euler_cmd;
183 
184 #if GUIDANCE_INDI_HYBRID_USE_WLS
185 #include "math/wls/wls_alloc.h"
186 float du_min_gih[GUIDANCE_INDI_HYBRID_U];
187 float du_max_gih[GUIDANCE_INDI_HYBRID_U];
188 float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
189 float *Bwls_gih[GUIDANCE_INDI_HYBRID_V];
190 #ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES
191 float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES;
192 #else
193 float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel
194 #endif
195 #ifdef GUIDANCE_INDI_HYBRID_WLS_WU
196 float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_HYBRID_WLS_WU;
197 #else
198 float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f };
199 #endif
200 #endif
201 
202 // The control objective
203 float v_gih[3];
204 
205 // Filters
207 
210 float thrust_in;
211 
212 struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0};
213 
214 #ifndef GUIDANCE_INDI_VEL_SP_ID
215 #define GUIDANCE_INDI_VEL_SP_ID ABI_BROADCAST
216 #endif
218 static void vel_sp_cb(uint8_t sender_id, struct FloatVect3 *vel_sp);
219 struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
220 float time_of_vel_sp = 0.0;
221 
223 
224 #if PERIODIC_TELEMETRY
226 static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
227 {
228  pprz_msg_send_GUIDANCE_INDI_HYBRID(trans, dev, AC_ID,
229  &sp_accel.x,
230  &sp_accel.y,
231  &sp_accel.z,
232  &euler_cmd.x,
233  &euler_cmd.y,
234  &euler_cmd.z,
235  &filt_accel_ned[0].o[0],
236  &filt_accel_ned[1].o[0],
237  &filt_accel_ned[2].o[0],
238  &gi_speed_sp.x,
239  &gi_speed_sp.y,
240  &gi_speed_sp.z);
241 }
242 
243 #if GUIDANCE_INDI_HYBRID_USE_WLS
244 static void debug(struct transport_tx *trans, struct link_device *dev, char* name, float* data, int datasize)
245 {
246  pprz_msg_send_DEBUG_VECT(trans, dev,AC_ID,
247  strlen(name), name,
248  datasize, data);
249 }
250 
251 static void send_guidance_indi_debug(struct transport_tx *trans, struct link_device *dev)
252 {
253  static int c = 0;
254  switch (c++)
255  {
256  case 0:
257  debug(trans, dev, "v_gih", v_gih, 3);
258  break;
259  case 1:
260  debug(trans, dev, "du_min_gih", du_min_gih, GUIDANCE_INDI_HYBRID_U);
261  break;
262  case 2:
263  debug(trans, dev, "du_max_gih", du_max_gih, GUIDANCE_INDI_HYBRID_U);
264  break;
265  case 3:
266  debug(trans, dev, "du_pref_gih", du_pref_gih, GUIDANCE_INDI_HYBRID_U);
267  break;
268  case 4:
269  debug(trans, dev, "Wu_gih", Wu_gih, GUIDANCE_INDI_HYBRID_U);
270  break;
271  case 5:
272  debug(trans, dev, "Wv_gih", Wv_gih, GUIDANCE_INDI_HYBRID_V);
273  break;
274  default:
275  debug(trans, dev, "Bwls_gih[0]", Bwls_gih[0], GUIDANCE_INDI_HYBRID_U);
276  c=0;
277  break;
278  }
279 }
280 #else
281 static void send_guidance_indi_debug(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
282 {
283 }
284 #endif // GUIDANCE_INDI_HYBRID_USE_WLS
285 
286 #endif // PERIODIC_TELEMETRY
287 
292 {
293  /*AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);*/
294  AbiBindMsgVEL_SP(GUIDANCE_INDI_VEL_SP_ID, &vel_sp_ev, vel_sp_cb);
295 
296 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
297 #ifdef GUIDANCE_INDI_THRUST_DYNAMICS
298  thrust_dyn = GUIDANCE_INDI_THRUST_DYNAMICS;
299 #else
300  thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
301 #endif
302 #endif
303 
304  float tau = 1.0/(2.0*M_PI*filter_cutoff);
305  float sample_time = 1.0/PERIODIC_FREQUENCY;
306  for(int8_t i=0; i<3; i++) {
307  init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
308  }
309  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
310  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
311  init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
312  init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
313 
314 #if GUIDANCE_INDI_HYBRID_USE_WLS
315  for (int8_t i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) {
316  Bwls_gih[i] = Ga[i];
317  }
318 #endif
319 
320 #if PERIODIC_TELEMETRY
321  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_guidance_indi_hybrid);
323 #endif
324 }
325 
331  /*Obtain eulers with zxy rotation order*/
334 
335  thrust_in = stabilization_cmd[COMMAND_THRUST];
338 
339  float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
340  float sample_time = 1.0 / PERIODIC_FREQUENCY;
341  for (int8_t i = 0; i < 3; i++) {
342  init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
343  }
344 
345  /*Obtain eulers with zxy rotation order*/
347 
351  init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
352 }
353 
362 struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
363 {
364  // set global accel sp variable FIXME clean this
365  sp_accel = *accel_sp;
366 
367  /* Obtain eulers with zxy rotation order */
369 
370  /* Calculate the transition percentage so that the ctrl_effecitveness scheduling works */
373  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
376 
377  // filter accel to get rid of noise and filter attitude to synchronize with accel
379 
380 #if GUIDANCE_INDI_RC_DEBUG
381 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
382  // for rc control horizontal, rotate from body axes to NED
383  float psi = eulers_zxy.psi;
384  float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
385  float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
386  sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
387  sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
388 
389  // for rc vertical control
390  sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
391 #endif
392 
393  struct FloatVect3 accel_filt;
394  accel_filt.x = filt_accel_ned[0].o[0];
395  accel_filt.y = filt_accel_ned[1].o[0];
396  accel_filt.z = filt_accel_ned[2].o[0];
397 
398  struct FloatVect3 a_diff;
399  a_diff.x = sp_accel.x - accel_filt.x;
400  a_diff.y = sp_accel.y - accel_filt.y;
401  a_diff.z = sp_accel.z - accel_filt.z;
402 
403  // Bound the acceleration error so that the linearization still holds
404  Bound(a_diff.x, -6.0, 6.0);
405  Bound(a_diff.y, -6.0, 6.0);
406  Bound(a_diff.z, -9.0, 9.0);
407 
408  // If the thrust to specific force ratio has been defined, include vertical control
409  // else ignore the vertical acceleration error
410 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
411 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
412  a_diff.z = 0.0;
413 #endif
414 #endif
415 
416 
417  // Calculate matrix of partial derivatives and control objective
419 
420 #if GUIDANCE_INDI_HYBRID_USE_WLS
421 
422  // Calculate the maximum deflections
424 
425  float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f};
426 
427  int num_iter UNUSED = wls_alloc(
428  du_gih, v_gih, du_min_gih, du_max_gih,
429  Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10,
430  GUIDANCE_INDI_HYBRID_U, GUIDANCE_INDI_HYBRID_V);
431 
432  euler_cmd.x = du_gih[0];
433  euler_cmd.y = du_gih[1];
434  euler_cmd.z = du_gih[2];
435 
436 #else
437  // compute inverse matrix of Ga
438  float Ga_inv[3][3] = {};
440  // Calculate roll,pitch and thrust command
441  float_mat3_mult(&euler_cmd, Ga_inv, a_diff);
442 #endif
443 
444  struct FloatVect3 thrust_vect;
445 #if GUIDANCE_INDI_HYBRID_U > 3
446  thrust_vect.x = du_gih[3];
447 #else
448  thrust_vect.x = 0;
449 #endif
450  thrust_vect.y = 0;
451  thrust_vect.z = euler_cmd.z;
452  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
453 
454  // Coordinated turn
455  // feedforward estimate angular rotation omega = g*tan(phi)/v
456  float omega;
457  const float max_phi = RadOfDeg(60.0f);
458 #if GUIDANCE_INDI_ZERO_AIRSPEED
459  float airspeed_turn = 0.f;
460 #else
461  float airspeed_turn = stateGetAirspeed_f();
462 #endif
463  // We are dividing by the airspeed, so a lower bound is important
464  Bound(airspeed_turn, 10.0f, 30.0f);
465 
468 
469  //Bound euler angles to prevent flipping
472 
473  // Use the current roll angle to determine the corresponding heading rate of change.
474  float coordinated_turn_roll = eulers_zxy.phi;
475 
477  coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0f) - (guidance_euler_cmd.phi < 0.0f)) * guidance_euler_cmd.theta;
478  }
479 
480  if (fabsf(coordinated_turn_roll) < max_phi) {
481  omega = 9.81f / airspeed_turn * tanf(coordinated_turn_roll);
482  } else { //max 60 degrees roll
483  omega = 9.81f / airspeed_turn * 1.72305f * ((coordinated_turn_roll > 0.0f) - (coordinated_turn_roll < 0.0f));
484  }
485 
486 #ifdef FWD_SIDESLIP_GAIN
487  // Add sideslip correction
488  omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN;
489 #endif
490 
491  // We can pre-compute the required rates to achieve this turn rate:
492  // NOTE: there *should* not be any problems possible with Euler singularities here
493  struct FloatEulers *euler_zyx = stateGetNedToBodyEulers_f();
494 
495  struct FloatRates ff_rates;
496 
497  ff_rates.p = -sinf(euler_zyx->theta) * omega;
498  ff_rates.q = cosf(euler_zyx->theta) * sinf(euler_zyx->phi) * omega;
499  ff_rates.r = cosf(euler_zyx->theta) * cosf(euler_zyx->phi) * omega;
500 
501  // For a hybrid it is important to reduce the sideslip, which is done by changing the heading.
502  // For experiments, it is possible to fix the heading to a different value.
503  if (take_heading_control) {
504  // heading is fixed by nav
505  guidance_euler_cmd.psi = heading_sp;
506  }
507  else {
508  // heading is free and controlled by guidance
509  guidance_indi_hybrid_heading_sp += omega / PERIODIC_FREQUENCY;
511  // limit heading setpoint to be within bounds of current heading
512 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
513  float delta_limit = STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
515  float delta_psi = guidance_indi_hybrid_heading_sp - heading;
516  FLOAT_ANGLE_NORMALIZE(delta_psi);
517  if (delta_psi > delta_limit) {
519  } else if (delta_psi < -delta_limit) {
521  }
523 #endif
525  }
526 
527 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
528  guidance_indi_filter_thrust();
529 
530  // Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
532  Bound(thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
533 
534 #if GUIDANCE_INDI_RC_DEBUG
535  if (radio_control.values[RADIO_THROTTLE] < 300) {
536  thrust_in = 0;
537  }
538 #endif
539 
540  // Overwrite the thrust command from guidance_v
541  stabilization_cmd[COMMAND_THRUST] = thrust_in;
542 #endif
543 
544  // Set the quaternion setpoint from eulers_zxy
545  struct FloatQuat sp_quat;
547  float_quat_normalize(&sp_quat);
548 
549  return stab_sp_from_quat_ff_rates_f(&sp_quat, &ff_rates);
550 }
551 
552 // compute accel setpoint from speed setpoint (use global variables ! FIXME)
553 static struct FloatVect3 compute_accel_from_speed_sp(void)
554 {
555  struct FloatVect3 accel_sp = { 0.f, 0.f, 0.f };
556 
558 
559  //for rc control horizontal, rotate from body axes to NED
560  float psi = eulers_zxy.psi;
561  float cpsi = cosf(psi);
562  float spsi = sinf(psi);
563  float speed_sp_b_x = cpsi * gi_speed_sp.x + spsi * gi_speed_sp.y;
564  float speed_sp_b_y = -spsi * gi_speed_sp.x + cpsi * gi_speed_sp.y;
565 
566  // Get airspeed or zero it
567 #if GUIDANCE_INDI_ZERO_AIRSPEED
568  float airspeed = 0.f;
569 #else
570  float airspeed = stateGetAirspeed_f();
571 #endif
572  struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
573  struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed };
574  struct FloatVect2 windspeed;
575  VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
576 
577  VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp
578  float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
579 
580  // Make turn instead of straight line
581  if ((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0f))) {
582 
583  // Give the wind cancellation priority.
584  if (norm_des_as > guidance_indi_max_airspeed) {
585  float groundspeed_factor = 0.0f;
586 
587  // if the wind is faster than we can fly, just fly in the wind direction
588  if (FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) {
589  float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y;
590  float bv = -2.f * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y);
591  float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
592 
593  float dv = bv * bv - 4.0f * av * cv;
594 
595  // dv can only be positive, but just in case
596  if (dv < 0.0f) {
597  dv = fabsf(dv);
598  }
599  float d_sqrt = sqrtf(dv);
600 
601  groundspeed_factor = (-bv + d_sqrt) / (2.0f * av);
602  }
603 
604  desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x;
605  desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y;
606 
607  speed_sp_b_x = guidance_indi_max_airspeed;
608  }
609 
610  // desired airspeed can not be larger than max airspeed
611  speed_sp_b_x = Min(norm_des_as, guidance_indi_max_airspeed);
612 
613  if (force_forward) {
614  speed_sp_b_x = guidance_indi_max_airspeed;
615  }
616 
617  // Calculate accel sp in body axes, because we need to regulate airspeed
618  struct FloatVect2 sp_accel_b;
619  // In turn acceleration proportional to heading diff
620  sp_accel_b.y = atan2f(desired_airspeed.y, desired_airspeed.x) - psi;
621  FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
622  sp_accel_b.y *= gih_params.heading_bank_gain;
623 
624  // Control the airspeed
625  sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
626 
627  accel_sp.x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y;
628  accel_sp.y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y;
630  }
631  else { // Go somewhere in the shortest way
632 
633  if (airspeed > 10.f) {
634  // Groundspeed vector in body frame
635  float groundspeed_x = cpsi * stateGetSpeedNed_f()->x + spsi * stateGetSpeedNed_f()->y;
636  float speed_increment = speed_sp_b_x - groundspeed_x;
637 
638  // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
639  if ((speed_increment + airspeed) > guidance_indi_max_airspeed) {
640  speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
641  }
642  }
643 
644  gi_speed_sp.x = cpsi * speed_sp_b_x - spsi * speed_sp_b_y;
645  gi_speed_sp.y = spsi * speed_sp_b_x + cpsi * speed_sp_b_y;
646 
650  }
651 
652  // Bound the acceleration setpoint
653  float accelbound = 3.0f + airspeed / guidance_indi_max_airspeed * 5.0f; // FIXME remove hard coded values
654  float_vect3_bound_in_2d(&accel_sp, accelbound);
655  /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
656  /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
657  BoundAbs(accel_sp.z, 3.0);
658 
659  return accel_sp;
660 }
661 
662 static float bound_vz_sp(float vz_sp)
663 {
664  // Bound vertical speed setpoint
665  if (stateGetAirspeed_f() > 13.f) {
666  Bound(vz_sp, -climb_vspeed_fwd, -descend_vspeed_fwd);
667  } else {
668  Bound(vz_sp, -nav.climb_vspeed, -nav.descend_vspeed); // FIXME don't use nav settings
669  }
670  return vz_sp;
671 }
672 
674 {
675  struct FloatVect3 pos_err = { 0 };
676  struct FloatVect3 accel_sp = { 0 };
677 
678  // First check for velocity setpoint from module // FIXME should be called like this
679  float dt = get_sys_time_float() - time_of_vel_sp;
680  // If the input command is not updated after a timeout, switch back to flight plan control
681  if (dt < 0.5) {
685  accel_sp = compute_accel_from_speed_sp(); // compute accel sp
686  return guidance_indi_run(&accel_sp, gh->sp.heading);
687  }
688 
689  if (h_mode == GUIDANCE_INDI_HYBRID_H_POS) {
690  //Linear controller to find the acceleration setpoint from position and velocity
691  pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
692  pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
693  gi_speed_sp.x = pos_err.x * gih_params.pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
694  gi_speed_sp.y = pos_err.y * gih_params.pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
695  if (v_mode == GUIDANCE_INDI_HYBRID_V_POS) {
696  pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
698  } else if (v_mode == GUIDANCE_INDI_HYBRID_V_SPEED) {
700  } else {
701  gi_speed_sp.z = 0.f;
702  }
703  accel_sp = compute_accel_from_speed_sp(); // compute accel sp
704  if (v_mode == GUIDANCE_INDI_HYBRID_V_ACCEL) {
705  accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
706  }
707  return guidance_indi_run(&accel_sp, gh->sp.heading);
708  }
709  else if (h_mode == GUIDANCE_INDI_HYBRID_H_SPEED) {
710  gi_speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
711  gi_speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
712  if (v_mode == GUIDANCE_INDI_HYBRID_V_POS) {
713  pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
715  } else if (v_mode == GUIDANCE_INDI_HYBRID_V_SPEED) {
717  } else {
718  gi_speed_sp.z = 0.f;
719  }
720  accel_sp = compute_accel_from_speed_sp(); // compute accel sp
721  if (v_mode == GUIDANCE_INDI_HYBRID_V_ACCEL) {
722  accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
723  }
724  return guidance_indi_run(&accel_sp, gh->sp.heading);
725  }
726  else { // H_ACCEL
727  gi_speed_sp.x = 0.f;
728  gi_speed_sp.y = 0.f;
729  if (v_mode == GUIDANCE_INDI_HYBRID_V_POS) {
730  pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
732  } else if (v_mode == GUIDANCE_INDI_HYBRID_V_SPEED) {
734  } else {
735  gi_speed_sp.z = 0.f;
736  }
737  accel_sp = compute_accel_from_speed_sp(); // compute accel sp in case z control is required
738  // overwrite accel X and Y
739  accel_sp.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
740  accel_sp.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
741  if (v_mode == GUIDANCE_INDI_HYBRID_V_ACCEL) {
742  accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
743  }
744  return guidance_indi_run(&accel_sp, gh->sp.heading);
745  }
746 }
747 
748 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
752 void guidance_indi_filter_thrust(void)
753 {
754  // Actuator dynamics
756 
757  // same filter as for the acceleration
759 }
760 
761 #endif
762 
770  struct NedCoor_f *accel = stateGetAccelNed_f();
774 
777 
778  // Propagate filter for sideslip correction
779  float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
781 }
782 
783 
791 float WEAK guidance_indi_get_liftd(float airspeed, float theta) {
792  float liftd = 0.0f;
793 
794  if (airspeed < 12.f) {
795  /* Assume the airspeed is too low to be measured accurately
796  * Use scheduling based on pitch angle instead.
797  * You can define two interpolation segments
798  */
799  float pitch_interp = DegOfRad(theta);
800  const float min_pitch = -80.0f;
801  const float middle_pitch = -50.0f;
802  const float max_pitch = -20.0f;
803 
804  Bound(pitch_interp, min_pitch, max_pitch);
805  if (pitch_interp > middle_pitch) {
806  float ratio = (pitch_interp - max_pitch)/(middle_pitch - max_pitch);
807  liftd = -gih_params.liftd_p50*ratio;
808  } else {
809  float ratio = (pitch_interp - middle_pitch)/(min_pitch - middle_pitch);
811  }
812  } else {
813  liftd = -gih_params.liftd_asq*airspeed*airspeed;
814  }
815 
816  //TODO: bound liftd
817  return liftd;
818 }
819 
823 static void vel_sp_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *vel_sp)
824 {
825  indi_vel_sp.x = vel_sp->x;
826  indi_vel_sp.y = vel_sp->y;
827  indi_vel_sp.z = vel_sp->z;
829 }
830 
831 
832 #if GUIDANCE_INDI_HYBRID_USE_AS_DEFAULT
833 // guidance indi control function is implementing the default functions of guidance
834 
835 void guidance_h_run_enter(void)
836 {
838 }
839 
840 void guidance_v_run_enter(void)
841 {
842  // nothing to do
843 }
844 
845 static struct VerticalGuidance *_gv = &guidance_v;
847 
848 struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
849 {
851 }
852 
853 struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
854 {
856 }
857 
858 struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
859 {
861 }
862 
863 int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
864 {
865  _gv = gv;
867  return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
868 }
869 
870 int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
871 {
872  _gv = gv;
874  return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
875 }
876 
877 int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
878 {
879  _gv = gv;
881  return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
882 }
883 
884 #endif
885 
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define THRUST_INCREMENT_ID
Core autopilot interface common to all firmwares.
uint8_t last_wp UNUSED
#define Min(x, y)
Definition: esc_dshot.c:101
float phi
in radians
float p
in rad/s
float theta
in radians
float psi
in radians
static void float_quat_normalize(struct FloatQuat *q)
#define FLOAT_ANGLE_NORMALIZE(_a)
void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
Multiply 3D matrix with vector.
void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound)
void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
3x3 matrix inverse
#define FLOAT_VECT2_NORM(_v)
euler angles
Roation quaternion.
angular rates
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_PERCENTAGE_FRAC
#define POS_FLOAT_OF_BFP(_ai)
#define SPEED_FLOAT_OF_BFP(_ai)
#define INT32_ANGLE_FRAC
#define BFP_OF_REAL(_vr, _frac)
#define ACCEL_FLOAT_OF_BFP(_ai)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
struct FloatMat33 Ga_inv
float guidance_indi_specific_force_gain
struct FloatVect3 indi_vel_sp
bool force_forward
forward flight for hybrid nav
Butterworth2LowPass accely_filt
static void vel_sp_cb(uint8_t sender_id, struct FloatVect3 *vel_sp)
ABI callback that obtains the velocity setpoint from a module.
float time_of_vel_sp
Butterworth2LowPass roll_filt
#define GUIDANCE_INDI_SPEED_GAINZ
#define GUIDANCE_INDI_CLIMB_SPEED_FWD
struct guidance_indi_hybrid_params gih_params
float thrust_in
struct FloatVect3 gi_speed_sp
float guidance_indi_min_pitch
float guidance_indi_max_bank
float climb_vspeed_fwd
struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode)
float inv_eff[4]
float descend_vspeed_fwd
struct FloatVect3 sp_accel
static void send_guidance_indi_debug(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
struct FloatEulers guidance_euler_cmd
float guidance_indi_pitch_pref_deg
#define GUIDANCE_INDI_LIFTD_ASQ
float thrust_act
void guidance_indi_enter(void)
Call upon entering indi guidance.
Butterworth2LowPass pitch_filt
#define GUIDANCE_INDI_POS_GAIN
bool take_heading_control
float guidance_indi_hybrid_heading_sp
float WEAK guidance_indi_get_liftd(float airspeed, float theta)
Get the derivative of lift w.r.t.
#define GUIDANCE_INDI_LIFTD_P50
static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
void guidance_indi_init(void)
Init function.
float filter_cutoff
#define GUIDANCE_INDI_FILTER_CUTOFF
#define GUIDANCE_INDI_DESCEND_SPEED_FWD
float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]
struct FloatVect3 euler_cmd
static float bound_vz_sp(float vz_sp)
struct FloatEulers eulers_zxy
state eulers in zxy order
Butterworth2LowPass filt_accel_ned[3]
#define GUIDANCE_INDI_POS_GAINZ
#define TURN_AIRSPEED_TH
float guidance_indi_max_airspeed
float thrust_dyn
static struct FloatVect3 compute_accel_from_speed_sp(void)
#define GUIDANCE_INDI_VEL_SP_ID
struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
float v_gih[3]
void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
#define GUIDANCE_INDI_LIFTD_P80
abi_event vel_sp_ev
Butterworth2LowPass thrust_filt
#define GUIDANCE_INDI_SPEED_GAIN
struct FloatVect2 desired_airspeed
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V])
Perform WLS.
GuidanceIndiHybrid_VMode
@ GUIDANCE_INDI_HYBRID_V_POS
@ GUIDANCE_INDI_HYBRID_V_SPEED
@ GUIDANCE_INDI_HYBRID_V_ACCEL
GuidanceIndiHybrid_HMode
@ GUIDANCE_INDI_HYBRID_H_SPEED
@ GUIDANCE_INDI_HYBRID_H_ACCEL
@ GUIDANCE_INDI_HYBRID_H_POS
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
#define GUIDANCE_INDI_MAX_PITCH
#define GUIDANCE_INDI_MIN_PITCH
static enum GuidanceOneloop_VMode _v_mode
void guidance_v_run_enter(void)
static struct VerticalGuidance * _gv
int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
Simple first order low pass filter with bilinear transform.
float o[2]
output history
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Second order low pass filter structure.
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
Some helper functions to check RC sticks.
int32_t transition_percentage
Definition: guidance_h.c:52
void guidance_h_run_enter(void)
struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
Definition: guidance_h.h:71
struct VerticalGuidance guidance_v
Definition: guidance_v.c:59
int32_t z_ref
altitude reference in meters.
Definition: guidance_v.h:63
int32_t zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.h:69
int32_t zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.h:75
struct RotorcraftNavigation nav
Definition: navigation.c:51
Rotorcraft navigation functions.
float descend_vspeed
descend speed setting, mostly used in flight plans
Definition: navigation.h:146
float heading
heading setpoint (in radians)
Definition: navigation.h:133
float climb_vspeed
climb speed setting, mostly used in flight plans
Definition: navigation.h:145
struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:34
int32_t transition_theta_offset
float stabilization_attitude_get_heading_f(void)
Read an attitude setpoint from the RC.
Rotorcraft attitude reference generation.
int num_iter
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Stabilization setpoint.
Definition: stabilization.h:42
union StabilizationSetpoint::@273 sp
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
float heading
Definition: wedgebug.c:258
int wls_alloc(float *u, float *v, float *umin, float *umax, float **B, float *u_guess, float *W_init, float *Wv, float *Wu, float *up, float gamma_sq, int imax, int n_u, int n_v)
active set algorithm for control allocation
Definition: wls_alloc.c:108