Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
hf_float.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #ifndef HF_FLOAT_H
30 #define HF_FLOAT_H
31 
32 #include "std.h"
34 #include "generated/airframe.h"
35 
36 #define HFF_STATE_SIZE 2
37 
38 #ifndef AHRS_PROPAGATE_FREQUENCY
39 #define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
40 #endif
41 
42 #ifndef HFF_PRESCALER
43 #if AHRS_PROPAGATE_FREQUENCY == 512
44 #define HFF_PRESCALER 16
45 #elif AHRS_PROPAGATE_FREQUENCY == 500
46 #define HFF_PRESCALER 10
47 #else
48 #error "HFF_PRESCALER needs to be a divisor of AHRS_PROPAGATE_FREQUENCY"
49 #endif
50 #endif
51 
52 /* horizontal filter propagation frequency */
53 #define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY/HFF_PRESCALER)
54 #define DT_HFILTER (1./HFF_FREQ)
55 
56 #define HFF_UPDATE_SPEED
57 
58 struct HfilterFloat {
59  float x;
60  /* float xbias; */
61  float xdot;
62  float xdotdot;
63  float y;
64  /* float ybias; */
65  float ydot;
66  float ydotdot;
70  bool_t rollback;
71 };
72 
73 extern struct HfilterFloat b2_hff_state;
74 
75 extern float b2_hff_x_meas;
76 extern float b2_hff_y_meas;
77 extern float b2_hff_xd_meas;
78 extern float b2_hff_yd_meas;
79 extern float b2_hff_xdd_meas;
80 extern float b2_hff_ydd_meas;
81 
82 extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
83 extern void b2_hff_propagate(void);
84 extern void b2_hff_update_gps(void);
85 extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
86 extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
87 extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
88 
89 #define HFF_LOST_LIMIT 1000
92 
93 extern void b2_hff_store_accel_body(void);
94 
95 extern struct HfilterFloat *b2_hff_rb_last;
96 extern int lag_counter_err;
97 extern int save_counter;
98 
99 #endif /* HF_FLOAT_H */
unsigned short uint16_t
Definition: types.h:16
struct HfilterFloat * b2_hff_rb_last
Definition: hf_float.c:202
float b2_hff_y_meas
Definition: hf_float.c:99
float xdot
Definition: hf_float.h:61
float yP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:68
uint8_t lag_counter
Definition: hf_float.h:69
bool_t rollback
Definition: hf_float.h:70
float x
Definition: hf_float.h:59
float xdotdot
Definition: hf_float.h:62
#define HFF_STATE_SIZE
Definition: hf_float.h:36
void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
Definition: hf_float.c:242
void b2_hff_propagate(void)
Definition: hf_float.c:426
void b2_hff_update_gps(void)
Definition: hf_float.c:493
float b2_hff_xd_meas
Definition: hf_float.c:94
float ydot
Definition: hf_float.h:65
uint16_t b2_hff_lost_counter
Definition: hf_float.c:217
uint16_t b2_hff_lost_limit
Definition: hf_float.c:216
Paparazzi floating point algebra.
int lag_counter_err
Definition: hf_float.c:207
float xP[HFF_STATE_SIZE][HFF_STATE_SIZE]
Definition: hf_float.h:67
float b2_hff_xdd_meas
Definition: hf_float.c:90
void b2_hff_store_accel_body(void)
Definition: hf_float.c:119
float ydotdot
Definition: hf_float.h:66
float b2_hff_ydd_meas
Definition: hf_float.c:91
unsigned char uint8_t
Definition: types.h:14
float y
Definition: hf_float.h:63
void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
Definition: hf_float.c:717
void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos)
Definition: hf_float.c:646
struct HfilterFloat b2_hff_state
Definition: hf_float.c:86
float b2_hff_x_meas
Definition: hf_float.c:98
float b2_hff_yd_meas
Definition: hf_float.c:95
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
Definition: hf_float.c:558
int save_counter
Definition: hf_float.c:210