attitude.c File Reference

Full attitude estimation algorithm. More...

#include "openpilot.h"
#include "pios.h"
#include "pios_thread.h"
#include "pios_queue.h"
#include "misc_math.h"
#include "physical_constants.h"
#include "coordinate_conversions.h"
#include "WorldMagModel.h"
#include "insgps.h"
#include "accels.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "baroaltitude.h"
#include "flightstatus.h"
#include "gpsposition.h"
#include "gpstime.h"
#include "gpsvelocity.h"
#include "gyros.h"
#include "gyrosbias.h"
#include "homelocation.h"
#include "sensorsettings.h"
#include "inssettings.h"
#include "insstate.h"
#include "magnetometer.h"
#include "nedaccel.h"
#include "nedposition.h"
#include "positionactual.h"
#include "stateestimation.h"
#include "systemalarms.h"
#include "velocityactual.h"

Go to the source code of this file.

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

Author

The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.

Tau Labs, http://taulabs.org Copyright (C) 2012-2016

dRonin, http://dronin.org Copyright (C) 2015-2017

See Also

The GNU Public License (GPL) Version 3

Definition in file attitude.c.