Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
30 #include "subsystems/abi.h"
31 #include "stabilization/stabilization_attitude.h"
32 #include "generated/airframe.h"
33 #include "EKF/ekf.h"
34 #include "math/pprz_isa.h"
35 #include "mcu_periph/sys_time.h"
36 #include "autopilot.h"
37 
39 #if defined SITL && USE_NPS
40 #include "nps_autopilot.h"
41 #include <stdio.h>
42 #endif
43 
45 #if USE_INS_NAV_INIT
46 #error INS initialization from flight plan is not yet supported
47 #endif
48 
50 #ifndef INS_EKF2_AGL_ID
51 #define INS_EKF2_AGL_ID ABI_BROADCAST
52 #endif
53 PRINT_CONFIG_VAR(INS_EKF2_AGL_ID)
54 
55 
56 #ifndef INS_SONAR_MIN_RANGE
57 #define INS_SONAR_MIN_RANGE 0.001
58 #endif
59 PRINT_CONFIG_VAR(INS_SONAR_MIN_RANGE)
60 
61 
62 #ifndef INS_SONAR_MAX_RANGE
63 #define INS_SONAR_MAX_RANGE 4.0
64 #endif
65 PRINT_CONFIG_VAR(INS_SONAR_MAX_RANGE)
66 
67 
68 #ifndef INS_EKF2_BARO_ID
69 #if USE_BARO_BOARD
70 #define INS_EKF2_BARO_ID BARO_BOARD_SENDER_ID
71 #else
72 #define INS_EKF2_BARO_ID ABI_BROADCAST
73 #endif
74 #endif
75 PRINT_CONFIG_VAR(INS_EKF2_BARO_ID)
76 
77 /* default Gyro to use in INS */
78 #ifndef INS_EKF2_GYRO_ID
79 #define INS_EKF2_GYRO_ID ABI_BROADCAST
80 #endif
81 PRINT_CONFIG_VAR(INS_EKF2_GYRO_ID)
82 
83 /* default Accelerometer to use in INS */
84 #ifndef INS_EKF2_ACCEL_ID
85 #define INS_EKF2_ACCEL_ID ABI_BROADCAST
86 #endif
87 PRINT_CONFIG_VAR(INS_EKF2_ACCEL_ID)
88 
89 /* default Magnetometer to use in INS */
90 #ifndef INS_EKF2_MAG_ID
91 #define INS_EKF2_MAG_ID ABI_BROADCAST
92 #endif
93 PRINT_CONFIG_VAR(INS_EKF2_MAG_ID)
94 
95 /* default GPS to use in INS */
96 #ifndef INS_EKF2_GPS_ID
97 #define INS_EKF2_GPS_ID GPS_MULTI_ID
98 #endif
99 PRINT_CONFIG_VAR(INS_EKF2_GPS_ID)
100 
101 /* All registered ABI events */
109 struct gps_message gps_msg = {};
110 
111 /* All ABI callbacks */
112 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
113 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
114 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
115 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
116 static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
117 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
118 static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
119 
120 /* Main EKF2 structure for keeping track of the status */
121 struct ekf2_t {
130 
132 
135 
138 };
139 
140 /* Static local functions */
141 static void ins_ekf2_publish_attitude(uint32_t stamp);
142 
143 /* Static local variables */
144 static Ekf ekf;
145 static parameters *ekf_params;
146 struct ekf2_t ekf2;
148 
149 /* External paramters */
151 
152 #if PERIODIC_TELEMETRY
154 
155 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
156 {
157  float qfe = 101325.0; //TODO: this is qnh not qfe?
158  if (ekf2.ltp_stamp > 0)
159  pprz_msg_send_INS_REF(trans, dev, AC_ID,
162  &ekf2.ltp_def.hmsl, &qfe);
163 }
164 
165 static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
166 {
167  uint16_t gps_check_status, filter_fault_status, soln_status;
168  uint32_t control_mode;
169  ekf.get_gps_check_status(&gps_check_status);
170  ekf.get_filter_fault_status(&filter_fault_status);
171  ekf.get_control_mode(&control_mode);
172  ekf.get_ekf_soln_status(&soln_status);
173 
174  uint16_t innov_test_status;
175  float mag, vel, pos, hgt, tas, hagl, beta, mag_decl;
176  ekf.get_innovation_test_status(&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta);
177  ekf.get_mag_decl_deg(&mag_decl);
178  pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
179  &control_mode, &filter_fault_status, &gps_check_status, &soln_status,
180  &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta,
181  &mag_decl);
182 }
183 
184 static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
185 {
186  float gps_drift[3], vibe[3];
187  bool gps_blocked;
188  uint8_t gps_blocked_b;
189  ekf.get_gps_drift_metrics(gps_drift, &gps_blocked);
190  ekf.get_imu_vibe_metrics(vibe);
191  gps_blocked_b = gps_blocked;
192 
193  pprz_msg_send_INS_EKF2_EXT(trans, dev, AC_ID,
194  &gps_drift[0], &gps_drift[1], &gps_drift[2], &gps_blocked_b,
195  &vibe[0], &vibe[1], &vibe[2]);
196 }
197 
198 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
199 {
200  uint32_t control_mode;
201  uint16_t filter_fault_status;
202  uint8_t mde = 0;
203  ekf.get_control_mode(&control_mode);
204  ekf.get_filter_fault_status(&filter_fault_status);
205 
206  // Check the alignment and if GPS is fused
207  if ((control_mode & 0x7) == 0x7) {
208  mde = 3;
209  } else if ((control_mode & 0x7) == 0x3) {
210  mde = 4;
211  } else {
212  mde = 2;
213  }
214 
215  // Check if there is a covariance error
216  if (filter_fault_status) {
217  mde = 6;
218  }
219 
220  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status);
221 }
222 
223 static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
224 {
225  float velNE_wind[2], tas;
226  uint8_t flags = 0x5;
227  float f_zero = 0;
228 
229  ekf.get_wind_velocity(velNE_wind);
230  ekf.get_true_airspeed(&tas);
231 
232  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &velNE_wind[1], &velNE_wind[0], &f_zero, &tas);
233 }
234 
235 static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
236 {
237  float accel_bias[3], gyro_bias[3], states[24];
238  ekf.get_accel_bias(accel_bias);
239  ekf.get_gyro_bias(gyro_bias);
240  ekf.get_state_delayed(states);
241 
242  pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID, &accel_bias[0], &accel_bias[1], &accel_bias[2],
243  &gyro_bias[0], &gyro_bias[1], &gyro_bias[2], &states[19], &states[20], &states[21]);
244 }
245 #endif
246 
247 /* Initialize the EKF */
248 void ins_ekf2_init(void)
249 {
250  /* Get the ekf parameters */
251  ekf_params = ekf.getParamHandle();
252  ekf_params->mag_fusion_type = MAG_FUSE_TYPE_HEADING;
253  ekf_params->is_moving_scaler = 0.8f;
254 
255  /* Initialize struct */
256  ekf2.ltp_stamp = 0;
257  ekf2.accel_stamp = 0;
258  ekf2.gyro_stamp = 0;
259  ekf2.gyro_valid = false;
260  ekf2.accel_valid = false;
261  ekf2.got_imu_data = false;
263 
264  /* Initialize the range sensor limits */
265  ekf.set_rangefinder_limits(INS_SONAR_MIN_RANGE, INS_SONAR_MAX_RANGE);
266 
267 #if PERIODIC_TELEMETRY
271  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
274 #endif
275 
276  /*
277  * Subscribe to scaled IMU measurements and attach callbacks
278  */
279  AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
280  AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
281  AbiBindMsgIMU_GYRO_INT32(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
282  AbiBindMsgIMU_ACCEL_INT32(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
283  AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
284  AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
285  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
286 }
287 
288 /* Update the INS state */
289 void ins_ekf2_update(void)
290 {
291  /* Set EKF settings */
292  ekf.set_in_air_status(autopilot_in_flight());
293 
294  /* Update the EKF */
295  if (ekf2.got_imu_data && ekf.update()) {
296  filter_control_status_u control_status;
297  ekf.get_control_mode(&control_status.value);
298 
299  // Only publish position after successful alignment
300  if (control_status.flags.tilt_align) {
301  /* Get the position */
302  float pos_f[3] = {};
303  struct NedCoor_f pos;
304  ekf.get_position(pos_f);
305  pos.x = pos_f[0];
306  pos.y = pos_f[1];
307  pos.z = pos_f[2];
308 
309  // Publish to the state
310  stateSetPositionNed_f(&pos);
311 
312  /* Get the velocity in NED frame */
313  float vel_f[3] = {};
314  struct NedCoor_f speed;
315  ekf.get_velocity(vel_f);
316  speed.x = vel_f[0];
317  speed.y = vel_f[1];
318  speed.z = vel_f[2];
319 
320  // Publish to state
321  stateSetSpeedNed_f(&speed);
322 
323  /* Get the accelrations in NED frame */
324  float vel_deriv_f[3] = {};
325  struct NedCoor_f accel;
326  ekf.get_vel_deriv_ned(vel_deriv_f);
327  accel.x = vel_deriv_f[0];
328  accel.y = vel_deriv_f[1];
329  accel.z = vel_deriv_f[2];
330 
331  // Publish to state
332  stateSetAccelNed_f(&accel);
333 
334  /* Get local origin */
335  // Position of local NED origin in GPS / WGS84 frame
336  struct map_projection_reference_s ekf_origin = {};
337  float ref_alt;
338  struct LlaCoor_i lla_ref;
339  uint64_t origin_time;
340 
341  // Only update the origin when the state estimator has updated the origin
342  bool ekf_origin_valid = ekf.get_ekf_origin(&origin_time, &ekf_origin, &ref_alt);
343  if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
344  lla_ref.lat = ekf_origin.lat_rad * 180.0 / M_PI * 1e7; // Reference point latitude in degrees
345  lla_ref.lon = ekf_origin.lon_rad * 180.0 / M_PI * 1e7; // Reference point longitude in degrees
346  lla_ref.alt = ref_alt * 1000.0;
347  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
349 
350  ekf2.ltp_stamp = origin_time;
351  }
352  }
353  }
354 
355 #if defined SITL && USE_NPS
356  if (nps_bypass_ins) {
358  }
359 #endif
360 
361  ekf2.got_imu_data = false;
362 }
363 
365  ekf_params->mag_fusion_type = ekf2_params.mag_fusion_type = unk;
366 }
367 
372 {
373  imuSample imu_sample;
374  imu_sample.time_us = stamp;
375  imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
376  imu_sample.delta_ang = Vector3f{ekf2.gyro.p, ekf2.gyro.q, ekf2.gyro.r} * imu_sample.delta_ang_dt;
377  imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
378  imu_sample.delta_vel = Vector3f{ekf2.accel.x, ekf2.accel.y, ekf2.accel.z} * imu_sample.delta_vel_dt;
379  ekf.setIMUData(imu_sample);
380 
381  if (ekf.attitude_valid()) {
382  // Calculate the quaternion
383  struct FloatQuat ltp_to_body_quat;
384  const Quatf att_q{ekf.calculate_quaternion()};
385  ltp_to_body_quat.qi = att_q(0);
386  ltp_to_body_quat.qx = att_q(1);
387  ltp_to_body_quat.qy = att_q(2);
388  ltp_to_body_quat.qz = att_q(3);
389 
390  // Publish it to the state
391  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
392 
393  /* Check the quaternion reset state */
394  float delta_q_reset[4];
395  uint8_t quat_reset_counter;
396  ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
397 
398 #ifndef NO_RESET_UPDATE_SETPOINT_HEADING
399 
400  if (ekf2.quat_reset_counter < quat_reset_counter) {
401  float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
402 #if defined STABILIZATION_ATTITUDE_TYPE_INT
404 #else
405  stab_att_sp_euler.psi += psi;
406 #endif
407  guidance_h.sp.heading += psi;
408  guidance_h.rc_sp.psi += psi;
412  ekf2.quat_reset_counter = quat_reset_counter;
413  }
414 #endif
415 
416  /* Get in-run gyro bias */
417  struct FloatRates body_rates;
418  float gyro_bias[3];
419  ekf.get_gyro_bias(gyro_bias);
420  body_rates.p = ekf2.gyro.p - gyro_bias[0];
421  body_rates.q = ekf2.gyro.q - gyro_bias[1];
422  body_rates.r = ekf2.gyro.r - gyro_bias[2];
423 
424  // Publish it to the state
425  stateSetBodyRates_f(&body_rates);
426 
427  /* Get the in-run acceleration bias */
428  struct Int32Vect3 accel;
429  float accel_bias[3];
430  ekf.get_accel_bias(accel_bias);
431  accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias[0]);
432  accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias[1]);
433  accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias[2]);
434 
435  // Publish it to the state
436  stateSetAccelBody_i(&accel);
437  }
438 
439  ekf2.gyro_valid = false;
440  ekf2.accel_valid = false;
441  ekf2.got_imu_data = true;
442 }
443 
444 /* Update INS based on Baro information */
445 static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
446 {
447  // Calculate the air density
448  float rho = pprz_isa_density_of_pressure(pressure,
449  20.0f); // TODO: add temperature compensation now set to 20 degree celcius
450  ekf.set_air_density(rho);
451 
452  // Calculate the height above mean sea level based on pressure
453  float height_amsl_m = pprz_isa_height_of_pressure_full(pressure,
454  101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h
455  ekf.setBaroData(stamp, height_amsl_m);
456 }
457 
458 /* Update INS based on AGL information */
459 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
460 {
461  ekf.setRangeData(stamp, distance);
462 }
463 
464 /* Update INS based on Gyro information */
465 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
466  uint32_t stamp, struct Int32Rates *gyro)
467 {
468  FloatRates imu_rate;
469  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
470 
471  // Convert Gyro information to float
472  RATES_FLOAT_OF_BFP(imu_rate, *gyro);
473 
474  // Rotate with respect to Body To IMU
475  float_rmat_transp_ratemult(&ekf2.gyro, body_to_imu_rmat, &imu_rate);
476 
477  // Calculate the Gyro interval
478  if (ekf2.gyro_stamp > 0) {
479  ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
480  ekf2.gyro_valid = true;
481  }
482  ekf2.gyro_stamp = stamp;
483 
484  /* When Gyro and accelerometer are valid enter it into the EKF */
485  if (ekf2.gyro_valid && ekf2.accel_valid) {
487  }
488 }
489 
490 /* Update INS based on Accelerometer information */
491 static void accel_cb(uint8_t sender_id __attribute__((unused)),
492  uint32_t stamp, struct Int32Vect3 *accel)
493 {
494  struct FloatVect3 accel_imu;
495  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
496 
497  // Convert Accelerometer information to float
498  ACCELS_FLOAT_OF_BFP(accel_imu, *accel);
499 
500  // Rotate with respect to Body To IMU
501  float_rmat_transp_vmult(&ekf2.accel, body_to_imu_rmat, &accel_imu);
502 
503  // Calculate the Accelerometer interval
504  if (ekf2.accel_stamp > 0) {
505  ekf2.accel_dt = stamp - ekf2.accel_stamp;
506  ekf2.accel_valid = true;
507  }
508  ekf2.accel_stamp = stamp;
509 
510  /* When Gyro and accelerometer are valid enter it into the EKF */
511  if (ekf2.gyro_valid && ekf2.accel_valid) {
513  }
514 }
515 
516 /* Update INS based on Magnetometer information */
517 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
518  uint32_t stamp,
519  struct Int32Vect3 *mag)
520 {
521  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
522  struct FloatVect3 mag_gauss, mag_body;
523 
524  // Convert Magnetometer information to float and to radius 0.2f
525  MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
526  mag_gauss.x *= 0.4f;
527  mag_gauss.y *= 0.4f;
528  mag_gauss.z *= 0.4f;
529 
530  // Rotate with respect to Body To IMU
531  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_gauss);
532 
533  // Publish information to the EKF
534  float mag_r[3];
535  mag_r[0] = mag_body.x;
536  mag_r[1] = mag_body.y;
537  mag_r[2] = mag_body.z;
538 
539  ekf.setMagData(stamp, mag_r);
540  ekf2.got_imu_data = true;
541 }
542 
543 /* Update INS based on GPS information */
544 static void gps_cb(uint8_t sender_id __attribute__((unused)),
545  uint32_t stamp,
546  struct GpsState *gps_s)
547 {
548  gps_msg.time_usec = stamp;
549  gps_msg.lat = gps_s->lla_pos.lat;
550  gps_msg.lon = gps_s->lla_pos.lon;
551  gps_msg.alt = gps_s->hmsl;
552  gps_msg.yaw = NAN;
553  gps_msg.yaw_offset = NAN;
554  gps_msg.fix_type = gps_s->fix;
555  gps_msg.eph = gps_s->hacc / 100.0;
556  gps_msg.epv = gps_s->vacc / 100.0;
557  gps_msg.sacc = gps_s->sacc / 100.0;
558  gps_msg.vel_m_s = gps_s->gspeed / 100.0;
559  gps_msg.vel_ned[0] = (gps_s->ned_vel.x) / 100.0;
560  gps_msg.vel_ned[1] = (gps_s->ned_vel.y) / 100.0;
561  gps_msg.vel_ned[2] = (gps_s->ned_vel.z) / 100.0;
562  gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
563  gps_msg.nsats = gps_s->num_sv;
564  gps_msg.gdop = 0.0f;
565 
566  ekf.setGpsData(stamp, gps_msg);
567 }
568 
569 /* Save the Body to IMU information */
570 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
571  struct FloatQuat *q_b2i_f)
572 {
574 }
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
OrientationReps
Definition: pprz_orientation_conversion.h:79
uint16_t
unsigned short uint16_t
Definition: types.h:16
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_ekf2.cpp:544
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
pprz_isa_density_of_pressure
static float pprz_isa_density_of_pressure(float pressure, float temp)
Get the air density (rho) from a given pressure and temperature.
Definition: pprz_isa.h:193
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
states
static struct nodeState states[UWB_SERIAL_COMM_DIST_NUM_NODES]
Definition: decawave_anchorless_communication.c:83
ins_ekf2_change_param
void ins_ekf2_change_param(int32_t unk)
Definition: ins_ekf2.cpp:364
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
gyro_ev
static abi_event gyro_ev
Definition: ins_ekf2.cpp:104
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
send_ahrs_bias
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:235
NedCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:66
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
ekf2_t::gyro_dt
uint32_t gyro_dt
Definition: ins_ekf2.cpp:123
GpsState
data structure for GPS information
Definition: gps.h:87
ekf2_t::ltp_stamp
uint64_t ltp_stamp
Definition: ins_ekf2.cpp:133
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_ekf2.cpp:570
ekf2_params
struct ekf2_parameters_t ekf2_params
Definition: ins_ekf2.cpp:150
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
uint32_t
unsigned long uint32_t
Definition: types.h:18
HorizontalGuidanceSetpoint::heading
float heading
Definition: guidance_h.h:76
GpsState::vacc
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:102
nps_bypass_ins
bool nps_bypass_ins
Definition: nps_autopilot_fixedwing.c:65
ekf2_t::accel_valid
bool accel_valid
Definition: ins_ekf2.cpp:129
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_ekf2.cpp:445
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:155
nps_autopilot.h
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:198
INS_SONAR_MIN_RANGE
#define INS_SONAR_MIN_RANGE
Default AGL sensor minimum range.
Definition: ins_ekf2.cpp:57
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
ahrs_ekf2_id
static uint8_t ahrs_ekf2_id
Component ID for EKF.
Definition: ins_ekf2.cpp:147
gps_msg
struct gps_message gps_msg
Definition: ins_ekf2.cpp:109
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
ekf2_t::ltp_def
struct LtpDef_i ltp_def
Definition: ins_ekf2.cpp:134
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
autopilot_in_flight
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:257
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_float.c:120
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
GpsState::hacc
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:101
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:107
ekf2_t::gyro_valid
bool gyro_valid
Definition: ins_ekf2.cpp:128
send_ins_ekf2
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:165
ekf2_t::gyro_stamp
uint32_t gyro_stamp
Definition: ins_ekf2.cpp:122
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
INS_EKF2_ACCEL_ID
#define INS_EKF2_ACCEL_ID
Definition: ins_ekf2.cpp:85
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
ekf2_parameters_t
Definition: ins_ekf2.h:39
ekf2_t::accel_stamp
uint32_t accel_stamp
Definition: ins_ekf2.cpp:124
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:220
uint8_t
unsigned char uint8_t
Definition: types.h:14
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
INS_EKF2_MAG_ID
#define INS_EKF2_MAG_ID
Definition: ins_ekf2.cpp:91
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
ekf2_parameters_t::mag_fusion_type
int32_t mag_fusion_type
Definition: ins_ekf2.h:40
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
ins_ekf2.h
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
pprz_isa_height_of_pressure_full
static float pprz_isa_height_of_pressure_full(float pressure, float ref_p)
Get relative altitude from pressure (using full equation).
Definition: pprz_isa.h:146
agl_cb
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: ins_ekf2.cpp:459
ekf2_t::body_to_imu
struct OrientationReps body_to_imu
Definition: ins_ekf2.cpp:136
mag_ev
static abi_event mag_ev
Definition: ins_ekf2.cpp:106
autopilot.h
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
HorizontalGuidance::sp
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
ins_ekf2_publish_attitude
static void ins_ekf2_publish_attitude(uint32_t stamp)
Publish the attitude and get the new state Directly called after a succeslfull gyro+accel reading.
Definition: ins_ekf2.cpp:371
ekf2_t::got_imu_data
bool got_imu_data
Definition: ins_ekf2.cpp:137
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
ekf_params
static parameters * ekf_params
The EKF parameters.
Definition: ins_ekf2.cpp:145
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
gps_ev
static abi_event gps_ev
Definition: ins_ekf2.cpp:107
send_wind_info_ret
static void send_wind_info_ret(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:223
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ins_ekf2.cpp:108
INS_EKF2_GPS_ID
#define INS_EKF2_GPS_ID
Definition: ins_ekf2.cpp:97
sim_overwrite_ins
void sim_overwrite_ins(void)
Definition: nps_autopilot_fixedwing.c:268
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ins_ekf2.cpp:465
INS_EKF2_AGL_ID
#define INS_EKF2_AGL_ID
For SITL and NPS we need special includes.
Definition: ins_ekf2.cpp:51
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
agl_ev
static abi_event agl_ev
Definition: ins_ekf2.cpp:102
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
stabilization_attitude_enter
void stabilization_attitude_enter(void)
Definition: stabilization_attitude_euler_float.c:136
ekf2_t::accel
FloatVect3 accel
Definition: ins_ekf2.cpp:127
int32_t
signed long int32_t
Definition: types.h:19
ekf2
struct ekf2_t ekf2
Local EKF2 status structure.
Definition: ins_ekf2.cpp:146
INS_SONAR_MAX_RANGE
#define INS_SONAR_MAX_RANGE
Default AGL sensor maximum range.
Definition: ins_ekf2.cpp:63
stateSetPositionNed_f
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
guidance_h_read_rc
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:302
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ins_ekf2.cpp:517
float_rmat_transp_ratemult
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_float.c:160
ekf
static Ekf ekf
EKF class itself.
Definition: ins_ekf2.cpp:144
AHRS_COMP_ID_EKF2
#define AHRS_COMP_ID_EKF2
Definition: ahrs.h:44
INS_EKF2_GYRO_ID
#define INS_EKF2_GYRO_ID
Definition: ins_ekf2.cpp:79
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
send_ins_ekf2_ext
static void send_ins_ekf2_ext(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:184
ekf2_t::accel_dt
uint32_t accel_dt
Definition: ins_ekf2.cpp:125
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
guidance_h
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
INS_EKF2_BARO_ID
#define INS_EKF2_BARO_ID
default barometer to use in INS
Definition: ins_ekf2.cpp:72
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
stab_att_sp_euler
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
Definition: stabilization_attitude_euler_float.c:45
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
ekf2_t::quat_reset_counter
uint8_t quat_reset_counter
Definition: ins_ekf2.cpp:131
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
baro_ev
static abi_event baro_ev
Definition: ins_ekf2.cpp:103
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
ekf2_t::gyro
FloatRates gyro
Definition: ins_ekf2.cpp:126
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
ekf2_t
Definition: ins_ekf2.cpp:121
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
ins_ekf2_init
void ins_ekf2_init(void)
Definition: ins_ekf2.cpp:248
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
HorizontalGuidance::rc_sp
struct FloatEulers rc_sp
Definition: guidance_h.h:106
ins_ekf2_update
void ins_ekf2_update(void)
Definition: ins_ekf2.cpp:289
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
uint64_t
unsigned long long uint64_t
Definition: types.h:20
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_ekf2.cpp:491
accel_ev
static abi_event accel_ev
Definition: ins_ekf2.cpp:105