33#include "generated/airframe.h"
51#ifdef GUIDANCE_INDI_POS_GAIN
57#ifdef GUIDANCE_INDI_SPEED_GAIN
63#ifndef GUIDANCE_INDI_ACCEL_SP_ID
64#define GUIDANCE_INDI_ACCEL_SP_ID ABI_BROADCAST
75#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
77static void guidance_indi_filter_thrust(
void);
79#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
80#ifndef STABILIZATION_INDI_ACT_FREQ_P
81#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
83#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
90#ifndef GUIDANCE_INDI_FILTER_CUTOFF
91#ifdef STABILIZATION_INDI_FILT_CUTOFF
92#define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
94#define GUIDANCE_INDI_FILTER_CUTOFF 3.0
106#if GUIDANCE_INDI_USE_WLS
110#ifndef GUIDANCE_INDI_WLS_PRIORITIES
111#define GUIDANCE_INDI_WLS_PRIORITIES {100.f, 100.f, 100.f}
115#ifndef GUIDANCE_INDI_WLS_WU
116#define GUIDANCE_INDI_WLS_WU {1000.f,1000.f,10.f,1.f,1.f,1.f}
124static struct WLS_t wls_guid_p = {
163#if PERIODIC_TELEMETRY
167 pprz_msg_send_GUIDANCE_INDI_HYBRID(trans,
dev, AC_ID,
171#
if GUIDANCE_INDI_USE_WLS
198#if PERIODIC_TELEMETRY
215#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
216 thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
220 float sample_time = 1.0 / PERIODIC_FREQUENCY;
221 for (
int8_t i = 0; i < 3; i++) {
251 Bound(a_diff.
x, -6.0, 6.0);
252 Bound(a_diff.
y, -6.0, 6.0);
253 Bound(a_diff.
z, -9.0, 9.0);
257#ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
258#ifndef STABILIZATION_ATTITUDE_INDI_FULL
263#if GUIDANCE_INDI_USE_WLS
264 const float m = GUIDANCE_INDI_MASS;
265 wls_guid_p.
v[0] = m*a_diff.
x;
266 wls_guid_p.
v[1] = m*a_diff.
y;
267 wls_guid_p.
v[2] = m*a_diff.
z;
272 Bwls_gi[i] =
Gmat[i];
274 wls_alloc(&wls_guid_p,Bwls_gi, 0, 0, 10);
276 du_guidance [i] = wls_guid_p.
u[i];
311#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
312 guidance_indi_filter_thrust();
426#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
430void guidance_indi_filter_thrust(
void)
465 }
else if (flag == 1) {
474#if GUIDANCE_INDI_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
#define FLOAT_EULERS_ZERO(_e)
void float_eulers_of_quat_yxz(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ,...
void float_quat_of_eulers_yxz(struct FloatQuat *q, const struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
#define MAT33_INV(_minv, _m)
#define RMAT_ELMT(_rm, _row, _col)
#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 * 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).
#define GUIDANCE_INDI_ACCEL_SP_ID
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
bool indi_accel_sp_set_3d
float guidance_indi_pos_gain
struct FloatVect3 sp_accel
struct FloatEulers guidance_euler_cmd
struct FloatVect3 speed_sp
void guidance_indi_enter(void)
Call upon entering indi guidance.
static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp)
ABI callback that obtains the acceleration setpoint from telemetry flag: 0 -> 2D, 1 -> 3D.
struct ThrustSetpoint thrust_sp
void guidance_indi_init(void)
Init function.
float time_of_accel_sp_3d
#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)
bool indi_accel_sp_set_2d
struct FloatVect3 indi_accel_sp
struct FloatVect3 control_increment
Butterworth2LowPass filt_accel_ned[3]
float guidance_indi_speed_gain
float time_of_accel_sp_2d
static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU]
struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
static void guidance_indi_propagate_filters(void)
Low pass the accelerometer measurements to remove noise from vibrations.
Butterworth2LowPass thrust_filt
static void send_indi_guidance(struct transport_tx *trans, struct link_device *dev)
A guidance mode based on Incremental Nonlinear Dynamic Inversion.
float guidance_indi_specific_force_gain
void guidance_indi_calcG(float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU], const struct FloatQuat *att)
void guidance_indi_set_wls_settings(struct WLS_t *wls, const struct FloatQuat *att, const float heading_sp)
#define GUIDANCE_INDI_WLS_WU
#define GUIDANCE_INDI_POS_GAIN
#define GUIDANCE_INDI_WLS_PRIORITIES
#define GUIDANCE_INDI_SPEED_GAIN
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.
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.
Horizontal guidance for rotorcrafts.
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
Vertical guidance for rotorcrafts.
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
struct Stabilization stabilization
struct ThrustSetpoint th_sp_from_incr_vect_f(float th_increment[3])
struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
struct ThrustSetpoint th_sp_from_thrust_i(int32_t thrust, uint8_t axis)
General stabilization interface for rotorcrafts.
#define THRUST_SP_SET_ZERO(_sp)
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 wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation