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;
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 
459 static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
460 {
461  struct Int32Quat ltp_to_body_quat;
462  const Quatf att_q{ekf.calculate_quaternion()};
463  ltp_to_body_quat.qi = QUAT1_BFP_OF_REAL(att_q(0));
464  ltp_to_body_quat.qx = QUAT1_BFP_OF_REAL(att_q(1));
465  ltp_to_body_quat.qy = QUAT1_BFP_OF_REAL(att_q(2));
466  ltp_to_body_quat.qz = QUAT1_BFP_OF_REAL(att_q(3));
467  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
468  float foo = 0.f;
469  uint8_t ahrs_id = 1; // generic
470  pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
471  &foo,
472  &ltp_to_body_quat.qi,
473  &ltp_to_body_quat.qx,
474  &ltp_to_body_quat.qy,
475  &ltp_to_body_quat.qz,
476  &(quat->qi),
477  &(quat->qx),
478  &(quat->qy),
479  &(quat->qz),
480  &ahrs_id);
481 }
482 
483 #endif
484 
485 /* Initialize the EKF */
486 void ins_ekf2_init(void)
487 {
488  /* Get the ekf parameters */
489  ekf_params = ekf.getParamHandle();
490  ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
491  ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
492  ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
493 
494  /* Set specific noise levels */
495  ekf_params->accel_bias_p_noise = 3.0e-3f;
496  ekf_params->gps_vel_noise = INS_EKF2_GPS_V_NOISE;
497  ekf_params->gps_pos_noise = INS_EKF2_GPS_P_NOISE;
498  ekf_params->baro_noise = INS_EKF2_BARO_NOISE;
499 
500  /* Set optical flow parameters */
501  ekf_params->flow_qual_min = INS_EKF2_MIN_FLOW_QUALITY;
502  ekf_params->flow_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
503  ekf_params->range_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
504  ekf_params->flow_noise = INS_EKF2_FLOW_NOISE;
505  ekf_params->flow_noise_qual_min = INS_EKF2_FLOW_NOISE_QMIN;
506  ekf_params->flow_innov_gate = INS_EKF2_FLOW_INNOV_GATE;
507 
508  /* Set the IMU position relative from the CoG in xyz (m) */
509  ekf_params->imu_pos_body = {
513  };
514 
515  /* Set the GPS position relative from the CoG in xyz (m) */
516  ekf_params->gps_pos_body = {
520  };
521 
522  /* Set flow sensor offset from CoG position in xyz (m) */
523  ekf_params->flow_pos_body = {
527  };
528 
529  /* Set range as default AGL measurement if possible */
530  ekf_params->range_aid = INS_EKF2_RANGE_MAIN_AGL;
531 
532  /* Initialize struct */
533  ekf2.ltp_stamp = 0;
534  ekf2.flow_stamp = 0;
535  ekf2.gyro_valid = false;
536  ekf2.accel_valid = false;
537  ekf2.got_imu_data = false;
539  ekf2.temp = 20.0f; // Default temperature of 20 degrees celcius
540  ekf2.qnh = 1013.25f; // Default atmosphere
541 
542  /* Initialize the range sensor limits */
543  ekf.set_rangefinder_limits(INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE);
544 
545  /* Initialize the flow sensor limits */
547 
548  /* Initialize the origin from flight plan */
549 #if USE_INS_NAV_INIT
550  if(ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0)*1e-3)) // EKF2 works HMSL
551  {
552  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
553  llh_nav0.lat = NAV_LAT0;
554  llh_nav0.lon = NAV_LON0;
555  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
556  llh_nav0.alt = NAV_ALT0 + NAV_MSL0; // in millimeters above WGS84 reference ellipsoid
557 
558  ltp_def_from_lla_i(&ekf2.ltp_def, &llh_nav0);
559  ekf2.ltp_def.hmsl = NAV_ALT0;
561 
562  /* update local ENU coordinates of global waypoints */
564 
565  ekf2.ltp_stamp = 1;
566  }
567 #endif
568 
569 #if PERIODIC_TELEMETRY
575  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
578  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_ahrs_quat);
579 #endif
580 
581  /*
582  * Subscribe to scaled IMU measurements and attach callbacks
583  */
584  AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
585  AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
586  AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
587  AbiBindMsgIMU_GYRO_INT(INS_EKF2_GYRO_ID, &gyro_int_ev, gyro_int_cb);
588  AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
589  AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
590  AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
591  AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
592 }
593 
595 {
596 #if USE_GPS
597  if (GpsFixValid()) {
598  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
599  if (ekf.setEkfGlobalOrigin(lla_pos.lat*1e-7, lla_pos.lon*1e-7, gps.hmsl*1e-3)) {
600  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_pos);
603  }
604  }
605 #endif
606 }
607 
608 /* Update the INS state */
609 void ins_ekf2_update(void)
610 {
611  /* Set EKF settings */
612  ekf.set_in_air_status(autopilot_in_flight());
613 
614  /* Update the EKF */
615  if (ekf2.got_imu_data) {
616  // Update the EKF but ignore the response and also copy the faster intermediate filter
617  ekf.update();
618  filter_control_status_u control_status = ekf.control_status();
619 
620  // Only publish position after successful alignment
621  if (control_status.flags.tilt_align) {
622  /* Get the position */
623  const Vector3f pos_f{ekf.getPosition()};
624  struct NedCoor_f pos;
625  pos.x = pos_f(0);
626  pos.y = pos_f(1);
627  pos.z = pos_f(2);
628 
629  // Publish to the state
630  stateSetPositionNed_f(&pos);
631 
632  /* Get the velocity in NED frame */
633  const Vector3f vel_f{ekf.getVelocity()};
634  struct NedCoor_f speed;
635  speed.x = vel_f(0);
636  speed.y = vel_f(1);
637  speed.z = vel_f(2);
638 
639  // Publish to state
640  stateSetSpeedNed_f(&speed);
641 
642  /* Get the accelerations in NED frame */
643  const Vector3f vel_deriv_f{ekf.getVelocityDerivative()};
644  struct NedCoor_f accel;
645  accel.x = vel_deriv_f(0);
646  accel.y = vel_deriv_f(1);
647  accel.z = vel_deriv_f(2);
648 
649  // Publish to state
650  stateSetAccelNed_f(&accel);
651 
652  /* Get local origin */
653  // Position of local NED origin in GPS / WGS84 frame
654  double ekf_origin_lat, ekf_origin_lon;
655  float ref_alt;
656  struct LlaCoor_i lla_ref;
657  uint64_t origin_time;
658 
659  // Only update the origin when the state estimator has updated the origin
660  bool ekf_origin_valid = ekf.getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
661  if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
662  lla_ref.lat = ekf_origin_lat * 1e7; // WGS-84 lat
663  lla_ref.lon = ekf_origin_lon * 1e7; // WGS-84 lon
664  lla_ref.alt = ref_alt * 1e3 + wgs84_ellipsoid_to_geoid_i(lla_ref.lat, lla_ref.lon); // in millimeters above WGS84 reference ellipsoid (ref_alt is in HMSL)
665  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
666  ekf2.ltp_def.hmsl = ref_alt * 1e3;
668 
669  /* update local ENU coordinates of global waypoints */
671 
672  ekf2.ltp_stamp = origin_time;
673  }
674  }
675  }
676 
677 #if defined SITL && USE_NPS
678  if (nps_bypass_ins) {
680  }
681 #endif
682 
683  ekf2.got_imu_data = false;
684 }
685 
687 {
688  ekf_params->mag_fusion_type = ekf2.mag_fusion_type = unk;
689 }
690 
692 {
693  if (mode) {
694  ekf_params->fusion_mode = ekf2.fusion_mode = (MASK_USE_OF | MASK_USE_GPSYAW);
695  } else {
697  }
698 }
699 
701  if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
702 
703  extVisionSample sample;
704  sample.time_us = get_sys_time_usec(); //FIXME
705  sample.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
706  sample.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
707  sample.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
708  sample.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
709  sample.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
710  sample.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
711  sample.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
712  sample.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
713  sample.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
714  sample.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
715  sample.posVar.setAll(INS_EKF2_EVP_NOISE);
716  sample.velCov = matrix::eye<float, 3>() * INS_EKF2_EVV_NOISE;
717  sample.angVar = INS_EKF2_EVA_NOISE;
718  sample.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
719 
720  ekf.setExtVisionData(sample);
721 }
722 
723 void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf) {
724 
725 }
726 
731 {
732  imuSample imu_sample = {};
733  imu_sample.time_us = stamp;
734  imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
735  imu_sample.delta_ang = Vector3f{ekf2.delta_gyro.p, ekf2.delta_gyro.q, ekf2.delta_gyro.r};
736  imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
737  imu_sample.delta_vel = Vector3f{ekf2.delta_accel.x, ekf2.delta_accel.y, ekf2.delta_accel.z};
738  ekf.setIMUData(imu_sample);
739 
740  if (ekf.attitude_valid()) {
741  // Calculate the quaternion
742  struct FloatQuat ltp_to_body_quat;
743  const Quatf att_q{ekf.calculate_quaternion()};
744  ltp_to_body_quat.qi = att_q(0);
745  ltp_to_body_quat.qx = att_q(1);
746  ltp_to_body_quat.qy = att_q(2);
747  ltp_to_body_quat.qz = att_q(3);
748 
749  // Publish it to the state
750  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
751 
752  /* Check the quaternion reset state */
753  float delta_q_reset[4];
754  uint8_t quat_reset_counter;
755  ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
756 
757 #ifndef NO_RESET_UPDATE_SETPOINT_HEADING
758 
759  if (ekf2.quat_reset_counter < quat_reset_counter) {
760  float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
761 #if defined STABILIZATION_ATTITUDE_TYPE_INT
763 #else
764  stab_att_sp_euler.psi += psi;
765 #endif
766  guidance_h.sp.heading += psi;
767  guidance_h.rc_sp.psi += psi;
768  nav.heading += psi;
771  ekf2.quat_reset_counter = quat_reset_counter;
772  }
773 #endif
774 
775  /* Get in-run gyro bias */
776  struct FloatRates body_rates;
777  Vector3f gyro_bias{ekf.getGyroBias()};
778  body_rates.p = (ekf2.delta_gyro.p / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
779  body_rates.q = (ekf2.delta_gyro.q / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
780  body_rates.r = (ekf2.delta_gyro.r / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
781 
782  // Publish it to the state
783  stateSetBodyRates_f(&body_rates);
784 
785  /* Get the in-run acceleration bias */
786  struct Int32Vect3 accel;
787  Vector3f accel_bias{ekf.getAccelBias()};
788  accel.x = ACCEL_BFP_OF_REAL((ekf2.delta_accel.x / (ekf2.accel_dt * 1e-6f)) - accel_bias(0));
789  accel.y = ACCEL_BFP_OF_REAL((ekf2.delta_accel.y / (ekf2.accel_dt * 1e-6f)) - accel_bias(1));
790  accel.z = ACCEL_BFP_OF_REAL((ekf2.delta_accel.z / (ekf2.accel_dt * 1e-6f)) - accel_bias(2));
791 
792  // Publish it to the state
793  stateSetAccelBody_i(&accel);
794  }
795 
796  ekf2.gyro_valid = false;
797  ekf2.accel_valid = false;
798  ekf2.got_imu_data = true;
799 }
800 
801 /* Update INS based on Baro information */
802 static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
803 {
804  baroSample sample;
805  sample.time_us = stamp;
806 
807  // Calculate the air density
808  float rho = pprz_isa_density_of_pressure(pressure, ekf2.temp);
809  ekf.set_air_density(rho);
810 
811  // Calculate the height above mean sea level based on pressure
812  sample.hgt = pprz_isa_height_of_pressure_full(pressure, ekf2.qnh * 100.0f);
813  ekf.setBaroData(sample);
814 }
815 
816 /* Save the latest temperature measurement for air density calculations */
817 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
818 {
819  ekf2.temp = temp;
820 }
821 
822 /* Update INS based on AGL information */
823 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
824 {
825  rangeSample sample;
826  sample.time_us = stamp;
827  sample.rng = distance;
828  sample.quality = -1;
829 
830  ekf.setRangeData(sample);
831 }
832 
833 /* Update INS based on Gyro information */
834 static void gyro_int_cb(uint8_t __attribute__((unused)) sender_id,
835  uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
836 {
837  // Copy and save the gyro data
838  RATES_COPY(ekf2.delta_gyro, *delta_gyro);
839  ekf2.gyro_dt = dt;
840  ekf2.gyro_valid = true;
841 
842  /* When Gyro and accelerometer are valid enter it into the EKF */
843  if (ekf2.gyro_valid && ekf2.accel_valid) {
845  }
846 }
847 
848 /* Update INS based on Accelerometer information */
849 static void accel_int_cb(uint8_t sender_id __attribute__((unused)),
850  uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
851 {
852  // Copy and save the gyro data
853  VECT3_COPY(ekf2.delta_accel, *delta_accel);
854  ekf2.accel_dt = dt;
855  ekf2.accel_valid = true;
856 
857  /* When Gyro and accelerometer are valid enter it into the EKF */
858  if (ekf2.gyro_valid && ekf2.accel_valid) {
860  }
861 }
862 
863 /* Update INS based on Magnetometer information */
864 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
865  uint32_t stamp,
866  struct Int32Vect3 *mag)
867 {
868  struct FloatVect3 mag_gauss;
869  magSample sample;
870  sample.time_us = stamp;
871 
872  // Convert Magnetometer information to float and to radius 0.2f
873  MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
874  mag_gauss.x *= 0.4f;
875  mag_gauss.y *= 0.4f;
876  mag_gauss.z *= 0.4f;
877 
878  // Publish information to the EKF
879  sample.mag(0) = mag_gauss.x;
880  sample.mag(1) = mag_gauss.y;
881  sample.mag(2) = mag_gauss.z;
882 
883  ekf.setMagData(sample);
884  ekf2.got_imu_data = true;
885 }
886 
887 /* Update INS based on GPS information */
888 static void gps_cb(uint8_t sender_id __attribute__((unused)),
889  uint32_t stamp,
890  struct GpsState *gps_s)
891 {
892  gps_message gps_msg = {};
893  gps_msg.time_usec = stamp;
894  struct LlaCoor_i lla_pos = lla_int_from_gps(gps_s);
895  gps_msg.lat = lla_pos.lat;
896  gps_msg.lon = lla_pos.lon;
897  gps_msg.alt = gps_s->hmsl; // EKF2 works with HMSL
898 #if INS_EKF2_GPS_COURSE_YAW
899  gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
900  gps_msg.yaw_offset = 0;
901 #else
902  gps_msg.yaw = NAN;
903  gps_msg.yaw_offset = NAN;
904 #endif
905  gps_msg.fix_type = gps_s->fix;
906  gps_msg.eph = gps_s->hacc / 100.0;
907  gps_msg.epv = gps_s->vacc / 100.0;
908  gps_msg.sacc = gps_s->sacc / 100.0;
909  gps_msg.vel_m_s = gps_s->gspeed / 100.0;
910  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
911  gps_msg.vel_ned(0) = ned_vel.x;
912  gps_msg.vel_ned(1) = ned_vel.y;
913  gps_msg.vel_ned(2) = ned_vel.z;
914  gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
915  gps_msg.nsats = gps_s->num_sv;
916  gps_msg.pdop = gps_s->pdop;
917 
918  ekf.setGpsData(gps_msg);
919 }
920 
921 /* Update INS based on Optical Flow information */
922 static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
923  uint32_t stamp,
924  int32_t flow_x,
925  int32_t flow_y,
926  int32_t flow_der_x __attribute__((unused)),
927  int32_t flow_der_y __attribute__((unused)),
928  float quality,
929  float size_divergence __attribute__((unused)))
930 {
931  flowSample sample;
932  sample.time_us = stamp;
933 
934  // Wait for two measurements in order to integrate
935  if (ekf2.flow_stamp <= 0) {
936  ekf2.flow_stamp = stamp;
937  return;
938  }
939 
940  // Calculate the timestamp
941  sample.dt = (stamp - ekf2.flow_stamp);
942  ekf2.flow_stamp = stamp;
943 
944  /* Build integrated flow and gyro messages for filter
945  NOTE: pure rotations should result in same flow_x and
946  gyro_roll and same flow_y and gyro_pitch */
947  Vector2f flowdata;
948  flowdata(0) = RadOfDeg(flow_y) * (1e-6 *
949  sample.dt); // INTEGRATED FLOW AROUND Y AXIS (RIGHT -X, LEFT +X)
950  flowdata(1) = - RadOfDeg(flow_x) * (1e-6 *
951  sample.dt); // INTEGRATED FLOW AROUND X AXIS (FORWARD +Y, BACKWARD -Y)
952 
953  sample.quality = quality; // quality indicator between 0 and 255
954  sample.flow_xy_rad =
955  flowdata; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
956  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
957 
958  // Update the optical flow data based on the callback
959  ekf.setOpticalFlowData(sample);
960 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#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:517
struct GpsState gps
global GPS state
Definition: gps.c:69
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:437
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:101
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:97
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:50
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:99
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:103
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:86
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:100
#define GpsFixValid()
Definition: gps.h:183
uint8_t num_sv
number of sat in fix
Definition: gps.h:104
uint8_t fix
status of fix
Definition: gps.h:105
data structure for GPS information
Definition: gps.h:85
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 QUAT1_BFP_OF_REAL(_qf)
#define ANGLE_BFP_OF_REAL(_af)
#define ACCEL_BFP_OF_REAL(_af)
#define POS_BFP_OF_REAL(_af)
#define SPEED_BFP_OF_REAL(_af)
Rotation quaternion.
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 struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
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
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_ekf2.cpp:594
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_ekf2.cpp:888
#define INS_EKF2_EVV_NOISE
Definition: ins_ekf2.cpp:248
void ins_ekf2_update(void)
Definition: ins_ekf2.cpp:609
#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:691
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:922
#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:864
static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
Definition: ins_ekf2.cpp:849
#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:817
#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:823
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:486
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:802
void ins_ekf2_change_param(int32_t unk)
Definition: ins_ekf2.cpp:686
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:700
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf)
Definition: ins_ekf2.cpp:723
#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
static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:459
#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:730
static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
Definition: ins_ekf2.cpp:834
#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
float parameters[20]
Definition: ins_flow.c:239
uint16_t foo
Definition: main_demo5.c:58
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:218
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:50
struct FloatEulers rc_sp
remote control setpoint
Definition: guidance_h.h:101
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:98
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