Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
43 #include "math/pprz_algebra_int.h"
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
56 bool 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 */
140 static 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 */
158 static inline void error_output(struct InsFloatInv *_ins);
159 
160 /* propagation model (called by runge-kutta library) */
161 static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m);
162 
163 
167 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
168  struct FloatVect3 *vi);
169 
170 
171 /* init state and measurements */
172 static inline void init_invariant_state(void)
173 {
174  // init state
179  ins_float_inv.state.as = 1.0f;
180  ins_float_inv.state.hb = 0.0f;
181 
182  // init measures
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
193 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
194 {
195  struct FloatEulers eulers;
197  pprz_msg_send_INV_FILTER(trans, dev,
198  AC_ID,
200  &eulers.phi,
201  &eulers.theta,
202  &eulers.psi,
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;
230  stateSetPositionUtm_f(&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;
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 
273  ins_float_inv.is_aligned = false;
274  ins_float_inv.reset = false;
275 
276 #if PERIODIC_TELEMETRY
278 #endif
279 }
280 
281 
283 {
284 #if FIXEDWING_FIRMWARE
285  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
286  // reset state UTM ref
288 #else
289  struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
290  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
291  struct LtpDef_i ltp_def;
292  ltp_def_from_ecef_i(&ltp_def, &ecef_pos);
293  ltp_def.lla.alt = lla_pos.alt;
294  ltp_def.hmsl = gps.hmsl;
296 #endif
297  // reset state position and velocity to zero
300 }
301 
303 {
304 #if FIXEDWING_FIRMWARE
305  struct UtmCoor_f utm = state.utm_origin_f;
306  utm.alt = gps.hmsl / 1000.0f;
308 #else
309  struct LlaCoor_i lla = {
311  .lon = state.ned_origin_i.lla.lon,
312  .alt = gps.lla_pos.alt
313  };
314  struct LtpDef_i ltp_def;
316  ltp_def.hmsl = gps.hmsl;
318 #endif
319  // reset vertical position and velocity to zero
320  ins_float_inv.state.pos.z = 0.f;
321  ins_float_inv.state.speed.z = 0.f;
322 }
323 
325  struct FloatVect3 *lp_accel,
326  struct FloatVect3 *lp_mag __attribute__((unused)))
327 {
328 #if USE_MAGNETOMETER
329  /* Compute an initial orientation from accel and mag directly as quaternion */
331 #else
332  /* Compute an initial orientation from accel only directly as quaternion */
334 #endif
335 
336  /* use average gyro as initial value for bias */
337  ins_float_inv.state.bias = *lp_gyro;
338 
339  /* push initial values to state interface */
341 
342  // ins and ahrs are now running
343  ins_float_inv.is_aligned = true;
344 }
345 
346 void ins_float_invariant_propagate(struct FloatRates* gyro, struct FloatVect3* accel, float dt)
347 {
348  struct FloatRates body_rates;
349 
350  // realign all the filter if needed
351  // a complete init cycle is required
352  if (ins_float_inv.reset) {
353  ins_float_inv.reset = false;
354  ins_float_inv.is_aligned = false;
356  }
357 
358  // fill command vector in body frame
361 
362  struct Int32Vect3 body_accel_i;
363  ACCELS_BFP_OF_REAL(body_accel_i, ins_float_inv.cmd.accel);
364  stateSetAccelBody_i(&body_accel_i);
365 
366  // update correction gains
368 
369  // propagate model
370  struct inv_state new_state;
371  runge_kutta_4_float((float *)&new_state,
372  (float *)&ins_float_inv.state, INV_STATE_DIM,
373  (float *)&ins_float_inv.cmd, INV_COMMAND_DIM,
374  invariant_model, dt);
375  ins_float_inv.state = new_state;
376 
377  // normalize quaternion
379 
380  // set global state
383  stateSetBodyRates_f(&body_rates);
386  // untilt accel and remove gravity
387  struct FloatQuat q_b2n;
389  struct FloatVect3 accel_n;
390  float_quat_vmult(&accel_n, &q_b2n, &ins_float_inv.cmd.accel);
391  VECT3_SMUL(accel_n, accel_n, 1.f / (ins_float_inv.state.as));
392  VECT3_ADD(accel_n, A);
393  stateSetAccelNed_f((struct NedCoor_f *)&accel_n);
394 
395  //------------------------------------------------------------//
396 
397 #if SEND_INVARIANT_FILTER
398  RunOnceEvery(3, send_inv_filter(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
399 #endif
400 
401 #if LOG_INVARIANT_FILTER
402  if (pprzLogFile != -1) {
403  if (!log_started) {
404  // log file header
405  sdLogWriteLog(pprzLogFile,
406  "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");
407  log_started = true;
408  } else {
409  sdLogWriteLog(pprzLogFile,
410  "%.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",
442  }
443  }
444 #endif
445 }
446 
448 {
449 
450  if (gps_s->fix >= GPS_FIX_3D && ins_float_inv.is_aligned) {
451 #if FIXEDWING_FIRMWARE
452  if (state.utm_initialized_f) {
453  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
454  // position (local ned)
458  // speed
460  }
461 #else
462  if (state.ned_initialized_f) {
463  // position
464  struct NedCoor_i gps_pos_cm_ned, ned_pos;
465  struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
466  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &ecef_pos_i);
469  // speed
470  struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
472  }
473 #endif
474  // when getting first GPS pos (or after reset), set state to current pos and speed
475  if (!ins_gps_fix_once) {
478  ins_gps_fix_once = true;
479  }
480  }
481 
482 #if !USE_MAGNETOMETER
483  struct FloatVect3 pseudo_mag = { 0.f, 0.f, 0.f };
484  bool update_mag = false;
485 #if INS_INV_USE_GPS_HEADING
486  // got a 3d fix, ground speed > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED (default 5.0 m/s)
487  // and course accuracy is better than 10deg
488  static const uint16_t gps_min_speed = INS_INV_HEADING_UPDATE_GPS_MIN_SPEED * 100;
489  static const uint32_t max_cacc = RadOfDeg(10 * 1e7);
490  if (gps_s->fix >= GPS_FIX_3D &&
491  gps_s->gspeed >= gps_min_speed &&
492  gps_s->cacc <= max_cacc) {
493  // gps_s->course is in rad * 1e7
494  float course = (float)gps_s->course / 1e7f;
495  pseudo_mag.x = cosf(course);
496  pseudo_mag.y = -sinf(course);
497  update_mag = true;
498  }
499 #else // else use GPS velocity (only for fixedwing)
500  // Use pseudo-mag rebuilt from GPS horizontal velocity
502  float vel_norm = float_vect2_norm(&vel);
503  if (vel_norm > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED) {
504  pseudo_mag.x = vel.x / vel_norm;
505  pseudo_mag.y = -vel.y / vel_norm;
506  update_mag = true;
507  }
508 #endif
509  if (update_mag) {
510  // create quat corresponding to psi
511  // quat_psi = [ cos(psi/2) 0 0 sin(psi/2) ]'
512  struct FloatEulers angles;
514  struct FloatQuat quat_psi = { cosf(angles.psi / 2.f), 0.f, 0.f, sinf(angles.psi / 2.f) };
515  // build quat corresponding to roll and pitch (from quat psi and current quat)
516  struct FloatQuat quat_horiz;
517  float_quat_inv_comp_norm_shortest(&quat_horiz, &quat_psi, &ins_float_inv.state.quat);
518  // rotate pseudo mag around horiz quaternion
519  struct FloatVect3 mag_final;
520  float_quat_vmult(&mag_final, &quat_horiz, &pseudo_mag);
521  ins_float_invariant_update_mag(&mag_final);
522  } else {
523  // if speed is tool low, better set measurements to zero
525  }
526 #endif
527 
528 }
529 
530 
532 {
533  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
534  static float alpha = 10.0f;
535  static int32_t i = 1;
536  static float baro_moy = 0.0f;
537  static float baro_prev = 0.0f;
538 
539  if (!ins_baro_initialized) {
540  // try to find a stable qfe
541  // TODO generic function in pprz_isa ?
542  if (i == 1) {
543  baro_moy = pressure;
544  baro_prev = pressure;
545  }
546  baro_moy = (baro_moy * (i - 1) + pressure) / i;
547  alpha = (10.f*alpha + (baro_moy - baro_prev)) / (11.0f);
548  baro_prev = baro_moy;
549  // test stop condition
550  if (fabs(alpha) < 0.1f) {
551  ins_qfe = baro_moy;
552  ins_baro_initialized = true;
553  }
554  if (i == 250) {
555  ins_qfe = pressure;
556  ins_baro_initialized = true;
557  }
558  i++;
559  } else { /* normal update with baro measurement */
560  ins_float_inv.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
561  }
562 }
563 
564 // assume mag is dead when values are not moving anymore
565 #define MAG_FROZEN_COUNT 30
566 
568 {
569  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
570  static int32_t last_mx = 0;
571 
572  if (last_mx == mag->x) {
573  mag_frozen_count--;
574  if (mag_frozen_count == 0) {
575  // if mag is dead, better set measurements to zero
577  mag_frozen_count = MAG_FROZEN_COUNT;
578  }
579  } else {
580  // new values in body frame
582  // reset counter
583  mag_frozen_count = MAG_FROZEN_COUNT;
584  }
585  last_mx = mag->x;
586 }
587 
588 
593 static inline void invariant_model(float *o, const float *x, const int n, const float *u,
594  const int m __attribute__((unused)))
595 {
596 
597 #pragma GCC diagnostic push // require GCC 4.6
598 #pragma GCC diagnostic ignored "-Wcast-qual"
599  struct inv_state *s = (struct inv_state *)x;
600  struct inv_command *c = (struct inv_command *)u;
601 #pragma GCC diagnostic pop // require GCC 4.6
602  struct inv_state s_dot;
603  struct FloatRates rates_unbiased;
604  struct FloatVect3 tmp_vect;
605  struct FloatQuat tmp_quat;
606 
607  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
608  RATES_DIFF(rates_unbiased, c->rates, s->bias);
609  /* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
610  float_quat_derivative(&s_dot.quat, &rates_unbiased, &(s->quat));
611 
612  float_quat_vmul_right(&tmp_quat, &(s->quat), &ins_float_inv.corr.LE);
613  QUAT_ADD(s_dot.quat, tmp_quat);
614 
615  float norm2_r = 1.f - FLOAT_QUAT_NORM2(s->quat);
616  QUAT_SMUL(tmp_quat, s->quat, norm2_r);
617  QUAT_ADD(s_dot.quat, tmp_quat);
618 
619  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
620  struct FloatQuat q_b2n;
621  float_quat_invert(&q_b2n, &(s->quat));
622  float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel));
623  VECT3_SMUL(s_dot.speed, s_dot.speed, 1.f / (s->as));
624  VECT3_ADD(s_dot.speed, A);
626 
627  /* dot_X = V + NE */
628  VECT3_SUM(s_dot.pos, s->speed, ins_float_inv.corr.NE);
629 
630  /* bias_dot = q-1 * (OE) * q */
631  float_quat_vmult(&tmp_vect, &(s->quat), &ins_float_inv.corr.OE);
632  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
633 
634  /* as_dot = as * RE */
635  s_dot.as = (s->as) * (ins_float_inv.corr.RE);
636  // keep as in a reasonable range, so 50% around the nominal value
637  if ( ((s->as < 0.5f) && (s_dot.as < 0.f)) || ((s->as > 1.5f) && (s_dot.as > 0.f)) ) {
638  s_dot.as = 0.f;
639  }
640 
641  /* hb_dot = SE */
642  s_dot.hb = ins_float_inv.corr.SE;
643 
644  // set output
645  memcpy(o, &s_dot, n * sizeof(float));
646 }
647 
652 static inline void error_output(struct InsFloatInv *_ins)
653 {
654 
655  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
656  float Eh;
657  float temp;
658 
659  /* YBt = q * yB * q-1 */
660  struct FloatQuat q_b2n;
661  float_quat_invert(&q_b2n, &(_ins->state.quat));
662  float_quat_vmult(&YBt, &q_b2n, &(_ins->meas.mag));
663 
664  float_quat_vmult(&I, &q_b2n, &(_ins->cmd.accel));
665  VECT3_SMUL(I, I, 1.f / (_ins->state.as));
666 
667  /*--------- E = ( ŷ - y ) ----------*/
668  /* Eb = ( B - YBt ) */
669  VECT3_DIFF(Eb, B, YBt);
670 
671  // pos and speed error only if GPS data are valid
672  // or while waiting first GPS data to prevent diverging
673  if ((gps.fix >= GPS_FIX_3D && _ins->is_aligned
674 #if FIXEDWING_FIRMWARE
676 #else
678 #endif
679  ) || !ins_gps_fix_once) {
680  /* Ev = (V - YV) */
681  VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
682  /* Ex = (X - YX) */
683  VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
684  } else {
685  FLOAT_VECT3_ZERO(Ev);
687  }
688  /* Eh = < X,e3 > - hb - YH */
689  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
690 
691  /*--------------Gains--------------*/
692 
693  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
694  VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.f);
695  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
696 
697  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
698  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
699  temp = (_ins->gains.lb / 100.f) * temp;
700 
701  VECT3_SMUL(Ebtemp, I, temp);
702  VECT3_ADD(Evtemp, Ebtemp);
703  VECT3_COPY(_ins->corr.LE, Evtemp);
704 
705  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
706  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.f;
707  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.f;
708  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
709 
710  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
711  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.f;
712  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.f;
713  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
714 
715  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
716  VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.f);
717  VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
718 
719  VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
720  temp = VECT3_DOT_PRODUCT(Ebtemp, I);
721  temp = (-_ins->gains.ob / 1000.f) * temp;
722 
723  VECT3_SMUL(Ebtemp, I, temp);
724  VECT3_ADD(Evtemp, Ebtemp);
725  VECT3_COPY(_ins->corr.OE, Evtemp);
726 
727  /* a scalar */
728  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
729  _ins->corr.RE = ((_ins->gains.rv / 100.f) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.f) * Eh);
730 
731  /****** ShEh ******/
732  _ins->corr.SE = (_ins->gains.sh) * Eh;
733 
734 }
735 
736 
737 void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q,
738  struct FloatVect3 *vi)
739 {
740  struct FloatVect3 qvec, v1, v2;
741  float qi;
742 
743  FLOAT_QUAT_EXTRACT(qvec, *q);
744  qi = - VECT3_DOT_PRODUCT(*vi, qvec);
745  VECT3_CROSS_PRODUCT(v1, *vi, qvec);
746  VECT3_SMUL(v2, *vi, q->qi);
747  VECT3_ADD(v2, v1);
748  QUAT_ASSIGN(*mright, qi, v2.x, v2.y, v2.z);
749 }
#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]
Definition: airspeed_uADC.c:58
int32_t nav_utm_east0
Definition: common_nav.c:43
uint8_t nav_utm_zone0
Definition: common_nav.c:45
int32_t nav_utm_north0
Definition: common_nav.c:44
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:550
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:517
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:482
struct GpsState gps
global GPS state
Definition: gps.c:69
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:437
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:466
Device independent GPS code (interface)
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:90
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:102
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:97
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
uint8_t fix
status of fix
Definition: gps.h:105
data structure for GPS information
Definition: gps.h:85
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)
Definition: pprz_algebra.h:161
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:330
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
Definition: pprz_algebra.h:244
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
#define QUAT_ASSIGN(_q, _i, _x, _y, _z)
Definition: pprz_algebra.h:580
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:611
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:619
#define VECT3_DOT_PRODUCT(_v1, _v2)
Definition: pprz_algebra.h:250
#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(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
struct State state
Definition: state.c:36
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
Integrated Navigation System interface.
#define B
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
void ins_reset_local_origin(void)
INS local origin reset.
#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_reset_altitude_ref(void)
INS altitude reference reset.
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
#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
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 x
in meters
float east
in meters
float north
in meters
float y
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.
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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:132
static float I
Definition: trilateration.c:35
static float Ex[3]
Definition: trilateration.c:33
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78