MAVLab Delft University of Technology This control algorithm is Incremental Nonlinear Dynamic Inversion (INDI) More...

#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
#include "state.h"
#include "generated/airframe.h"
#include "paparazzi.h"

Include dependency graph for stabilization_attitude_quat_indi.c:

Go to the source code of this file.

Macros
#define	INDI_EST_SCALE 0.001

#define	STABILIZATION_INDI_FILT_OMEGA 50.0

#define	STABILIZATION_INDI_FILT_ZETA 0.55

#define	STABILIZATION_INDI_FILT_OMEGA2 (STABILIZATION_INDI_FILT_OMEGA*STABILIZATION_INDI_FILT_OMEGA)

#define	STABILIZATION_INDI_FILT_OMEGA_R STABILIZATION_INDI_FILT_OMEGA

#define	STABILIZATION_INDI_FILT_ZETA_R STABILIZATION_INDI_FILT_ZETA

#define	STABILIZATION_INDI_FILT_OMEGA2_R (STABILIZATION_INDI_FILT_OMEGA_R*STABILIZATION_INDI_FILT_OMEGA_R)

Functions
void	stabilization_attitude_init (void)

void	stabilization_attitude_enter (void)

void	stabilization_attitude_set_failsafe_setpoint (void)

void	stabilization_attitude_set_rpy_setpoint_i (struct Int32Eulers *rpy)

void	stabilization_attitude_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)

static void	attitude_run_indi (int32_t indi_commands[], struct Int32Quat *att_err, bool_t in_flight)

void	stabilization_attitude_run (bool_t enable_integrator)

void	stabilization_attitude_read_rc (bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)

void	stabilization_indi_second_order_filter (struct FloatRates input, struct FloatRates filter_ddx, struct FloatRates filter_dx, struct FloatRates filter_x, float omega, float zeta, float omega_r)

void	lms_estimation (void)

Variables
int32_t	stabilization_att_indi_cmd [COMMANDS_NB]

struct FloatRates	g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R}

float	g2 = STABILIZATION_INDI_G2_R

struct ReferenceSystem	reference_acceleration

struct IndiVariables	indi

struct Int32Eulers	stab_att_sp_euler
	with INT32_ANGLE_FRAC More...

struct Int32Quat	stab_att_sp_quat
	with INT32_QUAT_FRAC More...

struct FloatRates	filtered_rate_estimation = {0., 0., 0.}

struct FloatRates	filtered_rate_deriv_estimation = {0., 0., 0.}

struct FloatRates	filtered_rate_2deriv_estimation = {0., 0., 0.}

struct FloatRates	indi_u_estimation = {0., 0., 0.}

struct FloatRates	udot_estimation = {0., 0., 0.}

struct FloatRates	udotdot_estimation = {0., 0., 0.}

struct FloatRates	g_est = {STABILIZATION_INDI_G1_P / INDI_EST_SCALE, STABILIZATION_INDI_G1_Q / INDI_EST_SCALE, STABILIZATION_INDI_G1_R / INDI_EST_SCALE}

float	g2_est = STABILIZATION_INDI_G2_R / INDI_EST_SCALE

float	mu = STABILIZATION_INDI_ADAPTIVE_MU

bool_t	use_adaptive_indi = FALSE

Detailed Description

MAVLab Delft University of Technology This control algorithm is Incremental Nonlinear Dynamic Inversion (INDI)

This is a simplified implementation of the (soon to be) publication in the journal of Control Guidance and Dynamics: Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Aerial Vehicles

Definition in file stabilization_attitude_quat_indi.c.

Macro Definition Documentation

#define INDI_EST_SCALE 0.001

Definition at line 79 of file stabilization_attitude_quat_indi.c.

Referenced by lms_estimation().

#define STABILIZATION_INDI_FILT_OMEGA 50.0

Definition at line 92 of file stabilization_attitude_quat_indi.c.

Referenced by attitude_run_indi(), and stabilization_attitude_run().

#define STABILIZATION_INDI_FILT_OMEGA2 (STABILIZATION_INDI_FILT_OMEGA*STABILIZATION_INDI_FILT_OMEGA)

Definition at line 99 of file stabilization_attitude_quat_indi.c.

