Paparazzi UAS  v5.15_devel-59-g8c145b7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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  */
30 
32 #include "state.h"
33 #include "subsystems/imu.h"
34 #include "subsystems/abi.h"
35 #include "generated/airframe.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 
122 
123 static TRICAL_instance_t mag_calib;
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 
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);
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) {
159  mag_calib_ukf_send_state = false;
160  }
162  TRICAL_reset(&mag_calib);
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]);
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 
211 {
212  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, mag_calib.state);
213 }
214 
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 
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 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
bool mag_calib_ukf_send_state
#define MAG_CALIB_UKF_NORM
Definition: mag_calib_ukf.c:99
Periodic telemetry system header (includes downlink utility and generated code).
#define MAG_CALIB_UKF_NOISE_RMS
Main include for ABI (AirBorneInterface).
static abi_event h_ev
#define MAG_FLOAT_OF_BFP(_ai)
#define MAG_CALIB_UKF_VERBOSE
Definition: mag_calib_ukf.c:82
struct Int32Vect3 calibrated_mag
struct Imu imu
global IMU state
Definition: imu.c:108
void mag_calib_hotstart_write(void)
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
void mag_calib_send_state(void)
Roation quaternion.
static abi_event mag_ev
static float float_vect3_norm(struct FloatVect3 *v)
#define VERBOSE_PRINT(...)
Definition: mag_calib_ukf.c:89
void mag_calib_hotstart_read(void)
#define MAG_CALIB_UKF_HOTSTART_SAVE_FILE
#define MAG_BFP_OF_REAL(_af)
void mag_calib_ukf_init(void)
#define MAG_CALIB_UKF_ABI_BIND_ID
Definition: mag_calib_ukf.c:94
unsigned long uint32_t
Definition: types.h:18
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define MAG_CALIB_UKF_ID
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
signed long int32_t
Definition: types.h:19
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static void mag_calib_update_field(uint8_t sender_id, struct FloatVect3 *h)
Callback function to update reference magnetic field from geo_mag module.
static TRICAL_instance_t mag_calib
static struct FloatVect3 H
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
Paparazzi double precision floating point algebra.
#define false
Definition: rtwtypes.h:24
bool mag_calib_ukf_reset_state
static void mag_calib_ukf_run(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Callback function run for every new mag measurement.
#define MAG_CALIB_UKF_HOTSTART