Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
hf_float.h
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #ifndef HF_FLOAT_H
25 #define HF_FLOAT_H
26 
27 #include "std.h"
29 
30 #define HFF_STATE_SIZE 2
31 
32 #ifndef AHRS_PROPAGATE_FREQUENCY
33 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
34 #endif
35 
36 #ifndef HFF_PRESCALER
37 #if AHRS_PROPAGATE_FREQUENCY == 512
38 #define HFF_PRESCALER 16
39 #elif AHRS_PROPAGATE_FREQUENCY == 500
40 #define HFF_PRESCALER 10
41 #else
42 #error "HFF_PRESCALER needs to be a divisor of AHRS_PROPAGATE_FREQUENCY"
43 #endif
44 #endif
45 
46 /* horizontal filter propagation frequency */
47 #define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY/HFF_PRESCALER)
48 #define DT_HFILTER (1./HFF_FREQ)
49 
50 #define HFF_UPDATE_SPEED
51 
52 struct HfilterFloat {
53  float x;
54  /* float xbias; */
55  float xdot;
56  float xdotdot;
57  float y;
58  /* float ybias; */
59  float ydot;
60  float ydotdot;
64  bool_t rollback;
65 };
66 
67 extern struct HfilterFloat b2_hff_state;
68 
69 extern float b2_hff_x_meas;
70 extern float b2_hff_y_meas;
71 extern float b2_hff_xd_meas;
72 extern float b2_hff_yd_meas;
73 extern float b2_hff_xdd_meas;
74 extern float b2_hff_ydd_meas;
75 
76 extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
77 extern void b2_hff_propagate(void);
78 extern void b2_hff_update_gps(void);
79 extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
80 extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
81 extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
82 
83 #define HFF_LOST_LIMIT 1000
86 
87 extern void b2_hff_store_accel_body(void);
88 
89 extern struct HfilterFloat *b2_hff_rb_last;
90 extern int lag_counter_err;
91 extern int save_counter;
92 
93 #endif /* HF_FLOAT_H */
unsigned short uint16_t
Definition: types.h:16
struct HfilterFloat * b2_hff_rb_last
Definition: hf_float.c:197
float b2_hff_y_meas
Definition: hf_float.c:94
float xdot
Definition: hf_float.h:55
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:62
uint8_t lag_counter
Definition: hf_float.h:63
bool_t rollback
Definition: hf_float.h:64
float x
Definition: hf_float.h:53
float xdotdot
Definition: hf_float.h:56
#define HFF_STATE_SIZE
Definition: hf_float.h:30
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:237
void b2_hff_propagate(void)
Definition: hf_float.c:421
void b2_hff_update_gps(void)
Definition: hf_float.c:487
float b2_hff_xd_meas
Definition: hf_float.c:89
float ydot
Definition: hf_float.h:59
uint16_t b2_hff_lost_counter
Definition: hf_float.c:212
uint16_t b2_hff_lost_limit
Definition: hf_float.c:211
Paparazzi floating point algebra.
int lag_counter_err
Definition: hf_float.c:202
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:61
float b2_hff_xdd_meas
Definition: hf_float.c:85
void b2_hff_store_accel_body(void)
Definition: hf_float.c:114
float ydotdot
Definition: hf_float.h:60
float b2_hff_ydd_meas
Definition: hf_float.c:86
unsigned char uint8_t
Definition: types.h:14
float y
Definition: hf_float.h:57
void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:711
void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Definition: hf_float.c:640
struct HfilterFloat b2_hff_state
Definition: hf_float.c:81
float b2_hff_x_meas
Definition: hf_float.c:93
float b2_hff_yd_meas
Definition: hf_float.c:90
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:552
int save_counter
Definition: hf_float.c:205