32 #include "generated/modules.h"
35 #ifndef COMMAND_THRUST_X
36 #error "Quadplanes require a forward thrust actuator"
39 #ifndef GUIDANCE_INDI_THRUST_Z_EFF
40 #error "You need to define GUIDANCE_INDI_THRUST_Z_EFF"
45 #ifndef GUIDANCE_INDI_THRUST_X_EFF
46 #error "You need to define GUIDANCE_INDI_THRUST_X_EFF"
64 float sample_time = 1.0 / PERIODIC_FREQUENCY;
104 #ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
105 #define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
114 Gmat[0][0] = -sphi*stheta*lift_thrust_bz;
115 Gmat[1][0] = -cphi*lift_thrust_bz;
116 Gmat[2][0] = -sphi*ctheta*lift_thrust_bz;
122 Gmat[0][2] = cphi*stheta;
124 Gmat[2][2] = cphi*ctheta;
128 Gmat[2][3] = -stheta;
131 body_v[0] = cpsi * a_diff.
x + spsi * a_diff.
y;
132 body_v[1] = -spsi * a_diff.
x + cpsi * a_diff.
y;
133 body_v[2] = a_diff.
z;
146 du_min_gih[0] = -roll_limit_rad - roll_angle;
147 du_min_gih[1] = min_pitch_limit_rad - pitch_angle;
152 du_max_gih[0] = roll_limit_rad - roll_angle;
153 du_max_gih[1] = max_pitch_limit_rad - pitch_angle;
158 du_pref_gih[0] = -roll_angle;
159 du_pref_gih[1] = -pitch_angle + pitch_pref_rad;
160 du_pref_gih[2] = du_max_gih[2];
161 du_pref_gih[3] = body_v[0];
Core autopilot interface common to all firmwares.
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 ACCEL_FLOAT_OF_BFP(_ai)
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
float guidance_indi_pitch_pref_deg
float WEAK guidance_indi_get_liftd(float airspeed, float theta)
Get the derivative of lift w.r.t.
struct FloatEulers eulers_zxy
state eulers in zxy order
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
float guidance_indi_thrust_z_eff
float guidance_indi_thrust_x_eff
void guidance_indi_quadplane_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
float bodyz_filter_cutoff
void guidance_indi_quadplane_init(void)
Call upon entering indi guidance.
bool autopilot_in_flight_end_detection(bool motors_on UNUSED)
Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts...
#define GUIDANCE_INDI_PITCH_EFF_SCALING
void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float body_v[GUIDANCE_INDI_HYBRID_V])
Perform WLS.
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
Butterworth2LowPass accel_bodyz_filt
#define GUIDANCE_INDI_MAX_PITCH
#define GUIDANCE_INDI_MIN_PITCH
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.
#define GUIDANCE_H_MAX_BANK
Max bank controlled by guidance.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Rotorcraft attitude reference generation.
API to get/set the generic vehicle states.