#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "math/wls/wls_alloc.h"

Include dependency graph for stabilization_indi.h:

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

Go to the source code of this file.

Data Structures
struct	Indi_gains

Macros
#define	INDI_G_SCALING 1000.0

Functions
void	stabilization_indi_init (void)
	Function that initializes important values upon engaging INDI.

void	stabilization_indi_enter (void)
	Function that resets important values upon engaging INDI.

void	stabilization_indi_rate_run (bool in_flight, struct StabilizationSetpoint rate_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

void	stabilization_indi_set_wls_settings (void)
	Function that sets the u_min, u_max and u_pref if function not elsewhere defined.

void	stabilization_indi_update_filt_freq (float freq)

struct FloatRates	stabilization_indi_attitude_controller (struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
	Default PD angular rate controller.

struct FloatRates	stabilization_indi_rate_controller (struct FloatRates rates, struct FloatRates sp)
	Default PD angular acceleration controller.

Variables
float	g1g2 [INDI_OUTPUTS][INDI_NUM_ACT]

float	actuator_state_filt_vect [INDI_NUM_ACT]

struct FloatVect3	stab_thrust_filt

int16_t	actuators_pprz [INDI_NUM_ACT+1]
	PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if the command_laws are set up appropriately in the airframe file.

bool	act_is_servo [INDI_NUM_ACT]

bool	indi_use_adaptive

float *	Bwls [INDI_OUTPUTS]

float	thrust_bx_eff

float	thrust_bx_act_dyn

float	actuator_thrust_bx_pprz

float	thrust_bx_state_filt

float	act_pref [INDI_NUM_ACT]

float	stablization_indi_yaw_dist_limit

struct Indi_gains	indi_gains

float	stabilization_indi_filter_freq

struct WLS_t	wls_stab_p

Data Structure Documentation

◆ Indi_gains

struct Indi_gains

Definition at line 58 of file stabilization_indi.h.

Collaboration diagram for Indi_gains:

Data Fields
struct FloatRates	att
struct FloatRates	rate

Macro Definition Documentation

◆ INDI_G_SCALING

#define INDI_G_SCALING 1000.0

Definition at line 31 of file stabilization_indi.h.

Function Documentation

◆ stabilization_indi_attitude_controller()

struct FloatRates stabilization_indi_attitude_controller	(	struct FloatQuat	att,
		struct FloatQuat	att_sp,
		struct FloatRates	rates_ff
	)

extern

Default PD angular rate controller.

Takes the current attitude filtered state and setpoint and compute the desired rates. Can be redefined elsewhere to use an other control scheme.

Default PD angular rate controller.

Takes the current attitude filtered state and setpoint and compute the desired rates. Can be redefined elsewhere to use an other control scheme.

Definition at line 821 of file stabilization_indi.c.

References Ap, Indi_gains::att, att_state, autopilot_in_flight(), Bp, Cp, Dp, float_quat_inv_comp_norm_shortest(), float_quat_tilt_twist(), FLOAT_RATES_ZERO, foo, indi_gains, FloatRates::p, FloatRates::q, FloatRates::r, Indi_gains::rate, and RATES_ADD.

Referenced by stabilization_indi_attitude_run().

Here is the call graph for this function:

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
	)

extern

runs stabilization indi

Parameters

	in_flight	enable integrator only in flight
	att_sp	attitude stabilization setpoint
	thrust	thrust setpoint
[out]	output	command vector

Function that should be called to run the INDI controller

Parameters

in_flight	not used
rate_control	rate control enabled, otherwise attitude control

Definition at line 799 of file stabilization_indi.c.

◆ stabilization_indi_enter()

void stabilization_indi_enter ( void )

extern

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 478 of file stabilization_indi.c.

◆ stabilization_indi_init()

void stabilization_indi_init ( void )

extern

Function that initializes important values upon engaging INDI.

Definition at line 403 of file stabilization_indi.c.

◆ stabilization_indi_rate_controller()

struct FloatRates stabilization_indi_rate_controller	(	struct FloatRates	rates,
		struct FloatRates	sp
	)

extern

Default PD angular acceleration controller.

Takes the current rates filtered state and setpoint and compute the desired acceleration. Can be redefined elsewhere to use an other control scheme.

Default PD angular acceleration controller.

Takes the current rates filtered state and setpoint and compute the desired acceleration.

Definition at line 855 of file stabilization_indi.c.

References Ad, autopilot_in_flight(), Bd, Cd, Dd, FLOAT_RATES_ZERO, foo, indi_gains, FloatRates::p, FloatRates::q, FloatRates::r, Indi_gains::rate, rate_state, and RATES_DIFF.

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:

Here is the caller 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
	)

extern

Does the INDI calculations.

Parameters

in_flight	boolean that states if the UAV is in flight or not
sp	rate setpoint
thrust	thrust setpoint
cmd	output command array

Function that calculates the INDI commands

Parameters

in_flight	true aircraft is flying
sp	rate setpoint
thrust	thrust setpoint
cmd	output command array

Definition at line 583 of file stabilization_indi.c.

◆ stabilization_indi_set_wls_settings()

void stabilization_indi_set_wls_settings ( void )

extern

Function that sets the u_min, u_max and u_pref if function not elsewhere defined.

Definition at line 869 of file stabilization_indi.c.

