Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization.c File Reference

General stabilization interface for rotorcrafts. More...

Include dependency graph for stabilization.c:

Go to the source code of this file.

Macros

#define STABILIZATION_RC_ID   ABI_BROADCAST
#define TRANSITION_TO_HOVER   false
 Transition from 0 to 100%, used for pitch offset of hybrids.
#define TRANSITION_TO_FORWARD   true
#define TRANSITION_TIME   3.f

Functions

static void rc_cb (uint8_t sender_id UNUSED, struct RadioControl *rc)
static void send_tune_hover (struct transport_tx *trans UNUSED, struct link_device *dev UNUSED)
void stabilization_init (void)
 Init function.
static void stabilization_attitude_reset_rc (void)
void stabilization_mode_changed (uint8_t new_mode, uint8_t submode)
 Check mode change.
struct StabilizationSetpoint WEAK stabilization_attitude_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn, struct RadioControl *rc)
 Retrun attitude setpoint from RC as euler angles.
static void transition_run (bool to_forward)
void stabilization_run (bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
 Call default stabilization control.
struct StabilizationSetpoint stabilization_get_failsafe_sp (void)
 Get stabilization setpoint for failsafe.
static struct Int32Eulers stab_sp_rotate_i (struct Int32Vect2 *vect, int32_t heading)
static struct FloatEulers stab_sp_rotate_f (struct FloatVect2 *vect, float heading)
void stabilization_filter_commands (void)
 Command filter for vibrating airframes.
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
static abi_event rc_ev
static const float transition_increment = 1.f / (TRANSITION_TIME * PERIODIC_FREQUENCY)

Detailed Description

General stabilization interface for rotorcrafts.

Definition in file stabilization.c.

Macro Definition Documentation

◆ STABILIZATION_RC_ID

#define STABILIZATION_RC_ID   ABI_BROADCAST

Definition at line 66 of file stabilization.c.

Referenced by stabilization_init().

◆ TRANSITION_TIME

#define TRANSITION_TIME   3.f

Definition at line 224 of file stabilization.c.

◆ TRANSITION_TO_FORWARD

◆ TRANSITION_TO_HOVER

#define TRANSITION_TO_HOVER   false

Transition from 0 to 100%, used for pitch offset of hybrids.

Definition at line 220 of file stabilization.c.

Referenced by control_mixing_heewing_attitude_direct(), control_mixing_heewing_nav_run(), and stabilization_run().

Function Documentation

◆ rc_cb()

◆ send_tune_hover()

void send_tune_hover ( struct transport_tx *trans UNUSED,
struct link_device *dev UNUSED )
static

Definition at line 75 of file stabilization.c.

References COMMAND_PITCH, COMMAND_ROLL, dev, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_YAW, stabilization, stateGetNedToBodyEulers_i(), and UNUSED.

Referenced by stabilization_init().

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

◆ stab_sp_from_eulers_f()

◆ stab_sp_from_eulers_i()

struct StabilizationSetpoint stab_sp_from_eulers_i ( struct Int32Eulers * eulers)

Definition at line 628 of file stabilization.c.

References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_EULERS, stab_sp_from_eulers_i(), and StabilizationSetpoint::STAB_SP_INT.

Referenced by guidance_module_run(), stab_sp_from_eulers_i(), and vertical_ctrl_module_run().

Here is the call graph for this function:
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 659 of file stabilization.c.

References heading, StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_ltp_f(), and StabilizationSetpoint::STAB_SP_LTP.

Referenced by stab_sp_from_ltp_f().

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

◆ stab_sp_from_ltp_i()

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

Definition at line 648 of file stabilization.c.

References heading, StabilizationSetpoint::sp, stab_sp_from_ltp_i(), StabilizationSetpoint::STAB_SP_INT, and StabilizationSetpoint::STAB_SP_LTP.

Referenced by guidance_pid_h_run(), and stab_sp_from_ltp_i().

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

◆ stab_sp_from_quat_f()

struct StabilizationSetpoint stab_sp_from_quat_f ( struct FloatQuat * quat)

Definition at line 607 of file stabilization.c.

References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_quat_f(), and StabilizationSetpoint::STAB_SP_QUAT.

Referenced by guidance_indi_run(), stab_sp_from_quat_f(), stabilization_attitude_read_rc(), and stabilization_attitude_reset_rc().

Here is the call graph for this function:
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 617 of file stabilization.c.

References StabilizationSetpoint::r_sp, StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_quat_ff_rates_f(), and StabilizationSetpoint::STAB_SP_QUAT_FF_RATE.

Referenced by guidance_indi_run(), and stab_sp_from_quat_ff_rates_f().

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

◆ stab_sp_from_quat_i()

struct StabilizationSetpoint stab_sp_from_quat_i ( struct Int32Quat * quat)

Definition at line 597 of file stabilization.c.

References StabilizationSetpoint::sp, stab_sp_from_quat_i(), StabilizationSetpoint::STAB_SP_INT, and StabilizationSetpoint::STAB_SP_QUAT.

Referenced by guidance_hybrid_set_cmd_i(), and stab_sp_from_quat_i().

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

◆ stab_sp_from_rates_f()

struct StabilizationSetpoint stab_sp_from_rates_f ( struct FloatRates * rates)

Definition at line 680 of file stabilization.c.

References StabilizationSetpoint::sp, StabilizationSetpoint::STAB_SP_FLOAT, stab_sp_from_rates_f(), and StabilizationSetpoint::STAB_SP_RATES.

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

Here is the call graph for this function:
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 670 of file stabilization.c.

References StabilizationSetpoint::sp, stab_sp_from_rates_i(), StabilizationSetpoint::STAB_SP_INT, and StabilizationSetpoint::STAB_SP_RATES.

Referenced by stab_sp_from_rates_i().

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

◆ stab_sp_rotate_f()

struct FloatEulers stab_sp_rotate_f ( struct FloatVect2 * vect,
float heading )
static

Definition at line 301 of file stabilization.c.

References heading, FloatEulers::phi, FloatEulers::psi, stab_sp_rotate_f(), stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by stab_sp_rotate_f(), stab_sp_to_eulers_f(), and stab_sp_to_eulers_i().

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

◆ stab_sp_rotate_i()

struct Int32Eulers stab_sp_rotate_i ( struct Int32Vect2 * vect,
int32_t heading )
static

Definition at line 287 of file stabilization.c.

References heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, stab_sp_rotate_i(), stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Referenced by stab_sp_rotate_i(), stab_sp_to_eulers_f(), and stab_sp_to_eulers_i().

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

◆ stab_sp_to_eulers_f()

struct FloatEulers stab_sp_to_eulers_f ( struct StabilizationSetpoint * sp)

◆ stab_sp_to_eulers_i()

struct Int32Eulers stab_sp_to_eulers_i ( struct StabilizationSetpoint * sp)

◆ stab_sp_to_quat_f()

struct FloatQuat stab_sp_to_quat_f ( struct StabilizationSetpoint * sp)

◆ stab_sp_to_quat_i()

struct Int32Quat stab_sp_to_quat_i ( struct StabilizationSetpoint * sp)

◆ stab_sp_to_rates_f()

struct FloatRates stab_sp_to_rates_f ( struct StabilizationSetpoint * sp)

Definition at line 510 of file stabilization.c.

References RATES_FLOAT_OF_BFP, and stab_sp_to_rates_f().

Referenced by stab_sp_to_rates_f(), stabilization_andi_run(), stabilization_indi_attitude_run(), stabilization_indi_rate_run(), and stabilization_rate_run().

Here is the call graph for this function:
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 493 of file stabilization.c.

References RATES_BFP_OF_REAL, and stab_sp_to_rates_i().

Referenced by stab_sp_to_rates_i().

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

◆ stabilization_attitude_read_rc()

struct StabilizationSetpoint WEAK stabilization_attitude_read_rc ( bool in_flight,
bool carefree,
bool coordinated_turn,
struct RadioControl * rc )

Retrun attitude setpoint from RC as euler angles.

weak function that can be re-implemeted if needed

Parameters
[in]in_flighttrue if in flight
[in]in_carefreetrue if in carefree mode
[in]coordinated_turntrue if in horizontal mode forward
[in]rcpointer to radio control structure
Returns
stabilization setpoint

Definition at line 172 of file stabilization.c.

References stab_sp_from_quat_f(), stabilization, stabilization_attitude_read_rc(), stabilization_attitude_read_rc_setpoint(), and stabilization_attitude_read_rc_setpoint_earth_bound().

Referenced by rc_cb(), stabilization_attitude_init(), and stabilization_attitude_read_rc().

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

◆ stabilization_attitude_reset_rc()

void stabilization_attitude_reset_rc ( void )
static

Definition at line 132 of file stabilization.c.

References stab_sp_from_quat_f(), stabilization, stabilization_attitude_reset_rc_setpoint(), and stateGetNedToBodyQuat_f().

Referenced by stabilization_mode_changed().

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

◆ stabilization_filter_commands()

void stabilization_filter_commands ( void )

Command filter for vibrating airframes.

Definition at line 314 of file stabilization.c.

References COMMAND_PITCH, COMMAND_ROLL, MAX_PPRZ, stabilization, and update_second_order_low_pass_int().

Referenced by stabilization_run().

Here is the call graph for this function:
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 275 of file stabilization.c.

References FloatEulers::psi, stab_sp_from_eulers_f(), stabilization_get_failsafe_sp(), and stateGetNedToBodyEulers_f().

Referenced by autopilot_static_periodic(), and stabilization_get_failsafe_sp().

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

◆ stabilization_init()

◆ stabilization_mode_changed()

◆ stabilization_run()

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

Call default stabilization control.

Parameters
[in]in_flighttrue if rotorcraft is flying
[in]sppointer to the stabilization setpoint, computed in guidance or from RC
[in]thrustpointer to thrust setpoint computed by vertical guidance
[out]cmdpointer to the output command vector

Definition at line 242 of file stabilization.c.

References StabilizationSetpoint::sp, stabilization, STABILIZATION_ATT_SUBMODE_FORWARD, stabilization_attitude_run(), stabilization_direct_run(), stabilization_filter_commands(), STABILIZATION_MODE_ATTITUDE, STABILIZATION_MODE_DIRECT, STABILIZATION_MODE_RATE, stabilization_rate_run(), transition_run(), TRANSITION_TO_FORWARD, and TRANSITION_TO_HOVER.

Referenced by autopilot_static_periodic(), control_mixing_heewing_attitude_direct(), control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), and guidance_module_run().

