Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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"
32 #include "subsystems/ins/ins_int.h"
34 #include "state.h"
35 #include "subsystems/imu.h"
40 #include "mcu_periph/sys_time.h"
41 #include "autopilot.h"
44 #include "stdio.h"
46 #include "subsystems/abi.h"
48 
49 
50 // The acceleration reference is calculated with these gains. If you use GPS,
51 // they are probably limited by the update rate of your GPS. The default
52 // values are tuned for 4 Hz GPS updates. If you have high speed position updates, the
53 // gains can be higher, depending on the speed of the inner loop.
54 #ifndef GUIDANCE_INDI_SPEED_GAIN
55 #define GUIDANCE_INDI_SPEED_GAIN 1.8
56 #define GUIDANCE_INDI_SPEED_GAINZ 1.8
57 #endif
58 
59 #ifndef GUIDANCE_INDI_POS_GAIN
60 #define GUIDANCE_INDI_POS_GAIN 0.5
61 #define GUIDANCE_INDI_POS_GAINZ 0.5
62 #endif
63 
66  .pos_gainz = GUIDANCE_INDI_POS_GAINZ,
67 
68  .speed_gain = GUIDANCE_INDI_SPEED_GAIN,
69  .speed_gainz = GUIDANCE_INDI_SPEED_GAINZ,
70 
71  .heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
72 };
73 
74 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
75 #error "You must have an airspeed sensor to use this guidance"
76 #endif
77 float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
78 
79 // Max ground speed that will be commanded
80 #define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + 10.0)
82 
83 #ifndef MAX_DECELERATION
84 #define MAX_DECELERATION 1.
85 #endif
86 
87 /*Boolean to force the heading to a static value (only use for specific experiments)*/
88 bool take_heading_control = false;
89 
90 struct FloatVect3 sp_accel = {0.0,0.0,0.0};
91 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
92 float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
93 static void guidance_indi_filter_thrust(void);
94 
95 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS
96 #ifndef STABILIZATION_INDI_ACT_DYN_P
97 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
98 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
99 #define GUIDANCE_INDI_THRUST_DYNAMICS STABILIZATION_INDI_ACT_DYN_P
100 #endif
101 #endif //GUIDANCE_INDI_THRUST_DYNAMICS
102 
103 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
104 
105 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
106 #ifdef STABILIZATION_INDI_FILT_CUTOFF
107 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
108 #else
109 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
110 #endif
111 #endif
112 
113 #ifdef GUIDANCE_INDI_LINE_GAIN
114 float guidance_indi_line_gain = GUIDANCE_INDI_LINE_GAIN;
115 #else
117 #endif
118 
119 float inv_eff[4];
120 
121 float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF;
122 
123 // Max bank angle in radians
125 
128 
129 float thrust_act = 0;
135 
137 
138 struct FloatMat33 Ga;
141 
143 
145 float thrust_in;
146 
147 struct FloatVect3 speed_sp = {0.0, 0.0, 0.0};
148 
150 static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
151 static float guidance_indi_get_liftd(float pitch, float theta);
152 struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain);
153 struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain);
154 struct FloatVect3 nav_get_speed_setpoint(float pos_gain);
155 
156 #if PERIODIC_TELEMETRY
158 static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
159 {
160  pprz_msg_send_GUIDANCE_INDI_HYBRID(trans, dev, AC_ID,
161  &sp_accel.x,
162  &sp_accel.y,
163  &sp_accel.z,
164  &euler_cmd.x,
165  &euler_cmd.y,
166  &euler_cmd.z,
167  &filt_accel_ned[0].o[0],
168  &filt_accel_ned[1].o[0],
169  &filt_accel_ned[2].o[0],
170  &speed_sp.x,
171  &speed_sp.y,
172  &speed_sp.z);
173 }
174 #endif
175 
180 {
181  /*AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);*/
182 
183  float tau = 1.0/(2.0*M_PI*filter_cutoff);
184  float sample_time = 1.0/PERIODIC_FREQUENCY;
185  for(int8_t i=0; i<3; i++) {
186  init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
187  }
188  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
189  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
190  init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
191  init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
192 
193 #if PERIODIC_TELEMETRY
194  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_guidance_indi_hybrid);
195 #endif
196 }
197 
203  thrust_in = stabilization_cmd[COMMAND_THRUST];
205 
206  float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
207  float sample_time = 1.0 / PERIODIC_FREQUENCY;
208  for (int8_t i = 0; i < 3; i++) {
209  init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
210  }
211  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi);
212  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta);
213  init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
214  init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
215 }
216 
223 void guidance_indi_run(float *heading_sp) {
224 
225  /*Obtain eulers with zxy rotation order*/
227 
228  /*Calculate the transition percentage so that the ctrl_effecitveness scheduling works*/
231  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
234 
235  //filter accel to get rid of noise and filter attitude to synchronize with accel
237 
238  //Linear controller to find the acceleration setpoint from position and velocity
239  float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
240  float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
242 
243  if(autopilot.mode == AP_MODE_NAV) {
244  speed_sp = nav_get_speed_setpoint(gih_params.pos_gain);
245  } else{
246  speed_sp.x = pos_x_err * gih_params.pos_gain;
247  speed_sp.y = pos_y_err * gih_params.pos_gain;
248  speed_sp.z = pos_z_err * gih_params.pos_gainz;
249  }
250 
251  //for rc control horizontal, rotate from body axes to NED
252  float psi = eulers_zxy.psi;
253  /*NAV mode*/
254  float speed_sp_b_x = cosf(psi) * speed_sp.x + sinf(psi) * speed_sp.y;
255  float speed_sp_b_y =-sinf(psi) * speed_sp.x + cosf(psi) * speed_sp.y;
256 
257  float airspeed = stateGetAirspeed_f();
258 
259  struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
260  struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
261  struct FloatVect2 windspeed;
262  VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
263 
264  VECT2_DIFF(desired_airspeed, speed_sp, windspeed); // Use 2d part of speed_sp
265  float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
266 
267  // Make turn instead of straight line
268  if((airspeed > 10.0) && (norm_des_as > 12.0)) {
269 
270  // Give the wind cancellation priority.
271  if (norm_des_as > guidance_indi_max_airspeed) {
272  float groundspeed_factor = 0.0;
273 
274  // if the wind is faster than we can fly, just fly in the wind direction
276  float av = speed_sp.x * speed_sp.x + speed_sp.y * speed_sp.y;
277  float bv = -2 * (windspeed.x * speed_sp.x + windspeed.y * speed_sp.y);
278  float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
279 
280  float dv = bv * bv - 4.0 * av * cv;
281 
282  // dv can only be positive, but just in case
283  if(dv < 0.0) {
284  dv = fabs(dv);
285  }
286  float d_sqrt = sqrtf(dv);
287 
288  groundspeed_factor = (-bv + d_sqrt) / (2.0 * av);
289  }
290 
291  desired_airspeed.x = groundspeed_factor * speed_sp.x - windspeed.x;
292  desired_airspeed.y = groundspeed_factor * speed_sp.y - windspeed.y;
293 
294  speed_sp_b_x = guidance_indi_max_airspeed;
295  }
296 
297  // desired airspeed can not be larger than max airspeed
298  speed_sp_b_x = Min(norm_des_as,guidance_indi_max_airspeed);
299 
300  if(force_forward) {
301  speed_sp_b_x = guidance_indi_max_airspeed;
302  }
303 
304  // Calculate accel sp in body axes, because we need to regulate airspeed
305  struct FloatVect2 sp_accel_b;
306  // In turn acceleration proportional to heading diff
307  sp_accel_b.y = atan2(desired_airspeed.y, desired_airspeed.x) - psi;
308  FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
309  sp_accel_b.y *= gih_params.heading_bank_gain;
310 
311  // Control the airspeed
312  sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
313 
314  sp_accel.x = cosf(psi) * sp_accel_b.x - sinf(psi) * sp_accel_b.y;
315  sp_accel.y = sinf(psi) * sp_accel_b.x + cosf(psi) * sp_accel_b.y;
316 
317  sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
318  } else { // Go somewhere in the shortest way
319 
320  if(airspeed > 10.0) {
321  // Groundspeed vector in body frame
322  float groundspeed_x = cosf(psi) * stateGetSpeedNed_f()->x + sinf(psi) * stateGetSpeedNed_f()->y;
323  float speed_increment = speed_sp_b_x - groundspeed_x;
324 
325  // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
326  if((speed_increment + airspeed) > guidance_indi_max_airspeed) {
327  speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
328  }
329  }
330  speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
331  speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
332 
333  sp_accel.x = (speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
334  sp_accel.y = (speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
335  sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
336  }
337 
338  // Bound the acceleration setpoint
339  float accelbound = 3.0 + airspeed/guidance_indi_max_airspeed*5.0;
340  vect_bound_in_2d(&sp_accel, accelbound);
341  /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
342  /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
343  BoundAbs(sp_accel.z, 3.0);
344 
345 #if GUIDANCE_INDI_RC_DEBUG
346 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
347  //for rc control horizontal, rotate from body axes to NED
348  float psi = eulers_zxy.psi;
349  float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
350  float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
351  sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
352  sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
353 
354  //for rc vertical control
355  sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
356 #endif
357 
358  //Calculate matrix of partial derivatives
360  //Invert this matrix
361  MAT33_INV(Ga_inv, Ga);
362 
363  struct FloatVect3 accel_filt;
364  accel_filt.x = filt_accel_ned[0].o[0];
365  accel_filt.y = filt_accel_ned[1].o[0];
366  accel_filt.z = filt_accel_ned[2].o[0];
367 
368  struct FloatVect3 a_diff;
369  a_diff.x = sp_accel.x - accel_filt.x;
370  a_diff.y = sp_accel.y - accel_filt.y;
371  a_diff.z = sp_accel.z - accel_filt.z;
372 
373  //Bound the acceleration error so that the linearization still holds
374  Bound(a_diff.x, -6.0, 6.0);
375  Bound(a_diff.y, -6.0, 6.0);
376  Bound(a_diff.z, -9.0, 9.0);
377 
378  //If the thrust to specific force ratio has been defined, include vertical control
379  //else ignore the vertical acceleration error
380 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
381 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
382  a_diff.z = 0.0;
383 #endif
384 #endif
385 
386  //Calculate roll,pitch and thrust command
387  MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
388 
389  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
390 
391  //Coordinated turn
392  //feedforward estimate angular rotation omega = g*tan(phi)/v
393  float omega;
394  const float max_phi = RadOfDeg(60.0);
395  float airspeed_turn = airspeed;
396  //We are dividing by the airspeed, so a lower bound is important
397  Bound(airspeed_turn,10.0,30.0);
398 
399  guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
400  guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
401 
402  //Bound euler angles to prevent flipping
404  Bound(guidance_euler_cmd.theta, -RadOfDeg(120.0), RadOfDeg(25.0));
405 
406  // Use the current roll angle to determine the corresponding heading rate of change.
407  float coordinated_turn_roll = eulers_zxy.phi;
408 
410  coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0) - (guidance_euler_cmd.phi < 0.0))*guidance_euler_cmd.theta;
411  }
412 
413  if (fabs(coordinated_turn_roll) < max_phi) {
414  omega = 9.81 / airspeed_turn * tanf(coordinated_turn_roll);
415  } else { //max 60 degrees roll
416  omega = 9.81 / airspeed_turn * 1.72305 * ((coordinated_turn_roll > 0.0) - (coordinated_turn_roll < 0.0));
417  }
418 
419 #ifdef FWD_SIDESLIP_GAIN
420  // Add sideslip correction
421  omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN;
422 #endif
423 
424 // For a hybrid it is important to reduce the sideslip, which is done by changing the heading.
425 // For experiments, it is possible to fix the heading to a different value.
426 #ifndef KNIFE_EDGE_TEST
428  *heading_sp = ANGLE_FLOAT_OF_BFP(nav_heading);
429  } else {
430  *heading_sp += omega / PERIODIC_FREQUENCY;
431  FLOAT_ANGLE_NORMALIZE(*heading_sp);
432  }
433 #endif
434 
435  guidance_euler_cmd.psi = *heading_sp;
436 
437 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
438  guidance_indi_filter_thrust();
439 
440  //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
442  Bound(thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
443 
444 #if GUIDANCE_INDI_RC_DEBUG
446  thrust_in = 0;
447  }
448 #endif
449 
450  //Overwrite the thrust command from guidance_v
451  stabilization_cmd[COMMAND_THRUST] = thrust_in;
452 #endif
453 
454  // Set the quaternion setpoint from eulers_zxy
455  struct FloatQuat sp_quat;
457  float_quat_normalize(&sp_quat);
459 }
460 
461 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
462 
465 void guidance_indi_filter_thrust(void)
466 {
467  // Actuator dynamics
468  thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
469 
470  // same filter as for the acceleration
472 }
473 #endif
474 
482  struct NedCoor_f *accel = stateGetAccelNed_f();
483  update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
484  update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
485  update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
486 
489 
490  // Propagate filter for sideslip correction
491  float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
492  update_butterworth_2_low_pass(&accely_filt, accely);
493 }
494 
503 
504  /*Pre-calculate sines and cosines*/
505  float sphi = sinf(eulers_zxy.phi);
506  float cphi = cosf(eulers_zxy.phi);
507  float stheta = sinf(eulers_zxy.theta);
508  float ctheta = cosf(eulers_zxy.theta);
509  float spsi = sinf(eulers_zxy.psi);
510  float cpsi = cosf(eulers_zxy.psi);
511  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
512 
513 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
514 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
515 #endif
516 
517  /*Amount of lift produced by the wing*/
518  float pitch_lift = eulers_zxy.theta;
519  Bound(pitch_lift,-M_PI_2,0);
520  float lift = sinf(pitch_lift)*9.81;
521  float T = cosf(pitch_lift)*-9.81;
522 
523  // get the derivative of the lift wrt to theta
525 
526  RMAT_ELMT(*Gmat, 0, 0) = cphi*ctheta*spsi*T + cphi*spsi*lift;
527  RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
528  RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*T -sphi*lift;
529  RMAT_ELMT(*Gmat, 0, 1) = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
530  RMAT_ELMT(*Gmat, 1, 1) = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
531  RMAT_ELMT(*Gmat, 2, 1) = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
532  RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
533  RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
534  RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
535 }
536 
544 float guidance_indi_get_liftd(float airspeed, float theta) {
545  float liftd = 0.0;
546  if(airspeed < 12) {
547  float pitch_interp = DegOfRad(theta);
548  Bound(pitch_interp, -80.0, -40.0);
549  float ratio = (pitch_interp + 40.0)/(-40.);
550  liftd = -24.0*ratio*lift_pitch_eff/0.12;
551  } else {
552  liftd = -(airspeed - 8.5)*lift_pitch_eff/M_PI*180.0;
553  }
554  //TODO: bound liftd
555  return liftd;
556 }
557 
567 struct FloatVect3 nav_get_speed_setpoint(float pos_gain) {
568  struct FloatVect3 speed_sp;
570  speed_sp = nav_get_speed_sp_from_line(line_vect, to_end_vect, navigation_target, pos_gain);
571  } else {
572  speed_sp = nav_get_speed_sp_from_go(navigation_target, pos_gain);
573  }
574  return speed_sp;
575 }
576 
586 struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain) {
587 
588  // enu -> ned
589  struct FloatVect2 line_v = {line_v_enu.y, line_v_enu.x};
590  struct FloatVect2 to_end_v = {to_end_v_enu.y, to_end_v_enu.x};
591 
592  struct NedCoor_f ned_target;
593  // Target in NED instead of ENU
595 
596  // Calculate magnitude of the desired speed vector based on distance to waypoint
597  float dist_to_target = float_vect2_norm(&to_end_v);
598  float desired_speed;
599  if(force_forward) {
600  desired_speed = nav_max_speed;
601  } else {
602  desired_speed = dist_to_target * pos_gain;
603  Bound(desired_speed, 0.0, nav_max_speed);
604  }
605 
606  // Calculate length of line segment
607  float length_line = float_vect2_norm(&line_v);
608  if(length_line < 0.01) {
609  length_line = 0.01;
610  }
611 
612  //Normal vector to the line, with length of the line
613  struct FloatVect2 normalv;
614  VECT2_ASSIGN(normalv, -line_v.y, line_v.x);
615  // Length of normal vector is the same as of the line segment
616  float length_normalv = length_line;
617  if(length_normalv < 0.01) {
618  length_normalv = 0.01;
619  }
620 
621  // Distance along the normal vector
622  float dist_to_line = (to_end_v.x*normalv.x + to_end_v.y*normalv.y)/length_normalv;
623 
624  // Normal vector scaled to be the distance to the line
625  struct FloatVect2 v_to_line, v_along_line;
626  v_to_line.x = dist_to_line*normalv.x/length_normalv*guidance_indi_line_gain;
627  v_to_line.y = dist_to_line*normalv.y/length_normalv*guidance_indi_line_gain;
628 
629  // Depending on the normal vector, the distance could be negative
630  float dist_to_line_abs = fabs(dist_to_line);
631 
632  // The distance that needs to be traveled along the line
633  /*float dist_along_line = (line_v.x*to_end_v.x + line_v.y*to_end_v.y)/length_line;*/
634  v_along_line.x = line_v.x/length_line*50.0;
635  v_along_line.y = line_v.y/length_line*50.0;
636 
637  // Calculate the desired direction to converge to the line
638  struct FloatVect2 direction;
639  VECT2_SMUL(direction, v_along_line, (1.0/(1+dist_to_line_abs*0.05)));
640  VECT2_ADD(direction, v_to_line);
641  float length_direction = float_vect2_norm(&direction);
642  if(length_direction < 0.01) {
643  length_direction = 0.01;
644  }
645 
646  // Scale to have the desired speed
647  struct FloatVect2 final_vector;
648  VECT2_SMUL(final_vector, direction, desired_speed/length_direction);
649 
650  struct FloatVect3 speed_sp_return = {final_vector.x, final_vector.y, gih_params.pos_gainz*(ned_target.z - stateGetPositionNed_f()->z)};
652  speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
653  }
654 
655  // Bound vertical speed setpoint
656  if(stateGetAirspeed_f() > 13.0) {
657  Bound(speed_sp_return.z, -4.0, 5.0);
658  } else {
659  Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
660  }
661 
662  return speed_sp_return;
663 }
664 
672 struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain) {
673  // The speed sp that will be returned
674  struct FloatVect3 speed_sp_return;
675  struct NedCoor_f ned_target;
676  // Target in NED instead of ENU
678 
679  // Calculate position error
680  struct FloatVect3 pos_error;
681  struct NedCoor_f *pos = stateGetPositionNed_f();
682  VECT3_DIFF(pos_error, ned_target, *pos);
683 
684  VECT3_SMUL(speed_sp_return, pos_error, pos_gain);
685  speed_sp_return.z = gih_params.pos_gainz*pos_error.z;
686 
688  speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
689  }
690 
691  if(force_forward) {
692  vect_scale(&speed_sp_return, nav_max_speed);
693  } else {
694  // Calculate distance to waypoint
695  float dist_to_wp = FLOAT_VECT2_NORM(pos_error);
696 
697  // Calculate max speed to decelerate from
698 
699  // dist_to_wp can only be positive, but just in case
700  float max_speed_decel2 = 2*dist_to_wp*MAX_DECELERATION;
701  if(max_speed_decel2 < 0.0) {
702  max_speed_decel2 = fabs(max_speed_decel2);
703  }
704  float max_speed_decel = sqrtf(max_speed_decel2);
705 
706  // Bound the setpoint velocity vector
707  float max_h_speed = Min(nav_max_speed, max_speed_decel);
708  vect_bound_in_2d(&speed_sp_return, max_h_speed);
709  }
710 
711  // Bound vertical speed setpoint
712  if(stateGetAirspeed_f() > 13.0) {
713  Bound(speed_sp_return.z, -4.0, 5.0);
714  } else {
715  Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
716  }
717 
718  return speed_sp_return;
719 }
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
struct EnuCoor_i navigation_target
Definition: navigation.c:91
#define FLOAT_ANGLE_NORMALIZE(_a)
int32_t transition_percentage
Definition: guidance_h.c:92
bool take_heading_control
Butterworth2LowPass accely_filt
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
#define RADIO_ROLL
Definition: intermcu_ap.h:41
float nav_climb_vspeed
Definition: navigation.c:111
float nav_max_speed
struct FloatVect2 line_vect to_end_vect
Definition: navigation.c:81
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
#define GUIDANCE_V_MODE_NAV
Definition: guidance_v.h:40
#define Min(x, y)
Definition: esc_dshot.c:85
float y
in meters
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:70
float phi
in radians
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
struct FloatMat33 Ga
float nav_descend_vspeed
Definition: navigation.c:111
void guidance_indi_init(void)
Init function.
#define INT32_PERCENTAGE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain)
Go to a waypoint in the shortest way.
Butterworth2LowPass roll_filt
struct FloatEulers eulers_zxy
state eulers in zxy order
static float guidance_indi_get_liftd(float pitch, float theta)
Get the derivative of lift w.r.t.
#define INT32_ANGLE_FRAC
Simple first order low pass filter with bilinear transform.
#define GUIDANCE_INDI_FILTER_CUTOFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
void guidance_indi_enter(void)
Call upon entering indi guidance.
Read an attitude setpoint from the RC.
struct FloatVect3 euler_cmd
Main include for ABI (AirBorneInterface).
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
float guidance_indi_line_gain
float psi
in radians
Vertical guidance for rotorcrafts.
#define SPEED_FLOAT_OF_BFP(_ai)
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:104
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
#define GUIDANCE_INDI_SPEED_GAINZ
struct FloatVect2 desired_airspeed
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
#define ANGLE_FLOAT_OF_BFP(_ai)
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
euler angles
float z
in meters
float o[2]
output history
Roation quaternion.
void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
float inv_eff[4]
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 ...
float thrust_in
#define BFP_OF_REAL(_vr, _frac)
float theta
in radians
float guidance_indi_max_bank
#define FLOAT_VECT2_NORM(_v)
Some helper functions to check RC sticks.
struct FloatVect3 nav_get_speed_setpoint(float pos_gain)
function that returns a speed setpoint based on flight plan.
void vect_scale(struct FloatVect3 *vect3, float norm_des)
vector in North East Down coordinates Units: meters
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static float float_vect2_norm(struct FloatVect2 *v)
struct FloatVect3 speed_sp
Architecture independent timing functions.
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
#define GUIDANCE_INDI_PITCH_EFF_SCALING
float filter_cutoff
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
#define ACCEL_FLOAT_OF_BFP(_ai)
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat)
Calculate the matrix of partial derivatives of the roll, pitch and thrust w.r.t.
int32_t transition_theta_offset
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
struct FloatEulers guidance_euler_cmd
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static void float_quat_normalize(struct FloatQuat *q)
struct RadioControl radio_control
Definition: radio_control.c:30
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:132
Inertial Measurement Unit interface.
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
float guidance_indi_max_airspeed
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:98
#define THRUST_INCREMENT_ID
struct guidance_indi_hybrid_params gih_params
#define ANGLE_BFP_OF_REAL(_af)
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
uint8_t guidance_v_mode
Definition: guidance_v.c:103
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:82
#define GUIDANCE_INDI_SPEED_GAIN
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
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
vector in East North Up coordinates
#define AP_MODE_NAV
void guidance_indi_run(float *heading_sp)
API to get/set the generic vehicle states.
#define RADIO_PITCH
Definition: intermcu_ap.h:42
#define INT_MULT_RSHIFT(_a, _b, _r)
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more! ...
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
#define GUIDANCE_INDI_POS_GAIN
struct FloatMat33 Ga_inv
void vect_bound_in_2d(struct FloatVect3 *vect3, float bound)
struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain)
follow a line.
int32_t guidance_v_zd_sp
vertical speed setpoint in meter/s (input).
Definition: guidance_v.c:130
bool force_forward
Definition: navigation.c:79
#define POS_FLOAT_OF_BFP(_ai)
Butterworth2LowPass thrust_filt
#define NAV_MAX_SPEED
Second order low pass filter structure.
float lift_pitch_eff
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
#define VERTICAL_MODE_CLIMB
Definition: navigation.h:74
Horizontal guidance for rotorcrafts.
#define MAT33_INV(_minv, _m)
Definition: pprz_algebra.h:489
struct FloatVect2 target
Butterworth2LowPass pitch_filt
#define MAX_DECELERATION
uint8_t vertical_mode
Definition: sim_ap.c:35
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
Butterworth2LowPass filt_accel_ned[3]
float guidance_indi_specific_force_gain
Rotorcraft attitude reference generation.
#define GUIDANCE_INDI_POS_GAINZ
struct FloatVect3 sp_accel
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float x
in meters
INS for rotorcrafts combining vertical and horizontal filters.
float thrust_act