Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ahrs_float_dcm.c File Reference

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

#include "std.h"
#include "modules/ahrs/ahrs_float_dcm.h"
#include "modules/ahrs/ahrs_float_utils.h"
#include "autopilot.h"
#include "modules/ahrs/ahrs_float_dcm_algebra.h"
#include "math/pprz_algebra_float.h"
#include "modules/gps/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)

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 378 of file ahrs_float_dcm.c.

Referenced by Drift_correction().

◆ ACCEL_WEIGHT_FILTER

#define ACCEL_WEIGHT_FILTER   8

Definition at line 371 of file ahrs_float_dcm.c.

Referenced by Drift_correction().

Function Documentation

◆ ahrs_dcm_align()

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

Definition at line 120 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(), and set_dcm_matrix_from_rmat().

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, FLOAT_EULERS_ZERO, FLOAT_RATES_ZERO, float_rmat_identity(), and set_dcm_matrix_from_rmat().

Referenced by ahrs_dcm_wrapper_init().

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 149 of file ahrs_float_dcm.c.

References ahrs_dcm, compute_ahrs_representations(), Matrix_update(), Normalize(), 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_update_accel()

void ahrs_dcm_update_accel ( struct FloatVect3 * accel)

Definition at line 201 of file ahrs_float_dcm.c.

References accel_float, ahrs_dcm, Drift_correction(), 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 176 of file ahrs_float_dcm.c.

References ahrs_dcm, AHRS_FLOAT_MIN_SPEED_GPS_COURSE, GPS_FIX_3D, and UNUSED.

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 228 of file ahrs_float_dcm.c.

References ahrs_dcm, DefaultChannel, DefaultDevice, MAG_Heading_X, MAG_Heading_Y, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by mag_cb().

Here is the caller graph for this function:

◆ compute_ahrs_representations()

void compute_ahrs_representations ( void )
static

Definition at line 524 of file ahrs_float_dcm.c.

References accel_float, ahrs_dcm, DCM_Matrix, and GRAVITY.

Referenced by ahrs_dcm_propagate().

Here is the caller graph for this function:

◆ Drift_correction()

void Drift_correction ( void )

◆ Matrix_update()

void Matrix_update ( float dt)

Definition at line 488 of file ahrs_float_dcm.c.

References ahrs_dcm, DCM_Matrix, Matrix_Multiply(), Omega, Omega_I, Omega_P, Omega_Vector, Temporary_Matrix, Update_Matrix, and Vector_Add().

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 DCM_Matrix, float_rmat_identity(), 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()

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}

◆ ahrs_dcm

◆ DCM_Matrix

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

◆ 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().