Paparazzi UAS  v7.0_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 #ifndef INS_EKF2_MAX_REL_LENGTH_ERROR
54 #define INS_EKF2_MAX_REL_LENGTH_ERROR 0.2 // Factor which gets multiplied by the reference distance
55 #endif
56 
58 #if INS_EKF2_OPTITRACK
59 #ifndef INS_EKF2_FUSION_MODE
60 #define INS_EKF2_FUSION_MODE (MASK_USE_EVPOS | MASK_USE_EVVEL | MASK_USE_EVYAW)
61 #endif
62 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
63 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_EV
64 #endif
65 #endif
66 
68 #ifndef INS_EKF2_FUSION_MODE
69 #define INS_EKF2_FUSION_MODE (MASK_USE_GPS)
70 #endif
71 PRINT_CONFIG_VAR(INS_EKF2_FUSION_MODE)
72 
73 
74 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
75 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO
76 #endif
77 PRINT_CONFIG_VAR(INS_EKF2_VDIST_SENSOR_TYPE)
78 
79 
80 #ifndef INS_EKF2_GPS_CHECK_MASK
81 #define INS_EKF2_GPS_CHECK_MASK 21 // (MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)
82 #endif
83 PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK)
84 
85 
86 #ifndef INS_EKF2_SONAR_MIN_RANGE
87 #define INS_EKF2_SONAR_MIN_RANGE 0.001
88 #endif
89 PRINT_CONFIG_VAR(INS_EKF2_SONAR_MIN_RANGE)
90 
91 
92 #ifndef INS_EKF2_SONAR_MAX_RANGE
93 #define INS_EKF2_SONAR_MAX_RANGE 4
94 #endif
95 PRINT_CONFIG_VAR(INS_EKF2_SONAR_MAX_RANGE)
96 
97 
98 #ifndef INS_EKF2_RANGE_MAIN_AGL
99 #define INS_EKF2_RANGE_MAIN_AGL 1
100 #endif
101 PRINT_CONFIG_VAR(INS_EKF2_RANGE_MAIN_AGL)
102 
103 
104 #ifndef INS_EKF2_BARO_ID
105 #if USE_BARO_BOARD
106 #define INS_EKF2_BARO_ID BARO_BOARD_SENDER_ID
107 #else
108 #define INS_EKF2_BARO_ID ABI_BROADCAST
109 #endif
110 #endif
111 PRINT_CONFIG_VAR(INS_EKF2_BARO_ID)
112 
113 
114 #ifndef INS_EKF2_TEMPERATURE_ID
115 #define INS_EKF2_TEMPERATURE_ID ABI_BROADCAST
116 #endif
117 PRINT_CONFIG_VAR(INS_EKF2_TEMPERATURE_ID)
118 
119 
120 #ifndef INS_EKF2_AGL_ID
121 #define INS_EKF2_AGL_ID ABI_BROADCAST
122 #endif
123 PRINT_CONFIG_VAR(INS_EKF2_AGL_ID)
124 
125 /* default Gyro to use in INS */
126 #ifndef INS_EKF2_GYRO_ID
127 #define INS_EKF2_GYRO_ID ABI_BROADCAST
128 #endif
129 PRINT_CONFIG_VAR(INS_EKF2_GYRO_ID)
130 
131 /* default Accelerometer to use in INS */
132 #ifndef INS_EKF2_ACCEL_ID
133 #define INS_EKF2_ACCEL_ID ABI_BROADCAST
134 #endif
135 PRINT_CONFIG_VAR(INS_EKF2_ACCEL_ID)
136 
137 /* default Magnetometer to use in INS */
138 #ifndef INS_EKF2_MAG_ID
139 #define INS_EKF2_MAG_ID ABI_BROADCAST
140 #endif
141 PRINT_CONFIG_VAR(INS_EKF2_MAG_ID)
142 
143 /* default GPS to use in INS */
144 #ifndef INS_EKF2_GPS_ID
145 #define INS_EKF2_GPS_ID GPS_MULTI_ID
146 #endif
147 PRINT_CONFIG_VAR(INS_EKF2_GPS_ID)
148 
149 /* default Optical Flow to use in INS */
150 #ifndef INS_EKF2_OF_ID
151 #define INS_EKF2_OF_ID ABI_BROADCAST
152 #endif
153 PRINT_CONFIG_VAR(INS_EKF2_OF_ID)
154 
155 /* IMU X offset from CoG position in meters */
156 #ifndef INS_EKF2_IMU_POS_X
157 #define INS_EKF2_IMU_POS_X 0
158 #endif
159 PRINT_CONFIG_VAR(INS_EKF2_IMU_POS_X)
160 
161 /* IMU Y offset from CoG position in meters */
162 #ifndef INS_EKF2_IMU_POS_Y
163 #define INS_EKF2_IMU_POS_Y 0
164 #endif
165 PRINT_CONFIG_VAR(INS_EKF2_IMU_POS_Y)
166 
167 /* IMU Z offset from CoG position in meters */
168 #ifndef INS_EKF2_IMU_POS_Z
169 #define INS_EKF2_IMU_POS_Z 0
170 #endif
171 PRINT_CONFIG_VAR(INS_EKF2_IMU_POS_Z)
172 
173 /* GPS X offset from CoG position in meters */
174 #ifndef INS_EKF2_GPS_POS_X
175 #define INS_EKF2_GPS_POS_X 0
176 #endif
177 PRINT_CONFIG_VAR(INS_EKF2_GPS_POS_X)
178 
179 /* GPS Y offset from CoG position in meters */
180 #ifndef INS_EKF2_GPS_POS_Y
181 #define INS_EKF2_GPS_POS_Y 0
182 #endif
183 PRINT_CONFIG_VAR(INS_EKF2_GPS_POS_Y)
184 
185 /* GPS Z offset from CoG position in meters */
186 #ifndef INS_EKF2_GPS_POS_Z
187 #define INS_EKF2_GPS_POS_Z 0
188 #endif
189 PRINT_CONFIG_VAR(INS_EKF2_GPS_POS_Z)
190 
191 /* Default flow/radar message delay (in ms) */
192 #ifndef INS_EKF2_FLOW_SENSOR_DELAY
193 #define INS_EKF2_FLOW_SENSOR_DELAY 15
194 #endif
195 PRINT_CONFIG_VAR(INS_FLOW_SENSOR_DELAY)
196 
197 /* Default minimum accepted quality (1 to 255) */
198 #ifndef INS_EKF2_MIN_FLOW_QUALITY
199 #define INS_EKF2_MIN_FLOW_QUALITY 100
200 #endif
201 PRINT_CONFIG_VAR(INS_EKF2_MIN_FLOW_QUALITY)
202 
203 /* Max flow rate that the sensor can measure (rad/sec) */
204 #ifndef INS_EKF2_MAX_FLOW_RATE
205 #define INS_EKF2_MAX_FLOW_RATE 200
206 #endif
207 PRINT_CONFIG_VAR(INS_EKF2_MAX_FLOW_RATE)
208 
209 /* Flow sensor X offset from CoG position in meters */
210 #ifndef INS_EKF2_FLOW_POS_X
211 #define INS_EKF2_FLOW_POS_X 0
212 #endif
213 PRINT_CONFIG_VAR(INS_EKF2_FLOW_POS_X)
214 
215 /* Flow sensor Y offset from CoG position in meters */
216 #ifndef INS_EKF2_FLOW_POS_Y
217 #define INS_EKF2_FLOW_POS_Y 0
218 #endif
219 PRINT_CONFIG_VAR(INS_EKF2_FLOW_POS_Y)
220 
221 /* Flow sensor Z offset from CoG position in meters */
222 #ifndef INS_EKF2_FLOW_POS_Z
223 #define INS_EKF2_FLOW_POS_Z 0
224 #endif
225 PRINT_CONFIG_VAR(INS_EKF2_FLOW_POS_Z)
226 
227 /* Flow sensor noise in rad/sec */
228 #ifndef INS_EKF2_FLOW_NOISE
229 #define INS_EKF2_FLOW_NOISE 0.03
230 #endif
231 PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE)
232 
233 /* Flow sensor noise at qmin in rad/sec */
234 #ifndef INS_EKF2_FLOW_NOISE_QMIN
235 #define INS_EKF2_FLOW_NOISE_QMIN 0.05
236 #endif
237 PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE_QMIN)
238 
239 /* Flow sensor innovation gate */
240 #ifndef INS_EKF2_FLOW_INNOV_GATE
241 #define INS_EKF2_FLOW_INNOV_GATE 4
242 #endif
243 PRINT_CONFIG_VAR(INS_EKF2_FLOW_INNOV_GATE)
244 
245 /* External vision position noise (m) */
246 #ifndef INS_EKF2_EVP_NOISE
247 #define INS_EKF2_EVP_NOISE 0.02f
248 #endif
249 PRINT_CONFIG_VAR(INS_EKF2_EVP_NOISE)
250 
251 /* External vision velocity noise (m/s) */
252 #ifndef INS_EKF2_EVV_NOISE
253 #define INS_EKF2_EVV_NOISE 0.1f
254 #endif
255 PRINT_CONFIG_VAR(INS_EKF2_EVV_NOISE)
256 
257 /* External vision angle noise (rad) */
258 #ifndef INS_EKF2_EVA_NOISE
259 #define INS_EKF2_EVA_NOISE 0.05f
260 #endif
261 PRINT_CONFIG_VAR(INS_EKF2_EVA_NOISE)
262 
263 /* GPS measurement noise for horizontal velocity (m/s) */
264 #ifndef INS_EKF2_GPS_V_NOISE
265 #define INS_EKF2_GPS_V_NOISE 0.3f
266 #endif
267 PRINT_CONFIG_VAR(INS_EKF2_GPS_V_NOISE)
268 
269 /* GPS measurement position noise (m) */
270 #ifndef INS_EKF2_GPS_P_NOISE
271 #define INS_EKF2_GPS_P_NOISE 0.5f
272 #endif
273 PRINT_CONFIG_VAR(INS_EKF2_GPS_P_NOISE)
274 
275 /* Barometric measurement noise for altitude (m) */
276 #ifndef INS_EKF2_BARO_NOISE
277 #define INS_EKF2_BARO_NOISE 3.5f
278 #endif
279 PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
280 
281 #ifdef INS_EXT_VISION_ROTATION
282 struct FloatQuat ins_ext_vision_rot;
283 #endif
284 
285 /* All registered ABI events */
294 
295 /* All ABI callbacks */
296 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
297 static void temperature_cb(uint8_t sender_id, float temp);
298 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
299 static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt);
300 static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt);
301 static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
302 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
303 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);
304 
305 /* Static local functions */
306 static void ins_ekf2_publish_attitude(uint32_t stamp);
307 
308 /* Static local variables */
309 static Ekf ekf;
311 struct ekf2_t ekf2;
312 static struct extVisionSample sample_ev;
313 #if PERIODIC_TELEMETRY
315 
316 static void send_ins(struct transport_tx *trans, struct link_device *dev)
317 {
318  struct NedCoor_i pos, speed, accel;
319 
320  // Get it from the EKF
321  const Vector3f pos_f{ekf.getPosition()};
322  const Vector3f speed_f{ekf.getVelocity()};
323  const Vector3f accel_f{ekf.getVelocityDerivative()};
324 
325  // Convert to integer
326  pos.x = POS_BFP_OF_REAL(pos_f(0));
327  pos.y = POS_BFP_OF_REAL(pos_f(1));
328  pos.z = POS_BFP_OF_REAL(pos_f(2));
329  speed.x = SPEED_BFP_OF_REAL(speed_f(0));
330  speed.y = SPEED_BFP_OF_REAL(speed_f(1));
331  speed.z = SPEED_BFP_OF_REAL(speed_f(2));
332  accel.x = ACCEL_BFP_OF_REAL(accel_f(0));
333  accel.y = ACCEL_BFP_OF_REAL(accel_f(1));
334  accel.z = ACCEL_BFP_OF_REAL(accel_f(2));
335 
336  // Send the message
337  pprz_msg_send_INS(trans, dev, AC_ID,
338  &pos.x, &pos.y, &pos.z,
339  &speed.x, &speed.y, &speed.z,
340  &accel.x, &accel.y, &accel.z);
341 }
342 
343 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
344 {
345  float baro_z = 0.0f;
346  int32_t pos_z, speed_z, accel_z;
347 
348  // Get it from the EKF
349  const Vector3f pos_f{ekf.getPosition()};
350  const Vector3f speed_f{ekf.getVelocity()};
351  const Vector3f accel_f{ekf.getVelocityDerivative()};
352 
353  // Convert to integer
354  pos_z = POS_BFP_OF_REAL(pos_f(2));
355  speed_z = SPEED_BFP_OF_REAL(speed_f(2));
356  accel_z = ACCEL_BFP_OF_REAL(accel_f(2));
357 
358  // Send the message
359  pprz_msg_send_INS_Z(trans, dev, AC_ID,
360  &baro_z, &pos_z, &speed_z, &accel_z);
361 }
362 
363 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
364 {
365  float qfe = 101325.0; //TODO: this is qnh not qfe?
366  if (ekf2.ltp_stamp > 0)
367  pprz_msg_send_INS_REF(trans, dev, AC_ID,
370  &ekf2.ltp_def.hmsl, &qfe);
371 }
372 
373 static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
374 {
375  uint16_t gps_check_status, soln_status;
376  uint16_t filter_fault_status = ekf.fault_status().value; // FIXME: 32bit instead of 16bit
377  uint32_t control_mode = ekf.control_status().value;
378  ekf.get_gps_check_status(&gps_check_status);
379  ekf.get_ekf_soln_status(&soln_status);
380 
381  uint16_t innov_test_status;
382  float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl;
383  uint8_t terrain_valid, dead_reckoning;
384  ekf.get_innovation_test_status(innov_test_status, mag, vel, pos, hgt, tas, hagl, beta);
385  //ekf.get_flow_innov(&flow);
386  ekf.get_mag_decl_deg(&mag_decl);
387 
388  if (ekf.isTerrainEstimateValid()) {
389  terrain_valid = 1;
390  } else {
391  terrain_valid = 0;
392  }
393 
394  if (ekf.inertial_dead_reckoning()) {
395  dead_reckoning = 1;
396  } else {
397  dead_reckoning = 0;
398  }
399 
400  pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
401  &control_mode, &filter_fault_status, &gps_check_status, &soln_status,
402  &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta,
403  &mag_decl, &terrain_valid, &dead_reckoning);
404 }
405 
406 static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
407 {
408  float gps_drift[3];
409  Vector3f vibe = ekf.getImuVibrationMetrics();
410  bool gps_blocked;
411  uint8_t gps_blocked_b;
412  ekf.get_gps_drift_metrics(gps_drift, &gps_blocked);
413  gps_blocked_b = gps_blocked;
414 
415  pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID,
416  &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b,
417  &vibe(0), &vibe(1), &vibe(2));
418 }
419 
420 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
421 {
422  uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2;
423  filter_control_status_u control_mode = ekf.control_status();
424  uint32_t filter_fault_status = ekf.fault_status().value;
425  uint16_t filter_fault_status_16 = filter_fault_status; //FIXME
426  uint8_t mde = 0;
427 
428  // Check the alignment and if GPS is fused
429  if (control_mode.flags.tilt_align && control_mode.flags.yaw_align && (control_mode.flags.gps || control_mode.flags.ev_pos)) {
430  mde = 3;
431  } else if (control_mode.flags.tilt_align && control_mode.flags.yaw_align) {
432  mde = 4;
433  } else {
434  mde = 2;
435  }
436 
437  // Check if there is a covariance error
438  if (filter_fault_status) {
439  mde = 6;
440  }
441 
442  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status_16);
443 }
444 
445 static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
446 {
447  float tas;
448  Vector2f wind = ekf.getWindVelocity();
449  uint8_t flags = 0x5;
450  float f_zero = 0;
451 
452  ekf.get_true_airspeed(&tas);
453 
454  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind(1), &wind(0), &f_zero, &tas);
455 }
456 
457 static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
458 {
459  Vector3f accel_bias = ekf.getAccelBias();
460  Vector3f gyro_bias = ekf.getGyroBias();
461  Vector3f mag_bias = ekf.getMagBias();
462 
463  pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias(0), &accel_bias(1), &accel_bias(2),
464  &gyro_bias(0), &gyro_bias(1), &gyro_bias(2), &mag_bias(0), &mag_bias(1), &mag_bias(2));
465 }
466 
467 static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
468 {
469  struct Int32Quat ltp_to_body_quat;
470  const Quatf att_q{ekf.calculate_quaternion()};
471  ltp_to_body_quat.qi = QUAT1_BFP_OF_REAL(att_q(0));
472  ltp_to_body_quat.qx = QUAT1_BFP_OF_REAL(att_q(1));
473  ltp_to_body_quat.qy = QUAT1_BFP_OF_REAL(att_q(2));
474  ltp_to_body_quat.qz = QUAT1_BFP_OF_REAL(att_q(3));
475  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
476  float foo = 0.f;
477  uint8_t ahrs_id = 1; // generic
478  pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
479  &foo,
480  &ltp_to_body_quat.qi,
481  &ltp_to_body_quat.qx,
482  &ltp_to_body_quat.qy,
483  &ltp_to_body_quat.qz,
484  &(quat->qi),
485  &(quat->qx),
486  &(quat->qy),
487  &(quat->qz),
488  &ahrs_id);
489 }
490 
491 
492 static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
493 {
494  if(sample_ev.time_us == 0){
495  return;
496  }
497  float sample_temp_ev[11];
498  sample_temp_ev[0] = (float) sample_ev.time_us;
499  sample_temp_ev[1] = sample_ev.pos(0) ;
500  sample_temp_ev[2] = sample_ev.pos(1) ;
501  sample_temp_ev[3] = sample_ev.pos(2) ;
502  sample_temp_ev[4] = sample_ev.vel(0) ;
503  sample_temp_ev[5] = sample_ev.vel(1) ;
504  sample_temp_ev[6] = sample_ev.vel(2) ;
505  sample_temp_ev[7] = sample_ev.quat(0);
506  sample_temp_ev[8] = sample_ev.quat(1);
507  sample_temp_ev[9] = sample_ev.quat(2);
508  sample_temp_ev[10] = sample_ev.quat(3);
509  pprz_msg_send_EXTERNAL_POSE_DOWN(trans, dev, AC_ID,
510  &sample_temp_ev[0],
511  &sample_temp_ev[1],
512  &sample_temp_ev[2],
513  &sample_temp_ev[3],
514  &sample_temp_ev[4],
515  &sample_temp_ev[5],
516  &sample_temp_ev[6],
517  &sample_temp_ev[7],
518  &sample_temp_ev[8],
519  &sample_temp_ev[9],
520  &sample_temp_ev[10] );
521 }
522 #endif
523 
524 /* Initialize the EKF */
525 void ins_ekf2_init(void)
526 {
527  /* Get the ekf parameters */
528  ekf_params = ekf.getParamHandle();
529  ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
530  ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
531  ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
532 
533  /* Set specific noise levels */
534  ekf_params->accel_bias_p_noise = 3.0e-3f;
535  ekf_params->gps_vel_noise = INS_EKF2_GPS_V_NOISE;
536  ekf_params->gps_pos_noise = INS_EKF2_GPS_P_NOISE;
537  ekf_params->baro_noise = INS_EKF2_BARO_NOISE;
538 
539  /* Set optical flow parameters */
540  ekf_params->flow_qual_min = INS_EKF2_MIN_FLOW_QUALITY;
541  ekf_params->flow_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
542  ekf_params->range_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
543  ekf_params->flow_noise = INS_EKF2_FLOW_NOISE;
544  ekf_params->flow_noise_qual_min = INS_EKF2_FLOW_NOISE_QMIN;
545  ekf_params->flow_innov_gate = INS_EKF2_FLOW_INNOV_GATE;
546 
547  /* Set the IMU position relative from the CoG in xyz (m) */
548  ekf_params->imu_pos_body = {
552  };
553 
554  /* Set the GPS position relative from the CoG in xyz (m) */
555  ekf_params->gps_pos_body = {
559  };
560 
561  /* Set flow sensor offset from CoG position in xyz (m) */
562  ekf_params->flow_pos_body = {
566  };
567 
568  /* Set range as default AGL measurement if possible */
569  ekf_params->range_aid = INS_EKF2_RANGE_MAIN_AGL;
570 
571  /* Initialize struct */
572  ekf2.ltp_stamp = 0;
573  ekf2.flow_stamp = 0;
574  ekf2.gyro_valid = false;
575  ekf2.accel_valid = false;
576  ekf2.got_imu_data = false;
578  ekf2.temp = 20.0f; // Default temperature of 20 degrees celcius
579  ekf2.qnh = 1013.25f; // Default atmosphere
580 
581  /* Initialize the range sensor limits */
582  ekf.set_rangefinder_limits(INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE);
583 
584  /* Initialize the flow sensor limits */
586 
587  // Don't send external vision data by default
588  sample_ev.time_us = 0;
589 
590  /* Initialize the origin from flight plan */
591 #if USE_INS_NAV_INIT
592  if(ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0)*1e-3)) // EKF2 works HMSL
593  {
594  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
595  llh_nav0.lat = NAV_LAT0;
596  llh_nav0.lon = NAV_LON0;
597  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
598  llh_nav0.alt = NAV_ALT0 + NAV_MSL0; // in millimeters above WGS84 reference ellipsoid
599 
600  ltp_def_from_lla_i(&ekf2.ltp_def, &llh_nav0);
601  ekf2.ltp_def.hmsl = NAV_ALT0;
603 
604  /* update local ENU coordinates of global waypoints */
606 
607  ekf2.ltp_stamp = 1;
608  }
609 #endif
610 
611 #if PERIODIC_TELEMETRY
617  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
620  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_ahrs_quat);
621  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EXTERNAL_POSE_DOWN, send_external_pose_down);
622 #endif
623 
624  /*
625  * Subscribe to scaled IMU measurements and attach callbacks
626  */
627  AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
628  AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
629  AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
630  AbiBindMsgIMU_GYRO_INT(INS_EKF2_GYRO_ID, &gyro_int_ev, gyro_int_cb);
631  AbiBindMsgIMU_ACCEL_INT(INS_EKF2_ACCEL_ID, &accel_int_ev, accel_int_cb);
632  AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
633  AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
634  AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
635 }
636 
638 {
639 #if USE_GPS
640  if (GpsFixValid()) {
641  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
642  if (ekf.setEkfGlobalOrigin(lla_pos.lat*1e-7, lla_pos.lon*1e-7, gps.hmsl*1e-3)) {
643  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_pos);
646  }
647  }
648 #endif
649 }
650 
652 {
653 #if USE_GPS
654  if (GpsFixValid()) {
655  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
656  struct LlaCoor_i lla = {
658  .lon = state.ned_origin_i.lla.lon,
659  .alt = lla_pos.alt
660  };
661  if (ekf.setEkfGlobalOrigin(lla.lat*1e-7, lla.lon*1e-7, gps.hmsl*1e-3)) {
665  }
666  }
667 #endif
668 }
669 
670 /* Update the INS state */
671 void ins_ekf2_update(void)
672 {
673  /* Set EKF settings */
674  ekf.set_in_air_status(autopilot_in_flight());
675 
676  /* Update the EKF */
677  if (ekf2.got_imu_data) {
678  // Update the EKF but ignore the response and also copy the faster intermediate filter
679  ekf.update();
680  filter_control_status_u control_status = ekf.control_status();
681 
682  // Only publish position after successful alignment
683  if (control_status.flags.tilt_align) {
684  /* Get the position */
685  const Vector3f pos_f{ekf.getPosition()};
686  struct NedCoor_f pos;
687  pos.x = pos_f(0);
688  pos.y = pos_f(1);
689  pos.z = pos_f(2);
690 
691  // Publish to the state
692  stateSetPositionNed_f(&pos);
693 
694  /* Get the velocity in NED frame */
695  const Vector3f vel_f{ekf.getVelocity()};
696  struct NedCoor_f speed;
697  speed.x = vel_f(0);
698  speed.y = vel_f(1);
699  speed.z = vel_f(2);
700 
701  // Publish to state
702  stateSetSpeedNed_f(&speed);
703 
704  /* Get the accelerations in NED frame */
705  const Vector3f vel_deriv_f{ekf.getVelocityDerivative()};
706  struct NedCoor_f accel;
707  accel.x = vel_deriv_f(0);
708  accel.y = vel_deriv_f(1);
709  accel.z = vel_deriv_f(2);
710 
711  // Publish to state
712  stateSetAccelNed_f(&accel);
713 
714  /* Get local origin */
715  // Position of local NED origin in GPS / WGS84 frame
716  double ekf_origin_lat, ekf_origin_lon;
717  float ref_alt;
718  struct LlaCoor_i lla_ref;
719  uint64_t origin_time;
720 
721  // Only update the origin when the state estimator has updated the origin
722  bool ekf_origin_valid = ekf.getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
723  if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
724  lla_ref.lat = ekf_origin_lat * 1e7; // WGS-84 lat
725  lla_ref.lon = ekf_origin_lon * 1e7; // WGS-84 lon
726  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)
727  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
728  ekf2.ltp_def.hmsl = ref_alt * 1e3;
730 
731  /* update local ENU coordinates of global waypoints */
733 
734  ekf2.ltp_stamp = origin_time;
735  }
736  }
737  }
738 
739 #if defined SITL && USE_NPS
740  if (nps_bypass_ins) {
742  }
743 #endif
744 
745  ekf2.got_imu_data = false;
746 }
747 
749 {
750  ekf_params->mag_fusion_type = ekf2.mag_fusion_type = unk;
751 }
752 
754 {
755  if (mode) {
756  ekf_params->fusion_mode = ekf2.fusion_mode = (MASK_USE_OF | MASK_USE_GPSYAW);
757  } else {
759  }
760 }
761 
763  if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
764 
765  sample_ev.time_us = get_sys_time_usec(); //FIXME
766  sample_ev.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
767  sample_ev.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
768  sample_ev.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
769  sample_ev.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
770  sample_ev.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
771  sample_ev.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
772  sample_ev.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
773  sample_ev.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
774  sample_ev.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
775  sample_ev.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
776 
777 #ifdef INS_EXT_VISION_ROTATION
778  // Rotate the quaternion
779  struct FloatQuat body_q = {sample_ev.quat(0), sample_ev.quat(1), sample_ev.quat(2), sample_ev.quat(3)};
780  struct FloatQuat rot_q;
781  float_quat_comp(&rot_q, &body_q, &ins_ext_vision_rot);
782  sample_ev.quat(0) = rot_q.qi;
783  sample_ev.quat(1) = rot_q.qx;
784  sample_ev.quat(2) = rot_q.qy;
785  sample_ev.quat(3) = rot_q.qz;
786 #endif
787 
788  sample_ev.posVar.setAll(INS_EKF2_EVP_NOISE);
789  sample_ev.velCov = matrix::eye<float, 3>() * INS_EKF2_EVV_NOISE;
790  sample_ev.angVar = INS_EKF2_EVA_NOISE;
791  sample_ev.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
792 
793  ekf.setExtVisionData(sample_ev);
794 }
795 
796 void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf) {
797 
798 }
799 
804 {
805  imuSample imu_sample = {};
806  imu_sample.time_us = stamp;
807  imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
808  imu_sample.delta_ang = Vector3f{ekf2.delta_gyro.p, ekf2.delta_gyro.q, ekf2.delta_gyro.r};
809  imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
810  imu_sample.delta_vel = Vector3f{ekf2.delta_accel.x, ekf2.delta_accel.y, ekf2.delta_accel.z};
811  ekf.setIMUData(imu_sample);
812 
813  if (ekf.attitude_valid()) {
814  // Calculate the quaternion
815  struct FloatQuat ltp_to_body_quat;
816  const Quatf att_q{ekf.calculate_quaternion()};
817  ltp_to_body_quat.qi = att_q(0);
818  ltp_to_body_quat.qx = att_q(1);
819  ltp_to_body_quat.qy = att_q(2);
820  ltp_to_body_quat.qz = att_q(3);
821 
822  // Publish it to the state
823  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
824 
825  /* Check the quaternion reset state */
826  float delta_q_reset[4];
827  uint8_t quat_reset_counter;
828  ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
829 
830 #ifndef NO_RESET_UPDATE_SETPOINT_HEADING
831  // FIXME is this hard reset of control setpoint really needed ? is it the right place ?
832  if (ekf2.quat_reset_counter < quat_reset_counter) {
833  float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
834  guidance_h.sp.heading += psi;
835  guidance_h.rc_sp.heading += psi;
836  nav.heading += psi;
837  //guidance_h_read_rc(autopilot_in_flight());
839  ekf2.quat_reset_counter = quat_reset_counter;
840  }
841 #endif
842 
843  /* Get in-run gyro bias */
844  struct FloatRates body_rates;
845  Vector3f gyro_bias{ekf.getGyroBias()};
846  body_rates.p = (ekf2.delta_gyro.p / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(0);
847  body_rates.q = (ekf2.delta_gyro.q / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(1);
848  body_rates.r = (ekf2.delta_gyro.r / (ekf2.gyro_dt * 1.e-6f)) - gyro_bias(2);
849 
850  // Publish it to the state
851  stateSetBodyRates_f(&body_rates);
852 
853  /* Get the in-run acceleration bias */
854  struct Int32Vect3 accel;
855  Vector3f accel_bias{ekf.getAccelBias()};
856  accel.x = ACCEL_BFP_OF_REAL((ekf2.delta_accel.x / (ekf2.accel_dt * 1e-6f)) - accel_bias(0));
857  accel.y = ACCEL_BFP_OF_REAL((ekf2.delta_accel.y / (ekf2.accel_dt * 1e-6f)) - accel_bias(1));
858  accel.z = ACCEL_BFP_OF_REAL((ekf2.delta_accel.z / (ekf2.accel_dt * 1e-6f)) - accel_bias(2));
859 
860  // Publish it to the state
861  stateSetAccelBody_i(&accel);
862  }
863 
864  ekf2.gyro_valid = false;
865  ekf2.accel_valid = false;
866  ekf2.got_imu_data = true;
867 }
868 
869 /* Update INS based on Baro information */
870 static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
871 {
872  baroSample sample;
873  sample.time_us = stamp;
874 
875  // Calculate the air density
876  float rho = pprz_isa_density_of_pressure(pressure, ekf2.temp);
877  ekf.set_air_density(rho);
878 
879  // Calculate the height above mean sea level based on pressure
880  sample.hgt = pprz_isa_height_of_pressure_full(pressure, ekf2.qnh * 100.0f);
881  ekf.setBaroData(sample);
882 }
883 
884 /* Save the latest temperature measurement for air density calculations */
885 static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
886 {
887  ekf2.temp = temp;
888 }
889 
890 /* Update INS based on AGL information */
891 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
892 {
893  rangeSample sample;
894  sample.time_us = stamp;
895  sample.rng = distance;
896  sample.quality = -1;
897 
898  ekf.setRangeData(sample);
899 }
900 
901 /* Update INS based on Gyro information */
902 static void gyro_int_cb(uint8_t __attribute__((unused)) sender_id,
903  uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
904 {
905  // Copy and save the gyro data
906  RATES_COPY(ekf2.delta_gyro, *delta_gyro);
907  ekf2.gyro_dt = dt;
908  ekf2.gyro_valid = true;
909 
910  /* When Gyro and accelerometer are valid enter it into the EKF */
911  if (ekf2.gyro_valid && ekf2.accel_valid) {
913  }
914 }
915 
916 /* Update INS based on Accelerometer information */
917 static void accel_int_cb(uint8_t sender_id __attribute__((unused)),
918  uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
919 {
920  // Copy and save the gyro data
921  VECT3_COPY(ekf2.delta_accel, *delta_accel);
922  ekf2.accel_dt = dt;
923  ekf2.accel_valid = true;
924 
925  /* When Gyro and accelerometer are valid enter it into the EKF */
926  if (ekf2.gyro_valid && ekf2.accel_valid) {
928  }
929 }
930 
931 /* Update INS based on Magnetometer information */
932 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
933  uint32_t stamp,
934  struct Int32Vect3 *mag)
935 {
936  struct FloatVect3 mag_gauss;
937  magSample sample;
938  sample.time_us = stamp;
939 
940  // Convert Magnetometer information to float and to radius 0.2f
941  MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
942  mag_gauss.x *= 0.4f;
943  mag_gauss.y *= 0.4f;
944  mag_gauss.z *= 0.4f;
945 
946  // Publish information to the EKF
947  sample.mag(0) = mag_gauss.x;
948  sample.mag(1) = mag_gauss.y;
949  sample.mag(2) = mag_gauss.z;
950 
951  ekf.setMagData(sample);
952  ekf2.got_imu_data = true;
953 }
954 
955 /* Update INS based on GPS information */
956 static void gps_cb(uint8_t sender_id __attribute__((unused)),
957  uint32_t stamp,
958  struct GpsState *gps_s)
959 {
960  gps_message gps_msg = {};
961  gps_msg.time_usec = stamp;
962  struct LlaCoor_i lla_pos = lla_int_from_gps(gps_s);
963  gps_msg.lat = lla_pos.lat;
964  gps_msg.lon = lla_pos.lon;
965  gps_msg.alt = gps_s->hmsl; // EKF2 works with HMSL
966 #if INS_EKF2_GPS_COURSE_YAW
967  gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
968  gps_msg.yaw_offset = 0;
969 #elif defined(INS_EKF2_GPS_YAW_OFFSET) && defined(INS_EKF2_ANTENNA_DISTANCE)
971  && fabsf(gps_relposned.relPosLength - INS_EKF2_ANTENNA_DISTANCE) <= INS_EKF2_MAX_REL_LENGTH_ERROR * INS_EKF2_ANTENNA_DISTANCE) {
972  gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading - INS_EKF2_GPS_YAW_OFFSET));
973  } else {
974  gps_msg.yaw = NAN;
975  }
976 
977  // Offset also needs to be substracted from the heading (this is for roll/pitch angle limits)
978  gps_msg.yaw_offset = RadOfDeg(INS_EKF2_GPS_YAW_OFFSET);
979 #else
980  gps_msg.yaw = NAN;
981  gps_msg.yaw_offset = NAN;
982 #endif
983  gps_msg.fix_type = gps_s->fix;
984  gps_msg.eph = gps_s->hacc / 100.0;
985  gps_msg.epv = gps_s->vacc / 100.0;
986  gps_msg.sacc = gps_s->sacc / 100.0;
987  gps_msg.vel_m_s = gps_s->gspeed / 100.0;
988  struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s);
989  gps_msg.vel_ned(0) = ned_vel.x;
990  gps_msg.vel_ned(1) = ned_vel.y;
991  gps_msg.vel_ned(2) = ned_vel.z;
992  gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
993  gps_msg.nsats = gps_s->num_sv;
994  gps_msg.pdop = gps_s->pdop;
995 
996  ekf.setGpsData(gps_msg);
997 }
998 
999 /* Update INS based on Optical Flow information */
1000 static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
1001  uint32_t stamp,
1002  int32_t flow_x,
1003  int32_t flow_y,
1004  int32_t flow_der_x __attribute__((unused)),
1005  int32_t flow_der_y __attribute__((unused)),
1006  float quality,
1007  float size_divergence __attribute__((unused)))
1008 {
1009  flowSample sample;
1010  sample.time_us = stamp;
1011 
1012  // Wait for two measurements in order to integrate
1013  if (ekf2.flow_stamp <= 0) {
1014  ekf2.flow_stamp = stamp;
1015  return;
1016  }
1017 
1018  // Calculate the timestamp
1019  sample.dt = (stamp - ekf2.flow_stamp);
1020  ekf2.flow_stamp = stamp;
1021 
1022  /* Build integrated flow and gyro messages for filter
1023  NOTE: pure rotations should result in same flow_x and
1024  gyro_roll and same flow_y and gyro_pitch */
1025  Vector2f flowdata;
1026  flowdata(0) = RadOfDeg(flow_y) * (1e-6 *
1027  sample.dt); // INTEGRATED FLOW AROUND Y AXIS (RIGHT -X, LEFT +X)
1028  flowdata(1) = - RadOfDeg(flow_x) * (1e-6 *
1029  sample.dt); // INTEGRATED FLOW AROUND X AXIS (FORWARD +Y, BACKWARD -Y)
1030 
1031  sample.quality = quality; // quality indicator between 0 and 255
1032  sample.flow_xy_rad =
1033  flowdata; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
1034  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
1035 
1036  // Update the optical flow data based on the callback
1037  ekf.setOpticalFlowData(sample);
1038 }
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:340
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:516
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:436
struct GpsRelposNED gps_relposned
Definition: gps.c:71
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:93
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:102
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:98
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:51
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:100
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:104
uint8_t diffSoln
Definition: gps.h:144
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:96
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:87
uint8_t carrSoln
Definition: gps.h:142
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:101
#define GpsFixValid()
Definition: gps.h:176
uint8_t relPosValid
Definition: gps.h:143
float relPosLength
Definition: gps.h:137
float relPosHeading
Definition: gps.h:138
uint8_t num_sv
number of sat in fix
Definition: gps.h:105
uint8_t fix
status of fix
Definition: gps.h:106
data structure for GPS information
Definition: gps.h:86
float q
in rad/s
float p
in rad/s
float r
in rad/s
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
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 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
struct State state
Definition: state.c:36
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
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:293
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_ekf2.cpp:637
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_ekf2.cpp:956
#define INS_EKF2_EVV_NOISE
Definition: ins_ekf2.cpp:253
void ins_ekf2_update(void)
Definition: ins_ekf2.cpp:671
#define INS_EKF2_IMU_POS_Z
Definition: ins_ekf2.cpp:169
#define INS_EKF2_GPS_ID
Definition: ins_ekf2.cpp:145
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:316
#define INS_EKF2_GPS_POS_Y
Definition: ins_ekf2.cpp:181
#define INS_EKF2_MAX_REL_LENGTH_ERROR
Maximum allowed error in distance between dual GPS antennae.
Definition: ins_ekf2.cpp:54
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:373
void ins_ekf2_remove_gps(int32_t mode)
Definition: ins_ekf2.cpp:753
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:406
static abi_event temperature_ev
Definition: ins_ekf2.cpp:287
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:1000
#define INS_EKF2_TEMPERATURE_ID
default temperature sensor to use in INS
Definition: ins_ekf2.cpp:115
#define INS_EKF2_VDIST_SENSOR_TYPE
The EKF2 primary vertical distance sensor type.
Definition: ins_ekf2.cpp:75
static abi_event mag_ev
Definition: ins_ekf2.cpp:291
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ins_ekf2.cpp:932
static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt)
Definition: ins_ekf2.cpp:917
#define INS_EKF2_FUSION_MODE
Special configuration for Optitrack.
Definition: ins_ekf2.cpp:69
#define INS_EKF2_GPS_CHECK_MASK
The EKF2 GPS checks before initialization.
Definition: ins_ekf2.cpp:81
#define INS_EKF2_RANGE_MAIN_AGL
If enabled uses radar sensor as primary AGL source, if possible.
Definition: ins_ekf2.cpp:99
#define INS_EKF2_MAX_FLOW_RATE
Definition: ins_ekf2.cpp:205
#define INS_EKF2_GPS_POS_Z
Definition: ins_ekf2.cpp:187
static abi_event gyro_int_ev
Definition: ins_ekf2.cpp:289
#define INS_EKF2_EVA_NOISE
Definition: ins_ekf2.cpp:259
#define INS_EKF2_BARO_NOISE
Definition: ins_ekf2.cpp:277
#define INS_EKF2_ACCEL_ID
Definition: ins_ekf2.cpp:133
#define INS_EKF2_GYRO_ID
Definition: ins_ekf2.cpp:127
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:363
static void temperature_cb(uint8_t sender_id, float temp)
Definition: ins_ekf2.cpp:885
#define INS_EKF2_FLOW_POS_Y
Definition: ins_ekf2.cpp:217
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: ins_ekf2.cpp:891
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:343
static abi_event baro_ev
Definition: ins_ekf2.cpp:286
static parameters * ekf_params
The EKF parameters.
Definition: ins_ekf2.cpp:310
void ins_ekf2_init(void)
Definition: ins_ekf2.cpp:525
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:420
#define INS_EKF2_FLOW_INNOV_GATE
Definition: ins_ekf2.cpp:241
#define INS_EKF2_FLOW_NOISE
Definition: ins_ekf2.cpp:229
#define INS_EKF2_EVP_NOISE
Definition: ins_ekf2.cpp:247
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_ekf2.cpp:870
void ins_ekf2_change_param(int32_t unk)
Definition: ins_ekf2.cpp:748
static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:445
#define INS_EKF2_MIN_FLOW_QUALITY
Definition: ins_ekf2.cpp:199
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_ekf2.cpp:651
#define INS_EKF2_GPS_V_NOISE
Definition: ins_ekf2.cpp:265
static Ekf ekf
EKF class itself.
Definition: ins_ekf2.cpp:309
struct ekf2_t ekf2
Local EKF2 status structure.
Definition: ins_ekf2.cpp:311
#define INS_EKF2_FLOW_SENSOR_DELAY
Definition: ins_ekf2.cpp:193
void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf)
Definition: ins_ekf2.cpp:762
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf)
Definition: ins_ekf2.cpp:796
#define INS_EKF2_MAG_ID
Definition: ins_ekf2.cpp:139
#define INS_EKF2_IMU_POS_X
Definition: ins_ekf2.cpp:157
#define INS_EKF2_GPS_P_NOISE
Definition: ins_ekf2.cpp:271
static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:467
#define INS_EKF2_AGL_ID
default AGL sensor to use in INS
Definition: ins_ekf2.cpp:121
#define INS_EKF2_OF_ID
Definition: ins_ekf2.cpp:151
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:803
static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *delta_gyro, uint16_t dt)
Definition: ins_ekf2.cpp:902
#define INS_EKF2_SONAR_MIN_RANGE
Default AGL sensor minimum range.
Definition: ins_ekf2.cpp:87
#define INS_EKF2_FLOW_NOISE_QMIN
Definition: ins_ekf2.cpp:235
#define INS_EKF2_IMU_POS_Y
Definition: ins_ekf2.cpp:163
static abi_event accel_int_ev
Definition: ins_ekf2.cpp:290
#define INS_EKF2_FLOW_POS_Z
Definition: ins_ekf2.cpp:223
static struct extVisionSample sample_ev
External vision sample.
Definition: ins_ekf2.cpp:312
#define INS_EKF2_BARO_ID
default barometer to use in INS
Definition: ins_ekf2.cpp:108
static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:492
#define INS_EKF2_SONAR_MAX_RANGE
Default AGL sensor maximum range.
Definition: ins_ekf2.cpp:93
#define INS_EKF2_GPS_POS_X
Definition: ins_ekf2.cpp:175
#define INS_EKF2_FLOW_POS_X
Definition: ins_ekf2.cpp:211
static abi_event gps_ev
Definition: ins_ekf2.cpp:292
static abi_event agl_ev
Definition: ins_ekf2.cpp:288
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:457
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:240
uint16_t foo
Definition: main_demo5.c:58
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:357
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.
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:45
struct HorizontalGuidanceRCInput rc_sp
remote control setpoint
Definition: guidance_h.h:110
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:108
struct RotorcraftNavigation nav
Definition: navigation.c:51
float heading
heading setpoint (in radians)
Definition: navigation.h:133
void stabilization_attitude_enter(void)
Attitude control enter function.
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
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