Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
hf_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
31 #include "subsystems/imu.h"
32 #include "state.h"
33 #include "subsystems/gps.h"
34 #include <stdlib.h>
36 #include "generated/airframe.h"
37 
38 #if PERIODIC_TELEMETRY
40 #endif
41 
42 #ifdef SITL
43 #include <stdio.h>
44 #define DBG_LEVEL 1
45 #define PRINT_DBG(_l, _p) { \
46  if (DBG_LEVEL >= _l) \
47  printf _p; \
48  }
49 #else
50 #define PRINT_DBG(_l, _p) {}
51 #endif
52 
53 
54 #ifndef AHRS_PROPAGATE_FREQUENCY
55 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
56 #endif
57 
58 #ifndef HFF_PRESCALER
59 #if AHRS_PROPAGATE_FREQUENCY == 512
60 #define HFF_PRESCALER 16
61 #elif AHRS_PROPAGATE_FREQUENCY == 500
62 #define HFF_PRESCALER 10
63 #elif AHRS_PROPAGATE_FREQUENCY == 200
64 #define HFF_PRESCALER 6
65 #else
66 #error "HFF_PRESCALER not set, needs to be a divisor of AHRS_PROPAGATE_FREQUENCY"
67 #endif
68 #endif
69 
71 #define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER)
72 #define HFF_DT (1./HFF_FREQ)
73 
75 #define HFF_INIT_PXX 1.
76 
77 #ifndef HFF_ACCEL_NOISE
78 #define HFF_ACCEL_NOISE 0.5
79 #endif
80 #define HFF_Q HFF_ACCEL_NOISE
81 #define HFF_Qdotdot HFF_ACCEL_NOISE
82 
83 //TODO: proper measurement noise
84 #ifndef HFF_R_POS
85 #define HFF_R_POS 8.
86 #endif
87 #ifndef HFF_R_POS_MIN
88 #define HFF_R_POS_MIN 3.
89 #endif
90 
91 #ifndef HFF_R_GPS_SPEED
92 #define HFF_R_GPS_SPEED 2.
93 #endif
94 #ifndef HFF_R_GPS_SPEED_MIN
95 #define HFF_R_GPS_SPEED_MIN 0.25
96 #endif
97 
98 #ifndef HFF_UPDATE_GPS_SPEED
99 #define HFF_UPDATE_GPS_SPEED TRUE
100 #endif
101 
102 #ifndef HFF_LOWPASS_CUTOFF_FREQUENCY
103 #define HFF_LOWPASS_CUTOFF_FREQUENCY 14
104 #endif
105 
106 #if HFF_LOWPASS_CUTOFF_FREQUENCY < 8
107 #error "It is not allowed to use a cutoff frequency lower than 8Hz due to overflow issues."
108 #endif
109 
110 /* low pass filter variables */
114 
115 /* gps measurement noise */
117 
118 /*
119  X_x = [ x xdot xbias ]
120  X_y = [ y ydot ybias ]
121 */
122 
123 /* output filter states */
125 
126 /* last acceleration measurement */
127 static float hff_xdd_meas = 0;
128 static float hff_ydd_meas = 0;
129 
130 /* last velocity measurement */
131 static float hff_xd_meas = 0;
132 static float hff_yd_meas = 0;
133 
134 /* last position measurement */
135 static float hff_x_meas = 0;
136 static float hff_y_meas = 0;
137 
139 static int hff_ps_counter;
140 
141 /* default parameters */
142 #define HFF_Qbiasbias 1e-7
143 
144 /*
145  * For GPS lag compensation
146  *
147  */
148 #ifdef GPS_LAG
149 /*
150  * GPS_LAG is defined in seconds in airframe file
151  */
152 
154 #define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
155 
156 #define GPS_DT_N ((int) (HFF_FREQ / 4))
157 
158 #define GPS_LAG_TOLERANCE 0.08
159 #define GPS_LAG_TOL_N ((int) (GPS_LAG_TOLERANCE * HFF_FREQ + 0.5))
160 
161 /* maximum number of past propagation steps to rerun per ap cycle
162  * make sure GPS_LAG_N/MAX_PP_STEPS < 128
163  * GPS_LAG_N/MAX_PP_STEPS/512 = seconds until re-propagation catches up with the present
164  */
165 #define MAX_PP_STEPS 6
166 
167 /* variables for mean accel buffer */
168 #define ACC_BUF_MAXN (GPS_LAG_N+10)
169 #define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; }
170 
172 struct FloatVect2 past_accel[ACC_BUF_MAXN];
173 
174 static unsigned int acc_buf_r;
175 static unsigned int acc_buf_w;
176 static unsigned int acc_buf_n;
177 
178 
179 /*
180  * stuff for ringbuffer to store past filter states
181  */
182 #define HFF_RB_MAXN ((int) (GPS_LAG * 4))
183 #define INC_RB_POINTER(ptr) { \
184  if (ptr == &hff_rb[HFF_RB_MAXN-1]) \
185  ptr = hff_rb; \
186  else \
187  ptr++; \
188  }
189 
191 struct HfilterFloat hff_rb[HFF_RB_MAXN];
192 struct HfilterFloat *hff_rb_put;
193 #endif /* GPS_LAG */
194 
196 static int hff_rb_n;
197 
198 
201 
204 static int past_save_counter;
205 #define SAVE_NOW 0
206 #define SAVING -1
207 #define SAVE_DONE -2
208 
209 #define HFF_LOST_LIMIT 1000
212 
213 #ifdef GPS_LAG
214 static void hff_get_past_accel(unsigned int back_n);
215 static void hff_rb_put_state(struct HfilterFloat *source);
216 static void hff_rb_drop_last(void);
217 static void hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source);
218 #endif
219 
220 static void hff_init_x(float init_x, float init_xdot, float init_xbias);
221 static void hff_init_y(float init_y, float init_ydot, float init_ybias);
222 
223 static void hff_propagate_x(struct HfilterFloat *filt, float dt);
224 static void hff_propagate_y(struct HfilterFloat *filt, float dt);
225 
226 static void hff_update_x(struct HfilterFloat *filt, float x_meas, float Rpos);
227 static void hff_update_y(struct HfilterFloat *filt, float y_meas, float Rpos);
228 
229 static void hff_update_xdot(struct HfilterFloat *filt, float vel, float Rvel);
230 static void hff_update_ydot(struct HfilterFloat *filt, float vel, float Rvel);
231 
232 #if PERIODIC_TELEMETRY
233 
234 static void send_hff(struct transport_tx *trans, struct link_device *dev)
235 {
236  pprz_msg_send_HFF(trans, dev, AC_ID,
237  &hff.x,
238  &hff.y,
239  &hff.xdot,
240  &hff.ydot,
241  &hff.xdotdot,
242  &hff.ydotdot,
243  &hff.xbias,
244  &hff.ybias);
245 }
246 
247 static void send_hff_debug(struct transport_tx *trans, struct link_device *dev)
248 {
249  pprz_msg_send_HFF_DBG(trans, dev, AC_ID,
250  &hff_x_meas,
251  &hff_y_meas,
252  &hff_xd_meas,
253  &hff_yd_meas,
254  &hff.xP[0][0],
255  &hff.yP[0][0],
256  &hff.xP[1][1],
257  &hff.yP[1][1],
258  &hff.xP[2][2],
259  &hff.yP[2][2]);
260 }
261 
262 #ifdef GPS_LAG
263 static void send_hff_gps(struct transport_tx *trans, struct link_device *dev)
264 {
265  pprz_msg_send_HFF_GPS(trans, dev, AC_ID,
266  &(hff_rb_last->lag_counter),
268  &save_counter);
269 }
270 #endif
271 
272 #endif
273 
274 void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
275 {
278  hff_init_x(init_x, init_xdot, 0.f);
279  hff_init_y(init_y, init_ydot, 0.f);
280 #ifdef GPS_LAG
281  /* init buffer for past mean accel values */
282  acc_buf_r = 0;
283  acc_buf_w = 0;
284  acc_buf_n = 0;
285  hff_rb_put = hff_rb;
286  hff_rb_last = hff_rb;
287  hff_rb_last->rollback = false;
288  hff_rb_last->lag_counter = 0;
289  hff.lag_counter = GPS_LAG_N;
290 #ifdef SITL
291  printf("GPS_LAG: %f\n", GPS_LAG);
292  printf("GPS_LAG_N: %d\n", GPS_LAG_N);
293  printf("GPS_DT_N: %d\n", GPS_DT_N);
294  printf("HFF_DT: %f\n", HFF_DT);
295  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
296 #endif
297 #else
298  hff_rb_last = &hff;
299  hff.lag_counter = 0;
300 #endif
301  hff_rb_n = 0;
302  hff.rollback = false;
303  lag_counter_err = 0;
304  save_counter = -1;
306  hff_lost_counter = 0;
309  hff_ps_counter = 0;
310 
311 #if PERIODIC_TELEMETRY
314 #ifdef GPS_LAG
315  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_GPS, send_hff_gps);
316 #endif
317 #endif
318 
322 }
323 
324 static void hff_init_x(float init_x, float init_xdot, float init_xbias)
325 {
326  hff.x = init_x;
327  hff.xdot = init_xdot;
328  hff.xbias = init_xbias;
329  int i, j;
330  for (i = 0; i < HFF_STATE_SIZE; i++) {
331  for (j = 0; j < HFF_STATE_SIZE; j++) {
332  hff.xP[i][j] = 0.;
333  }
334  hff.xP[i][i] = HFF_INIT_PXX;
335  }
336 }
337 
338 static void hff_init_y(float init_y, float init_ydot, float init_ybias)
339 {
340  hff.y = init_y;
341  hff.ydot = init_ydot;
342  hff.ybias = init_ybias;
343  int i, j;
344  for (i = 0; i < HFF_STATE_SIZE; i++) {
345  for (j = 0; j < HFF_STATE_SIZE; j++) {
346  hff.yP[i][j] = 0.;
347  }
348  hff.yP[i][i] = HFF_INIT_PXX;
349  }
350 }
351 
352 #ifdef GPS_LAG
353 static void hff_store_accel_ltp(float x, float y)
354 {
355  past_accel[acc_buf_w].x = x;
356  past_accel[acc_buf_w].y = y;
357  INC_ACC_IDX(acc_buf_w);
358 
359  if (acc_buf_n < ACC_BUF_MAXN) {
360  acc_buf_n++;
361  } else {
362  INC_ACC_IDX(acc_buf_r);
363  }
364 }
365 
366 /* get the accel values from back_n steps ago */
367 static void hff_get_past_accel(unsigned int back_n)
368 {
369  int i;
370  if (back_n > acc_buf_n) {
371  PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n));
372  back_n = acc_buf_n;
373  } else if (back_n == 0) {
374  PRINT_DBG(1, ("Cannot go back zero steps!\n"));
375  return;
376  }
377  if ((int)(acc_buf_w - back_n) < 0) {
378  i = acc_buf_w - back_n + ACC_BUF_MAXN;
379  } else {
380  i = acc_buf_w - back_n;
381  }
382  hff_xdd_meas = past_accel[i].x;
383  hff_ydd_meas = past_accel[i].y;
384  PRINT_DBG(3, ("get past accel. buf_n: %2d \tbuf_w: %2d \tback_n: %2d \ti: %2d \txdd: %f \tydd: %f\n", acc_buf_n,
385  acc_buf_w, back_n, i, hff_xdd_meas, hff_ydd_meas));
386 }
387 
388 static void hff_rb_put_state(struct HfilterFloat *source)
389 {
390  /* copy state from source into buffer */
391  hff_set_state(hff_rb_put, source);
392  hff_rb_put->lag_counter = 0;
393  hff_rb_put->rollback = false;
394 
395  /* forward write pointer */
396  INC_RB_POINTER(hff_rb_put);
397 
398  /* increase fill count and forward last pointer if neccessary */
399  if (hff_rb_n < HFF_RB_MAXN) {
400  hff_rb_n++;
401  } else {
402  INC_RB_POINTER(hff_rb_last);
403  }
404  PRINT_DBG(2, ("put state. fill count now: %d\n", hff_rb_n));
405 }
406 
407 static void hff_rb_drop_last(void)
408 {
409  if (hff_rb_n > 0) {
410  INC_RB_POINTER(hff_rb_last);
411  hff_rb_n--;
412  } else {
413  PRINT_DBG(2, ("hff ringbuffer empty!\n"));
414  hff_rb_last->lag_counter = 0;
415  hff_rb_last->rollback = false;
416  }
417  PRINT_DBG(2, ("drop last state. fill count now: %d\n", hff_rb_n));
418 }
419 
420 /* copy source state to dest state */
421 static void hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source)
422 {
423  dest->x = source->x;
424  dest->xdot = source->xdot;
425  dest->xdotdot = source->xdotdot;
426  dest->y = source->y;
427  dest->ydot = source->ydot;
428  dest->ydotdot = source->ydotdot;
429  for (int i = 0; i < HFF_STATE_SIZE; i++) {
430  for (int j = 0; j < HFF_STATE_SIZE; j++) {
431  dest->xP[i][j] = source->xP[i][j];
432  dest->yP[i][j] = source->yP[i][j];
433  }
434  }
435 }
436 
437 static void hff_propagate_past(struct HfilterFloat *filt_past)
438 {
439  PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
440  /* run max MAX_PP_STEPS propagation steps */
441  for (int i = 0; i < MAX_PP_STEPS; i++) {
442  if (hff_past->lag_counter > 0) {
443  hff_get_past_accel(hff_past->lag_counter);
444  PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
445  hff_propagate_x(hff_past, HFF_DT);
446  hff_propagate_y(hff_past, HFF_DT);
447  hff_past->lag_counter--;
448 
449  if (past_save_counter > 0) {
451  PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
452  } else if (past_save_counter == SAVE_NOW) {
453  /* next GPS measurement valid at this state -> save */
454  PRINT_DBG(2, ("save past state\n"));
455  hff_rb_put_state(hff_past);
457  } else if (past_save_counter == SAVING) {
458  /* increase lag counter on if next state is already saved */
459  if (hff_past == &hff_rb[HFF_RB_MAXN - 1]) {
460  hff_rb[0].lag_counter++;
461  } else {
462  (hff_past + 1)->lag_counter++;
463  }
464  }
465  }
466 
467  /* finished re-propagating the past values */
468  if (hff_past->lag_counter == 0) {
469  hff_set_state(&hff, hff_past);
470  hff_rb_drop_last();
472  break;
473  }
474  }
475 }
476 #endif /* GPS_LAG */
477 
478 
479 void hff_propagate(void)
480 {
483  }
484 
487  }
488 
489 #ifdef GPS_LAG
490  /* continue re-propagating to catch up with the present */
491  if (hff_rb_last->rollback) {
492  hff_propagate_past(hff_rb_last);
493  }
494 #endif
495 
496  /* rotate imu accel measurement to body frame and filter */
497  struct Int32Vect3 acc_body_filtered;
498  acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, stateGetAccelBody_i()->x);
499  acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, stateGetAccelBody_i()->y);
500  acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, stateGetAccelBody_i()->z);
501 
502  /* propagate current state if it is time */
503  if (hff_ps_counter >= HFF_PRESCALER) {
504  hff_ps_counter = 0;
505  struct Int32Vect3 filtered_accel_ltp;
506  struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
507  int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered);
508  hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
509  hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
510 
511 #ifdef GPS_LAG
512  hff_store_accel_ltp(hff_xdd_meas, hff_ydd_meas);
513 #endif
515  /*
516  * propagate current state
517  */
520 
521 #ifdef GPS_LAG
522  /* increase lag counter on last saved state */
523  if (hff_rb_n > 0) {
524  hff_rb_last->lag_counter++;
525  }
526 
527  /* save filter state if needed */
528  if (save_counter == 0) {
529  PRINT_DBG(1, ("save current state\n"));
530  hff_rb_put_state(&hff);
531  save_counter = -1;
532  } else if (save_counter > 0) {
533  save_counter--;
534  }
535 #endif
536  }
537  }
538  hff_ps_counter++;
539 }
540 
541 void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned __attribute__((unused)))
542 {
543  hff_lost_counter = 0;
544 
545 #if USE_GPS_ACC4R
546  Rgps_pos = (float) gps.pacc / 100.;
547  if (Rgps_pos < HFF_R_POS_MIN) {
549  }
550 
551  Rgps_vel = (float) gps.sacc / 100.;
554  }
555 #endif
556 
557 #ifdef GPS_LAG
558  if (GPS_LAG_N == 0) {
559 #endif
560 
561  /* update filter state with measurement */
562  hff_update_x(&hff, pos_ned->x, Rgps_pos);
563  hff_update_y(&hff, pos_ned->y, Rgps_pos);
564 #if HFF_UPDATE_GPS_SPEED
565  hff_update_xdot(&hff, speed_ned->x, Rgps_vel);
566  hff_update_ydot(&hff, speed_ned->y, Rgps_vel);
567 #endif
568 
569 
570 #ifdef GPS_LAG
571  } else if (hff_rb_n > 0) {
572  /* roll back if state was saved approx when GPS was valid */
573  lag_counter_err = hff_rb_last->lag_counter - GPS_LAG_N;
574  PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", hff_rb_n, hff_rb_last->lag_counter,
575  lag_counter_err));
576  if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
577  hff_rb_last->rollback = true;
578  hff_update_x(hff_rb_last, pos_ned->x, Rgps_pos);
579  hff_update_y(hff_rb_last, pos_ned->y, Rgps_pos);
580 #if HFF_UPDATE_GPS_SPEED
581  hff_update_xdot(hff_rb_last, speed_ned->x, Rgps_vel);
582  hff_update_ydot(hff_rb_last, speed_ned->y, Rgps_vel);
583 #endif
584  past_save_counter = GPS_DT_N - 1; // + lag_counter_err;
585  PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
586  hff_propagate_past(hff_rb_last);
587  } else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N + 1)) {
588  /* apparently missed a GPS update, try next saved state */
589  PRINT_DBG(2, ("try next saved state\n"));
590  hff_rb_drop_last();
591  hff_update_gps(pos_ned, speed_ned);
592  }
593  } else if (save_counter < 0) {
594  /* ringbuffer empty -> save output filter state at next GPS validity point in time */
595  save_counter = GPS_DT_N - 1 - (GPS_LAG_N % GPS_DT_N);
596  PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
597  }
598 #endif /* GPS_LAG */
599 }
600 
601 
602 void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
603 {
604  hff.x = pos.x;
605  hff.y = pos.y;
606  hff.xdot = vel.x;
607  hff.ydot = vel.y;
608  hff.xbias = 0.f;
609  hff.ybias = 0.f;
610 #ifdef GPS_LAG
611  while (hff_rb_n > 0) {
612  hff_rb_drop_last();
613  }
614  save_counter = -1;
616 #endif
617 }
618 
619 
638 static void hff_propagate_x(struct HfilterFloat *filt, float dt)
639 {
640  /* update state */
641  filt->xdotdot = hff_xdd_meas - filt->xbias;
642  filt->x = filt->x + filt->xdot * dt;// + filt->xdotdot * dt * dt / 2;
643  filt->xdot = filt->xdot + dt * filt->xdotdot;
644  /* update covariance */
645  const float FPF00 = filt->xP[0][0] + dt * (filt->xP[1][0] + filt->xP[0][1] + dt * filt->xP[1][1]);
646  const float FPF01 = filt->xP[0][1] + dt * (filt->xP[1][1] - filt->xP[0][2] - dt * filt->xP[1][2]);
647  const float FPF02 = filt->xP[0][2] + dt * (filt->xP[1][2]);
648  const float FPF10 = filt->xP[1][0] + dt * (-filt->xP[2][0] + filt->xP[1][1] - dt * filt->xP[2][1]);
649  const float FPF11 = filt->xP[1][1] + dt * (-filt->xP[2][1] - filt->xP[1][2] + dt * filt->xP[2][2]);
650  const float FPF12 = filt->xP[1][2] + dt * (-filt->xP[2][2]);
651  const float FPF20 = filt->xP[2][0] + dt * (filt->xP[2][1]);
652  const float FPF21 = filt->xP[2][1] + dt * (-filt->xP[2][2]);
653  const float FPF22 = filt->xP[2][2];
654 
655  filt->xP[0][0] = FPF00 + HFF_Q * dt * dt / 2.;
656  filt->xP[0][1] = FPF01;
657  filt->xP[0][2] = FPF02;
658  filt->xP[1][0] = FPF10;
659  filt->xP[1][1] = FPF11 + HFF_Qdotdot * dt;
660  filt->xP[1][2] = FPF12;
661  filt->xP[2][0] = FPF20;
662  filt->xP[2][1] = FPF21;
663  filt->xP[2][2] = FPF22 + HFF_Qbiasbias;
664 }
665 
666 static void hff_propagate_y(struct HfilterFloat *filt, float dt)
667 {
668  /* update state */
669  filt->ydotdot = hff_ydd_meas - filt->ybias;
670  filt->y = filt->y + dt * filt->ydot;// + filt->ydotdot * dt * dt / 2;
671  filt->ydot = filt->ydot + dt * filt->ydotdot;
672  /* update covariance */
673  const float FPF00 = filt->yP[0][0] + dt * (filt->yP[1][0] + filt->yP[0][1] + dt * filt->yP[1][1]);
674  const float FPF01 = filt->yP[0][1] + dt * (filt->yP[1][1] - filt->yP[0][2] - dt * filt->yP[1][2]);
675  const float FPF02 = filt->yP[0][2] + dt * (filt->yP[1][2]);
676  const float FPF10 = filt->yP[1][0] + dt * (-filt->yP[2][0] + filt->yP[1][1] - dt * filt->yP[2][1]);
677  const float FPF11 = filt->yP[1][1] + dt * (-filt->yP[2][1] - filt->yP[1][2] + dt * filt->yP[2][2]);
678  const float FPF12 = filt->yP[1][2] + dt * (-filt->yP[2][2]);
679  const float FPF20 = filt->yP[2][0] + dt * (filt->yP[2][1]);
680  const float FPF21 = filt->yP[2][1] + dt * (-filt->yP[2][2]);
681  const float FPF22 = filt->yP[2][2];
682 
683  filt->yP[0][0] = FPF00 + HFF_Q * dt * dt / 2.;
684  filt->yP[0][1] = FPF01;
685  filt->yP[0][2] = FPF02;
686  filt->yP[1][0] = FPF10;
687  filt->yP[1][1] = FPF11 + HFF_Qdotdot * dt;
688  filt->yP[1][2] = FPF12;
689  filt->yP[2][0] = FPF20;
690  filt->yP[2][1] = FPF21;
691  filt->yP[2][2] = FPF22 + HFF_Qbiasbias;
692 }
693 
694 
711 void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
712 {
713  hff_lost_counter = 0;
714  hff_update_x(&hff, pos.x, Rpos.x);
715  hff_update_y(&hff, pos.y, Rpos.y);
716 }
717 
718 static void hff_update_x(struct HfilterFloat *filt, float x_meas, float Rpos)
719 {
720  hff_x_meas = x_meas;
721 
722  const float y = x_meas - filt->x;
723  const float S = filt->xP[0][0] + Rpos;
724  const float K1 = filt->xP[0][0] * 1 / S;
725  const float K2 = filt->xP[1][0] * 1 / S;
726  const float K3 = filt->xP[2][0] * 1 / S;
727 
728  filt->x = filt->x + K1 * y;
729  filt->xdot = filt->xdot + K2 * y;
730  filt->xbias = filt->xbias + K3 * y;
731 
732  const float P11 = (1. - K1) * filt->xP[0][0];
733  const float P12 = (1. - K1) * filt->xP[0][1];
734  const float P13 = (1. - K1) * filt->xP[0][2];
735  const float P21 = -K2 * filt->xP[0][0] + filt->xP[1][0];
736  const float P22 = -K2 * filt->xP[0][1] + filt->xP[1][1];
737  const float P23 = -K2 * filt->xP[0][2] + filt->xP[1][2];
738  const float P31 = -K3 * filt->xP[0][0] + filt->xP[2][0];
739  const float P32 = -K3 * filt->xP[0][1] + filt->xP[2][1];
740  const float P33 = -K3 * filt->xP[0][2] + filt->xP[2][2];
741 
742  filt->xP[0][0] = P11;
743  filt->xP[0][1] = P12;
744  filt->xP[0][2] = P13;
745  filt->xP[1][0] = P21;
746  filt->xP[1][1] = P22;
747  filt->xP[1][2] = P23;
748  filt->xP[2][0] = P31;
749  filt->xP[2][1] = P32;
750  filt->xP[2][2] = P33;
751 }
752 
753 static void hff_update_y(struct HfilterFloat *filt, float y_meas, float Rpos)
754 {
755  hff_y_meas = y_meas;
756 
757  const float y = y_meas - filt->y;
758  const float S = filt->yP[0][0] + Rpos;
759  const float K1 = filt->yP[0][0] * 1 / S;
760  const float K2 = filt->yP[1][0] * 1 / S;
761  const float K3 = filt->yP[2][0] * 1 / S;
762 
763  filt->y = filt->y + K1 * y;
764  filt->ydot = filt->ydot + K2 * y;
765  filt->ybias = filt->ybias + K3 * y;
766 
767  const float P11 = (1. - K1) * filt->yP[0][0];
768  const float P12 = (1. - K1) * filt->yP[0][1];
769  const float P13 = (1. - K1) * filt->yP[0][2];
770  const float P21 = -K2 * filt->yP[0][0] + filt->yP[1][0];
771  const float P22 = -K2 * filt->yP[0][1] + filt->yP[1][1];
772  const float P23 = -K2 * filt->yP[0][2] + filt->yP[1][2];
773  const float P31 = -K3 * filt->yP[0][0] + filt->yP[2][0];
774  const float P32 = -K3 * filt->yP[0][1] + filt->yP[2][1];
775  const float P33 = -K3 * filt->yP[0][2] + filt->yP[2][2];
776 
777  filt->yP[0][0] = P11;
778  filt->yP[0][1] = P12;
779  filt->yP[0][2] = P13;
780  filt->yP[1][0] = P21;
781  filt->yP[1][1] = P22;
782  filt->yP[1][2] = P23;
783  filt->yP[2][0] = P31;
784  filt->yP[2][1] = P32;
785  filt->yP[2][2] = P33;
786 }
787 
788 
789 /*
790  *
791  * Update velocity
792  *
793  * H = [0 1 0];
794  * R = 0.1;
795  * // state residual
796  * yd = vx - H * Xm;
797  * // covariance residual
798  * S = H*Pm*H' + R;
799  * // kalman gain
800  * K = Pm*H'*inv(S);
801  * // update state
802  * Xp = Xm + K*yd;
803  * // update covariance
804  * Pp = Pm - K*H*Pm;
805 */
806 void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
807 {
808  if (Rvel.x >= 0.f) {
809  hff_update_xdot(&hff, vel.x, Rvel.x);
810  }
811  if (Rvel.y >= 0.f) {
812  hff_update_ydot(&hff, vel.y, Rvel.y);
813  }
814 
815  if (Rvel.x >= 0.f || Rvel.y >= 0.f) {
817  }
818 }
819 
820 static void hff_update_xdot(struct HfilterFloat *filt, float vel, float Rvel)
821 {
822  hff_xd_meas = vel;
823 
824  const float yd = vel - filt->xdot;
825  const float S = filt->xP[1][1] + Rvel;
826  const float K1 = filt->xP[0][1] * 1 / S;
827  const float K2 = filt->xP[1][1] * 1 / S;
828  const float K3 = filt->xP[2][1] * 1 / S;
829 
830  filt->x = filt->x + K1 * yd;
831  filt->xdot = filt->xdot + K2 * yd;
832  filt->xbias = filt->xbias + K3 * yd;
833 
834  const float P11 = -K1 * filt->xP[1][0] + filt->xP[0][0];
835  const float P12 = -K1 * filt->xP[1][1] + filt->xP[0][1];
836  const float P13 = -K1 * filt->xP[1][2] + filt->xP[0][2];
837  const float P21 = (1. - K2) * filt->xP[1][0];
838  const float P22 = (1. - K2) * filt->xP[1][1];
839  const float P23 = (1. - K2) * filt->xP[1][2];
840  const float P31 = -K3 * filt->xP[1][0] + filt->xP[2][0];
841  const float P32 = -K3 * filt->xP[1][1] + filt->xP[2][1];
842  const float P33 = -K3 * filt->xP[1][2] + filt->xP[2][2];
843 
844  filt->xP[0][0] = P11;
845  filt->xP[0][1] = P12;
846  filt->xP[0][2] = P13;
847  filt->xP[1][0] = P21;
848  filt->xP[1][1] = P22;
849  filt->xP[1][2] = P23;
850  filt->xP[2][0] = P31;
851  filt->xP[2][1] = P32;
852  filt->xP[2][2] = P33;
853 }
854 
855 static void hff_update_ydot(struct HfilterFloat *filt, float vel, float Rvel)
856 {
857  hff_yd_meas = vel;
858 
859  const float yd = vel - filt->ydot;
860  const float S = filt->yP[1][1] + Rvel;
861  const float K1 = filt->yP[0][1] * 1 / S;
862  const float K2 = filt->yP[1][1] * 1 / S;
863  const float K3 = filt->yP[2][1] * 1 / S;
864 
865  filt->y = filt->y + K1 * yd;
866  filt->ydot = filt->ydot + K2 * yd;
867  filt->ybias = filt->ybias + K3 * yd;
868 
869  const float P11 = -K1 * filt->yP[1][0] + filt->yP[0][0];
870  const float P12 = -K1 * filt->yP[1][1] + filt->yP[0][1];
871  const float P13 = -K1 * filt->yP[1][2] + filt->yP[0][2];
872  const float P21 = (1. - K2) * filt->yP[1][0];
873  const float P22 = (1. - K2) * filt->yP[1][1];
874  const float P23 = (1. - K2) * filt->yP[1][2];
875  const float P31 = -K3 * filt->yP[1][0] + filt->yP[2][0];
876  const float P32 = -K3 * filt->yP[1][1] + filt->yP[2][1];
877  const float P33 = -K3 * filt->yP[1][2] + filt->yP[2][2];
878 
879  filt->yP[0][0] = P11;
880  filt->yP[0][1] = P12;
881  filt->yP[0][2] = P13;
882  filt->yP[1][0] = P21;
883  filt->yP[1][1] = P22;
884  filt->yP[1][2] = P23;
885  filt->yP[2][0] = P31;
886  filt->yP[2][1] = P32;
887  filt->yP[2][2] = P33;
888 }
unsigned short uint16_t
Definition: types.h:16
static void hff_init_y(float init_y, float init_ydot, float init_ybias)
Definition: hf_float.c:338
#define HFF_R_POS
Definition: hf_float.c:85
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
Definition: state.h:953
#define SAVE_DONE
Definition: hf_float.c:207
static void hff_propagate_y(struct HfilterFloat *filt, float dt)
Definition: hf_float.c:666
static uint8_t dest[]
Definition: w5100.c:99
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
Horizontal filter (x,y) to estimate position and velocity.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
static int hff_ps_counter
counter for hff propagation
Definition: hf_float.c:139
#define HFF_Qdotdot
Definition: hf_float.c:81
static int16_t lag_counter_err
by how many steps the estimated GPS validity point in time differed from GPS_LAG_N ...
Definition: hf_float.c:200
Periodic telemetry system header (includes downlink utility and generated code).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
Definition: state.h:1119
static void hff_update_y(struct HfilterFloat *filt, float y_meas, float Rpos)
Definition: hf_float.c:753
Simple first order low pass filter with bilinear transform.
static float hff_ydd_meas
Definition: hf_float.c:128
void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:602
Butterworth2LowPass_int filter_y
Definition: hf_float.c:112
#define HFF_LOWPASS_CUTOFF_FREQUENCY
Definition: hf_float.c:103
static uint16_t hff_lost_limit
Definition: hf_float.c:210
#define SAVE_NOW
Definition: hf_float.c:205
static void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time, int32_t value)
Init a second order Butterworth filter.
static void hff_update_ydot(struct HfilterFloat *filt, float vel, float Rvel)
Definition: hf_float.c:855
#define HFF_STATE_SIZE
Definition: hf_float.h:38
float Rgps_pos
Definition: hf_float.c:116
static void hff_init_x(float init_x, float init_xdot, float init_xbias)
Definition: hf_float.c:324
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
static void hff_update_x(struct HfilterFloat *filt, float x_meas, float Rpos)
Definition: hf_float.c:718
#define PRINT_DBG(_l, _p)
Definition: hf_float.c:50
static void send_hff_debug(struct transport_tx *trans, struct link_device *dev)
Definition: hf_float.c:247
float ybias
Definition: hf_float.h:48
void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:806
static uint16_t hff_lost_counter
Definition: hf_float.c:211
Device independent GPS code (interface)
static float hff_xd_meas
Definition: hf_float.c:131
#define ACCEL_FLOAT_OF_BFP(_ai)
signed short int16_t
Definition: types.h:17
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
Update second order Butterworth low pass filter state with a new value(fixed point version)...
#define HFF_LOST_LIMIT
Definition: hf_float.c:209
#define HFF_R_GPS_SPEED
Definition: hf_float.c:92
static uint16_t hff_speed_lost_counter
Definition: hf_float.c:211
void hff_propagate(void)
Definition: hf_float.c:479
static float hff_xdd_meas
Definition: hf_float.c:127
Inertial Measurement Unit interface.
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:50
static float hff_y_meas
Definition: hf_float.c:136
struct HfilterFloat hff
Definition: hf_float.c:124
#define HFF_Qbiasbias
Definition: hf_float.c:142
void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Update position.
Definition: hf_float.c:711
#define HFF_DT
Definition: hf_float.c:72
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
float x
Definition: hf_float.h:41
static float hff_x_meas
Definition: hf_float.c:135
static int past_save_counter
Definition: hf_float.c:204
#define SAVING
Definition: hf_float.c:206
bool rollback
Definition: hf_float.h:52
float y
Definition: hf_float.h:45
static float hff_yd_meas
Definition: hf_float.c:132
static void hff_update_xdot(struct HfilterFloat *filt, float vel, float Rvel)
Definition: hf_float.c:820
void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:274
float xdot
Definition: hf_float.h:42
API to get/set the generic vehicle states.
#define HFF_Q
Definition: hf_float.c:80
float xdotdot
Definition: hf_float.h:43
#define HFF_R_GPS_SPEED_MIN
Definition: hf_float.c:95
float ydot
Definition: hf_float.h:46
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
static void send_hff(struct transport_tx *trans, struct link_device *dev)
Definition: hf_float.c:234
rotation matrix
Butterworth2LowPass_int filter_x
Definition: hf_float.c:111
uint16_t lag_counter
Definition: hf_float.h:51
void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition: hf_float.c:541
float Rgps_vel
Definition: hf_float.c:116
#define HFF_R_POS_MIN
Definition: hf_float.c:88
float xbias
Definition: hf_float.h:44
#define HFF_INIT_PXX
initial covariance diagonal
Definition: hf_float.c:75
static void hff_propagate_x(struct HfilterFloat *filt, float dt)
Propagate the filter in time.
Definition: hf_float.c:638
struct GpsState gps
global GPS state
Definition: gps.c:69
static int hff_rb_n
ringbuffer fill count
Definition: hf_float.c:196
struct HfilterFloat * hff_rb_last
ringbuffer read pointer
Definition: hf_float.c:195
Butterworth2LowPass_int filter_z
Definition: hf_float.c:113
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:49
float ydotdot
Definition: hf_float.h:47
static int16_t save_counter
counts down the propagation steps until the filter state is saved again
Definition: hf_float.c:203