Full attitude estimation method with selectable algorithms. More...

Files
file	attitude.c
	Full attitude estimation algorithm.

Data Structures
struct	complementary_filter_state

struct	cfvert
	Struture that tracks the data for the vertical complementary filter. More...

Macros
#define	STACK_SIZE_BYTES 2504

#define	TASK_PRIORITY PIOS_THREAD_PRIO_HIGH

#define	FAILSAFE_TIMEOUT_MS 10

Enumerations
enum	complementary_filter_status { CF_POWERON, CF_INITIALIZING, CF_ARMING, CF_NORMAL }

Functions
static void	AttitudeTask (void *parameters)

static int32_t	setNavigationRaw ()
	Set the navigation information to the raw estimates. More...

static int32_t	setNavigationNone ()
	Provide no navigation updates (indoor flying or without gps) More...

static int32_t	updateAttitudeComplementary (float dT, bool first_run, bool secondary, bool raw_gps, bool vel_compass)
	Update the complementary filter attitude estimate. More...

static int32_t	setAttitudeComplementary ()
	Set the AttitudeActual to the complementary filter estimate. More...

static float	calc_ned_accel (float q, float accels)

static void	cfvert_reset (struct cfvert *cf, float baro, float time_constant)
	Resets the vertical baro complementary filter and zeros the altitude. More...

static void	cfvert_predict_pos (struct cfvert *cf, float z_accel, float dt)
	Predict the position in the future. More...

static void	cfvert_update_baro (struct cfvert *cf, float baro, float dt)
	Update the baro feedback. More...

static int32_t	updateAttitudeINSGPS (bool first_run, bool outdoor_mode)
	Update the INSGPS attitude estimate. More...

static int32_t	setAttitudeINSGPS ()
	Set the attitude to the current INSGPS estimate. More...

static int32_t	setNavigationINSGPS ()
	Set the navigation to the current INSGPS estimate. More...

static void	updateNedAccel ()

static void	apply_accel_filter (const float raw, float filtered)
	A low pass filter on the accels which helps with vibration resistance. More...

static int32_t	getNED (GPSPositionData gpsPosition, float NED)
	Convert the GPS LLA position into NED coordinates. More...

static void	accumulate_gyro_compute ()
	Compute the mean gyro accumulated and assign the bias. More...

static void	accumulate_gyro_zero ()
	Zero the gyro accumulators. More...

static void	accumulate_gyro (GyrosData *gyrosData)
	Store a gyro sample. More...

static void	set_state_estimation_error (SystemAlarmsStateEstimationOptions error_code)
	Set alarm and alarm code. More...

static void	check_home_location ()
	Determine if it is safe to set the home location then do it. More...

int32_t	AttitudeInitialize (void)

int32_t	AttitudeStart (void)

Variables
static struct pios_thread *	attitudeTaskHandle

static struct pios_queue *	gyroQueue

static struct pios_queue *	accelQueue

static struct pios_queue *	magQueue

static struct pios_queue *	baroQueue

static struct pios_queue *	gpsQueue

static struct pios_queue *	gpsVelQueue

static AttitudeSettingsData	attitudeSettings

static HomeLocationData	homeLocation

static INSSettingsData	insSettings

static StateEstimationData	stateEstimation

static bool	gyroBiasSettingsUpdated = false

static volatile bool	settings_flag = true

static volatile bool	homeloc_flag = true

static volatile bool	sensors_flag = true

static bool	home_location_updated

static const float	zeros [3] = {0.0f, 0.0f, 0.0f}

static struct complementary_filter_state	complementary_filter_state

static struct cfvert	cfvert
	State information for vertical filter. More...

static float	dT_expected = 0.001f

static float	T [3]
	Scales used in NED transform (local tangent plane approx). More...

static float	cf_q [4]
	The complementary filter attitude estimate. More...

Detailed Description

Full attitude estimation method with selectable algorithms.

