Stabilization Module

Header for sensor functions. More...

Files
file	stabilization.h
	Attitude stabilization module.
file	virtualflybar.h
	Attitude stabilization module.
file	sensors.c
	Acquire sensor data from sensors registered with PIOS_Sensors.
file	stabilization.c
	Control the UAV attitude to StabilizationDesired.
file	virtualflybar.c
	Virtual flybar control mode.

Macros
#define	MAX_SENSOR_PERIOD   6
#define	REQUIRED_GOOD_CYCLES   50
#define	MAX_TIME_BETWEEN_VALID_BARO_DATAS_US   (100*1000)
#define	MAX_TIME_BETWEEN_VALID_MAG_DATAS_US   (300*1000)
#define	MAX_QUEUE_SIZE   1
#define	STACK_SIZE_BYTES   1200
#define	TASK_PRIORITY   PIOS_THREAD_PRIO_HIGHEST
#define	FAILSAFE_TIMEOUT_MS   30
#define	COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD   3.0f
#define	COORDINATED_FLIGHT_MAX_YAW_THRESHOLD   0.05f
#define	HORIZON_MODE_MAX_BLEND   0.85f
	Set the stick position that maximally transitions to rate. More...
#define	THROTTLE_EPSILON   0.000001f
	Minimum sane positive value for throttle. More...
#define	get_deadband(axis)   (deadbands ? (deadbands + axis) : NULL)

Enumerations
enum	{ ROLL, PITCH, YAW, MAX_AXES }
enum	mag_calibration_algo { MAG_CALIBRATION_PRELEMARI, MAG_CALIBRATION_NORMALIZE_LENGTH }
enum	{   PID_GROUP_RATE, PID_RATE_ROLL = PID_GROUP_RATE, PID_RATE_PITCH, PID_RATE_YAW,   PID_GROUP_ATT, PID_ATT_ROLL = PID_GROUP_ATT, PID_ATT_PITCH, PID_ATT_YAW,   PID_GROUP_VBAR, PID_VBAR_ROLL = PID_GROUP_VBAR, PID_VBAR_PITCH, PID_VBAR_YAW,   PID_COORDINATED_FLIGHT_YAW, PID_MAX }

Functions
int32_t	StabilizationInitialize ()
int	stabilization_virtual_flybar (float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, struct pid *pid, VbarSettingsData *settings)
int	stabilization_virtual_flybar_pirocomp (float z_gyro, float dT)
static void	update_accels (struct pios_sensor_accel_data *accels)
	Apply calibration and rotation to the raw accel data. More...
static void	update_gyros (struct pios_sensor_gyro_data *gyros)
	Apply calibration and rotation to the raw gyro data. More...
static void	update_mags (struct pios_sensor_mag_data *mag)
	Apply calibration and rotation to the raw mag data. More...
static void	update_baro (struct pios_sensor_baro_data *baro)
static void	mag_calibration_prelemari (MagnetometerData *mag)
static void	mag_calibration_fix_length (MagnetometerData *mag)
static void	updateTemperatureComp (float temperature, float *temp_bias)
static void	sensors_settings_update ()
int32_t	sensors_init (void)
bool	sensors_step ()
	DONT_BUILD_IF ((MAX_AXES+0!=3), stabAxisWrongCount)
	DONT_BUILD_IF ((PID_RATE_ROLL+0!=ROLL)||(PID_ATT_ROLL!=PID_GROUP_ATT+ROLL), stabAxisPidEnumMapping1)
	DONT_BUILD_IF ((PID_RATE_PITCH+0!=PITCH)||(PID_ATT_PITCH!=PID_GROUP_ATT+PITCH), stabAxisPidEnumMapping2)
	DONT_BUILD_IF ((PID_RATE_YAW+0!=YAW)||(PID_ATT_YAW!=PID_GROUP_ATT+YAW), stabAxisPidEnumMapping3)
	DONT_BUILD_IF (STABILIZATIONSETTINGS_MAXLEVELANGLE_ROLL!=ROLL+0, LevelAngleConstantRoll)
	DONT_BUILD_IF (STABILIZATIONSETTINGS_MAXLEVELANGLE_PITCH!=PITCH+0, LevelAngleConstantPitch)
	DONT_BUILD_IF (STABILIZATIONSETTINGS_MAXLEVELANGLE_YAW!=YAW+0, LevelAngleConstantYaw)
