#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#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 "subsystems/radio_control.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for stabilization_indi.c:

Macros
#define	STABILIZATION_INDI_FILT_OMEGA 50.0

#define	STABILIZATION_INDI_FILT_ZETA 0.55

#define	STABILIZATION_INDI_FILT_OMEGA_R STABILIZATION_INDI_FILT_OMEGA

#define	STABILIZATION_INDI_FILT_ZETA_R STABILIZATION_INDI_FILT_ZETA

#define	STABILIZATION_INDI_MAX_RATE 6.0

#define	STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R

#define	INDI_EST_SCALE 0.001

Functions
static void	stabilization_indi_calc_cmd (int32_t indi_commands[], struct Int32Quat *att_err, bool rate_control)

static void	stabilization_indi_second_order_filter_init (struct IndiFilter *filter, float omega, float zeta, float omega_r)

static void	stabilization_indi_second_order_filter (struct IndiFilter filter, struct FloatRates input)

static void	lms_estimation (void)

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

void	stabilization_indi_init (void)

void	stabilization_indi_enter (void)

void	stabilization_indi_set_failsafe_setpoint (void)

void	stabilization_indi_set_rpy_setpoint_i (struct Int32Eulers *rpy)

void	stabilization_indi_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)

void	stabilization_indi_run (bool enable_integrator, bool rate_control)

void	stabilization_indi_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)

Variables
struct Int32Eulers	stab_att_sp_euler
	with INT32_ANGLE_FRAC More...

struct Int32Quat	stab_att_sp_quat
	with INT32_QUAT_FRAC More...

static int32_t	stabilization_att_indi_cmd [COMMANDS_NB]

struct IndiVariables	indi

Macro Definition Documentation

#define INDI_EST_SCALE 0.001

Definition at line 87 of file stabilization_indi.c.

Referenced by lms_estimation(), and send_att_indi().

#define STABILIZATION_INDI_FILT_OMEGA 50.0

Definition at line 50 of file stabilization_indi.c.

Referenced by stabilization_indi_init().

#define STABILIZATION_INDI_FILT_OMEGA_R STABILIZATION_INDI_FILT_OMEGA

Definition at line 59 of file stabilization_indi.c.

Referenced by stabilization_indi_init().

#define STABILIZATION_INDI_FILT_ZETA 0.55

Definition at line 54 of file stabilization_indi.c.

Referenced by stabilization_indi_init().

#define STABILIZATION_INDI_FILT_ZETA_R STABILIZATION_INDI_FILT_ZETA

Definition at line 63 of file stabilization_indi.c.

#define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R

Definition at line 75 of file stabilization_indi.c.

#define STABILIZATION_INDI_MAX_RATE 6.0

Definition at line 67 of file stabilization_indi.c.

Function Documentation

static void lms_estimation ( void )

inlinestatic

Definition at line 355 of file stabilization_indi.c.

References IndiVariables::adaptive, IndiFilter::ddx, IndiFilter::dx, IndiVariables::est, IndiEstimation::g1, IndiVariables::g1, IndiEstimation::g2, IndiVariables::g2, INDI_EST_SCALE, IndiEstimation::mu, FloatRates::p, FloatRates::q, FloatRates::r, IndiEstimation::rate, stabilization_indi_second_order_filter(), stateGetBodyRates_f(), IndiEstimation::u, and IndiVariables::u_act_dyn.

Referenced by stabilization_indi_calc_cmd().

Here is the call graph for this function:

Here is the caller graph for this function:

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

static

Definition at line 122 of file stabilization_indi.c.

References IndiVariables::angular_accel_ref, IndiFilter::dx, IndiVariables::est, IndiEstimation::g1, IndiEstimation::g2, INDI_EST_SCALE, FloatRates::p, FloatRates::q, FloatRates::r, IndiVariables::rate, and RATES_SMUL.

Referenced by stabilization_indi_init().

Here is the caller graph for this function:

static void stabilization_indi_calc_cmd	(	int32_t	indi_commands[],
		struct Int32Quat *	att_err,
		bool	rate_control
	)

inlinestatic

Definition at line 208 of file stabilization_indi.c.

References IndiVariables::angular_accel_ref, IndiVariables::attitude_max_yaw_rate, IndiFilter::ddx, IndiVariables::du, IndiFilter::dx, ReferenceSystem::err_p, ReferenceSystem::err_q, ReferenceSystem::err_r, FLOAT_RATES_ZERO, IndiVariables::g1, IndiVariables::g2, lms_estimation(), MAX_PPRZ, IndiVariables::max_rate, FloatRates::p, FloatRates::q, QUAT1_FLOAT_OF_BFP, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, FloatRates::r, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, IndiVariables::rate, ReferenceSystem::rate_p, ReferenceSystem::rate_q, ReferenceSystem::rate_r, RATES_COPY, IndiVariables::reference_acceleration, stabilization_cmd, stabilization_indi_second_order_filter(), stateGetBodyRates_f(), IndiVariables::u, IndiVariables::u_act_dyn, IndiVariables::u_in, RadioControl::values, and IndiFilter::x.

