Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

store hfilter state in struct. Filter operates on state the "working"

Browse Source 
pointer indicates.
This commit is contained in:
Felix Ruess
2009-08-31 18:46:44 +00:00
parent 35691e9262
commit 4f3310ee63
4 changed files with 295 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_ins.c
+12 -12
View File
@@ -153,12 +153,12 @@ void booz_ins_propagate() {
/* propagate horizontal filter */
b2_hff_propagate(x_accel_mean_f, y_accel_mean_f);
/* update ins state from horizontal filter */
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
}
} else {
@@ -216,12 +216,12 @@ void booz_ins_update_gps(void) {
#ifdef USE_HFF
b2_hff_update_gps();
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifndef USE_VFF /* only hf */
booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
sw/airborne/booz/booz2_telemetry.h
+15 -15
View File
@@ -457,28 +457,28 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
&b2_hff_x_meas, \
&b2_hff_xdotdot, \
&b2_hff_x, \
&b2_hff_xdot, \
&b2_hff_xbias, \
& b2_hff_xP[0][0], \
& b2_hff_xP[1][1], \
& b2_hff_xP[2][2]); \
&b2_hff_state.xdotdot, \
&b2_hff_state.x, \
&b2_hff_state.xdot, \
&b2_hff_state.xbias, \
&b2_hff_state.xP[0][0], \
&b2_hff_state.xP[1][1], \
&b2_hff_state.xP[2][2]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
&b2_hff_y_meas, \
&b2_hff_ydotdot, \
&b2_hff_y, \
&b2_hff_ydot, \
&b2_hff_ybias, \
& b2_hff_yP[0][0], \
& b2_hff_yP[1][1], \
& b2_hff_yP[2][2]); \
&b2_hff_state.ydotdot, \
&b2_hff_state.y, \
&b2_hff_state.ydot, \
&b2_hff_state.ybias, \
&b2_hff_state.yP[0][0], \
&b2_hff_state.yP[1][1], \
&b2_hff_state.yP[2][2]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
&lag_counter, \
&b2_hff_save.lag_counter, \
&lag_counter_err, \
&save_counter); \
}
sw/airborne/booz/ins/booz2_hf_float.c
+239 -210
View File
File diff suppressed because it is too large Load Diff
sw/airborne/booz/ins/booz2_hf_float.h
+29 -11
View File
@@ -35,18 +35,36 @@
#define B2_HFF_UPDATE_SPEED
#define B2_HFF_UPDATE_POS
extern float b2_hff_x;
extern float b2_hff_xbias;
extern float b2_hff_xdot;
extern float b2_hff_xdotdot;
struct hfilter_f {
float x;
float xbias;
float xdot;
float xdotdot;
float y;
float ybias;
float ydot;
float ydotdot;
float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
uint8_t lag_counter;
};
extern float b2_hff_y;
extern float b2_hff_ybias;
extern float b2_hff_ydot;
extern float b2_hff_ydotdot;
extern struct hfilter_f b2_hff_state;
extern struct hfilter_f b2_hff_save;
extern struct hfilter_f *b2_hff_work;
extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
/* extern float b2_hff_x; */
/* extern float b2_hff_xbias; */
/* extern float b2_hff_xdot; */
/* extern float b2_hff_xdotdot; */
/* extern float b2_hff_y; */
/* extern float b2_hff_ybias; */
/* extern float b2_hff_ydot; */
/* extern float b2_hff_ydotdot; */
/* extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
/* extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE]; */
extern float b2_hff_x_meas;
extern float b2_hff_y_meas;
@@ -59,7 +77,7 @@ extern void b2_hff_update_v(float xspeed, float yspeed);
#ifdef GPS_LAG
extern void b2_hff_store_accel(float x, float y);
extern uint8_t lag_counter;
/* extern uint8_t lag_counter; */
extern int8_t lag_counter_err;
extern int8_t save_counter;
#endif