Paparazzi UAS  v6.0_unstable-38-g4d1c433-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 = 0.0;
204  thrust_act = 0;
205 }
206 
213 void guidance_indi_run(float *heading_sp) {
214 
215  /*Obtain eulers with zxy rotation order*/
217 
218  /*Calculate the transition percentage so that the ctrl_effecitveness scheduling works*/
221  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
224 
225  //filter accel to get rid of noise and filter attitude to synchronize with accel
227 
228  //Linear controller to find the acceleration setpoint from position and velocity
229  float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
230  float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
232 
233  if(autopilot.mode == AP_MODE_NAV) {
234  speed_sp = nav_get_speed_setpoint(gih_params.pos_gain);
235  } else{
236  speed_sp.x = pos_x_err * gih_params.pos_gain;
237  speed_sp.y = pos_y_err * gih_params.pos_gain;
238  speed_sp.z = pos_z_err * gih_params.pos_gainz;
239  }
240 
241  //for rc control horizontal, rotate from body axes to NED
242  float psi = eulers_zxy.psi;
243  /*NAV mode*/
244  float speed_sp_b_x = cosf(psi) * speed_sp.x + sinf(psi) * speed_sp.y;
245  float speed_sp_b_y =-sinf(psi) * speed_sp.x + cosf(psi) * speed_sp.y;
246 
247  float airspeed = stateGetAirspeed_f();
248 
249  struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
250  struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
251  struct FloatVect2 windspeed;
252  VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
253 
254  VECT2_DIFF(desired_airspeed, speed_sp, windspeed); // Use 2d part of speed_sp
255  float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
256 
257  // Make turn instead of straight line
258  if((airspeed > 10.0) && (norm_des_as > 12.0)) {
259 
260  // Give the wind cancellation priority.
261  if (norm_des_as > guidance_indi_max_airspeed) {
262  float groundspeed_factor = 0.0;
263 
264  // if the wind is faster than we can fly, just fly in the wind direction
266  float av = speed_sp.x * speed_sp.x + speed_sp.y * speed_sp.y;
267  float bv = -2 * (windspeed.x * speed_sp.x + windspeed.y * speed_sp.y);
268  float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
269 
270  float dv = bv * bv - 4.0 * av * cv;
271 
272  // dv can only be positive, but just in case
273  if(dv < 0.0) {
274  dv = fabs(dv);
275  }
276  float d_sqrt = sqrtf(dv);
277 
278  groundspeed_factor = (-bv + d_sqrt) / (2.0 * av);
279  }
280 
281  desired_airspeed.x = groundspeed_factor * speed_sp.x - windspeed.x;
282  desired_airspeed.y = groundspeed_factor * speed_sp.y - windspeed.y;
283 
284  speed_sp_b_x = guidance_indi_max_airspeed;
285  }
286 
287  // desired airspeed can not be larger than max airspeed
288  speed_sp_b_x = Min(norm_des_as,guidance_indi_max_airspeed);
289 
290  if(force_forward) {
291  speed_sp_b_x = guidance_indi_max_airspeed;
292  }
293 
294  // Calculate accel sp in body axes, because we need to regulate airspeed
295  struct FloatVect2 sp_accel_b;
296  // In turn acceleration proportional to heading diff
297  sp_accel_b.y = atan2(desired_airspeed.y, desired_airspeed.x) - psi;
298  FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
299  sp_accel_b.y *= gih_params.heading_bank_gain;
300 
301  // Control the airspeed
302  sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
303 
304  sp_accel.x = cosf(psi) * sp_accel_b.x - sinf(psi) * sp_accel_b.y;
305  sp_accel.y = sinf(psi) * sp_accel_b.x + cosf(psi) * sp_accel_b.y;
306 
307  sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
308  } else { // Go somewhere in the shortest way
309 
310  if(airspeed > 10.0) {
311  // Groundspeed vector in body frame
312  float groundspeed_x = cosf(psi) * stateGetSpeedNed_f()->x + sinf(psi) * stateGetSpeedNed_f()->y;
313  float speed_increment = speed_sp_b_x - groundspeed_x;
314 
315  // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
316  if((speed_increment + airspeed) > guidance_indi_max_airspeed) {
317  speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
318  }
319  }
320  speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
321  speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
322 
323  sp_accel.x = (speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
324  sp_accel.y = (speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
325  sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
326  }
327 
328  // Bound the acceleration setpoint
329  float accelbound = 3.0 + airspeed/guidance_indi_max_airspeed*5.0;
330  vect_bound_in_2d(&sp_accel, accelbound);
331  /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
332  /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
333  BoundAbs(sp_accel.z, 3.0);
334 
335 #if GUIDANCE_INDI_RC_DEBUG
336 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
337  //for rc control horizontal, rotate from body axes to NED
338  float psi = eulers_zxy.psi;
339  float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
340  float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
341  sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
342  sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
343 
344  //for rc vertical control
345  sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
346 #endif
347 
348  //Calculate matrix of partial derivatives
350  //Invert this matrix
351  MAT33_INV(Ga_inv, Ga);
352 
353  struct FloatVect3 accel_filt;
354  accel_filt.x = filt_accel_ned[0].o[0];
355  accel_filt.y = filt_accel_ned[1].o[0];
356  accel_filt.z = filt_accel_ned[2].o[0];
357 
358  struct FloatVect3 a_diff;
359  a_diff.x = sp_accel.x - accel_filt.x;
360  a_diff.y = sp_accel.y - accel_filt.y;
361  a_diff.z = sp_accel.z - accel_filt.z;
362 
363  //Bound the acceleration error so that the linearization still holds
364  Bound(a_diff.x, -6.0, 6.0);
365  Bound(a_diff.y, -6.0, 6.0);
366  Bound(a_diff.z, -9.0, 9.0);
367 
368  //If the thrust to specific force ratio has been defined, include vertical control
369  //else ignore the vertical acceleration error
370 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
371 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
372  a_diff.z = 0.0;
373 #endif
374 #endif
375 
376  //Calculate roll,pitch and thrust command
377  MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
378 
379  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
380 
381  //Coordinated turn
382  //feedforward estimate angular rotation omega = g*tan(phi)/v
383  float omega;
384  const float max_phi = RadOfDeg(60.0);
385  float airspeed_turn = airspeed;
386  //We are dividing by the airspeed, so a lower bound is important
387  Bound(airspeed_turn,10.0,30.0);
388 
389  guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
390  guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
391 
392  //Bound euler angles to prevent flipping
394  Bound(guidance_euler_cmd.theta, -RadOfDeg(120.0), RadOfDeg(25.0));
395 
396  // Use the current roll angle to determine the corresponding heading rate of change.
397  float coordinated_turn_roll = eulers_zxy.phi;
398 
400  coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0) - (guidance_euler_cmd.phi < 0.0))*guidance_euler_cmd.theta;
401  }
402 
403  if (fabs(coordinated_turn_roll) < max_phi) {
404  omega = 9.81 / airspeed_turn * tanf(coordinated_turn_roll);
405  } else { //max 60 degrees roll
406  omega = 9.81 / airspeed_turn * 1.72305 * ((coordinated_turn_roll > 0.0) - (coordinated_turn_roll < 0.0));
407  }
408 
409 #ifdef FWD_SIDESLIP_GAIN
410  // Add sideslip correction
411  omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN;
412 #endif
413 
414 // For a hybrid it is important to reduce the sideslip, which is done by changing the heading.
415 // For experiments, it is possible to fix the heading to a different value.
416 #ifndef KNIFE_EDGE_TEST
418  *heading_sp = ANGLE_FLOAT_OF_BFP(nav_heading);
419  } else {
420  *heading_sp += omega / PERIODIC_FREQUENCY;
421  FLOAT_ANGLE_NORMALIZE(*heading_sp);
422  }
423 #endif
424 
425  guidance_euler_cmd.psi = *heading_sp;
426 
427 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
428  guidance_indi_filter_thrust();
429 
430  //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
432  Bound(thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
433 
434 #if GUIDANCE_INDI_RC_DEBUG
436  thrust_in = 0;
437  }
438 #endif
439 
440  //Overwrite the thrust command from guidance_v
441  stabilization_cmd[COMMAND_THRUST] = thrust_in;
442 #endif
443 
444  // Set the quaternion setpoint from eulers_zxy
445  struct FloatQuat sp_quat;
447  float_quat_normalize(&sp_quat);
449 }
450 
451 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
452 
455 void guidance_indi_filter_thrust(void)
456 {
457  // Actuator dynamics
458  thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
459 
460  // same filter as for the acceleration
462 }
463 #endif
464 
472  struct NedCoor_f *accel = stateGetAccelNed_f();
473  update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
474  update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
475  update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
476 
479 
480  // Propagate filter for sideslip correction
481  float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
482  update_butterworth_2_low_pass(&accely_filt, accely);
483 }
484 
493 
494  /*Pre-calculate sines and cosines*/
495  float sphi = sinf(eulers_zxy.phi);
496  float cphi = cosf(eulers_zxy.phi);
497  float stheta = sinf(eulers_zxy.theta);
498  float ctheta = cosf(eulers_zxy.theta);
499  float spsi = sinf(eulers_zxy.psi);
500  float cpsi = cosf(eulers_zxy.psi);
501  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
502 
503 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
504 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
505 #endif
506 
507  /*Amount of lift produced by the wing*/
508  float pitch_lift = eulers_zxy.theta;
509  Bound(pitch_lift,-M_PI_2,0);
510  float lift = sinf(pitch_lift)*9.81;
511  float T = cosf(pitch_lift)*-9.81;
512 
513  // get the derivative of the lift wrt to theta
515 
516  RMAT_ELMT(*Gmat, 0, 0) = cphi*ctheta*spsi*T + cphi*spsi*lift;
517  RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
518  RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*T -sphi*lift;
519  RMAT_ELMT(*Gmat, 0, 1) = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
520  RMAT_ELMT(*Gmat, 1, 1) = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
521  RMAT_ELMT(*Gmat, 2, 1) = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
522  RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
523  RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
524  RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
525 }
526 
534 float guidance_indi_get_liftd(float airspeed, float theta) {
535  float liftd = 0.0;
536  if(airspeed < 12) {
537  float pitch_interp = DegOfRad(theta);
538  Bound(pitch_interp, -80.0, -40.0);
539  float ratio = (pitch_interp + 40.0)/(-40.);
540  liftd = -24.0*ratio*lift_pitch_eff/0.12;
541  } else {
542  liftd = -(airspeed - 8.5)*lift_pitch_eff/M_PI*180.0;
543  }
544  //TODO: bound liftd
545  return liftd;
546 }
547 
557 struct FloatVect3 nav_get_speed_setpoint(float pos_gain) {
558  struct FloatVect3 speed_sp;
560  speed_sp = nav_get_speed_sp_from_line(line_vect, to_end_vect, navigation_target, pos_gain);
561  } else {
562  speed_sp = nav_get_speed_sp_from_go(navigation_target, pos_gain);
563  }
564  return speed_sp;
565 }
566 
576 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) {
577 
578  // enu -> ned
579  struct FloatVect2 line_v = {line_v_enu.y, line_v_enu.x};
580  struct FloatVect2 to_end_v = {to_end_v_enu.y, to_end_v_enu.x};
581 
582  struct NedCoor_f ned_target;
583  // Target in NED instead of ENU
585 
586  // Calculate magnitude of the desired speed vector based on distance to waypoint
587  float dist_to_target = float_vect2_norm(&to_end_v);
588  float desired_speed;
589  if(force_forward) {
590  desired_speed = nav_max_speed;
591  } else {
592  desired_speed = dist_to_target * pos_gain;
593  Bound(desired_speed, 0.0, nav_max_speed);
594  }
595 
596  // Calculate length of line segment
597  float length_line = float_vect2_norm(&line_v);
598  if(length_line < 0.01) {
599  length_line = 0.01;
600  }
601 
602  //Normal vector to the line, with length of the line
603  struct FloatVect2 normalv;
604  VECT2_ASSIGN(normalv, -line_v.y, line_v.x);
605  // Length of normal vector is the same as of the line segment
606  float length_normalv = length_line;
607  if(length_normalv < 0.01) {
608  length_normalv = 0.01;
609  }
610 
611  // Distance along the normal vector
612  float dist_to_line = (to_end_v.x*normalv.x + to_end_v.y*normalv.y)/length_normalv;
613 
614  // Normal vector scaled to be the distance to the line
615  struct FloatVect2 v_to_line, v_along_line;
616  v_to_line.x = dist_to_line*normalv.x/length_normalv*guidance_indi_line_gain;
617  v_to_line.y = dist_to_line*normalv.y/length_normalv*guidance_indi_line_gain;
618 
619  // Depending on the normal vector, the distance could be negative
620  float dist_to_line_abs = fabs(dist_to_line);
621 
622  // The distance that needs to be traveled along the line
623  /*float dist_along_line = (line_v.x*to_end_v.x + line_v.y*to_end_v.y)/length_line;*/
624  v_along_line.x = line_v.x/length_line*50.0;
625  v_along_line.y = line_v.y/length_line*50.0;
626 
627  // Calculate the desired direction to converge to the line
628  struct FloatVect2 direction;
629  VECT2_SMUL(direction, v_along_line, (1.0/(1+dist_to_line_abs*0.05)));
630  VECT2_ADD(direction, v_to_line);
631  float length_direction = float_vect2_norm(&direction);
632  if(length_direction < 0.01) {
633  length_direction = 0.01;
634  }
635 
636  // Scale to have the desired speed
637  struct FloatVect2 final_vector;
638  VECT2_SMUL(final_vector, direction, desired_speed/length_direction);
639 
640  struct FloatVect3 speed_sp_return = {final_vector.x, final_vector.y, gih_params.pos_gainz*(ned_target.z - stateGetPositionNed_f()->z)};
642  speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
643  }
644 
645  // Bound vertical speed setpoint
646  if(stateGetAirspeed_f() > 13.0) {
647  Bound(speed_sp_return.z, -4.0, 5.0);
648  } else {
649  Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
650  }
651 
652  return speed_sp_return;
653 }
654 
662 struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain) {
663  // The speed sp that will be returned
664  struct FloatVect3 speed_sp_return;
665  struct NedCoor_f ned_target;
666  // Target in NED instead of ENU
668 
669  // Calculate position error
670  struct FloatVect3 pos_error;
671  struct NedCoor_f *pos = stateGetPositionNed_f();
672  VECT3_DIFF(pos_error, ned_target, *pos);
673 
674  VECT3_SMUL(speed_sp_return, pos_error, pos_gain);
675  speed_sp_return.z = gih_params.pos_gainz*pos_error.z;
676 
678  speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
679  }
680 
681  if(force_forward) {
682  vect_scale(&speed_sp_return, nav_max_speed);
683  } else {
684  // Calculate distance to waypoint
685  float dist_to_wp = FLOAT_VECT2_NORM(pos_error);
686 
687  // Calculate max speed to decelerate from
688 
689  // dist_to_wp can only be positive, but just in case
690  float max_speed_decel2 = 2*dist_to_wp*MAX_DECELERATION;
691  if(max_speed_decel2 < 0.0) {
692  max_speed_decel2 = fabs(max_speed_decel2);
693  }
694  float max_speed_decel = sqrtf(max_speed_decel2);
695 
696  // Bound the setpoint velocity vector
697  float max_h_speed = Min(nav_max_speed, max_speed_decel);
698  vect_bound_in_2d(&speed_sp_return, max_h_speed);
699  }
700 
701  // Bound vertical speed setpoint
702  if(stateGetAirspeed_f() > 13.0) {
703  Bound(speed_sp_return.z, -4.0, 5.0);
704  } else {
705  Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
706  }
707 
708  return speed_sp_return;
709 }
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
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