Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/ins/ins_int.h"
30 
31 #include "modules/core/abi.h"
32 
33 #include "modules/imu/imu.h"
34 #include "modules/gps/gps.h"
35 
36 #include "generated/airframe.h"
37 
38 #if USE_VFF_EXTENDED
40 #else
41 #include "modules/ins/vf_float.h"
42 #endif
43 
44 #if USE_HFF
45 #include "modules/ins/hf_float.h"
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 #endif // USE_SONAR
83 
84 #if USE_GPS
85 #ifndef INS_VFF_R_GPS
86 #define INS_VFF_R_GPS 2.0
87 #endif
88 
89 #ifndef INS_VFF_VZ_R_GPS
90 #define INS_VFF_VZ_R_GPS 2.0
91 #endif
92 #endif // USE_GPS
93 
95 #ifndef INS_MAX_PROPAGATION_STEPS
96 #define INS_MAX_PROPAGATION_STEPS 200
97 #endif
98 
99 #ifndef USE_INS_NAV_INIT
100 #define USE_INS_NAV_INIT TRUE
101 PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
102 #endif
103 
105 #define INS_BARO_MAX_INIT_VAR 1.f // variance threshold to set initial baro measurement
106 #ifndef INS_INT_BARO_ID
107 #if USE_BARO_BOARD
108 #define INS_INT_BARO_ID BARO_BOARD_SENDER_ID
109 #else
110 #define INS_INT_BARO_ID ABI_BROADCAST
111 #endif
112 #endif
113 PRINT_CONFIG_VAR(INS_INT_BARO_ID)
115 static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
116 
120 #ifndef INS_INT_IMU_ID
121 #define INS_INT_IMU_ID ABI_BROADCAST
122 #endif
123 PRINT_CONFIG_VAR(INS_INT_IMU_ID)
125 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
126 
127 #ifndef INS_INT_GPS_ID
128 #define INS_INT_GPS_ID GPS_MULTI_ID
129 #endif
130 PRINT_CONFIG_VAR(INS_INT_GPS_ID)
132 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
133 
137 #ifndef INS_INT_VEL_ID
138 #define INS_INT_VEL_ID ABI_BROADCAST
139 #endif
140 PRINT_CONFIG_VAR(INS_INT_VEL_ID)
142 static void vel_est_cb(uint8_t sender_id,
143  uint32_t stamp,
144  float x, float y, float z,
145  float noise_x, float noise_y, float noise_z);
146 #ifndef INS_INT_POS_ID
147 #define INS_INT_POS_ID ABI_BROADCAST
148 #endif
149 PRINT_CONFIG_VAR(INS_INT_POS_ID)
151 static void pos_est_cb(uint8_t sender_id,
152  uint32_t stamp,
153  float x, float y, float z,
154  float noise_x, float noise_y, float noise_z);
155 
159 #ifndef INS_INT_AGL_ID
160 #define INS_INT_AGL_ID ABI_BROADCAST
161 #endif
162 PRINT_CONFIG_VAR(INS_INT_AGL_ID)
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  ins_int.vf_reset = false;
221  ins_int.hf_realign = false;
222 
223  /* init vertical and horizontal filters */
224  vff_init_zero();
225 #if USE_HFF
226  hff_init(0., 0., 0., 0.);
227 #endif
228 
232 
233 #if PERIODIC_TELEMETRY
237 #endif
238 
239  /*
240  * Subscribe to scaled IMU measurements and attach callbacks
241  */
242  AbiBindMsgIMU_ACCEL(INS_INT_IMU_ID, &accel_ev, accel_cb);
243  AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
244  AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
245  AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
246  AbiBindMsgAGL(INS_INT_AGL_ID, &agl_ev, agl_cb); // ABI to the altitude above ground level
247 }
248 
250 {
251 #if USE_GPS
252  if (GpsFixValid()) {
253  struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
254  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
255  ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_pos);
256  ins_int.ltp_def.lla.alt = lla_pos.alt;
258  ins_int.ltp_initialized = true;
260  } else {
261  ins_int.ltp_initialized = false;
262  }
263 #else
264  ins_int.ltp_initialized = false;
265 #endif
266 
267 #if USE_HFF
268  ins_int.hf_realign = true;
269 #endif
270  ins_int.vf_reset = true;
271 }
272 
274 {
275 #if USE_GPS
276  if (GpsFixValid()) {
277  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
278  struct LlaCoor_i lla = {
280  .lon = state.ned_origin_i.lla.lon,
281  .alt = lla_pos.alt
282  };
286  }
287 #endif
288  ins_int.vf_reset = true;
289 }
290 
292 {
293  ins_int.vf_reset = true;
294 }
295 
296 void ins_int_propagate(struct Int32Vect3 *accel, float dt)
297 {
298  // Set body acceleration in the state
299  stateSetAccelBody_i(accel);
300 
301  /* untilt accels */
302  struct Int32Vect3 accel_meas_ltp;
303  int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
304 
305  float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
306 
307  /* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS.
308  * Otherwise halt the propagation to not diverge and only set the acceleration.
309  * This should only be relevant in the startup phase when the baro is not yet initialized
310  * and there is no gps fix yet...
311  */
313  vff_propagate(z_accel_meas_float, dt);
315  } else {
316  // feed accel from the sensors
317  // subtract -9.81m/s2 (acceleration measured due to gravity,
318  // but vehicle not accelerating in ltp)
319  ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
320  }
321 
322 #if USE_HFF
323  /* propagate horizontal filter */
324  hff_propagate();
325  /* convert and copy result to ins_int */
326  ins_update_from_hff();
327 #else
328  ins_int.ltp_accel.x = accel_meas_ltp.x;
329  ins_int.ltp_accel.y = accel_meas_ltp.y;
330 #endif /* USE_HFF */
331 
333 
334  /* increment the propagation counter, while making sure it doesn't overflow */
337  }
338 }
339 
340 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
341 {
342  if (pressure < 1.f)
343  {
344  // bad baro pressure, don't use
345  return;
346  }
347 
348  if (!ins_int.baro_initialized) {
349 #define press_hist_len 10
350  static float press_hist[press_hist_len];
351  static uint8_t idx = 0;
352 
353  press_hist[idx] = pressure;
354  idx = (idx + 1) % press_hist_len;
355  float var = variance_f(press_hist, press_hist_len);
356  if (var < INS_BARO_MAX_INIT_VAR){
357  // wait for a first positive value
358  ins_int.vf_reset = true;
359  ins_int.baro_initialized = true;
360  }
361  }
362 
364  float height_correction = 0.f;
366  // Calculate the distance to the origin
367  struct EnuCoor_f *enu = stateGetPositionEnu_f();
368  double dist2_to_origin = enu->x * enu->x + enu->y * enu->y;
369 
370  // correction for the earth's curvature
371  const double earth_radius = 6378137.0;
372  height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
373  }
374 
375  if (ins_int.vf_reset) {
376  ins_int.vf_reset = false;
377  ins_int.qfe = pressure;
378  vff_realign(height_correction);
380  }
381 
382  float baro_up = pprz_isa_height_of_pressure(pressure, ins_int.qfe);
383 
384  // The VFF will update in the NED frame
385  ins_int.baro_z = -(baro_up - height_correction);
386 
387 #if USE_VFF_EXTENDED
389 #else
391 #endif
392 
393  /* reset the counter to indicate we just had a measurement update */
395  }
396 }
397 
398 #if USE_GPS
399 void ins_int_update_gps(struct GpsState *gps_s)
400 {
401  if (gps_s->fix < GPS_FIX_3D) {
402  return;
403  }
404 
405  if (!ins_int.ltp_initialized) {
407  }
408 
409  struct NedCoor_i gps_pos_cm_ned;
410  struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
411  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &ecef_pos_i);
412 
413  /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
414 #ifdef INS_BODY_TO_GPS_X
415  /* body2gps translation in body frame */
416  struct Int32Vect3 b2g_b = {
417  .x = INS_BODY_TO_GPS_X,
418  .y = INS_BODY_TO_GPS_Y,
419  .z = INS_BODY_TO_GPS_Z
420  };
421  /* rotate offset given in body frame to navigation/ltp frame using current attitude */
422  struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i();
423  QUAT_INVERT(q_b2n, q_b2n);
424  struct Int32Vect3 b2g_n;
425  int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
426  /* subtract body2gps translation in ltp from gps position */
427  VECT3_SUB(gps_pos_cm_ned, b2g_n);
428 #endif
429 
431  struct NedCoor_i gps_speed_cm_s_ned;
432  struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s);
433  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &ecef_vel_i);
434 
435 #if INS_USE_GPS_ALT
436  vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
437 #endif
438 #if INS_USE_GPS_ALT_SPEED
439  vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
441 #endif
442 
443 #if USE_HFF
444  /* horizontal gps transformed to NED in meters as float */
445  struct FloatVect2 gps_pos_m_ned;
446  VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
447  VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
448 
449  struct FloatVect2 gps_speed_m_s_ned;
450  VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
451  VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.f);
452 
453  if (ins_int.hf_realign) {
454  ins_int.hf_realign = false;
455  hff_realign(gps_pos_m_ned, gps_speed_m_s_ned);
456  }
457  // run horizontal filter
458  hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
459  // convert and copy result to ins_int
460  ins_update_from_hff();
461 
462 #else /* hff not used */
463  /* simply copy horizontal pos/speed from gps */
464  INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
466  INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
468 #endif /* USE_HFF */
469 
471 
472  /* reset the counter to indicate we just had a measurement update */
474 }
475 #else
476 void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
477 #endif /* USE_GPS */
478 
483 #if USE_VFF_EXTENDED
484 static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float distance) {
485  if (distance <= 0 || !(ins_int.baro_initialized)) {
486  return;
487  }
488 
489 #if USE_SONAR
490  if (distance > INS_SONAR_MAX_RANGE || distance < INS_SONAR_MIN_RANGE){
491  return;
492  }
493 #endif
494 #ifdef INS_AGL_THROTTLE_THRESHOLD
495  if(stabilization_cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
496  return;
497  }
498 #endif
499 #ifdef INS_AGL_BARO_THRESHOLD
500  if(ins_int.baro_z < -INS_SONAR_BARO_THRESHOLD){ /* z down */
501  return;
502  }
503 #endif
504 
505 #if USE_SONAR
506  vff_update_agl(-distance, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
507 #else
508  // TODO: this assumes that you will either have sonar or other agl sensor never both
509  vff_update_agl(-distance, VFF_R_AGL);
510 #endif
511  /* reset the counter to indicate we just had a measurement update */
513 }
514 #else
515 static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, __attribute__((unused)) float distance) {}
516 #endif
517 
519 static void ins_ned_to_state(void)
520 {
524 
525 #if defined SITL && USE_NPS
526  if (nps_bypass_ins) {
528  }
529 #endif
530 }
531 
533 static void ins_update_from_vff(void)
534 {
538 }
539 
540 #if USE_HFF
541 
542 static void ins_update_from_hff(void)
543 {
550 }
551 #endif
552 
553 
554 static void accel_cb(uint8_t sender_id __attribute__((unused)),
555  uint32_t stamp, struct Int32Vect3 *accel)
556 {
557  PRINT_CONFIG_MSG("Calculating dt for INS int propagation.")
558  /* timestamp in usec when last callback was received */
559  static uint32_t last_stamp = 0;
560 
561  if (last_stamp > 0) {
562  float dt = (float)(stamp - last_stamp) * 1e-6;
563  ins_int_propagate(accel, dt);
564  }
565  last_stamp = stamp;
566 }
567 
568 static void gps_cb(uint8_t sender_id __attribute__((unused)),
569  uint32_t stamp __attribute__((unused)),
570  struct GpsState *gps_s)
571 {
572  ins_int_update_gps(gps_s);
573 }
574 
575 /* body relative velocity estimate
576  *
577  */
578 static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
579  uint32_t stamp __attribute__((unused)),
580  float x, float y, float z,
581  float noise_x, float noise_y, float noise_z)
582 {
583  struct FloatVect3 vel_body = {x, y, z};
584 
585  /* rotate velocity estimate to nav/ltp frame */
586  struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
587  QUAT_INVERT(q_b2n, q_b2n);
588  struct FloatVect3 vel_ned;
589  float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
590 
591  // abi message contains an update to the horizontal velocity estimate
592 #if USE_HFF
593  struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
594  struct FloatVect2 Rvel = {noise_x, noise_y};
595 
596  hff_update_vel(vel, Rvel);
597  ins_update_from_hff();
598 #else
599  if (noise_x >= 0.f)
600  {
602  }
603  if (noise_y >= 0.f)
604  {
606  }
607 
608  static uint32_t last_stamp_x = 0, last_stamp_y = 0;
609  if (noise_x >= 0.f) {
610  if (last_stamp_x > 0)
611  {
612  float dt = (float)(stamp - last_stamp_x) * 1e-6;
613  ins_int.ltp_pos.x += lround(POS_BFP_OF_REAL(dt * vel_ned.x));
614  }
615  last_stamp_x = stamp;
616  }
617 
618  if (noise_y >= 0.f)
619  {
620  if (last_stamp_y > 0)
621  {
622  float dt = (float)(stamp - last_stamp_y) * 1e-6;
623  ins_int.ltp_pos.y += lround(POS_BFP_OF_REAL(dt * vel_ned.y));
624  }
625  last_stamp_y = stamp;
626  }
627 #endif
628 
629  // abi message contains an update to the vertical velocity estimate
630  vff_update_vz_conf(vel_ned.z, noise_z);
631 
633 
634  /* reset the counter to indicate we just had a measurement update */
636 }
637 
638 /* NED position estimate relative to ltp origin
639  */
640 static void pos_est_cb(uint8_t sender_id __attribute__((unused)),
641  uint32_t stamp __attribute__((unused)),
642  float x, float y, float z,
643  float noise_x, float noise_y, float noise_z)
644 {
645 
646 #if USE_HFF
647  struct FloatVect2 pos = {x, y};
648  struct FloatVect2 Rpos = {noise_x, noise_y};
649 
650  hff_update_pos(pos, Rpos);
651  ins_update_from_hff();
652 #else
653  if (noise_x >= 0.f)
654  {
656  }
657  if (noise_y >= 0.f)
658  {
660  }
661 #endif
662 
663  vff_update_z_conf(z, noise_z);
664 
666 
667  /* reset the counter to indicate we just had a measurement update */
669 }
stateSetAccelNed_i
static void stateSetAccelNed_i(struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition: state.h:986
ins_update_from_vff
static void ins_update_from_vff(void)
update ins state from vertical filter
Definition: ins_int.c:533
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
vf_extended_float.h
ins_reset_local_origin
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_int.c:249
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
InsInt::hf_realign
bool hf_realign
request to realign horizontal filter.
Definition: ins_int.h:48
InsInt::baro_z
float baro_z
z-position calculated from baro in meters (z-down)
Definition: ins_int.h:61
INS_INT_GPS_ID
#define INS_INT_GPS_ID
Definition: ins_int.c:128
ins_int_propagate
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
Definition: ins_int.c:296
ltp_def_from_ecef_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Definition: pprz_geodetic_int.c:60
InsInt::ltp_accel
struct NedCoor_i ltp_accel
Definition: ins_int.h:58
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
vff_update_vz_conf
void vff_update_vz_conf(float vz_meas, float conf)
Definition: vf_extended_float.c:433
pprz_geodetic_int.h
Paparazzi fixed point math for geodetic calculations.
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
ins_int_update_gps
void ins_int_update_gps(struct GpsState *gps_s)
Definition: ins_int.c:399
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
GpsState
data structure for GPS information
Definition: gps.h:90
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
stateGetNedToBodyQuat_i
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
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:66
SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:219
baro_ev
abi_event baro_ev
Definition: ins_int.c:114
HfilterFloat::xdot
float xdot
Definition: hf_float.h:42
InsInt::ltp_def
struct LtpDef_i ltp_def
Definition: ins_int.h:40
InsInt::propagation_cnt
uint32_t propagation_cnt
number of propagation steps since the last measurement update
Definition: ins_int.h:43
ins_int.h
vff_update_z_conf
void vff_update_z_conf(float z_meas, float conf)
Definition: vf_extended_float.c:310
nps_bypass_ins
bool nps_bypass_ins
Definition: nps_autopilot_fixedwing.c:54
VECT3_SUB
#define VECT3_SUB(_a, _b)
Definition: pprz_algebra.h:154
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_int.c:340
send_ins_z
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_int.c:179
nps_autopilot.h
stateSetAccelBody_i
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
INT32_POS_OF_CM_NUM
#define INT32_POS_OF_CM_NUM
Definition: pprz_algebra_int.h:70
VffExtended::z
float z
z-position estimate in m (NED, z-down)
Definition: vf_extended_float.h:37
InsInt::qfe
float qfe
Definition: ins_int.h:62
INS_INT_IMU_ID
#define INS_INT_IMU_ID
ABI binding for IMU data.
Definition: ins_int.c:121
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
idx
static uint32_t idx
Definition: nps_radio_control_spektrum.c:105
INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_DEN
Definition: pprz_algebra_int.h:71
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
VECT2_SDIV
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:104
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_int.c:568
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
vff_update
void vff_update(float z_meas)
Definition: vf_float.c:189
INT32_VECT3_ZERO
#define INT32_VECT3_ZERO(_v)
Definition: pprz_algebra_int.h:289
telemetry.h
stateGetNedToBodyRMat_i
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
imu.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
hff_init
void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:274
INS_MAX_PROPAGATION_STEPS
#define INS_MAX_PROPAGATION_STEPS
maximum number of propagation steps without any updates in between
Definition: ins_int.c:96
HfilterFloat::ydot
float ydot
Definition: hf_float.h:46
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_int.c:554
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
ACCEL_FLOAT_OF_BFP
#define ACCEL_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:222
HfilterFloat::x
float x
Definition: hf_float.h:41
ecef_int_from_gps
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition: gps.c:455
gps.h
Device independent GPS code (interface)
int32_quat_vmult
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_int.c:353
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:110
INS_BARO_MAX_INIT_VAR
#define INS_BARO_MAX_INIT_VAR
default barometer to use in INS
Definition: ins_int.c:105
vff_update_agl
void vff_update_agl(float z_meas, float conf)
Definition: vf_extended_float.c:321
HfilterFloat::y
float y
Definition: hf_float.h:45
int32_rmat_transp_vmult
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_int.c:117
InsInt::ltp_pos
struct NedCoor_i ltp_pos
Definition: ins_int.h:56
stateGetNedToBodyQuat_f
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
press_hist_len
#define press_hist_len
InsInt::baro_initialized
bool baro_initialized
Definition: ins_int.h:63
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_int.h:50
NedCoor_i
vector in North East Down coordinates
Definition: pprz_geodetic_int.h:68
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
ecef_vel_int_from_gps
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
Definition: gps.c:484
INT32_SPEED_OF_CM_S_DEN
#define INT32_SPEED_OF_CM_S_DEN
Definition: pprz_algebra_int.h:76
hff_update_pos
void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Update position.
Definition: hf_float.c:711
INS_INT_VEL_ID
#define INS_INT_VEL_ID
ABI binding for VELOCITY_ESTIMATE.
Definition: ins_int.c:138
Int32Vect3
Definition: pprz_algebra_int.h:88
pprz_stat.h
Statistics functions.
ACCEL_BFP_OF_REAL
#define ACCEL_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:221
vf_float.h
INS_INT_POS_ID
#define INS_INT_POS_ID
Definition: ins_int.c:147
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:51
stateSetLocalOrigin_i
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
hf_float.h
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
agl_ev
static abi_event agl_ev
The agl ABI event.
Definition: ins_int.c:163
hff_update_gps
void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition: hf_float.c:541
VffExtended::zdot
float zdot
z-velocity estimate in m/s (NED, z-down)
Definition: vf_extended_float.h:38
accel_ev
static abi_event accel_ev
Definition: ins_int.c:124
ned_of_ecef_vect_i
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.
Definition: pprz_geodetic_int.c:186
send_ins
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_int.c:171
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
ins_init_origin_i_from_flightplan
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
stateSetPositionNed_i
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
vff_realign
void vff_realign(float z_meas)
Definition: vf_extended_float.c:384
InsInt
Ins implementation state (fixed point)
Definition: ins_int.h:39
INS_INT_BARO_ID
#define INS_INT_BARO_ID
Definition: ins_int.c:110
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
float_quat_vmult
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
Definition: pprz_algebra_float.c:421
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_int.c:185
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
pprz_isa_height_of_pressure
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
ins_int_init
void ins_int_init(void)
Definition: ins_int.c:203
HfilterFloat::ydotdot
float ydotdot
Definition: hf_float.h:47
vff_update_baro
void vff_update_baro(float z_meas)
Definition: vf_extended_float.c:333
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
VffExtended::zdotdot
float zdotdot
z-acceleration in m/s^2 (NED, z-down)
Definition: vf_extended_float.h:43
QUAT_INVERT
#define QUAT_INVERT(_qo, _qi)
Definition: pprz_algebra.h:627
GpsFixValid
#define GpsFixValid()
Definition: gps.h:46
VECT2_ASSIGN
#define VECT2_ASSIGN(_a, _x, _y)
Definition: pprz_algebra.h:62
sim_overwrite_ins
void sim_overwrite_ins(void)
Definition: nps_autopilot_fixedwing.c:247
agl_cb
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:515
variance_f
float variance_f(float *array, uint32_t n_elements)
Compute the variance of an array of values (float).
Definition: pprz_stat.c:139
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
lla_int_from_gps
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
Definition: gps.c:426
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
ins_ned_to_state
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition: ins_int.c:519
INS_VFF_R_GPS
#define INS_VFF_R_GPS
Definition: ins_int.c:86
vff
struct VffExtended vff
Definition: vf_extended_float.c:89
pos_est_cb
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:640
vff_init_zero
void vff_init_zero(void)
Definition: vf_extended_float.c:107
ins_reset_altitude_ref
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_int.c:273
vel_est_cb
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:578
ins_reset_vertical_pos
void ins_reset_vertical_pos(void)
INS vertical position reset.
Definition: ins_int.c:291
vff_propagate
void vff_propagate(float accel, float dt)
Propagate the filter in time.
Definition: vf_extended_float.c:181
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:97
hff_update_vel
void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:806
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
InsInt::ltp_speed
struct NedCoor_i ltp_speed
Definition: ins_int.h:57
stabilization_cmd
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
VFF_R_AGL
#define VFF_R_AGL
Definition: ins_int.c:59
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
ned_of_ecef_point_i
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.
Definition: pprz_geodetic_int.c:116
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
stabilization.h
InsInt::vf_reset
bool vf_reset
request to reset vertical filter.
Definition: ins_int.h:53
hff_propagate
void hff_propagate(void)
Definition: hf_float.c:479
gps_ev
static abi_event gps_ev
Definition: ins_int.c:131
hff_realign
void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:602
INS_INT_AGL_ID
#define INS_INT_AGL_ID
ABI binding for AGL.
Definition: ins_int.c:160
state
struct State state
Definition: state.c:36
pos_est_ev
static abi_event pos_est_ev
Definition: ins_int.c:150
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
stateSetSpeedNed_i
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
vel_est_ev
static abi_event vel_est_ev
Definition: ins_int.c:141
hff
struct HfilterFloat hff
Definition: hf_float.c:124
INT32_SPEED_OF_CM_S_NUM
#define INT32_SPEED_OF_CM_S_NUM
Definition: pprz_algebra_int.h:75
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
INS_VFF_VZ_R_GPS
#define INS_VFF_VZ_R_GPS
Definition: ins_int.c:90
HfilterFloat::xdotdot
float xdotdot
Definition: hf_float.h:43
ins_int
struct InsInt ins_int
global INS state
Definition: ins_int.c:166
InsInt::ltp_initialized
bool ltp_initialized
Definition: ins_int.h:41
INT32_VECT2_SCALE_2
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
Definition: pprz_algebra_int.h:279