General stabilization interface for rotorcrafts. More...

#include "std.h"
#include "generated/airframe.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"

Include dependency graph for stabilization.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	StabilizationSetpoint
	Stabilization setpoint. More...

struct	ThrustSetpoint
	Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with different format. More...

struct	Stabilization
	Stabilization structure. More...

union	StabilizationSetpoint.sp

struct	StabilizationSetpoint.sp.ltp_i

struct	StabilizationSetpoint.sp.ltp_f

union	StabilizationSetpoint.r_sp

union	ThrustSetpoint.sp

Macros
#define	STABILIZATION_MODE_NONE 0
	Stabilization modes. More...

#define	STABILIZATION_MODE_DIRECT 1

#define	STABILIZATION_MODE_RATE 2

#define	STABILIZATION_MODE_ATTITUDE 3

#define	STABILIZATION_ATT_SUBMODE_HEADING 0
	Stabilization sub-modes for attitude. More...

#define	STABILIZATION_ATT_SUBMODE_CARE_FREE 1

#define	STABILIZATION_ATT_SUBMODE_FORWARD 2

#define	THRUST_AXIS_X 0

#define	THRUST_AXIS_Y 1

#define	THRUST_AXIS_Z 2

#define	STAB_SP_SET_EULERS_ZERO(_sp)

#define	THRUST_SP_SET_ZERO(_sp)

Functions
void	stabilization_init (void)
	Init function. More...

void	stabilization_mode_changed (uint8_t new_mode, uint8_t submode)
	Check mode change. More...

void	stabilization_run (bool in_flight, struct StabilizationSetpoint sp, struct ThrustSetpoint thrust, int32_t *cmd)
	Call default stabilization control. More...

struct StabilizationSetpoint	stabilization_get_failsafe_sp (void)
	Get stabilization setpoint for failsafe. More...

void	stabilization_filter_commands (void)
	Command filter for vibrating airframes. More...

struct Int32Quat	stab_sp_to_quat_i (struct StabilizationSetpoint *sp)

struct FloatQuat	stab_sp_to_quat_f (struct StabilizationSetpoint *sp)

struct Int32Eulers	stab_sp_to_eulers_i (struct StabilizationSetpoint *sp)

struct FloatEulers	stab_sp_to_eulers_f (struct StabilizationSetpoint *sp)

struct Int32Rates	stab_sp_to_rates_i (struct StabilizationSetpoint *sp)

struct FloatRates	stab_sp_to_rates_f (struct StabilizationSetpoint *sp)

int32_t	th_sp_to_thrust_i (struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)

float	th_sp_to_thrust_f (struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)

int32_t	th_sp_to_incr_i (struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)

float	th_sp_to_incr_f (struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)

struct StabilizationSetpoint	stab_sp_from_quat_i (struct Int32Quat *quat)

struct StabilizationSetpoint	stab_sp_from_quat_f (struct FloatQuat *quat)

struct StabilizationSetpoint	stab_sp_from_quat_ff_rates_f (struct FloatQuat quat, struct FloatRates rates)

struct StabilizationSetpoint	stab_sp_from_eulers_i (struct Int32Eulers *eulers)

struct StabilizationSetpoint	stab_sp_from_eulers_f (struct FloatEulers *eulers)

struct StabilizationSetpoint	stab_sp_from_ltp_i (struct Int32Vect2 *vect, int32_t heading)

struct StabilizationSetpoint	stab_sp_from_ltp_f (struct FloatVect2 *vect, float heading)

struct StabilizationSetpoint	stab_sp_from_rates_i (struct Int32Rates *rates)

struct StabilizationSetpoint	stab_sp_from_rates_f (struct FloatRates *rates)

struct ThrustSetpoint	th_sp_from_thrust_i (int32_t thrust, uint8_t axis)

struct ThrustSetpoint	th_sp_from_thrust_f (float thrust, uint8_t axis)

struct ThrustSetpoint	th_sp_from_incr_i (int32_t th_increment, uint8_t axis)

struct ThrustSetpoint	th_sp_from_incr_f (float th_increment, uint8_t axis)

struct ThrustSetpoint	th_sp_from_thrust_vect_i (int32_t thrust[3])

struct ThrustSetpoint	th_sp_from_thrust_vect_f (float thrust[3])