static void	stabilizationTask (void *parameters)
static void	zero_pids (void)
static void	calculate_pids (float dT)
static void	update_settings (float dT)
static float	get_throttle (ActuatorDesiredData *actuator_desired, SystemSettingsAirframeTypeOptions *airframe_type)
int32_t	StabilizationStart ()
bool	stabilization_failsafe_checks (StabilizationDesiredData *stab_desired, ActuatorDesiredData *actuator_desired, SystemSettingsAirframeTypeOptions airframe_type, float *input, uint8_t *mode)
static void	calculate_attitude_errors (uint8_t *axis_mode, float *raw_input, AttitudeActualData *attitudeActual, float *local_attitude_error, float *horizon_rate_fraction)
	MODULE_HIPRI_INITCALL (StabilizationInitialize, StabilizationStart)
static float	calculate_thrust (StabilizationDesiredThrustModeOptions mode, FlightStatusData *flightStatus, SystemSettingsAirframeTypeOptions airframe_type, float desired_thrust)
static uint8_t	remap_smoothing_mode (uint8_t m)
static void	calculate_vert_pids (float dT)
static float	bound (float val, float range)
	Private methods. More...

Variables
static INSSettingsData	insSettings
static AccelsData	accelsData
static volatile bool	settings_updated = true
static bool	bias_correct_gyro = true
static float	mag_bias [3] = {0,0,0}
static float	mag_scale [3] = {0,0,0}
static float	accel_bias [3] = {0,0,0}
static float	accel_scale [3] = {0,0,0}
static float	gyro_scale [3] = {0,0,0}
static float	gyro_coeff_x [4] = {0,0,0,0}
static float	gyro_coeff_y [4] = {0,0,0,0}
static float	gyro_coeff_z [4] = {0,0,0,0}
static float	gyro_temp_bias [3] = {0,0,0}
static float	z_accel_offset = 0
static float	Rsb [3][3] = {{0}}
static int8_t	rotate = 0
	Rotation matrix that transforms from the body frame to the sensor board frame. More...
static enum mag_calibration_algo	mag_calibration_algo = MAG_CALIBRATION_PRELEMARI
	Select the algorithm to try and null out the magnetometer bias error. More...
static lpfilter_state_t	gyro_filter
static lpfilter_state_t	accel_filter
static struct pios_thread *	taskHandle
static StabilizationSettingsData	settings
static VbarSettingsData	vbar_settings
static SubTrimData	subTrim
uint16_t	ident_wiggle_points
static float	axis_lock_accum [MAX_AXES] = {0,0,0}
static uint8_t	max_axis_lock = 0
static uint8_t	max_axislock_rate = 0
static float	weak_leveling_kp = 0
static uint8_t	weak_leveling_max = 0
static bool	lowThrottleZeroIntegral
static float	max_rate_alpha = 0.8f
float	vbar_decay = 0.991f
static struct pid	pids [PID_MAX]
static smoothcontrol_state	rc_smoothing
struct pid_deadband *	deadbands = NULL
static float	vbar_integral [MAX_AXES]
	Private variables. More...
float	vbar_decay

Detailed Description

Header for sensor functions.

Macro Definition Documentation

#define COORDINATED_FLIGHT_MAX_YAW_THRESHOLD   0.05f

Definition at line 101 of file stabilization.c.

#define COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD   3.0f

Definition at line 100 of file stabilization.c.

#define FAILSAFE_TIMEOUT_MS   30

Definition at line 99 of file stabilization.c.

#define get_deadband

(

axis

)

(deadbands ? (deadbands + axis) : NULL)

