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