Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 #ifdef SITL
39 #include <stdio.h>
40 #define DBG_LEVEL 1
41 #define PRINT_DBG(_l, _p) { \
42  if (DBG_LEVEL >= _l) \
43  printf _p; \
44  }
45 #else
46 #define PRINT_DBG(_l, _p) {}
47 #endif
48 
49 
50 #ifndef AHRS_PROPAGATE_FREQUENCY
51 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
52 #endif
53 
54 #ifndef HFF_PRESCALER
55 #if AHRS_PROPAGATE_FREQUENCY == 512
56 #define HFF_PRESCALER 16
57 #elif AHRS_PROPAGATE_FREQUENCY == 500
58 #define HFF_PRESCALER 10
59 #else
60 #error "HFF_PRESCALER not set, needs to be a divisor of AHRS_PROPAGATE_FREQUENCY"
61 #endif
62 #endif
63 
65 #define HFF_FREQ ((AHRS_PROPAGATE_FREQUENCY)/HFF_PRESCALER)
66 #define DT_HFILTER (1./HFF_FREQ)
67 
69 #define INIT_PXX 1.
70 
71 #ifndef HFF_ACCEL_NOISE
72 #define HFF_ACCEL_NOISE 0.5
73 #endif
74 #define Q HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
75 #define Qdotdot HFF_ACCEL_NOISE*DT_HFILTER
76 
77 //TODO: proper measurement noise
78 #ifndef HFF_R_POS
79 #define HFF_R_POS 8.
80 #endif
81 #ifndef HFF_R_POS_MIN
82 #define HFF_R_POS_MIN 3.
83 #endif
84 
85 #ifndef HFF_R_SPEED
86 #define HFF_R_SPEED 2.
87 #endif
88 #ifndef HFF_R_SPEED_MIN
89 #define HFF_R_SPEED_MIN 1.
90 #endif
91 
92 #ifndef HFF_UPDATE_SPEED
93 #define HFF_UPDATE_SPEED TRUE
94 #endif
95 
96 #ifndef HFF_LOWPASS_CUTOFF_FREQUENCY
97 #define HFF_LOWPASS_CUTOFF_FREQUENCY 14
98 #endif
99 
100 #if HFF_LOWPASS_CUTOFF_FREQUENCY < 8
101 #error "It is not allowed to use a cutoff frequency lower than 8Hz due to overflow issues."
102 #endif
103 
104 /* low pass filter variables */
108 
109 /* gps measurement noise */
111 
112 /*
113 
114  X_x = [ x xdot]
115  X_y = [ y ydot]
116 
117 
118 */
119 /* output filter states */
121 
122 
123 /* last acceleration measurement */
124 static float b2_hff_xdd_meas;
125 static float b2_hff_ydd_meas;
126 
127 /* last velocity measurement */
128 static float b2_hff_xd_meas;
129 static float b2_hff_yd_meas;
130 
131 /* last position measurement */
132 static float b2_hff_x_meas;
133 static float b2_hff_y_meas;
134 
136 static int b2_hff_ps_counter;
137 
138 /*
139  * For GPS lag compensation
140  *
141  *
142  *
143  */
144 #ifdef GPS_LAG
145 /*
146  * GPS_LAG is defined in seconds in airframe file
147  */
148 
150 #define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
151 
152 #define GPS_DT_N ((int) (HFF_FREQ / 4))
153 
154 #define GPS_LAG_TOLERANCE 0.08
155 #define GPS_LAG_TOL_N ((int) (GPS_LAG_TOLERANCE * HFF_FREQ + 0.5))
156 
157 /* maximum number of past propagation steps to rerun per ap cycle
158  * make sure GPS_LAG_N/MAX_PP_STEPS < 128
159  * GPS_LAG_N/MAX_PP_STEPS/512 = seconds until re-propagation catches up with the present
160  */
161 #define MAX_PP_STEPS 6
162 
163 /* variables for mean accel buffer */
164 #define ACC_BUF_MAXN (GPS_LAG_N+10)
165 #define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; }
166 
168 struct FloatVect2 past_accel[ACC_BUF_MAXN];
169 
170 static unsigned int acc_buf_r;
171 static unsigned int acc_buf_w;
172 static unsigned int acc_buf_n;
173 
174 
175 /*
176  * stuff for ringbuffer to store past filter states
177  */
178 #define HFF_RB_MAXN ((int) (GPS_LAG * 4))
179 #define INC_RB_POINTER(ptr) { \
180  if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \
181  ptr = b2_hff_rb; \
182  else \
183  ptr++; \
184  }
185 
187 struct HfilterFloat b2_hff_rb[HFF_RB_MAXN];
188 struct HfilterFloat *b2_hff_rb_put;
189 #endif /* GPS_LAG */
190 
192 static int b2_hff_rb_n;
193 
194 
196 static int lag_counter_err;
197 
199 static int save_counter;
200 static int past_save_counter;
201 #define SAVE_NOW 0
202 #define SAVING -1
203 #define SAVE_DONE -2
204 
205 #define HFF_LOST_LIMIT 1000
208 
209 #ifdef GPS_LAG
210 static void b2_hff_get_past_accel(unsigned int back_n);
211 static void b2_hff_rb_put_state(struct HfilterFloat *source);
212 static void b2_hff_rb_drop_last(void);
213 static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source);
214 #endif
215 
216 
217 static void b2_hff_init_x(float init_x, float init_xdot);
218 static void b2_hff_init_y(float init_y, float init_ydot);
219 
220 static void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt);
221 static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt);
222 
223 static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos);
224 static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos);
225 
226 static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel);
227 static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel);
228 
229 #if PERIODIC_TELEMETRY
231 
232 static void send_hff(struct transport_tx *trans, struct link_device *dev)
233 {
234  pprz_msg_send_HFF(trans, dev, AC_ID,
235  &b2_hff_state.x,
236  &b2_hff_state.y,
241 }
242 
243 static void send_hff_debug(struct transport_tx *trans, struct link_device *dev)
244 {
245  pprz_msg_send_HFF_DBG(trans, dev, AC_ID,
246  &b2_hff_x_meas,
247  &b2_hff_y_meas,
250  &b2_hff_state.xP[0][0],
251  &b2_hff_state.yP[0][0],
252  &b2_hff_state.xP[1][1],
253  &b2_hff_state.yP[1][1]);
254 }
255 
256 #ifdef GPS_LAG
257 static void send_hff_gps(struct transport_tx *trans, struct link_device *dev)
258 {
259  pprz_msg_send_HFF_GPS(trans, dev, AC_ID,
260  &(b2_hff_rb_last->lag_counter),
262  &save_counter);
263 }
264 #endif
265 
266 #endif
267 
268 void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
269 {
272  b2_hff_init_x(init_x, init_xdot);
273  b2_hff_init_y(init_y, init_ydot);
274 #ifdef GPS_LAG
275  /* init buffer for past mean accel values */
276  acc_buf_r = 0;
277  acc_buf_w = 0;
278  acc_buf_n = 0;
279  b2_hff_rb_put = b2_hff_rb;
280  b2_hff_rb_last = b2_hff_rb;
281  b2_hff_rb_last->rollback = FALSE;
282  b2_hff_rb_last->lag_counter = 0;
283  b2_hff_state.lag_counter = GPS_LAG_N;
284 #ifdef SITL
285  printf("GPS_LAG: %f\n", GPS_LAG);
286  printf("GPS_LAG_N: %d\n", GPS_LAG_N);
287  printf("GPS_DT_N: %d\n", GPS_DT_N);
288  printf("DT_HFILTER: %f\n", DT_HFILTER);
289  printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
290 #endif
291 #else
292  b2_hff_rb_last = &b2_hff_state;
294 #endif
295  b2_hff_rb_n = 0;
297  lag_counter_err = 0;
298  save_counter = -1;
300  b2_hff_ps_counter = 1;
303 
304 #if PERIODIC_TELEMETRY
305  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF, send_hff);
306  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_DBG, send_hff_debug);
307 #ifdef GPS_LAG
308  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HFF_GPS, send_hff_gps);
309 #endif
310 #endif
311 
315 }
316 
317 static void b2_hff_init_x(float init_x, float init_xdot)
318 {
319  b2_hff_state.x = init_x;
320  b2_hff_state.xdot = init_xdot;
321  int i, j;
322  for (i = 0; i < HFF_STATE_SIZE; i++) {
323  for (j = 0; j < HFF_STATE_SIZE; j++) {
324  b2_hff_state.xP[i][j] = 0.;
325  }
326  b2_hff_state.xP[i][i] = INIT_PXX;
327  }
328 }
329 
330 static void b2_hff_init_y(float init_y, float init_ydot)
331 {
332  b2_hff_state.y = init_y;
333  b2_hff_state.ydot = init_ydot;
334  int i, j;
335  for (i = 0; i < HFF_STATE_SIZE; i++) {
336  for (j = 0; j < HFF_STATE_SIZE; j++) {
337  b2_hff_state.yP[i][j] = 0.;
338  }
339  b2_hff_state.yP[i][i] = INIT_PXX;
340  }
341 }
342 
343 #ifdef GPS_LAG
344 static void b2_hff_store_accel_ltp(float x, float y)
345 {
346  past_accel[acc_buf_w].x = x;
347  past_accel[acc_buf_w].y = y;
348  INC_ACC_IDX(acc_buf_w);
349 
350  if (acc_buf_n < ACC_BUF_MAXN) {
351  acc_buf_n++;
352  } else {
353  INC_ACC_IDX(acc_buf_r);
354  }
355 }
356 
357 /* get the accel values from back_n steps ago */
358 static void b2_hff_get_past_accel(unsigned int back_n)
359 {
360  int i;
361  if (back_n > acc_buf_n) {
362  PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n));
363  back_n = acc_buf_n;
364  } else if (back_n == 0) {
365  PRINT_DBG(1, ("Cannot go back zero steps!\n"));
366  return;
367  }
368  if ((int)(acc_buf_w - back_n) < 0) {
369  i = acc_buf_w - back_n + ACC_BUF_MAXN;
370  } else {
371  i = acc_buf_w - back_n;
372  }
373  b2_hff_xdd_meas = past_accel[i].x;
374  b2_hff_ydd_meas = past_accel[i].y;
375  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,
376  acc_buf_w, back_n, i, b2_hff_xdd_meas, b2_hff_ydd_meas));
377 }
378 
379 static void b2_hff_rb_put_state(struct HfilterFloat *source)
380 {
381  /* copy state from source into buffer */
382  b2_hff_set_state(b2_hff_rb_put, source);
383  b2_hff_rb_put->lag_counter = 0;
384  b2_hff_rb_put->rollback = FALSE;
385 
386  /* forward write pointer */
387  INC_RB_POINTER(b2_hff_rb_put);
388 
389  /* increase fill count and forward last pointer if neccessary */
390  if (b2_hff_rb_n < HFF_RB_MAXN) {
391  b2_hff_rb_n++;
392  } else {
393  INC_RB_POINTER(b2_hff_rb_last);
394  }
395  PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
396 }
397 
398 static void b2_hff_rb_drop_last(void)
399 {
400  if (b2_hff_rb_n > 0) {
401  INC_RB_POINTER(b2_hff_rb_last);
402  b2_hff_rb_n--;
403  } else {
404  PRINT_DBG(2, ("hff ringbuffer empty!\n"));
405  b2_hff_rb_last->lag_counter = 0;
406  b2_hff_rb_last->rollback = FALSE;
407  }
408  PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n));
409 }
410 
411 /* copy source state to dest state */
412 static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source)
413 {
414  dest->x = source->x;
415  dest->xdot = source->xdot;
416  dest->xdotdot = source->xdotdot;
417  dest->y = source->y;
418  dest->ydot = source->ydot;
419  dest->ydotdot = source->ydotdot;
420  for (int i = 0; i < HFF_STATE_SIZE; i++) {
421  for (int j = 0; j < HFF_STATE_SIZE; j++) {
422  dest->xP[i][j] = source->xP[i][j];
423  dest->yP[i][j] = source->yP[i][j];
424  }
425  }
426 }
427 
428 static void b2_hff_propagate_past(struct HfilterFloat *hff_past)
429 {
430  PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
431  /* run max MAX_PP_STEPS propagation steps */
432  for (int i = 0; i < MAX_PP_STEPS; i++) {
433  if (hff_past->lag_counter > 0) {
434  b2_hff_get_past_accel(hff_past->lag_counter);
435  PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
436  b2_hff_propagate_x(hff_past, DT_HFILTER);
437  b2_hff_propagate_y(hff_past, DT_HFILTER);
438  hff_past->lag_counter--;
439 
440  if (past_save_counter > 0) {
442  PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
443  } else if (past_save_counter == SAVE_NOW) {
444  /* next GPS measurement valid at this state -> save */
445  PRINT_DBG(2, ("save past state\n"));
446  b2_hff_rb_put_state(hff_past);
448  } else if (past_save_counter == SAVING) {
449  /* increase lag counter on if next state is already saved */
450  if (hff_past == &b2_hff_rb[HFF_RB_MAXN - 1]) {
451  b2_hff_rb[0].lag_counter++;
452  } else {
453  (hff_past + 1)->lag_counter++;
454  }
455  }
456  }
457 
458  /* finished re-propagating the past values */
459  if (hff_past->lag_counter == 0) {
460  b2_hff_set_state(&b2_hff_state, hff_past);
461  b2_hff_rb_drop_last();
463  break;
464  }
465  }
466 }
467 #endif /* GPS_LAG */
468 
469 
471 {
474  }
475 
476 #ifdef GPS_LAG
477  /* continue re-propagating to catch up with the present */
478  if (b2_hff_rb_last->rollback) {
479  b2_hff_propagate_past(b2_hff_rb_last);
480  }
481 #endif
482 
483  /* rotate imu accel measurement to body frame and filter */
484  struct Int32Vect3 acc_meas_body;
485  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
486  int32_rmat_transp_vmult(&acc_meas_body, body_to_imu_rmat, &imu.accel);
487 
488  struct Int32Vect3 acc_body_filtered;
489  acc_body_filtered.x = update_butterworth_2_low_pass_int(&filter_x, acc_meas_body.x);
490  acc_body_filtered.y = update_butterworth_2_low_pass_int(&filter_y, acc_meas_body.y);
491  acc_body_filtered.z = update_butterworth_2_low_pass_int(&filter_z, acc_meas_body.z);
492 
493  /* propagate current state if it is time */
494  if (b2_hff_ps_counter == HFF_PRESCALER) {
495  b2_hff_ps_counter = 1;
497  struct Int32Vect3 filtered_accel_ltp;
498  struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i();
499  int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered);
500  b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x);
501  b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y);
502 #ifdef GPS_LAG
503  b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
504 #endif
505  /*
506  * propagate current state
507  */
510 
511 #ifdef GPS_LAG
512  /* increase lag counter on last saved state */
513  if (b2_hff_rb_n > 0) {
514  b2_hff_rb_last->lag_counter++;
515  }
516 
517  /* save filter state if needed */
518  if (save_counter == 0) {
519  PRINT_DBG(1, ("save current state\n"));
520  b2_hff_rb_put_state(&b2_hff_state);
521  save_counter = -1;
522  } else if (save_counter > 0) {
523  save_counter--;
524  }
525 #endif
526  }
527  } else {
529  }
530 }
531 
532 void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
533 {
535 
536 #if USE_GPS_ACC4R
537  Rgps_pos = (float) gps.pacc / 100.;
540  }
541 
542  Rgps_vel = (float) gps.sacc / 100.;
545  }
546 #endif
547 
548 #ifdef GPS_LAG
549  if (GPS_LAG_N == 0) {
550 #endif
551 
552  /* update filter state with measurement */
553  b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos);
554  b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos);
555 #if HFF_UPDATE_SPEED
556  b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel);
557  b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel);
558 #endif
559 
560 
561 #ifdef GPS_LAG
562  } else if (b2_hff_rb_n > 0) {
563  /* roll back if state was saved approx when GPS was valid */
564  lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
565  PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter,
566  lag_counter_err));
567  if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
568  b2_hff_rb_last->rollback = TRUE;
569  b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos);
570  b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos);
571 #if HFF_UPDATE_SPEED
572  b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel);
573  b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel);
574 #endif
575  past_save_counter = GPS_DT_N - 1; // + lag_counter_err;
576  PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
577  b2_hff_propagate_past(b2_hff_rb_last);
578  } else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N + 1)) {
579  /* apparently missed a GPS update, try next saved state */
580  PRINT_DBG(2, ("try next saved state\n"));
581  b2_hff_rb_drop_last();
582  b2_hff_update_gps(pos_ned, speed_ned);
583  }
584  } else if (save_counter < 0) {
585  /* ringbuffer empty -> save output filter state at next GPS validity point in time */
586  save_counter = GPS_DT_N - 1 - (GPS_LAG_N % GPS_DT_N);
587  PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
588  }
589 
590 #endif /* GPS_LAG */
591 }
592 
593 
594 void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
595 {
596  b2_hff_state.x = pos.x;
597  b2_hff_state.y = pos.y;
598  b2_hff_state.xdot = vel.x;
599  b2_hff_state.ydot = vel.y;
600 #ifdef GPS_LAG
601  while (b2_hff_rb_n > 0) {
602  b2_hff_rb_drop_last();
603  }
604  save_counter = -1;
606 #endif
607 }
608 
609 
610 /*
611  *
612  * Propagation
613  *
614  *
615 
616  F = [ 1 dt
617  0 1 ];
618 
619  B = [ dt^2/2 dt]';
620 
621  Q = [ 0.01 0
622  0 0.01];
623 
624  Xk1 = F * Xk0 + B * accel;
625 
626  Pk1 = F * Pk0 * F' + Q;
627 
628 */
629 static void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt)
630 {
631  /* update state */
632  hff_work->xdotdot = b2_hff_xdd_meas;
633  hff_work->x = hff_work->x + dt * hff_work->xdot + dt * dt / 2 * hff_work->xdotdot;
634  hff_work->xdot = hff_work->xdot + dt * hff_work->xdotdot;
635  /* update covariance */
636  const float FPF00 = hff_work->xP[0][0] + dt * (hff_work->xP[1][0] + hff_work->xP[0][1] + dt * hff_work->xP[1][1]);
637  const float FPF01 = hff_work->xP[0][1] + dt * hff_work->xP[1][1];
638  const float FPF10 = hff_work->xP[1][0] + dt * hff_work->xP[1][1];
639  const float FPF11 = hff_work->xP[1][1];
640 
641  hff_work->xP[0][0] = FPF00 + Q;
642  hff_work->xP[0][1] = FPF01;
643  hff_work->xP[1][0] = FPF10;
644  hff_work->xP[1][1] = FPF11 + Qdotdot;
645 }
646 
647 static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt)
648 {
649  /* update state */
650  hff_work->ydotdot = b2_hff_ydd_meas;
651  hff_work->y = hff_work->y + dt * hff_work->ydot + dt * dt / 2 * hff_work->ydotdot;
652  hff_work->ydot = hff_work->ydot + dt * hff_work->ydotdot;
653  /* update covariance */
654  const float FPF00 = hff_work->yP[0][0] + dt * (hff_work->yP[1][0] + hff_work->yP[0][1] + dt * hff_work->yP[1][1]);
655  const float FPF01 = hff_work->yP[0][1] + dt * hff_work->yP[1][1];
656  const float FPF10 = hff_work->yP[1][0] + dt * hff_work->yP[1][1];
657  const float FPF11 = hff_work->yP[1][1];
658 
659  hff_work->yP[0][0] = FPF00 + Q;
660  hff_work->yP[0][1] = FPF01;
661  hff_work->yP[1][0] = FPF10;
662  hff_work->yP[1][1] = FPF11 + Qdotdot;
663 }
664 
665 
666 /*
667  *
668  * Update position
669  *
670  *
671 
672  H = [1 0];
673  R = 0.1;
674  // state residual
675  y = pos_measurement - H * Xm;
676  // covariance residual
677  S = H*Pm*H' + R;
678  // kalman gain
679  K = Pm*H'*inv(S);
680  // update state
681  Xp = Xm + K*y;
682  // update covariance
683  Pp = Pm - K*H*Pm;
684 */
685 void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
686 {
687  b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x);
688  b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y);
689 }
690 
691 static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos)
692 {
693  b2_hff_x_meas = x_meas;
694 
695  const float y = x_meas - hff_work->x;
696  const float S = hff_work->xP[0][0] + Rpos;
697  const float K1 = hff_work->xP[0][0] * 1 / S;
698  const float K2 = hff_work->xP[1][0] * 1 / S;
699 
700  hff_work->x = hff_work->x + K1 * y;
701  hff_work->xdot = hff_work->xdot + K2 * y;
702 
703  const float P11 = (1. - K1) * hff_work->xP[0][0];
704  const float P12 = (1. - K1) * hff_work->xP[0][1];
705  const float P21 = -K2 * hff_work->xP[0][0] + hff_work->xP[1][0];
706  const float P22 = -K2 * hff_work->xP[0][1] + hff_work->xP[1][1];
707 
708  hff_work->xP[0][0] = P11;
709  hff_work->xP[0][1] = P12;
710  hff_work->xP[1][0] = P21;
711  hff_work->xP[1][1] = P22;
712 }
713 
714 static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos)
715 {
716  b2_hff_y_meas = y_meas;
717 
718  const float y = y_meas - hff_work->y;
719  const float S = hff_work->yP[0][0] + Rpos;
720  const float K1 = hff_work->yP[0][0] * 1 / S;
721  const float K2 = hff_work->yP[1][0] * 1 / S;
722 
723  hff_work->y = hff_work->y + K1 * y;
724  hff_work->ydot = hff_work->ydot + K2 * y;
725 
726  const float P11 = (1. - K1) * hff_work->yP[0][0];
727  const float P12 = (1. - K1) * hff_work->yP[0][1];
728  const float P21 = -K2 * hff_work->yP[0][0] + hff_work->yP[1][0];
729  const float P22 = -K2 * hff_work->yP[0][1] + hff_work->yP[1][1];
730 
731  hff_work->yP[0][0] = P11;
732  hff_work->yP[0][1] = P12;
733  hff_work->yP[1][0] = P21;
734  hff_work->yP[1][1] = P22;
735 }
736 
737 
738 /*
739  *
740  * Update velocity
741  *
742  *
743 
744  H = [0 1];
745  R = 0.1;
746  // state residual
747  yd = vx - H * Xm;
748  // covariance residual
749  S = H*Pm*H' + R;
750  // kalman gain
751  K = Pm*H'*inv(S);
752  // update state
753  Xp = Xm + K*yd;
754  // update covariance
755  Pp = Pm - K*H*Pm;
756 */
757 void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
758 {
759  b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x);
760  b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y);
761 }
762 
763 static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel)
764 {
765  b2_hff_xd_meas = vel;
766 
767  const float yd = vel - hff_work->xdot;
768  const float S = hff_work->xP[1][1] + Rvel;
769  const float K1 = hff_work->xP[0][1] * 1 / S;
770  const float K2 = hff_work->xP[1][1] * 1 / S;
771 
772  hff_work->x = hff_work->x + K1 * yd;
773  hff_work->xdot = hff_work->xdot + K2 * yd;
774 
775  const float P11 = -K1 * hff_work->xP[1][0] + hff_work->xP[0][0];
776  const float P12 = -K1 * hff_work->xP[1][1] + hff_work->xP[0][1];
777  const float P21 = (1. - K2) * hff_work->xP[1][0];
778  const float P22 = (1. - K2) * hff_work->xP[1][1];
779 
780  hff_work->xP[0][0] = P11;
781  hff_work->xP[0][1] = P12;
782  hff_work->xP[1][0] = P21;
783  hff_work->xP[1][1] = P22;
784 }
785 
786 static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel)
787 {
788  b2_hff_yd_meas = vel;
789 
790  const float yd = vel - hff_work->ydot;
791  const float S = hff_work->yP[1][1] + Rvel;
792  const float K1 = hff_work->yP[0][1] * 1 / S;
793  const float K2 = hff_work->yP[1][1] * 1 / S;
794 
795  hff_work->y = hff_work->y + K1 * yd;
796  hff_work->ydot = hff_work->ydot + K2 * yd;
797 
798  const float P11 = -K1 * hff_work->yP[1][0] + hff_work->yP[0][0];
799  const float P12 = -K1 * hff_work->yP[1][1] + hff_work->yP[0][1];
800  const float P21 = (1. - K2) * hff_work->yP[1][0];
801  const float P22 = (1. - K2) * hff_work->yP[1][1];
802 
803  hff_work->yP[0][0] = P11;
804  hff_work->yP[0][1] = P12;
805  hff_work->yP[1][0] = P21;
806  hff_work->yP[1][1] = P22;
807 }
#define Q
Definition: hf_float.c:74
unsigned short uint16_t
Definition: types.h:16
static float b2_hff_x_meas
Definition: hf_float.c:132
#define HFF_R_POS
Definition: hf_float.c:79
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
#define SAVE_DONE
Definition: hf_float.c:203
static uint8_t dest[]
Definition: w5100.c:98
#define Qdotdot
Definition: hf_float.c:75
static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel)
Definition: hf_float.c:786
uint32_t pacc
position accuracy in cm
Definition: gps.h:77
Generic transmission transport header.
Definition: transport.h:89
Horizontal filter (x,y) to estimate position and velocity.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:51
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:594
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:1090
Simple first order low pass filter with bilinear transform.
static int b2_hff_rb_n
ringbuffer fill count
Definition: hf_float.c:192
Butterworth2LowPass_int filter_y
Definition: hf_float.c:106
#define HFF_LOWPASS_CUTOFF_FREQUENCY
Definition: hf_float.c:97
#define SAVE_NOW
Definition: hf_float.c:201
static void b2_hff_init_x(float init_x, float init_xdot)
Definition: hf_float.c:317
static int save_counter
counts down the propagation steps until the filter state is saved again
Definition: hf_float.c:199
struct HfilterFloat * b2_hff_rb_last
ringbuffer read pointer
Definition: hf_float.c:191
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.
#define HFF_R_SPEED_MIN
Definition: hf_float.c:89
#define HFF_STATE_SIZE
Definition: hf_float.h:36
static float b2_hff_xd_meas
Definition: hf_float.c:128
float Rgps_pos
Definition: hf_float.c:110
static float b2_hff_y_meas
Definition: hf_float.c:133
#define FALSE
Definition: std.h:5
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:78
static void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt)
Definition: hf_float.c:629
#define PRINT_DBG(_l, _p)
Definition: hf_float.c:46
#define TRUE
Definition: std.h:4
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:43
static float b2_hff_xdd_meas
Definition: hf_float.c:124
static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt)
Definition: hf_float.c:647
Device independent GPS code (interface)
#define ACCEL_FLOAT_OF_BFP(_ai)
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:43
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:268
static uint16_t b2_hff_lost_counter
Definition: hf_float.c:207
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)...
static uint16_t b2_hff_lost_limit
Definition: hf_float.c:206
#define HFF_LOST_LIMIT
Definition: hf_float.c:205
Inertial Measurement Unit interface.
struct HfilterFloat b2_hff_state
Definition: hf_float.c:120
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:48
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:53
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
float x
Definition: hf_float.h:39
static int past_save_counter
Definition: hf_float.c:200
#define SAVING
Definition: hf_float.c:202
float y
Definition: hf_float.h:43
float xdot
Definition: hf_float.h:41
API to get/set the generic vehicle states.
static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos)
Definition: hf_float.c:691
float xdotdot
Definition: hf_float.h:42
bool_t rollback
Definition: hf_float.h:50
void b2_hff_propagate(void)
Definition: hf_float.c:470
static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel)
Definition: hf_float.c:763
float ydot
Definition: hf_float.h:45
static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos)
Definition: hf_float.c:714
#define DT_HFILTER
Definition: hf_float.c:66
void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
Definition: hf_float.c:532
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
rotation matrix
Butterworth2LowPass_int filter_x
Definition: hf_float.c:105
static void b2_hff_init_y(float init_y, float init_ydot)
Definition: hf_float.c:330
float Rgps_vel
Definition: hf_float.c:110
void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Definition: hf_float.c:685
#define HFF_R_POS_MIN
Definition: hf_float.c:82
#define HFF_R_SPEED
Definition: hf_float.c:86
static int lag_counter_err
by how many steps the estimated GPS validity point in time differed from GPS_LAG_N ...
Definition: hf_float.c:196
static int b2_hff_ps_counter
counter for hff propagation
Definition: hf_float.c:136
static float b2_hff_ydd_meas
Definition: hf_float.c:125
struct GpsState gps
global GPS state
Definition: gps.c:41
uint8_t lag_counter
Definition: hf_float.h:49
static float b2_hff_yd_meas
Definition: hf_float.c:129
Butterworth2LowPass_int filter_z
Definition: hf_float.c:107
void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:757
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:47
#define INIT_PXX
initial covariance diagonal
Definition: hf_float.c:69
float ydotdot
Definition: hf_float.h:46
if(PrimarySpektrumState.SpektrumTimer)