30 #include "generated/airframe.h"
54 #ifndef GUIDANCE_INDI_SPEED_GAIN
55 #define GUIDANCE_INDI_SPEED_GAIN 1.8
56 #define GUIDANCE_INDI_SPEED_GAINZ 1.8
59 #ifndef GUIDANCE_INDI_POS_GAIN
60 #define GUIDANCE_INDI_POS_GAIN = 0.5;
61 #define GUIDANCE_INDI_POS_GAINZ = 0.5;
72 #ifdef GUIDANCE_INDI_MAX_AIRSPEED
74 #define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + 10.0)
77 #ifndef MAX_DECELERATION
78 #define MAX_DECELERATION 1.
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);
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
92 #endif //GUIDANCE_INDI_THRUST_DYNAMICS
94 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
96 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
97 #ifdef STABILIZATION_INDI_FILT_CUTOFF
98 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
100 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
104 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
105 #error "You must have an airspeed sensor to use this guidance"
151 float sample_time = 1.0/PERIODIC_FREQUENCY;
152 for(
int8_t i=0; i<3; i++) {
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;
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;
213 struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
215 VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
221 if((airspeed > 10.0) && (norm_des_as > 12.0)) {
225 float groundspeed_factor = 0.0;
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);
233 float dv = bv * bv - 4.0 * av * cv;
239 float d_sqrt = sqrtf(dv);
241 groundspeed_factor = (-bv + d_sqrt) / (2 * av);
258 sp_accel_b.
y *= GUIDANCE_INDI_HEADING_BANK_GAIN;
261 sp_accel_b.
x = (speed_sp_b_x - airspeed) * gih_params.
speed_gain;
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;
269 if(airspeed > 10.0) {
272 float speed_increment = speed_sp_b_x - groundspeed_x;
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;
292 BoundAbs(sp_accel.
z, 3.0);
294 #if GUIDANCE_INDI_RC_DEBUG
295 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
300 sp_accel.
x = cosf(psi) * rc_x - sinf(psi) * rc_y;
301 sp_accel.
y = sinf(psi) * rc_x + cosf(psi) * rc_y;
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];
317 struct FloatVect3 a_diff = { sp_accel.
x - accel_filt.
x, sp_accel.
y - accel_filt.
y, sp_accel.
z - accel_filt.
z};
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);
326 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
327 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
340 const float max_phi = RadOfDeg(60.0);
341 float airspeed_turn = airspeed;
343 Bound(airspeed_turn,10.0,30.0);
358 if (fabs(coordinated_turn_roll) < max_phi) {
359 omega = 9.81 / airspeed_turn * tanf(coordinated_turn_roll);
361 omega = 9.81 / airspeed_turn * 1.72305 * ((coordinated_turn_roll > 0.0) - (coordinated_turn_roll < 0.0));
364 #ifdef FWD_SIDESLIP_GAIN
366 omega -= accely_filt.
o[0]*FWD_SIDESLIP_GAIN;
369 #ifndef KNIFE_EDGE_TEST
370 *heading_sp += omega / PERIODIC_FREQUENCY;
376 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
377 guidance_indi_filter_thrust();
383 #if GUIDANCE_INDI_RC_DEBUG
400 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
404 void guidance_indi_filter_thrust(
void)
452 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
453 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
458 Bound(pitch_lift,-M_PI_2,0);
459 float lift = sinf(pitch_lift)*9.81;
460 float T = cosf(pitch_lift)*-9.81;
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;
471 RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
472 RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
486 float pitch_interp = DegOfRad(theta);
487 Bound(pitch_interp, -80.0, -40.0);
488 float ratio = (pitch_interp + 40.0)/(-40.);
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};
541 desired_speed = dist_to_target * pos_gain;
547 if(length_line < 0.01) {
555 float length_normalv = length_line;
556 if(length_normalv < 0.01) {
557 length_normalv = 0.01;
561 float dist_to_line = (to_end_v.
x*normalv.
x + to_end_v.
y*normalv.
y)/length_normalv;
565 v_to_line.
x = dist_to_line*normalv.
x/length_normalv;
566 v_to_line.
y = dist_to_line*normalv.
y/length_normalv;
569 float dist_to_line_abs = fabs(dist_to_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;
578 VECT2_SMUL(direction, v_along_line, (1.0/(1+dist_to_line_abs*0.05)));
581 if(length_direction < 0.01) {
582 length_direction = 0.01;
587 VECT2_SMUL(final_vector, direction, desired_speed/length_direction);
596 Bound(speed_sp_return.
z, -4.0, 5.0);
601 return speed_sp_return;
623 VECT3_SMUL(speed_sp_return, pos_error, pos_gain);
624 speed_sp_return.
z = gih_params.
pos_gainz*pos_error.
z;
645 Bound(speed_sp_return.
z, -4.0, 5.0);
650 return speed_sp_return;
struct EnuCoor_i navigation_target
#define FLOAT_ANGLE_NORMALIZE(_a)
int32_t transition_percentage
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.
struct FloatVect2 line_vect to_end_vect
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).
#define GUIDANCE_V_MODE_NAV
#define GUIDANCE_H_MAX_BANK
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.
Simple first order low pass filter with bilinear transform.
#define GUIDANCE_INDI_FILTER_CUTOFF
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ASSIGN(_a, _x, _y, _z)
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).
Vertical guidance for rotorcrafts.
#define SPEED_FLOAT_OF_BFP(_ai)
#define VECT2_DIFF(_c, _a, _b)
#define VECT2_ASSIGN(_a, _x, _y)
struct HorizontalGuidanceReference ref
reference calculated from setpoints
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#define GUIDANCE_INDI_SPEED_GAINZ
struct FloatVect2 desired_airspeed
#define VECT2_ADD(_a, _b)
struct pprz_autopilot autopilot
Global autopilot structure.
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
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 ...
#define BFP_OF_REAL(_vr, _frac)
#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).
static float float_vect2_norm(struct FloatVect2 *v)
struct FloatVect3 speed_sp
Architecture independent timing functions.
#define GUIDANCE_INDI_PITCH_EFF_SCALING
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
#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)
struct FloatEulers guidance_euler_cmd
static void float_quat_normalize(struct FloatQuat *q)
#define HORIZONTAL_MODE_ROUTE
struct RadioControl radio_control
struct HorizontalGuidance guidance_h
int32_t guidance_v_z_ref
altitude reference in meters.
Inertial Measurement Unit interface.
float guidance_indi_max_airspeed
#define VECT2_SMUL(_vo, _vi, _s)
#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.
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
struct Int32Vect2 pos
with INT32_POS_FRAC
#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)
vector in East North Up coordinates
void guidance_indi_run(float *heading_sp)
API to get/set the generic vehicle states.
#define INT_MULT_RSHIFT(_a, _b, _r)
#define RMAT_ELMT(_rm, _row, _col)
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.
#define GUIDANCE_INDI_POS_GAIN
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).
#define POS_FLOAT_OF_BFP(_ai)
Butterworth2LowPass thrust_filt
Second order low pass filter structure.
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
#define VERTICAL_MODE_CLIMB
Horizontal guidance for rotorcrafts.
#define MAT33_INV(_minv, _m)
Butterworth2LowPass pitch_filt
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Butterworth2LowPass filt_accel_ned[3]
Rotorcraft attitude reference generation.
#define GUIDANCE_INDI_POS_GAINZ
struct FloatVect3 sp_accel
uint8_t mode
current autopilot mode
INS for rotorcrafts combining vertical and horizontal filters.