Definition at line 188 of file stabilization.c.

#define HORIZON_MODE_MAX_BLEND   0.85f

Set the stick position that maximally transitions to rate.

Definition at line 106 of file stabilization.c.

#define MAX_QUEUE_SIZE   1

Definition at line 90 of file stabilization.c.

#define MAX_SENSOR_PERIOD   6

Definition at line 64 of file sensors.c.

#define MAX_TIME_BETWEEN_VALID_BARO_DATAS_US   (100*1000)

Definition at line 66 of file sensors.c.

#define MAX_TIME_BETWEEN_VALID_MAG_DATAS_US   (300*1000)

Definition at line 67 of file sensors.c.

#define REQUIRED_GOOD_CYCLES   50

Definition at line 65 of file sensors.c.

#define STACK_SIZE_BYTES   1200

Definition at line 95 of file stabilization.c.

#define TASK_PRIORITY   PIOS_THREAD_PRIO_HIGHEST

Definition at line 98 of file stabilization.c.

#define THROTTLE_EPSILON   0.000001f

Minimum sane positive value for throttle.

Definition at line 109 of file stabilization.c.

Enumeration Type Documentation

anonymous enum

Enumerator
ROLL
PITCH
YAW
MAX_AXES

Definition at line 35 of file stabilization.h.

anonymous enum

Enumerator
PID_GROUP_RATE
PID_RATE_ROLL
PID_RATE_PITCH
PID_RATE_YAW
PID_GROUP_ATT
PID_ATT_ROLL
PID_ATT_PITCH
PID_ATT_YAW
PID_GROUP_VBAR
PID_VBAR_ROLL
PID_VBAR_PITCH
PID_VBAR_YAW
PID_COORDINATED_FLIGHT_YAW
PID_MAX

Definition at line 111 of file stabilization.c.

enum mag_calibration_algo

Enumerator
MAG_CALIBRATION_PRELEMARI
MAG_CALIBRATION_NORMALIZE_LENGTH

Definition at line 70 of file sensors.c.

Function Documentation

static float bound ( float  val, float  range  )

static

Private methods.

Bound input value between limits

Definition at line 96 of file virtualflybar.c.

static void calculate_attitude_errors ( uint8_t *  axis_mode, float *  raw_input, AttitudeActualData *  attitudeActual, float *  local_attitude_error, float *  horizon_rate_fraction  )

static

Definition at line 331 of file stabilization.c.

static void calculate_pids ( float  dT )

static

Definition at line 1579 of file stabilization.c.

static float calculate_thrust ( StabilizationDesiredThrustModeOptions  mode, FlightStatusData *  flightStatus, SystemSettingsAirframeTypeOptions  airframe_type, float  desired_thrust  )

static

Definition at line 503 of file stabilization.c.

static void calculate_vert_pids ( float  dT )

static

Definition at line 1679 of file stabilization.c.

DONT_BUILD_IF	(	(MAX_AXES+0!=3)	,
		stabAxisWrongCount
	)

DONT_BUILD_IF	(	(PID_RATE_ROLL+0!=ROLL)||(PID_ATT_ROLL!=PID_GROUP_ATT+ROLL)	,
		stabAxisPidEnumMapping1
	)

DONT_BUILD_IF	(	(PID_RATE_PITCH+0!=PITCH)||(PID_ATT_PITCH!=PID_GROUP_ATT+PITCH)	,
		stabAxisPidEnumMapping2
	)

DONT_BUILD_IF	(	(PID_RATE_YAW+0!=YAW)||(PID_ATT_YAW!=PID_GROUP_ATT+YAW)	,
		stabAxisPidEnumMapping3
	)

DONT_BUILD_IF	(	STABILIZATIONSETTINGS_MAXLEVELANGLE_ROLL!	= ROLL+0,
		LevelAngleConstantRoll
	)

DONT_BUILD_IF	(	STABILIZATIONSETTINGS_MAXLEVELANGLE_PITCH!	= PITCH+0,
		LevelAngleConstantPitch
	)

