Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_ekf2.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 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 
31 #include "subsystems/abi.h"
32 #include "stabilization/stabilization_attitude.h"
33 #include "generated/airframe.h"
34 #include "EKF/ekf.h"
35 #include "math/pprz_isa.h"
36 #include "mcu_periph/sys_time.h"
37 #include "autopilot.h"
38 
40 #if defined SITL && USE_NPS
41 #include "nps_autopilot.h"
42 #include <stdio.h>
43 #endif
44 
46 #if USE_INS_NAV_INIT
47 #error INS initialization from flight plan is not yet supported
48 #endif
49 
51 #ifndef INS_EKF2_FUSION_MODE
52 #define INS_EKF2_FUSION_MODE (MASK_USE_GPS)
53 #endif
54 PRINT_CONFIG_VAR(INS_EKF2_FUSION_MODE)
55 
56 
57 #ifndef INS_EKF2_VDIST_SENSOR_TYPE
58 #define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO
59 #endif
60 PRINT_CONFIG_VAR(INS_EKF2_VDIST_SENSOR_TYPE)
61 
62 
63 #ifndef INS_EKF2_GPS_CHECK_MASK
64 #define INS_EKF2_GPS_CHECK_MASK 21 // (MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)
65 #endif
66 PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK)
67 
68 
69 #ifndef INS_EKF2_AGL_ID
70 #define INS_EKF2_AGL_ID ABI_BROADCAST
71 #endif
72 PRINT_CONFIG_VAR(INS_EKF2_AGL_ID)
73 
74 
75 #ifndef INS_EKF2_SONAR_MIN_RANGE
76 #define INS_EKF2_SONAR_MIN_RANGE 0.001
77 #endif
78 PRINT_CONFIG_VAR(INS_EKF2_SONAR_MIN_RANGE)
79 
80 
81 #ifndef INS_EKF2_SONAR_MAX_RANGE
82 #define INS_EKF2_SONAR_MAX_RANGE 4
83 #endif
84 PRINT_CONFIG_VAR(INS_EKF2_SONAR_MAX_RANGE)
85 
86 
87 #ifndef INS_EKF2_RANGE_MAIN_AGL
88 #define INS_EKF2_RANGE_MAIN_AGL 1
89 #endif
90 PRINT_CONFIG_VAR(INS_EKF2_RANGE_MAIN_AGL)
91 
92 
93 #ifndef INS_EKF2_BARO_ID
94 #if USE_BARO_BOARD
95 #define INS_EKF2_BARO_ID BARO_BOARD_SENDER_ID
96 #else
97 #define INS_EKF2_BARO_ID ABI_BROADCAST
98 #endif
99 #endif
100 PRINT_CONFIG_VAR(INS_EKF2_BARO_ID)
101 
102 /* default Gyro to use in INS */
103 #ifndef INS_EKF2_GYRO_ID
104 #define INS_EKF2_GYRO_ID ABI_BROADCAST
105 #endif
106 PRINT_CONFIG_VAR(INS_EKF2_GYRO_ID)
107 
108 /* default Accelerometer to use in INS */
109 #ifndef INS_EKF2_ACCEL_ID
110 #define INS_EKF2_ACCEL_ID ABI_BROADCAST
111 #endif
112 PRINT_CONFIG_VAR(INS_EKF2_ACCEL_ID)
113 
114 /* default Magnetometer to use in INS */
115 #ifndef INS_EKF2_MAG_ID
116 #define INS_EKF2_MAG_ID ABI_BROADCAST
117 #endif
118 PRINT_CONFIG_VAR(INS_EKF2_MAG_ID)
119 
120 /* default GPS to use in INS */
121 #ifndef INS_EKF2_GPS_ID
122 #define INS_EKF2_GPS_ID GPS_MULTI_ID
123 #endif
124 PRINT_CONFIG_VAR(INS_EKF2_GPS_ID)
125 
126 /* default Optical Flow to use in INS */
127 #ifndef INS_EKF2_OF_ID
128 #define INS_EKF2_OF_ID ABI_BROADCAST
129 #endif
130 PRINT_CONFIG_VAR(INS_EKF2_OF_ID)
131 
132 /* Default flow/radar message delay (in ms) */
133 #ifndef INS_EKF2_FLOW_SENSOR_DELAY
134 #define INS_EKF2_FLOW_SENSOR_DELAY 15
135 #endif
136 PRINT_CONFIG_VAR(INS_FLOW_SENSOR_DELAY)
137 
138 /* Default minimum accepted quality (1 to 255) */
139 #ifndef INS_EKF2_MIN_FLOW_QUALITY
140 #define INS_EKF2_MIN_FLOW_QUALITY 100
141 #endif
142 PRINT_CONFIG_VAR(INS_EKF2_MIN_FLOW_QUALITY)
143 
144 /* Max flow rate that the sensor can measure (rad/sec) */
145 #ifndef INS_EKF2_MAX_FLOW_RATE
146 #define INS_EKF2_MAX_FLOW_RATE 200
147 #endif
148 PRINT_CONFIG_VAR(INS_EKF2_MAX_FLOW_RATE)
149 
150 /* Flow sensor X offset from IMU position in meters */
151 #ifndef INS_EKF2_FLOW_OFFSET_X
152 #define INS_EKF2_FLOW_OFFSET_X 0
153 #endif
154 PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_X)
155 
156 /* Flow sensor Y offset from IMU position in meters */
157 #ifndef INS_EKF2_FLOW_OFFSET_Y
158 #define INS_EKF2_FLOW_OFFSET_Y 0
159 #endif
160 PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_Y)
161 
162 /* Flow sensor Z offset from IMU position in meters */
163 #ifndef INS_EKF2_FLOW_OFFSET_Z
164 #define INS_EKF2_FLOW_OFFSET_Z 0
165 #endif
166 PRINT_CONFIG_VAR(INS_EKF2_FLOW_OFFSET_Z)
167 
168 /* Flow sensor noise in rad/sec */
169 #ifndef INS_EKF2_FLOW_NOISE
170 #define INS_EKF2_FLOW_NOISE 0.03
171 #endif
172 PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE)
173 
174 /* Flow sensor noise at qmin in rad/sec */
175 #ifndef INS_EKF2_FLOW_NOISE_QMIN
176 #define INS_EKF2_FLOW_NOISE_QMIN 0.05
177 #endif
178 PRINT_CONFIG_VAR(INS_EKF2_FLOW_NOISE_QMIN)
179 
180 /* Flow sensor innovation gate */
181 #ifndef INS_EKF2_FLOW_INNOV_GATE
182 #define INS_EKF2_FLOW_INNOV_GATE 4
183 #endif
184 PRINT_CONFIG_VAR(INS_EKF2_FLOW_INNOV_GATE)
185 
186 /* All registered ABI events */
195 
196 /* Build optical flow and gps message struct based on flow message defined in common.h */
197 struct gps_message gps_msg = {};
198 struct flow_message flow_msg = {};
199 
200 /* All ABI callbacks */
201 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
202 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
203 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
204 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
205 static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
206 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
207 static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
208 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);
209 
210 /* Main EKF2 structure for keeping track of the status and use cross messaging */
211 struct ekf2_t
212 {
213 
214  // stamp and dt for sensors
221 
222  // gyro and accellerometer values
227 
228  // optical flow and gyro values
230  float flow_x;
231  float flow_y;
232  float gyro_roll;
233  float gyro_pitch;
234  float gyro_yaw;
235  float offset_x;
236  float offset_y;
237  float offset_z;
238 
239  // optical flow takeover
240  float flow_innov;
241 
247 };
248 
249 /* Static local functions */
250 static void ins_ekf2_publish_attitude(uint32_t stamp);
251 
252 /* Static local variables */
253 static Ekf ekf;
254 static parameters *ekf_params;
255 struct ekf2_t ekf2;
257 
258 /* External paramters */
260 
261 #if PERIODIC_TELEMETRY
263 
264 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
265 {
266  float qfe = 101325.0; //TODO: this is qnh not qfe?
267  if (ekf2.ltp_stamp > 0)
268  pprz_msg_send_INS_REF(trans, dev, AC_ID,
271  &ekf2.ltp_def.hmsl, &qfe);
272 }
273 
274 static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
275 {
276  uint16_t gps_check_status, filter_fault_status, soln_status;
277  uint32_t control_mode;
278  ekf.get_gps_check_status(&gps_check_status);
279  ekf.get_filter_fault_status(&filter_fault_status);
280  ekf.get_control_mode(&control_mode);
281  ekf.get_ekf_soln_status(&soln_status);
282 
283  uint16_t innov_test_status;
284  float mag, vel, pos, hgt, tas, hagl, flow, beta, mag_decl;
285  uint8_t terrain_valid, dead_reckoning;
286  ekf.get_innovation_test_status(&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta);
287  ekf.get_flow_innov(&flow);
288  ekf.get_mag_decl_deg(&mag_decl);
289 
290  uint32_t fix_status = (control_mode >> 2) & 1;
291 
292  if (ekf.get_terrain_valid()) {
293  terrain_valid = 1;
294  } else {
295  terrain_valid = 0;
296  }
297 
298  if (ekf.inertial_dead_reckoning()) {
299  dead_reckoning = 1;
300  } else {
301  dead_reckoning = 0;
302  }
303 
304  pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
305  &fix_status, &filter_fault_status, &gps_check_status, &soln_status,
306  &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &flow, &beta,
307  &mag_decl, &terrain_valid, &dead_reckoning);
308 }
309 
310 static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
311 {
312  float gps_drift[3], vibe[3];
313  bool gps_blocked;
314  uint8_t gps_blocked_b;
315  ekf.get_gps_drift_metrics(gps_drift, &gps_blocked);
316  ekf.get_imu_vibe_metrics(vibe);
317  gps_blocked_b = gps_blocked;
318 
319  pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID,
320  &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b,
321  &vibe[0], &vibe[1], &vibe[2]);
322 }
323 
324 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
325 {
326  uint32_t control_mode;
327  uint16_t filter_fault_status;
328  uint8_t mde = 0;
329  ekf.get_control_mode(&control_mode);
330  ekf.get_filter_fault_status(&filter_fault_status);
331 
332  // Check the alignment and if GPS is fused
333  if ((control_mode & 0x7) == 0x7) {
334  mde = 3;
335  } else if ((control_mode & 0x7) == 0x3) {
336  mde = 4;
337  } else {
338  mde = 2;
339  }
340 
341  // Check if there is a covariance error
342  if (filter_fault_status) {
343  mde = 6;
344  }
345 
346  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status);
347 }
348 
349 static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
350 {
351  float velNE_wind[2], tas;
352  uint8_t flags = 0x5;
353  float f_zero = 0;
354 
355  ekf.get_wind_velocity(velNE_wind);
356  ekf.get_true_airspeed(&tas);
357 
358  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &velNE_wind[1], &velNE_wind[0], &f_zero, &tas);
359 }
360 
361 static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
362 {
363  float accel_bias[3], gyro_bias[3], states[24];
364  ekf.get_accel_bias(accel_bias);
365  ekf.get_gyro_bias(gyro_bias);
366  ekf.get_state_delayed(states);
367 
368  pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias[0], &accel_bias[1], &accel_bias[2],
369  &gyro_bias[0], &gyro_bias[1], &gyro_bias[2], &states[19], &states[20], &states[21]);
370 }
371 #endif
372 
373 /* Initialize the EKF */
374 void ins_ekf2_init(void)
375 {
376  /* Get the ekf parameters */
377  ekf_params = ekf.getParamHandle();
378  ekf_params->mag_fusion_type = MAG_FUSE_TYPE_HEADING;
379  ekf_params->is_moving_scaler = 0.8f;
380  ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
381  ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
382  ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
383 
384  /* Set optical flow parameters */
385  ekf_params->flow_qual_min = INS_EKF2_MIN_FLOW_QUALITY;
386  ekf_params->flow_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
387  ekf_params->range_delay_ms = INS_EKF2_FLOW_SENSOR_DELAY;
388  ekf_params->flow_noise = INS_EKF2_FLOW_NOISE;
389  ekf_params->flow_noise_qual_min = INS_EKF2_FLOW_NOISE_QMIN;
390  ekf_params->flow_innov_gate = INS_EKF2_FLOW_INNOV_GATE;
391 
392  /* Set flow sensor offset from IMU position in xyz (m) */
396  ekf_params->flow_pos_body = {0.001f*ekf2.offset_x, 0.001f*ekf2.offset_y, 0.001f*ekf2.offset_z};
397 
398  /* Set range as default AGL measurement if possible */
399  ekf_params->range_aid = INS_EKF2_RANGE_MAIN_AGL;
400 
401  /* Initialize struct */
402  ekf2.ltp_stamp = 0;
403  ekf2.accel_stamp = 0;
404  ekf2.gyro_stamp = 0;
405  ekf2.flow_stamp = 0;
406  ekf2.gyro_valid = false;
407  ekf2.accel_valid = false;
408  ekf2.got_imu_data = false;
410 
411  /* Initialize the range sensor limits */
412  ekf.set_rangefinder_limits(INS_EKF2_SONAR_MIN_RANGE, INS_EKF2_SONAR_MAX_RANGE);
413 
414  /* Initialize the flow sensor limits */
416 
417 #if PERIODIC_TELEMETRY
421  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
424 #endif
425 
426  /*
427  * Subscribe to scaled IMU measurements and attach callbacks
428  */
429  AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
430  AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
431  AbiBindMsgIMU_GYRO_INT32(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
432  AbiBindMsgIMU_ACCEL_INT32(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
433  AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
434  AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
435  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
436  AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
437 }
438 
439 /* Update the INS state */
440 void ins_ekf2_update(void)
441 {
442  /* Set EKF settings */
443  ekf.set_in_air_status(autopilot_in_flight());
444 
445  /* Update the EKF */
446  if (ekf2.got_imu_data && ekf.update()) {
447  filter_control_status_u control_status;
448  ekf.get_control_mode(&control_status.value);
449 
450  // Only publish position after successful alignment
451  if (control_status.flags.tilt_align) {
452  /* Get the position */
453  float pos_f[3] = {};
454  struct NedCoor_f pos;
455  ekf.get_position(pos_f);
456  pos.x = pos_f[0];
457  pos.y = pos_f[1];
458  pos.z = pos_f[2];
459 
460  // Publish to the state
461  stateSetPositionNed_f(&pos);
462 
463  /* Get the velocity in NED frame */
464  float vel_f[3] = {};
465  struct NedCoor_f speed;
466  ekf.get_velocity(vel_f);
467  speed.x = vel_f[0];
468  speed.y = vel_f[1];
469  speed.z = vel_f[2];
470 
471  // Publish to state
472  stateSetSpeedNed_f(&speed);
473 
474  /* Get the accelrations in NED frame */
475  float vel_deriv_f[3] = {};
476  struct NedCoor_f accel;
477  ekf.get_vel_deriv_ned(vel_deriv_f);
478  accel.x = vel_deriv_f[0];
479  accel.y = vel_deriv_f[1];
480  accel.z = vel_deriv_f[2];
481 
482  // Publish to state
483  stateSetAccelNed_f(&accel);
484 
485  /* Get local origin */
486  // Position of local NED origin in GPS / WGS84 frame
487  struct map_projection_reference_s ekf_origin = {};
488  float ref_alt;
489  struct LlaCoor_i lla_ref;
490  uint64_t origin_time;
491 
492  // Only update the origin when the state estimator has updated the origin
493  bool ekf_origin_valid = ekf.get_ekf_origin(&origin_time, &ekf_origin, &ref_alt);
494  if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
495  lla_ref.lat = ekf_origin.lat_rad * 180.0 / M_PI * 1e7; // Reference point latitude in degrees
496  lla_ref.lon = ekf_origin.lon_rad * 180.0 / M_PI * 1e7; // Reference point longitude in degrees
497  lla_ref.alt = ref_alt * 1000.0;
498  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
500 
501  /* update local ENU coordinates of global waypoints */
503 
504  ekf2.ltp_stamp = origin_time;
505  }
506  }
507  }
508 
509 #if defined SITL && USE_NPS
510  if (nps_bypass_ins) {
512  }
513 #endif
514 
515  ekf2.got_imu_data = false;
516 }
517 
519 {
520  ekf_params->mag_fusion_type = ekf2_params.mag_fusion_type = unk;
521 }
522 
524 {
525  if (mode) {
526  ekf_params->fusion_mode = ekf2_params.fusion_mode = (MASK_USE_OF | MASK_USE_GPSYAW);
527  } else {
528  ekf_params->fusion_mode = ekf2_params.fusion_mode = INS_EKF2_FUSION_MODE;
529  }
530 }
531 
536 {
537  imuSample imu_sample;
538  imu_sample.time_us = stamp;
539  imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
540  imu_sample.delta_ang = Vector3f{ekf2.gyro.p, ekf2.gyro.q, ekf2.gyro.r} * imu_sample.delta_ang_dt;
541  imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
542  imu_sample.delta_vel = Vector3f{ekf2.accel.x, ekf2.accel.y, ekf2.accel.z} * imu_sample.delta_vel_dt;
543  ekf.setIMUData(imu_sample);
544 
545  if (ekf.attitude_valid()) {
546  // Calculate the quaternion
547  struct FloatQuat ltp_to_body_quat;
548  const Quatf att_q{ekf.calculate_quaternion()};
549  ltp_to_body_quat.qi = att_q(0);
550  ltp_to_body_quat.qx = att_q(1);
551  ltp_to_body_quat.qy = att_q(2);
552  ltp_to_body_quat.qz = att_q(3);
553 
554  // Publish it to the state
555  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
556 
557  /* Check the quaternion reset state */
558  float delta_q_reset[4];
559  uint8_t quat_reset_counter;
560  ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
561 
562 #ifndef NO_RESET_UPDATE_SETPOINT_HEADING
563 
564  if (ekf2.quat_reset_counter < quat_reset_counter) {
565  float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
566 #if defined STABILIZATION_ATTITUDE_TYPE_INT
568 #else
569  stab_att_sp_euler.psi += psi;
570 #endif
571  guidance_h.sp.heading += psi;
572  guidance_h.rc_sp.psi += psi;
576  ekf2.quat_reset_counter = quat_reset_counter;
577  }
578 #endif
579 
580  /* Get in-run gyro bias */
581  struct FloatRates body_rates;
582  float gyro_bias[3];
583  ekf.get_gyro_bias(gyro_bias);
584  body_rates.p = ekf2.gyro.p - gyro_bias[0];
585  body_rates.q = ekf2.gyro.q - gyro_bias[1];
586  body_rates.r = ekf2.gyro.r - gyro_bias[2];
587 
588  // Publish it to the state
589  stateSetBodyRates_f(&body_rates);
590 
591  /* Get the in-run acceleration bias */
592  struct Int32Vect3 accel;
593  float accel_bias[3];
594  ekf.get_accel_bias(accel_bias);
595  accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias[0]);
596  accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias[1]);
597  accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias[2]);
598 
599  // Publish it to the state
600  stateSetAccelBody_i(&accel);
601  }
602 
603  ekf2.gyro_valid = false;
604  ekf2.accel_valid = false;
605  ekf2.got_imu_data = true;
606 }
607 
608 /* Update INS based on Baro information */
609 static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
610 {
611  // Calculate the air density
612  float rho = pprz_isa_density_of_pressure(pressure,
613  20.0f); // TODO: add temperature compensation now set to 20 degree celcius
614  ekf.set_air_density(rho);
615 
616  // Calculate the height above mean sea level based on pressure
617  float height_amsl_m = pprz_isa_height_of_pressure_full(pressure,
618  101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h
619  ekf.setBaroData(stamp, height_amsl_m);
620 }
621 
622 /* Update INS based on AGL information */
623 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
624 {
625  ekf.setRangeData(stamp, distance);
626 }
627 
628 /* Update INS based on Gyro information */
629 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
630  uint32_t stamp, struct Int32Rates *gyro)
631 {
632  FloatRates imu_rate;
633  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
634 
635  // Convert Gyro information to float
636  RATES_FLOAT_OF_BFP(imu_rate, *gyro);
637 
638  // Rotate with respect to Body To IMU
639  float_rmat_transp_ratemult(&ekf2.gyro, body_to_imu_rmat, &imu_rate);
640 
641  // Calculate the Gyro interval
642  if (ekf2.gyro_stamp > 0) {
643  ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
644  ekf2.gyro_valid = true;
645  }
646  ekf2.gyro_stamp = stamp;
647 
648  /* When Gyro and accelerometer are valid enter it into the EKF */
649  if (ekf2.gyro_valid && ekf2.accel_valid) {
651  }
652 }
653 
654 /* Update INS based on Accelerometer information */
655 static void accel_cb(uint8_t sender_id __attribute__((unused)),
656  uint32_t stamp, struct Int32Vect3 *accel)
657 {
658  struct FloatVect3 accel_imu;
659  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
660 
661  // Convert Accelerometer information to float
662  ACCELS_FLOAT_OF_BFP(accel_imu, *accel);
663 
664  // Rotate with respect to Body To IMU
665  float_rmat_transp_vmult(&ekf2.accel, body_to_imu_rmat, &accel_imu);
666 
667  // Calculate the Accelerometer interval
668  if (ekf2.accel_stamp > 0) {
669  ekf2.accel_dt = stamp - ekf2.accel_stamp;
670  ekf2.accel_valid = true;
671  }
672  ekf2.accel_stamp = stamp;
673 
674  /* When Gyro and accelerometer are valid enter it into the EKF */
675  if (ekf2.gyro_valid && ekf2.accel_valid) {
677  }
678 }
679 
680 /* Update INS based on Magnetometer information */
681 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
682  uint32_t stamp,
683  struct Int32Vect3 *mag)
684 {
685  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
686  struct FloatVect3 mag_gauss, mag_body;
687 
688  // Convert Magnetometer information to float and to radius 0.2f
689  MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
690  mag_gauss.x *= 0.4f;
691  mag_gauss.y *= 0.4f;
692  mag_gauss.z *= 0.4f;
693 
694  // Rotate with respect to Body To IMU
695  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_gauss);
696 
697  // Publish information to the EKF
698  float mag_r[3];
699  mag_r[0] = mag_body.x;
700  mag_r[1] = mag_body.y;
701  mag_r[2] = mag_body.z;
702 
703  ekf.setMagData(stamp, mag_r);
704  ekf2.got_imu_data = true;
705 }
706 
707 /* Update INS based on GPS information */
708 static void gps_cb(uint8_t sender_id __attribute__((unused)),
709  uint32_t stamp,
710  struct GpsState *gps_s)
711 {
712  gps_msg.time_usec = stamp;
713  gps_msg.lat = gps_s->lla_pos.lat;
714  gps_msg.lon = gps_s->lla_pos.lon;
715  gps_msg.alt = gps_s->hmsl;
716 #if INS_EKF2_GPS_COURSE_YAW
717  gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
718  gps_msg.yaw_offset = 0;
719 #else
720  gps_msg.yaw = NAN;
721  gps_msg.yaw_offset = NAN;
722 #endif
723  gps_msg.fix_type = gps_s->fix;
724  gps_msg.eph = gps_s->hacc / 100.0;
725  gps_msg.epv = gps_s->vacc / 100.0;
726  gps_msg.sacc = gps_s->sacc / 100.0;
727  gps_msg.vel_m_s = gps_s->gspeed / 100.0;
728  gps_msg.vel_ned[0] = (gps_s->ned_vel.x) / 100.0;
729  gps_msg.vel_ned[1] = (gps_s->ned_vel.y) / 100.0;
730  gps_msg.vel_ned[2] = (gps_s->ned_vel.z) / 100.0;
731  gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
732  gps_msg.nsats = gps_s->num_sv;
733  gps_msg.gdop = 0.0f;
734 
735  ekf.setGpsData(stamp, gps_msg);
736 }
737 
738 /* Save the Body to IMU information */
739 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
740  struct FloatQuat *q_b2i_f)
741 {
743 }
744 
745 /* Update INS based on Optical Flow information */
746 static void optical_flow_cb(uint8_t sender_id __attribute__((unused)),
747  uint32_t stamp,
748  int32_t flow_x,
749  int32_t flow_y,
750  int32_t flow_der_x __attribute__((unused)),
751  int32_t flow_der_y __attribute__((unused)),
752  float quality,
753  float size_divergence __attribute__((unused)))
754 {
755  // update time
756  ekf2.flow_dt = stamp - ekf2.flow_stamp;
757  ekf2.flow_stamp = stamp;
758 
759  /* Build integrated flow and gyro messages for filter
760  NOTE: pure rotations should result in same flow_x and
761  gyro_roll and same flow_y and gyro_pitch */
762  ekf2.flow_quality = quality;
763  ekf2.flow_x = RadOfDeg(flow_y) * (1e-6 * ekf2.flow_dt); // INTEGRATED FLOW AROUND Y AXIS (RIGHT -X, LEFT +X)
764  ekf2.flow_y = - RadOfDeg(flow_x) * (1e-6 * ekf2.flow_dt); // INTEGRATED FLOW AROUND X AXIS (FORWARD +Y, BACKWARD -Y)
765  ekf2.gyro_roll = NAN;
766  ekf2.gyro_pitch = NAN;
767  ekf2.gyro_yaw = NAN;
768 
769  /* once callback initiated, build the
770  optical flow message with what is received */
771  flow_msg.quality = quality; // quality indicator between 0 and 255
772  flow_msg.flowdata = Vector2f(ekf2.flow_x, ekf2.flow_y); // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
773  flow_msg.gyrodata = Vector3f{ekf2.gyro_roll, ekf2.gyro_pitch, ekf2.gyro_yaw}; // measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
774  flow_msg.dt = ekf2.flow_dt; // amount of integration time (usec)
775 
776  // update the optical flow data based on the callback
777  ekf.setOpticalFlowData(stamp, &flow_msg);
778 }
#define INS_EKF2_FLOW_OFFSET_Z
Definition: ins_ekf2.cpp:164
bool nps_bypass_ins
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
int32_t z
in centimeters
struct flow_message flow_msg
Definition: ins_ekf2.cpp:198
angular rates
bool gyro_valid
Definition: ins_ekf2.cpp:225
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
void ins_ekf2_change_param(int32_t unk)
Definition: ins_ekf2.cpp:518
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:302
float y
in meters
#define INS_EKF2_MIN_FLOW_QUALITY
Definition: ins_ekf2.cpp:140
uint32_t gyro_dt
Definition: ins_ekf2.cpp:216
definition of the local (flat earth) coordinate system
float offset_x
Definition: ins_ekf2.cpp:235
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_ekf2.cpp:739
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
int32_t y
in centimeters
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:101
static struct nodeState states[UWB_SERIAL_COMM_DIST_NUM_NODES]
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
uint8_t quat_reset_counter
Definition: ins_ekf2.cpp:242
float r
in rad/s
uint64_t ltp_stamp
Definition: ins_ekf2.cpp:243
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
Main include for ABI (AirBorneInterface).
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:102
#define INS_EKF2_MAX_FLOW_RATE
Definition: ins_ekf2.cpp:146
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ins_ekf2.cpp:681
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:535
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
#define INS_EKF2_GPS_ID
Definition: ins_ekf2.cpp:122
float psi
in radians
#define INS_EKF2_FLOW_OFFSET_Y
Definition: ins_ekf2.cpp:158
struct ekf2_parameters_t ekf2_params
Definition: ins_ekf2.cpp:259
#define INS_EKF2_GPS_CHECK_MASK
The EKF2 GPS checks before initialization.
Definition: ins_ekf2.cpp:64
void stabilization_attitude_enter(void)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_ekf2.cpp:609
float flow_x
Definition: ins_ekf2.cpp:230
static parameters * ekf_params
The EKF parameters.
Definition: ins_ekf2.cpp:254
float flow_y
Definition: ins_ekf2.cpp:231
vector in Latitude, Longitude and Altitude
float q
in rad/s
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
float p
in rad/s
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:746
int32_t z
Down.
float gyro_yaw
Definition: ins_ekf2.cpp:234
float gyro_pitch
Definition: ins_ekf2.cpp:233
struct EcefCoor_i ecef
Reference point in ecef.
int32_t hmsl
Height above mean sea level in mm.
int32_t mag_fusion_type
Definition: ins_ekf2.h:40
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct gps_message gps_msg
Definition: ins_ekf2.cpp:197
float z
in meters
static abi_event agl_ev
Definition: ins_ekf2.cpp:187
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
Roation quaternion.
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:310
int32_t y
East.
bool accel_valid
Definition: ins_ekf2.cpp:226
#define INS_EKF2_ACCEL_ID
Definition: ins_ekf2.cpp:110
uint32_t accel_dt
Definition: ins_ekf2.cpp:218
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ins_ekf2.cpp:629
struct LtpDef_i ltp_def
Definition: ins_ekf2.cpp:244
struct ekf2_t ekf2
Local EKF2 status structure.
Definition: ins_ekf2.cpp:255
#define AHRS_COMP_ID_EKF2
Definition: ahrs.h:44
INS based in the EKF2 of PX4.
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
int32_t fusion_mode
Definition: ins_ekf2.h:41
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
static Ekf ekf
EKF class itself.
Definition: ins_ekf2.cpp:253
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
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
vector in North East Down coordinates Units: meters
#define INS_EKF2_VDIST_SENSOR_TYPE
The EKF2 primary vertical distance sensor type.
Definition: ins_ekf2.cpp:58
#define INS_EKF2_FLOW_SENSOR_DELAY
Definition: ins_ekf2.cpp:134
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:87
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
Paparazzi atmospheric pressure conversion utilities.
int32_t x
North.
#define INS_EKF2_AGL_ID
default AGL sensor to use in INS
Definition: ins_ekf2.cpp:70
static abi_event gps_ev
Definition: ins_ekf2.cpp:192
static abi_event baro_ev
Definition: ins_ekf2.cpp:188
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
uint32_t flow_dt
Definition: ins_ekf2.cpp:220
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t lon
in degrees*1e7
#define INS_EKF2_SONAR_MAX_RANGE
Default AGL sensor maximum range.
Definition: ins_ekf2.cpp:82
void ins_ekf2_update(void)
Definition: ins_ekf2.cpp:440
void ins_ekf2_init(void)
Definition: ins_ekf2.cpp:374
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
float offset_z
Definition: ins_ekf2.cpp:237
#define INS_EKF2_BARO_ID
default barometer to use in INS
Definition: ins_ekf2.cpp:97
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:274
#define INS_EKF2_FLOW_OFFSET_X
Definition: ins_ekf2.cpp:152
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 stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:264
#define ANGLE_BFP_OF_REAL(_af)
#define INS_EKF2_MAG_ID
Definition: ins_ekf2.cpp:116
#define INS_EKF2_RANGE_MAIN_AGL
If enabled uses radar sensor as primary AGL source, if possible.
Definition: ins_ekf2.cpp:88
uint32_t gyro_stamp
Definition: ins_ekf2.cpp:215
Core autopilot interface common to all firmwares.
void ins_ekf2_remove_gps(int32_t mode)
Definition: ins_ekf2.cpp:523
static abi_event body_to_imu_ev
Definition: ins_ekf2.cpp:193
static abi_event mag_ev
Definition: ins_ekf2.cpp:191
uint32_t flow_stamp
Definition: ins_ekf2.cpp:219
#define INS_EKF2_GYRO_ID
Definition: ins_ekf2.cpp:104
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: ins_ekf2.cpp:623
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
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
#define INS_EKF2_FLOW_INNOV_GATE
Definition: ins_ekf2.cpp:182
float offset_y
Definition: ins_ekf2.cpp:236
float flow_quality
Definition: ins_ekf2.cpp:229
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:361
#define INS_EKF2_OF_ID
Definition: ins_ekf2.cpp:128
float gyro_roll
Definition: ins_ekf2.cpp:232
static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:349
static abi_event optical_flow_ev
Definition: ins_ekf2.cpp:194
rotation matrix
struct OrientationReps body_to_imu
Definition: ins_ekf2.cpp:245
static uint8_t ahrs_ekf2_id
Component ID for EKF.
Definition: ins_ekf2.cpp:256
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
int32_t x
in centimeters
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
#define INS_EKF2_FLOW_NOISE_QMIN
Definition: ins_ekf2.cpp:176
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define INS_EKF2_FUSION_MODE
For SITL and NPS we need special includes.
Definition: ins_ekf2.cpp:52
FloatRates gyro
Definition: ins_ekf2.cpp:223
static abi_event accel_ev
Definition: ins_ekf2.cpp:190
struct FloatEulers rc_sp
Definition: guidance_h.h:106
bool got_imu_data
Definition: ins_ekf2.cpp:246
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
static abi_event gyro_ev
Definition: ins_ekf2.cpp:189
#define ACCEL_BFP_OF_REAL(_af)
float flow_innov
Definition: ins_ekf2.cpp:240
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
int32_t lat
in degrees*1e7
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
uint8_t fix
status of fix
Definition: gps.h:107
unsigned long long uint64_t
Definition: vl53l1_types.h:72
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
FloatVect3 accel
Definition: ins_ekf2.cpp:224
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
angular rates
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:324
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_ekf2.cpp:708
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
void sim_overwrite_ins(void)
uint32_t accel_stamp
Definition: ins_ekf2.cpp:217
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
#define INS_EKF2_FLOW_NOISE
Definition: ins_ekf2.cpp:170
#define INS_EKF2_SONAR_MIN_RANGE
Default AGL sensor minimum range.
Definition: ins_ekf2.cpp:76
float x
in meters
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_ekf2.cpp:655