Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mag_calib_ukf.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) w.vlenterie
3  *
4  * This file is part of paparazzi
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
29 #include "modules/calibration/mag_calib_ukf.h"
30 
31 #include "math/pprz_algebra_double.h"
32 #include "state.h"
33 #include "subsystems/imu.h"
34 #include "subsystems/abi.h"
35 #include "generated/airframe.h"
36 #include "subsystems/ahrs/ahrs_magnetic_field_model.h"
37 #include "subsystems/datalink/telemetry.h"
38 #include "TRICAL.h"
39 
40 //
41 // Try to print warnings to user for bad configuration
42 //
43 #if !defined AHRS_FC_MAG_ID && !defined AHRS_ICE_MAG_ID && !defined AHRS_MLKF_MAG_ID && !defined AHRS_FINV_MAG_ID && \
44  !defined AHRS_DCM_MAG_ID && !defined AHRS_ICQ_MAG_ID && !defined INS_FINV_MAG_ID
45 #warning "your AHRS/INS configuration might be wrong to use onboard mag calibration, please refer to the documentation"
46 #endif
47 
48 #if defined AHRS_FC_MAG_ID && (AHRS_FC_MAG_ID != MAG_CALIB_UKF_ID)
49 #warning "your AHRS_FC_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
50 #endif
51 
52 #if defined AHRS_ICE_MAG_ID && (AHRS_ICE_MAG_ID != MAG_CALIB_UKF_ID)
53 #warning "your AHRS_ICE_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
54 #endif
55 
56 #if defined AHRS_MLKF_MAG_ID && (AHRS_MLKF_MAG_ID != MAG_CALIB_UKF_ID)
57 #warning "your AHRS_MLKF_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
58 #endif
59 
60 #if defined AHRS_FINV_MAG_ID && (AHRS_FINV_MAG_ID != MAG_CALIB_UKF_ID)
61 #warning "your AHRS_FINV_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
62 #endif
63 
64 #if defined AHRS_DCM_MAG_ID && (AHRS_DCM_MAG_ID != MAG_CALIB_UKF_ID)
65 #warning "your AHRS_DCM_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
66 #endif
67 
68 #if defined AHRS_ICQ_MAG_ID && (AHRS_ICQ_MAG_ID != MAG_CALIB_UKF_ID)
69 #warning "your AHRS_ICQ_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
70 #endif
71 
72 #if defined INS_FINV_MAG_ID && (INS_FINV_MAG_ID != MAG_CALIB_UKF_ID)
73 #warning "your INS_FINV_MAG_ID might by wrong please set to MAG_CALIB_UKF_ID to use onboard mag calibration"
74 #endif
75 
76 // ABI callback declarations
77 static void mag_calib_ukf_run(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
78 static void mag_calib_update_field(uint8_t __attribute__((unused)) sender_id, struct FloatVect3 *h);
79 
80 // Verbose mode is only available on Linux-based autopilots
81 #ifndef MAG_CALIB_UKF_VERBOSE
82 #define MAG_CALIB_UKF_VERBOSE FALSE
83 #endif
84 
85 #if MAG_CALIB_UKF_VERBOSE
86 #include <stdio.h>
87 #define VERBOSE_PRINT(string,...) fprintf(stderr, "[CALIB_UKF->%s()] " string,__FUNCTION__ , ##__VA_ARGS__)
88 #else
89 #define VERBOSE_PRINT(...)
90 #endif
91 PRINT_CONFIG_VAR(MAG_CALIB_UKF_VERBOSE)
92 
93 #ifndef MAG_CALIB_UKF_ABI_BIND_ID
94 #define MAG_CALIB_UKF_ABI_BIND_ID ABI_BROADCAST
95 #endif
96 PRINT_CONFIG_VAR(MAG_CALIB_UKF_ABI_BIND_ID)
97 
98 #ifndef MAG_CALIB_UKF_NORM
99 #define MAG_CALIB_UKF_NORM 1.0f
100 #endif
101 PRINT_CONFIG_VAR(MAG_CALIB_UKF_NORM)
102 
103 #ifndef MAG_CALIB_UKF_NOISE_RMS
104 #define MAG_CALIB_UKF_NOISE_RMS 2e-1f
105 #endif
106 PRINT_CONFIG_VAR(MAG_CALIB_UKF_NOISE_RMS)
107 
108 // Hotstart is only available on Linux-based autopilots
109 #ifndef MAG_CALIB_UKF_HOTSTART
110 #define MAG_CALIB_UKF_HOTSTART FALSE
111 #endif
112 PRINT_CONFIG_VAR(MAG_CALIB_UKF_HOTSTART)
113 
114 #ifndef MAG_CALIB_UKF_HOTSTART_SAVE_FILE
115 #define MAG_CALIB_UKF_HOTSTART_SAVE_FILE /data/ftp/internal_000/mag_ukf_calib.txt
116 #endif
117 PRINT_CONFIG_VAR(MAG_CALIB_UKF_HOTSTART_SAVE_FILE)
118 
119 bool mag_calib_ukf_reset_state = false;
120 bool mag_calib_ukf_send_state = false;
121 struct Int32Vect3 calibrated_mag;
122 
123 static TRICAL_instance_t mag_calib;
124 static abi_event mag_ev;
125 static abi_event h_ev;
126 
127 static struct FloatVect3 H = { .x = AHRS_H_X, .y = AHRS_H_Y, .z = AHRS_H_Z};
128 
129 #if MAG_CALIB_UKF_HOTSTART
130 static FILE *fp;
131 static char hotstart_file_name[512];
132 #endif
133 
134 void mag_calib_ukf_init(void)
135 {
136  TRICAL_init(&mag_calib);
137  TRICAL_norm_set(&mag_calib, MAG_CALIB_UKF_NORM);
138  TRICAL_noise_set(&mag_calib, MAG_CALIB_UKF_NOISE_RMS);
139  mag_calib_hotstart_read();
140 #ifdef MAG_CALIB_UKF_INITIAL_STATE
141  float initial_state[12] = MAG_CALIB_UKF_INITIAL_STATE;
142  memcpy(&mag_calib.state, &initial_state, 12 * sizeof(float));
143 #endif
144  AbiBindMsgIMU_MAG_INT32(MAG_CALIB_UKF_ABI_BIND_ID, &mag_ev, mag_calib_ukf_run);
145  AbiBindMsgGEO_MAG(ABI_BROADCAST, &h_ev, mag_calib_update_field);
146 }
147 
150 void mag_calib_ukf_run(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
151 {
152  float measurement[3] = {0.0f, 0.0f, 0.0f};
153  float calibrated_measurement[3] = {0.0f, 0.0f, 0.0f};
154 
155  if (sender_id != MAG_CALIB_UKF_ID) {
157  if (mag_calib_ukf_send_state) {
158  mag_calib_send_state();
159  mag_calib_ukf_send_state = false;
160  }
161  if (mag_calib_ukf_reset_state) {
162  TRICAL_reset(&mag_calib);
163  mag_calib_ukf_reset_state = false;
164  }
166  if (mag->x != 0 || mag->y != 0 || mag->z != 0) {
168  struct FloatQuat *body_quat = stateGetNedToBodyQuat_f();
169  struct FloatVect3 expected_measurement;
170  float_quat_vmult(&expected_measurement, body_quat, &H);
171  float expected_mag_field[3] = { expected_measurement.x, expected_measurement.y, expected_measurement.z };
173  measurement[0] = MAG_FLOAT_OF_BFP(mag->x);
174  measurement[1] = MAG_FLOAT_OF_BFP(mag->y);
175  measurement[2] = MAG_FLOAT_OF_BFP(mag->z);
176  TRICAL_estimate_update(&mag_calib, measurement, expected_mag_field);
177  TRICAL_measurement_calibrate(&mag_calib, measurement, calibrated_measurement);
179  calibrated_mag.x = (int32_t) MAG_BFP_OF_REAL(calibrated_measurement[0]);
180  calibrated_mag.y = (int32_t) MAG_BFP_OF_REAL(calibrated_measurement[1]);
181  calibrated_mag.z = (int32_t) MAG_BFP_OF_REAL(calibrated_measurement[2]);
182  imu.mag.x = calibrated_mag.x;
183  imu.mag.y = calibrated_mag.y;
184  imu.mag.z = calibrated_mag.z;
185 
187  VERBOSE_PRINT("magnetometer measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n", measurement[0], measurement[1], measurement[2], hypot(hypot(measurement[0], measurement[1]), measurement[2]));
188  VERBOSE_PRINT("magnetometer bias_f (x: %4.2f y: %4.2f z: %4.2f)\n", mag_calib.state[0], mag_calib.state[1], mag_calib.state[2]);
189  VERBOSE_PRINT("expected measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n", expected_mag_field[0], expected_mag_field[1], expected_mag_field[2], hypot(hypot(expected_mag_field[0], expected_mag_field[1]), expected_mag_field[2]));
190  VERBOSE_PRINT("calibrated measurement (x: %4.2f y: %4.2f z: %4.2f) norm: %4.2f\n\n", calibrated_measurement[0], calibrated_measurement[1], calibrated_measurement[2], hypot(hypot(calibrated_measurement[0], calibrated_measurement[1]), calibrated_measurement[2]));
192  AbiSendMsgIMU_MAG_INT32(MAG_CALIB_UKF_ID, stamp, &calibrated_mag);
193  }
194  }
195 }
196 
199 void mag_calib_update_field(uint8_t __attribute__((unused)) sender_id, struct FloatVect3 *h)
200 {
201  double n = float_vect3_norm(h);
202  if (n > 0.01) {
203  H.x = (float)(h->x / n);
204  H.y = (float)(h->y / n);
205  H.z = (float)(h->z / n);
206  VERBOSE_PRINT("Updating local magnetic field from geo_mag module (Hx: %4.2f, Hy: %4.2f, Hz: %4.2f)\n", H.x, H.y, H.z);
207  }
208 }
209 
210 void mag_calib_send_state(void)
211 {
212  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, mag_calib.state);
213 }
214 
215 void mag_calib_hotstart_read(void)
216 {
217 #if MAG_CALIB_UKF_HOTSTART
218  snprintf(hotstart_file_name, 512, "%s", STRINGIFY(MAG_CALIB_UKF_HOTSTART_SAVE_FILE));
219  fp = fopen(hotstart_file_name, "r");
220  if (fp != NULL) {
221  fread(mag_calib.state, sizeof(float), 12, fp);
222  fclose(fp);
223  VERBOSE_PRINT("Loaded initial state from disk:\n"
224  "bias {%4.2f, %4.2f, %4.2f}\n"
225  "scale {%4.2f, %4.2f, %4.2f}\n"
226  " {%4.2f, %4.2f, %4.2f}\n"
227  " {%4.2f, %4.2f, %4.2f}\n",
228  mag_calib.state[0], mag_calib.state[1], mag_calib.state[2],
229  mag_calib.state[3], mag_calib.state[4], mag_calib.state[5],
230  mag_calib.state[6], mag_calib.state[7], mag_calib.state[8],
231  mag_calib.state[9], mag_calib.state[10], mag_calib.state[11]
232  );
233  }
234 #endif
235 }
236 
237 void mag_calib_hotstart_write(void)
238 {
239 #if USE_MAGNETOMETER && MAG_CALIB_UKF_HOTSTART
240  fp = fopen(hotstart_file_name, "w");
241  if (fp != NULL) {
242  fwrite(mag_calib.state, sizeof(float), 12, fp);
243  fclose(fp);
244  VERBOSE_PRINT("Wrote current state to disk:\n"
245  "bias {%4.2f, %4.2f, %4.2f}\n"
246  "scale {%4.2f, %4.2f, %4.2f}\n"
247  " {%4.2f, %4.2f, %4.2f}\n"
248  " {%4.2f, %4.2f, %4.2f}\n",
249  mag_calib.state[0], mag_calib.state[1], mag_calib.state[2],
250  mag_calib.state[3], mag_calib.state[4], mag_calib.state[5],
251  mag_calib.state[6], mag_calib.state[7], mag_calib.state[8],
252  mag_calib.state[9], mag_calib.state[10], mag_calib.state[11]
253  );
254  }
255 #endif
256 }
mag_calib
static TRICAL_instance_t mag_calib
Definition: mag_calib_ukf.c:123
H
static struct FloatVect3 H
Definition: mag_calib_ukf.c:127
mag_calib_ukf_reset_state
bool mag_calib_ukf_reset_state
Definition: mag_calib_ukf.c:119
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
abi.h
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
MAG_FLOAT_OF_BFP
#define MAG_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:223
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint32_t
unsigned long uint32_t
Definition: types.h:18
mag_calib_ukf_init
void mag_calib_ukf_init(void)
Definition: mag_calib_ukf.c:134
mag_calib_ukf_run
static void mag_calib_ukf_run(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Callback function run for every new mag measurement.
Definition: mag_calib_ukf.c:150
mag_calib_update_field
static void mag_calib_update_field(uint8_t sender_id, struct FloatVect3 *h)
Callback function to update reference magnetic field from geo_mag module.
Definition: mag_calib_ukf.c:199
FloatVect3
Definition: pprz_algebra_float.h:54
telemetry.h
imu.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
float_vect3_norm
static float float_vect3_norm(struct FloatVect3 *v)
Definition: pprz_algebra_float.h:171
MAG_BFP_OF_REAL
#define MAG_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:222
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
Int32Vect3
Definition: pprz_algebra_int.h:88
MAG_CALIB_UKF_NOISE_RMS
#define MAG_CALIB_UKF_NOISE_RMS
Definition: mag_calib_ukf.c:104
uint8_t
unsigned char uint8_t
Definition: types.h:14
MAG_CALIB_UKF_NORM
#define MAG_CALIB_UKF_NORM
Definition: mag_calib_ukf.c:99
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
mag_calib_send_state
void mag_calib_send_state(void)
Definition: mag_calib_ukf.c:210
imu
struct Imu imu
global IMU state
Definition: imu.c:108
float_quat_vmult
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_float.c:421
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
mag_calib_hotstart_write
void mag_calib_hotstart_write(void)
Definition: mag_calib_ukf.c:237
pprz_algebra_double.h
Paparazzi double precision floating point algebra.
MAG_CALIB_UKF_HOTSTART
#define MAG_CALIB_UKF_HOTSTART
Definition: mag_calib_ukf.c:110
MAG_CALIB_UKF_HOTSTART_SAVE_FILE
#define MAG_CALIB_UKF_HOTSTART_SAVE_FILE
Definition: mag_calib_ukf.c:115
MAG_CALIB_UKF_ID
#define MAG_CALIB_UKF_ID
Definition: abi_sender_ids.h:431
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
int32_t
signed long int32_t
Definition: types.h:19
false
#define false
Definition: microrl.h:7
h_ev
static abi_event h_ev
Definition: mag_calib_ukf.c:125
MAG_CALIB_UKF_VERBOSE
#define MAG_CALIB_UKF_VERBOSE
Definition: mag_calib_ukf.c:82
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
mag_calib_ukf_send_state
bool mag_calib_ukf_send_state
Definition: mag_calib_ukf.c:120
calibrated_mag
struct Int32Vect3 calibrated_mag
Definition: mag_calib_ukf.c:121
Imu::mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
VERBOSE_PRINT
#define VERBOSE_PRINT(...)
Definition: mag_calib_ukf.c:89
state.h
ahrs_magnetic_field_model.h
mag_ev
static abi_event mag_ev
Definition: mag_calib_ukf.c:124
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
MAG_CALIB_UKF_ABI_BIND_ID
#define MAG_CALIB_UKF_ABI_BIND_ID
Definition: mag_calib_ukf.c:94
mag_calib_ukf.h
mag_calib_hotstart_read
void mag_calib_hotstart_read(void)
Definition: mag_calib_ukf.c:215