#define STABILIZATION_INDI_FILT_OMEGA2_R (STABILIZATION_INDI_FILT_OMEGA_R*STABILIZATION_INDI_FILT_OMEGA_R)

Definition at line 106 of file stabilization_attitude_quat_indi.c.

#define STABILIZATION_INDI_FILT_OMEGA_R STABILIZATION_INDI_FILT_OMEGA

Definition at line 102 of file stabilization_attitude_quat_indi.c.

Referenced by attitude_run_indi(), and stabilization_attitude_run().

#define STABILIZATION_INDI_FILT_ZETA 0.55

Definition at line 96 of file stabilization_attitude_quat_indi.c.

Referenced by attitude_run_indi(), and stabilization_attitude_run().

#define STABILIZATION_INDI_FILT_ZETA_R STABILIZATION_INDI_FILT_ZETA

Definition at line 103 of file stabilization_attitude_quat_indi.c.

Function Documentation

static void attitude_run_indi	(	int32_t	indi_commands[],
		struct Int32Quat *	att_err,
		bool_t	in_flight
	)

static

Definition at line 194 of file stabilization_attitude_quat_indi.c.

References IndiVariables::angular_accel_ref, IndiVariables::du, ReferenceSystem::err_p, ReferenceSystem::err_q, ReferenceSystem::err_r, IndiVariables::filtered_rate, IndiVariables::filtered_rate_deriv, FLOAT_RATES_ZERO, g2, lms_estimation(), FloatRates::p, FloatRates::q, QUAT1_FLOAT_OF_BFP, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, FloatRates::r, ReferenceSystem::rate_p, ReferenceSystem::rate_q, ReferenceSystem::rate_r, stabilization_cmd, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_OMEGA_R, STABILIZATION_INDI_FILT_ZETA, stabilization_indi_second_order_filter(), stateGetBodyRates_f(), IndiVariables::u, IndiVariables::u_act_dyn, IndiVariables::u_in, IndiVariables::udot, and IndiVariables::udotdot.

Referenced by stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void lms_estimation ( void )

Definition at line 324 of file stabilization_attitude_quat_indi.c.

References g2, g2_est, INDI_EST_SCALE, mu, FloatRates::p, PERIODIC_FREQUENCY, FloatRates::q, FloatRates::r, stabilization_indi_second_order_filter(), stateGetBodyRates_f(), IndiVariables::u_act_dyn, and use_adaptive_indi.

Referenced by attitude_run_indi().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_attitude_enter ( void )

Definition at line 140 of file stabilization_attitude_quat_indi.c.

References IndiVariables::angular_accel_ref, IndiVariables::du, IndiVariables::filtered_rate, IndiVariables::filtered_rate_2deriv, IndiVariables::filtered_rate_deriv, FLOAT_RATES_ZERO, Int32Eulers::psi, stab_att_sp_euler, stabilization_attitude_get_heading_i(), IndiVariables::u, IndiVariables::u_act_dyn, IndiVariables::u_in, IndiVariables::udot, and IndiVariables::udotdot.

Here is the call graph for this function:

void stabilization_attitude_init ( void )

Definition at line 132 of file stabilization_attitude_quat_indi.c.

References DefaultPeriodic, and register_periodic_telemetry().

Here is the call graph for this function:

void stabilization_attitude_read_rc	(	bool_t	in_flight,
		bool_t	in_carefree,
		bool_t	coordinated_turn
	)

Definition at line 297 of file stabilization_attitude_quat_indi.c.

References QUAT_BFP_OF_REAL, stab_att_sp_quat, stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().

Here is the call graph for this function:

void stabilization_attitude_run ( bool_t enable_integrator )

Definition at line 266 of file stabilization_attitude_quat_indi.c.

References attitude_run_indi(), IndiVariables::filtered_rate, IndiVariables::filtered_rate_2deriv, IndiVariables::filtered_rate_deriv, int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), MAX_PPRZ, stab_att_sp_quat, stabilization_att_indi_cmd, stabilization_cmd, STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_OMEGA_R, STABILIZATION_INDI_FILT_ZETA, stabilization_indi_second_order_filter(), stateGetBodyRates_f(), and stateGetNedToBodyQuat_i().

Here is the call graph for this function:

void stabilization_attitude_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

Definition at line 177 of file stabilization_attitude_quat_indi.c.

References heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, quat_from_earth_cmd_i(), stab_att_sp_euler, stab_att_sp_quat, stateGetNedToBodyEulers_i(), Int32Eulers::theta, Int32Vect2::x, and Int32Vect2::y.

Here is the call graph for this function:

void stabilization_attitude_set_failsafe_setpoint ( void )

Definition at line 159 of file stabilization_attitude_quat_indi.c.

References PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stabilization_attitude_get_heading_i().

Here is the call graph for this function:

void stabilization_attitude_set_rpy_setpoint_i ( struct Int32Eulers * rpy )

Definition at line 169 of file stabilization_attitude_quat_indi.c.

References int32_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.

Here is the call graph for this function:

void stabilization_indi_second_order_filter	(	struct FloatRates *	input,
		struct FloatRates *	filter_ddx,
		struct FloatRates *	filter_dx,
		struct FloatRates *	filter_x,
		float	omega,
		float	zeta,
		float	omega_r
	)

Definition at line 309 of file stabilization_attitude_quat_indi.c.

References float_rates_integrate_fi(), omega_r, FloatRates::p, PERIODIC_FREQUENCY, FloatRates::q, and FloatRates::r.

Referenced by attitude_run_indi(), lms_estimation(), and stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct FloatRates filtered_rate_2deriv_estimation = {0., 0., 0.}

Definition at line 75 of file stabilization_attitude_quat_indi.c.

struct FloatRates filtered_rate_deriv_estimation = {0., 0., 0.}

Definition at line 74 of file stabilization_attitude_quat_indi.c.

struct FloatRates filtered_rate_estimation = {0., 0., 0.}

Definition at line 73 of file stabilization_attitude_quat_indi.c.

struct FloatRates g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R}

Definition at line 46 of file stabilization_attitude_quat_indi.c.

Referenced by gain_scheduling_periodic().

float g2 = STABILIZATION_INDI_G2_R

Definition at line 47 of file stabilization_attitude_quat_indi.c.

Referenced by attitude_run_indi(), gain_scheduling_periodic(), and lms_estimation().

float g2_est = STABILIZATION_INDI_G2_R / INDI_EST_SCALE

Definition at line 81 of file stabilization_attitude_quat_indi.c.

Referenced by lms_estimation().

struct FloatRates g_est = {STABILIZATION_INDI_G1_P / INDI_EST_SCALE, STABILIZATION_INDI_G1_Q / INDI_EST_SCALE, STABILIZATION_INDI_G1_R / INDI_EST_SCALE}

Definition at line 80 of file stabilization_attitude_quat_indi.c.

struct IndiVariables indi

Initial value:

= {
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.},
  {0., 0., 0.}
}

Definition at line 56 of file stabilization_attitude_quat_indi.c.

struct FloatRates indi_u_estimation = {0., 0., 0.}

Definition at line 76 of file stabilization_attitude_quat_indi.c.

float mu = STABILIZATION_INDI_ADAPTIVE_MU

Definition at line 82 of file stabilization_attitude_quat_indi.c.

Referenced by lms_estimation().

struct ReferenceSystem reference_acceleration

Initial value:

= {STABILIZATION_INDI_REF_ERR_P,
         STABILIZATION_INDI_REF_ERR_Q,
         STABILIZATION_INDI_REF_ERR_R,
         STABILIZATION_INDI_REF_RATE_P,
         STABILIZATION_INDI_REF_RATE_Q,
         STABILIZATION_INDI_REF_RATE_R,
}

Definition at line 48 of file stabilization_attitude_quat_indi.c.

struct Int32Eulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 69 of file stabilization_attitude_quat_indi.c.

Referenced by stabilization_attitude_enter(), stabilization_attitude_set_earth_cmd_i(), and stabilization_attitude_set_rpy_setpoint_i().

struct Int32Quat stab_att_sp_quat

with INT32_QUAT_FRAC

Definition at line 70 of file stabilization_attitude_quat_indi.c.

Referenced by stabilization_attitude_read_rc(), stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_failsafe_setpoint(), and stabilization_attitude_set_rpy_setpoint_i().

int32_t stabilization_att_indi_cmd[COMMANDS_NB]

Definition at line 44 of file stabilization_attitude_quat_indi.c.

Referenced by stabilization_attitude_run().