DONT_BUILD_IF	(	STABILIZATIONSETTINGS_MAXLEVELANGLE_YAW!	= YAW+0,
		LevelAngleConstantYaw
	)

static float get_throttle ( ActuatorDesiredData *  actuator_desired, SystemSettingsAirframeTypeOptions *  airframe_type  )

static

Definition at line 193 of file stabilization.c.

static void mag_calibration_fix_length ( MagnetometerData *  mag )

static

Perform an update of the MagBias based on an algorithm we developed that tries to drive the magnetometer length to the expected value. This algorithm seems to work better when not turning a lot.

Definition at line 625 of file sensors.c.

static void mag_calibration_prelemari ( MagnetometerData *  mag )

static

Perform an update of the MagBias based on Magnetometer Offset Cancellation: Theory and Implementation, revisited William Premerlani, October 14, 2011

Definition at line 577 of file sensors.c.

MODULE_HIPRI_INITCALL	(	StabilizationInitialize	,
		StabilizationStart
	)

static uint8_t remap_smoothing_mode ( uint8_t  m )

static

Definition at line 1563 of file stabilization.c.

int32_t sensors_init

(

void

)

API for sensor fusion algorithms: Configure(struct pios_queue *gyro, struct pios_queue *accel, struct pios_queue *mag, struct pios_queue *baro) Stores all the queues the algorithm will pull data from FinalizeSensors() – before saving the sensors modifies them based on internal state (gyro bias) Update() – queries queues and updates the attitude estiamte Initialise the module. Called before the start function

Returns

0 on success or -1 if initialisation failed

Definition at line 143 of file sensors.c.

static void sensors_settings_update ( )

static

Locally cache some variables from the AtttitudeSettings object

Definition at line 681 of file sensors.c.

bool sensors_step

(

)

This polls the gyros and pumps that data to other users.

Definition at line 213 of file sensors.c.

bool stabilization_failsafe_checks	(	StabilizationDesiredData *	stab_desired,
		ActuatorDesiredData *	actuator_desired,
		SystemSettingsAirframeTypeOptions	airframe_type,
		float *	input,
		uint8_t *	mode
	)

Definition at line 271 of file stabilization.c.

int stabilization_virtual_flybar	(	float	gyro,
		float	command,
		float *	output,
		float	dT,
		bool	reinit,
		uint32_t	axis,
		struct pid *	pid,
		VbarSettingsData *	settings
	)

Definition at line 44 of file virtualflybar.c.

int stabilization_virtual_flybar_pirocomp	(	float	z_gyro,
		float	dT
	)

Want to keep the virtual flybar fixed in world coordinates as we pirouette

Parameters

[in]	z_gyro	The deg/s of rotation along the z axis
[in]	dT	The time since last sample

Definition at line 79 of file virtualflybar.c.

int32_t StabilizationInitialize

(

)

Module initialization

Definition at line 230 of file stabilization.c.

int32_t StabilizationStart

(

)

Module initialization

Definition at line 215 of file stabilization.c.

static void stabilizationTask ( void *  parameters )

static

Module task

Definition at line 714 of file stabilization.c.

static void update_accels ( struct pios_sensor_accel_data *  accels )

static

Apply calibration and rotation to the raw accel data.

Parameters

[in]

accels

The raw accel data

Definition at line 338 of file sensors.c.

static void update_baro ( struct pios_sensor_baro_data *  baro )

static

Update the baro uavo from the data from the baro queue

Parameters

[in]

baro

raw baro data

Definition at line 475 of file sensors.c.

static void update_gyros ( struct pios_sensor_gyro_data *  gyros )

static

Apply calibration and rotation to the raw gyro data.

Parameters

[in]

gyros

The raw gyro data

Definition at line 371 of file sensors.c.

static void update_mags ( struct pios_sensor_mag_data *  mag )

static

Apply calibration and rotation to the raw mag data.

Parameters

[in]

mag

The raw mag data

