Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Macros | Functions | Variables
mag_calib_ukf.c File Reference
#include "modules/calibration/mag_calib_ukf.h"
#include "math/pprz_algebra_double.h"
#include "state.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
#include "generated/airframe.h"
#include "modules/ahrs/ahrs_magnetic_field_model.h"
#include "modules/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:

Definition in file mag_calib_ukf.c.

Macro Definition Documentation

◆ MAG_CALIB_UKF_ABI_BIND_ID

#define MAG_CALIB_UKF_ABI_BIND_ID   ABI_BROADCAST

Definition at line 94 of file mag_calib_ukf.c.

◆ MAG_CALIB_UKF_HOTSTART

#define MAG_CALIB_UKF_HOTSTART   FALSE

Definition at line 110 of file mag_calib_ukf.c.

◆ MAG_CALIB_UKF_HOTSTART_SAVE_FILE

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

◆ MAG_CALIB_UKF_NOISE_RMS

#define MAG_CALIB_UKF_NOISE_RMS   2e-1f

Definition at line 104 of file mag_calib_ukf.c.

◆ MAG_CALIB_UKF_NORM

#define MAG_CALIB_UKF_NORM   1.0f

Definition at line 99 of file mag_calib_ukf.c.

◆ MAG_CALIB_UKF_VERBOSE

#define MAG_CALIB_UKF_VERBOSE   FALSE

Definition at line 82 of file mag_calib_ukf.c.

◆ VERBOSE_PRINT

#define VERBOSE_PRINT (   ...)

Definition at line 89 of file mag_calib_ukf.c.

Function Documentation

◆ mag_calib_hotstart_read()

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:

◆ mag_calib_hotstart_write()

void mag_calib_hotstart_write ( void  )

Definition at line 237 of file mag_calib_ukf.c.

References mag_calib, and VERBOSE_PRINT.

◆ mag_calib_send_state()

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:

◆ mag_calib_ukf_init()

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:

◆ mag_calib_ukf_run()

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

◆ mag_calib_update_field()

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

◆ calibrated_mag

struct Int32Vect3 calibrated_mag

Definition at line 120 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_run().

◆ H

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

Definition at line 125 of file mag_calib_ukf.c.

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

◆ h_ev

abi_event h_ev
static

Definition at line 125 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().

◆ mag_calib

TRICAL_instance_t mag_calib
static

Definition at line 123 of file mag_calib_ukf.c.

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

◆ mag_calib_ukf_reset_state

bool mag_calib_ukf_reset_state = false

Definition at line 119 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_run().

◆ mag_calib_ukf_send_state

bool mag_calib_ukf_send_state = false

Definition at line 120 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_run().

◆ mag_ev

abi_event mag_ev
static

Definition at line 124 of file mag_calib_ukf.c.

Referenced by mag_calib_ukf_init().