30 #include "generated/airframe.h"
48 #ifndef GUIDANCE_INDI_SPEED_GAIN
49 #define GUIDANCE_INDI_SPEED_GAIN 1.8
50 #define GUIDANCE_INDI_SPEED_GAINZ 1.8
53 #ifndef GUIDANCE_INDI_POS_GAIN
54 #define GUIDANCE_INDI_POS_GAIN 0.5
55 #define GUIDANCE_INDI_POS_GAINZ 0.5
58 #ifndef GUIDANCE_INDI_LIFTD_ASQ
59 #define GUIDANCE_INDI_LIFTD_ASQ 0.20
66 #ifndef GUIDANCE_INDI_LIFTD_P50
67 #define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12)
68 #define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
78 .heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
84 #ifndef GUIDANCE_INDI_MAX_AIRSPEED
85 #error "You must have an airspeed sensor to use this guidance"
94 #if GUIDANCE_INDI_HYBRID_USE_WLS
95 #if GUIDANCE_INDI_HYBRID_U > WLS_N_U
96 #error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file
99 #if GUIDANCE_INDI_HYBRID_V > WLS_N_V
100 #error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file
107 #ifndef GUIDANCE_INDI_ZERO_AIRSPEED
108 #define GUIDANCE_INDI_ZERO_AIRSPEED FALSE
112 #ifndef TURN_AIRSPEED_TH
113 #define TURN_AIRSPEED_TH 10.0
122 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
124 static void guidance_indi_filter_thrust(
void);
126 #ifdef GUIDANCE_INDI_THRUST_DYNAMICS
127 #warning GUIDANCE_INDI_THRUST_DYNAMICS is deprecated, use GUIDANCE_INDI_THRUST_DYNAMICS_FREQ instead.
128 #warning "The thrust dynamics are now specified in continuous time with the corner frequency of the first order model!"
129 #warning "define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ in rad/s"
130 #warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
133 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
134 #ifndef STABILIZATION_INDI_ACT_FREQ_P
135 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
137 #define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
143 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
144 #ifdef STABILIZATION_INDI_FILT_CUTOFF
145 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
147 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
151 #ifndef GUIDANCE_INDI_CLIMB_SPEED_FWD
152 #define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0
155 #ifndef GUIDANCE_INDI_DESCEND_SPEED_FWD
156 #define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0
181 float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
184 #if GUIDANCE_INDI_HYBRID_USE_WLS
186 float du_min_gih[GUIDANCE_INDI_HYBRID_U];
187 float du_max_gih[GUIDANCE_INDI_HYBRID_U];
188 float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
189 float *Bwls_gih[GUIDANCE_INDI_HYBRID_V];
190 #ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES
191 float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES;
193 float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f };
195 #ifdef GUIDANCE_INDI_HYBRID_WLS_WU
196 float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_HYBRID_WLS_WU;
198 float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f };
214 #ifndef GUIDANCE_INDI_VEL_SP_ID
215 #define GUIDANCE_INDI_VEL_SP_ID ABI_BROADCAST
224 #if PERIODIC_TELEMETRY
228 pprz_msg_send_GUIDANCE_INDI_HYBRID(trans,
dev, AC_ID,
243 #if GUIDANCE_INDI_HYBRID_USE_WLS
244 static void debug(
struct transport_tx *trans,
struct link_device *
dev,
char* name,
float* data,
int datasize)
246 pprz_msg_send_DEBUG_VECT(trans,
dev,AC_ID,
260 debug(trans,
dev,
"du_min_gih", du_min_gih, GUIDANCE_INDI_HYBRID_U);
263 debug(trans,
dev,
"du_max_gih", du_max_gih, GUIDANCE_INDI_HYBRID_U);
266 debug(trans,
dev,
"du_pref_gih", du_pref_gih, GUIDANCE_INDI_HYBRID_U);
269 debug(trans,
dev,
"Wu_gih", Wu_gih, GUIDANCE_INDI_HYBRID_U);
272 debug(trans,
dev,
"Wv_gih", Wv_gih, GUIDANCE_INDI_HYBRID_V);
275 debug(trans,
dev,
"Bwls_gih[0]", Bwls_gih[0], GUIDANCE_INDI_HYBRID_U);
296 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
297 #ifdef GUIDANCE_INDI_THRUST_DYNAMICS
300 thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
305 float sample_time = 1.0/PERIODIC_FREQUENCY;
306 for(
int8_t i=0; i<3; i++) {
314 #if GUIDANCE_INDI_HYBRID_USE_WLS
315 for (
int8_t i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) {
320 #if PERIODIC_TELEMETRY
340 float sample_time = 1.0 / PERIODIC_FREQUENCY;
341 for (
int8_t i = 0; i < 3; i++) {
380 #if GUIDANCE_INDI_RC_DEBUG
381 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
386 sp_accel.
x = cosf(psi) * rc_x - sinf(psi) * rc_y;
387 sp_accel.
y = sinf(psi) * rc_x + cosf(psi) * rc_y;
404 Bound(a_diff.x, -6.0, 6.0);
405 Bound(a_diff.y, -6.0, 6.0);
406 Bound(a_diff.z, -9.0, 9.0);
410 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
411 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
420 #if GUIDANCE_INDI_HYBRID_USE_WLS
425 float du_gih[GUIDANCE_INDI_HYBRID_U];
428 du_gih,
v_gih, du_min_gih, du_max_gih,
429 Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10,
430 GUIDANCE_INDI_HYBRID_U, GUIDANCE_INDI_HYBRID_V);
445 #if GUIDANCE_INDI_HYBRID_U > 3
446 thrust_vect.
x = du_gih[3];
457 const float max_phi = RadOfDeg(60.0f);
458 #if GUIDANCE_INDI_ZERO_AIRSPEED
459 float airspeed_turn = 0.f;
464 Bound(airspeed_turn, 10.0f, 30.0f);
487 if (fabsf(coordinated_turn_roll) < max_phi) {
488 omega = 9.81f / airspeed_turn * tanf(coordinated_turn_roll);
490 omega = 9.81f / airspeed_turn * 1.72305f * ((coordinated_turn_roll > 0.0f) - (coordinated_turn_roll < 0.0f));
493 #ifdef FWD_SIDESLIP_GAIN
504 ff_rates.
p = -sinf(euler_zyx->
theta) * omega;
505 ff_rates.q = cosf(euler_zyx->
theta) * sinf(euler_zyx->
phi) * omega;
506 ff_rates.r = cosf(euler_zyx->
theta) * cosf(euler_zyx->
phi) * omega;
519 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
520 float delta_limit = STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
524 if (delta_psi > delta_limit) {
526 }
else if (delta_psi < -delta_limit) {
534 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
535 guidance_indi_filter_thrust();
539 Bound(
thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
541 #if GUIDANCE_INDI_RC_DEBUG
562 struct FloatVect3 accel_sp = { 0.f, 0.f, 0.f };
568 float cpsi = cosf(psi);
569 float spsi = sinf(psi);
574 #if GUIDANCE_INDI_ZERO_AIRSPEED
575 float airspeed = 0.f;
580 struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed };
582 VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
592 float groundspeed_factor = 0.0f;
600 float dv = bv * bv - 4.0f * av * cv;
606 float d_sqrt = sqrtf(dv);
608 groundspeed_factor = (-bv + d_sqrt) / (2.0f * av);
634 accel_sp.
x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y;
635 accel_sp.
y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y;
640 if (airspeed > 10.f) {
643 float speed_increment = speed_sp_b_x - groundspeed_x;
651 gi_speed_sp.
x = cpsi * speed_sp_b_x - spsi * speed_sp_b_y;
652 gi_speed_sp.
y = spsi * speed_sp_b_x + cpsi * speed_sp_b_y;
664 BoundAbs(accel_sp.
z, 3.0);
755 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
759 void guidance_indi_filter_thrust(
void)
801 if (airspeed < 12.f) {
806 float pitch_interp = DegOfRad(theta);
807 const float min_pitch = -80.0f;
808 const float middle_pitch = -50.0f;
809 const float max_pitch = -20.0f;
811 Bound(pitch_interp, min_pitch, max_pitch);
812 if (pitch_interp > middle_pitch) {
813 float ratio = (pitch_interp - max_pitch)/(middle_pitch - max_pitch);
816 float ratio = (pitch_interp - middle_pitch)/(min_pitch - middle_pitch);
839 #if GUIDANCE_INDI_HYBRID_USE_AS_DEFAULT
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
#define THRUST_INCREMENT_ID
Core autopilot interface common to all firmwares.
static void float_quat_normalize(struct FloatQuat *q)
#define FLOAT_ANGLE_NORMALIZE(_a)
void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
Multiply 3D matrix with vector.
void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound)
void float_quat_of_eulers_zxy(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_eulers_of_quat_zxy(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
3x3 matrix inverse
#define FLOAT_VECT2_NORM(_v)
#define VECT2_DIFF(_c, _a, _b)
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_PERCENTAGE_FRAC
#define POS_FLOAT_OF_BFP(_ai)
#define SPEED_FLOAT_OF_BFP(_ai)
#define BFP_OF_REAL(_vr, _frac)
#define ACCEL_FLOAT_OF_BFP(_ai)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
static float stateGetAirspeed_f(void)
Get airspeed (float).
float guidance_indi_specific_force_gain
struct FloatVect3 indi_vel_sp
bool force_forward
forward flight for hybrid nav
Butterworth2LowPass accely_filt
static void vel_sp_cb(uint8_t sender_id, struct FloatVect3 *vel_sp)
ABI callback that obtains the velocity setpoint from a module.
Butterworth2LowPass roll_filt
#define GUIDANCE_INDI_SPEED_GAINZ
#define GUIDANCE_INDI_CLIMB_SPEED_FWD
struct guidance_indi_hybrid_params gih_params
struct FloatVect3 gi_speed_sp
float guidance_indi_min_pitch
float guidance_indi_max_bank
struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode)
struct FloatVect3 sp_accel
static void send_guidance_indi_debug(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
struct FloatEulers guidance_euler_cmd
float guidance_indi_pitch_pref_deg
#define GUIDANCE_INDI_LIFTD_ASQ
void guidance_indi_enter(void)
Call upon entering indi guidance.
Butterworth2LowPass pitch_filt
#define GUIDANCE_INDI_POS_GAIN
bool take_heading_control
float guidance_indi_hybrid_heading_sp
float WEAK guidance_indi_get_liftd(float airspeed, float theta)
Get the derivative of lift w.r.t.
#define GUIDANCE_INDI_LIFTD_P50
static void send_guidance_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
void guidance_indi_init(void)
Init function.
#define GUIDANCE_INDI_FILTER_CUTOFF
#define GUIDANCE_INDI_DESCEND_SPEED_FWD
float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]
struct FloatVect3 euler_cmd
static float bound_vz_sp(float vz_sp)
struct FloatEulers eulers_zxy
state eulers in zxy order
Butterworth2LowPass filt_accel_ned[3]
#define GUIDANCE_INDI_POS_GAINZ
float guidance_indi_max_airspeed
static struct FloatVect3 compute_accel_from_speed_sp(void)
#define GUIDANCE_INDI_VEL_SP_ID
struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
#define GUIDANCE_INDI_LIFTD_P80
Butterworth2LowPass thrust_filt
#define GUIDANCE_INDI_SPEED_GAIN
struct FloatVect2 desired_airspeed
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V])
Perform WLS.
@ GUIDANCE_INDI_HYBRID_V_POS
@ GUIDANCE_INDI_HYBRID_V_SPEED
@ GUIDANCE_INDI_HYBRID_V_ACCEL
@ GUIDANCE_INDI_HYBRID_H_SPEED
@ GUIDANCE_INDI_HYBRID_H_ACCEL
@ GUIDANCE_INDI_HYBRID_H_POS
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
#define GUIDANCE_INDI_MAX_PITCH
#define GUIDANCE_INDI_MIN_PITCH
static enum GuidanceOneloop_VMode _v_mode
void guidance_v_run_enter(void)
static struct VerticalGuidance * _gv
int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
Simple first order low pass filter with bilinear transform.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
Second order low pass filter structure.
vector in North East Down coordinates Units: meters
struct RadioControl radio_control
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
#define RADIO_ROLL
Redefining RADIO_* Do not use with radio.h (ppm rc)
Some helper functions to check RC sticks.
int32_t transition_percentage
void guidance_h_run_enter(void)
struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
struct VerticalGuidance guidance_v
int32_t z_ref
altitude reference in meters.
int32_t zd_ref
vertical speed reference in meter/s.
int32_t zdd_ref
vertical acceleration reference in meter/s^2.
struct RotorcraftNavigation nav
Rotorcraft navigation functions.
float descend_vspeed
descend speed setting, mostly used in flight plans
float heading
heading setpoint (in radians)
float climb_vspeed
climb speed setting, mostly used in flight plans
struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
int32_t transition_theta_offset
float stabilization_attitude_get_heading_f(void)
Read an attitude setpoint from the RC.
Rotorcraft attitude reference generation.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
union StabilizationSetpoint::@273 sp
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
int wls_alloc(float *u, float *v, float *umin, float *umax, float **B, float *u_guess, float *W_init, float *Wv, float *Wu, float *up, float gamma_sq, int imax, int n_u, int n_v)
active set algorithm for control allocation