Based on the value of StateEstimation this module will select between the complementary filter and the INSGPS to fuse data from Gyros, Accels, Magnetometer, GPSPosition, GPSVelocity, and BaroAltitude to estimation PositionActual, VelocityActual and AttitudeActual.

Macro Definition Documentation

#define FAILSAFE_TIMEOUT_MS 10

Definition at line 85 of file attitude.c.

#define STACK_SIZE_BYTES 2504

Input objects: None, takes sensor data via pios Output objects: AttitudeActual, PositionActual, VelocityActual

This module performs a state estimate from the sensor data using either the INSGPS algorithm for attitude, velocity, and position, or the complementary filter algorithm for just attitude. In complementary mode it also runs a simple filter to smooth the altitude.

Definition at line 83 of file attitude.c.

#define TASK_PRIORITY PIOS_THREAD_PRIO_HIGH

Definition at line 84 of file attitude.c.

Enumeration Type Documentation

enum complementary_filter_status

Enumerator
CF_POWERON
CF_INITIALIZING
CF_ARMING
CF_NORMAL

Definition at line 90 of file attitude.c.

Function Documentation

static void accumulate_gyro ( GyrosData * gyrosData )

static

Store a gyro sample.

Accumulate a set of gyro samples for computing the bias

Parameters

[in] gyrosData The samples of data to accumulate

Definition at line 1037 of file attitude.c.

static void accumulate_gyro_compute ( )

static

Compute the mean gyro accumulated and assign the bias.

If accumulating data and enough samples acquired then recompute the gyro bias based on the mean accumulated

Definition at line 1003 of file attitude.c.

static void accumulate_gyro_zero ( )

static

Zero the gyro accumulators.

Zero the accumulation of gyro data

Definition at line 1024 of file attitude.c.

static void apply_accel_filter	(	const float *	raw,
		float *	filtered
	)

static

A low pass filter on the accels which helps with vibration resistance.

Definition at line 1440 of file attitude.c.

int32_t AttitudeInitialize ( 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 225 of file attitude.c.

int32_t AttitudeStart ( void )

Start the task. Expects all objects to be initialized by this point.

Returns: 0 on success or -1 if initialisation failed

Definition at line 260 of file attitude.c.

static void AttitudeTask ( void * parameters )

static

Module thread, should not return.

Definition at line 313 of file attitude.c.

static float calc_ned_accel	(	float *	q,
		float *	accels
	)

static

Calculate the acceleration in the NED frame. This is used by the altitude controller. Returns the down component for convenience.

Definition at line 859 of file attitude.c.

static void cfvert_predict_pos	(	struct cfvert *	cf,
		float	z_accel,
		float	dt
	)

static

Predict the position in the future.

Definition at line 893 of file attitude.c.

static void cfvert_reset	(	struct cfvert *	cf,
		float	baro,
		float	time_constant
	)

static

Resets the vertical baro complementary filter and zeros the altitude.

Definition at line 880 of file attitude.c.

static void cfvert_update_baro	(	struct cfvert *	cf,
		float	baro,
		float	dt
	)

static

Update the baro feedback.

Definition at line 911 of file attitude.c.

static void check_home_location ( )

static

Determine if it is safe to set the home location then do it.

Check if it is safe to update the home location and do it

Definition at line 1522 of file attitude.c.

static int32_t getNED	(	GPSPositionData *	gpsPosition,
		float *	NED
	)

static

Convert the GPS LLA position into NED coordinates.

Note: this method uses a taylor expansion around the home coordinates to convert to NED which allows it to be done with all floating calculations

Parameters

[in]	Current	lat-lon coordinates on WGS84 ellipsoid, altitude referenced to MSL geoid (likely EGM 1996, but no guarantees)
[out]	NED	frame coordinates

Returns: 0 for success, -1 for failure

Definition at line 1463 of file attitude.c.

static void set_state_estimation_error ( SystemAlarmsStateEstimationOptions error_code )

static

Set alarm and alarm code.

Set the error code and alarm state

Parameters

[in] error code

Definition at line 1571 of file attitude.c.

static int32_t setAttitudeComplementary ( )

static

Set the AttitudeActual to the complementary filter estimate.

Set the AttitudeActual UAVO to the complementary filter estimate

Definition at line 989 of file attitude.c.

static int32_t setAttitudeINSGPS ( )

static

Set the attitude to the current INSGPS estimate.

Definition at line 1402 of file attitude.c.

static int32_t setNavigationINSGPS ( )

static

Set the navigation to the current INSGPS estimate.

Definition at line 1427 of file attitude.c.

static int32_t setNavigationNone ( )

static

Provide no navigation updates (indoor flying or without gps)

Set the navigation information to the raw estimates.

Definition at line 977 of file attitude.c.

static int32_t setNavigationRaw ( )

static

Set the navigation information to the raw estimates.

Definition at line 922 of file attitude.c.

static int32_t updateAttitudeComplementary	(	float	dT,
		bool	first_run,
		bool	secondary,
		bool	raw_gps,
		bool	vel_compass
	)

static

Update the complementary filter attitude estimate.

Update the complementary filter estimate of attitude

Parameters

[in]	first_run	indicates the filter was just selected
[in]	secondary	indicates the EKF is running as well

Definition at line 493 of file attitude.c.

static int32_t updateAttitudeINSGPS	(	bool	first_run,
		bool	outdoor_mode
	)

static

Update the INSGPS attitude estimate.

Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) [in] first_run This is the first run so trigger reinitialization [in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)

