Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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  */
21 
22 
34 
37 #include "subsystems/ahrs.h"
38 
39 #include "subsystems/ins.h"
40 #include "subsystems/gps.h"
41 #include "subsystems/imu.h"
42 
43 #include "generated/airframe.h"
44 #include "generated/flight_plan.h"
45 #if INS_UPDATE_FW_ESTIMATOR
47 #endif
48 
50 #include "math/pprz_algebra_int.h"
51 #include "math/pprz_rk_float.h"
52 #include "math/pprz_isa.h"
53 
54 #include "subsystems/abi.h"
55 #include "state.h"
56 
57 #include "led.h"
58 
59 #include "mcu_periph/uart.h"
60 #include "messages.h"
62 
63 #if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
65 static void send_ins_ref(void) {
66  float foo = 0.;
68  DOWNLINK_SEND_INS_REF(DefaultChannel, DefaultDevice,
71  &state.ned_origin_i.hmsl, &foo);
72  }
73 }
74 #endif
75 
76 
77 #if LOG_INVARIANT_FILTER
78 #include "sdLog.h"
79 #include "subsystems/chibios-libopencm3/chibios_sdlog.h"
80 bool_t log_started = FALSE;
81 #endif
82 
83 /*------------- =*= Invariant Observers =*= -------------*
84  *
85  * State vector :
86  *
87  * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
88  *
89  * Dynamic model (dim = 15) :
90  *
91  * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
92  * x_Vdot = A + 1/as (q * am * (q)-1);
93  * x_Xdot = V;
94  * x_bias_dot = 0;
95  * x_asdot = 0;
96  * x_hbdot = 0;
97  *
98  * Observation model (dim = 10):
99  * yv = V;
100  * yx = X;
101  * yh = <X,e3> - hb;
102  * yb = (q)-1 *B * q; (B : magnetometers)
103  *
104  *------------------------------------------------------*/
105 
106 // Default values for the tuning gains
107 // Tuning parameter of speed error on attitude (e-2)
108 #ifndef INS_INV_LV
109 #define INS_INV_LV 2.
110 #endif
111 // Tuning parameter of mag error on attitude (e-2)
112 #ifndef INS_INV_LB
113 #define INS_INV_LB 6.
114 #endif
115 // Tuning parameter of horizontal speed error on speed
116 #ifndef INS_INV_MV
117 #define INS_INV_MV 8.
118 #endif
119 // Tuning parameter of vertical speed error on speed
120 #ifndef INS_INV_MVZ
121 #define INS_INV_MVZ 15.
122 #endif
123 // Tuning parameter of baro error on vertical speed
124 #ifndef INS_INV_MH
125 #define INS_INV_MH 0.2
126 #endif
127 // Tuning parameter of horizontal position error on position
128 #ifndef INS_INV_NX
129 #define INS_INV_NX 0.8
130 #endif
131 // Tuning parameter of vertical position error on position
132 #ifndef INS_INV_NXZ
133 #define INS_INV_NXZ 0.5
134 #endif
135 // Tuning parameter of baro error on vertical position
136 #ifndef INS_INV_NH
137 #define INS_INV_NH 1.2
138 #endif
139 // Tuning parameter of speed error on gyro biases (e-3)
140 #ifndef INS_INV_OV
141 #define INS_INV_OV 1.2
142 #endif
143 // Tuning parameter of mag error on gyro biases (e-3)
144 #ifndef INS_INV_OB
145 #define INS_INV_OB 1.
146 #endif
147 // Tuning parameter of speed error on accel biases (e-2)
148 #ifndef INS_INV_RV
149 #define INS_INV_RV 4.
150 #endif
151 // Tuning parameter of baro error on accel biases (vertical projection) (e-8)
152 #ifndef INS_INV_RH
153 #define INS_INV_RH 8.
154 #endif
155 // Tuning parameter of baro error on baro bias
156 #ifndef INS_INV_SH
157 #define INS_INV_SH 0.01
158 #endif
159 
160 
161 // FIXME this is still needed for fixedwing integration
162 #if INS_UPDATE_FW_ESTIMATOR
163 // remotely settable
164 #ifndef INS_ROLL_NEUTRAL_DEFAULT
165 #define INS_ROLL_NEUTRAL_DEFAULT 0.
166 #endif
167 #ifndef INS_PITCH_NEUTRAL_DEFAULT
168 #define INS_PITCH_NEUTRAL_DEFAULT 0.
169 #endif
172 #endif
173 
175 
176 /* integration time step */
177 static const float dt = (1./ ((float)AHRS_PROPAGATE_FREQUENCY));
178 
179 /* earth gravity model */
180 static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
181 
182 /* earth magnetic model */
183 static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
184 
185 /* barometer */
187 // Baro event on ABI
188 #ifndef INS_BARO_ID
189 #if USE_BARO_BOARD
190 #define INS_BARO_ID BARO_BOARD_SENDER_ID
191 #else
192 #define INS_BARO_ID ABI_BROADCAST
193 #endif
194 #endif
197 static void baro_cb(uint8_t sender_id, const float *pressure);
198 
199 /* gps */
201 
202 /* error computation */
203 static inline void error_output(struct InsFloatInv * _ins);
204 
205 /* propagation model (called by runge-kutta library) */
206 static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m);
207 
208 /* init state and measurements */
209 static inline void init_invariant_state(void) {
210  // init state
215  ins_impl.state.as = 1.0f;
216  ins_impl.state.hb = 0.0f;
217 
218  // init measures
221  ins_impl.meas.baro_alt = 0.0f;
222 
223  // init baro
224  ins_baro_initialized = FALSE;
225  ins_gps_fix_once = FALSE;
226 }
227 
228 void ins_init() {
229 
230  // init position
231 #if INS_UPDATE_FW_ESTIMATOR
232  struct UtmCoor_f utm0;
233  utm0.north = (float)nav_utm_north0;
234  utm0.east = (float)nav_utm_east0;
235  utm0.alt = GROUND_ALT;
236  utm0.zone = nav_utm_zone0;
238  stateSetPositionUtm_f(&utm0);
239 #else
240  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
241  llh_nav0.lat = NAV_LAT0;
242  llh_nav0.lon = NAV_LON0;
243  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
244  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
245  struct EcefCoor_i ecef_nav0;
246  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
247  struct LtpDef_i ltp_def;
248  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
249  ltp_def.hmsl = NAV_ALT0;
250  stateSetLocalOrigin_i(&ltp_def);
251 #endif
252 
253  // Bind to BARO_ABS message
254  AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
255 
256  // init state and measurements
258 
259  // init gains
273 
275  ins_impl.reset = FALSE;
276 
277 #if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY
278  register_periodic_telemetry(DefaultPeriodic, "INS_REF", send_ins_ref);
279 #endif
280 }
281 
282 
284 #if INS_UPDATE_FW_ESTIMATOR
285  struct UtmCoor_f utm;
286 #ifdef GPS_USE_LATLONG
287  /* Recompute UTM coordinates in this zone */
288  struct LlaCoor_f lla;
290  utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
291  utm_of_lla_f(&utm, &lla);
292 #else
293  utm.zone = gps.utm_pos.zone;
294  utm.east = gps.utm_pos.east / 100.0f;
295  utm.north = gps.utm_pos.north / 100.0f;
296 #endif
297  // ground_alt
298  utm.alt = gps.hmsl / 1000.0f;
299  // reset state UTM ref
301 #else
302  struct LtpDef_i ltp_def;
303  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
304  ltp_def.hmsl = gps.hmsl;
305  stateSetLocalOrigin_i(&ltp_def);
306 #endif
307 }
308 
310 #if INS_UPDATE_FW_ESTIMATOR
311  struct UtmCoor_f utm = state.utm_origin_f;
312  utm.alt = gps.hmsl / 1000.0f;
314 #else
315  struct LlaCoor_i lla = {
318  gps.lla_pos.alt
319  };
320  struct LtpDef_i ltp_def;
321  ltp_def_from_lla_i(&ltp_def, &lla),
322  ltp_def.hmsl = gps.hmsl;
323  stateSetLocalOrigin_i(&ltp_def);
324 #endif
325 }
326 
327 void ahrs_init(void) {
329 }
330 
331 void ahrs_align(void)
332 {
333  /* Compute an initial orientation from accel and mag directly as quaternion */
335 
336  /* use average gyro as initial value for bias */
337  struct FloatRates bias0;
340 
341  // ins and ahrs are now running
344 }
345 
346 void ahrs_propagate(void) {
347  struct NedCoor_f accel;
348  struct FloatRates body_rates;
349 
350  // realign all the filter if needed
351  // a complete init cycle is required
352  if (ins_impl.reset) {
353  ins_impl.reset = FALSE;
357  }
358 
359  // fill command vector
360  struct Int32Rates gyro_meas_body;
361  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
362  INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, *body_to_imu_rmat, imu.gyro);
363  RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body);
364  struct Int32Vect3 accel_meas_body;
365  INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel);
366  ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body);
367 
368  // update correction gains
370 
371  // propagate model
372  struct inv_state new_state;
373  runge_kutta_4_float((float*)&new_state,
374  (float*)&ins_impl.state, INV_STATE_DIM,
375  (float*)&ins_impl.cmd, INV_COMMAND_DIM,
376  invariant_model, dt);
377  ins_impl.state = new_state;
378 
379  // normalize quaternion
381 
382  // set global state
383 #if INS_UPDATE_FW_ESTIMATOR || SEND_INVARIANT_FILTER
384  struct FloatEulers eulers;
386 #endif
387 #if INS_UPDATE_FW_ESTIMATOR
388  // Some stupid lines of code for neutrals
389  eulers.phi -= ins_roll_neutral;
390  eulers.theta -= ins_pitch_neutral;
391  stateSetNedToBodyEulers_f(&eulers);
392 #else
394 #endif
396  stateSetBodyRates_f(&body_rates);
399  // untilt accel and remove gravity
401  FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
402  FLOAT_VECT3_ADD(accel, A);
403  stateSetAccelNed_f(&accel);
404 
405  //------------------------------------------------------------//
406 
407 #if SEND_INVARIANT_FILTER
408  RunOnceEvery(3,{
409  DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
411  &eulers.phi,
412  &eulers.theta,
413  &eulers.psi,
417  &ins_impl.state.pos.x,
418  &ins_impl.state.pos.y,
419  &ins_impl.state.pos.z,
420  &ins_impl.state.bias.p,
421  &ins_impl.state.bias.q,
422  &ins_impl.state.bias.r,
423  &ins_impl.state.as,
424  &ins_impl.state.hb,
427  });
428 #endif
429 
430 #if LOG_INVARIANT_FILTER
431  if (pprzLogFile.fs != NULL) {
432  if (!log_started) {
433  // log file header
434  sdLogWriteLog(&pprzLogFile, "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");
435  log_started = TRUE;
436  }
437  else {
438  sdLogWriteLog(&pprzLogFile, "%.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",
451  ins_impl.meas.mag.x,
452  ins_impl.meas.mag.y,
453  ins_impl.meas.mag.z,
468  ins_impl.state.hb,
469  ins_impl.state.as);
470  }
471  }
472 #endif
473 }
474 
475 void ahrs_update_gps(void) {
476 
477  if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) {
479 
480 #if INS_UPDATE_FW_ESTIMATOR
481  if (state.utm_initialized_f) {
482  // position (local ned)
485  ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.0f);
486  // speed
487  ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.0f;
488  ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.0f;
489  ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.0f;
490  }
491 #else
492  if (state.ned_initialized_f) {
493  struct EcefCoor_f ecef_pos, ecef_vel;
494  ECEF_FLOAT_OF_BFP(ecef_pos, gps.ecef_pos);
496  ECEF_FLOAT_OF_BFP(ecef_vel, gps.ecef_vel);
498  }
499 #endif
500  }
501 
502 }
503 
504 
505 static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
506  static float ins_qfe = 101325.0f;
507  static float alpha = 10.0f;
508  static int32_t i = 1;
509  static float baro_moy = 0.0f;
510  static float baro_prev = 0.0f;
511 
512  if (!ins_baro_initialized) {
513  // try to find a stable qfe
514  // TODO generic function in pprz_isa ?
515  if (i == 1) {
516  baro_moy = *pressure;
517  baro_prev = *pressure;
518  }
519  baro_moy = (baro_moy*(i-1) + *pressure) / i;
520  alpha = (10.*alpha + (baro_moy-baro_prev)) / (11.0f);
521  baro_prev = baro_moy;
522  // test stop condition
523  if (fabs(alpha) < 0.005f) {
524  ins_qfe = baro_moy;
525  ins_baro_initialized = TRUE;
526  }
527  if (i == 250) {
528  ins_qfe = *pressure;
529  ins_baro_initialized = TRUE;
530  }
531  i++;
532  }
533  else { /* normal update with baro measurement */
534  ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); // Z down
535  }
536 }
537 
538 void ahrs_update_accel(void) {
539 }
540 
541 // assume mag is dead when values are not moving anymore
542 #define MAG_FROZEN_COUNT 30
543 
544 void ahrs_update_mag(void) {
545  static uint32_t mag_frozen_count = MAG_FROZEN_COUNT;
546  static int32_t last_mx = 0;
547 
548  if (last_mx == imu.mag.x) {
549  mag_frozen_count--;
550  if (mag_frozen_count == 0) {
551  // if mag is dead, better set measurements to zero
553  mag_frozen_count = MAG_FROZEN_COUNT;
554  }
555  }
556  else {
557  // values are moving
558  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
559  struct Int32Vect3 mag_meas_body;
560  // new values in body frame
561  INT32_RMAT_TRANSP_VMULT(mag_meas_body, *body_to_imu_rmat, imu.mag);
562  MAGS_FLOAT_OF_BFP(ins_impl.meas.mag, mag_meas_body);
563  // reset counter
564  mag_frozen_count = MAG_FROZEN_COUNT;
565  }
566  last_mx = imu.mag.x;
567 }
568 
569 
574 static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m __attribute__((unused))) {
575 
576  const struct inv_state * s = (const struct inv_state *)x;
577  const struct inv_command * c = (const struct inv_command *)u;
578  struct inv_state s_dot;
579  struct FloatRates rates;
580  struct FloatVect3 tmp_vect;
581  struct FloatQuat tmp_quat;
582  float norm;
583 
584  // test accel sensitivity
585  if (fabs(s->as) < 0.1) {
586  // too small, return x_dot = 0 to avoid division by 0
587  float_vect_zero(o, n);
588  // TODO set ins state to error
589  return;
590  }
591 
592  /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
593  RATES_DIFF(rates, c->rates, s->bias);
594  FLOAT_VECT3_ASSIGN(tmp_vect, rates.p, rates.q, rates.r);
595  FLOAT_QUAT_VMUL_LEFT(s_dot.quat, s->quat, tmp_vect);
596  FLOAT_QUAT_SMUL(s_dot.quat, s_dot.quat, 0.5);
597 
598  FLOAT_QUAT_VMUL_RIGHT(tmp_quat, s->quat, ins_impl.corr.LE);
599  FLOAT_QUAT_ADD(s_dot.quat, tmp_quat);
600 
601  norm = FLOAT_QUAT_NORM(s->quat);
602  norm = 1. - (norm*norm);
603  FLOAT_QUAT_SMUL(tmp_quat, s->quat, norm);
604  FLOAT_QUAT_ADD(s_dot.quat, tmp_quat);
605 
606  /* dot_V = A + (1/as) * (q * am * q-1) + ME */
607  FLOAT_QUAT_RMAT_B2N(s_dot.speed, s->quat, c->accel);
608  FLOAT_VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
609  FLOAT_VECT3_ADD(s_dot.speed, A);
611 
612  /* dot_X = V + NE */
613  FLOAT_VECT3_SUM(s_dot.pos, s->speed, ins_impl.corr.NE);
614 
615  /* bias_dot = q-1 * (OE) * q */
616  FLOAT_QUAT_RMAT_N2B(tmp_vect, s->quat, ins_impl.corr.OE);
617  RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
618 
619  /* as_dot = as * RE */
620  s_dot.as = (s->as) * (ins_impl.corr.RE);
621 
622  /* hb_dot = SE */
623  s_dot.hb = ins_impl.corr.SE;
624 
625  // set output
626  memcpy(o, &s_dot, n*sizeof(float));
627 }
628 
633 static inline void error_output(struct InsFloatInv * _ins) {
634 
635  struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
636  float Eh;
637  float temp;
638 
639  // test accel sensitivity
640  if (fabs(_ins->state.as) < 0.1) {
641  // too small, don't do anything to avoid division by 0
642  return;
643  }
644 
645  /* YBt = q * yB * q-1 */
646  FLOAT_QUAT_RMAT_B2N(YBt, _ins->state.quat, _ins->meas.mag);
647 
648  FLOAT_QUAT_RMAT_B2N(I, _ins->state.quat, _ins->cmd.accel);
649  FLOAT_VECT3_SMUL(I, I, 1. / (_ins->state.as));
650 
651  /*--------- E = ( ŷ - y ) ----------*/
652  /* Eb = ( B - YBt ) */
653  FLOAT_VECT3_DIFF(Eb, B, YBt);
654 
655  // pos and speed error only if GPS data are valid
656  // or while waiting first GPS data to prevent diverging
657  if ((gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING
658 #if INS_UPDATE_FW_ESTIMATOR
660 #else
662 #endif
663  ) || !ins_gps_fix_once) {
664  /* Ev = (V - YV) */
665  FLOAT_VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
666  /* Ex = (X - YX) */
667  FLOAT_VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
668  }
669  else {
670  FLOAT_VECT3_ZERO(Ev);
671  FLOAT_VECT3_ZERO(Ex);
672  }
673  /* Eh = < X,e3 > - hb - YH */
674  Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
675 
676  /*--------------Gains--------------*/
677 
678  /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
679  FLOAT_VECT3_SMUL(Itemp, I, -_ins->gains.lv/100.);
680  FLOAT_VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
681 
682  FLOAT_VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
683  temp = FLOAT_VECT3_DOT_PRODUCT(Ebtemp, I);
684  temp = (_ins->gains.lb/100.) * temp;
685 
686  FLOAT_VECT3_SMUL(Ebtemp, I, temp);
687  FLOAT_VECT3_ADD(Evtemp, Ebtemp);
688  FLOAT_VECT3_COPY(_ins->corr.LE, Evtemp);
689 
690  /***** MvEv + MhEh = -mv * Ev + (-mh * <Eh,e3>)********/
691  _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
692  _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
693  _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
694 
695  /****** NxEx + NhEh = -nx * Ex + (-nh * <Eh, e3>) ********/
696  _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
697  _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
698  _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
699 
700  /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
701  FLOAT_VECT3_SMUL(Itemp, I, _ins->gains.ov/1000.);
702  FLOAT_VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
703 
704  FLOAT_VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
705  temp = FLOAT_VECT3_DOT_PRODUCT(Ebtemp, I);
706  temp = (-_ins->gains.ob/1000.) * temp;
707 
708  FLOAT_VECT3_SMUL(Ebtemp, I, temp);
709  FLOAT_VECT3_ADD(Evtemp, Ebtemp);
710  FLOAT_VECT3_COPY(_ins->corr.OE, Evtemp);
711 
712  /* a scalar */
713  /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
714  _ins->corr.RE = ((_ins->gains.rv/100.) * FLOAT_VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh/10000.) * Eh);
715 
716  /****** ShEh ******/
717  _ins->corr.SE = (_ins->gains.sh) * Eh;
718 
719 }
720 
#define FLOAT_RATES_ZERO(_r)
struct Ins ins
global INS state
Definition: ins.c:36
#define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z)
struct NedCoor_f pos
Estimates position.
Interface to align the AHRS via low-passed measurements at startup.
#define INV_COMMAND_DIM
Invariant filter command vector dimension.
#define FLOAT_QUAT_VMUL_RIGHT(_mright, _qi, _vi)
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:361
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
struct FloatVect3 OE
Correction gains on gyro biases.
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
struct FloatQuat quat
Estimated attitude (quaternion)
float as
Estimated accelerometer sensitivity.
vector in EarthCenteredEarthFixed coordinates
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
static void float_vect_zero(float *a, const int n)
a = 0
#define INV_STATE_DIM
Invariant filter state dimension.
Attitude and Heading Reference System interface.
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
float mvz
Tuning parameter of vertical speed error on speed.
#define FLOAT_QUAT_SMUL(_qo, _qi, _s)
static const struct FloatVect3 A
#define INS_INV_RV
bool_t ins_baro_initialized
#define FLOAT_VECT3_ADD(_a, _b)
vector in EarthCenteredEarthFixed coordinates
void ins_init()
INS initialization.
uint8_t zone
UTM zone number.
#define INS_INV_OB
float nh
Tuning parameter of baro error on vertical position.
Periodic telemetry system header (includes downlink utility and generated code).
void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
angular rates
static const float dt
#define INS_BARO_ID
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
float rv
Tuning parameter of speed error on accel biases.
float hb
Estimates barometers bias.
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
float psi
in radians
struct FloatVect3 ME
Correction gains on speed.
enum InsStatus status
status of the INS
Definition: ins.h:47
float y
in meters
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
#define INS_INV_SH
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
bool_t log_started
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
Main include for ABI (AirBorneInterface).
float baro_alt
Measured barometric altitude.
abi_event baro_ev
#define INS_ROLL_NEUTRAL_DEFAULT
bool_t ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:219
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:985
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
int32_t x
North.
#define INS_INV_LB
uint8_t nav_utm_zone0
Definition: common_nav.c:44
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
Integrated Navigation System interface.
struct inv_correction_gains corr
correction gains
float SE
Correction gains on barometer bias.
static float pprz_isa_height_of_pressure(float pressure, float ref)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:73
struct LlaCoor_i lla
Reference point in lla.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
#define FLOAT_VECT3_COPY(_a, _b)
bool_t ins_gps_fix_once
vector in Latitude, Longitude and Altitude
Definition: ins.h:36
struct inv_gains gains
tuning gains
#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2)
float theta
in radians
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
void ins_reset_altitude_ref(void)
INS altitude reference reset.
#define INS_INV_NXZ
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in Latitude, Longitude and Altitude
float rh
Tuning parameter of baro error on accel biases (vertical projection)
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
#define FALSE
Definition: imu_chimu.h:141
static void error_output(struct InsFloatInv *_ins)
Compute correction vectors E = ( ŷ - y ) LE, ME, NE, OE : ( gain matrix * error ) ...
float ov
Tuning parameter of speed error on gyro biases.
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:720
struct InsFloatInv ins_impl
global INS state
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:555
float p
in rad/s
#define AHRS_PROPAGATE_FREQUENCY
Definition: main_ap.c:134
Roation quaternion.
int32_t z
Down.
float x
in meters
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
definition of the local (flat earth) coordinate system
uint8_t zone
UTM zone number.
#define ECEF_FLOAT_OF_BFP(_o, _i)
#define FLOAT_QUAT_VMUL_LEFT(_mleft, _qi, _vi)
#define INS_INV_LV
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
#define FLOAT_QUAT_NORM(_q)
int32_t nav_utm_north0
Definition: common_nav.c:43
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
Paparazzi floating point algebra.
Invariant filter structure.
vector in North East Down coordinates Units: meters
float phi
in radians
float sh
Tuning parameter of baro error on baro bias.
float mh
Tuning parameter of baro error on vertical speed.
static void invariant_model(float *o, const float *x, const int n, const float *u, const int m)
Compute dynamic mode.
float north
in meters
bool_t reset
flag to request reset/reinit the filter
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:440
Device independent GPS code (interface)
#define INS_INV_MH
Paparazzi atmospheric pressure convertion utilities.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
static void init_invariant_state(void)
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
float z
in meters
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
#define INS_PITCH_NEUTRAL_DEFAULT
#define INS_INV_NX
float lv
Tuning parameter of speed error on attitude.
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define FLOAT_VECT3_DIFF(_c, _a, _b)
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct Int32Vect3 *accel, struct Int32Vect3 *mag)
uint16_t foo
Definition: main_demo5.c:54
Invariant filter command vector.
#define INS_INV_MVZ
int32_t y
East.
int32_t north
in centimeters
int32_t x
in centimeters
Inertial Measurement Unit interface.
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:569
angular rates
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:228
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:672
float mv
Tuning parameter of horizontal speed error on speed.
struct FloatVect3 NE
Correction gains on position.
float ins_pitch_neutral
Definition: ins_arduimu.c:15
float r
in rad/s
#define FLOAT_VECT3_SUM(_a, _b, _c)
float nx
Tuning parameter of horizontal position error on position.
#define INS_INV_NH
signed long int32_t
Definition: types.h:19
Utility functions for fixed point AHRS implementations.
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
#define FLOAT_QUAT_NORMALIZE(_q)
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
struct inv_command cmd
command vector
struct FloatVect3 accel
Input accelerometers.
float RE
Correction gains on accel bias.
#define MAG_FROZEN_COUNT
int32_t east
in centimeters
bool_t utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:231
Invariant filter state.
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct inv_state state
state vector
float nxz
Tuning parameter of vertical position error on position.
struct NedCoor_f speed
Estimates speed.
void ahrs_align(void)
Aligns the AHRS.
int32_t lon
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
#define FLOAT_EULERS_OF_QUAT(_e, _q)
int32_t nav_utm_east0
Definition: common_nav.c:42
#define LLA_FLOAT_OF_BFP(_o, _i)
#define INS_INV_OV
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
float ob
Tuning parameter of mag error on gyro biases.
struct EcefCoor_i ecef
Reference point in ecef.
struct NedCoor_f pos_gps
Measured gps position.
#define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi)
struct inv_measures meas
measurement vector
#define FLOAT_VECT3_SMUL(_vo, _vi, _s)
static void baro_cb(uint8_t sender_id, const float *pressure)
void ahrs_propagate(void)
Propagation.
#define INS_INV_MV
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
arch independent LED (Light Emitting Diodes) API
rotation matrix
#define FLOAT_QUAT_ADD(_qo, _qi)
struct NedCoor_f speed_gps
Measured gps speed.
float lb
Tuning parameter of mag error on attitude.
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
float q
in rad/s
static const struct FloatVect3 B
float ins_roll_neutral
Definition: ins_arduimu.c:14
struct FloatVect3 LE
Correction gains on attitude.
#define FLOAT_QUAT_RMAT_B2N(_b2n, _qi, _vi)
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
struct FloatRates rates
Input gyro rates.
struct FloatVect3 mag
Measured magnetic field.
void ins_reset_local_origin(void)
INS local origin reset.
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
#define INS_INV_RH
float east
in meters
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_QUAT_ZERO(_q)
float alt
in meters above WGS84 reference ellipsoid
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:908
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:459
struct GpsState gps
global GPS state
Definition: gps.c:41
#define AHRS_RUNNING
Definition: ahrs.h:36
struct State state
Definition: state.c:36
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
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.
struct FloatRates bias
Estimated gyro biases.
void ahrs_init(void)
AHRS initialization.
Event structure to store callbacks in a linked list.
Definition: abi_common.h:58