Referenced by stabilization_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_enter ( void )

Definition at line 156 of file stabilization_indi.c.

References IndiVariables::angular_accel_ref, IndiFilter::ddx, IndiVariables::du, IndiFilter::dx, FLOAT_RATES_ZERO, Int32Eulers::psi, IndiVariables::rate, stab_att_sp_euler, stabilization_attitude_get_heading_i(), IndiVariables::u, IndiVariables::u_act_dyn, IndiVariables::u_in, and IndiFilter::x.

Referenced by stabilization_attitude_enter(), and stabilization_rate_enter().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_init ( void )

Definition at line 143 of file stabilization_indi.c.

References DefaultPeriodic, IndiVariables::est, IndiEstimation::rate, IndiVariables::rate, register_periodic_telemetry(), send_att_indi(), STABILIZATION_INDI_FILT_OMEGA, STABILIZATION_INDI_FILT_OMEGA_R, STABILIZATION_INDI_FILT_ZETA, stabilization_indi_second_order_filter_init(), IndiEstimation::u, and IndiVariables::u.

Referenced by stabilization_attitude_init(), and stabilization_rate_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_read_rc	(	bool	in_flight,
		bool	in_carefree,
		bool	coordinated_turn
	)

Definition at line 321 of file stabilization_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().

Referenced by stabilization_attitude_read_rc().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_run	(	bool	enable_integrator,
		bool	rate_control
	)

Definition at line 296 of file stabilization_indi.c.

References 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_calc_cmd(), and stateGetNedToBodyQuat_i().

Referenced by stabilization_attitude_run(), and stabilization_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

static void stabilization_indi_second_order_filter	(	struct IndiFilter *	filter,
		struct FloatRates *	input
	)

static

Definition at line 343 of file stabilization_indi.c.

References IndiFilter::ddx, IndiFilter::dx, float_rates_integrate_fi(), IndiFilter::omega, IndiFilter::omega2, IndiFilter::omega2_r, IndiFilter::omega_r, FloatRates::p, FloatRates::q, FloatRates::r, IndiFilter::x, and IndiFilter::zeta.

Referenced by lms_estimation(), and stabilization_indi_calc_cmd().

Here is the call graph for this function:

Here is the caller graph for this function:

static void stabilization_indi_second_order_filter_init	(	struct IndiFilter *	filter,
		float	omega,
		float	zeta,
		float	omega_r
	)

static

Definition at line 333 of file stabilization_indi.c.

References IndiFilter::omega, IndiFilter::omega2, IndiFilter::omega2_r, omega_r, IndiFilter::omega_r, and IndiFilter::zeta.

Referenced by stabilization_indi_init().

Here is the caller graph for this function:

void stabilization_indi_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

Definition at line 191 of file stabilization_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.

Referenced by stabilization_attitude_set_earth_cmd_i().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_set_failsafe_setpoint ( void )

Definition at line 173 of file stabilization_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().

Referenced by stabilization_attitude_set_failsafe_setpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_indi_set_rpy_setpoint_i ( struct Int32Eulers * rpy )

Definition at line 183 of file stabilization_indi.c.

References quat_from_rpy_cmd_i(), stab_att_sp_euler, and stab_att_sp_quat.

Referenced by stabilization_attitude_set_rpy_setpoint_i().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct IndiVariables indi

Initial value:

= {
  .max_rate = STABILIZATION_INDI_MAX_RATE,
  .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
  .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
  .g2 = STABILIZATION_INDI_G2_R,
  .reference_acceleration = {
    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},
  
  .est = {
    .g1 = {
      STABILIZATION_INDI_G1_P / INDI_EST_SCALE,
      STABILIZATION_INDI_G1_Q / INDI_EST_SCALE,
      STABILIZATION_INDI_G1_R / INDI_EST_SCALE},
    .g2 = STABILIZATION_INDI_G2_R / INDI_EST_SCALE,
    .mu = STABILIZATION_INDI_ADAPTIVE_MU,
  },
  .adaptive = FALSE,
}

Definition at line 88 of file stabilization_indi.c.

struct Int32Eulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 78 of file stabilization_indi.c.

Referenced by stabilization_indi_enter(), stabilization_indi_set_earth_cmd_i(), and stabilization_indi_set_rpy_setpoint_i().

struct Int32Quat stab_att_sp_quat

with INT32_QUAT_FRAC

Definition at line 79 of file stabilization_indi.c.

Referenced by stabilization_indi_read_rc(), stabilization_indi_run(), stabilization_indi_set_earth_cmd_i(), stabilization_indi_set_failsafe_setpoint(), and stabilization_indi_set_rpy_setpoint_i().

int32_t stabilization_att_indi_cmd[COMMANDS_NB]

static

Definition at line 81 of file stabilization_indi.c.

Referenced by stabilization_indi_run().

Macros

Functions

Variables

Macro Definition Documentation

Function Documentation

Variable Documentation