Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_int.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include "subsystems/ins/ins_int.h"
30 
31 #include "subsystems/abi.h"
32 
33 #include "subsystems/imu.h"
34 #include "subsystems/gps.h"
35 
36 #include "generated/airframe.h"
37 
38 #if USE_VFF_EXTENDED
40 #else
42 #endif
43 
44 #if USE_HFF
46 #endif
47 
48 #if defined SITL && USE_NPS
49 //#include "nps_fdm.h"
50 #include "nps_autopilot.h"
51 #include <stdio.h>
52 #endif
53 
54 #include "math/pprz_geodetic_int.h"
55 #include "math/pprz_isa.h"
56 #include "math/pprz_stat.h"
57 
58 #ifndef VFF_R_AGL
59 #define VFF_R_AGL 0.2
60 #endif
61 
62 #if USE_SONAR
63 #if !USE_VFF_EXTENDED
64 #error USE_SONAR needs USE_VFF_EXTENDED
65 #endif
66 
67 #ifdef INS_SONAR_THROTTLE_THRESHOLD
69 #endif
70 
71 #ifndef INS_SONAR_MIN_RANGE
72 #define INS_SONAR_MIN_RANGE 0.001
73 #endif
74 #ifndef INS_SONAR_MAX_RANGE
75 #define INS_SONAR_MAX_RANGE 4.0
76 #endif
77 #define VFF_R_SONAR_0 0.2
78 #ifndef VFF_R_SONAR_OF_M
79 #define VFF_R_SONAR_OF_M 0.2
80 #endif
81 
82 #ifndef INS_SONAR_UPDATE_ON_AGL
83 #define INS_SONAR_UPDATE_ON_AGL FALSE
84 PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE")
85 #endif
86 
87 #endif // USE_SONAR
88 
89 #if USE_GPS
90 #ifndef INS_VFF_R_GPS
91 #define INS_VFF_R_GPS 2.0
92 #endif
93 
94 #ifndef INS_VFF_VZ_R_GPS
95 #define INS_VFF_VZ_R_GPS 2.0
96 #endif
97 #endif // USE_GPS
98 
100 #ifndef INS_MAX_PROPAGATION_STEPS
101 #define INS_MAX_PROPAGATION_STEPS 200
102 #endif
103 
104 #ifndef USE_INS_NAV_INIT
105 #define USE_INS_NAV_INIT TRUE
106 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
107 #endif
108 
110 #define INS_BARO_MAX_INIT_VAR 1.f // variance threshold to set initial baro measurement
111 #ifndef INS_INT_BARO_ID
112 #if USE_BARO_BOARD
113 #define INS_INT_BARO_ID BARO_BOARD_SENDER_ID
114 #else
115 #define INS_INT_BARO_ID ABI_BROADCAST
116 #endif
117 #endif
118 PRINT_CONFIG_VAR(INS_INT_BARO_ID)
120 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
121 
125 #ifndef INS_INT_IMU_ID
126 #define INS_INT_IMU_ID ABI_BROADCAST
127 #endif
129 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
130 
131 #ifndef INS_INT_GPS_ID
132 #define INS_INT_GPS_ID GPS_MULTI_ID
133 #endif
135 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
136 
140 #ifndef INS_INT_VEL_ID
141 #define INS_INT_VEL_ID ABI_BROADCAST
142 #endif
144 static void vel_est_cb(uint8_t sender_id,
145  uint32_t stamp,
146  float x, float y, float z,
147  float noise_x, float noise_y, float noise_z);
148 #ifndef INS_INT_POS_ID
149 #define INS_INT_POS_ID ABI_BROADCAST
150 #endif
152 static void pos_est_cb(uint8_t sender_id,
153  uint32_t stamp,
154  float x, float y, float z,
155  float noise_x, float noise_y, float noise_z);
156 
160 #ifndef INS_INT_AGL_ID
161 #define INS_INT_AGL_ID ABI_BROADCAST
162 #endif
164 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
165 
167 
168 #if PERIODIC_TELEMETRY
170 
171 static void send_ins(struct transport_tx *trans, struct link_device *dev)
172 {
173  pprz_msg_send_INS(trans, dev, AC_ID,
177 }
178 
179 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
180 {
181  pprz_msg_send_INS_Z(trans, dev, AC_ID,
183 }
184 
185 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
186 {
187  if (ins_int.ltp_initialized) {
188  pprz_msg_send_INS_REF(trans, dev, AC_ID,
192  }
193 }
194 #endif
195 
196 static void ins_ned_to_state(void);
197 static void ins_update_from_vff(void);
198 #if USE_HFF
199 static void ins_update_from_hff(void);
200 #endif
201 
202 
203 void ins_int_init(void)
204 {
205 
206 #if USE_INS_NAV_INIT
208  ins_int.ltp_initialized = true;
209 #else
210  ins_int.ltp_initialized = false;
211 #endif
212 
213  /* we haven't had any measurement updates yet, so set the counter to max */
215 
216  // Bind to BARO_ABS message
217  AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb);
218  ins_int.baro_initialized = false;
219 
220 #if USE_SONAR
221  ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
222 #endif
223 
224  ins_int.vf_reset = false;
225  ins_int.hf_realign = false;
226 
227  /* init vertical and horizontal filters */
228  vff_init_zero();
229 #if USE_HFF
230  hff_init(0., 0., 0., 0.);
231 #endif
232 
236 
237 #if PERIODIC_TELEMETRY
241 #endif
242 
243  /*
244  * Subscribe to scaled IMU measurements and attach callbacks
245  */
246  AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
247  AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
248  AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
249  AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
250  AbiBindMsgAGL(INS_INT_AGL_ID, &agl_ev, agl_cb); // ABI to the altitude above ground level
251 }
252 
254 {
255 #if USE_GPS
256  if (GpsFixValid()) {
260  ins_int.ltp_initialized = true;
262  } else {
263  ins_int.ltp_initialized = false;
264  }
265 #else
266  ins_int.ltp_initialized = false;
267 #endif
268 
269 #if USE_HFF
270  ins_int.hf_realign = true;
271 #endif
272  ins_int.vf_reset = true;
273 }
274 
276 {
277 #if USE_GPS
278  if (GpsFixValid()) {
279  struct LlaCoor_i lla = {
281  .lon = state.ned_origin_i.lla.lon,
282  .alt = gps.lla_pos.alt
283  };
287  }
288 #endif
289  ins_int.vf_reset = true;
290 }
291 
292 void ins_int_propagate(struct Int32Vect3 *accel, float dt)
293 {
294  /* untilt accels */
295  struct Int32Vect3 accel_meas_body;
296  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
297  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
298  stateSetAccelBody_i(&accel_meas_body);
299  struct Int32Vect3 accel_meas_ltp;
300  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
301 
302  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
303 
304  /* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS.
305  * Otherwise halt the propagation to not diverge and only set the acceleration.
306  * This should only be relevant in the startup phase when the baro is not yet initialized
307  * and there is no gps fix yet...
308  */
310  vff_propagate(z_accel_meas_float, dt);
312  } else {
313  // feed accel from the sensors
314  // subtract -9.81m/s2 (acceleration measured due to gravity,
315  // but vehicle not accelerating in ltp)
316  ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
317  }
318 
319 #if USE_HFF
320  /* propagate horizontal filter */
321  hff_propagate();
322  /* convert and copy result to ins_int */
323  ins_update_from_hff();
324 #else
325  ins_int.ltp_accel.x = accel_meas_ltp.x;
326  ins_int.ltp_accel.y = accel_meas_ltp.y;
327 #endif /* USE_HFF */
328 
330 
331  /* increment the propagation counter, while making sure it doesn't overflow */
334  }
335 }
336 
337 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
338 {
339  if (pressure < 1.f)
340  {
341  // bad baro pressure, don't use
342  return;
343  }
344 
345  if (!ins_int.baro_initialized) {
346 #define press_hist_len 10
347  static float press_hist[press_hist_len];
348  static uint8_t idx = 0;
349 
350  press_hist[idx] = pressure;
351  idx = (idx + 1) % press_hist_len;
352  float var = variance_f(press_hist, press_hist_len);
353  if (var < INS_BARO_MAX_INIT_VAR){
354  // wait for a first positive value
355  ins_int.vf_reset = true;
356  ins_int.baro_initialized = true;
357  }
358  }
359 
361  float height_correction = 0.f;
363  // Calculate the distance to the origin
364  struct EnuCoor_f *enu = stateGetPositionEnu_f();
365  double dist2_to_origin = enu->x * enu->x + enu->y * enu->y;
366 
367  // correction for the earth's curvature
368  const double earth_radius = 6378137.0;
369  height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
370  }
371 
372  if (ins_int.vf_reset) {
373  ins_int.vf_reset = false;
374  ins_int.qfe = pressure;
375  vff_realign(height_correction);
377  }
378 
379  float baro_up = pprz_isa_height_of_pressure(pressure, ins_int.qfe);
380 
381  // The VFF will update in the NED frame
382  ins_int.baro_z = -(baro_up - height_correction);
383 
384 #if USE_VFF_EXTENDED
386 #else
388 #endif
389 
390  /* reset the counter to indicate we just had a measurement update */
392  }
393 }
394 
395 #if USE_GPS
396 void ins_int_update_gps(struct GpsState *gps_s)
397 {
398  if (gps_s->fix < GPS_FIX_3D) {
399  return;
400  }
401 
402  if (!ins_int.ltp_initialized) {
404  }
405 
406  struct NedCoor_i gps_pos_cm_ned;
407  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos);
408 
409  /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
410 #ifdef INS_BODY_TO_GPS_X
411  /* body2gps translation in body frame */
412  struct Int32Vect3 b2g_b = {
413  .x = INS_BODY_TO_GPS_X,
414  .y = INS_BODY_TO_GPS_Y,
415  .z = INS_BODY_TO_GPS_Z
416  };
417  /* rotate offset given in body frame to navigation/ltp frame using current attitude */
418  struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i();
419  QUAT_INVERT(q_b2n, q_b2n);
420  struct Int32Vect3 b2g_n;
421  int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
422  /* subtract body2gps translation in ltp from gps position */
423  VECT3_SUB(gps_pos_cm_ned, b2g_n);
424 #endif
425 
427  struct NedCoor_i gps_speed_cm_s_ned;
428  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel);
429 
430 #if INS_USE_GPS_ALT
431  vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
432 #endif
433 #if INS_USE_GPS_ALT_SPEED
434  vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
436 #endif
437 
438 #if USE_HFF
439  /* horizontal gps transformed to NED in meters as float */
440  struct FloatVect2 gps_pos_m_ned;
441  VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
442  VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
443 
444  struct FloatVect2 gps_speed_m_s_ned;
445  VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
446  VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.f);
447 
448  if (ins_int.hf_realign) {
449  ins_int.hf_realign = false;
450  hff_realign(gps_pos_m_ned, gps_speed_m_s_ned);
451  }
452  // run horizontal filter
453  hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
454  // convert and copy result to ins_int
455  ins_update_from_hff();
456 
457 #else /* hff not used */
458  /* simply copy horizontal pos/speed from gps */
459  INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
461  INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
463 #endif /* USE_HFF */
464 
466 
467  /* reset the counter to indicate we just had a measurement update */
469 }
470 #else
471 void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
472 #endif /* USE_GPS */
473 
478 #if USE_VFF_EXTENDED
479 static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float distance) {
480  if (distance <= 0 || !(ins_int.baro_initialized)) {
481  return;
482  }
483 
484 #if USE_SONAR
485  if (distance > INS_SONAR_MAX_RANGE || distance < INS_SONAR_MIN_RANGE){
486  return;
487  }
488 #endif
489 #ifdef INS_AGL_THROTTLE_THRESHOLD
490  if(stabilization_cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
491  return;
492  }
493 #endif
494 #ifdef INS_AGL_BARO_THRESHOLD
495  if(ins_int.baro_z < -INS_SONAR_BARO_THRESHOLD){ /* z down */
496  return;
497  }
498 #endif
499 
500 #if USE_SONAR
501  vff_update_agl(-distance, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
502 #else
503  // TODO: this assumes that you will either have sonar or other agl sensor never both
504  vff_update_agl(-distance, VFF_R_AGL);
505 #endif
506  /* reset the counter to indicate we just had a measurement update */
508 }
509 #else
510 static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, __attribute__((unused)) float distance) {}
511 #endif
512 
514 static void ins_ned_to_state(void)
515 {
519 
520 #if defined SITL && USE_NPS
521  if (nps_bypass_ins) {
523  }
524 #endif
525 }
526 
528 static void ins_update_from_vff(void)
529 {
533 }
534 
535 #if USE_HFF
536 
537 static void ins_update_from_hff(void)
538 {
545 }
546 #endif
547 
548 
549 static void accel_cb(uint8_t sender_id __attribute__((unused)),
550  uint32_t stamp, struct Int32Vect3 *accel)
551 {
552  PRINT_CONFIG_MSG("Calculating dt for INS int propagation.")
553  /* timestamp in usec when last callback was received */
554  static uint32_t last_stamp = 0;
555 
556  if (last_stamp > 0) {
557  float dt = (float)(stamp - last_stamp) * 1e-6;
558  ins_int_propagate(accel, dt);
559  }
560  last_stamp = stamp;
561 }
562 
563 static void gps_cb(uint8_t sender_id __attribute__((unused)),
564  uint32_t stamp __attribute__((unused)),
565  struct GpsState *gps_s)
566 {
567  ins_int_update_gps(gps_s);
568 }
569 
570 /* body relative velocity estimate
571  *
572  */
573 static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
574  uint32_t stamp __attribute__((unused)),
575  float x, float y, float z,
576  float noise_x, float noise_y, float noise_z)
577 {
578  struct FloatVect3 vel_body = {x, y, z};
579 
580  /* rotate velocity estimate to nav/ltp frame */
581  struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
582  QUAT_INVERT(q_b2n, q_b2n);
583  struct FloatVect3 vel_ned;
584  float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
585 
586  // abi message contains an update to the horizontal velocity estimate
587 #if USE_HFF
588  struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
589  struct FloatVect2 Rvel = {noise_x, noise_y};
590 
591  hff_update_vel(vel, Rvel);
592  ins_update_from_hff();
593 #else
594  if (noise_x >= 0.f)
595  {
597  }
598  if (noise_y >= 0.f)
599  {
601  }
602 
603  static uint32_t last_stamp_x = 0, last_stamp_y = 0;
604  if (noise_x >= 0.f) {
605  if (last_stamp_x > 0)
606  {
607  float dt = (float)(stamp - last_stamp_x) * 1e-6;
608  ins_int.ltp_pos.x += lround(POS_BFP_OF_REAL(dt * vel_ned.x));
609  }
610  last_stamp_x = stamp;
611  }
612 
613  if (noise_y >= 0.f)
614  {
615  if (last_stamp_y > 0)
616  {
617  float dt = (float)(stamp - last_stamp_y) * 1e-6;
618  ins_int.ltp_pos.y += lround(POS_BFP_OF_REAL(dt * vel_ned.y));
619  }
620  last_stamp_y = stamp;
621  }
622 #endif
623 
624  // abi message contains an update to the vertical velocity estimate
625  vff_update_vz_conf(vel_ned.z, noise_z);
626 
628 
629  /* reset the counter to indicate we just had a measurement update */
631 }
632 
633 /* NED position estimate relative to ltp origin
634  */
635 static void pos_est_cb(uint8_t sender_id __attribute__((unused)),
636  uint32_t stamp __attribute__((unused)),
637  float x, float y, float z,
638  float noise_x, float noise_y, float noise_z)
639 {
640 
641 #if USE_HFF
642  struct FloatVect2 pos = {x, y};
643  struct FloatVect2 Rpos = {noise_x, noise_y};
644 
645  hff_update_pos(pos, Rpos);
646  ins_update_from_hff();
647 #else
648  if (noise_x >= 0.f)
649  {
651  }
652  if (noise_y >= 0.f)
653  {
655  }
656 #endif
657 
658  vff_update_z_conf(z, noise_z);
659 
661 
662  /* reset the counter to indicate we just had a measurement update */
664 }
bool nps_bypass_ins
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
#define VECT3_SUB(_a, _b)
Definition: pprz_algebra.h:154
int32_t z
in centimeters
static abi_event gps_ev
Definition: ins_int.c:134
static uint32_t idx
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
#define INS_INT_AGL_ID
ABI binding for AGL.
Definition: ins_int.c:161
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_int.c:337
bool vf_reset
request to reset vertical filter.
Definition: ins_int.h:53
static abi_event pos_est_ev
Definition: ins_int.c:151
void vff_update_z_conf(float z_meas, float conf)
Interface for extended vertical filter (in float).
Horizontal filter (x,y) to estimate position and velocity.
#define INS_SONAR_MIN_RANGE
Default AGL sensor minimum range.
Definition: ins_ekf2.cpp:57
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
#define INS_VFF_VZ_R_GPS
Definition: ins_int.c:95
static abi_event vel_est_ev
Definition: ins_int.c:143
struct InsInt ins_int
global INS state
Definition: ins_int.c:166
int32_t y
in centimeters
vector in East North Up coordinates Units: meters
float qfe
Definition: ins_int.h:62
#define POS_BFP_OF_REAL(_af)
#define QUAT_INVERT(_qo, _qi)
Definition: pprz_algebra.h:627
static void ins_update_from_vff(void)
update ins state from vertical filter
Definition: ins_int.c:528
#define INT32_VECT3_ZERO(_v)
void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:602
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
Main include for ABI (AirBorneInterface).
void vff_update_vz_conf(float vz_meas, float conf)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_int.c:563
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_int.c:253
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
float z
z-position estimate in m (NED, z-down)
struct Imu imu
global IMU state
Definition: imu.c:108
#define INT32_SPEED_OF_CM_S_DEN
vector in Latitude, Longitude and Altitude
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
int32_t z
Down.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
#define press_hist_len
bool baro_initialized
Definition: ins_int.h:63
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
#define INS_INT_POS_ID
Definition: ins_int.c:149
struct EcefCoor_i ecef
Reference point in ecef.
int32_t hmsl
Height above mean sea level in mm.
void ins_int_update_gps(struct GpsState *gps_s)
Definition: ins_int.c:396
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
int32_t alt
in millimeters above WGS84 reference ellipsoid
Vertical filter (in float) estimating altitude, velocity and accel bias.
Roation quaternion.
static abi_event agl_ev
The agl ABI event.
Definition: ins_int.c:163
abi_event baro_ev
Definition: ins_int.c:119
int32_t y
East.
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_int.c:179
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
float x
in meters
#define INS_INT_BARO_ID
Definition: ins_int.c:115
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_int.c:185
data structure for GPS information
Definition: gps.h:87
void ins_int_init(void)
Definition: ins_int.c:203
#define INT32_POS_OF_CM_NUM
void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:806
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
Device independent GPS code (interface)
Paparazzi atmospheric pressure conversion utilities.
#define ACCEL_FLOAT_OF_BFP(_ai)
bool ltp_initialized
Definition: ins_int.h:41
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
#define INS_INT_IMU_ID
ABI binding for IMU data.
Definition: ins_int.c:126
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_int.c:514
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_MAX_PROPAGATION_STEPS
maximum number of propagation steps without any updates in between
Definition: ins_int.c:101
static void pos_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
Definition: ins_int.c:635
void ins_init_origin_i_from_flightplan(struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition: ins.c:39
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_int.c:549
Paparazzi fixed point math for geodetic calculations.
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
void vff_init_zero(void)
bool hf_realign
request to realign horizontal filter.
Definition: ins_int.h:48
void hff_propagate(void)
Definition: hf_float.c:479
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
#define INS_SONAR_MAX_RANGE
Default AGL sensor maximum range.
Definition: ins_ekf2.cpp:63
Inertial Measurement Unit interface.
void vff_update_agl(float z_meas, float conf)
struct NedCoor_i ltp_pos
Definition: ins_int.h:56
void vff_propagate(float accel, float dt)
Propagate the filter in time.
struct HfilterFloat hff
Definition: hf_float.c:124
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Update position.
Definition: hf_float.c:711
struct NedCoor_i ltp_accel
Definition: ins_int.h:58
#define INS_BARO_MAX_INIT_VAR
default barometer to use in INS
Definition: ins_int.c:110
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_int.h:61
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:104
uint32_t propagation_cnt
number of propagation steps since the last measurement update
Definition: ins_int.h:43
float variance_f(float *array, uint32_t n_elements)
Compute the variance of an array of values (float).
Definition: pprz_stat.c:139
void vff_realign(float z_meas)
float x
Definition: hf_float.h:41
float y
Definition: hf_float.h:45
Ins implementation state (fixed point)
Definition: ins_int.h:39
#define INT32_POS_OF_CM_DEN
void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:274
unsigned char uint8_t
Definition: types.h:14
float xdot
Definition: hf_float.h:42
struct VffExtended vff
float zdot
z-velocity estimate in m/s (NED, z-down)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
vector in North East Down coordinates
#define INS_INT_VEL_ID
ABI binding for VELOCITY_ESTIMATE.
Definition: ins_int.c:141
float xdotdot
Definition: hf_float.h:43
static abi_event accel_ev
Definition: ins_int.c:128
#define INT32_SPEED_OF_CM_S_NUM
float ydot
Definition: hf_float.h:46
Statistics functions.
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_int.c:171
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
void vff_update(float z_meas)
Definition: vf_float.c:189
int32_t x
in centimeters
struct LtpDef_i ltp_def
Definition: ins_int.h:40
void vff_update_baro(float z_meas)
rotation matrix
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
#define SPEED_BFP_OF_REAL(_af)
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:102
#define ACCEL_BFP_OF_REAL(_af)
void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition: hf_float.c:541
int32_t lat
in degrees*1e7
#define INS_VFF_R_GPS
Definition: ins_int.c:91
uint8_t fix
status of fix
Definition: gps.h:107
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
agl_cb This callback handles all estimates of the height of the vehicle above the ground under it Thi...
Definition: ins_int.c:510
#define INS_INT_GPS_ID
Definition: ins_int.c:132
float y
in meters
Rotation quaternion.
struct GpsState gps
global GPS state
Definition: gps.c:69
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
Definition: ins_int.c:292
struct NedCoor_i ltp_speed
Definition: ins_int.h:57
void sim_overwrite_ins(void)
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_int.c:275
static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise_x, float noise_y, float noise_z)
Definition: ins_int.c:573
float ydotdot
Definition: hf_float.h:47
INS for rotorcrafts combining vertical and horizontal filters.
#define VFF_R_AGL
Definition: ins_int.c:59
#define GpsFixValid()
Definition: gps.h:43