Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_indi_simple.c File Reference
Include dependency graph for stabilization_indi_simple.c:

Go to the source code of this file.

Macros

#define STABILIZATION_INDI_FILT_CUTOFF   8.0
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT   STABILIZATION_INDI_FILT_CUTOFF
#define STABILIZATION_INDI_MAX_RATE   6.0
#define STABILIZATION_INDI_MAX_R   STABILIZATION_ATTITUDE_SP_MAX_R
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF   4.0
#define STABILIZATION_INDI_FILT_CUTOFF_P   20.0
#define STABILIZATION_INDI_FILT_CUTOFF_Q   20.0
#define STABILIZATION_INDI_FILT_CUTOFF_R   20.0
#define INDI_EST_SCALE   0.001

Functions

static void lms_estimation (void)
 This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online, by comparing the expected angular acceleration based on the inputs with the measured angular acceleration.
static void indi_init_filters (void)
static void send_att_indi (struct transport_tx *trans, struct link_device *dev)
static void send_eff_mat_g_indi_simple (struct transport_tx *trans, struct link_device *dev)
static void send_ahrs_ref_quat (struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_init (void)
 Function that initializes important values upon engaging INDI.
void stabilization_indi_simple_reset_r_filter_cutoff (float new_cutoff)
void stabilization_indi_enter (void)
 Function that resets important values upon engaging INDI.
static void filter_pqr (Butterworth2LowPass *filter, struct FloatRates *new_values)
 Update butterworth filter for p, q and r of a FloatRates struct.
static void finite_difference_from_filter (float *output, Butterworth2LowPass *filter)
 Caclulate finite difference form a filter array The filter already contains the previous values.
static void finite_difference (float output[3], float new[3], float old[3])
 Calculate derivative of an array via finite difference.
void stabilization_indi_rate_run (bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 Does the INDI calculations.
void stabilization_indi_attitude_run (bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 runs stabilization indi

Variables

struct Int32Eulers stab_att_sp_euler
struct Int32Quat stab_att_sp_quat
static struct FirstOrderLowPass rates_filt_fo [3]
struct IndiVariables indi

Macro Definition Documentation

◆ INDI_EST_SCALE

#define INDI_EST_SCALE   0.001

Definition at line 107 of file stabilization_indi_simple.c.

Referenced by lms_estimation(), and send_eff_mat_g_indi_simple().

◆ STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF

#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF   4.0

Definition at line 77 of file stabilization_indi_simple.c.

Referenced by indi_init_filters(), and init_filters().

◆ STABILIZATION_INDI_FILT_CUTOFF

#define STABILIZATION_INDI_FILT_CUTOFF   8.0

Definition at line 56 of file stabilization_indi_simple.c.

Referenced by indi_init_filters(), and init_filters().

◆ STABILIZATION_INDI_FILT_CUTOFF_P

#define STABILIZATION_INDI_FILT_CUTOFF_P   20.0

Definition at line 83 of file stabilization_indi_simple.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_Q

#define STABILIZATION_INDI_FILT_CUTOFF_Q   20.0

Definition at line 89 of file stabilization_indi_simple.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_R

#define STABILIZATION_INDI_FILT_CUTOFF_R   20.0

Definition at line 95 of file stabilization_indi_simple.c.

◆ STABILIZATION_INDI_FILT_CUTOFF_RDOT

#define STABILIZATION_INDI_FILT_CUTOFF_RDOT   STABILIZATION_INDI_FILT_CUTOFF

Definition at line 61 of file stabilization_indi_simple.c.

Referenced by indi_init_filters().

◆ STABILIZATION_INDI_MAX_R

#define STABILIZATION_INDI_MAX_R   STABILIZATION_ATTITUDE_SP_MAX_R

Definition at line 73 of file stabilization_indi_simple.c.

◆ STABILIZATION_INDI_MAX_RATE

#define STABILIZATION_INDI_MAX_RATE   6.0

Definition at line 65 of file stabilization_indi_simple.c.

Referenced by stabilization_rate_read_rc().

Function Documentation

◆ filter_pqr()

void filter_pqr ( Butterworth2LowPass * filter,
struct FloatRates * new_values )
inlinestatic

Update butterworth filter for p, q and r of a FloatRates struct.

Parameters
filterThe filter array to use
new_valuesThe new values

Definition at line 267 of file stabilization_indi_simple.c.

References FloatRates::p, FloatRates::q, FloatRates::r, and update_butterworth_2_low_pass().

Referenced by lms_estimation(), and stabilization_indi_rate_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ finite_difference()

void finite_difference ( float output[3],
float new[3],
float old[3] )
inlinestatic

Calculate derivative of an array via finite difference.

Parameters
output[3]The output array
new[3]The newest values
old[3]The values of the previous timestep

Definition at line 295 of file stabilization_indi_simple.c.

Referenced by lms_estimation().

Here is the caller graph for this function:

◆ finite_difference_from_filter()

void finite_difference_from_filter ( float * output,
Butterworth2LowPass * filter )
inlinestatic

Caclulate finite difference form a filter array The filter already contains the previous values.

Parameters
outputThe output array
filterThe filter array input

Definition at line 281 of file stabilization_indi_simple.c.

References SecondOrderLowPass::o.

Referenced by lms_estimation(), and stabilization_indi_rate_run().

Here is the caller graph for this function:

◆ indi_init_filters()

◆ lms_estimation()

void lms_estimation ( void )
inlinestatic

This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online, by comparing the expected angular acceleration based on the inputs with the measured angular acceleration.

Definition at line 462 of file stabilization_indi_simple.c.

References filter_pqr(), finite_difference(), finite_difference_from_filter(), float_vect_copy(), float_vect_scale(), IndiEstimation::g1, IndiEstimation::g2, indi, INDI_EST_SCALE, IndiEstimation::mu, FloatRates::p, FloatRates::q, FloatRates::r, IndiEstimation::rate, IndiEstimation::rate_d, IndiEstimation::rate_dd, stateGetBodyRates_f(), IndiEstimation::u, IndiEstimation::u_d, and IndiEstimation::u_dd.

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_ahrs_ref_quat()

void send_ahrs_ref_quat ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 179 of file stabilization_indi_simple.c.

References dev, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stateGetNedToBodyQuat_i().

Here is the call graph for this function:

◆ send_att_indi()

void send_att_indi ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 150 of file stabilization_indi_simple.c.

References dev, and indi.

Referenced by stabilization_indi_init().

Here is the caller graph for this function:

◆ send_eff_mat_g_indi_simple()

void send_eff_mat_g_indi_simple ( struct transport_tx * trans,
struct link_device * dev )
static

Definition at line 166 of file stabilization_indi_simple.c.

References dev, indi, INDI_EST_SCALE, FloatRates::p, FloatRates::q, FloatRates::r, and RATES_SMUL.

Referenced by stabilization_indi_init().

Here is the caller graph for this function:

◆ stabilization_indi_attitude_run()

void stabilization_indi_attitude_run ( bool in_flight,
struct StabilizationSetpoint * att_sp,
struct ThrustSetpoint * thrust,
int32_t * cmd )

◆ stabilization_indi_enter()

void stabilization_indi_enter ( void )

Function that resets important values upon engaging INDI.

Don't reset inputs and filters, because it is unlikely to switch stabilization in flight, and there are multiple modes that use (the same) stabilization. Resetting the controller is not so nice when you are flying. FIXME: Ideally we should detect when coming from something that is not INDI

Definition at line 248 of file stabilization_indi_simple.c.

References FLOAT_RATES_ZERO, indi, indi_init_filters(), stab_att_sp_euler, and stabilization_attitude_get_heading_i().

Here is the call graph for this function:

◆ stabilization_indi_init()

void stabilization_indi_init ( void )

Function that initializes important values upon engaging INDI.

Definition at line 194 of file stabilization_indi_simple.c.

References DefaultPeriodic, indi, indi_init_filters(), register_periodic_telemetry(), send_ahrs_ref_quat(), send_att_indi(), and send_eff_mat_g_indi_simple().

Here is the call graph for this function:

◆ stabilization_indi_rate_run()

void stabilization_indi_rate_run ( bool in_flight,
struct StabilizationSetpoint * sp,
struct ThrustSetpoint * thrust,
int32_t * cmd )

Does the INDI calculations.

Parameters
in_flighttrue aircraft is flying
sprate setpoint
thrustthrust setpoint
cmdoutput command array

Definition at line 310 of file stabilization_indi_simple.c.

References COMMAND_PITCH, COMMAND_ROLL, filter_pqr(), finite_difference_from_filter(), FLOAT_RATES_ZERO, indi, lms_estimation(), MAX_PPRZ, FloatRates::p, FloatRates::q, FloatRates::r, rates_filt_fo, stab_sp_to_rates_f(), stateGetBodyRates_f(), th_sp_to_thrust_i(), THRUST_AXIS_Z, and update_first_order_low_pass().

Referenced by stabilization_indi_attitude_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ stabilization_indi_simple_reset_r_filter_cutoff()

void stabilization_indi_simple_reset_r_filter_cutoff ( float new_cutoff)

Definition at line 241 of file stabilization_indi_simple.c.

References indi, init_first_order_low_pass(), rates_filt_fo, and stateGetBodyRates_f().

Here is the call graph for this function:

Variable Documentation

◆ indi

◆ rates_filt_fo

struct FirstOrderLowPass rates_filt_fo[3]
static

Definition at line 101 of file stabilization_indi_simple.c.

◆ stab_att_sp_euler

struct Int32Eulers stab_att_sp_euler

Definition at line 98 of file stabilization_indi_simple.c.

◆ stab_att_sp_quat

struct Int32Quat stab_att_sp_quat

Definition at line 99 of file stabilization_indi_simple.c.