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