Definition at line 432 of file sensors.c.

static void update_settings ( float  dT )

static

Definition at line 1690 of file stabilization.c.

static void updateTemperatureComp ( float  temperature, float *  temp_bias  )

static

Compute the bias expected from temperature variation for each gyro channel

Definition at line 542 of file sensors.c.

static void zero_pids ( void  )

static

Clear the accumulators and derivatives for all the axes

Definition at line 1553 of file stabilization.c.

Variable Documentation

float accel_bias[3] = {0,0,0}

static

Definition at line 114 of file sensors.c.

lpfilter_state_t accel_filter

static

Definition at line 129 of file sensors.c.

float accel_scale[3] = {0,0,0}

static

Definition at line 115 of file sensors.c.

AccelsData accelsData

static

Definition at line 101 of file sensors.c.

float axis_lock_accum[MAX_AXES] = {0,0,0}

static

Definition at line 154 of file stabilization.c.

bool bias_correct_gyro = true

static

Definition at line 106 of file sensors.c.

struct pid_deadband* deadbands = NULL

Definition at line 178 of file stabilization.c.

float gyro_coeff_x[4] = {0,0,0,0}

static

Definition at line 117 of file sensors.c.

float gyro_coeff_y[4] = {0,0,0,0}

static

Definition at line 118 of file sensors.c.

float gyro_coeff_z[4] = {0,0,0,0}

static

Definition at line 119 of file sensors.c.

lpfilter_state_t gyro_filter

static

Definition at line 128 of file sensors.c.

float gyro_scale[3] = {0,0,0}

static

Definition at line 116 of file sensors.c.

float gyro_temp_bias[3] = {0,0,0}

static

Definition at line 120 of file sensors.c.

uint16_t ident_wiggle_points

Definition at line 152 of file stabilization.c.

INSSettingsData insSettings

static

Definition at line 100 of file sensors.c.

bool lowThrottleZeroIntegral

static

Definition at line 159 of file stabilization.c.

float mag_bias[3] = {0,0,0}

static

Definition at line 112 of file sensors.c.

enum mag_calibration_algo mag_calibration_algo = MAG_CALIBRATION_PRELEMARI

static

Select the algorithm to try and null out the magnetometer bias error.

Definition at line 126 of file sensors.c.

float mag_scale[3] = {0,0,0}

static

Definition at line 113 of file sensors.c.

uint8_t max_axis_lock = 0

static

Definition at line 155 of file stabilization.c.

uint8_t max_axislock_rate = 0

static

Definition at line 156 of file stabilization.c.

float max_rate_alpha = 0.8f

static

Definition at line 160 of file stabilization.c.

struct pid pids[PID_MAX]

static

Definition at line 171 of file stabilization.c.

smoothcontrol_state rc_smoothing

static

Definition at line 172 of file stabilization.c.

int8_t rotate = 0

static

Rotation matrix that transforms from the body frame to the sensor board frame.

Definition at line 123 of file sensors.c.

float Rsb[3][3] = {{0}}

static

Definition at line 122 of file sensors.c.

StabilizationSettingsData settings

static

Definition at line 147 of file stabilization.c.

volatile bool settings_updated = true

static

Definition at line 103 of file sensors.c.

SubTrimData subTrim

static

Definition at line 150 of file stabilization.c.

struct pios_thread* taskHandle

static

Definition at line 141 of file stabilization.c.

float vbar_decay

Definition at line 161 of file stabilization.c.

float vbar_decay = 0.991f

Definition at line 161 of file stabilization.c.

float vbar_integral[MAX_AXES]

static

Private variables.

Definition at line 38 of file virtualflybar.c.

VbarSettingsData vbar_settings

static

Definition at line 148 of file stabilization.c.

float weak_leveling_kp = 0

static

Definition at line 157 of file stabilization.c.

uint8_t weak_leveling_max = 0

static

Definition at line 158 of file stabilization.c.

float z_accel_offset = 0

static

Definition at line 121 of file sensors.c.