struct ThrustSetpoint	th_sp_from_incr_vect_i (int32_t th_increment[3])

struct ThrustSetpoint	th_sp_from_incr_vect_f (float th_increment[3])

Variables
struct Stabilization	stabilization

Detailed Description

General stabilization interface for rotorcrafts.

Definition in file stabilization.h.

Data Structure Documentation

◆ Stabilization

struct Stabilization

Stabilization structure.

Definition at line 102 of file stabilization.h.

Collaboration diagram for Stabilization:

Data Fields
uint8_t	att_submode	current attitude sub-mode
int32_t	cmd[COMMANDS_NB]	output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
uint8_t	mode	current mode
struct AttitudeRCInput	rc_in	RC input.
struct StabilizationSetpoint	rc_sp	Keep it ? FIXME.
struct StabilizationSetpoint	sp	current attitude setpoint (store for messages)
float	transition_ratio	transition percentage for hybrids (0.: hover; 1.: forward)

◆ StabilizationSetpoint.sp

union StabilizationSetpoint.sp

Definition at line 65 of file stabilization.h.

Data Fields
struct FloatEulers	eulers_f
struct Int32Eulers	eulers_i
sp	ltp_f
sp	ltp_i
struct FloatQuat	quat_f
struct Int32Quat	quat_i

◆ StabilizationSetpoint.sp.ltp_i

struct StabilizationSetpoint.sp.ltp_i

Definition at line 70 of file stabilization.h.

Data Fields
int32_t	heading
struct Int32Vect2	vect

◆ StabilizationSetpoint.sp.ltp_f

struct StabilizationSetpoint.sp.ltp_f

Definition at line 71 of file stabilization.h.

Data Fields
float	heading
struct FloatVect2	vect

◆ StabilizationSetpoint.r_sp

union StabilizationSetpoint.r_sp

Definition at line 73 of file stabilization.h.

Data Fields
struct FloatRates	rates_f
struct Int32Rates	rates_i

◆ ThrustSetpoint.sp

union ThrustSetpoint.sp

Definition at line 91 of file stabilization.h.

Data Fields
float	th_incr_f[3]
int32_t	th_incr_i[3]
float	thrust_f[3]
int32_t	thrust_i[3]

Macro Definition Documentation

◆ STAB_SP_SET_EULERS_ZERO

#define STAB_SP_SET_EULERS_ZERO ( _sp )

Value:

  { \
  _sp.type = STAB_SP_EULERS;  \
  _sp.format = STAB_SP_INT;   \
  _sp.sp.eulers_i.phi = 0;    \
  _sp.sp.eulers_i.theta = 0;  \
  _sp.sp.eulers_i.psi = 0;    \
}

Definition at line 176 of file stabilization.h.

◆ STABILIZATION_ATT_SUBMODE_CARE_FREE

#define STABILIZATION_ATT_SUBMODE_CARE_FREE 1

Definition at line 46 of file stabilization.h.

◆ STABILIZATION_ATT_SUBMODE_FORWARD

#define STABILIZATION_ATT_SUBMODE_FORWARD 2

Definition at line 47 of file stabilization.h.

◆ STABILIZATION_ATT_SUBMODE_HEADING

#define STABILIZATION_ATT_SUBMODE_HEADING 0

Stabilization sub-modes for attitude.

Definition at line 45 of file stabilization.h.

◆ STABILIZATION_MODE_ATTITUDE

#define STABILIZATION_MODE_ATTITUDE 3

Definition at line 41 of file stabilization.h.

◆ STABILIZATION_MODE_DIRECT

#define STABILIZATION_MODE_DIRECT 1

Definition at line 39 of file stabilization.h.

◆ STABILIZATION_MODE_NONE

#define STABILIZATION_MODE_NONE 0

Stabilization modes.

Definition at line 38 of file stabilization.h.

◆ STABILIZATION_MODE_RATE

#define STABILIZATION_MODE_RATE 2

Definition at line 40 of file stabilization.h.

◆ THRUST_AXIS_X

#define THRUST_AXIS_X 0

Definition at line 172 of file stabilization.h.

◆ THRUST_AXIS_Y

#define THRUST_AXIS_Y 1

Definition at line 173 of file stabilization.h.

◆ THRUST_AXIS_Z

#define THRUST_AXIS_Z 2

Definition at line 174 of file stabilization.h.