References act_is_servo, act_pref, actuator_state_filt_vect, actuators_pprz, CMH_ACT_MOTOR_LEFT, CMH_ACT_MOTOR_RIGHT, CMH_ACT_MOTOR_TAIL, CMH_ACT_YAW, CMH_MOTOR_IDLE, CMH_TILT_DIFF_MAX, eff_sched_pusher_time, foo, guidance_h, GUIDANCE_H_MODE_HOVER, GUIDANCE_H_MODE_NAV, MAX_PPRZ, HorizontalGuidance::mode, rw_flap_offset, STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD, stateGetAirspeed_f(), WLS_t::u_max, WLS_t::u_min, WLS_t::u_pref, and wls_stab_p.

Referenced by stabilization_indi_rate_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ stabilization_indi_update_filt_freq()

void stabilization_indi_update_filt_freq ( float freq )

extern

Definition at line 484 of file stabilization_indi.c.

References foo, init_butterworth_2_low_pass(), init_first_order_low_pass(), p, rates_filt_fo, stabilization_indi_filter_freq, and stateGetBodyRates_f().

Here is the call graph for this function:

Variable Documentation

◆ act_is_servo

bool act_is_servo[INDI_NUM_ACT]

extern

Definition at line 205 of file stabilization_indi.c.

Referenced by ground_detect_periodic(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

◆ act_pref

float act_pref[INDI_NUM_ACT]

extern

Definition at line 167 of file stabilization_indi.c.

Referenced by ctrl_eff_scheduling_periodic(), and stabilization_indi_set_wls_settings().

◆ actuator_state_filt_vect

float actuator_state_filt_vect[INDI_NUM_ACT]

extern

Definition at line 240 of file stabilization_indi.c.

Referenced by eff_scheduling_cyfoam_periodic(), eff_scheduling_periodic_b(), eff_scheduling_rotwing_update_cmd(), eff_scheduling_rotwing_update_hover_motor_effectiveness(), ground_detect_periodic(), guidance_indi_hybrid_set_wls_settings(), stabilization_indi_init(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

◆ actuator_thrust_bx_pprz

float actuator_thrust_bx_pprz

extern

◆ actuators_pprz

int16_t actuators_pprz[INDI_NUM_ACT+1]

extern

PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if the command_laws are set up appropriately in the airframe file.

FIXME add an extra slot for specific case (e.g. rotwing in simulation)

Definition at line 314 of file stabilization_indi.c.

Referenced by actuators_hitl_periodic(), control_mixing_heewing_attitude_direct(), control_mixing_heewing_attitude_plane(), control_mixing_heewing_nav_run(), ekf_aw_wrapper_fetch(), nps_autopilot_run_step(), rotwing_state_periodic(), schdule_control_effectiveness(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

◆ Bwls

float* Bwls[INDI_OUTPUTS]

extern

Definition at line 172 of file stabilization_indi.c.

Referenced by eff_scheduling_cyfoam_periodic(), stabilization_indi_init(), and stabilization_indi_rate_run().

◆ g1g2

float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]

extern

◆ indi_gains

struct Indi_gains indi_gains

extern

Definition at line 185 of file stabilization_indi.c.

Referenced by eff_scheduling_periodic_b(), stabilization_indi_attitude_controller(), and stabilization_indi_rate_controller().

◆ indi_use_adaptive

bool indi_use_adaptive

extern

Definition at line 198 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ stab_thrust_filt

struct FloatVect3 stab_thrust_filt

extern

Definition at line 246 of file stabilization_indi.c.

Referenced by guidance_indi_calcG_yxz(), guidance_indi_set_wls_settings(), and stabilization_indi_rate_run().

◆ stabilization_indi_filter_freq

float stabilization_indi_filter_freq

extern

Definition at line 251 of file stabilization_indi.c.

Referenced by stabilization_indi_update_filt_freq().

◆ stablization_indi_yaw_dist_limit

float stablization_indi_yaw_dist_limit

extern

Definition at line 237 of file stabilization_indi.c.

Referenced by stabilization_indi_rate_run().

◆ thrust_bx_act_dyn

float thrust_bx_act_dyn

extern

◆ thrust_bx_eff

float thrust_bx_eff

extern

◆ thrust_bx_state_filt

float thrust_bx_state_filt

extern

◆ wls_stab_p

struct WLS_t wls_stab_p

extern

Definition at line 325 of file stabilization_andi.c.

Referenced by schdule_control_effectiveness(), send_wls_u_stab(), send_wls_v_stab(), send_wls_v_stabilization_andi(), stabilization_andi_run(), stabilization_indi_rate_run(), and stabilization_indi_set_wls_settings().

Data Structures

Macros

Functions

Variables

Data Structure Documentation

◆ Indi_gains

Macro Definition Documentation

◆ INDI_G_SCALING

Function Documentation

◆ stabilization_indi_attitude_controller()

◆ stabilization_indi_attitude_run()

◆ stabilization_indi_enter()

◆ stabilization_indi_init()

◆ stabilization_indi_rate_controller()

◆ stabilization_indi_rate_run()

◆ stabilization_indi_set_wls_settings()

◆ stabilization_indi_update_filt_freq()

Variable Documentation

◆ act_is_servo

◆ act_pref

◆ actuator_state_filt_vect

◆ actuator_thrust_bx_pprz

◆ actuators_pprz

◆ Bwls

◆ g1g2

◆ indi_gains

◆ indi_use_adaptive

◆ stab_thrust_filt

◆ stabilization_indi_filter_freq

◆ stablization_indi_yaw_dist_limit

◆ thrust_bx_act_dyn

◆ thrust_bx_eff

◆ thrust_bx_state_filt

◆ wls_stab_p