Paparazzi UAS  v5.15_devel-109-gee85905
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 
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 
110 /* All ABI callbacks */
111 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
112 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
113 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
114 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
115 static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
116 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
117 static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
118 
119 /* Main EKF2 structure for keeping track of the status */
120 struct ekf2_t {
129 
131 
134 
137 };
138 
139 /* Static local functions */
140 static void ins_ekf2_publish_attitude(uint32_t stamp);
141 
142 /* Static local variables */
143 static Ekf ekf;
144 static parameters *ekf2_params;
145 static struct ekf2_t ekf2;
147 
148 
149 #if PERIODIC_TELEMETRY
151 
152 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
153 {
154  float qfe = 101325.0; //TODO: this is qnh not qfe?
155  if (ekf2.ltp_stamp > 0)
156  pprz_msg_send_INS_REF(trans, dev, AC_ID,
159  &ekf2.ltp_def.hmsl, &qfe);
160 }
161 
162 static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
163 {
164  uint16_t gps_check_status, filter_fault_status, soln_status;
165  uint32_t control_mode;
166  ekf.get_gps_check_status(&gps_check_status);
167  ekf.get_filter_fault_status(&filter_fault_status);
168  ekf.get_control_mode(&control_mode);
169  ekf.get_ekf_soln_status(&soln_status);
170 
171  uint16_t innov_test_status;
172  float mag, vel, pos, hgt, tas, hagl, beta;
173  ekf.get_innovation_test_status(&innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta);
174  pprz_msg_send_INS_EKF2(trans, dev, AC_ID,
175  &control_mode, &filter_fault_status, &gps_check_status, &soln_status,
176  &innov_test_status, &mag, &vel, &pos, &hgt, &tas, &hagl, &beta);
177 }
178 
179 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
180 {
181  uint32_t control_mode;
182  uint16_t filter_fault_status;
183  uint8_t mde = 0;
184  ekf.get_control_mode(&control_mode);
185  ekf.get_filter_fault_status(&filter_fault_status);
186 
187  // Check the alignment and if GPS is fused
188  if ((control_mode & 0x7) == 0x7) {
189  mde = 3;
190  } else if ((control_mode & 0x7) == 0x3) {
191  mde = 4;
192  } else {
193  mde = 2;
194  }
195 
196  // Check if there is a covariance error
197  if (filter_fault_status) {
198  mde = 6;
199  }
200 
201  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ekf2_id, &mde, &filter_fault_status);
202 }
203 #endif
204 
205 /* Initialize the EKF */
206 void ins_ekf2_init(void)
207 {
208  /* Get the ekf parameters */
209  ekf2_params = ekf.getParamHandle();
210  ekf2_params->mag_fusion_type = MAG_FUSE_TYPE_HEADING;
211 
212  /* Initialize struct */
213  ekf2.ltp_stamp = 0;
214  ekf2.accel_stamp = 0;
215  ekf2.gyro_stamp = 0;
216  ekf2.gyro_valid = false;
217  ekf2.accel_valid = false;
218  ekf2.got_imu_data = false;
220 
221  /* Initialize the range sensor limits */
222  ekf.set_rangefinder_limits(INS_SONAR_MIN_RANGE, INS_SONAR_MAX_RANGE);
223 
224 #if PERIODIC_TELEMETRY
227  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
228 #endif
229 
230  /*
231  * Subscribe to scaled IMU measurements and attach callbacks
232  */
233  AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
234  AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
235  AbiBindMsgIMU_GYRO_INT32(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
236  AbiBindMsgIMU_ACCEL_INT32(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
237  AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
238  AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
239  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
240 }
241 
242 /* Update the INS state */
243 void ins_ekf2_update(void)
244 {
245  /* Set EKF settings */
246  ekf.set_in_air_status(autopilot_in_flight());
247 
248  /* Update the EKF */
249  if (ekf2.got_imu_data && ekf.update()) {
250  filter_control_status_u control_status;
251  ekf.get_control_mode(&control_status.value);
252 
253  // Only publish position after successful alignment
254  if (control_status.flags.tilt_align) {
255  /* Get the position */
256  float pos_f[3] = {};
257  struct NedCoor_f pos;
258  ekf.get_position(pos_f);
259  pos.x = pos_f[0];
260  pos.y = pos_f[1];
261  pos.z = pos_f[2];
262 
263  // Publish to the state
264  stateSetPositionNed_f(&pos);
265 
266  /* Get the velocity in NED frame */
267  float vel_f[3] = {};
268  struct NedCoor_f speed;
269  ekf.get_velocity(vel_f);
270  speed.x = vel_f[0];
271  speed.y = vel_f[1];
272  speed.z = vel_f[2];
273 
274  // Publish to state
275  stateSetSpeedNed_f(&speed);
276 
277  /* Get the accelrations in NED frame */
278  float vel_deriv_f[3] = {};
279  struct NedCoor_f accel;
280  ekf.get_vel_deriv_ned(vel_deriv_f);
281  accel.x = vel_deriv_f[0];
282  accel.y = vel_deriv_f[1];
283  accel.z = vel_deriv_f[2];
284 
285  // Publish to state
286  stateSetAccelNed_f(&accel);
287 
288  /* Get local origin */
289  // Position of local NED origin in GPS / WGS84 frame
290  struct map_projection_reference_s ekf_origin = {};
291  float ref_alt;
292  struct LlaCoor_i lla_ref;
293  uint64_t origin_time;
294 
295  // Only update the origin when the state estimator has updated the origin
296  bool ekf_origin_valid = ekf.get_ekf_origin(&origin_time, &ekf_origin, &ref_alt);
297  if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
298  lla_ref.lat = ekf_origin.lat_rad * 180.0 / M_PI * 1e7; // Reference point latitude in degrees
299  lla_ref.lon = ekf_origin.lon_rad * 180.0 / M_PI * 1e7; // Reference point longitude in degrees
300  lla_ref.alt = ref_alt * 1000.0;
301  ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
303 
304  ekf2.ltp_stamp = origin_time;
305  }
306  }
307  }
308 
309 #if defined SITL && USE_NPS
310  if (nps_bypass_ins) {
312  }
313 #endif
314 
315  ekf2.got_imu_data = false;
316 }
317 
322 {
323  imuSample imu_sample;
324  imu_sample.time_us = stamp;
325  imu_sample.delta_ang_dt = ekf2.gyro_dt * 1.e-6f;
326  imu_sample.delta_ang = Vector3f{ekf2.gyro.p, ekf2.gyro.q, ekf2.gyro.r} * imu_sample.delta_ang_dt;
327  imu_sample.delta_vel_dt = ekf2.accel_dt * 1.e-6f;
328  imu_sample.delta_vel = Vector3f{ekf2.accel.x, ekf2.accel.y, ekf2.accel.z} * imu_sample.delta_vel_dt;
329  ekf.setIMUData(imu_sample);
330 
331  if (ekf.attitude_valid()) {
332  // Calculate the quaternion
333  struct FloatQuat ltp_to_body_quat;
334  const Quatf att_q{ekf.calculate_quaternion()};
335  ltp_to_body_quat.qi = att_q(0);
336  ltp_to_body_quat.qx = att_q(1);
337  ltp_to_body_quat.qy = att_q(2);
338  ltp_to_body_quat.qz = att_q(3);
339 
340  // Publish it to the state
341  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
342 
343  /* Check the quaternion reset state */
344  float delta_q_reset[4];
345  uint8_t quat_reset_counter;
346  ekf.get_quat_reset(delta_q_reset, &quat_reset_counter);
347 
348  // If reset update the setpoint heading
349  if (ekf2.quat_reset_counter < quat_reset_counter) {
350  float psi = matrix::Eulerf(matrix::Quatf(delta_q_reset)).psi();
351 #if defined STABILIZATION_ATTITUDE_TYPE_INT
353 #else
354  stab_att_sp_euler.psi += psi;
355 #endif
356  guidance_h.sp.heading += psi;
357  guidance_h.rc_sp.psi += psi;
361  ekf2.quat_reset_counter = quat_reset_counter;
362  }
363 
364  /* Get in-run gyro bias */
365  struct FloatRates body_rates;
366  float gyro_bias[3];
367  ekf.get_gyro_bias(gyro_bias);
368  body_rates.p = ekf2.gyro.p - gyro_bias[0];
369  body_rates.q = ekf2.gyro.q - gyro_bias[1];
370  body_rates.r = ekf2.gyro.r - gyro_bias[2];
371 
372  // Publish it to the state
373  stateSetBodyRates_f(&body_rates);
374 
375  /* Get the in-run acceleration bias */
376  struct Int32Vect3 accel;
377  float accel_bias[3];
378  ekf.get_accel_bias(accel_bias);
379  accel.x = ACCEL_BFP_OF_REAL(ekf2.accel.x - accel_bias[0]);
380  accel.y = ACCEL_BFP_OF_REAL(ekf2.accel.y - accel_bias[1]);
381  accel.z = ACCEL_BFP_OF_REAL(ekf2.accel.z - accel_bias[2]);
382 
383  // Publish it to the state
384  stateSetAccelBody_i(&accel);
385  }
386 
387  ekf2.gyro_valid = false;
388  ekf2.accel_valid = false;
389  ekf2.got_imu_data = true;
390 }
391 
392 /* Update INS based on Baro information */
393 static void baro_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float pressure)
394 {
395  // Calculate the air density
396  float rho = pprz_isa_density_of_pressure(pressure,
397  20.0f); // TODO: add temperature compensation now set to 20 degree celcius
398  ekf.set_air_density(rho);
399 
400  // Calculate the height above mean sea level based on pressure
401  float height_amsl_m = pprz_isa_height_of_pressure_full(pressure,
402  101325.0); //101325.0 defined as PPRZ_ISA_SEA_LEVEL_PRESSURE in pprz_isa.h
403  ekf.setBaroData(stamp, height_amsl_m);
404 }
405 
406 /* Update INS based on AGL information */
407 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t stamp, float distance)
408 {
409  ekf.setRangeData(stamp, distance);
410 }
411 
412 /* Update INS based on Gyro information */
413 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
414  uint32_t stamp, struct Int32Rates *gyro)
415 {
416  FloatRates imu_rate;
417  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
418 
419  // Convert Gyro information to float
420  RATES_FLOAT_OF_BFP(imu_rate, *gyro);
421 
422  // Rotate with respect to Body To IMU
423  float_rmat_transp_ratemult(&ekf2.gyro, body_to_imu_rmat, &imu_rate);
424 
425  // Calculate the Gyro interval
426  if (ekf2.gyro_stamp > 0) {
427  ekf2.gyro_dt = stamp - ekf2.gyro_stamp;
428  ekf2.gyro_valid = true;
429  }
430  ekf2.gyro_stamp = stamp;
431 
432  /* When Gyro and accelerometer are valid enter it into the EKF */
433  if (ekf2.gyro_valid && ekf2.accel_valid) {
435  }
436 }
437 
438 /* Update INS based on Accelerometer information */
439 static void accel_cb(uint8_t sender_id __attribute__((unused)),
440  uint32_t stamp, struct Int32Vect3 *accel)
441 {
442  struct FloatVect3 accel_imu;
443  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
444 
445  // Convert Accelerometer information to float
446  ACCELS_FLOAT_OF_BFP(accel_imu, *accel);
447 
448  // Rotate with respect to Body To IMU
449  float_rmat_transp_vmult(&ekf2.accel, body_to_imu_rmat, &accel_imu);
450 
451  // Calculate the Accelerometer interval
452  if (ekf2.accel_stamp > 0) {
453  ekf2.accel_dt = stamp - ekf2.accel_stamp;
454  ekf2.accel_valid = true;
455  }
456  ekf2.accel_stamp = stamp;
457 
458  /* When Gyro and accelerometer are valid enter it into the EKF */
459  if (ekf2.gyro_valid && ekf2.accel_valid) {
461  }
462 }
463 
464 /* Update INS based on Magnetometer information */
465 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
466  uint32_t stamp,
467  struct Int32Vect3 *mag)
468 {
469  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ekf2.body_to_imu);
470  struct FloatVect3 mag_gauss, mag_body;
471 
472  // Convert Magnetometer information to float and to radius 0.2f
473  MAGS_FLOAT_OF_BFP(mag_gauss, *mag);
474  mag_gauss.x *= 0.4f;
475  mag_gauss.y *= 0.4f;
476  mag_gauss.z *= 0.4f;
477 
478  // Rotate with respect to Body To IMU
479  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_gauss);
480 
481  // Publish information to the EKF
482  float mag_r[3];
483  mag_r[0] = mag_body.x;
484  mag_r[1] = mag_body.y;
485  mag_r[2] = mag_body.z;
486 
487  ekf.setMagData(stamp, mag_r);
488  ekf2.got_imu_data = true;
489 }
490 
491 /* Update INS based on GPS information */
492 static void gps_cb(uint8_t sender_id __attribute__((unused)),
493  uint32_t stamp,
494  struct GpsState *gps_s)
495 {
496  struct gps_message gps_msg = {};
497  gps_msg.time_usec = stamp;
498  gps_msg.lat = gps_s->lla_pos.lat;
499  gps_msg.lon = gps_s->lla_pos.lon;
500  gps_msg.alt = gps_s->hmsl;
501  gps_msg.yaw = NAN;
502  gps_msg.yaw_offset = NAN;
503  gps_msg.fix_type = gps_s->fix;
504  gps_msg.eph = gps_s->hacc / 1000.0;
505  gps_msg.epv = gps_s->vacc / 1000.0;
506  gps_msg.sacc = gps_s->sacc / 100.0;
507  gps_msg.vel_m_s = gps_s->gspeed / 100.0;
508  gps_msg.vel_ned[0] = (gps_s->ned_vel.x) / 100.0;
509  gps_msg.vel_ned[1] = (gps_s->ned_vel.y) / 100.0;
510  gps_msg.vel_ned[2] = (gps_s->ned_vel.z) / 100.0;
511  gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
512  gps_msg.nsats = gps_s->num_sv;
513  gps_msg.gdop = 0.0f;
514 
515  ekf.setGpsData(stamp, gps_msg);
516 }
517 
518 /* Save the Body to IMU information */
519 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
520  struct FloatQuat *q_b2i_f)
521 {
523 }
bool nps_bypass_ins
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
unsigned short uint16_t
Definition: types.h:16
int32_t z
in centimeters
angular rates
bool gyro_valid
Definition: ins_ekf2.cpp:127
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
void guidance_h_read_rc(bool in_flight)
Definition: guidance_h.c:292
float y
in meters
uint32_t gyro_dt
Definition: ins_ekf2.cpp:122
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:519
#define INS_SONAR_MIN_RANGE
Default AGL sensor minimum range.
Definition: ins_ekf2.cpp:57
#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 FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
uint8_t quat_reset_counter
Definition: ins_ekf2.cpp:130
float r
in rad/s
uint64_t ltp_stamp
Definition: ins_ekf2.cpp:132
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:465
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:321
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:97
float psi
in radians
void stabilization_attitude_enter(void)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_ekf2.cpp:393
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 alt
in millimeters above WGS84 reference ellipsoid
float z
in meters
static abi_event agl_ev
Definition: ins_ekf2.cpp:102
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
Roation quaternion.
int32_t y
East.
bool accel_valid
Definition: ins_ekf2.cpp:128
#define INS_EKF2_ACCEL_ID
Definition: ins_ekf2.cpp:85
uint32_t accel_dt
Definition: ins_ekf2.cpp:124
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:413
struct LtpDef_i ltp_def
Definition: ins_ekf2.cpp:133
static struct ekf2_t ekf2
Local EKF EKF status structure.
Definition: ins_ekf2.cpp:145
#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
unsigned long long uint64_t
Definition: types.h:20
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:143
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:104
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:51
unsigned long uint32_t
Definition: types.h:18
static abi_event gps_ev
Definition: ins_ekf2.cpp:107
static abi_event baro_ev
Definition: ins_ekf2.cpp:103
static parameters * ekf2_params
The EKF parameters.
Definition: ins_ekf2.cpp:144
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:243
void ins_ekf2_init(void)
Definition: ins_ekf2.cpp:206
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
#define INS_EKF2_BARO_ID
default barometer to use in INS
Definition: ins_ekf2.cpp:72
static void send_ins_ekf2(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ekf2.cpp:162
#define INS_SONAR_MAX_RANGE
Default AGL sensor maximum range.
Definition: ins_ekf2.cpp:63
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:152
#define ANGLE_BFP_OF_REAL(_af)
#define INS_EKF2_MAG_ID
Definition: ins_ekf2.cpp:91
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
uint32_t gyro_stamp
Definition: ins_ekf2.cpp:121
Core autopilot interface common to all firmwares.
static abi_event body_to_imu_ev
Definition: ins_ekf2.cpp:108
static abi_event mag_ev
Definition: ins_ekf2.cpp:106
#define INS_EKF2_GYRO_ID
Definition: ins_ekf2.cpp:79
unsigned char uint8_t
Definition: types.h:14
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: ins_ekf2.cpp:407
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
rotation matrix
struct OrientationReps body_to_imu
Definition: ins_ekf2.cpp:135
static uint8_t ahrs_ekf2_id
Component ID for EKF.
Definition: ins_ekf2.cpp:146
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:125
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
static abi_event accel_ev
Definition: ins_ekf2.cpp:105
struct FloatEulers rc_sp
Definition: guidance_h.h:106
bool got_imu_data
Definition: ins_ekf2.cpp:136
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:104
#define ACCEL_BFP_OF_REAL(_af)
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
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:126
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:179
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_ekf2.cpp:492
void sim_overwrite_ins(void)
uint32_t accel_stamp
Definition: ins_ekf2.cpp:123
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
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_ekf2.cpp:439