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 26 Packages Projects Releases Wiki Activity

simple horizontal filter with GPS lag compensation.

Browse Source 
include booz2_ins_hff.makefile to use the filter and define GPS_LAG in
seconds: e.g. -DGPS_LAG=0.8
removed bias estimation in hff since that was plain wrong
This commit is contained in:
Felix Ruess
2009-08-31 18:46:59 +00:00
parent 4f3310ee63
commit d70badc2bf
10 changed files with 445 additions and 465 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_flixr.xml
+3 -2
View File
@@ -203,8 +203,9 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
#enable horizontal filter
ap.CFLAGS += -DUSE_HFF -DHFF_PRESCALER=16 -DGPS_LAG=0.2
include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8
</makefile>
conf/autopilot/booz2_autopilot.makefile
+9 -2
View File
@@ -130,8 +130,15 @@ ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
ap.srcs += $(SRC_BOOZ)/booz2_ins.c
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
# horizontal filter float version
ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
#
# INS choice
#
# include booz2_ins_hff.makefile
# or
# nothing
#
# vertical filter float version
ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
conf/autopilot/booz2_simulator_nps.makefile
+7 -1
View File
@@ -138,8 +138,14 @@ sim.srcs += $(SRC_BOOZ)/booz2_ins.c
# vertical filter float version
sim.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
sim.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)"
sim.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
#
# INS choice
#
# include booz2_ins_hff.makefile
# or
# nothing
#
sim.srcs += $(SRC_BOOZ)/booz2_navigation.c
conf/autopilot/subsystems/booz2_ins_hff.makefile
+9
View File
@@ -0,0 +1,9 @@
#
# simple horizontal filter for INS
#
ap.CFLAGS += -DUSE_HFF
ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
sim.CFLAGS += -DUSE_HFF
sim.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
conf/messages.xml
-4
View File
@@ -1069,10 +1069,8 @@
<field name="xdd" type="float"/>
<field name="x" type="float"/>
<field name="xd" type="float"/>
<field name="xbias" type="float"/>
<field name="Pxx" type="float"/>
<field name="Pxdxd" type="float"/>
<field name="Pbb" type="float"/>
</message>
<message name="BOOZ2_HFF_Y" id="165">
@@ -1080,10 +1078,8 @@
<field name="ydd" type="float"/>
<field name="y" type="float"/>
<field name="yd" type="float"/>
<field name="ybias" type="float"/>
<field name="Pyy" type="float"/>
<field name="Pydyd" type="float"/>
<field name="Pbb" type="float"/>
</message>
<message name="BOOZ2_HFF_GPS" id="166">
sw/airborne/booz/booz2_ins.c
+17 -37
View File
@@ -52,9 +52,11 @@ struct LtpDef_i booz_ins_ltp_def;
bool_t booz_ins_ltp_initialised;
struct NedCoor_i booz_ins_gps_pos_cm_ned;
struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
#ifdef USE_HFF
/* horizontal gps transformed to NED in meters as float */
struct FloatVect2 booz_ins_gps_pos_m_ned;
struct FloatVect2 booz_ins_gps_speed_m_s_ned;
#endif
/* barometer */
#ifdef USE_VFF
@@ -72,11 +74,6 @@ struct EnuCoor_i booz_ins_enu_pos;
struct EnuCoor_i booz_ins_enu_speed;
struct EnuCoor_i booz_ins_enu_accel;
#ifdef USE_HFF
/* counter for hff propagation*/
uint8_t b2_hff_ps_counter;
#endif
void booz_ins_init() {
#ifdef USE_NAV_INS_INIT
@@ -93,8 +90,7 @@ void booz_ins_init() {
b2_vff_init(0., 0., 0.);
#endif
#ifdef USE_HFF
b2_hff_init(0., 0., 0., 0., 0., 0.);
b2_hff_ps_counter = 1;
b2_hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(booz_ins_ltp_pos);
INT32_VECT3_ZERO(booz_ins_ltp_speed);
@@ -135,34 +131,18 @@ void booz_ins_propagate() {
#endif /* USE_VFF */
#ifdef USE_HFF
if (b2_hff_ps_counter == HFF_PRESCALER) {
b2_hff_ps_counter = 1;
if (booz_ahrs.status == BOOZ_AHRS_RUNNING ) {
/* compute float ltp mean acceleration */
booz_ahrs_compute_accel_mean(HFF_PRESCALER);
struct Int32Vect3 mean_accel_body;
INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, booz_ahrs_accel_mean);
struct Int32Vect3 mean_accel_ltp;
INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
float x_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
float y_accel_mean_f = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
#ifdef GPS_LAG
b2_hff_store_accel(x_accel_mean_f, y_accel_mean_f);
#endif
if ( booz_ins_ltp_initialised ) {
/* 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_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);
}
if (booz_ahrs.status == BOOZ_AHRS_RUNNING ) {
/* propagate horizontal filter */
b2_hff_propagate();
if ( booz_ins_ltp_initialised ) {
/* update ins state from horizontal filter */
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 {
b2_hff_ps_counter++;
}
#else
booz_ins_ltp_accel.x = accel_ltp.x;
@@ -208,13 +188,13 @@ void booz_ins_update_gps(void) {
booz_ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x, booz_ins_gps_pos_cm_ned.y);
VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
#ifdef USE_HFF
b2_hff_update_gps();
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);
sw/airborne/booz/booz2_ins.h
+2
View File
@@ -50,9 +50,11 @@ extern struct NedCoor_i booz_ins_ltp_accel;
extern struct EnuCoor_i booz_ins_enu_pos;
extern struct EnuCoor_i booz_ins_enu_speed;
extern struct EnuCoor_i booz_ins_enu_accel;
#ifdef USE_HFF
/* horizontal gps transformed to NED in meters as float */
extern struct FloatVect2 booz_ins_gps_pos_m_ned;
extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
#endif
extern void booz_ins_init( void );
extern void booz_ins_periodic( void );
sw/airborne/booz/booz2_telemetry.h
+14 -18
View File
@@ -454,31 +454,27 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_HFF
#include "ins/booz2_hf_float.h"
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
&b2_hff_x_meas, \
&b2_hff_state.xdotdot, \
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_X(_chan, \
&b2_hff_x_meas, \
&b2_hff_state.xdotdot, \
&b2_hff_state.x, \
&b2_hff_state.xdot, \
&b2_hff_state.xbias, \
&b2_hff_state.xdot, \
&b2_hff_state.xP[0][0], \
&b2_hff_state.xP[1][1], \
&b2_hff_state.xP[2][2]); \
&b2_hff_state.xP[1][1]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
&b2_hff_y_meas, \
&b2_hff_state.ydotdot, \
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
&b2_hff_y_meas, \
&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]); \
&b2_hff_state.ydot, \
&b2_hff_state.yP[0][0], \
&b2_hff_state.yP[1][1]); \
}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
&b2_hff_save.lag_counter, \
&b2_hff_rb_last->lag_counter, \
&lag_counter_err, \
&save_counter); \
}
sw/airborne/booz/ins/booz2_hf_float.c
+377 -379
View File
File diff suppressed because it is too large Load Diff
sw/airborne/booz/ins/booz2_hf_float.h
+7 -22
View File
@@ -33,9 +33,8 @@
#endif
#define B2_HFF_UPDATE_SPEED
#define B2_HFF_UPDATE_POS
struct hfilter_f {
struct HfilterFloat {
float x;
float xbias;
float xdot;
@@ -47,40 +46,26 @@ struct hfilter_f {
float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
uint8_t lag_counter;
uint8_t rollback;
};
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_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 struct HfilterFloat b2_hff_state;
extern float b2_hff_x_meas;
extern float b2_hff_y_meas;
extern void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias);
extern void b2_hff_propagate(float xaccel, float yaccel);
extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
extern void b2_hff_propagate(void);
extern void b2_hff_update_gps(void);
extern void b2_hff_update_pos(float xpos, float ypos);
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; */
#endif
extern struct HfilterFloat *b2_hff_rb_last;
extern int8_t lag_counter_err;
extern int8_t save_counter;
#endif
#endif /* BOOZ2_HF_FLOAT_H */