diff --git a/conf/autopilot/booz2_simulator.makefile b/conf/autopilot/booz2_simulator.makefile
index 61a3085a63..f8e35df245 100644
--- a/conf/autopilot/booz2_simulator.makefile
+++ b/conf/autopilot/booz2_simulator.makefile
@@ -19,7 +19,7 @@ sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy
#sim.CFLAGS += -DBYPASS_AHRS
#sim.CFLAGS += -DBYPASS_INS
-sim.CFLAGS += -DINIT_WIND_X=-5.0
+sim.CFLAGS += -DINIT_WIND_X=-0.0
sim.CFLAGS += -DINIT_WIND_Y=-0.0
sim.CFLAGS += -DINIT_WIND_Z=-0.0
@@ -106,6 +106,7 @@ sim.srcs += $(SRC_BOOZ)/booz2_ins.c
# vertical filter float version
sim.srcs += $(SRC_BOOZ)/booz2_vf_float.c
sim.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
+sim.srcs += $(SRC_BOOZ)/booz2_hf_float.c
diff --git a/conf/messages.xml b/conf/messages.xml
index 8580a1add8..227a3823ba 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -510,6 +510,12 @@
+
+
+
+
+
+
@@ -870,8 +876,22 @@
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+
+
diff --git a/sw/airborne/6dof.h b/sw/airborne/6dof.h
index 37d03a548e..029585afc0 100644
--- a/sw/airborne/6dof.h
+++ b/sw/airborne/6dof.h
@@ -19,4 +19,13 @@
#define EULER_PSI 2
#define EULER_NB 3
+#define QUAT_QI 0
+#define QUAT_QX 1
+#define QUAT_QY 2
+#define QUAT_QZ 3
+#define QUAT_NB 4
+
+
+
+
#endif /* _6DOF_H */
diff --git a/sw/airborne/booz/booz2_filter_attitude.h b/sw/airborne/booz/booz2_filter_attitude.h
index 868c06cf71..7c094ab8c6 100644
--- a/sw/airborne/booz/booz2_filter_attitude.h
+++ b/sw/airborne/booz/booz2_filter_attitude.h
@@ -16,6 +16,8 @@ struct Booz_ahrs_state {
extern struct Booz_ahrs_state booz_ahrs_state;
extern struct booz_ieuler booz2_filter_attitude_euler_aligned;
+extern struct booz_iquat booz2_filter_attitude_quat_aligned;
+
extern struct booz_ieuler booz2_filter_attitude_euler;
extern struct booz_ivect booz2_filter_attitude_rate;
diff --git a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
index 3147bbae4a..31cdcd49e8 100644
--- a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
+++ b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
@@ -14,7 +14,7 @@ struct booz_ieuler booz2_filter_attitude_euler;
struct booz_ivect booz2_filter_attitude_rate;
struct booz_ieuler booz2_filter_attitude_euler_aligned;
-
+struct booz_iquat booz2_filter_attitude_quat_aligned;
struct booz_ivect booz2_face_gyro_bias;
struct booz_ieuler booz2_face_measure;
@@ -161,10 +161,10 @@ void booz2_filter_attitude_propagate(void) {
/* scale our result */
BOOZ_IEULER_SDIV(booz2_filter_attitude_euler, booz2_face_corrected, F_UPDATE);
- /* convert to quaternion */
- // BOOZ_IQUAT_OF_EULER(booz2_filter_attitude_quat, booz2_filter_attitude_euler);
apply_alignment();
+ /* convert to quaternion */
+ BOOZ_IQUAT_OF_EULER(booz2_filter_attitude_quat_aligned, booz2_filter_attitude_euler_aligned);
}
diff --git a/sw/airborne/booz/booz2_hf_float.c b/sw/airborne/booz/booz2_hf_float.c
index b7695dd83e..5d9457c6d0 100644
--- a/sw/airborne/booz/booz2_hf_float.c
+++ b/sw/airborne/booz/booz2_hf_float.c
@@ -1,22 +1,97 @@
#include "booz2_hf_float.h"
+#include "booz2_imu.h"
+#include "booz2_filter_attitude.h"
+#include "booz_geometry_mixed.h"
-struct FloatVect3 bhff_accel_bias;
-struct FloatVect3 bhff_pos;
-struct FloatVect3 bhff_speed_ltp;
-struct FloatVect3 bhff_accel_ltp;
+struct Int32Vect3 b2ins_accel_bias;
+struct Int32Vect3 b2ins_accel_ltp;
+struct Int32Vect3 b2ins_speed_ltp;
+struct Int64Vect3 b2ins_pos_ltp;
+
+struct Int32Eulers b2ins_body_to_imu_eulers;
+struct Int32Quat b2ins_body_to_imu_quat;
+struct Int32Quat b2ins_imu_to_body_quat;
+
+struct Int32Vect2 b2ins_ltp_meas_gps;
+#include "downlink.h"
+
+void b2ins_init(void) {
+ INT32_VECT3_ZERO(b2ins_accel_bias);
+ b2ins_body_to_imu_eulers.phi = FILTER_ALIGNMENT_DPHI;
+ b2ins_body_to_imu_eulers.theta = FILTER_ALIGNMENT_DTHETA;
+ b2ins_body_to_imu_eulers.psi = FILTER_ALIGNMENT_DPSI;
+ BOOZ_IQUAT_OF_EULER(b2ins_body_to_imu_quat, b2ins_body_to_imu_eulers);
+ INT32_QUAT_INVERT(b2ins_imu_to_body_quat, b2ins_body_to_imu_quat);
+
+}
+
+#include "booz_flight_model.h"
+#include "6dof.h"
+void b2ins_propagate(void) {
+
+#ifdef BYPASS_AHRS
+ booz2_filter_attitude_quat_aligned.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
+ booz2_filter_attitude_quat_aligned.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
+ booz2_filter_attitude_quat_aligned.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
+ booz2_filter_attitude_quat_aligned.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
+#endif /* BYPASS_AHRS */
-void bhff_update(void) {
-
- struct FloatVect3 accel_body;
- /* unbias accels */
- FLOAT_VECT3_SUB(accel_body, booz2_imu_accel, bhff_accel_bias);
+ struct Int32Vect3 accel_imu;
+ /* unbias accelerometers */
+ INT32_VECT3_DIFF(accel_imu, booz2_imu_accel, b2ins_accel_bias);
/* convert to LTP */
-
+ struct Int32Quat body_to_ltp;
+ BOOZ_IQUAT_INVERT(body_to_ltp, booz2_filter_attitude_quat_aligned);
+ struct Int32Quat imu_to_ltp;
+ INT32_QUAT_MULT(imu_to_ltp, body_to_ltp, b2ins_imu_to_body_quat);
+ BOOZ_IQUAT_VMULT(b2ins_accel_ltp, imu_to_ltp, accel_imu);
+ /* correct for gravity */
+ b2ins_accel_ltp.z += BOOZ_ACCEL_I_OF_F(9.81);
+
+ /* propagate speed */
+ b2ins_speed_ltp.x += b2ins_accel_ltp.x;
+ b2ins_speed_ltp.y += b2ins_accel_ltp.y;
+ b2ins_speed_ltp.z += b2ins_accel_ltp.z;
+
+
+ /* propagate position */
+ b2ins_pos_ltp.x += b2ins_speed_ltp.x;
+ b2ins_pos_ltp.y += b2ins_speed_ltp.y;
+ b2ins_pos_ltp.z += b2ins_speed_ltp.z;
+
+
+
+ RunOnceEvery(10, {
+ struct Int32Vect3 pos_low_res; \
+ pos_low_res.x = (int32_t)(b2ins_pos_ltp.x>> 20); \
+ pos_low_res.y = (int32_t)(b2ins_pos_ltp.y>> 20); \
+ pos_low_res.z = (int32_t)(b2ins_pos_ltp.z>> 20); \
+ DOWNLINK_SEND_BOOZ2_INS2(&b2ins_accel_ltp.x, \
+ &b2ins_accel_ltp.y, \
+ &b2ins_accel_ltp.z, \
+ &b2ins_speed_ltp.x, \
+ &b2ins_speed_ltp.y, \
+ &b2ins_speed_ltp.z, \
+ &pos_low_res.x, \
+ &pos_low_res.y, \
+ &pos_low_res.z \
+ ); \
+ });
}
+
+void b2ins_update_gps(void) {
+
+ b2ins_ltp_meas_gps.x = ;
+ b2ins_ltp_meas_gps.y = ;
+
+
+
+}
+
diff --git a/sw/airborne/booz/booz2_hf_float.h b/sw/airborne/booz/booz2_hf_float.h
index 77c64561e2..aa28aa4664 100644
--- a/sw/airborne/booz/booz2_hf_float.h
+++ b/sw/airborne/booz/booz2_hf_float.h
@@ -1,7 +1,11 @@
-#ifnedf BOOZ2_HF_FLOAT_H
+#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
#include "pprz_algebra_float.h"
+#include "pprz_algebra_int.h"
+void b2ins_init(void);
+extern void b2ins_propagate(void);
+extern void b2ins_update_gps(void);
#endif /* BOOZ2_HF_FLOAT_H */
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index 130ecfc3cb..e06af124a4 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -10,6 +10,10 @@
#ifdef USE_VFF
#include "booz2_vf_float.h"
#endif
+#ifdef SITL
+#include "booz2_filter_attitude.h"
+#include "booz2_hf_float.h"
+#endif
struct Pprz_int32_lla booz_ins_position_init_lla; // LLA
@@ -38,6 +42,9 @@ void booz_ins_init() {
booz_ins_baro_initialised = FALSE;
b2_vff_init(0., 0., 0.);
#endif
+#ifdef SITL
+ b2ins_init();
+#endif
}
void booz_ins_propagate() {
@@ -57,6 +64,10 @@ void booz_ins_propagate() {
booz_ins_position.z = BOOZ_POS_I_OF_F(b2_vff_z);
}
#endif
+#ifdef SITL
+ if (booz2_filter_attitude_status == BOOZ2_FILTER_ATTITUDE_RUNNING)
+ b2ins_propagate();
+#endif
}
void booz_ins_update_baro() {
@@ -90,6 +101,9 @@ void booz_ins_update_gps(void) {
// copy to pos NED
PPRZ_INT32_VECT2_OF_LL(booz_ins_position, booz_ins_position_lla);
}
+
+
+
}
diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 5d2d96a1b3..6a66bce775 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -193,6 +193,7 @@ static inline void on_imu_event( void ) {
// LED_ON(7);
booz2_filter_attitude_propagate();
booz2_filter_attitude_update();
+
// LED_OFF(7);
booz_ins_propagate();
}
diff --git a/sw/airborne/booz/booz2_vf_float.c b/sw/airborne/booz/booz2_vf_float.c
index 4a838f5521..a0b83bf389 100644
--- a/sw/airborne/booz/booz2_vf_float.c
+++ b/sw/airborne/booz/booz2_vf_float.c
@@ -5,8 +5,8 @@
X = [ z zdot bias ]
temps :
- predict 86us
- update 46us
+ propagate 86us
+ update 46us
*/
/* initial covariance diagonal */
diff --git a/sw/airborne/pprz_algebra_int.h b/sw/airborne/pprz_algebra_int.h
index e1385d7682..6526eb4648 100644
--- a/sw/airborne/pprz_algebra_int.h
+++ b/sw/airborne/pprz_algebra_int.h
@@ -15,6 +15,27 @@ struct Int32Vect3 {
int32_t z;
};
+struct Int64Vect3 {
+ int64_t x;
+ int64_t y;
+ int64_t z;
+};
+
+
+
+struct Int32Quat {
+ int32_t qi;
+ int32_t qx;
+ int32_t qy;
+ int32_t qz;
+};
+
+struct Int32Eulers {
+ int32_t phi;
+ int32_t theta;
+ int32_t psi;
+};
+
#if 0
struct Int32Mat33 {
@@ -23,16 +44,57 @@ struct Int32Mat33 {
#endif
typedef int32_t Int32Mat33[3*3];
-#define PPRZ_INT32_VECT3_COPY(_o, _i) { \
+#define INT32_VECT3_ZERO(_o) { \
+ _o.x = 0; \
+ _o.y = 0; \
+ _o.z = 0; \
+ }
+
+#define INT32_VECT3_COPY(_o, _i) { \
_o.x = _i.x; \
_o.y = _i.y; \
_o.z = _i.z; \
}
-#define PPRZ_INT32_MAT33_VECT3_MUL(_o, _m, _v) { \
+#define INT32_VECT3_DIFF(_c, _a, _b) { \
+ _c.x = _a.x - _b.x; \
+ _c.y = _a.y - _b.y; \
+ _c.z = _a.z - _b.z; \
+ }
+
+
+
+
+
+
+#define INT32_MAT33_VECT3_MUL(_o, _m, _v) { \
_o.x = _m[0][0]*_v.x + _m[0][1]*_v.y + _m[0][2]*_v.z; \
_o.y = _m[1][0]*_v.x + _m[1][1]*_v.y + _m[1][2]*_v.z; \
_o.z = _m[2][0]*_v.x + _m[2][1]*_v.y + _m[2][2]*_v.z; \
}
+
+#define INT32_QUAT_ZERO(_q) { \
+ _q.qi = (1 << IQUAT_RES); \
+ _q.qx = 0; \
+ _q.qy = 0; \
+ _q.qz = 0; \
+ }
+
+#define INT32_QUAT_MULT(c, a, b) { \
+ c.qi = (a.qi*b.qi - a.qx*b.qx - a.qy*b.qy - a.qz*b.qz)>>IQUAT_RES; \
+ c.qx = (a.qi*b.qx + a.qx*b.qi + a.qy*b.qz - a.qz*b.qy)>>IQUAT_RES; \
+ c.qy = (a.qi*b.qy - a.qx*b.qz + a.qy*b.qi + a.qz*b.qx)>>IQUAT_RES; \
+ c.qz = (a.qi*b.qz + a.qx*b.qy - a.qy*b.qx + a.qz*b.qi)>>IQUAT_RES; \
+ }
+
+#define INT32_QUAT_INVERT(_qo, _qi) { \
+ _qo.qi = _qi.qi; \
+ _qo.qx = -_qi.qx; \
+ _qo.qy = -_qi.qy; \
+ _qo.qz = -_qi.qz; \
+ }
+
+
+
#endif /* PPRZ_ALGEBRA_INT_H */
diff --git a/sw/simulator/booz2_sim_main.c b/sw/simulator/booz2_sim_main.c
index 5f5f1513a2..a24f8cdb25 100644
--- a/sw/simulator/booz2_sim_main.c
+++ b/sw/simulator/booz2_sim_main.c
@@ -118,6 +118,8 @@ static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) {
}
+#include "booz2_filter_attitude.h"
+
static void sim_run_one_step(void) {
/* read actuators positions */
@@ -191,6 +193,13 @@ static void sim_overwrite_ahrs(void) {
booz2_filter_attitude_euler_aligned.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]);
booz2_filter_attitude_euler_aligned.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]);
+
+ booz2_filter_attitude_quat_aligned.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
+ booz2_filter_attitude_quat_aligned.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
+ booz2_filter_attitude_quat_aligned.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
+ booz2_filter_attitude_quat_aligned.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
+
+
booz2_filter_attitude_rate.x = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]);
booz2_filter_attitude_rate.y = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]);
booz2_filter_attitude_rate.z = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]);
@@ -271,11 +280,19 @@ static void booz2_sim_display(void) {
(bfm.pos_ltp->ve[AXIS_X]),
(bfm.pos_ltp->ve[AXIS_Y]),
(bfm.pos_ltp->ve[AXIS_Z]));
+#if 0
IvySendMsg("%d BOOZ_SIM_WIND %f %f %f",
AC_ID,
bwm.velocity->ve[AXIS_X],
bwm.velocity->ve[AXIS_Y],
bwm.velocity->ve[AXIS_Z]);
+#endif
+ IvySendMsg("%d BOOZ_SIM_ACCEL_LTP %f %f %f",
+ AC_ID,
+ bfm.accel_ltp->ve[AXIS_X],
+ bfm.accel_ltp->ve[AXIS_Y],
+ bfm.accel_ltp->ve[AXIS_Z]);
+
}
}
diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c
index 716eb04015..a5f6fc3ebf 100644
--- a/sw/simulator/booz_flight_model.c
+++ b/sw/simulator/booz_flight_model.c
@@ -169,7 +169,9 @@ static void booz_flight_model_update_byproducts(void) {
dcm_of_eulers(bfm.eulers, bfm.dcm);
/* transpose of dcm ( body to inertial ) */
m_transp(bfm.dcm, bfm.dcm_t);
-
+ /* quaternion */
+ quat_of_eulers(bfm.quat, bfm.eulers);
+
BoozFlighModelGetPos(bfm.pos_ltp);
/* extract speed in ltp frame from state */
BoozFlighModelGetSpeedLtp(bfm.speed_ltp);
@@ -188,8 +190,7 @@ static void booz_flight_model_update_byproducts(void) {
/* rotational accelerations */
- /* quaternion */
-
+
}
diff --git a/sw/simulator/booz_flight_model_utils.c b/sw/simulator/booz_flight_model_utils.c
index 7a7258787e..0ad50d91dd 100644
--- a/sw/simulator/booz_flight_model_utils.c
+++ b/sw/simulator/booz_flight_model_utils.c
@@ -39,6 +39,7 @@ void rk4(ode_fun f, VEC* x, VEC* u, double dt) {
}
+
MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) {
dcm = m_resize(dcm, 3,3);
@@ -63,6 +64,30 @@ MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) {
return dcm;
}
+
+VEC* quat_of_eulers(VEC* quat, VEC* eulers) {
+
+ double phi2 = eulers->ve[EULER_PHI] / 2.0;
+ double theta2 = eulers->ve[EULER_THETA] / 2.0;
+ double psi2 = eulers->ve[EULER_PSI] / 2.0;
+
+ double sinphi2 = sin( phi2 );
+ double cosphi2 = cos( phi2 );
+ double sintheta2 = sin( theta2 );
+ double costheta2 = cos( theta2 );
+ double sinpsi2 = sin( psi2 );
+ double cospsi2 = cos( psi2 );
+
+ quat->ve[QUAT_QI] = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
+ quat->ve[QUAT_QX] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
+ quat->ve[QUAT_QY] = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
+ quat->ve[QUAT_QZ] = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
+
+ return quat;
+}
+
+
+
VEC* out_prod( VEC* a, VEC* b, VEC* out) {
if ( a->dim != 3 || b->dim != 3 )
error(E_SIZES,"out_prod");
diff --git a/sw/simulator/booz_flight_model_utils.h b/sw/simulator/booz_flight_model_utils.h
index a57f81ef15..a53798608c 100644
--- a/sw/simulator/booz_flight_model_utils.h
+++ b/sw/simulator/booz_flight_model_utils.h
@@ -8,6 +8,7 @@ typedef void (*ode_fun)(VEC* x, VEC* u, VEC* xdot);
extern void rk4(ode_fun f, VEC* x, VEC* u, double dt);
extern MAT* dcm_of_eulers (VEC* eulers, MAT* dcm );
+extern VEC* quat_of_eulers(VEC* quat, VEC* eulers);
extern VEC* out_prod( VEC* a, VEC* b, VEC* out);
diff --git a/sw/simulator/booz_rc_sim.h b/sw/simulator/booz_rc_sim.h
index c74bf84cb0..fb9d39083b 100644
--- a/sw/simulator/booz_rc_sim.h
+++ b/sw/simulator/booz_rc_sim.h
@@ -22,7 +22,7 @@
else { \
ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.99 * (2050-1223); \
- ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO2; \
+ ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO1; \
} \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
diff --git a/sw/simulator/booz_sensors_model_params_booz2_a1.h b/sw/simulator/booz_sensors_model_params_booz2_a1.h
index 275b1b604b..2a3bb8ceb8 100644
--- a/sw/simulator/booz_sensors_model_params_booz2_a1.h
+++ b/sw/simulator/booz_sensors_model_params_booz2_a1.h
@@ -45,13 +45,13 @@
#define BSM_ACCEL_NEUTRAL_Y 33738
#define BSM_ACCEL_NEUTRAL_Z 32441
/* m2s-4 */
-//#define BSM_ACCEL_NOISE_STD_DEV_X 0
-//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
-//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
+#define BSM_ACCEL_NOISE_STD_DEV_X 0
+#define BSM_ACCEL_NOISE_STD_DEV_Y 0
+#define BSM_ACCEL_NOISE_STD_DEV_Z 0
-#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1
+//#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1
+//#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
+//#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1
/* ms-2 */
#define BSM_ACCEL_BIAS_X 0
@@ -76,9 +76,13 @@
#define BSM_GYRO_NEUTRAL_Q 33417
#define BSM_GYRO_NEUTRAL_R 32809
-#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
-#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
-#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
+#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
+#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
+#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
+
+//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
+//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
+//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)