[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6008] rename b2_vff/B2_VFF and booz2_vff/BOOZ2_VFF
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6008] rename b2_vff/B2_VFF and booz2_vff/BOOZ2_VFF to vff/VFF |
|
Date: |
Tue, 28 Sep 2010 14:05:27 +0000 |
Revision: 6008
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6008
Author: flixr
Date: 2010-09-28 14:05:27 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
rename b2_vff/B2_VFF and booz2_vff/BOOZ2_VFF to vff/VFF
Modified Paths:
--------------
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-09-28 14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/conf/messages.xml 2010-09-28 14:05:27 UTC (rev 6008)
@@ -1158,7 +1158,7 @@
<field name="value_filtered" type="uint16"/>
</message>
- <message name="BOOZ2_VFF" id="162">
+ <message name="VFF" id="162">
<field name="measure" type="float"/>
<field name="z" type="float"/>
<field name="zd" type="float"/>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-09-28 14:05:17 UTC
(rev 6007)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-09-28 14:05:27 UTC
(rev 6008)
@@ -75,7 +75,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
- <message name="BOOZ2_VFF" period=".05"/>
+ <message name="VFF" period=".05"/>
<message name="BOOZ2_VERT_LOOP" period=".05"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
<message name="INS" period=".05"/>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2010-09-28
14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2010-09-28
14:05:27 UTC (rev 6008)
@@ -74,7 +74,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
- <message name="BOOZ2_VFF" period=".1"/>
+ <message name="VFF" period=".1"/>
<message name="BOOZ2_VERT_LOOP" period=".1"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
<message name="INS" period=".2"/>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-28 14:05:17 UTC
(rev 6007)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-28 14:05:27 UTC
(rev 6008)
@@ -473,18 +473,18 @@
#ifdef USE_VFF
#include "ins/vf_float.h"
-#define PERIODIC_SEND_BOOZ2_VFF(_chan) { \
- DOWNLINK_SEND_BOOZ2_VFF(_chan, \
- &b2_vff_z_meas, \
- &b2_vff_z, \
- &b2_vff_zdot, \
- &b2_vff_bias, \
- & b2_vff_P[0][0], \
- & b2_vff_P[1][1], \
- & b2_vff_P[2][2]); \
+#define PERIODIC_SEND_VFF(_chan) { \
+ DOWNLINK_SEND_VFF(_chan, \
+ &vff_z_meas, \
+ &vff_z, \
+ &vff_zdot, \
+ &vff_bias, \
+ & vff_P[0][0], \
+ & vff_P[1][1], \
+ & vff_P[2][2]); \
}
#else
-#define PERIODIC_SEND_BOOZ2_VFF(_chan) {}
+#define PERIODIC_SEND_VFF(_chan) {}
#endif
#ifdef USE_HFF
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
2010-09-28 14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
2010-09-28 14:05:27 UTC (rev 6008)
@@ -41,24 +41,24 @@
#define Qbiasbias 1e-7
#define R 1.
-float b2_vff_z;
-float b2_vff_bias;
-float b2_vff_zdot;
-float b2_vff_zdotdot;
+float vff_z;
+float vff_bias;
+float vff_zdot;
+float vff_zdotdot;
-float b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE];
+float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
-float b2_vff_z_meas;
+float vff_z_meas;
-void b2_vff_init(float init_z, float init_zdot, float init_bias) {
- b2_vff_z = init_z;
- b2_vff_zdot = init_zdot;
- b2_vff_bias = init_bias;
+void vff_init(float init_z, float init_zdot, float init_bias) {
+ vff_z = init_z;
+ vff_zdot = init_zdot;
+ vff_bias = init_bias;
int i, j;
- for (i=0; i<B2_VFF_STATE_SIZE; i++) {
- for (j=0; j<B2_VFF_STATE_SIZE; j++)
- b2_vff_P[i][j] = 0.;
- b2_vff_P[i][i] = INIT_PXX;
+ for (i=0; i<VFF_STATE_SIZE; i++) {
+ for (j=0; j<VFF_STATE_SIZE; j++)
+ vff_P[i][j] = 0.;
+ vff_P[i][i] = INIT_PXX;
}
}
@@ -81,31 +81,31 @@
Pk1 = F * Pk0 * F' + Q;
*/
-void b2_vff_propagate(float accel) {
+void vff_propagate(float accel) {
/* update state */
- b2_vff_zdotdot = accel + 9.81 - b2_vff_bias;
- b2_vff_z = b2_vff_z + DT_VFILTER * b2_vff_zdot;
- b2_vff_zdot = b2_vff_zdot + DT_VFILTER * b2_vff_zdotdot;
+ vff_zdotdot = accel + 9.81 - vff_bias;
+ vff_z = vff_z + DT_VFILTER * vff_zdot;
+ vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
/* update covariance */
- const float FPF00 = b2_vff_P[0][0] + DT_VFILTER * ( b2_vff_P[1][0] +
b2_vff_P[0][1] + DT_VFILTER * b2_vff_P[1][1] );
- const float FPF01 = b2_vff_P[0][1] + DT_VFILTER * ( b2_vff_P[1][1] -
b2_vff_P[0][2] - DT_VFILTER * b2_vff_P[1][2] );
- const float FPF02 = b2_vff_P[0][2] + DT_VFILTER * ( b2_vff_P[1][2] );
- const float FPF10 = b2_vff_P[1][0] + DT_VFILTER * (-b2_vff_P[2][0] +
b2_vff_P[1][1] - DT_VFILTER * b2_vff_P[2][1] );
- const float FPF11 = b2_vff_P[1][1] + DT_VFILTER * (-b2_vff_P[2][1] -
b2_vff_P[1][2] + DT_VFILTER * b2_vff_P[2][2] );
- const float FPF12 = b2_vff_P[1][2] + DT_VFILTER * (-b2_vff_P[2][2] );
- const float FPF20 = b2_vff_P[2][0] + DT_VFILTER * ( b2_vff_P[2][1] );
- const float FPF21 = b2_vff_P[2][1] + DT_VFILTER * (-b2_vff_P[2][2] );
- const float FPF22 = b2_vff_P[2][2];
+ const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] +
DT_VFILTER * vff_P[1][1] );
+ const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] -
DT_VFILTER * vff_P[1][2] );
+ const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
+ const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] -
DT_VFILTER * vff_P[2][1] );
+ const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] +
DT_VFILTER * vff_P[2][2] );
+ const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
+ const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
+ const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
+ const float FPF22 = vff_P[2][2];
- b2_vff_P[0][0] = FPF00 + Qzz;
- b2_vff_P[0][1] = FPF01;
- b2_vff_P[0][2] = FPF02;
- b2_vff_P[1][0] = FPF10;
- b2_vff_P[1][1] = FPF11 + Qzdotzdot;
- b2_vff_P[1][2] = FPF12;
- b2_vff_P[2][0] = FPF20;
- b2_vff_P[2][1] = FPF21;
- b2_vff_P[2][2] = FPF22 + Qbiasbias;
+ vff_P[0][0] = FPF00 + Qzz;
+ vff_P[0][1] = FPF01;
+ vff_P[0][2] = FPF02;
+ vff_P[1][0] = FPF10;
+ vff_P[1][1] = FPF11 + Qzdotzdot;
+ vff_P[1][2] = FPF12;
+ vff_P[2][0] = FPF20;
+ vff_P[2][1] = FPF21;
+ vff_P[2][2] = FPF22 + Qbiasbias;
}
/*
@@ -123,45 +123,45 @@
Pp = Pm - K*H*Pm;
*/
static inline void update_z_conf(float z_meas, float conf) {
- b2_vff_z_meas = z_meas;
+ vff_z_meas = z_meas;
- const float y = z_meas - b2_vff_z;
- const float S = b2_vff_P[0][0] + conf;
- const float K1 = b2_vff_P[0][0] * 1/S;
- const float K2 = b2_vff_P[1][0] * 1/S;
- const float K3 = b2_vff_P[2][0] * 1/S;
+ const float y = z_meas - vff_z;
+ const float S = vff_P[0][0] + conf;
+ const float K1 = vff_P[0][0] * 1/S;
+ const float K2 = vff_P[1][0] * 1/S;
+ const float K3 = vff_P[2][0] * 1/S;
- b2_vff_z = b2_vff_z + K1 * y;
- b2_vff_zdot = b2_vff_zdot + K2 * y;
- b2_vff_bias = b2_vff_bias + K3 * y;
+ vff_z = vff_z + K1 * y;
+ vff_zdot = vff_zdot + K2 * y;
+ vff_bias = vff_bias + K3 * y;
- const float P11 = (1. - K1) * b2_vff_P[0][0];
- const float P12 = (1. - K1) * b2_vff_P[0][1];
- const float P13 = (1. - K1) * b2_vff_P[0][2];
- const float P21 = -K2 * b2_vff_P[0][0] + b2_vff_P[1][0];
- const float P22 = -K2 * b2_vff_P[0][1] + b2_vff_P[1][1];
- const float P23 = -K2 * b2_vff_P[0][2] + b2_vff_P[1][2];
- const float P31 = -K3 * b2_vff_P[0][0] + b2_vff_P[2][0];
- const float P32 = -K3 * b2_vff_P[0][1] + b2_vff_P[2][1];
- const float P33 = -K3 * b2_vff_P[0][2] + b2_vff_P[2][2];
+ const float P11 = (1. - K1) * vff_P[0][0];
+ const float P12 = (1. - K1) * vff_P[0][1];
+ const float P13 = (1. - K1) * vff_P[0][2];
+ const float P21 = -K2 * vff_P[0][0] + vff_P[1][0];
+ const float P22 = -K2 * vff_P[0][1] + vff_P[1][1];
+ const float P23 = -K2 * vff_P[0][2] + vff_P[1][2];
+ const float P31 = -K3 * vff_P[0][0] + vff_P[2][0];
+ const float P32 = -K3 * vff_P[0][1] + vff_P[2][1];
+ const float P33 = -K3 * vff_P[0][2] + vff_P[2][2];
- b2_vff_P[0][0] = P11;
- b2_vff_P[0][1] = P12;
- b2_vff_P[0][2] = P13;
- b2_vff_P[1][0] = P21;
- b2_vff_P[1][1] = P22;
- b2_vff_P[1][2] = P23;
- b2_vff_P[2][0] = P31;
- b2_vff_P[2][1] = P32;
- b2_vff_P[2][2] = P33;
+ vff_P[0][0] = P11;
+ vff_P[0][1] = P12;
+ vff_P[0][2] = P13;
+ vff_P[1][0] = P21;
+ vff_P[1][1] = P22;
+ vff_P[1][2] = P23;
+ vff_P[2][0] = P31;
+ vff_P[2][1] = P32;
+ vff_P[2][2] = P33;
}
-void b2_vff_update(float z_meas) {
+void vff_update(float z_meas) {
update_z_conf(z_meas, R);
}
-void b2_vff_update_z_conf(float z_meas, float conf) {
+void vff_update_z_conf(float z_meas, float conf) {
update_z_conf(z_meas, conf);
}
@@ -180,43 +180,43 @@
Pp = Pm - K*H*Pm;
*/
static inline void update_vz_conf(float vz, float conf) {
- const float yd = vz - b2_vff_zdot;
- const float S = b2_vff_P[1][1] + conf;
- const float K1 = b2_vff_P[0][1] * 1/S;
- const float K2 = b2_vff_P[1][1] * 1/S;
- const float K3 = b2_vff_P[2][1] * 1/S;
+ const float yd = vz - vff_zdot;
+ const float S = vff_P[1][1] + conf;
+ const float K1 = vff_P[0][1] * 1/S;
+ const float K2 = vff_P[1][1] * 1/S;
+ const float K3 = vff_P[2][1] * 1/S;
- b2_vff_z = b2_vff_z + K1 * yd;
- b2_vff_zdot = b2_vff_zdot + K2 * yd;
- b2_vff_bias = b2_vff_bias + K3 * yd;
+ vff_z = vff_z + K1 * yd;
+ vff_zdot = vff_zdot + K2 * yd;
+ vff_bias = vff_bias + K3 * yd;
- const float P11 = -K1 * b2_vff_P[1][0] + b2_vff_P[0][0];
- const float P12 = -K1 * b2_vff_P[1][1] + b2_vff_P[0][1];
- const float P13 = -K1 * b2_vff_P[1][2] + b2_vff_P[0][2];
- const float P21 = (1. - K2) * b2_vff_P[1][0];
- const float P22 = (1. - K2) * b2_vff_P[1][1];
- const float P23 = (1. - K2) * b2_vff_P[1][2];
- const float P31 = -K3 * b2_vff_P[1][0] + b2_vff_P[2][0];
- const float P32 = -K3 * b2_vff_P[1][1] + b2_vff_P[2][1];
- const float P33 = -K3 * b2_vff_P[1][2] + b2_vff_P[2][2];
+ const float P11 = -K1 * vff_P[1][0] + vff_P[0][0];
+ const float P12 = -K1 * vff_P[1][1] + vff_P[0][1];
+ const float P13 = -K1 * vff_P[1][2] + vff_P[0][2];
+ const float P21 = (1. - K2) * vff_P[1][0];
+ const float P22 = (1. - K2) * vff_P[1][1];
+ const float P23 = (1. - K2) * vff_P[1][2];
+ const float P31 = -K3 * vff_P[1][0] + vff_P[2][0];
+ const float P32 = -K3 * vff_P[1][1] + vff_P[2][1];
+ const float P33 = -K3 * vff_P[1][2] + vff_P[2][2];
- b2_vff_P[0][0] = P11;
- b2_vff_P[0][1] = P12;
- b2_vff_P[0][2] = P13;
- b2_vff_P[1][0] = P21;
- b2_vff_P[1][1] = P22;
- b2_vff_P[1][2] = P23;
- b2_vff_P[2][0] = P31;
- b2_vff_P[2][1] = P32;
- b2_vff_P[2][2] = P33;
+ vff_P[0][0] = P11;
+ vff_P[0][1] = P12;
+ vff_P[0][2] = P13;
+ vff_P[1][0] = P21;
+ vff_P[1][1] = P22;
+ vff_P[1][2] = P23;
+ vff_P[2][0] = P31;
+ vff_P[2][1] = P32;
+ vff_P[2][2] = P33;
}
-void b2_vff_update_vz_conf(float vz_meas, float conf) {
+void vff_update_vz_conf(float vz_meas, float conf) {
update_vz_conf(vz_meas, conf);
}
-void b2_vff_realign(float z_meas) {
- b2_vff_z = z_meas;
- b2_vff_zdot = 0;
+void vff_realign(float z_meas) {
+ vff_z = z_meas;
+ vff_zdot = 0;
}
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h
2010-09-28 14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h
2010-09-28 14:05:27 UTC (rev 6008)
@@ -21,24 +21,24 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_VF_FLOAT_H
-#define BOOZ2_VF_FLOAT_H
+#ifndef VF_FLOAT_H
+#define VF_FLOAT_H
-#define B2_VFF_STATE_SIZE 3
+#define VFF_STATE_SIZE 3
-extern float b2_vff_z;
-extern float b2_vff_zdot;
-extern float b2_vff_bias;
-extern float b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE];
-extern float b2_vff_zdotdot;
+extern float vff_z;
+extern float vff_zdot;
+extern float vff_bias;
+extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
+extern float vff_zdotdot;
-extern float b2_vff_z_meas;
+extern float vff_z_meas;
-extern void b2_vff_init(float z, float zdot, float bias);
-extern void b2_vff_propagate(float accel);
-extern void b2_vff_update(float z_meas);
-extern void b2_vff_update_z_conf(float z_meas, float conf);
-extern void b2_vff_update_vz_conf(float vz_meas, float conf);
-extern void b2_vff_realign(float z_meas);
+extern void vff_init(float z, float zdot, float bias);
+extern void vff_propagate(float accel);
+extern void vff_update(float z_meas);
+extern void vff_update_z_conf(float z_meas, float conf);
+extern void vff_update_vz_conf(float vz_meas, float conf);
+extern void vff_realign(float z_meas);
-#endif /* BOOZ2_VF_FLOAT_H */
+#endif /* VF_FLOAT_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
2010-09-28 14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
2010-09-28 14:05:27 UTC (rev 6008)
@@ -25,42 +25,42 @@
#include "booz_geometry_mixed.h"
-int64_t b2_vfi_z;
-int32_t b2_vfi_zd;
-int32_t b2_vfi_abias;
-int32_t b2_vfi_zdd;
-int32_t b2_vfi_P[B2_VFI_S_SIZE][B2_VFI_S_SIZE];
+int64_t vfi_z;
+int32_t vfi_zd;
+int32_t vfi_abias;
+int32_t vfi_zdd;
+int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE];
/* initial covariance */
-#define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
-#define VFI_INIT_PZDZD BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
-#define VFI_INIT_PABAB BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
+#define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
+#define VFI_INIT_PZDZD BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
+#define VFI_INIT_PABAB BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
/* system and measurement noise */
#define VFI_ACCEL_NOISE 0.1
#define VFI_DT2_2 (1./(512.*512.)/2.)
#define VFI_DT (1./512.)
-#define VFI_QZZ BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT2_2,
B2_VFI_P_FRAC)
-#define VFI_QZDZD BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT,
B2_VFI_P_FRAC)
-#define VFI_QABAB BOOZ_INT_OF_FLOAT(1e-7, B2_VFI_P_FRAC)
-#define VFI_R BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
+#define VFI_QZZ BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT2_2,
VFI_P_FRAC)
+#define VFI_QZDZD BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT, VFI_P_FRAC)
+#define VFI_QABAB BOOZ_INT_OF_FLOAT(1e-7, VFI_P_FRAC)
+#define VFI_R BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
void vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
// initialize state vector
- b2_vfi_z = z0;
- b2_vfi_zd = zd0;
- b2_vfi_abias = bias0;
- b2_vfi_zdd = 0;
+ vfi_z = z0;
+ vfi_zd = zd0;
+ vfi_abias = bias0;
+ vfi_zdd = 0;
// initialize covariance
int i, j;
- for (i=0; i<B2_VFI_S_SIZE; i++)
- for (j=0; j<B2_VFI_S_SIZE; j++)
- b2_vfi_P[i][j] = 0;
- b2_vfi_P[B2_VFI_S_Z][B2_VFI_S_Z] = VFI_INIT_PZZ;
- b2_vfi_P[B2_VFI_S_ZD][B2_VFI_S_ZD] = VFI_INIT_PZDZD;
- b2_vfi_P[B2_VFI_S_AB][B2_VFI_S_AB] = VFI_INIT_PABAB;
+ for (i=0; i<VFI_S_SIZE; i++)
+ for (j=0; j<VFI_S_SIZE; j++)
+ vfi_P[i][j] = 0;
+ vfi_P[VFI_S_Z][VFI_S_Z] = VFI_INIT_PZZ;
+ vfi_P[VFI_S_ZD][VFI_S_ZD] = VFI_INIT_PZDZD;
+ vfi_P[VFI_S_AB][VFI_S_AB] = VFI_INIT_PABAB;
}
@@ -85,65 +85,65 @@
void vfi_propagate( int32_t accel_reading ) {
// compute unbiased vertical acceleration
- b2_vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, B2_VFI_ZDD_FRAC) -
b2_vfi_abias;
+ vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, VFI_ZDD_FRAC) - vfi_abias;
// propagate state
- const int32_t dz = b2_vfi_zd >> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZD_FRAC -
B2_VFI_Z_FRAC);
- b2_vfi_z += dz;
- const int32_t dzd = b2_vfi_zdd >> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZDD_FRAC -
B2_VFI_ZD_FRAC);
- b2_vfi_zd += dzd;
+ const int32_t dz = vfi_zd >> ( VFI_F_UPDATE_FRAC + VFI_ZD_FRAC -
VFI_Z_FRAC);
+ vfi_z += dz;
+ const int32_t dzd = vfi_zdd >> ( VFI_F_UPDATE_FRAC + VFI_ZDD_FRAC -
VFI_ZD_FRAC);
+ vfi_zd += dzd;
// propagate covariance
- const int32_t tmp1 = b2_vfi_P[1][0] + b2_vfi_P[0][1] +
(b2_vfi_P[1][1]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF00 = b2_vfi_P[0][0] + (tmp1>>B2_VFI_F_UPDATE_FRAC);
- const int32_t tmp2 = b2_vfi_P[1][1] - b2_vfi_P[0][2] -
(b2_vfi_P[1][2]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF01 = b2_vfi_P[0][1] + (tmp2>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF02 = b2_vfi_P[0][2] + (b2_vfi_P[1][2] >>
B2_VFI_F_UPDATE_FRAC);;
- const int32_t tmp3 = -b2_vfi_P[2][0] + b2_vfi_P[1][1] -
(b2_vfi_P[2][1]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF10 = b2_vfi_P[1][0] + (tmp3>>B2_VFI_F_UPDATE_FRAC);
- const int32_t tmp4 = -b2_vfi_P[2][1] - b2_vfi_P[1][2] +
(b2_vfi_P[2][2]>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF11 = b2_vfi_P[1][1] + (tmp4>>B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF12 = b2_vfi_P[1][2] - (b2_vfi_P[2][2] >>
B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF20 = b2_vfi_P[2][0] + (b2_vfi_P[2][1] >>
B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF21 = b2_vfi_P[2][1] - (b2_vfi_P[2][2] >>
B2_VFI_F_UPDATE_FRAC);
- const int32_t FPF22 = b2_vfi_P[2][2];
+ const int32_t tmp1 = vfi_P[1][0] + vfi_P[0][1] +
(vfi_P[1][1]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF00 = vfi_P[0][0] + (tmp1>>VFI_F_UPDATE_FRAC);
+ const int32_t tmp2 = vfi_P[1][1] - vfi_P[0][2] -
(vfi_P[1][2]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF01 = vfi_P[0][1] + (tmp2>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF02 = vfi_P[0][2] + (vfi_P[1][2] >> VFI_F_UPDATE_FRAC);;
+ const int32_t tmp3 = -vfi_P[2][0] + vfi_P[1][1] -
(vfi_P[2][1]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF10 = vfi_P[1][0] + (tmp3>>VFI_F_UPDATE_FRAC);
+ const int32_t tmp4 = -vfi_P[2][1] - vfi_P[1][2] +
(vfi_P[2][2]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF11 = vfi_P[1][1] + (tmp4>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF12 = vfi_P[1][2] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF20 = vfi_P[2][0] + (vfi_P[2][1] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF21 = vfi_P[2][1] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF22 = vfi_P[2][2];
- b2_vfi_P[0][0] = FPF00 + VFI_QZZ;
- b2_vfi_P[0][1] = FPF01;
- b2_vfi_P[0][2] = FPF02;
- b2_vfi_P[1][0] = FPF10;
- b2_vfi_P[1][1] = FPF11 + VFI_QZDZD;
- b2_vfi_P[1][2] = FPF12;
- b2_vfi_P[2][0] = FPF20;
- b2_vfi_P[2][1] = FPF21;
- b2_vfi_P[2][2] = FPF22 + VFI_QABAB;
+ vfi_P[0][0] = FPF00 + VFI_QZZ;
+ vfi_P[0][1] = FPF01;
+ vfi_P[0][2] = FPF02;
+ vfi_P[1][0] = FPF10;
+ vfi_P[1][1] = FPF11 + VFI_QZDZD;
+ vfi_P[1][2] = FPF12;
+ vfi_P[2][0] = FPF20;
+ vfi_P[2][1] = FPF21;
+ vfi_P[2][2] = FPF22 + VFI_QABAB;
}
void vfi_update( int32_t z_meas ) {
- const int64_t y = (z_meas<<(B2_VFI_Z_FRAC-B2_VFI_MEAS_Z_FRAC)) - b2_vfi_z;
- const int32_t S = b2_vfi_P[0][0] + VFI_R;
+ const int64_t y = (z_meas<<(VFI_Z_FRAC-VFI_MEAS_Z_FRAC)) - vfi_z;
+ const int32_t S = vfi_P[0][0] + VFI_R;
- const int32_t K1 = b2_vfi_P[0][0] / S;
- const int32_t K2 = b2_vfi_P[1][0] / S;
- const int32_t K3 = b2_vfi_P[2][0] / S;
+ const int32_t K1 = vfi_P[0][0] / S;
+ const int32_t K2 = vfi_P[1][0] / S;
+ const int32_t K3 = vfi_P[2][0] / S;
- b2_vfi_z = b2_vfi_z + ((K1 * y)>>B2_VFI_P_FRAC);
- b2_vfi_zd = b2_vfi_zd + ((K2 * y)>>B2_VFI_P_FRAC);
- b2_vfi_abias = b2_vfi_abias + ((K3 * y)>>B2_VFI_P_FRAC);
+ vfi_z = vfi_z + ((K1 * y)>>VFI_P_FRAC);
+ vfi_zd = vfi_zd + ((K2 * y)>>VFI_P_FRAC);
+ vfi_abias = vfi_abias + ((K3 * y)>>VFI_P_FRAC);
#if 0
- const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) *
b2_vfi_P[0][0])>>B2_VFI_P_RES;
- const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) *
b2_vfi_P[0][1];
- const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) *
b2_vfi_P[0][2];
- const int32_t P21 = -K2 * b2_vfi_P[0][0] + b2_vfi_P[1][0];
- const int32_t P22 = -K2 * b2_vfi_P[0][1] + b2_vfi_P[1][1];
- const int32_t P23 = -K2 * b2_vfi_P[0][2] + b2_vfi_P[1][2];
- const int32_t P31 = -K3 * b2_vfi_P[0][0] + b2_vfi_P[2][0];
- const int32_t P32 = -K3 * b2_vfi_P[0][1] + b2_vfi_P[2][1];
- const int32_t P33 = -K3 * b2_vfi_P[0][2] + b2_vfi_P[2][2];
+ const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) *
vfi_P[0][0])>>VFI_P_RES;
+ const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][1];
+ const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][2];
+ const int32_t P21 = -K2 * vfi_P[0][0] + vfi_P[1][0];
+ const int32_t P22 = -K2 * vfi_P[0][1] + vfi_P[1][1];
+ const int32_t P23 = -K2 * vfi_P[0][2] + vfi_P[1][2];
+ const int32_t P31 = -K3 * vfi_P[0][0] + vfi_P[2][0];
+ const int32_t P32 = -K3 * vfi_P[0][1] + vfi_P[2][1];
+ const int32_t P33 = -K3 * vfi_P[0][2] + vfi_P[2][2];
tl_vf_P[0][0] = P11;
tl_vf_P[0][1] = P12;
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
2010-09-28 14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
2010-09-28 14:05:27 UTC (rev 6008)
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_VF_INT_H
-#define BOOZ2_VF_INT_H
+#ifndef VF_INT_H
+#define VF_INT_H
#include "std.h"
#include "booz_geometry_int.h"
@@ -33,43 +33,43 @@
/* z_meas : altitude measurement in meter */
/* Q23.8 : accuracy 0.004m range 8388km */
extern void vfi_update( int32_t z_meas );
-#define B2_VFI_Z_MEAS_FRAC IPOS_FRAC
+#define VFI_Z_MEAS_FRAC IPOS_FRAC
/* propagate frequency : 512 Hz */
-#define B2_VFI_F_UPDATE_FRAC 9
-#define B2_VFI_F_UPDATE (1<<B2_VFI_F_UPDATE_RES)
+#define VFI_F_UPDATE_FRAC 9
+#define VFI_F_UPDATE (1<<VFI_F_UPDATE_RES)
/* vertical acceleration in m/s^2 */
/* Q21.10 : accuracy 0.001m/s^2, range 2097km/s2 */
-extern int32_t b2_vfi_zdd;
-#define B2_VFI_ZDD_FRAC IACCEL_RES
+extern int32_t vfi_zdd;
+#define VFI_ZDD_FRAC IACCEL_RES
/* vertical accelerometer bias in m/s^2 */
/* Q21.10 : accuracy 0.001m/s^2, range 2097km/s2 */
-extern int32_t b2_vfi_abias;
-#define B2_VFI_BIAS_FRAC IACCEL_RES
+extern int32_t vfi_abias;
+#define VFI_BIAS_FRAC IACCEL_RES
/* vertical speed in m/s */
/* Q12.19 : accuracy 0.000002 , range 4096m/s2 */
-extern int32_t b2_vfi_zd;
-#define B2_VFI_ZD_FRAC (B2_VFI_ZDD_FRAC + B2_VFI_F_UPDATE_FRAC)
+extern int32_t vfi_zd;
+#define VFI_ZD_FRAC (VFI_ZDD_FRAC + VFI_F_UPDATE_FRAC)
/* altitude in m */
/* Q35.28 : accuracy 3.7e-9 , range 3.4e10m */
-extern int64_t b2_vfi_z;
-#define B2_VFI_Z_FRAC (B2_VFI_ZD_FRAC + B2_VFI_F_UPDATE_FRAC)
+extern int64_t vfi_z;
+#define VFI_Z_FRAC (VFI_ZD_FRAC + VFI_F_UPDATE_FRAC)
/* Kalman filter state */
-#define B2_VFI_S_Z 0
-#define B2_VFI_S_ZD 1
-#define B2_VFI_S_AB 2
-#define B2_VFI_S_SIZE 3
+#define VFI_S_Z 0
+#define VFI_S_ZD 1
+#define VFI_S_AB 2
+#define VFI_S_SIZE 3
/* Kalman filter covariance */
/* Q3.28 */
-extern int32_t b2_vfi_P[B2_VFI_S_SIZE][B2_VFI_S_SIZE];
-#define B2_VFI_P_FRAC 28
+extern int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE];
+#define VFI_P_FRAC 28
-#endif /* BOOZ2_VF_INT_H */
+#endif /* VF_INT_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-09-28
14:05:17 UTC (rev 6007)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-09-28
14:05:27 UTC (rev 6008)
@@ -113,7 +113,7 @@
#ifdef BOOZ2_SONAR
ins_update_on_agl = FALSE;
#endif
- b2_vff_init(0., 0., 0.);
+ vff_init(0., 0., 0.);
#endif
ins_vf_realign = FALSE;
ins_hf_realign = FALSE;
@@ -142,7 +142,7 @@
void ins_realign_v(float z) {
#ifdef USE_VFF
- b2_vff_realign(z);
+ vff_realign(z);
#endif
}
@@ -156,10 +156,10 @@
#ifdef USE_VFF
if (baro.status == BS_RUNNING && ins_baro_initialised) {
- b2_vff_propagate(z_accel_float);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ vff_propagate(z_accel_float);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
@@ -196,15 +196,15 @@
#ifdef BOOZ2_SONAR
ins_sonar_offset = sonar_meas;
#endif
- b2_vff_realign(0.);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ vff_realign(0.);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
ins_enu_pos.z = -ins_ltp_pos.z;
ins_enu_speed.z = -ins_ltp_speed.z;
ins_enu_accel.z = -ins_ltp_accel.z;
}
- b2_vff_update(alt_float);
+ vff_update(alt_float);
}
#endif
}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6008] rename b2_vff/B2_VFF and booz2_vff/BOOZ2_VFF to vff/VFF,
Felix Ruess <=