30#include "generated/airframe.h"
49#ifndef GUIDANCE_INDI_SPEED_GAIN
50#define GUIDANCE_INDI_SPEED_GAIN 1.8
51#define GUIDANCE_INDI_SPEED_GAINZ 1.8
54#ifndef GUIDANCE_INDI_POS_GAIN
55#define GUIDANCE_INDI_POS_GAIN 0.5
56#define GUIDANCE_INDI_POS_GAINZ 0.5
59#ifndef GUIDANCE_INDI_LIFTD_ASQ
60#define GUIDANCE_INDI_LIFTD_ASQ 0.20
63#ifndef GUIDANCE_INDI_MAX_PUSHER_INCREMENT
64#define GUIDANCE_INDI_MAX_PUSHER_INCREMENT MAX_PPRZ
71#ifndef GUIDANCE_INDI_LIFTD_P50
72#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12)
73#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2)
76#ifndef GUIDANCE_INDI_MAX_AIRSPEED
77#error "You must have an airspeed sensor to use this guidance"
80#ifndef GUIDANCE_INDI_MIN_AIRSPEED
81#define GUIDANCE_INDI_MIN_AIRSPEED -10.f
87#ifndef GUIDANCE_INDI_FWD_CLIMB_SPEED
88#define GUIDANCE_INDI_FWD_CLIMB_SPEED 4.0
94#ifndef GUIDANCE_INDI_FWD_DESCEND_SPEED
95#define GUIDANCE_INDI_FWD_DESCEND_SPEED -4.0
101#ifndef GUIDANCE_INDI_QUAD_CLIMB_SPEED
102#define GUIDANCE_INDI_QUAD_CLIMB_SPEED 2.0
108#ifndef GUIDANCE_INDI_QUAD_DESCEND_SPEED
109#define GUIDANCE_INDI_QUAD_DESCEND_SPEED -2.0
119 .heading_bank_gain = GUIDANCE_INDI_HEADING_BANK_GAIN,
124 .max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED,
125 .stall_protect_gain = 1.5,
137#if GUIDANCE_INDI_HYBRID_USE_WLS
138#if GUIDANCE_INDI_HYBRID_U > WLS_N_U_MAX
139#error Matrix-WLS_N_U_MAX too small: increase WLS_N_U_MAX in airframe file
142#if GUIDANCE_INDI_HYBRID_V > WLS_N_V_MAX
143#error Matrix-WLS_N_V_MAX too small: increase WLS_N_V_MAX in airframe file
148#ifndef GUIDANCE_INDI_WLS_PRIORITIES
149#define GUIDANCE_INDI_WLS_PRIORITIES { 100.f, 100.f, 1.f }
153#ifndef GUIDANCE_INDI_WLS_WU
154#define GUIDANCE_INDI_WLS_WU {[0 ... GUIDANCE_INDI_HYBRID_U - 1] = 1.0}
159#ifndef GUIDANCE_INDI_ZERO_AIRSPEED
160#define GUIDANCE_INDI_ZERO_AIRSPEED FALSE
164#ifndef TURN_AIRSPEED_TH
165#define TURN_AIRSPEED_TH 13.0
178#ifndef GUIDANCE_INDI_FILTER_CUTOFF
179#ifdef STABILIZATION_INDI_FILT_CUTOFF
180#define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
182#define GUIDANCE_INDI_FILTER_CUTOFF 3.0
186#ifndef GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF
187#define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF 0.5
190#ifndef GUIDANCE_INDI_MAX_LAT_ACCEL
191#define GUIDANCE_INDI_MAX_LAT_ACCEL 9.81
194#ifndef GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED
195#define GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED 10.0
198#ifndef GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED
199#define GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED 30.0
202#ifndef GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN
203#define GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN 0.0
212#if defined(ROTWING_STATE_FW_MAX_AIRSPEED) && defined(ROTWING_STATE_QUAD_MAX_AIRSPEED)
230#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
235static void guidance_indi_filter_thrust(
void);
237#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
238#ifndef STABILIZATION_INDI_ACT_FREQ_P
239#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
241#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
250float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
255#if GUIDANCE_INDI_HYBRID_USE_WLS
257float *Bwls_gih[GUIDANCE_INDI_HYBRID_V];
258struct WLS_t wls_guid_p = {
259 .nu = GUIDANCE_INDI_HYBRID_U,
260 .nv = GUIDANCE_INDI_HYBRID_V,
261 .gamma_sq = 100000.0,
287#ifndef GUIDANCE_INDI_VEL_SP_ID
288#define GUIDANCE_INDI_VEL_SP_ID ABI_BROADCAST
298#if PERIODIC_TELEMETRY
302 pprz_msg_send_EFF_MAT_GUID(trans,
dev, AC_ID,
303 GUIDANCE_INDI_HYBRID_U,
Ga[0],
304 GUIDANCE_INDI_HYBRID_U,
Ga[1],
305 GUIDANCE_INDI_HYBRID_U,
Ga[2]);
310 pprz_msg_send_GUIDANCE_INDI_HYBRID(trans,
dev, AC_ID,
325#if GUIDANCE_INDI_HYBRID_USE_WLS
326static void send_wls_v_guid(
struct transport_tx *trans,
struct link_device *
dev)
330static void send_wls_u_guid(
struct transport_tx *trans,
struct link_device *
dev)
346#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
347 thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
353#if GUIDANCE_INDI_HYBRID_USE_WLS
354 for (
int8_t i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) {
359#if PERIODIC_TELEMETRY
362#if GUIDANCE_INDI_HYBRID_USE_WLS
393 gih_params.climb_vspeed_quad = max_climb_speed_quad;
394 gih_params.climb_vspeed_fwd = max_climb_speed_fwd;
398 gih_params.descend_vspeed_quad = max_descend_speed_quad;
399 gih_params.descend_vspeed_fwd = max_descend_speed_fwd;
431 Bound(a_diff.
x, -6.0, 6.0);
432 Bound(a_diff.
y, -6.0, 6.0);
433 Bound(a_diff.
z, -9.0, 9.0);
437#ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
438#ifndef STABILIZATION_ATTITUDE_INDI_FULL
446#if GUIDANCE_INDI_HYBRID_USE_WLS
451 float du_gih[GUIDANCE_INDI_HYBRID_U];
453 for (
int i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) {
454 wls_guid_p.
v[i] =
v_gih[i];
456 wls_alloc(&wls_guid_p, Bwls_gih, 0, 0, 10);
457 for (
int i = 0; i < GUIDANCE_INDI_HYBRID_U; i++) {
475 const float max_phi = RadOfDeg(60.0f);
476#if GUIDANCE_INDI_ZERO_AIRSPEED
477 float airspeed_turn = 0.f;
506 if (fabsf(coordinated_turn_roll) <
max_phi) {
507 omega = 9.81f / airspeed_turn * tanf(coordinated_turn_roll);
509 omega = 9.81f / airspeed_turn * 1.72305f * ((coordinated_turn_roll > 0.0f) - (coordinated_turn_roll < 0.0f));
512#ifdef FWD_SIDESLIP_GAIN
521 ff_rates.
p = -sinf(euler_zyx->
theta) * omega;
522 ff_rates.
q = cosf(euler_zyx->
theta) * sinf(euler_zyx->
phi) * omega;
523 ff_rates.
r = cosf(euler_zyx->
theta) * cosf(euler_zyx->
phi) * omega;
536#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
537 float delta_limit = STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
541 if (delta_psi > delta_limit) {
543 }
else if (delta_psi < -delta_limit) {
553#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
554 guidance_indi_filter_thrust();
563#if GUIDANCE_INDI_HYBRID_U > 3
588 struct FloatVect3 accel_sp = { 0.f, 0.f, 0.f };
594 float cpsi = cosf(psi);
595 float spsi = sinf(psi);
600#if GUIDANCE_INDI_ZERO_AIRSPEED
601 float airspeed = 0.f;
604 Bound(airspeed, 0.0f, 100.0f);
610 struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed };
612 VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
624 float gi_airspeed_sp = norm_des_as;
631 float groundspeed_factor = 0.0f;
637 float cv = windspeed.
x * windspeed.
x + windspeed.
y * windspeed.
y -
gih_params.max_airspeed *
gih_params.max_airspeed;
639 float dv = bv * bv - 4.0f * av * cv;
645 float d_sqrt = sqrtf(dv);
647 groundspeed_factor = (-bv + d_sqrt) / (2.0f * av);
670 sp_accel_b.
x = (gi_airspeed_sp - airspeed) *
gih_params.speed_gain;
672 accel_sp.
x = cpsi * sp_accel_b.
x - spsi * sp_accel_b.
y;
673 accel_sp.
y = spsi * sp_accel_b.
x + cpsi * sp_accel_b.
y;
678 if (airspeed > 10.f) {
681 float speed_increment = speed_sp_b_x - groundspeed_x;
684 if ((speed_increment + airspeed) >
gih_params.max_airspeed) {
685 speed_sp_b_x =
gih_params.max_airspeed + groundspeed_x - airspeed;
689 float speed_sp_x = cpsi * speed_sp_b_x - spsi * speed_sp_b_y;
690 float speed_sp_y = spsi * speed_sp_b_x + cpsi * speed_sp_b_y;
698 float accelbound = 3.0f + airspeed /
gih_params.max_airspeed * 5.0f;
700 BoundAbs(accel_sp.
z, 3.0);
702#ifdef ROTWING_FW_MIN_AIRSPEED
704 accel_sp.
z =
gih_params.stall_protect_gain * (gi_airspeed_sp - airspeed);
705 BoundAbs(accel_sp.
z, 5.0);
810 const float sample_time = 1.0f / PERIODIC_FREQUENCY;
812 const float tau = 1.0f / omega_c;
813 const struct FloatVect3 omegas = { omega_c, omega_c, omega_c };
823#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
846 Bound(airspeed, 0.0f, 100.0f);
850#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
854void guidance_indi_filter_thrust(
void)
874 if (airspeed < 12.f) {
879 float pitch_interp = DegOfRad(theta);
880 const float min_pitch = -80.0f;
881 const float middle_pitch = -50.0f;
882 const float max_pitch = -20.0f;
884 Bound(pitch_interp, min_pitch, max_pitch);
885 if (pitch_interp > middle_pitch) {
886 float ratio = (pitch_interp - max_pitch)/(middle_pitch - max_pitch);
889 float ratio = (pitch_interp - middle_pitch)/(min_pitch - middle_pitch);
893 liftd = -
gih_params.liftd_asq*airspeed*airspeed;
912#if GUIDANCE_INDI_HYBRID_USE_AS_DEFAULT
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Core autopilot interface common to all firmwares.
static struct uart_periph * dev
static void float_quat_normalize(struct FloatQuat *q)
#define FLOAT_ANGLE_NORMALIZE(_a)
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
void float_mat3_mult(struct FloatVect3 *vect_out, const float mat[3][3], const struct FloatVect3 vect_in)
Multiply 3D matrix with vector.
bool float_mat_inv_3d(float inv_out[3][3], const float mat_in[3][3])
3x3 matrix inverse
void float_quat_of_eulers_zxy(struct FloatQuat *q, const struct FloatEulers *e)
quat from euler rotation 'ZXY' This rotation order is useful if you need 90 deg pitch
void float_vect3_bound_in_2d(struct FloatVect3 *vect3, const float bound)
#define FLOAT_VECT2_NORM(_v)
#define VECT2_DIFF(_c, _a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define POS_FLOAT_OF_BFP(_ai)
#define SPEED_FLOAT_OF_BFP(_ai)
#define ACCEL_FLOAT_OF_BFP(_ai)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (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).
QuatButterworthLowPass quat_filt
struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
float guidance_indi_max_bank
struct FloatVect3 sp_accel
struct FloatEulers guidance_euler_cmd
struct FloatVect3 speed_sp
struct ThrustSetpoint thrust_sp
#define GUIDANCE_INDI_FILTER_CUTOFF
struct FloatVect3 WEAK guidance_indi_controller(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode)
Butterworth2LowPass filt_accel_ned[3]
struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
Butterworth2LowPass thrust_filt
float guidance_indi_specific_force_gain
struct FloatVect3 indi_vel_sp
#define GUIDANCE_INDI_WLS_WU
bool force_forward
forward flight for hybrid nav
#define GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN
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.
#define GUIDANCE_INDI_QUAD_DESCEND_SPEED
Descend speed when navigation is doing direct lines.
#define GUIDANCE_INDI_SPEED_GAINZ
#define GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED
struct guidance_indi_hybrid_params gih_params
#define GUIDANCE_INDI_MAX_LAT_ACCEL
struct FloatVect3 gi_speed_sp
float guidance_indi_min_pitch
#define GUIDANCE_INDI_MIN_AIRSPEED
float gih_coordinated_turn_min_airspeed
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)
void guidance_set_max_bank_angle(float max_bank)
static void guidance_indi_init_filters(struct FloatEulers *eulers)
Initialized low pass filters.
#define GUIDANCE_INDI_FWD_DESCEND_SPEED
Descend speed when navigation is making turns instead of direct lines.
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd)
float gih_coordinated_turn_max_airspeed
float guidance_indi_pitch_pref_deg
#define GUIDANCE_INDI_LIFTD_ASQ
#define GUIDANCE_INDI_QUAD_CLIMB_SPEED
Climb speed when navigation is doing direct lines.
float guidance_indi_airspeed_filt_cutoff
void guidance_indi_enter(void)
Call upon entering indi guidance.
Butterworth2LowPass guidance_indi_airspeed_filt
#define GUIDANCE_INDI_POS_GAIN
#define GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED
#define GUIDANCE_INDI_FWD_CLIMB_SPEED
Climb speed when navigation is making turns instead of direct lines.
bool take_heading_control
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed)
struct FloatVect3 WEAK guidance_indi_controller(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode)
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)
#define GUIDANCE_INDI_WLS_PRIORITIES
void guidance_indi_init(void)
Init function.
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd)
#define GUIDANCE_INDI_MAX_PUSHER_INCREMENT
struct FloatVect3 euler_cmd
static float bound_vz_sp(float vz_sp)
struct FloatEulers eulers_zxy
state eulers in zxy order
float gi_unbounded_airspeed_sp
#define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF
#define GUIDANCE_INDI_POS_GAINZ
static void send_eff_mat_guid_indi_hybrid(struct transport_tx *trans, struct link_device *dev)
#define GUIDANCE_INDI_VEL_SP_ID
struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
bool coordinated_turn_use_accel
static void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
bool guidance_indi_airspeed_filtering
#define GUIDANCE_INDI_LIFTD_P80
static struct FloatVect3 compute_accel_from_speed_sp(struct FloatVect3 *speed_sp)
#define GUIDANCE_INDI_SPEED_GAIN
float du_gih[GUIDANCE_INDI_HYBRID_U]
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
struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
Definitions and inline functions for 1st order low-pass filter vector types.
static struct FloatVect3 get_butterworth_2_low_pass_vect3(const struct Butterworth2LowPassVect3 *filter)
Retrieve the filtered output from 3D vector Butterworth filters.
static void init_butterworth_2_low_pass_vect3(struct Butterworth2LowPassVect3 *filter, const struct FloatVect3 *omega, const float dt)
Initialize a set of Butterworth low-pass filters to zero for 3D vector data.
static struct FloatVect3 update_butterworth_2_low_pass_vect3(struct Butterworth2LowPassVect3 *filter, const struct FloatVect3 *input)
Update 3D vector Butterworth filters with new input data.
vector in North East Down coordinates Units: meters
Quaternion second order filter.
static struct FloatQuat update_quat_butterworth_low_pass(QuatButterworthLowPass *filter, const struct FloatQuat quat)
Update second order quaternion Butterworth low pass filter state with a new value.
static void init_quat_butterworth_low_pass(QuatButterworthLowPass *filter, const float omega, const float sample_time, const struct FloatQuat q0)
Init a second order quaternion Butterworth filter.
struct QuatSecondOrderLowPass QuatButterworthLowPass
Second order Butterworth low pass filter.
Generic interface for radio control modules.
Some helper functions to check RC sticks.
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.
bool rotwing_state_pusher_motor_running(void)
bool rotwing_state_hover_motors_running(void)
struct Stabilization stabilization
struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3])
struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
float stabilization_attitude_get_heading_f(void)
Get attitude heading as float (avoiding jumps).
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]
API to get/set the generic vehicle states.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation
void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)