[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4036] lots of fixes for horizontal filter, ins
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [4036] lots of fixes for horizontal filter, ins |
|
Date: |
Mon, 31 Aug 2009 18:47:20 +0000 |
Revision: 4036
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4036
Author: flixr
Date: 2009-08-31 18:47:17 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
lots of fixes for horizontal filter, ins
Modified Paths:
--------------
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c
paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2009-08-31 18:46:59 UTC (rev 4035)
+++ paparazzi3/trunk/conf/messages.xml 2009-08-31 18:47:17 UTC (rev 4036)
@@ -1066,26 +1066,28 @@
<message name="BOOZ2_HFF_X" id="164">
<field name="x_measure" type="float"/>
- <field name="xdd" type="float"/>
+ <field name="xd_measure" type="float"/>
<field name="x" type="float"/>
<field name="xd" type="float"/>
+ <field name="xdd" type="float"/>
<field name="Pxx" type="float"/>
<field name="Pxdxd" type="float"/>
</message>
<message name="BOOZ2_HFF_Y" id="165">
<field name="y_measure" type="float"/>
- <field name="ydd" type="float"/>
+ <field name="yd_measure" type="float"/>
<field name="y" type="float"/>
<field name="yd" type="float"/>
+ <field name="ydd" type="float"/>
<field name="Pyy" type="float"/>
<field name="Pydyd" type="float"/>
</message>
<message name="BOOZ2_HFF_GPS" id="166">
- <field name="lag_cnt" type="uint8"/>
- <field name="lag_cnt_err" type="int8"/>
- <field name="save_cnt" type="int8"/>
+ <field name="lag_cnt" type="uint16"/>
+ <field name="lag_cnt_err" type="int16"/>
+ <field name="save_cnt" type="int16"/>
</message>
<message name="EKF7_XHAT" id="170">
@@ -1304,7 +1306,7 @@
<field name="yaw_gamma_d" type="float" />
<field name="yaw_gamma_i" type="float" />
</message>
-
+
<message name="BOOZ_AHRS_LKF" id="193">
<field name="phi" type="float" unit="rad" alt_unit="degres"
alt_unit_coef="57.29578" />
<field name="theta" type="float" unit="rad" alt_unit="degres"
alt_unit_coef="57.29578" />
@@ -1323,7 +1325,7 @@
<field name="my" type="float" />
<field name="mz" type="float" />
</message>
-
+
<message name="BOOZ_AHRS_LKF_DEBUG" id="194">
<field name="phi_err" type="float" unit="rad" alt_unit="degres"
alt_unit_coef="57.29578" />
<field name="theta_err" type="float" unit="rad" alt_unit="degres"
alt_unit_coef="57.29578" />
@@ -1341,7 +1343,7 @@
<field name="bq_cov" type="float" />
<field name="br_cov" type="float" />
</message>
-
+
<message name="BOOZ_AHRS_LKF_ACC_DBG" id="195">
<field name="qi_err" type="float" />
<field name="qx_err" type="float" />
@@ -1351,7 +1353,7 @@
<field name="bq_err" type="float" unit="rad/s" alt_unit="degres/s"
alt_unit_coef="57.29578" />
<field name="br_err" type="float" unit="rad/s" alt_unit="degres/s"
alt_unit_coef="57.29578" />
</message>
-
+
<message name="BOOZ_AHRS_LKF_MAG_DBG" id="196">
<field name="qi_err" type="float" />
<field name="qx_err" type="float" />
@@ -1361,7 +1363,7 @@
<field name="bq_err" type="float" unit="rad/s" alt_unit="degres/s"
alt_unit_coef="57.29578" />
<field name="br_err" type="float" unit="rad/s" alt_unit="degres/s"
alt_unit_coef="57.29578" />
</message>
-
+
<message name="BOOZ_SIM_SENSORS_SCALED" id="197">
<field name="acc_x" type="float" />
<field name="acc_y" type="float" />
@@ -1371,6 +1373,18 @@
<field name="mag_z" type="float" />
</message>
+ <message name="BOOZ_INS" id="198">
+ <field name="ins_x" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
+ <field name="ins_y" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
+ <field name="ins_z" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
+ <field name="ins_xd" type="int32" alt_unit="m/s"
alt_unit_coef="0.0000019"/>
+ <field name="ins_yd" type="int32" alt_unit="m/s"
alt_unit_coef="0.0000019"/>
+ <field name="ins_zd" type="int32" alt_unit="m/s"
alt_unit_coef="0.0000019"/>
+ <field name="ins_xdd" type="int32" alt_unit="m/s2"
alt_unit_coef="0.0009766"/>
+ <field name="ins_ydd" type="int32" alt_unit="m/s2"
alt_unit_coef="0.0009766"/>
+ <field name="ins_zdd" type="int32" alt_unit="m/s2"
alt_unit_coef="0.0009766"/>
+ </message>
+
</class>
Modified: paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
===================================================================
--- paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
2009-08-31 18:46:59 UTC (rev 4035)
+++ paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
2009-08-31 18:47:17 UTC (rev 4036)
@@ -27,7 +27,7 @@
#include "airframe.h"
-#if 1
+#if 0
#define NPS_BODY_TO_IMU_PHI RadOfDeg(4.)
#define NPS_BODY_TO_IMU_THETA RadOfDeg(3.)
#define NPS_BODY_TO_IMU_PSI RadOfDeg(0.)
@@ -63,8 +63,8 @@
-/*
- * Gyrometer
+/*
+ * Gyrometer
*/
#define NPS_GYRO_RESOLUTION 65536
@@ -85,9 +85,9 @@
#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
-#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
/* s */
#define NPS_GYRO_DT (1./512.)
@@ -148,23 +148,23 @@
#define NPS_GPS_POS_BIAS_INITIAL_X 0.
#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
#define NPS_GPS_POS_LATENCY 0.
#else
-#define NPS_GPS_SPEED_NOISE_STD_DEV 1e-1
-#define NPS_GPS_SPEED_LATENCY 0.25
-#define NPS_GPS_POS_NOISE_STD_DEV 1e-1
+#define NPS_GPS_SPEED_NOISE_STD_DEV 1
+#define NPS_GPS_SPEED_LATENCY 0.8
+#define NPS_GPS_POS_NOISE_STD_DEV 2
#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
-#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
-#define NPS_GPS_POS_LATENCY 0.25
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
+#define NPS_GPS_POS_LATENCY 0.8
#endif /* GPS_PERFECT */
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2009-08-31
18:46:59 UTC (rev 4035)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2009-08-31
18:47:17 UTC (rev 4036)
@@ -22,15 +22,15 @@
<message name="BOOZ2_RADIO_CONTROL" period="0.5"/>
<message name="BOOZ_STATUS" period="1"/>
</mode>
-
+
<mode name="raw_sensors">
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
- <message name="IMU_MAG_RAW" period=".05"/>
- <message name="BOOZ2_BARO_RAW" period=".1"/>
+ <message name="IMU_MAG_RAW" period=".05"/>
+ <message name="BOOZ2_BARO_RAW" period=".1"/>
</mode>
<mode name="scaled_sensors">
@@ -90,6 +90,7 @@
<message name="BOOZ2_HFF_X" period=".05"/>
<message name="BOOZ2_HFF_Y" period=".05"/>
<message name="BOOZ2_HFF_GPS" period=".03"/>
+ <message name="BOOZ_INS" period=".1"/>
</mode>
<mode name="aligner">
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:46:59 UTC
(rev 4035)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2009-08-31 18:47:17 UTC
(rev 4036)
@@ -148,7 +148,7 @@
booz_ins_ltp_accel.x = accel_ltp.x;
booz_ins_ltp_accel.y = accel_ltp.y;
#endif /* USE_HFF */
-
+
INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
@@ -213,7 +213,7 @@
VECT2_COPY(d_pos, booz_ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
VECT2_ADD(booz_ins_ltp_pos, d_pos);
-#endif
+#endif
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2009-08-31 18:46:59 UTC
(rev 4035)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2009-08-31 18:47:17 UTC
(rev 4036)
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
+ * Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_INS_H
@@ -39,7 +39,7 @@
extern int32_t booz_ins_baro_alt;
extern int32_t booz_ins_qfe;
extern bool_t booz_ins_baro_initialised;
-extern bool_t booz_ins_vff_realign;
+extern bool_t booz_ins_vff_realign;
#endif
/* output LTP NED */
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-08-31 18:46:59 UTC
(rev 4035)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-08-31 18:47:17 UTC
(rev 4036)
@@ -456,22 +456,25 @@
#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, \
- &b2_hff_state.x, \
- &b2_hff_state.xdot, \
- &b2_hff_state.xP[0][0], \
- &b2_hff_state.xP[1][1]); \
+ &b2_hff_x_meas,
\
+ &b2_hff_xd_meas,
\
+ &b2_hff_state.x,
\
+ &b2_hff_state.xdot,
\
+
&b2_hff_state.xdotdot, \
+
&b2_hff_state.xP[0][0], \
+
&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, \
- &b2_hff_state.y, \
- &b2_hff_state.ydot, \
- &b2_hff_state.yP[0][0], \
- &b2_hff_state.yP[1][1]); \
+ DOWNLINK_SEND_BOOZ2_HFF_Y(_chan, \
+ &b2_hff_y_meas,
\
+ &b2_hff_yd_meas,
\
+ &b2_hff_state.y,
\
+ &b2_hff_state.ydot,
\
+
&b2_hff_state.ydotdot, \
+
&b2_hff_state.yP[0][0], \
+
&b2_hff_state.yP[1][1]); \
}
+#ifdef GPS_LAG
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF_GPS(_chan, \
&b2_hff_rb_last->lag_counter, \
@@ -479,6 +482,9 @@
&save_counter);
\
}
#else
+#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
+#endif
+#else
#define PERIODIC_SEND_BOOZ2_HFF_X(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_Y(_chan) {}
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
@@ -534,6 +540,19 @@
#define PERIODIC_SEND_BOOZ2_INS3(_chan) {}
#endif /* USE_GPS */
+#define PERIODIC_SEND_BOOZ_INS(_chan) { \
+ DOWNLINK_SEND_BOOZ_INS(_chan, \
+ &booz_ins_ltp_pos.x, \
+ &booz_ins_ltp_pos.y, \
+ &booz_ins_ltp_pos.z, \
+ &booz_ins_ltp_speed.x, \
+ &booz_ins_ltp_speed.y, \
+ &booz_ins_ltp_speed.z, \
+ &booz_ins_ltp_accel.x, \
+ &booz_ins_ltp_accel.y, \
+ &booz_ins_ltp_accel.z); \
+ }
+
#define PERIODIC_SEND_BOOZ2_INS_REF(_chan) { \
DOWNLINK_SEND_BOOZ2_INS_REF(_chan, \
&booz_ins_ltp_def.ecef.x, \
Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c 2009-08-31 18:46:59 UTC
(rev 4035)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.c 2009-08-31 18:47:17 UTC
(rev 4036)
@@ -64,10 +64,14 @@
if (n > rb_n) {
n = rb_n;
}
- for (i = 0; i < n; i++) {
- j = (rb_r + i) < RB_MAXN ? rb_r + i : rb_r + i - RB_MAXN;
+ for (i = 1; i <= n; i++) {
+ j = (rb_w - i) > 0 ? rb_w - i : rb_w - i + RB_MAXN;
VECT3_ADD(sum, accel_buf[j]);
}
- VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
+ if (n > 1) {
+ VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
+ } else {
+ VECT3_COPY(booz_ahrs_accel_mean, sum);
+ }
}
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:46:59 UTC (rev 4035)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c 2009-08-31
18:47:17 UTC (rev 4036)
@@ -27,12 +27,15 @@
#include "booz_imu.h"
#include "booz_ahrs.h"
#include <stdlib.h>
+#include <stdio.h>
+#define PRINT_DBG 0
+
/* horizontal filter propagation frequency */
-#define HFF_FREQ 512./HFF_PRESCALER
-#define DT_HFILTER 1./HFF_FREQ
+#define HFF_FREQ (512./HFF_PRESCALER)
+#define DT_HFILTER (1./HFF_FREQ)
/* initial covariance diagonal */
#define INIT_PXX 1.
@@ -53,14 +56,15 @@
*/
/* output filter states */
struct HfilterFloat b2_hff_state;
-/* pointer to filter states to operate on */
-//struct HfilterFloat *b2_hff_work;
+/* last acceleration measurement */
+float b2_hff_xdd_meas;
+float b2_hff_ydd_meas;
-/* acceleration used in propagation step */
-float b2_hff_xaccel;
-float b2_hff_yaccel;
+/* last velocity measurement */
+float b2_hff_xd_meas;
+float b2_hff_yd_meas;
/* last position measurement */
float b2_hff_x_meas;
@@ -76,7 +80,6 @@
*
*
*/
-
#ifdef GPS_LAG
/*
* GPS_LAG is defined in seconds in airframe file
@@ -88,16 +91,19 @@
#define GPS_DT_N ((int) (HFF_FREQ / 4))
/* maximum number of past propagation steps to rerun per ap cycle
- * make sure GPS_LAG_N/MAX_PP_STEPS < GPS_DT_N
+ * make sure GPS_LAG_N/MAX_PP_STEPS < 128
+ * GPS_LAG_N/MAX_PP_STEPS/512 = seconds until re-propagation catches up with
the present
*/
#define MAX_PP_STEPS 6
/* variables for accel buffer */
-#define ACC_BUF_MAXN GPS_LAG_N+10
+#define ACC_BUF_MAXN (GPS_LAG_N+10)
+#define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0;
}
+
struct FloatVect2 past_accel[ACC_BUF_MAXN]; /* buffer with past mean accel
values for redoing the propagation */
-uint8_t acc_buf_r; /* pos to read from, oldest measurement */
-uint8_t acc_buf_w; /* pos to write to */
-uint8_t acc_buf_n; /* number of elements in buffer */
+unsigned int acc_buf_r; /* pos to read from, oldest measurement */
+unsigned int acc_buf_w; /* pos to write to */
+unsigned int acc_buf_n; /* number of elements in buffer */
/*
@@ -106,9 +112,9 @@
#define HFF_RB_MAXN ((int) (GPS_LAG * 4))
#define INC_RB_POINTER(ptr) { \
if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \
- ptr = b2_hff_rb;
\
- else
\
- ptr++;
\
+ ptr = b2_hff_rb;
\
+ else
\
+ ptr++;
\
}
struct HfilterFloat b2_hff_rb[HFF_RB_MAXN]; /* ringbuffer with state and
covariance when GPS was valid */
@@ -120,16 +126,16 @@
/* by how many steps the estimated GPS validity point in time differed from
GPS_LAG_N */
-int8_t lag_counter_err;
+int lag_counter_err;
/* counts down the propagation steps until the filter state is saved again */
-int8_t save_counter;
-int8_t past_save_counter;
+int save_counter;
+int past_save_counter;
#ifdef GPS_LAG
-static inline void b2_hff_get_past_accel(int back_n);
+static inline void b2_hff_get_past_accel(unsigned int back_n);
static inline void b2_hff_rb_put_state(struct HfilterFloat* source);
-static inline void b2_hff_rb_next(void);
+static inline void b2_hff_rb_drop_last(void);
static inline void b2_hff_set_state(struct HfilterFloat* dest, struct
HfilterFloat* source);
#endif
@@ -158,14 +164,24 @@
acc_buf_n = 0;
b2_hff_rb_put = b2_hff_rb;
b2_hff_rb_last = b2_hff_rb;
+ b2_hff_rb_last->rollback = FALSE;
+ b2_hff_rb_last->lag_counter = 0;
b2_hff_state.lag_counter = GPS_LAG_N;
+#ifdef SITL
+ printf("GPS_LAG: %f\n", GPS_LAG);
+ printf("GPS_LAG_N: %d\n", GPS_LAG_N);
+ printf("GPS_DT_N: %d\n", GPS_DT_N);
+ printf("DT_HFILTER: %f\n", DT_HFILTER);
+#endif
#else
- b2_hff_rb_last = NULL;
+ b2_hff_rb_last = &b2_hff_state;
b2_hff_state.lag_counter = 0;
#endif
b2_hff_rb_n = 0;
- b2_hff_state.rollback = 0;
+ b2_hff_state.rollback = FALSE;
lag_counter_err = 0;
+ save_counter = -1;
+ past_save_counter = -1;
b2_hff_ps_counter = 1;
}
@@ -196,35 +212,40 @@
void b2_hff_store_accel(float x, float y) {
past_accel[acc_buf_w].x = x;
past_accel[acc_buf_w].y = y;
- acc_buf_w = (acc_buf_w + 1) < ACC_BUF_MAXN ? (acc_buf_w + 1) : 0;
+ INC_ACC_IDX(acc_buf_w);
if (acc_buf_n < ACC_BUF_MAXN) {
acc_buf_n++;
} else {
- acc_buf_r = (acc_buf_r + 1) < ACC_BUF_MAXN ? (acc_buf_r + 1) : 0;
+ INC_ACC_IDX(acc_buf_r);
}
}
/* get the accel values from back_n steps ago */
-static inline void b2_hff_get_past_accel(int back_n) {
+static inline void b2_hff_get_past_accel(unsigned int back_n) {
int i;
if (back_n > acc_buf_n) {
- //printf("Cannot go back %d steps, going back only %d instead!\n",
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);
back_n = acc_buf_n;
- } else if (back_n <= 0) {
- //printf("Cannot go back %d steps, not getting past accel value!\n",
back_n);
+ } else if (back_n == 0) {
+ if (PRINT_DBG)
+ printf("Cannot go back zero steps!\n");
return;
}
- i = (acc_buf_w - back_n) > 0 ? (acc_buf_w - back_n) : (acc_buf_w +
ACC_BUF_MAXN - back_n);
- b2_hff_xaccel = past_accel[i].x;
- b2_hff_yaccel = past_accel[i].y;
+ if ((int)(acc_buf_w - back_n) < 0)
+ i = acc_buf_w - back_n + ACC_BUF_MAXN;
+ else
+ i = acc_buf_w - back_n;
+ b2_hff_xdd_meas = past_accel[i].x;
+ b2_hff_ydd_meas = past_accel[i].y;
}
static inline void b2_hff_rb_put_state(struct HfilterFloat* source) {
/* copy state from source into buffer */
b2_hff_set_state(b2_hff_rb_put, source);
b2_hff_rb_put->lag_counter = 0;
- b2_hff_rb_put->rollback = 0;
+ b2_hff_rb_put->rollback = FALSE;
/* forward write pointer */
INC_RB_POINTER(b2_hff_rb_put);
@@ -235,15 +256,22 @@
} else {
INC_RB_POINTER(b2_hff_rb_last);
}
+ if (PRINT_DBG)
+ printf("put state. fill count now: %d\n", b2_hff_rb_n);
}
-static inline void b2_hff_rb_next(void) {
+static inline void b2_hff_rb_drop_last(void) {
if (b2_hff_rb_n > 0) {
INC_RB_POINTER(b2_hff_rb_last);
b2_hff_rb_n--;
} else {
- //printf("hff ringbuffer empty!\n");
+ if (PRINT_DBG)
+ printf("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);
}
@@ -264,24 +292,41 @@
}
static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
+ if (PRINT_DBG)
+ printf("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++) {
- b2_hff_get_past_accel(hff_past->lag_counter);
- b2_hff_propagate_x(hff_past);
- b2_hff_propagate_y(hff_past);
- hff_past->lag_counter--;
+ 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);
+ 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 */
- b2_hff_rb_put_state(hff_past);
- past_save_counter = -1;
- } else if (save_counter > 0) {
- past_save_counter--;
+ if (past_save_counter == 0) {
+ /* next GPS measurement valid at this state -> save */
+ if (PRINT_DBG)
+ printf("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);
+ } else {
+ /* increase lag counter on if next state is already saved */
+ if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1])
+ b2_hff_rb[0].lag_counter++;
+ else
+ (hff_past+1)->lag_counter++;
+ }
}
+ /* finished re-propagating the past values */
if (hff_past->lag_counter == 0) {
b2_hff_set_state(&b2_hff_state, hff_past);
- b2_hff_rb_next();
+ b2_hff_rb_drop_last();
break;
}
}
@@ -292,7 +337,7 @@
void b2_hff_propagate(void) {
#ifdef GPS_LAG
- /* continue to redo the propagation to catch up with the present */
+ /* continue re-propagating to catch up with the present */
if (b2_hff_rb_last->rollback) {
b2_hff_propagate_past(b2_hff_rb_last);
}
@@ -308,8 +353,11 @@
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);
- b2_hff_xaccel = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
- b2_hff_yaccel = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
+ b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
+ b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
+#ifdef GPS_LAG
+ b2_hff_store_accel(b2_hff_xdd_meas, b2_hff_ydd_meas);
+#endif
/*
* propagate current state
@@ -319,14 +367,14 @@
b2_hff_propagate_y(&b2_hff_state);
}
#ifdef GPS_LAG
- b2_hff_store_accel(b2_hff_xaccel, b2_hff_yaccel);
- /* increase all lag counters */
- for (int i=0; i < b2_hff_rb_n; i++ ) {
- (b2_hff_rb_last+i)->lag_counter++;
- }
+ /* increase lag counter on last saved state */
+ if (b2_hff_rb_n > 0)
+ b2_hff_rb_last->lag_counter++;
- /* save filter state if ringbuffer is empty */
+ /* save filter state if needed */
if (save_counter == 0) {
+ if (PRINT_DBG)
+ printf("save current state\n");
b2_hff_rb_put_state(&b2_hff_state);
save_counter = -1;
} else if (save_counter > 0) {
@@ -348,24 +396,30 @@
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);
if (abs(lag_counter_err) < 3) {
- b2_hff_rb_last->rollback = 1;
+ b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x);
b2_hff_update_y(b2_hff_rb_last, booz_ins_gps_pos_m_ned.y);
#ifdef B2_HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.x);
b2_hff_update_ydot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.y);
#endif
- past_save_counter = GPS_DT_N + lag_counter_err;
+ past_save_counter = GPS_DT_N-1 + lag_counter_err;
+ if (PRINT_DBG)
+ printf("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 */
- b2_hff_rb_next();
+ b2_hff_rb_drop_last();
b2_hff_update_gps();
}
- } else {
+ } else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity
point in time */
- save_counter = GPS_DT_N - (GPS_LAG_N % GPS_DT_N);
+ 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);
}
#else /* GPS_LAG */
@@ -403,7 +457,7 @@
*/
static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
/* update state */
- hff_work->xdotdot = b2_hff_xaccel;
+ hff_work->xdotdot = b2_hff_xdd_meas;
hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot;
hff_work->xdot = hff_work->xdot + DT_HFILTER * hff_work->xdotdot;
/* update covariance */
@@ -420,8 +474,8 @@
static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) {
/* update state */
- hff_work->ydotdot = b2_hff_yaccel;
- hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot;
+ hff_work->ydotdot = b2_hff_ydd_meas;
+ hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot +
DT_HFILTER*DT_HFILTER/2 * hff_work->ydotdot;
hff_work->ydot = hff_work->ydot + DT_HFILTER * hff_work->ydotdot;
/* update covariance */
const float FPF00 = hff_work->yP[0][0] + DT_HFILTER * ( hff_work->yP[1][0] +
hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1] );
@@ -532,6 +586,8 @@
}
static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float v) {
+ b2_hff_xd_meas = v;
+
const float yd = v - hff_work->xdot;
const float S = hff_work->xP[1][1] + Rspeed;
const float K1 = hff_work->xP[0][1] * 1/S;
@@ -552,6 +608,8 @@
}
static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float v) {
+ b2_hff_yd_meas = v;
+
const float yd = v - hff_work->ydot;
const float S = hff_work->yP[1][1] + Rspeed;
const float K1 = hff_work->yP[0][1] * 1/S;
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:46:59 UTC (rev 4035)
+++ paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h 2009-08-31
18:47:17 UTC (rev 4036)
@@ -24,7 +24,8 @@
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
-#include <inttypes.h>
+#include "std.h"
+#include "math/pprz_algebra_float.h"
#define B2_HFF_STATE_SIZE 3
@@ -46,26 +47,31 @@
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;
+ bool_t rollback;
};
extern struct HfilterFloat b2_hff_state;
extern float b2_hff_x_meas;
extern float b2_hff_y_meas;
+extern float b2_hff_xd_meas;
+extern float b2_hff_yd_meas;
+extern float b2_hff_xdd_meas;
+extern float b2_hff_ydd_meas;
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);
+extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 speed);
#ifdef GPS_LAG
extern void b2_hff_store_accel(float x, float y);
#endif
extern struct HfilterFloat *b2_hff_rb_last;
-extern int8_t lag_counter_err;
-extern int8_t save_counter;
+extern int lag_counter_err;
+extern int save_counter;
#endif /* BOOZ2_HF_FLOAT_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4036] lots of fixes for horizontal filter, ins,
Felix Ruess <=