[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4037] hfilter realignment possible and better debug
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [4037] hfilter realignment possible and better debug msg handling |
|
Date: |
Mon, 31 Aug 2009 18:47:35 +0000 |
Revision: 4037
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4037
Author: flixr
Date: 2009-08-31 18:47:32 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
hfilter realignment possible and better debug msg handling
Modified Paths:
--------------
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2009-08-31 18:47:17 UTC
(rev 4036)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2009-08-31 18:47:32 UTC
(rev 4037)
@@ -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">
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:47:17 UTC
(rev 4036)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:47:32 UTC
(rev 4037)
@@ -42,7 +42,12 @@
#include "ins/booz2_hf_float.h"
#endif
+#ifdef SITL
+#include "nps_fdm.h"
+#include <stdio.h>
+#endif
+
#include "math/pprz_geodetic_int.h"
#include "flight_plan.h"
@@ -57,6 +62,7 @@
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 @@
#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 @@
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);
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2009-08-31 18:47:17 UTC
(rev 4036)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2009-08-31 18:47:32 UTC
(rev 4037)
@@ -55,6 +55,7 @@
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 );
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-08-31
18:47:17 UTC (rev 4036)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-08-31
18:47:32 UTC (rev 4037)
@@ -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 @@
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 @@
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 @@
} 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 @@
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_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 @@
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 @@
/* 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 @@
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 @@
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,7 +428,21 @@
}
+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
+}
+
/*
*
* Propagation
Modified: paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h 2009-08-31
18:47:17 UTC (rev 4036)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h 2009-08-31
18:47:32 UTC (rev 4037)
@@ -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 {
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4037] hfilter realignment possible and better debug msg handling,
Felix Ruess <=