Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
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
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
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
101PRINT_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#define INS_INT_BARO_ID AIR_DATA_SENDER_ID
108#endif
111static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure);
112
116#ifndef INS_INT_IMU_ID
117#define INS_INT_IMU_ID ABI_BROADCAST
118#endif
121static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
122
123#ifndef INS_INT_GPS_ID
124#define INS_INT_GPS_ID GPS_MULTI_ID
125#endif
128static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
129
133#ifndef INS_INT_VEL_ID
134#define INS_INT_VEL_ID ABI_BROADCAST
135#endif
138static void vel_est_cb(uint8_t sender_id,
139 uint32_t stamp,
140 float x, float y, float z,
141 float noise_x, float noise_y, float noise_z);
142#ifndef INS_INT_POS_ID
143#define INS_INT_POS_ID ABI_BROADCAST
144#endif
147static void pos_est_cb(uint8_t sender_id,
148 uint32_t stamp,
149 float x, float y, float z,
150 float noise_x, float noise_y, float noise_z);
151
155#ifndef INS_INT_AGL_ID
156#define INS_INT_AGL_ID ABI_BROADCAST
157#endif
160static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
161
162
164static void reset_cb(uint8_t sender_id, uint8_t flag);
165
167
168#if PERIODIC_TELEMETRY
170
171static void send_ins(struct transport_tx *trans, struct link_device *dev)
172{
173 pprz_msg_send_INS(trans, dev, AC_ID,
174 &ins_int.ltp_pos.x, &ins_int.ltp_pos.y, &ins_int.ltp_pos.z,
175 &ins_int.ltp_speed.x, &ins_int.ltp_speed.y, &ins_int.ltp_speed.z,
176 &ins_int.ltp_accel.x, &ins_int.ltp_accel.y, &ins_int.ltp_accel.z);
177}
178
179static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
180{
181 pprz_msg_send_INS_Z(trans, dev, AC_ID,
182 &ins_int.baro_z, &ins_int.ltp_pos.z, &ins_int.ltp_speed.z, &ins_int.ltp_accel.z);
183}
184
185static 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,
189 &ins_int.ltp_def.ecef.x, &ins_int.ltp_def.ecef.y, &ins_int.ltp_def.ecef.z,
190 &ins_int.ltp_def.lla.lat, &ins_int.ltp_def.lla.lon, &ins_int.ltp_def.lla.alt,
191 &ins_int.ltp_def.hmsl, &ins_int.qfe);
192 }
193}
194#endif
195
196static void ins_ned_to_state(void);
197static void ins_update_from_vff(void);
198#if USE_HFF
199static void ins_update_from_hff(void);
200#endif
201
202
203void ins_int_init(void)
204{
205
206#if USE_INS_NAV_INIT
207 ins_init_origin_i_from_flightplan(MODULE_INS_INT_COMMON_ID, &ins_int.ltp_def);
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 */
214 ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
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 */
225#if USE_HFF
226 hff_init(0., 0., 0., 0.);
227#endif
228
229 INT32_VECT3_ZERO(ins_int.ltp_pos);
230 INT32_VECT3_ZERO(ins_int.ltp_speed);
231 INT32_VECT3_ZERO(ins_int.ltp_accel);
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 AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
248}
249
250static void reset_ref(void)
251{
252#if USE_GPS
253 if (GpsFixValid()) {
254 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
255 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
256 ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_pos);
257 ins_int.ltp_def.lla.alt = lla_pos.alt;
258 ins_int.ltp_def.hmsl = gps.hmsl;
259 ins_int.ltp_initialized = true;
260 stateSetLocalOrigin_i(MODULE_INS_INT_COMMON_ID, &ins_int.ltp_def);
261 } else {
262 ins_int.ltp_initialized = false;
263 }
264#else
265 ins_int.ltp_initialized = false;
266#endif
267
268#if USE_HFF
269 ins_int.hf_realign = true;
270#endif
271 ins_int.vf_reset = true;
272}
273
274static void reset_vertical_ref(void)
275{
276#if USE_GPS
277 if (GpsFixValid()) {
278 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
279 struct LlaCoor_i lla = {
280 .lat = stateGetLlaOrigin_i().lat,
281 .lon = stateGetLlaOrigin_i().lon,
282 .alt = lla_pos.alt
283 };
284 ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
285 ins_int.ltp_def.hmsl = gps.hmsl;
286 stateSetLocalOrigin_i(MODULE_INS_INT_COMMON_ID, &ins_int.ltp_def);
287 }
288#endif
289 ins_int.vf_reset = true;
290}
291
292static void reset_vertical_pos(void)
293{
294 ins_int.vf_reset = true;
295}
296
297static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
298{
299 switch (flag) {
300 case INS_RESET_REF:
301 reset_ref();
302 break;
305 break;
308 break;
309 default:
310 // unsupported cases
311 break;
312 }
313}
314
315void ins_int_propagate(struct Int32Vect3 *accel, float dt)
316{
317 // Set body acceleration in the state
318 stateSetAccelBody_i(MODULE_INS_INT_COMMON_ID, accel);
319
320 /* untilt accels */
321 struct Int32Vect3 accel_meas_ltp;
322 int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), accel);
323
324 float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
325
326 /* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS.
327 * Otherwise halt the propagation to not diverge and only set the acceleration.
328 * This should only be relevant in the startup phase when the baro is not yet initialized
329 * and there is no gps fix yet...
330 */
331 if (ins_int.propagation_cnt < INS_MAX_PROPAGATION_STEPS) {
332 vff_propagate(z_accel_meas_float, dt);
334 } else {
335 // feed accel from the sensors
336 // subtract -9.81m/s2 (acceleration measured due to gravity,
337 // but vehicle not accelerating in ltp)
338 ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
339 }
340
341#if USE_HFF
342 /* propagate horizontal filter */
344 /* convert and copy result to ins_int */
345 ins_update_from_hff();
346#else
347 ins_int.ltp_accel.x = accel_meas_ltp.x;
348 ins_int.ltp_accel.y = accel_meas_ltp.y;
349#endif /* USE_HFF */
350
352
353 /* increment the propagation counter, while making sure it doesn't overflow */
354 if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) {
355 ins_int.propagation_cnt++;
356 }
357}
358
359static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
360{
361 if (pressure < 1.f)
362 {
363 // bad baro pressure, don't use
364 return;
365 }
366
367 if (!ins_int.baro_initialized) {
368#define press_hist_len 10
369 static float press_hist[press_hist_len];
370 static uint8_t idx = 0;
371
372 press_hist[idx] = pressure;
373 idx = (idx + 1) % press_hist_len;
374 float var = variance_f(press_hist, press_hist_len);
375 if (var < INS_BARO_MAX_INIT_VAR){
376 // wait for a first positive value
377 ins_int.vf_reset = true;
378 ins_int.baro_initialized = true;
379 }
380 }
381
382 if (ins_int.baro_initialized) {
383 float height_correction = 0.f;
384 if(ins_int.ltp_initialized){
385 // Calculate the distance to the origin
386 struct EnuCoor_f *enu = stateGetPositionEnu_f();
387 double dist2_to_origin = enu->x * enu->x + enu->y * enu->y;
388
389 // correction for the earth's curvature
390 const double earth_radius = 6378137.0;
391 height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
392 }
393
394 if (ins_int.vf_reset) {
395 ins_int.vf_reset = false;
396 ins_int.qfe = pressure;
397 vff_realign(height_correction);
399 }
400
401 float baro_up = pprz_isa_height_of_pressure(pressure, ins_int.qfe);
402
403 // The VFF will update in the NED frame
404 ins_int.baro_z = -(baro_up - height_correction);
405
406#if USE_VFF_EXTENDED
407 vff_update_baro(ins_int.baro_z);
408#else
409 vff_update(ins_int.baro_z);
410#endif
411
412 /* reset the counter to indicate we just had a measurement update */
413 ins_int.propagation_cnt = 0;
414 }
415}
416
417#if USE_GPS
418void ins_int_update_gps(struct GpsState *gps_s)
419{
420 if (gps_s->fix < GPS_FIX_3D) {
421 return;
422 }
423
424 if (!ins_int.ltp_initialized) {
425 reset_ref();
426 }
427
428 struct NedCoor_i gps_pos_cm_ned;
429 struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
430 ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &ecef_pos_i);
431
432 /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
433#ifdef INS_BODY_TO_GPS_X
434 /* body2gps translation in body frame */
435 struct Int32Vect3 b2g_b = {
436 .x = INS_BODY_TO_GPS_X,
437 .y = INS_BODY_TO_GPS_Y,
438 .z = INS_BODY_TO_GPS_Z
439 };
440 /* rotate offset given in body frame to navigation/ltp frame using current attitude */
441 struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i();
442 QUAT_INVERT(q_b2n, q_b2n);
443 struct Int32Vect3 b2g_n;
444 int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
445 /* subtract body2gps translation in ltp from gps position */
446 VECT3_SUB(gps_pos_cm_ned, b2g_n);
447#endif
448
450 struct NedCoor_i gps_speed_cm_s_ned;
451 struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s);
452 ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &ecef_vel_i);
453
454#if INS_USE_GPS_ALT
455 vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
456#endif
457#if INS_USE_GPS_ALT_SPEED
458 vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
459 ins_int.propagation_cnt = 0;
460#endif
461
462#if USE_HFF
463 /* horizontal gps transformed to NED in meters as float */
464 struct FloatVect2 gps_pos_m_ned;
465 VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
466 VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
467
468 struct FloatVect2 gps_speed_m_s_ned;
469 VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
470 VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.f);
471
472 if (ins_int.hf_realign) {
473 ins_int.hf_realign = false;
474 hff_realign(gps_pos_m_ned, gps_speed_m_s_ned);
475 }
476 // run horizontal filter
477 hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
478 // convert and copy result to ins_int
479 ins_update_from_hff();
480
481#else /* hff not used */
482 /* simply copy horizontal pos/speed from gps */
483 INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
485 INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
487#endif /* USE_HFF */
488
490
491 /* reset the counter to indicate we just had a measurement update */
492 ins_int.propagation_cnt = 0;
493}
494#else
495void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
496#endif /* USE_GPS */
497
502#if USE_VFF_EXTENDED
503static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float distance) {
504 if (distance <= 0 || !(ins_int.baro_initialized)) {
505 return;
506 }
507
508#if USE_SONAR
509 if (distance > INS_SONAR_MAX_RANGE || distance < INS_SONAR_MIN_RANGE){
510 return;
511 }
512#endif
513#ifdef INS_AGL_THROTTLE_THRESHOLD
514 if(stabilization.cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
515 return;
516 }
517#endif
518#ifdef INS_AGL_BARO_THRESHOLD
519 if(ins_int.baro_z < -INS_SONAR_BARO_THRESHOLD){ /* z down */
520 return;
521 }
522#endif
523
524#if USE_SONAR
525 vff_update_agl(-distance, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
526#else
527 // TODO: this assumes that you will either have sonar or other agl sensor never both
528 vff_update_agl(-distance, VFF_R_AGL);
529#endif
530 /* reset the counter to indicate we just had a measurement update */
531 ins_int.propagation_cnt = 0;
532}
533#else
534static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, __attribute__((unused)) float distance) {}
535#endif
536
538static void ins_ned_to_state(void)
539{
540 stateSetPositionNed_i(MODULE_INS_INT_COMMON_ID, &ins_int.ltp_pos);
541 stateSetSpeedNed_i(MODULE_INS_INT_COMMON_ID, &ins_int.ltp_speed);
542 stateSetAccelNed_i(MODULE_INS_INT_COMMON_ID, &ins_int.ltp_accel);
543
544#if defined SITL && USE_NPS
545 if (nps_bypass_ins) {
547 }
548#endif
549}
550
552static void ins_update_from_vff(void)
553{
554 ins_int.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot);
555 ins_int.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot);
556 ins_int.ltp_pos.z = POS_BFP_OF_REAL(vff.z);
557}
558
559#if USE_HFF
561static void ins_update_from_hff(void)
562{
563 ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(hff.xdotdot);
564 ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(hff.ydotdot);
565 ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(hff.xdot);
566 ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(hff.ydot);
567 ins_int.ltp_pos.x = POS_BFP_OF_REAL(hff.x);
568 ins_int.ltp_pos.y = POS_BFP_OF_REAL(hff.y);
569}
570#endif
571
572
573static void accel_cb(uint8_t sender_id __attribute__((unused)),
574 uint32_t stamp, struct Int32Vect3 *accel)
575{
576 PRINT_CONFIG_MSG("Calculating dt for INS int propagation.")
577 /* timestamp in usec when last callback was received */
578 static uint32_t last_stamp = 0;
579
580 if (last_stamp > 0) {
581 float dt = (float)(stamp - last_stamp) * 1e-6;
582 ins_int_propagate(accel, dt);
583 }
584 last_stamp = stamp;
585}
586
587static void gps_cb(uint8_t sender_id __attribute__((unused)),
588 uint32_t stamp __attribute__((unused)),
589 struct GpsState *gps_s)
590{
591 ins_int_update_gps(gps_s);
592}
593
594/* body relative velocity estimate
595 *
596 */
597static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
598 uint32_t stamp __attribute__((unused)),
599 float x, float y, float z,
600 float noise_x, float noise_y, float noise_z)
601{
602 struct FloatVect3 vel_body = {x, y, z};
603
604 /* rotate velocity estimate to nav/ltp frame */
605 struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
606 QUAT_INVERT(q_b2n, q_b2n);
607 struct FloatVect3 vel_ned;
608 float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
609
610 // abi message contains an update to the horizontal velocity estimate
611#if USE_HFF
612 struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
613 struct FloatVect2 Rvel = {noise_x, noise_y};
614
615 hff_update_vel(vel, Rvel);
616 ins_update_from_hff();
617#else
618 if (noise_x >= 0.f)
619 {
620 ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
621 }
622 if (noise_y >= 0.f)
623 {
624 ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
625 }
626
627 static uint32_t last_stamp_x = 0, last_stamp_y = 0;
628 if (noise_x >= 0.f) {
629 if (last_stamp_x > 0)
630 {
631 float dt = (float)(stamp - last_stamp_x) * 1e-6;
632 ins_int.ltp_pos.x += lround(POS_BFP_OF_REAL(dt * vel_ned.x));
633 }
634 last_stamp_x = stamp;
635 }
636
637 if (noise_y >= 0.f)
638 {
639 if (last_stamp_y > 0)
640 {
641 float dt = (float)(stamp - last_stamp_y) * 1e-6;
642 ins_int.ltp_pos.y += lround(POS_BFP_OF_REAL(dt * vel_ned.y));
643 }
644 last_stamp_y = stamp;
645 }
646#endif
647
648 // abi message contains an update to the vertical velocity estimate
649 vff_update_vz_conf(vel_ned.z, noise_z);
650
652
653 /* reset the counter to indicate we just had a measurement update */
654 ins_int.propagation_cnt = 0;
655}
656
657/* NED position estimate relative to ltp origin
658 */
659static void pos_est_cb(uint8_t sender_id __attribute__((unused)),
660 uint32_t stamp __attribute__((unused)),
661 float x, float y, float z,
662 float noise_x, float noise_y, float noise_z)
663{
664
665#if USE_HFF
666 struct FloatVect2 pos = {x, y};
667 struct FloatVect2 Rpos = {noise_x, noise_y};
668
669 hff_update_pos(pos, Rpos);
670 ins_update_from_hff();
671#else
672 if (noise_x >= 0.f)
673 {
674 ins_int.ltp_pos.x = POS_BFP_OF_REAL(x);
675 }
676 if (noise_y >= 0.f)
677 {
678 ins_int.ltp_pos.y = POS_BFP_OF_REAL(y);
679 }
680#endif
681
682 vff_update_z_conf(z, noise_z);
683
685
686 /* reset the counter to indicate we just had a measurement update */
687 ins_int.propagation_cnt = 0;
688}
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event gps_ev
#define UNUSED(x)
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct GpsState gps
global GPS state
Definition gps.c:74
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:478
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:536
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:507
Device independent GPS code (interface).
#define GpsFixValid()
Definition gps.h:171
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
uint8_t fix
status of fix
Definition gps.h:110
data structure for GPS information
Definition gps.h:90
void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
Roation quaternion.
#define QUAT_INVERT(_qo, _qi)
#define VECT3_SUB(_a, _b)
#define VECT2_SDIV(_vo, _vi, _s)
#define VECT2_ASSIGN(_a, _x, _y)
#define INT32_POS_OF_CM_DEN
#define INT32_SPEED_OF_CM_S_NUM
#define ACCEL_BFP_OF_REAL(_af)
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den)
void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in)
rotate 3D vector by quaternion.
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_ZERO(_v)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
#define POS_BFP_OF_REAL(_af)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
Rotation quaternion.
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t y
East.
int32_t lon
in degrees*1e7
int32_t x
North.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
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.
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.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates
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
static void stateSetAccelNed_i(uint16_t id, struct NedCoor_i *ned_accel)
Set acceleration in NED coordinates (int).
Definition state.h:1146
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition state.h:1301
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:530
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition state.h:649
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition state.h:903
void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition hf_float.c:274
struct HfilterFloat hff
Definition hf_float.c:124
void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition hf_float.c:541
void hff_propagate(void)
Definition hf_float.c:479
void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition hf_float.c:806
void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition hf_float.c:602
void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Update position.
Definition hf_float.c:711
Horizontal filter (x,y) to estimate position and velocity.
Inertial Measurement Unit interface.
void ins_init_origin_i_from_flightplan(uint16_t id, struct LtpDef_i *ltp_def)
initialize the local origin (ltp_def in fixed point) from flight plan position
Definition ins.c:33
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_VERTICAL_POS
Definition ins.h:38
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
abi_event baro_ev
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:563
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:608
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:589
struct InsInt ins_int
global INS state
Definition ins_int.c:166
#define INS_INT_IMU_ID
ABI binding for IMU data.
Definition ins_int.c:117
static void reset_vertical_ref(void)
Definition ins_int.c:274
#define INS_INT_AGL_ID
ABI binding for AGL.
Definition ins_int.c:156
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition ins_int.c:587
static void ins_update_from_vff(void)
update ins state from vertical filter
Definition ins_int.c:552
static void reset_cb(uint8_t sender_id, uint8_t flag)
Definition ins_int.c:297
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_int.c:171
static abi_event pos_est_ev
Definition ins_int.c:146
#define VFF_R_AGL
Definition ins_int.c:59
#define INS_INT_GPS_ID
Definition ins_int.c:124
static void reset_ref(void)
Definition ins_int.c:250
#define INS_BARO_MAX_INIT_VAR
default barometer to use in INS
Definition ins_int.c:105
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
Definition ins_int.c:315
void ins_int_update_gps(struct GpsState *gps_s)
Definition ins_int.c:418
void ins_int_init(void)
Definition ins_int.c:203
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_int.c:185
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:534
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition ins_int.c:179
#define INS_INT_VEL_ID
ABI binding for VELOCITY_ESTIMATE.
Definition ins_int.c:134
#define INS_MAX_PROPAGATION_STEPS
maximum number of propagation steps without any updates in between
Definition ins_int.c:96
static abi_event vel_est_ev
Definition ins_int.c:137
static void ins_ned_to_state(void)
copy position and speed to state interface
Definition ins_int.c:538
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition ins_int.c:359
#define INS_VFF_VZ_R_GPS
Definition ins_int.c:90
static void reset_vertical_pos(void)
Definition ins_int.c:292
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition ins_int.c:573
#define INS_INT_POS_ID
Definition ins_int.c:143
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:597
#define press_hist_len
#define INS_INT_BARO_ID
Definition ins_int.c:107
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:659
#define INS_VFF_R_GPS
Definition ins_int.c:86
INS for rotorcrafts combining vertical and horizontal filters.
Ins implementation state (fixed point).
Definition ins_int.h:39
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
bool nps_bypass_ins
void sim_overwrite_ins(void)
static uint32_t idx
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static abi_event agl_ev
The altitude ABI event.
float y
in meters
float x
in meters
vector in East North Up coordinates Units: meters
Paparazzi fixed point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
float variance_f(float *array, uint32_t n_elements)
Compute the variance of an array of values (float).
Definition pprz_stat.c:139
Statistics functions.
struct Stabilization stabilization
General stabilization interface for rotorcrafts.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
void vff_init_zero(void)
void vff_update_z_conf(float z_meas, float conf)
void vff_update_vz_conf(float vz_meas, float conf)
void vff_update_baro(float z_meas)
void vff_realign(float z_meas)
void vff_propagate(float accel, float dt)
Propagate the filter in time.
struct VffExtended vff
void vff_update_agl(float z_meas, float conf)
Interface for extended vertical filter (in float).
void vff_update(float z_meas)
Definition vf_float.c:189
Vertical filter (in float) estimating altitude, velocity and accel bias.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.