Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_float_invariant.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
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, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
30
32
33#include "modules/ins/ins.h"
34#include "modules/gps/gps.h"
35
36#include "generated/airframe.h"
37#include "generated/flight_plan.h"
38#if FIXEDWING_FIRMWARE
40#endif
41
44#include "math/pprz_rk_float.h"
45#include "math/pprz_isa.h"
46
47#include "state.h"
48
49// for debugging
50#if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
52#endif
53
54#if LOG_INVARIANT_FILTER
56bool log_started = false;
57#endif
58
59/*------------- =*= Invariant Observers =*= -------------*
60 *
61 * State vector :
62 *
63 * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
64 *
65 * Dynamic model (dim = 15) :
66 *
67 * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
68 * x_Vdot = A + 1/as (q * am * (q)-1);
69 * x_Xdot = V;
70 * x_bias_dot = 0;
71 * x_asdot = 0;
72 * x_hbdot = 0;
73 *
74 * Observation model (dim = 10):
75 * yv = V;
76 * yx = X;
77 * yh = <X,e3> - hb;
78 * yb = (q)-1 *B * q; (B : magnetometers)
79 *
80 *------------------------------------------------------*/
81
82// Default values for the tuning gains
83// Tuning parameter of speed error on attitude (e-2)
84#ifndef INS_INV_LV
85#define INS_INV_LV 2.f
86#endif
87// Tuning parameter of mag error on attitude (e-2)
88#ifndef INS_INV_LB
89#define INS_INV_LB 6.f
90#endif
91// Tuning parameter of horizontal speed error on speed
92#ifndef INS_INV_MV
93#define INS_INV_MV 8.f
94#endif
95// Tuning parameter of vertical speed error on speed
96#ifndef INS_INV_MVZ
97#define INS_INV_MVZ 15.f
98#endif
99// Tuning parameter of baro error on vertical speed
100#ifndef INS_INV_MH
101#define INS_INV_MH 0.2f
102#endif
103// Tuning parameter of horizontal position error on position
104#ifndef INS_INV_NX
105#define INS_INV_NX 0.8f
106#endif
107// Tuning parameter of vertical position error on position
108#ifndef INS_INV_NXZ
109#define INS_INV_NXZ 0.5f
110#endif
111// Tuning parameter of baro error on vertical position
112#ifndef INS_INV_NH
113#define INS_INV_NH 1.2f
114#endif
115// Tuning parameter of speed error on gyro biases (e-3)
116#ifndef INS_INV_OV
117#define INS_INV_OV 1.2f
118#endif
119// Tuning parameter of mag error on gyro biases (e-3)
120#ifndef INS_INV_OB
121#define INS_INV_OB 1.f
122#endif
123// Tuning parameter of speed error on accel biases (e-2)
124#ifndef INS_INV_RV
125#define INS_INV_RV 4.f
126#endif
127// Tuning parameter of baro error on accel biases (vertical projection) (e-8)
128#ifndef INS_INV_RH
129#define INS_INV_RH 8.f
130#endif
131// Tuning parameter of baro error on baro bias
132#ifndef INS_INV_SH
133#define INS_INV_SH 0.01f
134#endif
135
136
138
139/* earth gravity model */
140static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
141
142/* earth magnetic model */
143//static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
144#define B ins_float_inv.mag_h
145
146/* barometer */
148
149/* gps */
151
152/* min speed to update heading from GPS when mag are not used */
153#ifndef INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
154#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED 5.f
155#endif
156
157/* error computation */
158static inline void error_output(struct InsFloatInv *_ins);
159
160/* propagation model (called by runge-kutta library) */
161static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
162
163
167void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
168 struct FloatVect3 *vi);
169
170
171/* init state and measurements */
191
192#if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
217#endif
218
220{
221
222 // init position
223#if FIXEDWING_FIRMWARE
224 struct UtmCoor_f utm0;
226 utm0.east = (float)nav_utm_east0;
227 utm0.alt = GROUND_ALT;
228 utm0.zone = nav_utm_zone0;
231#else
232 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
234 llh_nav0.lon = NAV_LON0;
235 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
236 llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
237 struct EcefCoor_i ecef_nav0;
239 struct LtpDef_i ltp_def;
243#endif
244
245#if USE_MAGNETOMETER
246 B.x = INS_H_X;
247 B.y = INS_H_Y;
248 B.z = INS_H_Z;
249#else
250 B.x = 1.f; // when using GPS as magnetometer, mag field is true north
251 B.y = 0.f;
252 B.z = 0.f;
253#endif
254
255 // init state and measurements
257
258 // init gains
272
274 ins_float_inv.reset = false;
275
276#if PERIODIC_TELEMETRY
278#endif
279}
280
282{
283#if FIXEDWING_FIRMWARE
284 struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
285 // reset state UTM ref
287#else
288 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
289 struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
290 struct LtpDef_i ltp_def;
291 ltp_def_from_ecef_i(&ltp_def, &ecef_pos);
292 ltp_def.lla.alt = lla_pos.alt;
295#endif
296 // reset state position and velocity to zero
299}
300
302{
303#if FIXEDWING_FIRMWARE
305 utm.alt = gps.hmsl / 1000.0f;
307#else
308 struct LlaCoor_i lla = {
310 .lon = stateGetLlaOrigin_i().lon,
311 .alt = gps.lla_pos.alt
312 };
313 struct LtpDef_i ltp_def;
317#endif
318 // reset vertical position and velocity to zero
319 ins_float_inv.state.pos.z = 0.f;
321}
322
324 struct FloatVect3 *lp_accel,
325 struct FloatVect3 *lp_mag __attribute__((unused)))
326{
327#if USE_MAGNETOMETER
328 /* Compute an initial orientation from accel and mag directly as quaternion */
330#else
331 /* Compute an initial orientation from accel only directly as quaternion */
333#endif
334
335 /* use average gyro as initial value for bias */
336 ins_float_inv.state.bias = *lp_gyro;
337
338 /* push initial values to state interface */
340
341 // ins and ahrs are now running
343}
344
345void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
346{
347 struct FloatRates body_rates;
348
349 // realign all the filter if needed
350 // a complete init cycle is required
351 if (ins_float_inv.reset) {
352 ins_float_inv.reset = false;
355 }
356
357 // fill command vector in body frame
360
361 struct Int32Vect3 body_accel_i;
364
365 // update correction gains
367
368 // propagate model
369 struct inv_state new_state;
373 invariant_model, dt);
375
376 // normalize quaternion
378
379 // set global state
385 // untilt accel and remove gravity
386 struct FloatQuat q_b2n;
388 struct FloatVect3 accel_n;
392 struct NedCoor_i accel_coord;
395
396 //------------------------------------------------------------//
397
398#if SEND_INVARIANT_FILTER
399 RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
400#endif
401
402#if LOG_INVARIANT_FILTER
403 if (pprzLogFile != -1) {
404 if (!log_started) {
405 // log file header
407 "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n");
408 log_started = true;
409 } else {
411 "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
443 }
444 }
445#endif
446}
447
449{
450
451 if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
452#if FIXEDWING_FIRMWARE
455 // position (local ned)
459 // speed
461 }
462#else
464 // position
466 struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
470 // speed
471 struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
473 }
474#endif
475 // when getting first GPS pos (or after reset), set state to current pos and speed
476 if (!ins_gps_fix_once) {
479 ins_gps_fix_once = true;
480 }
481 }
482
483#if !USE_MAGNETOMETER
484 struct FloatVect3 pseudo_mag = { 0.f, 0.f, 0.f };
485 bool update_mag = false;
486#if INS_INV_USE_GPS_HEADING
487 // got a 3d fix, ground speed > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
488 // and course accuracy is better than 10deg
490 static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
491 if (gps_s->fix >= GPS_FIX_3D &&
492 gps_s->gspeed >= gps_min_speed &&
493 gps_s->cacc <= max_cacc) {
494 // gps_s->course is in rad * 1e7
495 float course = (float)gps_s->course / 1e7f;
497 pseudo_mag.y = -sinf(course);
498 update_mag = true;
499 }
500#else // else use GPS velocity (only for fixedwing)
501 // Use pseudo-mag rebuilt from GPS horizontal velocity
503 float vel_norm = float_vect2_norm(&vel);
505 pseudo_mag.x = vel.x / vel_norm;
506 pseudo_mag.y = -vel.y / vel_norm;
507 update_mag = true;
508 }
509#endif
510 if (update_mag) {
511 // create quat corresponding to psi
512 // quat_psi = [ cos(psi/2) 0 0 sin(psi/2) ]'
513 struct FloatEulers angles;
515 struct FloatQuat quat_psi = { cosf(angles.psi / 2.f), 0.f, 0.f, sinf(angles.psi / 2.f) };
516 // build quat corresponding to roll and pitch (from quat psi and current quat)
517 struct FloatQuat quat_horiz;
519 // rotate pseudo mag around horiz quaternion
520 struct FloatVect3 mag_final;
523 } else {
524 // if speed is tool low, better set measurements to zero
526 }
527#endif
528
529}
530
531
533{
535 static float alpha = 10.0f;
536 static int32_t i = 1;
537 static float baro_moy = 0.0f;
538 static float baro_prev = 0.0f;
539
541 // try to find a stable qfe
542 // TODO generic function in pprz_isa ?
543 if (i == 1) {
544 baro_moy = pressure;
545 baro_prev = pressure;
546 }
547 baro_moy = (baro_moy * (i - 1) + pressure) / i;
548 alpha = (10.f*alpha + (baro_moy - baro_prev)) / (11.0f);
550 // test stop condition
551 if (fabs(alpha) < 0.1f) {
554 }
555 if (i == 250) {
556 ins_qfe = pressure;
558 }
559 i++;
560 } else { /* normal update with baro measurement */
562 }
563}
564
565// assume mag is dead when values are not moving anymore
566#define MAG_FROZEN_COUNT 30
567
569{
571 static int32_t last_mx = 0;
572
573 if (last_mx == mag->x) {
575 if (mag_frozen_count == 0) {
576 // if mag is dead, better set measurements to zero
579 }
580 } else {
581 // new values in body frame
583 // reset counter
585 }
586 last_mx = mag->x;
587}
588
589
594static inline void invariant_model(float *o, const float *x, const int n, const float *u,
595 const int m __attribute__((unused)))
596{
597
598#pragma GCC diagnostic push // require GCC 4.6
599#pragma GCC diagnostic ignored "-Wcast-qual"
600 struct inv_state *s = (struct inv_state *)x;
601 struct inv_command *c = (struct inv_command *)u;
602#pragma GCC diagnostic pop // require GCC 4.6
603 struct inv_state s_dot;
605 struct FloatVect3 tmp_vect;
606 struct FloatQuat tmp_quat;
607
608 /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
609 RATES_DIFF(rates_unbiased, c->rates, s->bias);
610 /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
611 float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
612
614 QUAT_ADD(s_dot.quat, tmp_quat);
615
616 float norm2_r = 1.f - FLOAT_QUAT_NORM2(s->quat);
617 QUAT_SMUL(tmp_quat, s->quat, norm2_r);
618 QUAT_ADD(s_dot.quat, tmp_quat);
619
620 /* dot_V = A + (1/as) * (q * am * q-1) + ME */
621 struct FloatQuat q_b2n;
622 float_quat_invert(&q_b2n, &(s->quat));
624 VECT3_COPY(s_dot.speed, tmp_vect);
625 VECT3_SMUL(s_dot.speed, s_dot.speed, 1.f / (s->as));
626 VECT3_ADD(s_dot.speed, A);
628
629 /* dot_X = V + NE */
630 VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
631
632 /* bias_dot = q-1 * (OE) * q */
635
636 /* as_dot = as * RE */
637 s_dot.as = (s->as) * (ins_float_inv.corr.RE);
638 // keep as in a reasonable range, so 50% around the nominal value
639 if ( ((s->as < 0.5f) && (s_dot.as < 0.f)) || ((s->as > 1.5f) && (s_dot.as > 0.f)) ) {
640 s_dot.as = 0.f;
641 }
642
643 /* hb_dot = SE */
645
646 // set output
647 memcpy(o, &s_dot, n * sizeof(float));
648}
649
654static inline void error_output(struct InsFloatInv *_ins)
655{
656
657 struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
658 float Eh;
659 float temp;
660
661 /* YBt = q * yB * q-1 */
662 struct FloatQuat q_b2n;
663 float_quat_invert(&q_b2n, &(_ins->state.quat));
664 float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
665
666 float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
667 VECT3_SMUL(I, I, 1.f / (_ins->state.as));
668
669 /*--------- E = ( ŷ - y ) ----------*/
670 /* Eb = ( B - YBt ) */
671 VECT3_DIFF(Eb, B, YBt);
672
673 // pos and speed error only if GPS data are valid
674 // or while waiting first GPS data to prevent diverging
675 if ((gps.fix >= GPS_FIX_3D && _ins->is_aligned
678#else
680#endif
681 ) || !ins_gps_fix_once) {
682 /* Ev = (V - YV) */
683 VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
684 /* Ex = (X - YX) */
685 VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
686 } else {
689 }
690 /* Eh = < X,e3 > - hb - YH */
691 Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
692
693 /*--------------Gains--------------*/
694
695 /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
696 VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.f);
698
700 temp = VECT3_DOT_PRODUCT(Ebtemp, I);
701 temp = (_ins->gains.lb / 100.f) * temp;
702
703 VECT3_SMUL(Ebtemp, I, temp);
705 VECT3_COPY(_ins->corr.LE, Evtemp);
706
707 /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
708 _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.f;
709 _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.f;
710 _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
711
712 /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
713 _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.f;
714 _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.f;
715 _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
716
717 /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
718 VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.f);
720
722 temp = VECT3_DOT_PRODUCT(Ebtemp, I);
723 temp = (-_ins->gains.ob / 1000.f) * temp;
724
725 VECT3_SMUL(Ebtemp, I, temp);
727 VECT3_COPY(_ins->corr.OE, Evtemp);
728
729 /* a scalar */
730 /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
731 _ins->corr.RE = ((_ins->gains.rv / 100.f) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.f) * Eh);
732
733 /****** ShEh ******/
734 _ins->corr.SE = (_ins->gains.sh) * Eh;
735
736}
737
738
739void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
740 struct FloatVect3 *vi)
741{
742 struct FloatVect3 qvec, v1, v2;
743 float qi;
744
746 qi = - VECT3_DOT_PRODUCT(*vi, qvec);
748 VECT3_SMUL(v2, *vi, q->qi);
749 VECT3_ADD(v2, v1);
750 QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
751}
#define INV_STATE_DIM
Invariant filter state dimension.
#define INV_COMMAND_DIM
Invariant filter command vector dimension.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
static void ahrs_float_get_quat_from_accel(struct FloatQuat *q, struct FloatVect3 *accel)
Compute a quaternion representing roll and pitch from an accelerometer measurement.
Utility functions for fixed point AHRS implementations.
static int16_t course[3]
int32_t nav_utm_east0
Definition common_nav.c:48
uint8_t nav_utm_zone0
Definition common_nav.c:50
int32_t nav_utm_north0
Definition common_nav.c:49
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition gps.c:577
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
Definition gps.c:544
struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (float) Converted on the fly if not available.
Definition gps.c:509
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:464
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:493
Device independent GPS code (interface)
int32_t hmsl
height above mean sea level (MSL) in mm
Definition gps.h:94
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition gps.h:92
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
uint8_t fix
status of fix
Definition gps.h:107
data structure for GPS information
Definition gps.h:87
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
static void float_quat_normalize(struct FloatQuat *q)
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_VECT3_ZERO(_v)
static float float_vect2_norm(struct FloatVect2 *v)
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
#define FLOAT_QUAT_NORM2(_q)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
#define FLOAT_RATES_ZERO(_r)
void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q)
Quaternion derivative from rotational velocity.
euler angles
Roation quaternion.
angular rates
#define VECT3_SUM(_c, _a, _b)
#define RATES_COPY(_a, _b)
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define VECT3_SMUL(_vo, _vi, _s)
#define VECT3_COPY(_a, _b)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
#define RATES_DIFF(_c, _a, _b)
#define VECT3_DIFF(_c, _a, _b)
#define VECT3_ADD(_a, _b)
#define QUAT_SMUL(_qo, _qi, _s)
#define QUAT_ADD(_qo, _qi)
#define VECT3_DOT_PRODUCT(_v1, _v2)
#define INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
#define NED_FLOAT_OF_BFP(_o, _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.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
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
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition pprz_isa.h:50
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1147
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1167
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1253
struct State state
Definition state.c:36
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float)
Definition state.h:566
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:718
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:519
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition state.h:251
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition state.h:698
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition state.h:264
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int)
Definition state.h:576
static void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition state.h:541
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float)
Definition state.c:204
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int)
Definition state.c:124
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int)
Definition state.h:556
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1346
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:947
Integrated Navigation System interface.
void ins_float_invariant_reset_ref(void)
#define B
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
#define INS_INV_LB
bool ins_gps_fix_once
void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, struct FloatVect3 *vi)
Right multiplication by a quaternion.
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
void ins_float_invariant_init(void)
#define INS_INV_OV
#define INS_INV_LV
#define INS_INV_MVZ
#define INS_INV_NH
static void init_invariant_state(void)
bool ins_baro_initialized
void ins_float_invariant_update_gps(struct GpsState *gps_s)
#define INS_INV_NX
static const struct FloatVect3 A
#define INS_INV_MH
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
#define INS_INV_SH
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
#define INS_INV_OB
#define INS_INV_NXZ
#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
struct InsFloatInv ins_float_inv
void ins_float_invariant_update_baro(float pressure)
#define INS_INV_MV
static void error_output(struct InsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error )
#define MAG_FROZEN_COUNT
void ins_float_invariant_reset_vertical_ref(void)
#define INS_INV_RV
#define INS_INV_RH
INS using invariant filter.
float rv
Tuning parameter of speed error on accel biases.
float mvz
Tuning parameter of vertical speed error on speed.
struct FloatVect3 accel
Input accelerometers.
float nxz
Tuning parameter of vertical position error on position.
float lv
Tuning parameter of speed error on attitude.
struct NedCoor_f speed
Estimates speed.
float mh
Tuning parameter of baro error on vertical speed.
float hb
Estimates barometers bias.
struct FloatRates bias
Estimated gyro biases.
float ob
Tuning parameter of mag error on gyro biases.
float rh
Tuning parameter of baro error on accel biases (vertical projection)
struct inv_gains gains
tuning gains
float OE
Correction gains on magnetometer sensitivity.
float mv
Tuning parameter of horizontal speed error on speed.
struct inv_state state
state vector
struct FloatVect3 LE
Correction gains on attitude.
struct FloatVect3 ME
Correction gains on gyro biases.
struct inv_command cmd
command vector
float as
Estimated accelerometer sensitivity.
struct FloatVect3 mag
Measured magnetic field.
struct NedCoor_f pos
Estimates position.
struct inv_measures meas
measurement vector
struct FloatQuat quat
Estimated attitude (quaternion)
float lb
Tuning parameter of mag error on attitude.
float SE
Correction gains on barometer bias.
float nx
Tuning parameter of horizontal position error on position.
bool reset
flag to request reset/reinit the filter
float NE
Correction gains on accel bias.
struct FloatRates rates
Input gyro rates.
float RE
Correction gains on accel bias.
struct NedCoor_f pos_gps
Measured gps position.
float baro_alt
Measured barometric altitude.
float ov
Tuning parameter of speed error on gyro biases.
struct NedCoor_f speed_gps
Measured gps speed.
struct inv_correction_gains corr
correction gains
float nh
Tuning parameter of baro error on vertical position.
float sh
Tuning parameter of baro error on baro bias.
Invariant filter structure.
Invariant filter command vector.
Invariant filter state.
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
bool log_started
Fixedwing Navigation library.
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
float z
in meters
float x
in meters
float east
in meters
float north
in meters
float y
in meters
vector in EarthCenteredEarthFixed coordinates
position in UTM coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
Runge-Kutta library (float version)
static void runge_kutta_4_float(float *xo, const float *x, const int n, const float *u, const int m, void(*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt)
Fourth-Order Runge-Kutta.
FileDes pprzLogFile
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
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
float alpha
Definition textons.c:133
static float Ex[3]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.