ahrs_float_dcm.c File Reference

#include "std.h"
#include "subsystems/ahrs/ahrs_float_dcm.h"
#include "subsystems/ahrs/ahrs_float_utils.h"
#include "autopilot.h"
#include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
#include "math/pprz_algebra_float.h"
#include "subsystems/gps.h"
#include <string.h>
#include "led.h"

Include dependency graph for ahrs_float_dcm.c:

Go to the source code of this file.

Macros
#define	ACCEL_WEIGHT_FILTER   8
#define	ACCEL_WEIGHT_BAND   1.f

Functions
static void	compute_ahrs_representations (void)
static void	set_dcm_matrix_from_rmat (struct FloatRMat *rmat)
void	Normalize (void)
void	Drift_correction (void)
void	Matrix_update (float dt)
void	ahrs_dcm_init (void)
bool	ahrs_dcm_align (struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void	ahrs_dcm_propagate (struct FloatRates *gyro, float dt)
void	ahrs_dcm_update_gps (struct GpsState *gps_s UNUSED)
void	ahrs_dcm_update_accel (struct FloatVect3 *accel)
void	ahrs_dcm_update_mag (struct FloatVect3 *mag)
void	ahrs_dcm_set_body_to_imu (struct OrientationReps *body_to_imu)
void	ahrs_dcm_set_body_to_imu_quat (struct FloatQuat *q_b2i)

Variables
struct AhrsFloatDCM	ahrs_dcm
struct FloatVect3	accel_float = {0, 0, 0}
struct FloatVect3	mag_float = {0, 0, 0}
float	Omega_Vector [3] = {0, 0, 0}
float	Omega_P [3] = {0, 0, 0}
float	Omega_I [3] = {0, 0, 0}
float	Omega [3] = {0, 0, 0}
float	DCM_Matrix [3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
float	Update_Matrix [3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}
float	Temporary_Matrix [3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}
float	MAG_Heading_X = 1
float	MAG_Heading_Y = 0

Detailed Description

Attitude estimation for fixedwings based on the DCM.

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

Options:

• USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good
• USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed

Definition in file ahrs_float_dcm.c.

Macro Definition Documentation

ACCEL_WEIGHT_BAND

#define ACCEL_WEIGHT_BAND   1.f

Definition at line 376 of file ahrs_float_dcm.c.

ACCEL_WEIGHT_FILTER

#define ACCEL_WEIGHT_FILTER   8

Definition at line 369 of file ahrs_float_dcm.c.

Function Documentation

ahrs_dcm_align()

bool ahrs_dcm_align	(	struct FloatRates *	lp_gyro,
		struct FloatVect3 *	lp_accel,
		struct FloatVect3 *	lp_mag
	)

Definition at line 119 of file ahrs_float_dcm.c.

References ahrs_dcm, AHRS_DCM_RUNNING, ahrs_float_get_quat_from_accel(), ahrs_float_get_quat_from_accel_mag(), float_eulers_of_quat(), float_rmat_of_quat(), AhrsFloatDCM::gyro_bias, AhrsFloatDCM::is_aligned, AhrsFloatDCM::ltp_to_imu_euler, set_dcm_matrix_from_rmat(), and AhrsFloatDCM::status.

Referenced by aligner_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_dcm_init()

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, 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:

ahrs_dcm_propagate()

void ahrs_dcm_propagate	(	struct FloatRates *	gyro,
		float	dt
	)

Definition at line 148 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, and RATES_DIFF.

Referenced by gyro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ahrs_dcm_set_body_to_imu()

void ahrs_dcm_set_body_to_imu

(

struct OrientationReps *

body_to_imu

)

Definition at line 536 of file ahrs_float_dcm.c.

References ahrs_dcm_set_body_to_imu_quat(), body_to_imu, and orientationGetQuat_f().

Here is the call graph for this function:

ahrs_dcm_set_body_to_imu_quat()

void ahrs_dcm_set_body_to_imu_quat

(

struct FloatQuat *

q_b2i

)

Definition at line 541 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:

ahrs_dcm_update_accel()

void ahrs_dcm_update_accel

(

struct FloatVect3 *

accel

)

Definition at line 200 of file ahrs_float_dcm.c.

References accel_float, 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:

ahrs_dcm_update_gps()

void ahrs_dcm_update_gps

(

struct GpsState *gps_s

UNUSED

)

Definition at line 175 of file ahrs_float_dcm.c.

References ahrs_dcm, f, AhrsFloatDCM::gps_acceleration, AhrsFloatDCM::gps_age, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, GPS_FIX_3D, and AhrsFloatDCM::gps_speed.

Referenced by gps_cb().

Here is the caller graph for this function:

ahrs_dcm_update_mag()

void ahrs_dcm_update_mag

(

struct FloatVect3 *

mag

)

Definition at line 227 of file ahrs_float_dcm.c.

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

Referenced by mag_cb().

Here is the caller graph for this function:

compute_ahrs_representations()

static void compute_ahrs_representations ( void  )

static

Definition at line 522 of file ahrs_float_dcm.c.

References accel_float, ahrs_dcm, DCM_Matrix, GRAVITY, AhrsFloatDCM::ltp_to_imu_euler, FloatEulers::phi, FloatEulers::psi, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_dcm_propagate().

Here is the caller graph for this function:

Drift_correction()

void Drift_correction

(

void

)

Definition at line 379 of file ahrs_float_dcm.c.

References accel_float, ACCEL_WEIGHT_BAND, ACCEL_WEIGHT_FILTER, ahrs_dcm, autopilot, course, DCM_Matrix, f, FALSE, AhrsFloatDCM::gps_course, AhrsFloatDCM::gps_course_valid, GRAVITY, imu_health, Ki_ROLLPITCH, Ki_YAW, Kp_ROLLPITCH, Kp_YAW, pprz_autopilot::launch, MAG_Heading_X, MAG_Heading_Y, Max, Omega_I, Omega_P, PRINT_CONFIG_MSG(), Vector_Add(), Vector_Cross_Product(), Vector_Dot_Product(), Vector_Scale(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ahrs_dcm_update_accel().

Here is the call graph for this function:

Here is the caller graph for this function:

Matrix_update()

void Matrix_update

(

float

dt

)

Definition at line 486 of file ahrs_float_dcm.c.

References ahrs_dcm, DCM_Matrix, AhrsFloatDCM::imu_rate, Matrix_Multiply(), Omega, Omega_I, Omega_P, Omega_Vector, FloatRates::p, FloatRates::q, FloatRates::r, Temporary_Matrix, Update_Matrix, Vector_Add(), FloatVect3::x, and FloatVect3::y.

Referenced by ahrs_dcm_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

Normalize()

void Normalize

(

void

)

Definition at line 286 of file ahrs_float_dcm.c.

References ahrs_dcm, AhrsFloatDCM::body_to_imu, DCM_Matrix, f, orientationGetRMat_f(), renorm_blowup_count, renorm_sqrt_count, set_dcm_matrix_from_rmat(), Vector_Add(), Vector_Cross_Product(), Vector_Dot_Product(), and Vector_Scale().

Referenced by ahrs_dcm_propagate().

Here is the call graph for this function:

Here is the caller graph for this function:

set_dcm_matrix_from_rmat()

static void set_dcm_matrix_from_rmat ( struct FloatRMat *  rmat )

inlinestatic

Definition at line 90 of file ahrs_float_dcm.c.

References DCM_Matrix, and RMAT_ELMT.

Referenced by ahrs_dcm_align(), ahrs_dcm_init(), and Normalize().

Here is the caller graph for this function:

Variable Documentation

accel_float

struct FloatVect3 accel_float = {0, 0, 0}

Definition at line 59 of file ahrs_float_dcm.c.

Referenced by ahrs_dcm_update_accel(), compute_ahrs_representations(), Drift_correction(), and send_accel().

ahrs_dcm

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(), send_filter_status(), and set_body_orientation_and_rates().

DCM_Matrix

float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}

Definition at line 67 of file ahrs_float_dcm.c.

Referenced by compute_ahrs_representations(), Drift_correction(), Matrix_update(), Normalize(), and set_dcm_matrix_from_rmat().

mag_float

struct FloatVect3 mag_float = {0, 0, 0}

Definition at line 60 of file ahrs_float_dcm.c.

Referenced by send_mag().

MAG_Heading_X

float MAG_Heading_X = 1

Definition at line 72 of file ahrs_float_dcm.c.

Referenced by ahrs_dcm_update_mag(), and Drift_correction().

MAG_Heading_Y

float MAG_Heading_Y = 0

Definition at line 73 of file ahrs_float_dcm.c.

Referenced by ahrs_dcm_update_mag(), and Drift_correction().

Omega

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

Definition at line 65 of file ahrs_float_dcm.c.

Referenced by ahrs_dcm_update_accel(), and Matrix_update().

Omega_I

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

Definition at line 64 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

Omega_P

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

Definition at line 63 of file ahrs_float_dcm.c.

Referenced by Drift_correction(), and Matrix_update().

Omega_Vector

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

Definition at line 62 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

Temporary_Matrix

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

Definition at line 69 of file ahrs_float_dcm.c.

Referenced by Matrix_update().

Update_Matrix

float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}

Definition at line 68 of file ahrs_float_dcm.c.

Referenced by Matrix_update().