Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
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 */
172static inline void init_invariant_state(void)
173{
174 // init state
178 FLOAT_VECT3_ZERO(ins_float_inv.state.speed);
179 ins_float_inv.state.as = 1.0f;
180 ins_float_inv.state.hb = 0.0f;
181
182 // init measures
183 FLOAT_VECT3_ZERO(ins_float_inv.meas.pos_gps);
184 FLOAT_VECT3_ZERO(ins_float_inv.meas.speed_gps);
185 ins_float_inv.meas.baro_alt = 0.0f;
186
187 // init baro
188 ins_baro_initialized = false;
189 ins_gps_fix_once = false;
190}
191
192#if SEND_INVARIANT_FILTER || PERIODIC_TELEMETRY
193static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
194{
195 struct FloatEulers eulers;
196 float_eulers_of_quat(&eulers, &ins_float_inv.state.quat);
197 pprz_msg_send_INV_FILTER(trans, dev,
198 AC_ID,
199 &ins_float_inv.state.quat.qi,
200 &eulers.phi,
201 &eulers.theta,
202 &eulers.psi,
203 &ins_float_inv.state.speed.x,
204 &ins_float_inv.state.speed.y,
205 &ins_float_inv.state.speed.z,
206 &ins_float_inv.state.pos.x,
207 &ins_float_inv.state.pos.y,
208 &ins_float_inv.state.pos.z,
209 &ins_float_inv.state.bias.p,
210 &ins_float_inv.state.bias.q,
211 &ins_float_inv.state.bias.r,
212 &ins_float_inv.state.as,
213 &ins_float_inv.state.hb,
214 &ins_float_inv.meas.baro_alt,
215 &ins_float_inv.meas.pos_gps.z);
216}
217#endif
218
220{
221
222 // init position
223#if FIXEDWING_FIRMWARE
224 struct UtmCoor_f utm0;
225 utm0.north = (float)nav_utm_north0;
226 utm0.east = (float)nav_utm_east0;
227 utm0.alt = GROUND_ALT;
228 utm0.zone = nav_utm_zone0;
229 stateSetLocalUtmOrigin_f(MODULE_INS_FLOAT_INVARIANT_ID, &utm0);
230 stateSetPositionUtm_f(MODULE_INS_FLOAT_INVARIANT_ID, &utm0);
231#else
232 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
233 llh_nav0.lat = NAV_LAT0;
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;
238 ecef_of_lla_i(&ecef_nav0, &llh_nav0);
239 struct LtpDef_i ltp_def;
240 ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
241 ltp_def.hmsl = NAV_ALT0;
242 stateSetLocalOrigin_i(MODULE_INS_FLOAT_INVARIANT_ID, &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
259 ins_float_inv.gains.lv = INS_INV_LV;
260 ins_float_inv.gains.lb = INS_INV_LB;
261 ins_float_inv.gains.mv = INS_INV_MV;
262 ins_float_inv.gains.mvz = INS_INV_MVZ;
263 ins_float_inv.gains.mh = INS_INV_MH;
264 ins_float_inv.gains.nx = INS_INV_NX;
265 ins_float_inv.gains.nxz = INS_INV_NXZ;
266 ins_float_inv.gains.nh = INS_INV_NH;
267 ins_float_inv.gains.ov = INS_INV_OV;
268 ins_float_inv.gains.ob = INS_INV_OB;
269 ins_float_inv.gains.rv = INS_INV_RV;
270 ins_float_inv.gains.rh = INS_INV_RH;
271 ins_float_inv.gains.sh = INS_INV_SH;
272
273 ins_float_inv.is_aligned = false;
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
286 stateSetLocalUtmOrigin_f(MODULE_INS_FLOAT_INVARIANT_ID, &utm);
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;
293 ltp_def.hmsl = gps.hmsl;
294 stateSetLocalOrigin_i(MODULE_INS_FLOAT_INVARIANT_ID, &ltp_def);
295#endif
296 // reset state position and velocity to zero
298 FLOAT_VECT3_ZERO(ins_float_inv.state.speed);
299}
300
302{
303#if FIXEDWING_FIRMWARE
304 struct UtmCoor_f utm = *stateGetUtmOrigin_f();
305 utm.alt = gps.hmsl / 1000.0f;
306 stateSetLocalUtmOrigin_f(MODULE_INS_FLOAT_INVARIANT_ID, &utm);
307#else
308 struct LlaCoor_i lla = {
309 .lat = stateGetLlaOrigin_i().lat,
310 .lon = stateGetLlaOrigin_i().lon,
311 .alt = gps.lla_pos.alt
312 };
313 struct LtpDef_i ltp_def;
315 ltp_def.hmsl = gps.hmsl;
316 stateSetLocalOrigin_i(MODULE_INS_FLOAT_INVARIANT_ID, &ltp_def);
317#endif
318 // reset vertical position and velocity to zero
319 ins_float_inv.state.pos.z = 0.f;
320 ins_float_inv.state.speed.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 */
329 ahrs_float_get_quat_from_accel_mag(&ins_float_inv.state.quat, lp_accel, lp_mag);
330#else
331 /* Compute an initial orientation from accel only directly as quaternion */
332 ahrs_float_get_quat_from_accel(&ins_float_inv.state.quat, lp_accel);
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 */
339 stateSetNedToBodyQuat_f(MODULE_INS_FLOAT_INVARIANT_ID, &ins_float_inv.state.quat);
340
341 // ins and ahrs are now running
342 ins_float_inv.is_aligned = true;
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;
353 ins_float_inv.is_aligned = false;
355 }
356
357 // fill command vector in body frame
358 RATES_COPY(ins_float_inv.cmd.rates, *gyro);
359 VECT3_COPY(ins_float_inv.cmd.accel, *accel);
360
361 struct Int32Vect3 body_accel_i;
362 ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
363 stateSetAccelBody_i(MODULE_INS_FLOAT_INVARIANT_ID, &body_accel_i);
364
365 // update correction gains
367
368 // propagate model
369 struct inv_state new_state;
370 runge_kutta_4_float((float *)&new_state,
371 (float *)&ins_float_inv.state, INV_STATE_DIM,
372 (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
373 invariant_model, dt);
374 ins_float_inv.state = new_state;
375
376 // normalize quaternion
378
379 // set global state
380 stateSetNedToBodyQuat_f(MODULE_INS_FLOAT_INVARIANT_ID, &ins_float_inv.state.quat);
381 RATES_DIFF(body_rates, ins_float_inv.cmd.rates, ins_float_inv.state.bias);
382 stateSetBodyRates_f(MODULE_INS_FLOAT_INVARIANT_ID, &body_rates);
383 stateSetPositionNed_f(MODULE_INS_FLOAT_INVARIANT_ID, &ins_float_inv.state.pos);
384 stateSetSpeedNed_f(MODULE_INS_FLOAT_INVARIANT_ID, &ins_float_inv.state.speed);
385 // untilt accel and remove gravity
386 struct FloatQuat q_b2n;
387 float_quat_invert(&q_b2n, &ins_float_inv.state.quat);
388 struct FloatVect3 accel_n;
389 float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
390 VECT3_SMUL(accel_n, accel_n, 1.f / (ins_float_inv.state.as));
391 VECT3_ADD(accel_n, A);
392 struct NedCoor_f accel_coord;
393 VECT3_COPY(accel_coord, accel_n);
394 stateSetAccelNed_f(MODULE_INS_FLOAT_INVARIANT_ID, &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",
412 ins_float_inv.cmd.rates.p,
413 ins_float_inv.cmd.rates.q,
414 ins_float_inv.cmd.rates.r,
415 ins_float_inv.cmd.accel.x,
416 ins_float_inv.cmd.accel.y,
417 ins_float_inv.cmd.accel.z,
418 ins_float_inv.meas.pos_gps.x,
419 ins_float_inv.meas.pos_gps.y,
420 ins_float_inv.meas.pos_gps.z,
421 ins_float_inv.meas.speed_gps.x,
422 ins_float_inv.meas.speed_gps.y,
423 ins_float_inv.meas.speed_gps.z,
424 ins_float_inv.meas.mag.x,
425 ins_float_inv.meas.mag.y,
426 ins_float_inv.meas.mag.z,
427 ins_float_inv.meas.baro_alt,
428 ins_float_inv.state.quat.qi,
429 ins_float_inv.state.quat.qx,
430 ins_float_inv.state.quat.qy,
431 ins_float_inv.state.quat.qz,
432 ins_float_inv.state.bias.p,
433 ins_float_inv.state.bias.q,
434 ins_float_inv.state.bias.r,
435 ins_float_inv.state.speed.x,
436 ins_float_inv.state.speed.y,
437 ins_float_inv.state.speed.z,
438 ins_float_inv.state.pos.x,
439 ins_float_inv.state.pos.y,
440 ins_float_inv.state.pos.z,
441 ins_float_inv.state.hb,
442 ins_float_inv.state.as);
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
453 if (state.utm_initialized_f) {
454 struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
455 // position (local ned)
456 ins_float_inv.meas.pos_gps.x = utm.north - stateGetUtmOrigin_f()->north;
457 ins_float_inv.meas.pos_gps.y = utm.east - stateGetUtmOrigin_f()->east;
458 ins_float_inv.meas.pos_gps.z = stateGetHmslOrigin_f() - utm.alt;
459 // speed
460 ins_float_inv.meas.speed_gps = ned_vel_float_from_gps(gps_s);
461 }
462#else
463 if (state.ned_initialized_f) {
464 // position
465 struct NedCoor_i gps_pos_cm_ned, ned_pos;
466 struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
467 ned_of_ecef_point_i(&gps_pos_cm_ned, stateGetNedOrigin_i(), &ecef_pos_i);
469 NED_FLOAT_OF_BFP(ins_float_inv.meas.pos_gps, ned_pos);
470 // speed
471 struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
472 ned_of_ecef_vect_f(&ins_float_inv.meas.speed_gps, stateGetNedOrigin_f(), &ecef_vel);
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) {
477 ins_float_inv.state.pos = ins_float_inv.meas.pos_gps;
478 ins_float_inv.state.speed = ins_float_inv.meas.speed_gps;
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
489 static const uint16_t gps_min_speed = INS_INV_HEADING_UPDATE_GPS_MIN_SPEED * 100;
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;
496 pseudo_mag.x = cosf(course);
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
502 struct FloatVect2 vel = { ins_float_inv.meas.speed_gps.x, ins_float_inv.meas.speed_gps.y };
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;
514 float_eulers_of_quat(&angles, &ins_float_inv.state.quat);
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;
518 float_quat_inv_comp_norm_shortest(&quat_horiz, &quat_psi, &ins_float_inv.state.quat);
519 // rotate pseudo mag around horiz quaternion
520 struct FloatVect3 mag_final;
521 float_quat_vmult(&mag_final, &quat_horiz, &pseudo_mag);
523 } else {
524 // if speed is tool low, better set measurements to zero
526 }
527#endif
528
529}
530
531
533{
534 static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
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);
549 baro_prev = baro_moy;
550 // test stop condition
551 if (fabs(alpha) < 0.1f) {
552 ins_qfe = baro_moy;
554 }
555 if (i == 250) {
556 ins_qfe = pressure;
558 }
559 i++;
560 } else { /* normal update with baro measurement */
561 ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
562 }
563}
564
565// assume mag is dead when values are not moving anymore
566#define MAG_FROZEN_COUNT 30
567
569{
570 static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
571 static int32_t last_mx = 0;
572
573 if (last_mx == mag->x) {
574 mag_frozen_count--;
575 if (mag_frozen_count == 0) {
576 // if mag is dead, better set measurements to zero
578 mag_frozen_count = MAG_FROZEN_COUNT;
579 }
580 } else {
581 // new values in body frame
582 VECT3_COPY(ins_float_inv.meas.mag, *mag);
583 // reset counter
584 mag_frozen_count = MAG_FROZEN_COUNT;
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;
604 struct FloatRates rates_unbiased;
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
613 float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
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));
623 float_quat_vmult(&tmp_vect, &q_b2n, &(c->accel));
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);
627 VECT3_ADD(s_dot.speed, ins_float_inv.corr.ME);
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 */
633 float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
634 RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
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 */
644 s_dot.hb = ins_float_inv.corr.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
676#if FIXEDWING_FIRMWARE
677 && state.utm_initialized_f
678#else
679 && state.ned_initialized_f
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);
697 VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
698
699 VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
700 temp = VECT3_DOT_PRODUCT(Ebtemp, I);
701 temp = (_ins->gains.lb / 100.f) * temp;
702
703 VECT3_SMUL(Ebtemp, I, temp);
704 VECT3_ADD(Evtemp, Ebtemp);
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);
719 VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
720
721 VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
722 temp = VECT3_DOT_PRODUCT(Ebtemp, I);
723 temp = (-_ins->gains.ob / 1000.f) * temp;
724
725 VECT3_SMUL(Ebtemp, I, temp);
726 VECT3_ADD(Evtemp, Ebtemp);
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
745 FLOAT_QUAT_EXTRACT(qvec, *q);
746 qi = - VECT3_DOT_PRODUCT(*vi, qvec);
747 VECT3_CROSS_PRODUCT(v1, *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 B
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
#define MAG_FROZEN_COUNT
#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
static struct uart_periph * dev
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:591
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:558
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:523
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_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).
uint32_t cacc
course accuracy in rad*1e7
Definition gps.h:107
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north).
Definition gps.h:102
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition gps.h:100
#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
float phi
in radians
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
#define FLOAT_QUAT_EXTRACT(_vo, _qi)
#define FLOAT_VECT3_ZERO(_v)
static void float_quat_invert(struct FloatQuat *qo, const struct FloatQuat *qi)
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
static float float_vect2_norm(const struct FloatVect2 *v)
#define FLOAT_QUAT_NORM2(_q)
#define FLOAT_RATES_ZERO(_r)
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_derivative(struct FloatQuat *qd, const struct FloatRates *r, const struct FloatQuat *q)
Quaternion derivative from rotational velocity.
void float_quat_vmult(struct FloatVect3 *v_out, const struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
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 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
#define A
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:1166
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1186
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
struct State state
Definition state.c:36
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
Definition state.h:577
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:729
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
static void stateSetPositionUtm_f(uint16_t id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition state.h:709
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int).
Definition state.h:587
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:552
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
Definition state.h:567
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:958
Integrated Navigation System interface.
void ins_float_invariant_reset_ref(void)
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
#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 ).
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.
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
static FILE * pprzLogFile
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 alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float z
in meters
float east
in meters
float north
in meters
vector in EarthCenteredEarthFixed coordinates
vector in North East Down coordinates Units: meters
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.
SdioError sdLogWriteLog(const FileDes fd, const char *fmt,...)
API to get/set the generic vehicle states.
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.