Returns: 0 for success, -1 for failure

Definition at line 1066 of file attitude.c.

static void updateNedAccel ( )

static

Keep a running filtered version of the acceleration in the NED frame

Todo:: this is redundant with another set when in INS modes...

Definition at line 1479 of file attitude.c.

Variable Documentation

struct pios_queue* accelQueue

static

Definition at line 140 of file attitude.c.

AttitudeSettingsData attitudeSettings

static

Definition at line 146 of file attitude.c.

struct pios_thread* attitudeTaskHandle

static

Definition at line 137 of file attitude.c.

struct pios_queue* baroQueue

static

Definition at line 142 of file attitude.c.

float cf_q[4]

static

The complementary filter attitude estimate.

Definition at line 486 of file attitude.c.

struct cfvert cfvert

static

State information for vertical filter.

Definition at line 159 of file attitude.c.

struct complementary_filter_state complementary_filter_state

static

Definition at line 158 of file attitude.c.

float dT_expected = 0.001f

static

Definition at line 161 of file attitude.c.

struct pios_queue* gpsQueue

static

Definition at line 143 of file attitude.c.

struct pios_queue* gpsVelQueue

static

Definition at line 144 of file attitude.c.

bool gyroBiasSettingsUpdated = false

static

Definition at line 150 of file attitude.c.

struct pios_queue* gyroQueue

static

Definition at line 139 of file attitude.c.

bool home_location_updated

static

Definition at line 154 of file attitude.c.

volatile bool homeloc_flag = true

static

Definition at line 152 of file attitude.c.

HomeLocationData homeLocation

static

Definition at line 147 of file attitude.c.

INSSettingsData insSettings

static

Definition at line 148 of file attitude.c.

struct pios_queue* magQueue

static

Definition at line 141 of file attitude.c.

volatile bool sensors_flag = true

static

Definition at line 153 of file attitude.c.

volatile bool settings_flag = true

static

Definition at line 151 of file attitude.c.

StateEstimationData stateEstimation

static

Definition at line 149 of file attitude.c.

float T[3]

static

Scales used in NED transform (local tangent plane approx).

Definition at line 210 of file attitude.c.

const float zeros[3] = {0.0f, 0.0f, 0.0f}

static

Definition at line 156 of file attitude.c.

Files

Data Structures

Macros

Enumerations

Functions

Variables

Detailed Description

Macro Definition Documentation

Enumeration Type Documentation

Function Documentation

Variable Documentation