◆ THRUST_SP_SET_ZERO

#define THRUST_SP_SET_ZERO ( _sp )

Value:

  { \
  _sp.type = THRUST_SP;       \
  _sp.format = THRUST_SP_INT; \
  _sp.sp.thrust_i[0] = 0;     \
  _sp.sp.thrust_i[1] = 0;     \
  _sp.sp.thrust_i[2] = 0;     \
}

Definition at line 184 of file stabilization.h.

Function Documentation

◆ stab_sp_from_eulers_f()

struct StabilizationSetpoint stab_sp_from_eulers_f ( struct FloatEulers * eulers )

Definition at line 579 of file stabilization.c.

References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.

◆ stab_sp_from_eulers_i()

struct StabilizationSetpoint stab_sp_from_eulers_i ( struct Int32Eulers * eulers )

Definition at line 579 of file stabilization.c.

Referenced by guidance_module_run(), and vertical_ctrl_module_run().

Here is the caller graph for this function:

◆ stab_sp_from_ltp_f()

struct StabilizationSetpoint stab_sp_from_ltp_f	(	struct FloatVect2 *	vect,
		float	heading
	)

Definition at line 579 of file stabilization.c.

◆ stab_sp_from_ltp_i()

struct StabilizationSetpoint stab_sp_from_ltp_i	(	struct Int32Vect2 *	vect,
		int32_t	heading
	)

Definition at line 579 of file stabilization.c.

◆ stab_sp_from_quat_f()

struct StabilizationSetpoint stab_sp_from_quat_f ( struct FloatQuat * quat )

Definition at line 579 of file stabilization.c.

Referenced by stabilization_attitude_reset_rc().

Here is the caller graph for this function:

◆ stab_sp_from_quat_ff_rates_f()

struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f	(	struct FloatQuat *	quat,
		struct FloatRates *	rates
	)

Definition at line 579 of file stabilization.c.

◆ stab_sp_from_quat_i()

struct StabilizationSetpoint stab_sp_from_quat_i ( struct Int32Quat * quat )

Definition at line 579 of file stabilization.c.

◆ stab_sp_from_rates_f()

struct StabilizationSetpoint stab_sp_from_rates_f ( struct FloatRates * rates )

Definition at line 579 of file stabilization.c.

Referenced by stabilization_indi_attitude_run().

Here is the caller graph for this function:

◆ stab_sp_from_rates_i()

struct StabilizationSetpoint stab_sp_from_rates_i ( struct Int32Rates * rates )

Definition at line 579 of file stabilization.c.

◆ stab_sp_to_eulers_f()

struct FloatEulers stab_sp_to_eulers_f ( struct StabilizationSetpoint * sp )

Definition at line 314 of file stabilization.c.

References Stabilization::cmd, MAX_PPRZ, stabilization, and update_second_order_low_pass_int().

Referenced by stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stab_sp_to_eulers_i()

struct Int32Eulers stab_sp_to_eulers_i ( struct StabilizationSetpoint * sp )

Definition at line 314 of file stabilization.c.

Referenced by reset_horizontal_vars(), stabilization_attitude_run(), and stabilization_indi_attitude_run().

Here is the caller graph for this function:

◆ stab_sp_to_quat_f()

struct FloatQuat stab_sp_to_quat_f ( struct StabilizationSetpoint * sp )

Definition at line 314 of file stabilization.c.

Referenced by stabilization_attitude_run(), and stabilization_indi_attitude_run().

Here is the caller graph for this function:

◆ stab_sp_to_quat_i()

struct Int32Quat stab_sp_to_quat_i ( struct StabilizationSetpoint * sp )

Definition at line 314 of file stabilization.c.

Referenced by stabilization_attitude_run(), and stabilization_indi_attitude_run().

Here is the caller graph for this function:

◆ stab_sp_to_rates_f()

struct FloatRates stab_sp_to_rates_f ( struct StabilizationSetpoint * sp )

Definition at line 314 of file stabilization.c.

Referenced by stabilization_indi_attitude_run(), stabilization_indi_rate_run(), and stabilization_rate_read_rc().

Here is the caller graph for this function:

◆ stab_sp_to_rates_i()

struct Int32Rates stab_sp_to_rates_i ( struct StabilizationSetpoint * sp )

