Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_ekf2.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include "modules/ins/ins_ekf2.h"
30 #include "modules/nav/waypoints.h"
31 #include "modules/core/abi.h"
32 #include "stabilization/stabilization_attitude.h"
33 #include "generated/airframe.h"
34 #include "generated/flight_plan.h"
35 #include "EKF/ekf.h"
36 #include "math/pprz_isa.h"
38 #include "mcu_periph/sys_time.h"
39 #include "autopilot.h"
40 
42 #if defined SITL && USE_NPS
43 #include "nps_autopilot.h"
44 #include <stdio.h>
45 #endif
46 
48 #if USE_INS_NAV_INIT
49 #error INS initialization from flight plan is not yet supported
50 #endif
51 
53 #if INS_EKF2_OPTITRACK
54 #ifndef INS_EKF2_FUSION_MODE
55 #define INS_EKF2_FUSION_MODE (MASK_USE_EVPOS | MASK_USE_EVVEL | MASK_USE_EVYAW)
56 #endif
57 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
58 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_EV
59 #endif
60 #ifndef USE_INS_NAV_INIT
61 #define USE_INS_NAV_INIT true
62 #endif
63 #endif
64 
66 #ifndef INS_EKF2_FUSION_MODE
67 #define INS_EKF2_FUSION_MODE (MASK_USE_GPS)
68 #endif
69 PRINT_CONFIG_VAR(INS_EKF2_FUSION_MODE)
70 
71 
72 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
73 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO
74 #endif
75 PRINT_CONFIG_VAR(INS_EKF2_VDIST_SENSOR_TYPE)
76 
77 
78 #ifndef INS_EKF2_GPS_CHECK_MASK
79 #define INS_EKF2_GPS_CHECK_MASK 21 // (MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)
80 #endif
81 PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK)
82 
83 
84 #ifndef INS_EKF2_SONAR_MIN_RANGE
85 #define INS_EKF2_SONAR_MIN_RANGE 0.001
86 #endif
87 PRINT_CONFIG_VAR(INS_EKF2_SONAR_MIN_RANGE)
88 
89 
90 #ifndef INS_EKF2_SONAR_MAX_RANGE
91 #define INS_EKF2_SONAR_MAX_RANGE 4
92 #endif
93 PRINT_CONFIG_VAR(INS_EKF2_SONAR_MAX_RANGE)
94 
95 
96 #ifndef INS_EKF2_RANGE_MAIN_AGL
97 #define INS_EKF2_RANGE_MAIN_AGL 1
98 #endif
99 PRINT_CONFIG_VAR(INS_EKF2_RANGE_MAIN_AGL)
100 
101 
102 #ifndef INS_EKF2_BARO_ID
103 #if USE_BARO_BOARD
104 #define INS_EKF2_BARO_ID BARO_BOARD_SENDER_ID
105 #else
106 #define INS_EKF2_BARO_ID ABI_BROADCAST
107 #endif
108 #endif
109 PRINT_CONFIG_VAR(INS_EKF2_BARO_ID)
110 
111 
112 #ifndef INS_EKF2_TEMPERATURE_ID
113 #define INS_EKF2_TEMPERATURE_ID ABI_BROADCAST
114 #endif
115 PRINT_CONFIG_VAR(INS_EKF2_TEMPERATURE_ID)
116 
117 
118 #ifndef INS_EKF2_AGL_ID
119 #define INS_EKF2_AGL_ID ABI_BROADCAST
120 #endif
121 PRINT_CONFIG_VAR(INS_EKF2_AGL_ID)
122 
123 /* default Gyro to use in INS */
124 #ifndef INS_EKF2_GYRO_ID
125 #define INS_EKF2_GYRO_ID ABI_BROADCAST
126 #endif
127 PRINT_CONFIG_VAR(INS_EKF2_GYRO_ID)
128 
129 /* default Accelerometer to use in INS */
130 #ifndef INS_EKF2_ACCEL_ID
131 #define INS_EKF2_ACCEL_ID ABI_BROADCAST
132 #endif
133 PRINT_CONFIG_VAR(INS_EKF2_ACCEL_ID)
134 
135 /* default Magnetometer to use in INS */
136 #ifndef INS_EKF2_MAG_ID
137 #define INS_EKF2_MAG_ID ABI_BROADCAST
138 #endif
139 PRINT_CONFIG_VAR(INS_EKF2_MAG_ID)
140 
141 /* default GPS to use in INS */
142 #ifndef INS_EKF2_GPS_ID
143 #define INS_EKF2_GPS_ID GPS_MULTI_ID
144 #endif
145 PRINT_CONFIG_VAR(INS_EKF2_GPS_ID)
146 
147 /* default Optical Flow to use in INS */
148 #ifndef INS_EKF2_OF_ID
149 #define INS_EKF2_OF_ID ABI_BROADCAST
150 #endif
151 PRINT_CONFIG_VAR(INS_EKF2_OF_ID)
152 
153 /* IMU X offset from CoG position in meters */
154 #ifndef INS_EKF2_IMU_POS_X
155 #define INS_EKF2_IMU_POS_X 0
156 #endif
157 PRINT_CONFIG_VAR(INS_EKF2_IMU_POS_X)
158 
159 /* IMU Y offset from CoG position in meters */
160 #ifndef INS_EKF2_IMU_POS_Y
161 #define INS_EKF2_IMU_POS_Y 0
162 #endif
163 PRINT_CONFIG_VAR(INS_EKF2_IMU_POS_Y)
164 
165 /* IMU Z offset from CoG position in meters */
166 #ifndef INS_EKF2_IMU_POS_Z
167 #define INS_EKF2_IMU_POS_Z 0
168 #endif
169 PRINT_CONFIG_VAR(INS_EKF2_IMU_POS_Z)
170 
171 /* GPS X offset from CoG position in meters */
172 #ifndef INS_EKF2_GPS_POS_X
173 #define INS_EKF2_GPS_POS_X 0
174 #endif
175 PRINT_CONFIG_VAR(INS_EKF2_GPS_POS_X)
176 
177 /* GPS Y offset from CoG position in meters */
178 #ifndef INS_EKF2_GPS_POS_Y
179 #define INS_EKF2_GPS_POS_Y 0
180 #endif
181 PRINT_CONFIG_VAR(INS_EKF2_GPS_POS_Y)
182 
183 /* GPS Z offset from CoG position in meters */
184 #ifndef INS_EKF2_GPS_POS_Z
185 #define INS_EKF2_GPS_POS_Z 0
186 #endif
187 PRINT_CONFIG_VAR(INS_EKF2_GPS_POS_Z)
188 
189 /* Default flow/radar message delay (in ms) */
190 #ifndef INS_EKF2_FLOW_SENSOR_DELAY
191 #define INS_EKF2_FLOW_SENSOR_DELAY 15
192 #endif
193 PRINT_CONFIG_VAR(INS_FLOW_SENSOR_DELAY)
194 
195 /* Default minimum accepted quality (1 to 255) */
196 #ifndef INS_EKF2_MIN_FLOW_QUALITY
197 #define INS_EKF2_MIN_FLOW_QUALITY 100
198 #endif
199 PRINT_CONFIG_VAR(INS_EKF2_MIN_FLOW_QUALITY)
200 
201 /* Max flow rate that the sensor can measure (rad/sec) */
202 #ifndef INS_EKF2_MAX_FLOW_RATE
203 #define INS_EKF2_MAX_FLOW_RATE 200
204 #endif
205 PRINT_CONFIG_VAR(INS_EKF2_MAX_FLOW_RATE)
206 
207 /* Flow sensor X offset from CoG position in meters */
208 #ifndef INS_EKF2_FLOW_POS_X
209 #define INS_EKF2_FLOW_POS_X 0
210 #endif
211 PRINT_CONFIG_VAR(INS_EKF2_FLOW_POS_X)
212 
213 /* Flow sensor Y offset from CoG position in meters */
214 #ifndef INS_EKF2_FLOW_POS_Y
215 #define INS_EKF2_FLOW_POS_Y 0
216 #endif
217 PRINT_CONFIG_VAR(INS_EKF2_FLOW_POS_Y)
218 
219 /* Flow sensor Z offset from CoG position in meters */
220 #ifndef INS_EKF2_FLOW_POS_Z
221 #define INS_EKF2_FLOW_POS_Z 0
222 #endif
223 PRINT_CONFIG_VAR(INS_EKF2_FLOW_POS_Z)
224 
225 /* Flow sensor noise in rad/sec */
226 #ifndef INS_EKF2_FLOW_NOISE
227 #define INS_EKF2_FLOW_NOISE 0.03
228 #endif
229 PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE)
230 
231 /* Flow sensor noise at qmin in rad/sec */
232 #ifndef INS_EKF2_FLOW_NOISE_QMIN
233 #define INS_EKF2_FLOW_NOISE_QMIN 0.05
234 #endif
235 PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE_QMIN)
236 
237 /* Flow sensor innovation gate */
238 #ifndef INS_EKF2_FLOW_INNOV_GATE
239 #define INS_EKF2_FLOW_INNOV_GATE 4
240 #endif
241 PRINT_CONFIG_VAR(INS_EKF2_FLOW_INNOV_GATE)
242 
243 /* External vision position noise (m) */
244 #ifndef INS_EKF2_EVP_NOISE
245 #define INS_EKF2_EVP_NOISE 0.02f
246 #endif
247 PRINT_CONFIG_VAR(INS_EKF2_EVP_NOISE)
248 
249 /* External vision velocity noise (m/s) */
250 #ifndef INS_EKF2_EVV_NOISE
251 #define INS_EKF2_EVV_NOISE 0.1f
252 #endif
253 PRINT_CONFIG_VAR(INS_EKF2_EVV_NOISE)
254 
255 /* External vision angle noise (rad) */
256 #ifndef INS_EKF2_EVA_NOISE
257 #define INS_EKF2_EVA_NOISE 0.05f
258 #endif
259 PRINT_CONFIG_VAR(INS_EKF2_EVA_NOISE)
260 
261 /* GPS measurement noise for horizontal velocity (m/s) */
262 #ifndef INS_EKF2_GPS_V_NOISE
263 #define INS_EKF2_GPS_V_NOISE 0.3f
264 #endif
265 PRINT_CONFIG_VAR(INS_EKF2_GPS_V_NOISE)
266 
267 /* GPS measurement position noise (m) */
268 #ifndef INS_EKF2_GPS_P_NOISE
269 #define INS_EKF2_GPS_P_NOISE 0.5f
270 #endif
271 PRINT_CONFIG_VAR(INS_EKF2_GPS_P_NOISE)
272 
273 /* Barometric measurement noise for altitude (m) */
274 #ifndef INS_EKF2_BARO_NOISE
275 #define INS_EKF2_BARO_NOISE 3.5f
276 #endif
277 PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
278 
279 /* All registered ABI events */
289 
290 /* All ABI callbacks */
291 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
292 static void temperature_cb(uint8_t sender_id, float temp);
293 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
294 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
295 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
296 static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
297 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
298 static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
299 static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x,
300  int32_t flow_der_y, float quality, float size_divergence);
301 
302 /* Static local functions */
303 static void ins_ekf2_publish_attitude(uint32_t stamp);
304 
305 /* Static local variables */
306 static Ekf ekf;
307 static parameters *ekf_params;
308 struct ekf2_t ekf2;
309 
310 #if PERIODIC_TELEMETRY
312 
313 static void send_ins(struct transport_tx *trans, struct link_device *dev)
314 {
315  struct NedCoor_i pos, speed, accel;
316 
317  // Get it from the EKF
318  const Vector3f pos_f{ekf.getPosition()};
319  const Vector3f speed_f{ekf.getVelocity()};
320  const Vector3f accel_f{ekf.getVelocityDerivative()};
321 
322  // Convert to integer
323  pos.x = POS_BFP_OF_REAL(pos_f(0));
324  pos.y = POS_BFP_OF_REAL(pos_f(1));
325  pos.z = POS_BFP_OF_REAL(pos_f(2));
326  speed.x = SPEED_BFP_OF_REAL(speed_f(0));
327  speed.y = SPEED_BFP_OF_REAL(speed_f(1));
328  speed.z = SPEED_BFP_OF_REAL(speed_f(2));
329  accel.x = ACCEL_BFP_OF_REAL(accel_f(0));
330  accel.y = ACCEL_BFP_OF_REAL(accel_f(1));
331  accel.z = ACCEL_BFP_OF_REAL(accel_f(2));
332 
333  // Send the message
334  pprz_msg_send_INS(trans, dev, AC_ID,
335  &pos.x, &pos.y, &pos.z,
336  &speed.x, &speed.y, &speed.z,
337  &accel.x, &accel.y, &accel.z);
338 }
339 
340 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
341 {
342  float baro_z = 0.0f;
343  int32_t pos_z, speed_z, accel_z;
344 
345  // Get it from the EKF
346  const Vector3f pos_f{ekf.getPosition()};
347  const Vector3f speed_f{ekf.getVelocity()};
348  const Vector3f accel_f{ekf.getVelocityDerivative()};
349 
350  // Convert to integer
351  pos_z = POS_BFP_OF_REAL(pos_f(2));
352  speed_z = SPEED_BFP_OF_REAL(speed_f(2));
353  accel_z = ACCEL_BFP_OF_REAL(accel_f(2));
354 
355  // Send the message
356  pprz_msg_send_INS_Z(trans, dev, AC_ID,
357  &baro_z, &pos_z, &speed_z, &accel_z);
358 }
359 
360 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
361 {
362  float qfe = 101325.0; //TODO: this is qnh not qfe?
363  if (ekf2.ltp_stamp > 0)
364  pprz_msg_send_INS_REF(trans, dev, AC_ID,
367  &ekf2.ltp_def.hmsl, &qfe);
368 }
369 
370 static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
371 {
372  uint16_t gps_check_status, soln_status;
373  uint16_t filter_fault_status = ekf.fault_status().value; // FIXME: 32bit instead of 16bit
374  uint32_t control_mode = ekf.control_status().value;
375  ekf.get_gps_check_status(&gps_check_status);
376  ekf.get_ekf_soln_status(&soln_status);
377 
378  uint16_t innov_test_status;
379  float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl;
380  uint8_t terrain_valid, dead_reckoning;
381  ekf.get_innovation_test_status(innov_test_status, mag, vel, pos, hgt, tas, hagl, beta);
382  //ekf.get_flow_innov(&flow);
383  ekf.get_mag_decl_deg(&mag_decl);
384 
385  if (ekf.isTerrainEstimateValid()) {
386  terrain_valid = 1;
387  } else {
388  terrain_valid = 0;
389  }
390 
391  if (ekf.inertial_dead_reckoning()) {
392  dead_reckoning = 1;
393  } else {
394  dead_reckoning = 0;
395  }
396 
397  pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
398  &control_mode, &filter_fault_status, &gps_check_status, &soln_status,
399  &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta,
400  &mag_decl, &terrain_valid, &dead_reckoning);
401 }
402 
403 static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
404 {
405  float gps_drift[3];
406  Vector3f vibe = ekf.getImuVibrationMetrics();
407  bool gps_blocked;
408  uint8_t gps_blocked_b;
409  ekf.get_gps_drift_metrics(gps_drift, &gps_blocked);
410  gps_blocked_b = gps_blocked;
411 
412  pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID,
413  &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b,
414  &vibe(0), &vibe(1), &vibe(2));
415 }
416 
417 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
418 {
419  uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2;
420  filter_control_status_u control_mode = ekf.control_status();
421  uint32_t filter_fault_status = ekf.fault_status().value;
422  uint16_t filter_fault_status_16 = filter_fault_status; //FIXME
423  uint8_t mde = 0;
424 
425  // Check the alignment and if GPS is fused
426  if (control_mode.flags.tilt_align && control_mode.flags.yaw_align && control_mode.flags.gps) {
427  mde = 3;
428  } else if (control_mode.flags.tilt_align && control_mode.flags.yaw_align) {
429  mde = 4;
430  } else {
431  mde = 2;
432  }
433 
434  // Check if there is a covariance error
435  if (filter_fault_status) {
436  mde = 6;
437  }
438 
439  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status_16);
440 }
441 
442 static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
443 {
444  float tas;
445  Vector2f wind = ekf.getWindVelocity();
446  uint8_t flags = 0x5;
447  float f_zero = 0;
448 
449  ekf.get_true_airspeed(&tas);
450 
451  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind(1), &wind(0), &f_zero, &tas);
452 }
453 
454 static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
455 {
456  Vector3f accel_bias = ekf.getAccelBias();
457  Vector3f gyro_bias = ekf.getGyroBias();
458  Vector3f mag_bias = ekf.getMagBias();
459 
460  pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias(0), &accel_bias(1), &accel_bias(2),
461  &gyro_bias(0), &gyro_bias(1), &gyro_bias(2), &mag_bias(0), &mag_bias(1), &mag_bias(2));
462 }
463 #endif
464 
465 /* Initialize the EKF */
466 void ins_ekf2_init(void)
467 {
468  /* Get the ekf parameters */
469  ekf_params = ekf.getParamHandle();
470  ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
471  ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
472  ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
473 
474  /* Set specific noise levels */
475  ekf_params->accel_bias_p_noise = 3.0e-3f;
476  ekf_params->gps_vel_noise = INS_EKF2_GPS_V_NOISE;
477  ekf_params->gps_pos_noise = INS_EKF2_GPS_P_NOISE;
478  ekf_params->baro_noise = INS_EKF2_BARO_NOISE;
479 
480  /* Set optical flow parameters */
481  ekf_params->flow_qual_min = INS_EKF2_MIN_FLOW_QUALITY;
482  ekf_params->flow_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
483  ekf_params->range_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
484  ekf_params->flow_noise = INS_EKF2_FLOW_NOISE;
485  ekf_params->flow_noise_qual_min = INS_EKF2_FLOW_NOISE_QMIN;
486  ekf_params->flow_innov_gate = INS_EKF2_FLOW_INNOV_GATE;
487 
488  /* Set the IMU position relative from the CoG in xyz (m) */
489  ekf_params->imu_pos_body = {
493  };
494 
495  /* Set the GPS position relative from the CoG in xyz (m) */
496  ekf_params->gps_pos_body = {
500  };
501 
502  /* Set flow sensor offset from CoG position in xyz (m) */
503  ekf_params->flow_pos_body = {
507  };
508 
509  /* Set range as default AGL measurement if possible */
510  ekf_params->range_aid = INS_EKF2_RANGE_MAIN_AGL;
511 
512  /* Initialize struct */
513  ekf2.ltp_stamp = 0;
514  ekf2.accel_stamp = 0;
515  ekf2.gyro_stamp = 0;
516  ekf2.flow_stamp = 0;
517  ekf2.gyro_valid = false;
518  ekf2.accel_valid = false;
519  ekf2.got_imu_data = false;
521  ekf2.temp = 20.0f; // Default temperature of 20 degrees celcius
522  ekf2.qnh = 1013.25f; // Default atmosphere
523 
524  /* Initialize the range sensor limits */
525  ekf.set_rangefinder_limits(INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE);
526 
527  /* Initialize the flow sensor limits */
529 
530  /* Initialize the origin from flight plan */
531 #if USE_INS_NAV_INIT
532  if(ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0 + NAV_MSL0)*1e-3))
533  {
534  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
535  llh_nav0.lat = NAV_LAT0;
536  llh_nav0.lon = NAV_LON0;
537  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
538  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
539 
540  ltp_def_from_lla_i(&ekf2.ltp_def, &llh_nav0);
541  ekf2.ltp_def.hmsl = NAV_ALT0;
543 
544  /* update local ENU coordinates of global waypoints */
546 
547  ekf2.ltp_stamp = 1;
548  }
549 #endif
550 
551 #if PERIODIC_TELEMETRY
557  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
560 #endif
561 
562  /*
563  * Subscribe to scaled IMU measurements and attach callbacks
564  */
565  AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
566  AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
567  AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
568  AbiBindMsgIMU_GYRO_INT32(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
569  AbiBindMsgIMU_ACCEL_INT32(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
570  AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
571  AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
572  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
573  AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
574 }
575 
576 /* Update the INS state */
577 void ins_ekf2_update(void)
578 {
579  /* Set EKF settings */
580  ekf.set_in_air_status(autopilot_in_flight());
581 
582  /* Update the EKF */
583  if (ekf2.got_imu_data) {
584  // Update the EKF but ignore the response and also copy the faster intermediate filter
585  ekf.update();
586  filter_control_status_u control_status = ekf.control_status();
587 
588  // Only publish position after successful alignment
589  if (control_status.flags.tilt_align) {
590  /* Get the position */
591  const Vector3f pos_f{ekf.getPosition()};
592  struct NedCoor_f pos;
593  pos.x = pos_f(0);
594  pos.y = pos_f(1);
595  pos.z = pos_f(2);
596 
597  // Publish to the state
598  stateSetPositionNed_f(&pos);
599 
600  /* Get the velocity in NED frame */
601  const Vector3f vel_f{ekf.getVelocity()};
602  struct NedCoor_f speed;
603  speed.x = vel_f(0);
604  speed.y = vel_f(1);
605  speed.z = vel_f(2);
606 
607  // Publish to state
608  stateSetSpeedNed_f(&speed);
609 
610  /* Get the accelerations in NED frame */
611  const Vector3f vel_deriv_f{ekf.getVelocityDerivative()};
612  struct NedCoor_f accel;
613  accel.x = vel_deriv_f(0);
614  accel.y = vel_deriv_f(1);
615  accel.z = vel_deriv_f(2);
616 
617  // Publish to state
618  stateSetAccelNed_f(&accel);
619 
620  /* Get local origin */
621  // Position of local NED origin in GPS / WGS84 frame
622  double ekf_origin_lat, ekf_origin_lon;
623  float ref_alt;
624  struct LlaCoor_i lla_ref;
625  uint64_t origin_time;
626 
627  // Only update the origin when the state estimator has updated the origin
628  bool ekf_origin_valid = ekf.getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
629  if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
630  lla_ref.lat = ekf_origin_lat * 1e7; // WGS-84 lat
631  lla_ref.lon = ekf_origin_lon * 1e7; // WGS-84 lon
632  lla_ref.alt = ref_alt * 1e3 + wgs84_ellipsoid_to_geoid_i(lla_ref.lat, lla_ref.lon); // WGS-84 height
633  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
634  ekf2.ltp_def.hmsl = ref_alt * 1e3;
636 
637  /* update local ENU coordinates of global waypoints */
639 
640  ekf2.ltp_stamp = origin_time;
641  }
642  }
643  }
644 
645 #if defined SITL && USE_NPS
646  if (nps_bypass_ins) {
648  }
649 #endif
650 
651  ekf2.got_imu_data = false;
652 }
653 
655 {
656  ekf_params->mag_fusion_type = ekf2.mag_fusion_type = unk;
657 }
658 
660 {
661  if (mode) {
662  ekf_params->fusion_mode = ekf2.fusion_mode = (MASK_USE_OF | MASK_USE_GPSYAW);
663  } else {
665  }
666 }
667 
669  if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
670 
671  extVisionSample sample;
672  sample.time_us = get_sys_time_usec(); //FIXME
673  sample.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
674  sample.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
675  sample.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
676  sample.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
677  sample.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
678  sample.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
679  sample.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
680  sample.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
681  sample.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
682  sample.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
683  sample.posVar.setAll(INS_EKF2_EVP_NOISE);
684  sample.velCov = matrix::eye<float, 3>() * INS_EKF2_EVV_NOISE;
685  sample.angVar = INS_EKF2_EVA_NOISE;
686  sample.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
687 
688  ekf.setExtVisionData(sample);
689 }
690 
691 void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf) {
692 
693 }
694 
699 {
700  imuSample imu_sample = {};
701  imu_sample.time_us = stamp;
702  imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
703  imu_sample.delta_ang = Vector3f{ekf2.gyro.p, ekf2.gyro.q, ekf2.gyro.r} * imu_sample.delta_ang_dt;
704  imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
705  imu_sample.delta_vel = Vector3f{ekf2.accel.x, ekf2.accel.y, ekf2.accel.z} * imu_sample.delta_vel_dt;
706  ekf.setIMUData(imu_sample);
707 
708  if (ekf.attitude_valid()) {
709  // Calculate the quaternion
710  struct FloatQuat ltp_to_body_quat;
711  const Quatf att_q{ekf.calculate_quaternion()};
712  ltp_to_body_quat.qi = att_q(0);
713  ltp_to_body_quat.qx = att_q(1);
714  ltp_to_body_quat.qy = att_q(2);
715  ltp_to_body_quat.qz = att_q(3);
716 
717  // Publish it to the state
718  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
719 
720  /* Check the quaternion reset state */
721  float delta_q_reset[4];
722  uint8_t quat_reset_counter;
723  ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
724 
725 #ifndef NO_RESET_UPDATE_SETPOINT_HEADING
726 
727  if (ekf2.quat_reset_counter < quat_reset_counter) {
728  float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
729 #if defined STABILIZATION_ATTITUDE_TYPE_INT
731 #else
732  stab_att_sp_euler.psi += psi;
733 #endif
734  guidance_h.sp.heading += psi;
735  guidance_h.rc_sp.psi += psi;
739  ekf2.quat_reset_counter = quat_reset_counter;
740  }
741 #endif
742 
743  /* Get in-run gyro bias */
744  struct FloatRates body_rates;
745  Vector3f gyro_bias{ekf.getGyroBias()};
746  body_rates.p = ekf2.gyro.p - gyro_bias(0);
747  body_rates.q = ekf2.gyro.q - gyro_bias(1);
748  body_rates.r = ekf2.gyro.r - gyro_bias(2);
749 
750  // Publish it to the state
751  stateSetBodyRates_f(&body_rates);
752 
753  /* Get the in-run acceleration bias */
754  struct Int32Vect3 accel;
755  Vector3f accel_bias{ekf.getAccelBias()};
756  accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias(0));
757  accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias(1));
758  accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias(2));
759 
760  // Publish it to the state
761  stateSetAccelBody_i(&accel);
762  }
763 
764  ekf2.gyro_valid = false;
765  ekf2.accel_valid = false;
766  ekf2.got_imu_data = true;
767 }
768 
769 /* Update INS based on Baro information */
770 static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
771 {
772  baroSample sample;
773  sample.time_us = stamp;
774 
775  // Calculate the air density
776  float rho = pprz_isa_density_of_pressure(pressure, ekf2.temp);
777  ekf.set_air_density(rho);
778 
779  // Calculate the height above mean sea level based on pressure
780  sample.hgt = pprz_isa_height_of_pressure_full(pressure, ekf2.qnh * 100.0f);
781  ekf.setBaroData(sample);
782 }
783 
784 /* Save the latest temperature measurement for air density calculations */
785 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
786 {
787  ekf2.temp = temp;
788 }
789 
790 /* Update INS based on AGL information */
791 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
792 {
793  rangeSample sample;
794  sample.time_us = stamp;
795  sample.rng = distance;
796  sample.quality = -1;
797 
798  ekf.setRangeData(sample);
799 }
800 
801 /* Update INS based on Gyro information */
802 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
803  uint32_t stamp, struct Int32Rates *gyro)
804 {
805  FloatRates imu_rate;
806  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
807 
808  // Convert Gyro information to float
809  RATES_FLOAT_OF_BFP(imu_rate, *gyro);
810 
811  // Rotate with respect to Body To IMU
812  float_rmat_transp_ratemult(&ekf2.gyro, body_to_imu_rmat, &imu_rate);
813 
814  // Calculate the Gyro interval
815  if (ekf2.gyro_stamp > 0) {
816  ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
817  ekf2.gyro_valid = true;
818  }
819  ekf2.gyro_stamp = stamp;
820 
821  /* When Gyro and accelerometer are valid enter it into the EKF */
822  if (ekf2.gyro_valid && ekf2.accel_valid) {
824  }
825 }
826 
827 /* Update INS based on Accelerometer information */
828 static void accel_cb(uint8_t sender_id __attribute__((unused)),
829  uint32_t stamp, struct Int32Vect3 *accel)
830 {
831  struct FloatVect3 accel_imu;
832  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
833 
834  // Convert Accelerometer information to float
835  ACCELS_FLOAT_OF_BFP(accel_imu, *accel);
836 
837  // Rotate with respect to Body To IMU
838  float_rmat_transp_vmult(&ekf2.accel, body_to_imu_rmat, &accel_imu);
839 
840  // Calculate the Accelerometer interval
841  if (ekf2.accel_stamp > 0) {
842  ekf2.accel_dt = stamp - ekf2.accel_stamp;
843  ekf2.accel_valid = true;
844  }
845  ekf2.accel_stamp = stamp;
846 
847  /* When Gyro and accelerometer are valid enter it into the EKF */
848  if (ekf2.gyro_valid && ekf2.accel_valid) {
850  }
851 }
852 
853 /* Update INS based on Magnetometer information */
854 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
855  uint32_t stamp,
856  struct Int32Vect3 *mag)
857 {
858  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
859  struct FloatVect3 mag_gauss, mag_body;
860  magSample sample;
861  sample.time_us = stamp;
862 
863  // Convert Magnetometer information to float and to radius 0.2f
864  MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
865  mag_gauss.x *= 0.4f;
866  mag_gauss.y *= 0.4f;
867  mag_gauss.z *= 0.4f;
868 
869  // Rotate with respect to Body To IMU
870  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_gauss);
871 
872  // Publish information to the EKF
873  sample.mag(0) = mag_body.x;
874  sample.mag(1) = mag_body.y;
875  sample.mag(2) = mag_body.z;
876 
877  ekf.setMagData(sample);
878  ekf2.got_imu_data = true;
879 }
880 
881 /* Update INS based on GPS information */
882 static void gps_cb(uint8_t sender_id __attribute__((unused)),
883  uint32_t stamp,
884  struct GpsState *gps_s)
885 {
886  gps_message gps_msg = {};
887  gps_msg.time_usec = stamp;
888  gps_msg.lat = gps_s->lla_pos.lat;
889  gps_msg.lon = gps_s->lla_pos.lon;
890  gps_msg.alt = gps_s->hmsl;
891 #if INS_EKF2_GPS_COURSE_YAW
892  gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
893  gps_msg.yaw_offset = 0;
894 #else
895  gps_msg.yaw = NAN;
896  gps_msg.yaw_offset = NAN;
897 #endif
898  gps_msg.fix_type = gps_s->fix;
899  gps_msg.eph = gps_s->hacc / 100.0;
900  gps_msg.epv = gps_s->vacc / 100.0;
901  gps_msg.sacc = gps_s->sacc / 100.0;
902  gps_msg.vel_m_s = gps_s->gspeed / 100.0;
903  gps_msg.vel_ned(0) = (gps_s->ned_vel.x) / 100.0;
904  gps_msg.vel_ned(1) = (gps_s->ned_vel.y) / 100.0;
905  gps_msg.vel_ned(2) = (gps_s->ned_vel.z) / 100.0;
906  gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
907  gps_msg.nsats = gps_s->num_sv;
908  gps_msg.pdop = gps_s->pdop;
909 
910  ekf.setGpsData(gps_msg);
911 }
912 
913 /* Save the Body to IMU information */
914 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
915  struct FloatQuat *q_b2i_f)
916 {
918 }
919 
920 /* Update INS based on Optical Flow information */
921 static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
922  uint32_t stamp,
923  int32_t flow_x,
924  int32_t flow_y,
925  int32_t flow_der_x __attribute__((unused)),
926  int32_t flow_der_y __attribute__((unused)),
927  float quality,
928  float size_divergence __attribute__((unused)))
929 {
930  flowSample sample;
931  sample.time_us = stamp;
932 
933  // Wait for two measurements in order to integrate
934  if (ekf2.flow_stamp <= 0) {
935  ekf2.flow_stamp = stamp;
936  return;
937  }
938 
939  // Calculate the timestamp
940  sample.dt = (stamp - ekf2.flow_stamp);
941  ekf2.flow_stamp = stamp;
942 
943  /* Build integrated flow and gyro messages for filter
944  NOTE: pure rotations should result in same flow_x and
945  gyro_roll and same flow_y and gyro_pitch */
946  Vector2f flowdata;
947  flowdata(0) = RadOfDeg(flow_y) * (1e-6 *
948  sample.dt); // INTEGRATED FLOW AROUND Y AXIS (RIGHT -X, LEFT +X)
949  flowdata(1) = - RadOfDeg(flow_x) * (1e-6 *
950  sample.dt); // INTEGRATED FLOW AROUND X AXIS (FORWARD +Y, BACKWARD -Y)
951 
952  sample.quality = quality; // quality indicator between 0 and 255
953  sample.flow_xy_rad =
954  flowdata; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
955  sample.gyro_xyz = Vector3f{NAN, NAN, NAN}; // measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
956 
957  // Update the optical flow data based on the callback
958  ekf.setOpticalFlowData(sample);
959 }
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
ekf2_t::gyro
struct FloatRates gyro
Last gyroscope measurements.
Definition: ins_ekf2.h:47
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_ekf2.cpp:882
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
INS_EKF2_FLOW_POS_Z
#define INS_EKF2_FLOW_POS_Z
Definition: ins_ekf2.cpp:221
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
pprz_isa_density_of_pressure
static float pprz_isa_density_of_pressure(float pressure, float temp)
Get the air density (rho) from a given pressure and temperature.
Definition: pprz_isa.h:193
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
INS_EKF2_GPS_CHECK_MASK
#define INS_EKF2_GPS_CHECK_MASK
The EKF2 GPS checks before initialization.
Definition: ins_ekf2.cpp:79
INS_EKF2_FLOW_POS_Y
#define INS_EKF2_FLOW_POS_Y
Definition: ins_ekf2.cpp:215
ins_ekf2_change_param
void ins_ekf2_change_param(int32_t unk)
Definition: ins_ekf2.cpp:654
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
gyro_ev
static abi_event gyro_ev
Definition: ins_ekf2.cpp:283
uint64_t
unsigned long long uint64_t
Definition: vl53l1_types.h:72
waypoints_localize_all
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
INS_EKF2_GPS_POS_Z
#define INS_EKF2_GPS_POS_Z
Definition: ins_ekf2.cpp:185
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
send_ahrs_bias
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:454
send_ins_z
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:340
NedCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:66
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
optical_flow_ev
static abi_event optical_flow_ev
Definition: ins_ekf2.cpp:288
wgs84_ellipsoid_to_geoid_i
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
Definition: pprz_geodetic_wgs84.h:81
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
INS_EKF2_MAX_FLOW_RATE
#define INS_EKF2_MAX_FLOW_RATE
Definition: ins_ekf2.cpp:203
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
GpsState
data structure for GPS information
Definition: gps.h:87
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:219
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_ekf2.cpp:914
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
INS_EKF2_GPS_P_NOISE
#define INS_EKF2_GPS_P_NOISE
Definition: ins_ekf2.cpp:269
ekf2_t::quat_reset_counter
uint8_t quat_reset_counter
Amount of quaternion resets from the EKF2.
Definition: ins_ekf2.h:54
HorizontalGuidanceSetpoint::heading
float heading
Definition: guidance_h.h:76
GpsState::vacc
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:102
nps_bypass_ins
bool nps_bypass_ins
Definition: nps_autopilot_fixedwing.c:54
ekf2_t::accel_stamp
uint32_t accel_stamp
Accelerometer last abi message timestamp.
Definition: ins_ekf2.h:43
ekf2_t::gyro_stamp
uint32_t gyro_stamp
Gyroscope last abi message timestamp.
Definition: ins_ekf2.h:41
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_ekf2.cpp:770
INS_EKF2_RANGE_MAIN_AGL
#define INS_EKF2_RANGE_MAIN_AGL
If enabled uses radar sensor as primary AGL source, if possible.
Definition: ins_ekf2.cpp:97
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:113
ekf2_t::accel
struct FloatVect3 accel
Last accelerometer measurements.
Definition: ins_ekf2.h:48
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:360
nps_autopilot.h
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:417
INS_EKF2_FLOW_SENSOR_DELAY
#define INS_EKF2_FLOW_SENSOR_DELAY
Definition: ins_ekf2.cpp:191
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
ekf2_t::qnh
float qnh
QNH value in hPa.
Definition: ins_ekf2.h:53
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
INS_EKF2_GPS_POS_Y
#define INS_EKF2_GPS_POS_Y
Definition: ins_ekf2.cpp:179
INS_EKF2_GPS_POS_X
#define INS_EKF2_GPS_POS_X
Definition: ins_ekf2.cpp:173
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:289
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_float.c:120
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
send_ins
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:313
GpsState::hacc
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:101
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
pos_z
static const float pos_z[]
Definition: dw1000_arduino.c:198
temperature_ev
static abi_event temperature_ev
Definition: ins_ekf2.cpp:281
INS_EKF2_GPS_V_NOISE
#define INS_EKF2_GPS_V_NOISE
Definition: ins_ekf2.cpp:263
send_ins_ekf2
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:370
INS_EKF2_EVA_NOISE
#define INS_EKF2_EVA_NOISE
Definition: ins_ekf2.cpp:257
ins_ekf2_remove_gps
void ins_ekf2_remove_gps(int32_t mode)
Definition: ins_ekf2.cpp:659
ekf2_t::accel_dt
uint32_t accel_dt
Accelerometer delta timestamp between abi messages.
Definition: ins_ekf2.h:44
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:211
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
INS_EKF2_ACCEL_ID
#define INS_EKF2_ACCEL_ID
Definition: ins_ekf2.cpp:131
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
ekf2_t::ltp_def
struct LtpDef_i ltp_def
Latest LTP definition from the quat_reset_counterEKF2.
Definition: ins_ekf2.h:56
ekf2_t::ltp_stamp
uint64_t ltp_stamp
Last LTP change timestamp from the EKF2.
Definition: ins_ekf2.h:55
Int32Vect3
Definition: pprz_algebra_int.h:88
INS_EKF2_IMU_POS_X
#define INS_EKF2_IMU_POS_X
Definition: ins_ekf2.cpp:155
sys_time.h
Architecture independent timing functions.
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:221
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
INS_EKF2_MAG_ID
#define INS_EKF2_MAG_ID
Definition: ins_ekf2.cpp:137
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
ins_ekf2.h
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
pprz_isa_height_of_pressure_full
static float pprz_isa_height_of_pressure_full(float pressure, float ref_p)
Get relative altitude from pressure (using full equation).
Definition: pprz_isa.h:146
agl_cb
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: ins_ekf2.cpp:791
ins_ekf2_parse_EXTERNAL_POSE_SMALL
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf)
Definition: ins_ekf2.cpp:691
mag_ev
static abi_event mag_ev
Definition: ins_ekf2.cpp:285
ekf2_t::mag_fusion_type
int32_t mag_fusion_type
Definition: ins_ekf2.h:60
autopilot.h
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
INS_EKF2_SONAR_MIN_RANGE
#define INS_EKF2_SONAR_MIN_RANGE
Default AGL sensor minimum range.
Definition: ins_ekf2.cpp:85
INS_EKF2_IMU_POS_Z
#define INS_EKF2_IMU_POS_Z
Definition: ins_ekf2.cpp:167
INS_EKF2_IMU_POS_Y
#define INS_EKF2_IMU_POS_Y
Definition: ins_ekf2.cpp:161
HorizontalGuidance::sp
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
ins_ekf2_publish_attitude
static void ins_ekf2_publish_attitude(uint32_t stamp)
Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading.
Definition: ins_ekf2.cpp:698
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
INS_EKF2_FLOW_POS_X
#define INS_EKF2_FLOW_POS_X
Definition: ins_ekf2.cpp:209
INS_EKF2_EVV_NOISE
#define INS_EKF2_EVV_NOISE
Definition: ins_ekf2.cpp:251
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
ekf_params
static parameters * ekf_params
The EKF parameters.
Definition: ins_ekf2.cpp:307
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
ekf2_t::fusion_mode
int32_t fusion_mode
Definition: ins_ekf2.h:61
INS_EKF2_FLOW_NOISE_QMIN
#define INS_EKF2_FLOW_NOISE_QMIN
Definition: ins_ekf2.cpp:233
gps_ev
static abi_event gps_ev
Definition: ins_ekf2.cpp:286
send_wind_info_ret
static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:442
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ins_ekf2.cpp:287
ekf2_t::body_to_imu
struct OrientationReps body_to_imu
Body to IMU rotation.
Definition: ins_ekf2.h:57
INS_EKF2_GPS_ID
#define INS_EKF2_GPS_ID
Definition: ins_ekf2.cpp:143
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
INS_EKF2_FLOW_INNOV_GATE
#define INS_EKF2_FLOW_INNOV_GATE
Definition: ins_ekf2.cpp:239
sim_overwrite_ins
void sim_overwrite_ins(void)
Definition: nps_autopilot_fixedwing.c:247
INS_EKF2_MIN_FLOW_QUALITY
#define INS_EKF2_MIN_FLOW_QUALITY
Definition: ins_ekf2.cpp:197
INS_EKF2_TEMPERATURE_ID
#define INS_EKF2_TEMPERATURE_ID
default temperature sensor to use in INS
Definition: ins_ekf2.cpp:113
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ins_ekf2.cpp:802
INS_EKF2_AGL_ID
#define INS_EKF2_AGL_ID
default AGL sensor to use in INS
Definition: ins_ekf2.cpp:119
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
agl_ev
static abi_event agl_ev
Definition: ins_ekf2.cpp:282
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_euler_float.c:136
ekf2
struct ekf2_t ekf2
Local EKF2 status structure.
Definition: ins_ekf2.cpp:308
stateSetPositionNed_f
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
guidance_h_read_rc
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:308
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
ins_ekf2_parse_EXTERNAL_POSE
void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf)
Definition: ins_ekf2.cpp:668
INS_EKF2_FLOW_NOISE
#define INS_EKF2_FLOW_NOISE
Definition: ins_ekf2.cpp:227
INS_EKF2_SONAR_MAX_RANGE
#define INS_EKF2_SONAR_MAX_RANGE
Default AGL sensor maximum range.
Definition: ins_ekf2.cpp:91
INS_EKF2_BARO_NOISE
#define INS_EKF2_BARO_NOISE
Definition: ins_ekf2.cpp:275
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ins_ekf2.cpp:854
float_rmat_transp_ratemult
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_float.c:160
ekf
static Ekf ekf
EKF class itself.
Definition: ins_ekf2.cpp:306
AHRS_COMP_ID_EKF2
#define AHRS_COMP_ID_EKF2
Definition: ahrs.h:44
INS_EKF2_GYRO_ID
#define INS_EKF2_GYRO_ID
Definition: ins_ekf2.cpp:125
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
send_ins_ekf2_ext
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:403
INS_EKF2_VDIST_SENSOR_TYPE
#define INS_EKF2_VDIST_SENSOR_TYPE
The EKF2 primary vertical distance sensor type.
Definition: ins_ekf2.cpp:73
ekf2_t::gyro_valid
bool gyro_valid
If we received a gyroscope measurement.
Definition: ins_ekf2.h:49
INS_EKF2_FUSION_MODE
#define INS_EKF2_FUSION_MODE
For SITL and NPS we need special includes.
Definition: ins_ekf2.cpp:67
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
guidance_h
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
INS_EKF2_BARO_ID
#define INS_EKF2_BARO_ID
default barometer to use in INS
Definition: ins_ekf2.cpp:106
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
stab_att_sp_euler
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_euler_float.c:45
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
temperature_cb
static void temperature_cb(uint8_t sender_id, float temp)
Definition: ins_ekf2.cpp:785
ekf2_t
Definition: ins_ekf2.h:40
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
mode
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
waypoints.h
ekf2_t::gyro_dt
uint32_t gyro_dt
Gyroscope delta timestamp between abi messages.
Definition: ins_ekf2.h:42
ekf2_t::accel_valid
bool accel_valid
If we received a acceleration measurement.
Definition: ins_ekf2.h:50
ekf2_t::flow_stamp
uint32_t flow_stamp
Optic flow last abi message timestamp.
Definition: ins_ekf2.h:45
optical_flow_cb
static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
Definition: ins_ekf2.cpp:921
INS_EKF2_EVP_NOISE
#define INS_EKF2_EVP_NOISE
Definition: ins_ekf2.cpp:245
baro_ev
static abi_event baro_ev
Definition: ins_ekf2.cpp:280
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:57
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
ekf2_t::got_imu_data
bool got_imu_data
If we received valid IMU data (any sensor)
Definition: ins_ekf2.h:58
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
ins_ekf2_init
void ins_ekf2_init(void)
Definition: ins_ekf2.cpp:466
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
pprz_geodetic_wgs84.h
WGS-84 Geoid Heights.
HorizontalGuidance::rc_sp
struct FloatEulers rc_sp
Definition: guidance_h.h:106
INS_EKF2_OF_ID
#define INS_EKF2_OF_ID
Definition: ins_ekf2.cpp:149
ins_ekf2_update
void ins_ekf2_update(void)
Definition: ins_ekf2.cpp:577
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_ekf2.cpp:828
ekf2_t::temp
float temp
Latest temperature measurement in degrees celcius.
Definition: ins_ekf2.h:52
accel_ev
static abi_event accel_ev
Definition: ins_ekf2.cpp:284