29#include "generated/modules.h"
46 float sphi = sinf(eulers_filtered.
phi);
47 float cphi = cosf(eulers_filtered.
phi);
48 float stheta = sinf(eulers_filtered.
theta);
49 float ctheta = cosf(eulers_filtered.
theta);
50 float spsi = sinf(eulers_filtered.
psi);
51 float cpsi = cosf(eulers_filtered.
psi);
54#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
55#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
59 float pitch_lift = eulers_filtered.
theta;
60 Bound(pitch_lift,-M_PI_2,0);
61 float lift = sinf(pitch_lift)*9.81;
62 float T = cosf(pitch_lift)*-9.81;
67 Gmat[0][0] = cphi*ctheta*spsi*T + cphi*spsi*lift;
68 Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
69 Gmat[2][0] = -sphi*ctheta*T -sphi*lift;
73 Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi;
74 Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi;
75 Gmat[2][2] = cphi*ctheta;
83#if GUIDANCE_INDI_HYBRID_USE_WLS
100 wls_guid_p.u_pref[0] = -roll_angle;
101 wls_guid_p.u_pref[1] = -pitch_angle;
102 wls_guid_p.u_pref[2] = wls_guid_p.u_max[2];
void float_eulers_of_quat_zxy(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
static float stateGetAirspeed_f(void)
Get airspeed (float).
QuatButterworthLowPass quat_filt
float guidance_indi_max_bank
static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU]
float guidance_indi_min_pitch
float WEAK guidance_indi_get_liftd(float airspeed, float theta)
Get the derivative of lift w.r.t.
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
#define GUIDANCE_INDI_PITCH_EFF_SCALING
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
#define GUIDANCE_INDI_MAX_PITCH
void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V])
Calculate the matrix of partial derivatives of the roll, pitch and thrust w.r.t.
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
float actuator_state_filt_vect[INDI_NUM_ACT]
API to get/set the generic vehicle states.