Here is the call graph for this function:
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 720 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_incr_f(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_FLOAT.

Referenced by th_sp_from_incr_f().

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

◆ th_sp_from_incr_i()

struct ThrustSetpoint th_sp_from_incr_i ( int32_t th_increment,
uint8_t axis )

Definition at line 710 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_incr_i(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_INT.

Referenced by th_sp_from_incr_i().

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

◆ th_sp_from_incr_vect_f()

struct ThrustSetpoint th_sp_from_incr_vect_f ( float th_increment[3])

Definition at line 766 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_incr_vect_f(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_FLOAT.

Referenced by guidance_indi_run(), and th_sp_from_incr_vect_f().

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

◆ th_sp_from_incr_vect_i()

struct ThrustSetpoint th_sp_from_incr_vect_i ( int32_t th_increment[3])

Definition at line 754 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_incr_vect_i(), ThrustSetpoint::THRUST_INCR_SP, and ThrustSetpoint::THRUST_SP_INT.

Referenced by th_sp_from_incr_vect_i().

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

◆ th_sp_from_thrust_f()

struct ThrustSetpoint th_sp_from_thrust_f ( float thrust,
uint8_t axis )

Definition at line 700 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_thrust_f(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_FLOAT.

Referenced by th_sp_from_thrust_f().

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

◆ th_sp_from_thrust_i()

◆ th_sp_from_thrust_vect_f()

struct ThrustSetpoint th_sp_from_thrust_vect_f ( float thrust[3])

Definition at line 742 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_thrust_vect_f(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_FLOAT.

Referenced by guidance_set_rc_h_thrust(), and th_sp_from_thrust_vect_f().

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

◆ th_sp_from_thrust_vect_i()

struct ThrustSetpoint th_sp_from_thrust_vect_i ( int32_t thrust[3])

Definition at line 730 of file stabilization.c.

References ThrustSetpoint::sp, th_sp_from_thrust_vect_i(), ThrustSetpoint::THRUST_SP, and ThrustSetpoint::THRUST_SP_INT.

Referenced by th_sp_from_thrust_vect_i().

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

◆ 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.

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

Referenced by stabilization_indi_rate_run().

Here is the caller graph for this function:

◆ th_sp_to_incr_i()

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

◆ 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.

Referenced by guidance_set_rc_h_thrust(), and stabilization_andi_run().

Here is the caller graph for this function:

◆ th_sp_to_thrust_i()

◆ transition_run()

void transition_run ( bool to_forward)
inlinestatic

Definition at line 229 of file stabilization.c.

References stabilization, and transition_increment.

Referenced by stabilization_run().

Here is the caller graph for this function:

Variable Documentation

◆ rc_ev

abi_event rc_ev
static

Definition at line 69 of file stabilization.c.

◆ stabilization

◆ transition_increment

const float transition_increment = 1.f / (TRANSITION_TIME * PERIODIC_FREQUENCY)
static

Definition at line 227 of file stabilization.c.

Referenced by transition_run(), and transition_run().