Definition at line 314 of file stabilization.c.

◆ stabilization_filter_commands()

void stabilization_filter_commands ( void )

Command filter for vibrating airframes.

Definition at line 314 of file stabilization.c.

Referenced by stab_sp_rotate_f().

Here is the caller graph for this function:

◆ stabilization_get_failsafe_sp()

struct StabilizationSetpoint stabilization_get_failsafe_sp ( void )

Get stabilization setpoint for failsafe.

Definition at line 242 of file stabilization.c.

Referenced by autopilot_static_periodic().

Here is the caller graph for this function:

◆ stabilization_init()

void stabilization_init ( void )

Init function.

Definition at line 94 of file stabilization.c.

References Stabilization::att_submode, Stabilization::cmd, DefaultPeriodic, SecondOrderLowPass_int::i, init_second_order_low_pass_int(), Stabilization::mode, rc_cb(), rc_ev, Stabilization::rc_in, Stabilization::rc_sp, register_periodic_telemetry(), send_tune_hover(), STAB_SP_SET_EULERS_ZERO, stabilization, STABILIZATION_ATT_SUBMODE_HEADING, stabilization_attitude_rc_setpoint_init(), STABILIZATION_MODE_NONE, and STABILIZATION_RC_ID.

Here is the call graph for this function:

◆ stabilization_mode_changed()

void stabilization_mode_changed	(	uint8_t	new_mode,
		uint8_t	submode
	)

Check mode change.

Definition at line 138 of file stabilization.c.

Referenced by autopilot_static_set_mode().

Here is the caller graph for this function:

◆ stabilization_run()

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

Call default stabilization control.

Parameters

[in]	in_flight	true if rotorcraft is flying
[in]	sp	pointer to the stabilization setpoint, computed in guidance or from RC
[in]	thrust	pointer to thrust setpoint computed by vertical guidance
[out]	cmd	pointer to the output command vector

Definition at line 242 of file stabilization.c.

Referenced by autopilot_static_periodic(), and guidance_module_run().

Here is the caller graph for this function:

◆ th_sp_from_incr_f()

struct ThrustSetpoint th_sp_from_incr_f	(	float	th_increment,
		uint8_t	axis
	)

Definition at line 579 of file stabilization.c.

◆ th_sp_from_incr_i()

struct ThrustSetpoint th_sp_from_incr_i	(	int32_t	th_increment,
		uint8_t	axis
	)

Definition at line 579 of file stabilization.c.

◆ th_sp_from_incr_vect_f()

struct ThrustSetpoint th_sp_from_incr_vect_f ( float th_increment[3] )

Definition at line 579 of file stabilization.c.

◆ th_sp_from_incr_vect_i()

struct ThrustSetpoint th_sp_from_incr_vect_i ( int32_t th_increment[3] )

Definition at line 579 of file stabilization.c.

◆ th_sp_from_thrust_f()

struct ThrustSetpoint th_sp_from_thrust_f	(	float	thrust,
		uint8_t	axis
	)

Definition at line 579 of file stabilization.c.

◆ th_sp_from_thrust_i()

struct ThrustSetpoint th_sp_from_thrust_i	(	int32_t	thrust,
		uint8_t	axis
	)

Definition at line 579 of file stabilization.c.

Referenced by guidance_module_run(), and vertical_ctrl_module_run().

Here is the caller graph for this function:

◆ th_sp_from_thrust_vect_f()

struct ThrustSetpoint th_sp_from_thrust_vect_f ( float thrust[3] )

Definition at line 579 of file stabilization.c.

◆ th_sp_from_thrust_vect_i()

struct ThrustSetpoint th_sp_from_thrust_vect_i ( int32_t thrust[3] )

Definition at line 579 of file stabilization.c.

◆ th_sp_to_incr_f()

float th_sp_to_incr_f	(	struct ThrustSetpoint *	th,
		int32_t	thrust,
		uint8_t	axis
	)

Definition at line 579 of file stabilization.c.

◆ th_sp_to_incr_i()

int32_t th_sp_to_incr_i	(	struct ThrustSetpoint *	th,
		int32_t	thrust,
		uint8_t	axis
	)

Definition at line 562 of file stabilization.c.

References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.

◆ th_sp_to_thrust_f()

float th_sp_to_thrust_f	(	struct ThrustSetpoint *	th,
		int32_t	thrust,
		uint8_t	axis
	)

