Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

hfilter realignment possible and better debug msg handling

Browse Source
This commit is contained in:
Felix Ruess
2009-08-31 18:47:32 +00:00
parent 45b07cf6b0
commit 5ea27c4841
5 changed files with 71 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/settings_booz2.xml
+8 -14
View File
@@ -34,24 +34,18 @@
<dl_setting var="booz2_guidance_v_kp" min="-600" step="1" max="0" module="guidance/booz2_guidance_v" shortname="kp"/>
<dl_setting var="booz2_guidance_v_kd" min="-600" step="1" max="0" module="guidance/booz2_guidance_v" shortname="kd"/>
<dl_setting var="booz2_guidance_v_ki" min="-300" step="1" max="0" module="guidance/booz2_guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="booz2_guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/booz2_guidance_v"
shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz2_guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/booz2_guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz_ins_vff_realign" min="0" step="1" max="1" module="booz2_ins" shortname="vff_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
<dl_setting var="booz2_guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/booz2_guidance_h"
shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz2_guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/booz2_guidance_h"
shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz2_guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5" module="guidance/booz2_guidance_h"
shortname="sp_psi" unit="1/2^20r" alt_unit="deg" alt_unit_coef="0.000054641513360"/>
<dl_setting var="booz2_guidance_h_pgain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h"
shortname="kp"/>
<dl_setting var="booz2_guidance_h_dgain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h"
shortname="kd"/>
<dl_setting var="booz2_guidance_h_igain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h"
shortname="ki" handler="SetKi"/>
<dl_setting var="booz2_guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1" module="guidance/booz2_guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz2_guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1" module="guidance/booz2_guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz2_guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5" module="guidance/booz2_guidance_h" shortname="sp_psi" unit="1/2^20r" alt_unit="deg" alt_unit_coef="0.000054641513360"/>
<dl_setting var="booz2_guidance_h_pgain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="kp"/>
<dl_setting var="booz2_guidance_h_dgain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="kd"/>
<dl_setting var="booz2_guidance_h_igain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="booz_ins_hff_realign" min="0" step="1" max="1" module="booz2_ins" shortname="hff_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Analog Baro">
sw/airborne/booz/booz2_ins.c
+18
View File
@@ -42,6 +42,11 @@
#include "ins/booz2_hf_float.h"
#endif
#ifdef SITL
#include "nps_fdm.h"
#include <stdio.h>
#endif
#include "math/pprz_geodetic_int.h"
@@ -57,6 +62,7 @@ struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
struct FloatVect2 booz_ins_gps_pos_m_ned;
struct FloatVect2 booz_ins_gps_speed_m_s_ned;
#endif
bool_t booz_ins_hff_realign;
/* barometer */
#ifdef USE_VFF
@@ -91,6 +97,7 @@ void booz_ins_init() {
#endif
#ifdef USE_HFF
b2_hff_init(0., 0., 0., 0.);
booz_ins_hff_realign = FALSE;
#endif
INT32_VECT3_ZERO(booz_ins_ltp_pos);
INT32_VECT3_ZERO(booz_ins_ltp_speed);
@@ -195,6 +202,17 @@ void booz_ins_update_gps(void) {
VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
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.);
if (booz_ins_hff_realign) {
booz_ins_hff_realign = FALSE;
#ifdef SITL
struct FloatVect2 true_pos, true_speed;
VECT2_COPY(true_pos, fdm.ltpprz_pos);
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
b2_hff_realign(true_pos, true_speed);
#else
b2_hff_realign(booz_ins_gps_pos_m_ned, booz_ins_gps_speed_m_s_ned);
#endif
}
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
+1
View File
@@ -55,6 +55,7 @@ extern struct EnuCoor_i booz_ins_enu_accel;
extern struct FloatVect2 booz_ins_gps_pos_m_ned;
extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
#endif
extern bool_t booz_ins_hff_realign;
extern void booz_ins_init( void );
extern void booz_ins_periodic( void );
sw/airborne/booz/ins/booz2_hf_float.c
+40 -33
View File
@@ -27,16 +27,19 @@
#include "booz_imu.h"
#include "booz_ahrs.h"
#include <stdlib.h>
#ifdef SITL
#include <stdio.h>
#define DBG_LEVEL 1
#define PRINT_DBG(_l, _p) { \
if (DBG_LEVEL >= _l) \
printf _p; \
}
#else
#define PRINT_DBG(_l, _p) {}
#endif
#define PRINT_DBG 0
/* horizontal filter propagation frequency */
#define HFF_FREQ (512./HFF_PRESCALER)
#define DT_HFILTER (1./HFF_FREQ)
/* initial covariance diagonal */
#define INIT_PXX 1.
/* process noise (is the same for x and y)*/
@@ -225,12 +228,10 @@ void b2_hff_store_accel(float x, float y) {
static inline void b2_hff_get_past_accel(unsigned int back_n) {
int i;
if (back_n > acc_buf_n) {
if (PRINT_DBG)
printf("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n);
PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n));
back_n = acc_buf_n;
} else if (back_n == 0) {
if (PRINT_DBG)
printf("Cannot go back zero steps!\n");
PRINT_DBG(1, ("Cannot go back zero steps!\n"));
return;
}
if ((int)(acc_buf_w - back_n) < 0)
@@ -239,6 +240,7 @@ static inline void b2_hff_get_past_accel(unsigned int back_n) {
i = acc_buf_w - back_n;
b2_hff_xdd_meas = past_accel[i].x;
b2_hff_ydd_meas = past_accel[i].y;
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, acc_buf_w, back_n, i, b2_hff_xdd_meas, b2_hff_ydd_meas));
}
static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
@@ -256,8 +258,7 @@ static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
} else {
INC_RB_POINTER(b2_hff_rb_last);
}
if (PRINT_DBG)
printf("put state. fill count now: %d\n", b2_hff_rb_n);
PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
}
static inline void b2_hff_rb_drop_last(void) {
@@ -265,13 +266,11 @@ static inline void b2_hff_rb_drop_last(void) {
INC_RB_POINTER(b2_hff_rb_last);
b2_hff_rb_n--;
} else {
if (PRINT_DBG)
printf("hff ringbuffer empty!\n");
PRINT_DBG(2, ("hff ringbuffer empty!\n"));
b2_hff_rb_last->lag_counter = 0;
b2_hff_rb_last->rollback = FALSE;
}
if (PRINT_DBG)
printf("drop last state. fill count now: %d\n", b2_hff_rb_n);
PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n));
}
@@ -292,28 +291,24 @@ static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFlo
}
static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
if (PRINT_DBG)
printf("enter propagate past: %d\n", hff_past->lag_counter);
PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter));
/* run max MAX_PP_STEPS propagation steps */
for (int i=0; i < MAX_PP_STEPS; i++) {
if (hff_past->lag_counter > 0) {
b2_hff_get_past_accel(hff_past->lag_counter);
//if (PRINT_DBG)
//printf("propagate past: %d\n", hff_past->lag_counter);
PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
b2_hff_propagate_x(hff_past);
b2_hff_propagate_y(hff_past);
hff_past->lag_counter--;
if (past_save_counter == 0) {
/* next GPS measurement valid at this state -> save */
if (PRINT_DBG)
printf("save past state\n");
PRINT_DBG(2, ("save past state\n"));
b2_hff_rb_put_state(hff_past);
past_save_counter = -1;
} else if (past_save_counter > 0) {
past_save_counter--;
//if (PRINT_DBG)
//printf("dec past_save_counter: %d\n", past_save_counter);
PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter));
} else {
/* increase lag counter on if next state is already saved */
if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1])
@@ -366,6 +361,7 @@ void b2_hff_propagate(void) {
b2_hff_propagate_x(&b2_hff_state);
b2_hff_propagate_y(&b2_hff_state);
}
#ifdef GPS_LAG
/* increase lag counter on last saved state */
if (b2_hff_rb_n > 0)
@@ -373,8 +369,7 @@ void b2_hff_propagate(void) {
/* save filter state if needed */
if (save_counter == 0) {
if (PRINT_DBG)
printf("save current state\n");
PRINT_DBG(1, ("save current state\n"));
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
@@ -396,8 +391,7 @@ void b2_hff_update_gps(void) {
if (b2_hff_rb_n > 0) {
/* roll back if state was saved approx when GPS was valid */
lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
//if (PRINT_DBG)
//printf("update. rb_n: %d lag cnt err: %d\n", b2_hff_rb_n, lag_counter_err);
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, lag_counter_err));
if (abs(lag_counter_err) < 3) {
b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x);
@@ -407,19 +401,18 @@ void b2_hff_update_gps(void) {
b2_hff_update_ydot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.y);
#endif
past_save_counter = GPS_DT_N-1 + lag_counter_err;
if (PRINT_DBG)
printf("gps updated. past_save_counter: %d\n", past_save_counter);
PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
b2_hff_propagate_past(b2_hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N-2) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps();
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
if (PRINT_DBG)
printf("rb empty, save counter set: %d\n", save_counter);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#else /* GPS_LAG */
@@ -435,6 +428,20 @@ void b2_hff_update_gps(void) {
}
void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 speed) {
b2_hff_state.x = pos.x;
b2_hff_state.y = pos.y;
b2_hff_state.xdot = speed.x;
b2_hff_state.ydot = speed.y;
#ifdef GPS_LAG
while (b2_hff_rb_n > 0) {
b2_hff_rb_drop_last();
}
save_counter = -1;
past_save_counter = -1;
#endif
}
/*
*
sw/airborne/booz/ins/booz2_hf_float.h
+4
View File
@@ -33,6 +33,10 @@
#define HFF_PRESCALER 16
#endif
/* horizontal filter propagation frequency */
#define HFF_FREQ (512./HFF_PRESCALER)
#define DT_HFILTER (1./HFF_FREQ)
#define B2_HFF_UPDATE_SPEED
struct HfilterFloat {