diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/firmwares/rotorcraft/ins.c similarity index 99% rename from sw/airborne/booz/booz2_ins.c rename to sw/airborne/firmwares/rotorcraft/ins.c index e285becb89..b0bdd722d3 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/firmwares/rotorcraft/ins.c @@ -280,4 +280,3 @@ void booz_ins_update_sonar() { } #endif } - diff --git a/sw/airborne/booz/booz2_ins.h b/sw/airborne/firmwares/rotorcraft/ins.h similarity index 99% rename from sw/airborne/booz/booz2_ins.h rename to sw/airborne/firmwares/rotorcraft/ins.h index 4dff386179..2e17709765 100644 --- a/sw/airborne/booz/booz2_ins.h +++ b/sw/airborne/firmwares/rotorcraft/ins.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. diff --git a/sw/airborne/booz/ins/booz2_hf_float-old.c b/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c similarity index 99% rename from sw/airborne/booz/ins/booz2_hf_float-old.c rename to sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c index beb9b2471d..e902f46b1c 100644 --- a/sw/airborne/booz/ins/booz2_hf_float-old.c +++ b/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c @@ -116,4 +116,3 @@ void b2ins_update_gps(void) { #endif /* UPDATE_FROM_SPEED */ } - diff --git a/sw/airborne/booz/ins/booz2_hf_float-old.h b/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h similarity index 97% rename from sw/airborne/booz/ins/booz2_hf_float-old.h rename to sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h index c3f47b3321..bfd15b9559 100644 --- a/sw/airborne/booz/ins/booz2_hf_float-old.h +++ b/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -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_HF_FLOAT_H diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/firmwares/rotorcraft/ins/hf_float.c similarity index 99% rename from sw/airborne/booz/ins/booz2_hf_float.c rename to sw/airborne/firmwares/rotorcraft/ins/hf_float.c index 78ec710af7..a20cc7d09f 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.c +++ b/sw/airborne/firmwares/rotorcraft/ins/hf_float.c @@ -756,5 +756,3 @@ static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, hff_work->yP[1][0] = P21; hff_work->yP[1][1] = P22; } - - diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/firmwares/rotorcraft/ins/hf_float.h similarity index 99% rename from sw/airborne/booz/ins/booz2_hf_float.h rename to sw/airborne/firmwares/rotorcraft/ins/hf_float.h index 6ba479f545..737b356e06 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.h +++ b/sw/airborne/firmwares/rotorcraft/ins/hf_float.h @@ -81,4 +81,3 @@ extern int lag_counter_err; extern int save_counter; #endif /* BOOZ2_HF_FLOAT_H */ - diff --git a/sw/airborne/booz/ins/booz2_vf_float.c b/sw/airborne/firmwares/rotorcraft/ins/vf_float.c similarity index 95% rename from sw/airborne/booz/ins/booz2_vf_float.c rename to sw/airborne/firmwares/rotorcraft/ins/vf_float.c index 5fcf9eff88..41305ede79 100644 --- a/sw/airborne/booz/ins/booz2_vf_float.c +++ b/sw/airborne/firmwares/rotorcraft/ins/vf_float.c @@ -182,13 +182,13 @@ void b2_vff_update_z_conf(float z_meas, float conf) { 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; - - b2_vff_z = b2_vff_z + K1 * yd; - b2_vff_zdot = b2_vff_zdot + K2 * yd; - b2_vff_bias = b2_vff_bias + K3 * yd; + 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; + + b2_vff_z = b2_vff_z + K1 * yd; + b2_vff_zdot = b2_vff_zdot + K2 * yd; + b2_vff_bias = b2_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]; diff --git a/sw/airborne/booz/ins/booz2_vf_float.h b/sw/airborne/firmwares/rotorcraft/ins/vf_float.h similarity index 97% rename from sw/airborne/booz/ins/booz2_vf_float.h rename to sw/airborne/firmwares/rotorcraft/ins/vf_float.h index 187f873fc0..284455e824 100644 --- a/sw/airborne/booz/ins/booz2_vf_float.h +++ b/sw/airborne/firmwares/rotorcraft/ins/vf_float.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -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_VF_FLOAT_H @@ -42,4 +42,3 @@ extern void b2_vff_update_vz_conf(float vz_meas, float conf); extern void b2_vff_realign(float z_meas); #endif /* BOOZ2_VF_FLOAT_H */ - diff --git a/sw/airborne/booz/ins/booz2_vf_int.c b/sw/airborne/firmwares/rotorcraft/ins/vf_int.c similarity index 91% rename from sw/airborne/booz/ins/booz2_vf_int.c rename to sw/airborne/firmwares/rotorcraft/ins/vf_int.c index 9d0fa75745..947640fbe8 100644 --- a/sw/airborne/booz/ins/booz2_vf_int.c +++ b/sw/airborne/firmwares/rotorcraft/ins/vf_int.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -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. */ #include "booz2_vf_int.h" @@ -32,7 +32,7 @@ int32_t b2_vfi_zdd; int32_t b2_vfi_P[B2_VFI_S_SIZE][B2_VFI_S_SIZE]; /* initial covariance */ -#define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC) +#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) @@ -56,7 +56,7 @@ void booz2_vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) { // initialize covariance int i, j; for (i=0; i>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); + 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; + + 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); #if 0 diff --git a/sw/airborne/booz/ins/booz2_vf_int.h b/sw/airborne/firmwares/rotorcraft/ins/vf_int.h similarity index 98% rename from sw/airborne/booz/ins/booz2_vf_int.h rename to sw/airborne/firmwares/rotorcraft/ins/vf_int.h index 170b9f6b47..600efc1781 100644 --- a/sw/airborne/booz/ins/booz2_vf_int.h +++ b/sw/airborne/firmwares/rotorcraft/ins/vf_int.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -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_VF_INT_H