Definition at line 544 of file stabilization.c.

References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.

◆ th_sp_to_thrust_i()

int32_t th_sp_to_thrust_i	(	struct ThrustSetpoint *	th,
		int32_t	thrust,
		uint8_t	axis
	)

Definition at line 527 of file stabilization.c.

References ThrustSetpoint::format, MAX_PPRZ, ThrustSetpoint::sp, and ThrustSetpoint::type.

Referenced by stabilization_attitude_run(), stabilization_direct_run(), stabilization_indi_rate_run(), and stabilization_rate_read_rc().

Here is the caller graph for this function:

Variable Documentation

◆ stabilization

struct Stabilization stabilization

extern

Definition at line 1 of file stabilization.c.

Referenced by autopilot_check_in_flight(), autopilot_in_flight_end_detection(), autopilot_static_periodic(), autopilot_static_set_mode(), compute_accel_from_speed_sp(), ctrl_module_run(), direct_memory_logger_periodic(), draw_osd(), eff_scheduling_generic_periodic(), eff_scheduling_periodic_a(), guidance_flip_run(), guidance_indi_enter(), guidance_indi_hybrid_set_wls_settings(), guidance_module_enter(), guidance_module_run(), guidance_v_run(), ins_flow_update(), logger_control_effectiveness_periodic(), logger_file_write_row(), nav_heli_spinup_run(), nav_heli_spinup_setup(), oneloop_andi_run(), periodic_rotwing_state(), rc_cb(), reset_horizontal_vars(), rotwing_check_set_current_state(), send_att(), send_bebop_actuators(), send_rate(), send_tune_hover(), send_windtunnel_meas(), stab_sp_rotate_f(), stab_sp_to_eulers_f(), stabilization_attitude_read_rc(), stabilization_attitude_reset_rc(), stabilization_init(), transition_run(), and vertical_ctrl_module_run().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

◆ Stabilization

◆ StabilizationSetpoint.sp

◆ StabilizationSetpoint.sp.ltp_i

◆ StabilizationSetpoint.sp.ltp_f

◆ StabilizationSetpoint.r_sp

◆ ThrustSetpoint.sp

Macro Definition Documentation

◆ STAB_SP_SET_EULERS_ZERO

◆ STABILIZATION_ATT_SUBMODE_CARE_FREE

◆ STABILIZATION_ATT_SUBMODE_FORWARD

◆ STABILIZATION_ATT_SUBMODE_HEADING

◆ STABILIZATION_MODE_ATTITUDE

◆ STABILIZATION_MODE_DIRECT

◆ STABILIZATION_MODE_NONE

◆ STABILIZATION_MODE_RATE

◆ THRUST_AXIS_X

◆ THRUST_AXIS_Y

◆ THRUST_AXIS_Z

◆ THRUST_SP_SET_ZERO

Function Documentation

◆ stab_sp_from_eulers_f()

◆ stab_sp_from_eulers_i()

◆ stab_sp_from_ltp_f()

◆ stab_sp_from_ltp_i()

◆ stab_sp_from_quat_f()

◆ stab_sp_from_quat_ff_rates_f()

◆ stab_sp_from_quat_i()

◆ stab_sp_from_rates_f()

◆ stab_sp_from_rates_i()

◆ stab_sp_to_eulers_f()

◆ stab_sp_to_eulers_i()

◆ stab_sp_to_quat_f()

◆ stab_sp_to_quat_i()

◆ stab_sp_to_rates_f()

◆ stab_sp_to_rates_i()

◆ stabilization_filter_commands()

◆ stabilization_get_failsafe_sp()

◆ stabilization_init()

◆ stabilization_mode_changed()

◆ stabilization_run()

◆ th_sp_from_incr_f()

◆ th_sp_from_incr_i()

◆ th_sp_from_incr_vect_f()

◆ th_sp_from_incr_vect_i()

◆ th_sp_from_thrust_f()

◆ th_sp_from_thrust_i()

◆ th_sp_from_thrust_vect_f()

◆ th_sp_from_thrust_vect_i()

◆ th_sp_to_incr_f()

◆ th_sp_to_incr_i()

◆ th_sp_to_thrust_f()

◆ th_sp_to_thrust_i()

Variable Documentation

◆ stabilization