Paparazzi UAS  v5.17_devel-24-g2ae834f
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 
72 #ifdef GUIDANCE_INDI_MAX_AIRSPEED
73 // Max ground speed that will be commanded
74 #define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + 10.0)
75 float nav_max_speed = NAV_MAX_SPEED;
76 #endif
77 #ifndef MAX_DECELERATION
78 #define MAX_DECELERATION 1.
79 #endif
80 
81 struct FloatVect3 sp_accel = {0.0,0.0,0.0};
82 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
83 float thrust_in_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
84 static void guidance_indi_filter_thrust(void);
85 
86 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS
87 #ifndef STABILIZATION_INDI_ACT_DYN_P
88 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
89 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
90 #define GUIDANCE_INDI_THRUST_DYNAMICS STABILIZATION_INDI_ACT_DYN_P
91 #endif
92 #endif //GUIDANCE_INDI_THRUST_DYNAMICS
93 
94 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
95 
96 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
97 #ifdef STABILIZATION_INDI_FILT_CUTOFF
98 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
99 #else
100 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
101 #endif
102 #endif
103 
104 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
105 #error "You must have an airspeed sensor to use this guidance"
106 #endif
107 float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
108 
109 float inv_eff[4];
110 
111 float lift_pitch_eff = GUIDANCE_INDI_PITCH_LIFT_EFF;
112 
115 
116 float thrust_act = 0;
122 
124 
125 struct FloatMat33 Ga;
128 
130 
132 float thrust_in;
133 
134 struct FloatVect3 speed_sp = {0.0, 0.0, 0.0};
135 
137 static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
138 static float guidance_indi_get_liftd(float pitch, float theta);
139 struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain);
140 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);
141 struct FloatVect3 nav_get_speed_setpoint(float pos_gain);
142 
147 {
148  /*AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);*/
149 
150  float tau = 1.0/(2.0*M_PI*filter_cutoff);
151  float sample_time = 1.0/PERIODIC_FREQUENCY;
152  for(int8_t i=0; i<3; i++) {
153  init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
154  }
155  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
156  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
157  init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
158  init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
159 }
160 
166  thrust_in = 0.0;
167  thrust_act = 0;
168 }
169 
176 void guidance_indi_run(float *heading_sp) {
177 
178  /*Obtain eulers with zxy rotation order*/
180 
181  /*Calculate the transition percentage so that the ctrl_effecitveness scheduling works*/
184  const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
187 
188  //filter accel to get rid of noise and filter attitude to synchronize with accel
190 
191  //Linear controller to find the acceleration setpoint from position and velocity
192  float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
193  float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
195 
196  if(autopilot.mode == AP_MODE_NAV) {
197  speed_sp = nav_get_speed_setpoint(gih_params.pos_gain);
198  } else{
199  speed_sp.x = pos_x_err * gih_params.pos_gain;
200  speed_sp.y = pos_y_err * gih_params.pos_gain;
201  speed_sp.z = pos_z_err * gih_params.pos_gainz;
202  }
203 
204  //for rc control horizontal, rotate from body axes to NED
205  float psi = eulers_zxy.psi;
206  /*NAV mode*/
207  float speed_sp_b_x = cosf(psi) * speed_sp.x + sinf(psi) * speed_sp.y;
208  float speed_sp_b_y =-sinf(psi) * speed_sp.x + cosf(psi) * speed_sp.y;
209 
210  float airspeed = stateGetAirspeed_f();
211 
212  struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
213  struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
214  struct FloatVect2 windspeed;
215  VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
216 
217  VECT2_DIFF(desired_airspeed, speed_sp, windspeed); // Use 2d part of speed_sp
218  float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
219 
220  // Make turn instead of straight line
221  if((airspeed > 10.0) && (norm_des_as > 12.0)) {
222 
223  // Give the wind cancellation priority.
224  if (norm_des_as > guidance_indi_max_airspeed) {
225  float groundspeed_factor = 0.0;
226 
227  // if the wind is faster than we can fly, just fly in the wind direction
229  float av = speed_sp.x * speed_sp.x + speed_sp.y * speed_sp.y;
230  float bv = -2 * (windspeed.x * speed_sp.x + windspeed.y * speed_sp.y);
231  float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
232 
233  float dv = bv * bv - 4.0 * av * cv;
234 
235  // dv can only be positive, but just in case
236  if(dv < 0) {
237  dv = fabs(dv);
238  }
239  float d_sqrt = sqrtf(dv);
240 
241  groundspeed_factor = (-bv + d_sqrt) / (2 * av);
242  }
243 
244  desired_airspeed.x = groundspeed_factor * speed_sp.x - windspeed.x;
245  desired_airspeed.y = groundspeed_factor * speed_sp.y - windspeed.y;
246 
247  speed_sp_b_x = guidance_indi_max_airspeed;
248  }
249 
250  // desired airspeed can not be larger than max airspeed
251  speed_sp_b_x = Min(norm_des_as,guidance_indi_max_airspeed);
252 
253  // Calculate accel sp in body axes, because we need to regulate airspeed
254  struct FloatVect2 sp_accel_b;
255  // In turn acceleration proportional to heading diff
256  sp_accel_b.y = atan2(desired_airspeed.y, desired_airspeed.x) - psi;
257  FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
258  sp_accel_b.y *= GUIDANCE_INDI_HEADING_BANK_GAIN;
259 
260  // Control the airspeed
261  sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
262 
263  sp_accel.x = cosf(psi) * sp_accel_b.x - sinf(psi) * sp_accel_b.y;
264  sp_accel.y = sinf(psi) * sp_accel_b.x + cosf(psi) * sp_accel_b.y;
265 
266  sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
267  } else { // Go somewhere in the shortest way
268 
269  if(airspeed > 10.0) {
270  // Groundspeed vector in body frame
271  float groundspeed_x = cosf(psi) * stateGetSpeedNed_f()->x + sinf(psi) * stateGetSpeedNed_f()->y;
272  float speed_increment = speed_sp_b_x - groundspeed_x;
273 
274  // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
275  if((speed_increment + airspeed) > guidance_indi_max_airspeed) {
276  speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
277  }
278  }
279  speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
280  speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
281 
282  sp_accel.x = (speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
283  sp_accel.y = (speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
284  sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
285  }
286 
287  // Bound the acceleration setpoint
288  float accelbound = 3.0 + airspeed/guidance_indi_max_airspeed*5.0;
289  vect_bound_in_2d(&sp_accel, accelbound);
290  /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
291  /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
292  BoundAbs(sp_accel.z, 3.0);
293 
294 #if GUIDANCE_INDI_RC_DEBUG
295 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
296  //for rc control horizontal, rotate from body axes to NED
297  float psi = eulers_zxy.psi;
298  float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
299  float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
300  sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
301  sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
302 
303  //for rc vertical control
304  sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
305 #endif
306 
307  //Calculate matrix of partial derivatives
309  //Invert this matrix
310  MAT33_INV(Ga_inv, Ga);
311 
312  struct FloatVect3 accel_filt;
313  accel_filt.x = filt_accel_ned[0].o[0];
314  accel_filt.y = filt_accel_ned[1].o[0];
315  accel_filt.z = filt_accel_ned[2].o[0];
316 
317  struct FloatVect3 a_diff = { sp_accel.x - accel_filt.x, sp_accel.y - accel_filt.y, sp_accel.z - accel_filt.z};
318 
319  //Bound the acceleration error so that the linearization still holds
320  Bound(a_diff.x, -6.0, 6.0);
321  Bound(a_diff.y, -6.0, 6.0);
322  Bound(a_diff.z, -9.0, 9.0);
323 
324  //If the thrust to specific force ratio has been defined, include vertical control
325  //else ignore the vertical acceleration error
326 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
327 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
328  a_diff.z = 0.0;
329 #endif
330 #endif
331 
332  //Calculate roll,pitch and thrust command
333  MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
334 
335  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
336 
337  //Coordinated turn
338  //feedforward estimate angular rotation omega = g*tan(phi)/v
339  float omega;
340  const float max_phi = RadOfDeg(60.0);
341  float airspeed_turn = airspeed;
342  //We are dividing by the airspeed, so a lower bound is important
343  Bound(airspeed_turn,10.0,30.0);
344 
345  guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
346  guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
347 
348  //Bound euler angles to prevent flipping
350  Bound(guidance_euler_cmd.theta, -RadOfDeg(120.0), RadOfDeg(25.0));
351 
352  float coordinated_turn_roll = guidance_euler_cmd.phi;
353 
355  coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0) - (guidance_euler_cmd.phi < 0.0))*guidance_euler_cmd.theta;
356  }
357 
358  if (fabs(coordinated_turn_roll) < max_phi) {
359  omega = 9.81 / airspeed_turn * tanf(coordinated_turn_roll);
360  } else { //max 60 degrees roll
361  omega = 9.81 / airspeed_turn * 1.72305 * ((coordinated_turn_roll > 0.0) - (coordinated_turn_roll < 0.0));
362  }
363 
364 #ifdef FWD_SIDESLIP_GAIN
365  // Add sideslip correction
366  omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN;
367 #endif
368 
369 #ifndef KNIFE_EDGE_TEST
370  *heading_sp += omega / PERIODIC_FREQUENCY;
371  FLOAT_ANGLE_NORMALIZE(*heading_sp);
372 #endif
373 
374  guidance_euler_cmd.psi = *heading_sp;
375 
376 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
377  guidance_indi_filter_thrust();
378 
379  //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
380  thrust_in = thrust_filt.o[0] + euler_cmd.z*thrust_in_specific_force_gain;
381  Bound(thrust_in, 0, 9600);
382 
383 #if GUIDANCE_INDI_RC_DEBUG
385  thrust_in = 0;
386  }
387 #endif
388 
389  //Overwrite the thrust command from guidance_v
390  stabilization_cmd[COMMAND_THRUST] = thrust_in;
391 #endif
392 
393  // Set the quaternion setpoint from eulers_zxy
394  struct FloatQuat sp_quat;
396  float_quat_normalize(&sp_quat);
398 }
399 
400 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
401 
404 void guidance_indi_filter_thrust(void)
405 {
406  // Actuator dynamics
407  thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
408 
409  // same filter as for the acceleration
411 }
412 #endif
413 
421  struct NedCoor_f *accel = stateGetAccelNed_f();
422  update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
423  update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
424  update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
425 
428 
429  // Propagate filter for sideslip correction
430  float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
431  update_butterworth_2_low_pass(&accely_filt, accely);
432 }
433 
442 
443  /*Pre-calculate sines and cosines*/
444  float sphi = sinf(eulers_zxy.phi);
445  float cphi = cosf(eulers_zxy.phi);
446  float stheta = sinf(eulers_zxy.theta);
447  float ctheta = cosf(eulers_zxy.theta);
448  float spsi = sinf(eulers_zxy.psi);
449  float cpsi = cosf(eulers_zxy.psi);
450  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
451 
452 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
453 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
454 #endif
455 
456  /*Amount of lift produced by the wing*/
457  float pitch_lift = eulers_zxy.theta;
458  Bound(pitch_lift,-M_PI_2,0);
459  float lift = sinf(pitch_lift)*9.81;
460  float T = cosf(pitch_lift)*-9.81;
461 
462  // get the derivative of the lift wrt to theta
464 
465  RMAT_ELMT(*Gmat, 0, 0) = cphi*ctheta*spsi*T + cphi*spsi*lift;
466  RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
467  RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*T -sphi*lift;
468  RMAT_ELMT(*Gmat, 0, 1) = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
469  RMAT_ELMT(*Gmat, 1, 1) = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
470  RMAT_ELMT(*Gmat, 2, 1) = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
471  RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
472  RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
473  RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
474 }
475 
483 float guidance_indi_get_liftd(float airspeed, float theta) {
484  float liftd = 0.0;
485  if(airspeed < 12) {
486  float pitch_interp = DegOfRad(theta);
487  Bound(pitch_interp, -80.0, -40.0);
488  float ratio = (pitch_interp + 40.0)/(-40.);
489  liftd = -24.0*ratio;
490  } else {
491  liftd = -(airspeed - 8.5)*lift_pitch_eff/M_PI*180.0;
492  }
493  //TODO: bound liftd
494  return liftd;
495 }
496 
506 struct FloatVect3 nav_get_speed_setpoint(float pos_gain) {
507  struct FloatVect3 speed_sp;
509  speed_sp = nav_get_speed_sp_from_line(line_vect, to_end_vect, navigation_target, pos_gain);
510  } else {
511  speed_sp = nav_get_speed_sp_from_go(navigation_target, pos_gain);
512  }
513  return speed_sp;
514 }
515 
525 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) {
526 
527  // enu -> ned
528  struct FloatVect2 line_v = {line_v_enu.y, line_v_enu.x};
529  struct FloatVect2 to_end_v = {to_end_v_enu.y, to_end_v_enu.x};
530 
531  struct NedCoor_f ned_target;
532  // Target in NED instead of ENU
534 
535  // Calculate magnitude of the desired speed vector based on distance to waypoint
536  float dist_to_target = float_vect2_norm(&to_end_v);
537  float desired_speed;
538  if(force_forward) {
539  desired_speed = nav_max_speed;
540  } else {
541  desired_speed = dist_to_target * pos_gain;
542  Bound(desired_speed, 0.0, nav_max_speed);
543  }
544 
545  // Calculate length of line segment
546  float length_line = float_vect2_norm(&line_v);
547  if(length_line < 0.01) {
548  length_line = 0.01;
549  }
550 
551  //Normal vector to the line, with length of the line
552  struct FloatVect2 normalv;
553  VECT2_ASSIGN(normalv, -line_v.y, line_v.x);
554  // Length of normal vector is the same as of the line segment
555  float length_normalv = length_line;
556  if(length_normalv < 0.01) {
557  length_normalv = 0.01;
558  }
559 
560  // Distance along the normal vector
561  float dist_to_line = (to_end_v.x*normalv.x + to_end_v.y*normalv.y)/length_normalv;
562 
563  // Normal vector scaled to be the distance to the line
564  struct FloatVect2 v_to_line, v_along_line;
565  v_to_line.x = dist_to_line*normalv.x/length_normalv;
566  v_to_line.y = dist_to_line*normalv.y/length_normalv;
567 
568  // Depending on the normal vector, the distance could be negative
569  float dist_to_line_abs = fabs(dist_to_line);
570 
571  // The distance that needs to be traveled along the line
572  /*float dist_along_line = (line_v.x*to_end_v.x + line_v.y*to_end_v.y)/length_line;*/
573  v_along_line.x = line_v.x/length_line*50.0;
574  v_along_line.y = line_v.y/length_line*50.0;
575 
576  // Calculate the desired direction to converge to the line
577  struct FloatVect2 direction;
578  VECT2_SMUL(direction, v_along_line, (1.0/(1+dist_to_line_abs*0.05)));
579  VECT2_ADD(direction, v_to_line);
580  float length_direction = float_vect2_norm(&direction);
581  if(length_direction < 0.01) {
582  length_direction = 0.01;
583  }
584 
585  // Scale to have the desired speed
586  struct FloatVect2 final_vector;
587  VECT2_SMUL(final_vector, direction, desired_speed/length_direction);
588 
589  struct FloatVect3 speed_sp_return = {final_vector.x, final_vector.y, gih_params.pos_gainz*(ned_target.z - stateGetPositionNed_f()->z)};
591  speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
592  }
593 
594  // Bound vertical speed setpoint
595  if(stateGetAirspeed_f() > 13.0) {
596  Bound(speed_sp_return.z, -4.0, 5.0);
597  } else {
598  Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
599  }
600 
601  return speed_sp_return;
602 }
603 
611 struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain) {
612  // The speed sp that will be returned
613  struct FloatVect3 speed_sp_return;
614  struct NedCoor_f ned_target;
615  // Target in NED instead of ENU
617 
618  // Calculate position error
619  struct FloatVect3 pos_error;
620  struct NedCoor_f *pos = stateGetPositionNed_f();
621  VECT3_DIFF(pos_error, ned_target, *pos);
622 
623  VECT3_SMUL(speed_sp_return, pos_error, pos_gain);
624  speed_sp_return.z = gih_params.pos_gainz*pos_error.z;
625 
627  speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
628  }
629 
630  if(force_forward) {
631  vect_scale(&speed_sp_return, nav_max_speed);
632  } else {
633  // Calculate distance to waypoint
634  float dist_to_wp = FLOAT_VECT2_NORM(pos_error);
635  // Calculate max speed to decelerate from
636  float max_speed_decel = sqrt(2*dist_to_wp*MAX_DECELERATION);
637 
638  // Bound the setpoint velocity vector
639  float max_h_speed = Min(nav_max_speed, max_speed_decel);
640  vect_bound_in_2d(&speed_sp_return, max_h_speed);
641  }
642 
643  // Bound vertical speed setpoint
644  if(stateGetAirspeed_f() > 13.0) {
645  Bound(speed_sp_return.z, -4.0, 5.0);
646  } else {
647  Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
648  }
649 
650  return speed_sp_return;
651 }
struct EnuCoor_i navigation_target
Definition: navigation.c:91
#define FLOAT_ANGLE_NORMALIZE(_a)
int32_t transition_percentage
Definition: guidance_h.c:92
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
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
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
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 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
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
#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.
#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
struct FloatEulers guidance_euler_cmd
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
signed long int32_t
Definition: types.h:19
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
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.
signed char int8_t
Definition: types.h:15
#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]
Rotorcraft attitude reference generation.
#define GUIDANCE_INDI_POS_GAINZ
struct FloatVect3 sp_accel
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
float nav_max_speed
float x
in meters
INS for rotorcrafts combining vertical and horizontal filters.
float thrust_act