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

added mean accel measurement to hff message.

Browse Source
This commit is contained in:
Felix Ruess
2009-08-21 13:47:37 +00:00
parent 2b21e6814f
commit 6801c13e4e
3 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+4 -2
View File
@@ -1065,7 +1065,8 @@
</message>
<message name="BOOZ2_HFF_X" id="164">
<field name="measure" type="float"/>
<field name="x_measure" type="float"/>
<field name="xdd" type="float"/>
<field name="x" type="float"/>
<field name="xd" type="float"/>
<field name="xbias" type="float"/>
@@ -1075,7 +1076,8 @@
</message>
<message name="BOOZ2_HFF_Y" id="165">
<field name="measure" type="float"/>
<field name="y_measure" type="float"/>
<field name="ydd" type="float"/>
<field name="y" type="float"/>
<field name="yd" type="float"/>
<field name="ybias" type="float"/>
sw/airborne/booz/booz2_telemetry.h
+2
View File
@@ -457,6 +457,7 @@ 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, \
@@ -467,6 +468,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#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, \
sw/airborne/booz/ins/booz2_hf_float.c
+3 -3
View File
@@ -44,8 +44,8 @@ X_y = [ y ydot ybias ]
#define Qdotdot ACCEL_NOISE*DT_HFILTER
#define Qbiasbias 1e-7*HFF_PRESCALER
//TODO: proper measurement noise
#define Rpos 1.
#define Rspeed 2.
#define Rpos 5.
#define Rspeed 1.
float b2_hff_x;
float b2_hff_xbias;
@@ -135,7 +135,7 @@ void b2_hff_propagate(float xaccel, float yaccel) {
static inline void b2_hff_propagate_x(float xaccel) {
/* update state */
b2_hff_xdotdot = xaccel - b2_hff_xbias;
b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot;
b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot + DT_HFILTER * DT_HFILTER / 2 * b2_hff_xdotdot;
b2_hff_xdot = b2_hff_xdot + DT_HFILTER * b2_hff_xdotdot;
/* update covariance */
const float FPF00 = b2_hff_xP[0][0] + DT_HFILTER * ( b2_hff_xP[1][0] + b2_hff_xP[0][1] + DT_HFILTER * b2_hff_xP[1][1] );