ahrs_float_dcm.h File Reference

Attitude estimation for fixedwings based on the DCM. More...

#include <inttypes.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
#include "subsystems/gps.h"

Include dependency graph for ahrs_float_dcm.h:

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

Go to the source code of this file.

Data Structures
struct	AhrsFloatDCM

Macros
#define	Kp_ROLLPITCH   0.015
#define	Ki_ROLLPITCH   0.000010
#define	Kp_YAW   0.9
#define	Ki_YAW   0.00005
#define	GRAVITY   9.81
#define	OUTPUTMODE   1
#define	PERFORMANCE_REPORTING   1

Enumerations
enum	AhrsDCMStatus { AHRS_DCM_UNINIT, AHRS_DCM_RUNNING }

Functions
void	ahrs_dcm_init (void)
void	ahrs_dcm_set_body_to_imu (struct OrientationReps *body_to_imu)
void	ahrs_dcm_set_body_to_imu_quat (struct FloatQuat *q_b2i)
bool_t	ahrs_dcm_align (struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void	ahrs_dcm_propagate (struct Int32Rates *gyro, float dt)
void	ahrs_dcm_update_accel (struct Int32Vect3 *accel)
void	ahrs_dcm_update_mag (struct Int32Vect3 *mag)
void	ahrs_dcm_update_gps (struct GpsState *gps_s)

Variables
struct AhrsFloatDCM	ahrs_dcm
int	renorm_sqrt_count
int	renorm_blowup_count
float	imu_health

Detailed Description

Attitude estimation for fixedwings based on the DCM.

Theory: http://code.google.com/p/gentlenav/downloads/list file DCMDraft2.pdf

Definition in file ahrs_float_dcm.h.

---

Data Structure Documentation

struct AhrsFloatDCM

Definition at line 37 of file ahrs_float_dcm.h.

Collaboration diagram for AhrsFloatDCM:

Data Fields
struct OrientationReps	body_to_imu
float	gps_acceleration
uint8_t	gps_age
float	gps_course
bool_t	gps_course_valid
float	gps_speed
struct FloatRates	gyro_bias
struct FloatRates	imu_rate
bool_t	is_aligned
struct FloatEulers	ltp_to_imu_euler
struct FloatRates	rate_correction
enum AhrsDCMStatus	status

Macro Definition Documentation

#define GRAVITY   9.81

Definition at line 65 of file ahrs_float_dcm.h.

Referenced by compute_ahrs_representations(), and Drift_correction().

#define Ki_ROLLPITCH   0.000010

Definition at line 61 of file ahrs_float_dcm.h.

Referenced by Drift_correction().

#define Ki_YAW   0.00005

Definition at line 63 of file ahrs_float_dcm.h.

Referenced by Drift_correction().

#define Kp_ROLLPITCH   0.015

Definition at line 60 of file ahrs_float_dcm.h.

Referenced by Drift_correction().

#define Kp_YAW   0.9

Definition at line 62 of file ahrs_float_dcm.h.

Referenced by Drift_correction().

#define OUTPUTMODE   1

Definition at line 69 of file ahrs_float_dcm.h.

#define PERFORMANCE_REPORTING   1

Definition at line 76 of file ahrs_float_dcm.h.

Enumeration Type Documentation

enum AhrsDCMStatus

Enumerator
AHRS_DCM_UNINIT
AHRS_DCM_RUNNING

Definition at line 32 of file ahrs_float_dcm.h.

Function Documentation

bool_t ahrs_dcm_align	(	struct Int32Rates *	lp_gyro,
		struct Int32Vect3 *	lp_accel,
		struct Int32Vect3 *	lp_mag
	)

Definition at line 119 of file ahrs_float_dcm.c.

References ahrs_dcm, AHRS_DCM_RUNNING, ahrs_float_get_euler_from_accel_mag(), float_rmat_of_eulers, AhrsFloatDCM::gyro_bias, AhrsFloatDCM::is_aligned, AhrsFloatDCM::ltp_to_imu_euler, RATES_COPY, RATES_FLOAT_OF_BFP, set_dcm_matrix_from_rmat(), AhrsFloatDCM::status, and TRUE.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_dcm_init

(

void

)

Definition at line 99 of file ahrs_float_dcm.c.

References ahrs_dcm, AHRS_DCM_UNINIT, AhrsFloatDCM::body_to_imu, FALSE, FLOAT_EULERS_ZERO, FLOAT_RATES_ZERO, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, AhrsFloatDCM::gps_speed, AhrsFloatDCM::imu_rate, AhrsFloatDCM::is_aligned, AhrsFloatDCM::ltp_to_imu_euler, orientationGetRMat_f(), set_dcm_matrix_from_rmat(), and AhrsFloatDCM::status.

Referenced by ahrs_dcm_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_dcm_propagate	(	struct Int32Rates *	gyro,
		float	dt
	)

Definition at line 144 of file ahrs_float_dcm.c.

References ahrs_dcm, compute_ahrs_representations(), AhrsFloatDCM::gyro_bias, AhrsFloatDCM::imu_rate, Matrix_update(), Normalize(), FloatRates::p, FloatRates::q, FloatRates::r, RATES_DIFF, and RATES_FLOAT_OF_BFP.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_dcm_set_body_to_imu

(

struct OrientationReps *

body_to_imu

)

Definition at line 522 of file ahrs_float_dcm.c.

References ahrs_dcm_set_body_to_imu_quat(), and orientationGetQuat_f().

Here is the call graph for this function:

void ahrs_dcm_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 527 of file ahrs_float_dcm.c.

References ahrs_dcm, AhrsFloatDCM::body_to_imu, AhrsFloatDCM::is_aligned, AhrsFloatDCM::ltp_to_imu_euler, orientationGetEulers_f(), and orientationSetQuat_f().

Referenced by ahrs_dcm_set_body_to_imu(), and body_to_imu_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_dcm_update_accel

(

struct Int32Vect3 *

accel

)

Definition at line 200 of file ahrs_float_dcm.c.

References ACCELS_FLOAT_OF_BFP, ahrs_dcm, Drift_correction(), AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_speed, Omega, PRINT_CONFIG_MSG(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by accel_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ahrs_dcm_update_gps

(

struct GpsState *

gps_s

)

Definition at line 175 of file ahrs_float_dcm.c.

References ahrs_dcm, GpsState::course, FALSE, GpsState::fix, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, GPS_FIX_3D, AhrsFloatDCM::gps_speed, GpsState::gspeed, GpsState::speed_3d, and TRUE.

Referenced by gps_cb().

Here is the caller graph for this function:

void ahrs_dcm_update_mag

(

struct Int32Vect3 *

mag

)

Definition at line 229 of file ahrs_float_dcm.c.

References ahrs_dcm, DefaultChannel, DefaultDevice, AhrsFloatDCM::ltp_to_imu_euler, MAG_FLOAT_OF_BFP, MAG_Heading_X, MAG_Heading_Y, FloatEulers::phi, FloatEulers::theta, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by mag_cb().

Here is the caller graph for this function:

Variable Documentation

struct AhrsFloatDCM ahrs_dcm

Definition at line 52 of file ahrs_float_dcm.c.

Referenced by accel_cb(), ahrs_dcm_align(), ahrs_dcm_init(), ahrs_dcm_propagate(), ahrs_dcm_set_body_to_imu_quat(), ahrs_dcm_update_accel(), ahrs_dcm_update_gps(), ahrs_dcm_update_mag(), aligner_cb(), compute_ahrs_representations(), Drift_correction(), gyro_cb(), mag_cb(), Matrix_update(), Normalize(), and set_body_orientation_and_rates().

float imu_health

Referenced by Drift_correction().

int renorm_blowup_count

Referenced by Normalize().

int renorm_sqrt_count

Referenced by Normalize().