mag_calib_ukf.c File Reference

#include "modules/calibration/mag_calib_ukf.h"
#include "math/pprz_algebra_double.h"
#include "state.h"
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "generated/airframe.h"
#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
#include "subsystems/datalink/telemetry.h"
#include "TRICAL.h"

Include dependency graph for mag_calib_ukf.c:

Go to the source code of this file.

Macros
#define	MAG_CALIB_UKF_VERBOSE   FALSE
#define	VERBOSE_PRINT(...)
#define	MAG_CALIB_UKF_ABI_BIND_ID   ABI_BROADCAST
#define	MAG_CALIB_UKF_NORM   1.0f
#define	MAG_CALIB_UKF_NOISE_RMS   2e-1f
#define	MAG_CALIB_UKF_HOTSTART   FALSE
#define	MAG_CALIB_UKF_HOTSTART_SAVE_FILE   /data/ftp/internal_000/mag_ukf_calib.txt

Functions
static void	mag_calib_ukf_run (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
	Callback function run for every new mag measurement. More...
static void	mag_calib_update_field (uint8_t sender_id, struct FloatVect3 *h)
	Callback function to update reference magnetic field from geo_mag module. More...
void	mag_calib_ukf_init (void)
void	mag_calib_send_state (void)
void	mag_calib_hotstart_read (void)
void	mag_calib_hotstart_write (void)

Variables
bool	mag_calib_ukf_reset_state = false
bool	mag_calib_ukf_send_state = false
struct Int32Vect3	calibrated_mag
static TRICAL_instance_t	mag_calib
static abi_event	mag_ev
static abi_event	h_ev
static struct FloatVect3	H = { .x = AHRS_H_X, .y = AHRS_H_Y, .z = AHRS_H_Z}

Detailed Description

Author

w.vlenterie Calibrate the magnetometer using an unscented kalman filter For more information please visit the following links:

• https://github.com/sfwa/trical
• http://au.tono.my/log/20131213-trical-magnetometer-calibration.html
• http://www.acsu.buffalo.edu/~johnc/mag_cal05.pdf

Definition in file mag_calib_ukf.c.

Macro Definition Documentation

#define MAG_CALIB_UKF_ABI_BIND_ID   ABI_BROADCAST

Definition at line 94 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().

#define MAG_CALIB_UKF_HOTSTART   FALSE

Definition at line 110 of file mag_calib_ukf.c.

#define MAG_CALIB_UKF_HOTSTART_SAVE_FILE   /data/ftp/internal_000/mag_ukf_calib.txt

Definition at line 115 of file mag_calib_ukf.c.

Referenced by mag_calib_hotstart_read().

#define MAG_CALIB_UKF_NOISE_RMS   2e-1f

Definition at line 104 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().

#define MAG_CALIB_UKF_NORM   1.0f

Definition at line 99 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().

#define MAG_CALIB_UKF_VERBOSE   FALSE

Definition at line 82 of file mag_calib_ukf.c.

#define VERBOSE_PRINT

(

...

)

Definition at line 89 of file mag_calib_ukf.c.

Referenced by mag_calib_hotstart_read(), mag_calib_hotstart_write(), mag_calib_ukf_run(), and mag_calib_update_field().

Function Documentation

void mag_calib_hotstart_read

(

void

)

Definition at line 215 of file mag_calib_ukf.c.

References mag_calib, MAG_CALIB_UKF_HOTSTART_SAVE_FILE, and VERBOSE_PRINT.

Referenced by mag_calib_ukf_init().

Here is the caller graph for this function:

void mag_calib_hotstart_write

(

void

)

Definition at line 237 of file mag_calib_ukf.c.

References mag_calib, and VERBOSE_PRINT.

void mag_calib_send_state

(

void

)

Definition at line 210 of file mag_calib_ukf.c.

References DefaultChannel, DefaultDevice, and mag_calib.

Referenced by mag_calib_ukf_run().

Here is the caller graph for this function:

void mag_calib_ukf_init

(

void

)

< GEO_MAG_SENDER_ID is defined in geo_mag.c so unknown

Definition at line 134 of file mag_calib_ukf.c.

References ABI_BROADCAST, h_ev, mag_calib, mag_calib_hotstart_read(), MAG_CALIB_UKF_ABI_BIND_ID, MAG_CALIB_UKF_NOISE_RMS, MAG_CALIB_UKF_NORM, mag_calib_ukf_run(), mag_calib_update_field(), and mag_ev.

Here is the call graph for this function:

void mag_calib_ukf_run ( uint8_t  sender_id, uint32_t  stamp, struct Int32Vect3 *  mag  )

static

Callback function run for every new mag measurement.

See if we need to reset the state

Update magnetometer UKF and calibrate measurement

Rotate the local magnetic field by our current attitude

Update magnetometer UKF

Save calibrated result

Debug print

Forward calibrated data

Definition at line 150 of file mag_calib_ukf.c.

References calibrated_mag, float_quat_vmult(), H, imu, Imu::mag, MAG_BFP_OF_REAL, mag_calib, mag_calib_send_state(), MAG_CALIB_UKF_ID, mag_calib_ukf_reset_state, mag_calib_ukf_send_state, MAG_FLOAT_OF_BFP, stateGetNedToBodyQuat_f(), VERBOSE_PRINT, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by mag_calib_ukf_init().

Here is the call graph for this function:

Here is the caller graph for this function:

void mag_calib_update_field ( uint8_t  sender_id, struct FloatVect3 *  h  )

static

Callback function to update reference magnetic field from geo_mag module.

Definition at line 199 of file mag_calib_ukf.c.

References float_vect3_norm(), H, VERBOSE_PRINT, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by mag_calib_ukf_init().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

struct Int32Vect3 calibrated_mag

Definition at line 121 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_run().

struct FloatVect3 H = { .x = AHRS_H_X, .y = AHRS_H_Y, .z = AHRS_H_Z}

static

Definition at line 127 of file mag_calib_ukf.c.

Referenced by get_ajusted_value(), ins_mekf_wind_propagate_ahrs(), ins_mekf_wind_update_airspeed(), ins_mekf_wind_update_baro(), ins_mekf_wind_update_incidence(), ins_mekf_wind_update_mag(), ins_mekf_wind_update_pos_speed(), mag_calib_ukf_run(), mag_calib_update_field(), pprz_svd_float(), update_state(), and update_state_heading().

abi_event h_ev

static

Definition at line 125 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().

TRICAL_instance_t mag_calib

static

Definition at line 123 of file mag_calib_ukf.c.

Referenced by mag_calib_hotstart_read(), mag_calib_hotstart_write(), mag_calib_send_state(), mag_calib_ukf_init(), and mag_calib_ukf_run().

bool mag_calib_ukf_reset_state = false

Definition at line 119 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_run().

bool mag_calib_ukf_send_state = false

Definition at line 120 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_run().

abi_event mag_ev

static

Definition at line 124 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().