[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6002] moving booz ins, still need to rename
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6002] moving booz ins, still need to rename |
|
Date: |
Tue, 28 Sep 2010 14:04:22 +0000 |
Revision: 6002
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6002
Author: flixr
Date: 2010-09-28 14:04:22 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
moving booz ins, still need to rename
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.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
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
paparazzi3/trunk/sw/airborne/booz/ins/
Deleted: paparazzi3/trunk/sw/airborne/booz/booz2_ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2010-09-28 14:04:15 UTC
(rev 6001)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.c 2010-09-28 14:04:22 UTC
(rev 6002)
@@ -1,283 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- * Copyright (C) 2009 Felix Ruess <address@hidden>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * 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.
- */
-
-#include "booz2_ins.h"
-
-#include <firmwares/rotorcraft/imu.h>
-#include "firmwares/rotorcraft/baro.h"
-#include "booz_gps.h"
-
-#include "airframe.h"
-#include "math/pprz_algebra_int.h"
-#include "math/pprz_algebra_float.h"
-
-#include "ahrs.h"
-
-#ifdef USE_VFF
-#include "ins/booz2_vf_float.h"
-#endif
-
-#ifdef USE_HFF
-#include "ins/booz2_hf_float.h"
-#endif
-
-#ifdef BOOZ2_SONAR
-#include "modules.h"
-#endif
-
-#ifdef SITL
-#include "nps_fdm.h"
-#include <stdio.h>
-#endif
-
-
-#include "math/pprz_geodetic_int.h"
-
-#include "flight_plan.h"
-
-/* gps transformed to LTP-NED */
-struct LtpDef_i booz_ins_ltp_def;
- bool_t booz_ins_ltp_initialised;
-struct NedCoor_i booz_ins_gps_pos_cm_ned;
-struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
-#ifdef USE_HFF
-/* horizontal gps transformed to NED in meters as float */
-struct FloatVect2 booz_ins_gps_pos_m_ned;
-struct FloatVect2 booz_ins_gps_speed_m_s_ned;
-#endif
-bool_t booz_ins_hf_realign;
-
-/* barometer */
-#ifdef USE_VFF
-int32_t booz_ins_qfe;
-bool_t booz_ins_baro_initialised;
-int32_t booz_ins_baro_alt;
-#ifdef BOOZ2_SONAR
-bool_t booz_ins_update_on_agl;
-int32_t booz_ins_sonar_offset;
-#endif
-#endif
-bool_t booz_ins_vf_realign;
-
-/* output */
-struct NedCoor_i booz_ins_ltp_pos;
-struct NedCoor_i booz_ins_ltp_speed;
-struct NedCoor_i booz_ins_ltp_accel;
-struct EnuCoor_i booz_ins_enu_pos;
-struct EnuCoor_i booz_ins_enu_speed;
-struct EnuCoor_i booz_ins_enu_accel;
-
-
-void booz_ins_init() {
-#ifdef USE_INS_NAV_INIT
- booz_ins_ltp_initialised = TRUE;
-
- /** FIXME: should use the same code than MOVE_WP in booz2_datalink.c */
- struct LlaCoor_i llh; /* Height above the ellipsoid */
- llh.lat = INT32_RAD_OF_DEG(NAV_LAT0);
- llh.lon = INT32_RAD_OF_DEG(NAV_LON0);
- //llh.alt = NAV_ALT0 - booz_ins_ltp_def.hmsl + booz_ins_ltp_def.lla.alt;
- llh.alt = NAV_ALT0 + NAV_HMSL0;
-
- struct EcefCoor_i nav_init;
- ecef_of_lla_i(&nav_init, &llh);
-
- ltp_def_from_ecef_i(&booz_ins_ltp_def, &nav_init);
- booz_ins_ltp_def.hmsl = NAV_ALT0;
-#else
- booz_ins_ltp_initialised = FALSE;
-#endif
-#ifdef USE_VFF
- booz_ins_baro_initialised = FALSE;
-#ifdef BOOZ2_SONAR
- booz_ins_update_on_agl = FALSE;
-#endif
- b2_vff_init(0., 0., 0.);
-#endif
- booz_ins_vf_realign = FALSE;
- booz_ins_hf_realign = FALSE;
-#ifdef USE_HFF
- b2_hff_init(0., 0., 0., 0.);
-#endif
- INT32_VECT3_ZERO(booz_ins_ltp_pos);
- INT32_VECT3_ZERO(booz_ins_ltp_speed);
- INT32_VECT3_ZERO(booz_ins_ltp_accel);
- INT32_VECT3_ZERO(booz_ins_enu_pos);
- INT32_VECT3_ZERO(booz_ins_enu_speed);
- INT32_VECT3_ZERO(booz_ins_enu_accel);
-}
-
-void booz_ins_periodic( void ) {
-}
-
-#ifdef USE_HFF
-void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
- b2_hff_realign(pos, speed);
-}
-#else
-void booz_ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct
FloatVect2 speed __attribute__ ((unused))) {}
-#endif /* USE_HFF */
-
-
-void booz_ins_realign_v(float z) {
-#ifdef USE_VFF
- b2_vff_realign(z);
-#endif
-}
-
-void booz_ins_propagate() {
- /* untilt accels */
- struct Int32Vect3 accel_body;
- INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
- struct Int32Vect3 accel_ltp;
- INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
- float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
-
-#ifdef USE_VFF
- if (baro.status == BS_RUNNING && booz_ins_baro_initialised) {
- b2_vff_propagate(z_accel_float);
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
- }
- else { // feed accel from the sensors
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
- }
-#else
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
-#endif /* USE_VFF */
-
-#ifdef USE_HFF
- /* propagate horizontal filter */
- b2_hff_propagate();
-#else
- 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);
-}
-
-void booz_ins_update_baro() {
-#ifdef USE_VFF
- if (baro.status == BS_RUNNING) {
- if (!booz_ins_baro_initialised) {
- booz_ins_qfe = baro.absolute;
- booz_ins_baro_initialised = TRUE;
- }
- booz_ins_baro_alt = ((baro.absolute - booz_ins_qfe) *
BOOZ_INS_BARO_SENS_NUM)/BOOZ_INS_BARO_SENS_DEN;
- float alt_float = POS_FLOAT_OF_BFP(booz_ins_baro_alt);
- if (booz_ins_vf_realign) {
- booz_ins_vf_realign = FALSE;
- booz_ins_qfe = baro.absolute;
-#ifdef BOOZ2_SONAR
- booz_ins_sonar_offset = sonar_meas;
-#endif
- b2_vff_realign(0.);
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
- booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
- booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
- booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
- }
- b2_vff_update(alt_float);
- }
-#endif
-}
-
-
-void booz_ins_update_gps(void) {
-#ifdef USE_GPS
- if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
- if (!booz_ins_ltp_initialised) {
- ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos);
- booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
- booz_ins_ltp_def.hmsl = booz_gps_state.hmsl;
- booz_ins_ltp_initialised = TRUE;
- }
- ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def,
&booz_gps_state.ecef_pos);
- ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def,
&booz_gps_state.ecef_vel);
-#ifdef USE_HFF
- VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x,
booz_ins_gps_pos_cm_ned.y);
- VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
- VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x,
booz_ins_gps_speed_cm_s_ned.y);
- VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
- if (booz_ins_hf_realign) {
- booz_ins_hf_realign = FALSE;
-#ifdef SITL
- struct FloatVect2 true_pos, true_speed;
- VECT2_COPY(true_pos, fdm.ltpprz_pos);
- VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
- b2_hff_realign(true_pos, true_speed);
-#else
- const struct FloatVect2 zero = {0.0, 0.0};
- b2_hff_realign(booz_ins_gps_pos_m_ned, zero);
-#endif
- }
- b2_hff_update_gps();
-#ifndef USE_VFF /* vff not used */
- booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) /
INT32_POS_OF_CM_DEN;
- booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z *
INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
-#endif /* vff not used */
-#endif /* hff used */
-
-
-#ifndef USE_HFF /* hff not used */
-#ifndef USE_VFF /* neither hf nor vf used */
- INT32_VECT3_SCALE_3(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
- INT32_VECT3_SCALE_3(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
-#else /* only vff used */
- INT32_VECT2_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
- INT32_VECT2_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);
-#endif
-
-#ifdef USE_GPS_LAG_HACK
- 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 /* hff not used */
-
- 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);
- }
-#endif /* USE_GPS */
-}
-
-void booz_ins_update_sonar() {
-#if defined BOOZ2_SONAR && defined USE_VFF
- static int32_t sonar_filtered = 0;
- sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
- /* update baro_qfe assuming a flat ground */
- if (booz_ins_update_on_agl && booz2_analog_baro_status ==
BOOZ2_ANALOG_BARO_RUNNING) {
- int32_t d_sonar = (((int32_t)sonar_filtered - booz_ins_sonar_offset) *
BOOZ_INS_SONAR_SENS_NUM) / BOOZ_INS_SONAR_SENS_DEN;
- booz_ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar *
(BOOZ_INS_BARO_SENS_DEN))/BOOZ_INS_BARO_SENS_NUM;
- }
-#endif
-}
-
Deleted: paparazzi3/trunk/sw/airborne/booz/booz2_ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2010-09-28 14:04:15 UTC
(rev 6001)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_ins.h 2010-09-28 14:04:22 UTC
(rev 6002)
@@ -1,75 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * 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.
- */
-
-#ifndef BOOZ2_INS_H
-#define BOOZ2_INS_H
-
-#include "std.h"
-#include "math/pprz_geodetic_int.h"
-#include "math/pprz_algebra_float.h"
-
-/* gps transformed to LTP-NED */
-extern struct LtpDef_i booz_ins_ltp_def;
-extern bool_t booz_ins_ltp_initialised;
-extern struct NedCoor_i booz_ins_gps_pos_cm_ned;
-extern struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
-
-/* barometer */
-#ifdef USE_VFF
-extern int32_t booz_ins_baro_alt;
-extern int32_t booz_ins_qfe;
-extern bool_t booz_ins_baro_initialised;
-#ifdef BOOZ2_SONAR
-extern bool_t booz_ins_update_on_agl; /* use sonar to update agl if available
*/
-extern int32_t booz_ins_sonar_offset;
-#endif
-#endif
-
-/* output LTP NED */
-extern struct NedCoor_i booz_ins_ltp_pos;
-extern struct NedCoor_i booz_ins_ltp_speed;
-extern struct NedCoor_i booz_ins_ltp_accel;
-/* output LTP ENU */
-extern struct EnuCoor_i booz_ins_enu_pos;
-extern struct EnuCoor_i booz_ins_enu_speed;
-extern struct EnuCoor_i booz_ins_enu_accel;
-#ifdef USE_HFF
-/* horizontal gps transformed to NED in meters as float */
-extern struct FloatVect2 booz_ins_gps_pos_m_ned;
-extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
-#endif
-
-extern bool_t booz_ins_hf_realign;
-extern bool_t booz_ins_vf_realign;
-
-extern void booz_ins_init( void );
-extern void booz_ins_periodic( void );
-extern void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
-extern void booz_ins_realign_v(float z);
-extern void booz_ins_propagate( void );
-extern void booz_ins_update_baro( void );
-extern void booz_ins_update_gps( void );
-extern void booz_ins_update_sonar( void );
-
-
-#endif /* BOOZ2_INS_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
(from rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float-old.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,118 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#include "booz2_hf_float.h"
+#include "booz2_ins.h"
+
+#include <firmwares/rotorcraft/imu.h>
+#include "ahrs.h"
+#include "math/pprz_algebra_int.h"
+
+
+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 Int32Vect3 b2ins_meas_gps_pos_ned;
+struct Int32Vect3 b2ins_meas_gps_speed_ned;
+
+
+#include "downlink.h"
+
+void b2ins_init(void) {
+ INT32_VECT3_ZERO(b2ins_accel_bias);
+}
+
+void b2ins_propagate(void) {
+
+ struct Int32Vect3 scaled_biases;
+ VECT3_SDIV(scaled_biases, b2ins_accel_bias,
(1<<(B2INS_ACCEL_BIAS_FRAC-B2INS_ACCEL_LTP_FRAC)));
+ struct Int32Vect3 accel_imu;
+ /* unbias accelerometers */
+ VECT3_DIFF(accel_imu, imu.accel, scaled_biases);
+ /* convert to LTP */
+ // BOOZ_IQUAT_VDIV(b2ins_accel_ltp, ahrs.ltp_to_imu_quat, accel_imu);
+ INT32_RMAT_TRANSP_VMULT(b2ins_accel_ltp, ahrs.ltp_to_imu_rmat, accel_imu);
+ /* correct for gravity */
+ b2ins_accel_ltp.z += ACCEL_BFP_OF_REAL(9.81);
+ /* propagate position */
+ VECT3_ADD(b2ins_pos_ltp, b2ins_speed_ltp);
+ /* propagate speed */
+ VECT3_ADD(b2ins_speed_ltp, b2ins_accel_ltp);
+
+}
+
+#define K_POS 3
+/* make sure >=9 */
+#define K_SPEED 9
+
+#define UPDATE_FROM_POS 1
+#define UPDATE_FROM_SPEED 1
+
+void b2ins_update_gps(void) {
+
+#ifdef UPDATE_FROM_POS
+ struct Int64Vect2 scaled_pos_meas;
+ /* INT32 pos in cm -> INT64 pos in cm*/
+ VECT2_COPY(scaled_pos_meas, booz_ins_gps_pos_cm_ned);
+ /* to BFP but still in cm */
+ VECT2_SMUL(scaled_pos_meas, scaled_pos_meas, (1<<B2INS_POS_LTP_FRAC));
+ /* INT64 BFP pos in cm -> INT64 BFP pos in m */
+ VECT2_SDIV(scaled_pos_meas, scaled_pos_meas, 100);
+ struct Int64Vect2 pos_residual;
+ VECT2_DIFF(pos_residual, scaled_pos_meas, b2ins_pos_ltp);
+ struct Int32Vect2 pos_cor_1;
+ VECT2_SDIV(pos_cor_1, pos_residual, (1<<K_POS));
+ VECT2_ADD(b2ins_pos_ltp, pos_cor_1);
+ struct Int32Vect2 speed_cor_1;
+ VECT2_SDIV(speed_cor_1, pos_residual, (1<<(K_POS+9)));
+ VECT2_ADD(b2ins_speed_ltp, speed_cor_1);
+#endif /* UPDATE_FROM_POS */
+
+#ifdef UPDATE_FROM_SPEED
+ INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+ struct Int32Vect2 scaled_speed_meas;
+ VECT2_SMUL(scaled_speed_meas, b2ins_meas_gps_speed_ned,
(1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)));
+ struct Int32Vect2 speed_residual;
+ VECT2_DIFF(speed_residual, scaled_speed_meas, b2ins_speed_ltp);
+ struct Int32Vect2 pos_cor_s;
+ VECT2_SDIV(pos_cor_s, speed_residual, (1<<(K_SPEED-9)));
+ VECT2_ADD(b2ins_pos_ltp, pos_cor_s);
+ struct Int32Vect2 speed_cor_s;
+ VECT2_SDIV(speed_cor_s, speed_residual, (1<<K_SPEED));
+ VECT2_ADD(b2ins_speed_ltp, speed_cor_s);
+
+ struct Int32Vect3 speed_residual3;
+ VECT2_SDIV(speed_residual3, speed_residual, (1<<9));
+ speed_residual3.z = 0;
+ struct Int32Vect3 bias_cor_s;
+ INT32_RMAT_VMULT( bias_cor_s, ahrs.ltp_to_imu_rmat, speed_residual3);
+ VECT3_ADD(b2ins_accel_bias, bias_cor_s);
+#endif /* UPDATE_FROM_SPEED */
+
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h
(from rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float-old.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.h
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,46 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#ifndef BOOZ2_HF_FLOAT_H
+#define BOOZ2_HF_FLOAT_H
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+
+extern struct Int32Vect3 b2ins_accel_bias;
+#define B2INS_ACCEL_BIAS_FRAC 19
+extern struct Int32Vect3 b2ins_accel_ltp;
+#define B2INS_ACCEL_LTP_FRAC 10
+extern struct Int32Vect3 b2ins_speed_ltp;
+#define B2INS_SPEED_LTP_FRAC 19
+extern struct Int64Vect3 b2ins_pos_ltp;
+#define B2INS_POS_LTP_FRAC 28
+
+extern struct Int32Vect3 b2ins_meas_gps_pos_ned;
+extern struct Int32Vect3 b2ins_meas_gps_speed_ned;
+
+extern void b2ins_init(void);
+extern void b2ins_propagate(void);
+extern void b2ins_update_gps(void);
+
+#endif /* BOOZ2_HF_FLOAT_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c (from
rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,758 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * Copyright (C) 2009 Felix Ruess <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#include "booz2_hf_float.h"
+#include "booz2_ins.h"
+#include <firmwares/rotorcraft/imu.h>
+#include "ahrs.h"
+#include "booz_gps.h"
+#include <stdlib.h>
+
+#include "airframe.h"
+
+#ifdef SITL
+#include <stdio.h>
+#define DBG_LEVEL 1
+#define PRINT_DBG(_l, _p) { \
+ if (DBG_LEVEL >= _l) \
+ printf _p;
\
+}
+#else
+#define PRINT_DBG(_l, _p) {}
+#endif
+
+
+/* initial covariance diagonal */
+#define INIT_PXX 1.
+/* process noise (is the same for x and y)*/
+#ifndef B2_HFF_ACCEL_NOISE
+#define B2_HFF_ACCEL_NOISE 0.5
+#endif
+#define Q B2_HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
+#define Qdotdot B2_HFF_ACCEL_NOISE*DT_HFILTER
+
+//TODO: proper measurement noise
+#ifndef B2_HFF_R_POS
+#define B2_HFF_R_POS 8.
+#endif
+#ifndef B2_HFF_R_POS_MIN
+#define B2_HFF_R_POS_MIN 3.
+#endif
+
+#ifndef B2_HFF_R_SPEED
+#define B2_HFF_R_SPEED 2.
+#endif
+#ifndef B2_HFF_R_SPEED_MIN
+#define B2_HFF_R_SPEED_MIN 1.
+#endif
+
+/* gps measurement noise */
+float Rgps_pos, Rgps_vel;
+
+/*
+
+ X_x = [ x xdot]
+ X_y = [ y ydot]
+
+
+*/
+/* output filter states */
+struct HfilterFloat b2_hff_state;
+
+
+/* last acceleration measurement */
+float b2_hff_xdd_meas;
+float b2_hff_ydd_meas;
+
+/* last velocity measurement */
+float b2_hff_xd_meas;
+float b2_hff_yd_meas;
+
+/* last position measurement */
+float b2_hff_x_meas;
+float b2_hff_y_meas;
+
+/* counter for hff propagation*/
+int b2_hff_ps_counter;
+
+
+/*
+ * accel(in body frame) buffer
+ */
+#define ACC_RB_MAXN 64
+struct AccBuf {
+ struct Int32Vect3 buf[ACC_RB_MAXN];
+ int r; /* pos to read from, oldest measurement */
+ int w; /* pos to write to */
+ int n; /* number of elements in rb */
+ int size;
+};
+struct AccBuf acc_body;
+struct Int32Vect3 acc_body_mean;
+
+void b2_hff_store_accel_body(void) {
+ INT32_RMAT_TRANSP_VMULT(acc_body.buf[acc_body.w], imu.body_to_imu_rmat,
imu.accel);
+ acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0;
+
+ /* once the buffer is full it always has the last acc_body.size accel
measurements */
+ if (acc_body.n < acc_body.size) {
+ acc_body.n++;
+ } else {
+ acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0;
+ }
+}
+
+/* compute the mean of the last n accel measurements */
+static inline void b2_hff_compute_accel_body_mean(uint8_t n) {
+ struct Int32Vect3 sum;
+ int i, j;
+
+ INT_VECT3_ZERO(sum);
+
+ if (n > 1) {
+ if (n > acc_body.n) {
+ n = acc_body.n;
+ }
+ for (i = 1; i <= n; i++) {
+ j = (acc_body.w - i) > 0 ? (acc_body.w - i) : (acc_body.w - i +
acc_body.size);
+ VECT3_ADD(sum, acc_body.buf[j]);
+ }
+ VECT3_SDIV(acc_body_mean, sum, n);
+ } else {
+ VECT3_COPY(acc_body_mean, sum);
+ }
+}
+
+/*
+ * For GPS lag compensation
+ *
+ *
+ *
+ */
+#ifdef GPS_LAG
+/*
+ * GPS_LAG is defined in seconds in airframe file
+ */
+
+/* number of propagaton steps to redo according to GPS_LAG */
+#define GPS_LAG_N ((int) (GPS_LAG * HFF_FREQ + 0.5))
+/* number of propagation steps between two GPS updates */
+#define GPS_DT_N ((int) (HFF_FREQ / 4))
+/* tolerance of the GPS lag accuracy is +- GPS_LAG_TOLERANCE seconds */
+#define GPS_LAG_TOLERANCE 0.08
+#define GPS_LAG_TOL_N ((int) (GPS_LAG_TOLERANCE * HFF_FREQ + 0.5))
+
+/* maximum number of past propagation steps to rerun per ap cycle
+ * 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 mean accel buffer */
+#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 */
+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 */
+
+
+/*
+ * stuff for ringbuffer to store past filter states
+ */
+#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++;
\
+ }
+
+struct HfilterFloat b2_hff_rb[HFF_RB_MAXN]; /* ringbuffer with state and
covariance when GPS was valid */
+struct HfilterFloat *b2_hff_rb_put; /* write pointer */
+#endif /* GPS_LAG */
+
+struct HfilterFloat *b2_hff_rb_last; /* read pointer */
+int b2_hff_rb_n; /* fill count */
+
+
+/* by how many steps the estimated GPS validity point in time differed from
GPS_LAG_N */
+int lag_counter_err;
+
+/* counts down the propagation steps until the filter state is saved again */
+int save_counter;
+int past_save_counter;
+#define SAVE_NOW 0
+#define SAVING -1
+#define SAVE_DONE -2
+
+uint16_t b2_hff_lost_limit;
+uint16_t b2_hff_lost_counter;
+
+#ifdef GPS_LAG
+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_drop_last(void);
+static inline void b2_hff_set_state(struct HfilterFloat* dest, struct
HfilterFloat* source);
+#endif
+
+
+
+static inline void b2_hff_init_x(float init_x, float init_xdot);
+static inline void b2_hff_init_y(float init_y, float init_ydot);
+
+static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work);
+static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work);
+
+static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float
x_meas, float Rpos);
+static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float
y_meas, float Rpos);
+
+static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float
vel, float Rvel);
+static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float
vel, float Rvel);
+
+
+
+void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
{
+ Rgps_pos = B2_HFF_R_POS;
+ Rgps_vel = B2_HFF_R_SPEED;
+ b2_hff_init_x(init_x, init_xdot);
+ b2_hff_init_y(init_y, init_ydot);
+ /* init buffer for mean accel calculation */
+ acc_body.r = 0;
+ acc_body.w = 0;
+ acc_body.n = 0;
+ acc_body.size = ACC_RB_MAXN;
+#ifdef GPS_LAG
+ /* init buffer for past mean accel values */
+ acc_buf_r = 0;
+ acc_buf_w = 0;
+ 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);
+ printf("GPS_LAG_TOL_N: %f\n", GPS_LAG_TOL_N);
+#endif
+#else
+ b2_hff_rb_last = &b2_hff_state;
+ b2_hff_state.lag_counter = 0;
+#endif
+ b2_hff_rb_n = 0;
+ b2_hff_state.rollback = FALSE;
+ lag_counter_err = 0;
+ save_counter = -1;
+ past_save_counter = SAVE_DONE;
+ b2_hff_ps_counter = 1;
+ b2_hff_lost_counter = 0;
+ b2_hff_lost_limit = B2_HFF_LOST_LIMIT;
+}
+
+static inline void b2_hff_init_x(float init_x, float init_xdot) {
+ b2_hff_state.x = init_x;
+ b2_hff_state.xdot = init_xdot;
+ int i, j;
+ for (i=0; i<B2_HFF_STATE_SIZE; i++) {
+ for (j=0; j<B2_HFF_STATE_SIZE; j++)
+ b2_hff_state.xP[i][j] = 0.;
+ b2_hff_state.xP[i][i] = INIT_PXX;
+ }
+
+}
+
+static inline void b2_hff_init_y(float init_y, float init_ydot) {
+ b2_hff_state.y = init_y;
+ b2_hff_state.ydot = init_ydot;
+ int i, j;
+ for (i=0; i<B2_HFF_STATE_SIZE; i++) {
+ for (j=0; j<B2_HFF_STATE_SIZE; j++)
+ b2_hff_state.yP[i][j] = 0.;
+ b2_hff_state.yP[i][i] = INIT_PXX;
+ }
+}
+
+#ifdef GPS_LAG
+static inline void b2_hff_store_accel_ltp(float x, float y) {
+ past_accel[acc_buf_w].x = x;
+ past_accel[acc_buf_w].y = y;
+ INC_ACC_IDX(acc_buf_w);
+
+ if (acc_buf_n < ACC_BUF_MAXN) {
+ acc_buf_n++;
+ } else {
+ INC_ACC_IDX(acc_buf_r);
+ }
+}
+
+/* get the accel values from back_n steps ago */
+static inline void b2_hff_get_past_accel(unsigned int back_n) {
+ int i;
+ if (back_n > acc_buf_n) {
+ PRINT_DBG(1, ("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) {
+ PRINT_DBG(1, ("Cannot go back zero steps!\n"));
+ return;
+ }
+ 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;
+ PRINT_DBG(3, ("get past accel. buf_n: %2d \tbuf_w: %2d \tback_n: %2d \ti:
%2d \txdd: %f \tydd: %f\n", acc_buf_n, acc_buf_w, back_n, i, b2_hff_xdd_meas,
b2_hff_ydd_meas));
+}
+
+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 = FALSE;
+
+ /* forward write pointer */
+ INC_RB_POINTER(b2_hff_rb_put);
+
+ /* increase fill count and forward last pointer if neccessary */
+ if (b2_hff_rb_n < HFF_RB_MAXN) {
+ b2_hff_rb_n++;
+ } else {
+ INC_RB_POINTER(b2_hff_rb_last);
+ }
+ PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n));
+}
+
+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 {
+ PRINT_DBG(2, ("hff ringbuffer empty!\n"));
+ b2_hff_rb_last->lag_counter = 0;
+ b2_hff_rb_last->rollback = FALSE;
+ }
+ PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n));
+}
+
+
+/* copy source state to dest state */
+static inline void b2_hff_set_state(struct HfilterFloat* dest, struct
HfilterFloat* source) {
+ dest->x = source->x;
+ dest->xdot = source->xdot;
+ dest->xdotdot = source->xdotdot;
+ dest->y = source->y;
+ dest->ydot = source->ydot;
+ dest->ydotdot = source->ydotdot;
+ for (int i=0; i < B2_HFF_STATE_SIZE; i++) {
+ for (int j=0; j < B2_HFF_STATE_SIZE; j++) {
+ dest->xP[i][j] = source->xP[i][j];
+ dest->yP[i][j] = source->yP[i][j];
+ }
+ }
+}
+
+static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) {
+ PRINT_DBG(1, ("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++) {
+ if (hff_past->lag_counter > 0) {
+ b2_hff_get_past_accel(hff_past->lag_counter);
+ PRINT_DBG(2, ("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) {
+ past_save_counter--;
+ PRINT_DBG(2, ("dec past_save_counter: %d\n",
past_save_counter));
+ } else if (past_save_counter == SAVE_NOW) {
+ /* next GPS measurement valid at this state -> save */
+ PRINT_DBG(2, ("save past state\n"));
+ b2_hff_rb_put_state(hff_past);
+ past_save_counter = SAVING;
+ } else if (past_save_counter == SAVING) {
+ /* 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_drop_last();
+ past_save_counter = SAVE_DONE;
+ break;
+ }
+ }
+}
+#endif /* GPS_LAG */
+
+
+
+void b2_hff_propagate(void) {
+ if (b2_hff_lost_counter < b2_hff_lost_limit)
+ b2_hff_lost_counter++;
+
+#ifdef GPS_LAG
+ /* continue re-propagating to catch up with the present */
+ if (b2_hff_rb_last->rollback) {
+ b2_hff_propagate_past(b2_hff_rb_last);
+ }
+#endif
+
+ /* store body accelerations for mean computation */
+ b2_hff_store_accel_body();
+
+ /* propagate current state if it is time */
+ if (b2_hff_ps_counter == HFF_PRESCALER) {
+ b2_hff_ps_counter = 1;
+
+ if (b2_hff_lost_counter < b2_hff_lost_limit) {
+ /* compute float ltp mean acceleration */
+ b2_hff_compute_accel_body_mean(HFF_PRESCALER);
+ struct Int32Vect3 mean_accel_ltp;
+ INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, ahrs.ltp_to_body_rmat,
acc_body_mean);
+ 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_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
+#endif
+
+ /*
+ * propagate current state
+ */
+ b2_hff_propagate_x(&b2_hff_state);
+ b2_hff_propagate_y(&b2_hff_state);
+
+ /* update ins state from horizontal filter */
+ booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+
+#ifdef GPS_LAG
+ /* increase lag counter on last saved state */
+ if (b2_hff_rb_n > 0)
+ b2_hff_rb_last->lag_counter++;
+
+ /* save filter state if needed */
+ if (save_counter == 0) {
+ PRINT_DBG(1, ("save current state\n"));
+ b2_hff_rb_put_state(&b2_hff_state);
+ save_counter = -1;
+ } else if (save_counter > 0) {
+ save_counter--;
+ }
+#endif
+ }
+ } else {
+ b2_hff_ps_counter++;
+ }
+}
+
+
+
+
+void b2_hff_update_gps(void) {
+ b2_hff_lost_counter = 0;
+
+#ifdef USE_GPS_ACC4R
+ Rgps_pos = (float) booz_gps_state.pacc / 100.;
+ if (Rgps_pos < B2_HFF_R_POS_MIN)
+ Rgps_pos = B2_HFF_R_POS_MIN;
+
+ Rgps_vel = (float) booz_gps_state.sacc / 100.;
+ if (Rgps_vel < B2_HFF_R_SPEED_MIN)
+ Rgps_vel = B2_HFF_R_SPEED_MIN;
+#endif
+
+#ifdef GPS_LAG
+ if (GPS_LAG_N == 0) {
+#endif
+
+ /* update filter state with measurement */
+ b2_hff_update_x(&b2_hff_state, booz_ins_gps_pos_m_ned.x, Rgps_pos);
+ b2_hff_update_y(&b2_hff_state, booz_ins_gps_pos_m_ned.y, Rgps_pos);
+#ifdef B2_HFF_UPDATE_SPEED
+ b2_hff_update_xdot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.x, Rgps_vel);
+ b2_hff_update_ydot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.y, Rgps_vel);
+#endif
+
+ /* update ins state */
+ booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+
+#ifdef GPS_LAG
+ } else 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;
+ PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n",
b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
+ if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
+ b2_hff_rb_last->rollback = TRUE;
+ b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x, Rgps_pos);
+ b2_hff_update_y(b2_hff_rb_last, booz_ins_gps_pos_m_ned.y, Rgps_pos);
+#ifdef B2_HFF_UPDATE_SPEED
+ b2_hff_update_xdot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.x,
Rgps_vel);
+ b2_hff_update_ydot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.y,
Rgps_vel);
+#endif
+ past_save_counter = GPS_DT_N-1;// + lag_counter_err;
+ PRINT_DBG(2, ("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 - (GPS_LAG_TOL_N+1)) {
+ /* apparently missed a GPS update, try next saved state */
+ PRINT_DBG(2, ("try next saved state\n"));
+ b2_hff_rb_drop_last();
+ b2_hff_update_gps();
+ }
+ } else if (save_counter < 0) {
+ /* ringbuffer empty -> save output filter state at next GPS validity point
in time */
+ save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
+ PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
+ }
+
+#endif /* GPS_LAG */
+}
+
+
+void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) {
+ b2_hff_state.x = pos.x;
+ b2_hff_state.y = pos.y;
+ b2_hff_state.xdot = vel.x;
+ b2_hff_state.ydot = vel.y;
+#ifdef GPS_LAG
+ while (b2_hff_rb_n > 0) {
+ b2_hff_rb_drop_last();
+ }
+ save_counter = -1;
+ past_save_counter = SAVE_DONE;
+#endif
+}
+
+
+/*
+ *
+ * Propagation
+ *
+ *
+
+ F = [ 1 dt
+ 0 1 ];
+
+ B = [ dt^2/2 dt]';
+
+ Q = [ 0.01 0
+ 0 0.01];
+
+ Xk1 = F * Xk0 + B * accel;
+
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) {
+ /* update state */
+ 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 */
+ const float FPF00 = hff_work->xP[0][0] + DT_HFILTER * ( hff_work->xP[1][0] +
hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1] );
+ const float FPF01 = hff_work->xP[0][1] + DT_HFILTER * hff_work->xP[1][1];
+ const float FPF10 = hff_work->xP[1][0] + DT_HFILTER * hff_work->xP[1][1];
+ const float FPF11 = hff_work->xP[1][1];
+
+ hff_work->xP[0][0] = FPF00 + Q;
+ hff_work->xP[0][1] = FPF01;
+ hff_work->xP[1][0] = FPF10;
+ hff_work->xP[1][1] = FPF11 + Qdotdot;
+}
+
+static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) {
+ /* update state */
+ 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] );
+ const float FPF01 = hff_work->yP[0][1] + DT_HFILTER * hff_work->yP[1][1];
+ const float FPF10 = hff_work->yP[1][0] + DT_HFILTER * hff_work->yP[1][1];
+ const float FPF11 = hff_work->yP[1][1];
+
+ hff_work->yP[0][0] = FPF00 + Q;
+ hff_work->yP[0][1] = FPF01;
+ hff_work->yP[1][0] = FPF10;
+ hff_work->yP[1][1] = FPF11 + Qdotdot;
+}
+
+
+/*
+ *
+ * Update position
+ *
+ *
+
+ H = [1 0];
+ R = 0.1;
+ // state residual
+ y = pos_measurement - H * Xm;
+ // covariance residual
+ S = H*Pm*H' + R;
+ // kalman gain
+ K = Pm*H'*inv(S);
+ // update state
+ Xp = Xm + K*y;
+ // update covariance
+ Pp = Pm - K*H*Pm;
+*/
+void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) {
+ b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x);
+ b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y);
+}
+
+static inline void b2_hff_update_x(struct HfilterFloat* hff_work, float
x_meas, float Rpos) {
+ b2_hff_x_meas = x_meas;
+
+ const float y = x_meas - hff_work->x;
+ const float S = hff_work->xP[0][0] + Rpos;
+ const float K1 = hff_work->xP[0][0] * 1/S;
+ const float K2 = hff_work->xP[1][0] * 1/S;
+
+ hff_work->x = hff_work->x + K1 * y;
+ hff_work->xdot = hff_work->xdot + K2 * y;
+
+ const float P11 = (1. - K1) * hff_work->xP[0][0];
+ const float P12 = (1. - K1) * hff_work->xP[0][1];
+ const float P21 = -K2 * hff_work->xP[0][0] + hff_work->xP[1][0];
+ const float P22 = -K2 * hff_work->xP[0][1] + hff_work->xP[1][1];
+
+ hff_work->xP[0][0] = P11;
+ hff_work->xP[0][1] = P12;
+ hff_work->xP[1][0] = P21;
+ hff_work->xP[1][1] = P22;
+}
+
+static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float
y_meas, float Rpos) {
+ b2_hff_y_meas = y_meas;
+
+ const float y = y_meas - hff_work->y;
+ const float S = hff_work->yP[0][0] + Rpos;
+ const float K1 = hff_work->yP[0][0] * 1/S;
+ const float K2 = hff_work->yP[1][0] * 1/S;
+
+ hff_work->y = hff_work->y + K1 * y;
+ hff_work->ydot = hff_work->ydot + K2 * y;
+
+ const float P11 = (1. - K1) * hff_work->yP[0][0];
+ const float P12 = (1. - K1) * hff_work->yP[0][1];
+ const float P21 = -K2 * hff_work->yP[0][0] + hff_work->yP[1][0];
+ const float P22 = -K2 * hff_work->yP[0][1] + hff_work->yP[1][1];
+
+ hff_work->yP[0][0] = P11;
+ hff_work->yP[0][1] = P12;
+ hff_work->yP[1][0] = P21;
+ hff_work->yP[1][1] = P22;
+}
+
+
+
+
+/*
+ *
+ * Update velocity
+ *
+ *
+
+ H = [0 1];
+ R = 0.1;
+ // state residual
+ yd = vx - H * Xm;
+ // covariance residual
+ S = H*Pm*H' + R;
+ // kalman gain
+ K = Pm*H'*inv(S);
+ // update state
+ Xp = Xm + K*yd;
+ // update covariance
+ Pp = Pm - K*H*Pm;
+*/
+void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) {
+ b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x);
+ b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y);
+}
+
+static inline void b2_hff_update_xdot(struct HfilterFloat* hff_work, float
vel, float Rvel) {
+ b2_hff_xd_meas = vel;
+
+ const float yd = vel - hff_work->xdot;
+ const float S = hff_work->xP[1][1] + Rvel;
+ const float K1 = hff_work->xP[0][1] * 1/S;
+ const float K2 = hff_work->xP[1][1] * 1/S;
+
+ hff_work->x = hff_work->x + K1 * yd;
+ hff_work->xdot = hff_work->xdot + K2 * yd;
+
+ const float P11 = -K1 * hff_work->xP[1][0] + hff_work->xP[0][0];
+ const float P12 = -K1 * hff_work->xP[1][1] + hff_work->xP[0][1];
+ const float P21 = (1. - K2) * hff_work->xP[1][0];
+ const float P22 = (1. - K2) * hff_work->xP[1][1];
+
+ hff_work->xP[0][0] = P11;
+ hff_work->xP[0][1] = P12;
+ hff_work->xP[1][0] = P21;
+ hff_work->xP[1][1] = P22;
+}
+
+static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float
vel, float Rvel) {
+ b2_hff_yd_meas = vel;
+
+ const float yd = vel - hff_work->ydot;
+ const float S = hff_work->yP[1][1] + Rvel;
+ const float K1 = hff_work->yP[0][1] * 1/S;
+ const float K2 = hff_work->yP[1][1] * 1/S;
+
+ hff_work->y = hff_work->y + K1 * yd;
+ hff_work->ydot = hff_work->ydot + K2 * yd;
+
+ const float P11 = -K1 * hff_work->yP[1][0] + hff_work->yP[0][0];
+ const float P12 = -K1 * hff_work->yP[1][1] + hff_work->yP[0][1];
+ const float P21 = (1. - K2) * hff_work->yP[1][0];
+ const float P22 = (1. - K2) * hff_work->yP[1][1];
+
+ hff_work->yP[0][0] = P11;
+ hff_work->yP[0][1] = P12;
+ hff_work->yP[1][0] = P21;
+ hff_work->yP[1][1] = P22;
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.h (from
rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_hf_float.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.h
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,83 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#ifndef BOOZ2_HF_FLOAT_H
+#define BOOZ2_HF_FLOAT_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+#define B2_HFF_STATE_SIZE 2
+
+#ifndef HFF_PRESCALER
+#define HFF_PRESCALER 16
+#endif
+
+/* horizontal filter propagation frequency */
+#define HFF_FREQ (512./HFF_PRESCALER)
+#define DT_HFILTER (1./HFF_FREQ)
+
+#define B2_HFF_UPDATE_SPEED
+
+struct HfilterFloat {
+ float x;
+ /* float xbias; */
+ float xdot;
+ float xdotdot;
+ float y;
+ /* float ybias; */
+ float ydot;
+ float ydotdot;
+ float xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+ float yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+ uint8_t lag_counter;
+ 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(struct FloatVect2 pos, struct FloatVect2 Rpos);
+extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
+extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
+
+#define B2_HFF_LOST_LIMIT 1000
+extern uint16_t b2_hff_lost_limit;
+extern uint16_t b2_hff_lost_counter;
+
+extern void b2_hff_store_accel_body(void);
+
+extern struct HfilterFloat *b2_hff_rb_last;
+extern int lag_counter_err;
+extern int save_counter;
+
+#endif /* BOOZ2_HF_FLOAT_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c (from
rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,222 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#include "booz2_vf_float.h"
+
+/*
+
+X = [ z zdot bias ]
+
+temps :
+ propagate 86us
+ update 46us
+
+*/
+/* initial covariance diagonal */
+#define INIT_PXX 1.
+/* process noise */
+#define ACCEL_NOISE 0.5
+#define Qzz ACCEL_NOISE/512./512./2.
+#define Qzdotzdot ACCEL_NOISE/512.
+#define Qbiasbias 1e-7
+#define R 1.
+
+float b2_vff_z;
+float b2_vff_bias;
+float b2_vff_zdot;
+float b2_vff_zdotdot;
+
+float b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE];
+
+float b2_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;
+ 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;
+ }
+
+}
+
+
+/*
+
+ F = [ 1 dt -dt^2/2
+ 0 1 -dt
+ 0 0 1 ];
+
+ B = [ dt^2/2 dt 0]';
+
+ Q = [ 0.01 0 0
+ 0 0.01 0
+ 0 0 0.001 ];
+
+ Xk1 = F * Xk0 + B * accel;
+
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+void b2_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;
+ /* 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];
+
+ 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;
+
+}
+/*
+ H = [1 0 0];
+ R = 0.1;
+ // state residual
+ y = rangemeter - H * Xm;
+ // covariance residual
+ S = H*Pm*H' + R;
+ // kalman gain
+ K = Pm*H'*inv(S);
+ // update state
+ Xp = Xm + K*y;
+ // update covariance
+ Pp = Pm - K*H*Pm;
+*/
+static inline void update_z_conf(float z_meas, float conf) {
+ b2_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;
+
+ b2_vff_z = b2_vff_z + K1 * y;
+ b2_vff_zdot = b2_vff_zdot + K2 * y;
+ b2_vff_bias = b2_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];
+
+ 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;
+
+}
+
+void b2_vff_update(float z_meas) {
+ update_z_conf(z_meas, R);
+}
+
+void b2_vff_update_z_conf(float z_meas, float conf) {
+ update_z_conf(z_meas, conf);
+}
+
+/*
+ H = [0 1 0];
+ R = 0.1;
+ // state residual
+ yd = vz - H * Xm;
+ // covariance residual
+ S = H*Pm*H' + R;
+ // kalman gain
+ K = Pm*H'*inv(S);
+ // update state
+ Xp = Xm + K*yd;
+ // update covariance
+ 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;
+
+ 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];
+ 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];
+
+ 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;
+
+}
+
+void b2_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;
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h (from
rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_float.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,44 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#ifndef BOOZ2_VF_FLOAT_H
+#define BOOZ2_VF_FLOAT_H
+
+#define B2_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 b2_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);
+
+#endif /* BOOZ2_VF_FLOAT_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c (from
rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_int.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,158 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#include "booz2_vf_int.h"
+
+#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];
+
+/* 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)
+
+/* 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)
+
+
+void booz2_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;
+ // 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;
+
+}
+
+/*
+
+ F = [ 1 dt -dt^2/2
+ 0 1 -dt
+ 0 0 1 ];
+
+ B = [ dt^2/2 dt 0]';
+
+ Q = [ 0.01 0 0
+ 0 0.01 0
+ 0 0 0.001 ];
+
+ Xk1 = F * Xk0 + B * accel;
+
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+
+void booz2_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;
+ // 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;
+
+ // 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];
+
+ 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;
+
+}
+
+
+void booz2_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 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
+
+ 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];
+
+ tl_vf_P[0][0] = P11;
+ tl_vf_P[0][1] = P12;
+ tl_vf_P[0][2] = P13;
+ tl_vf_P[1][0] = P21;
+ tl_vf_P[1][1] = P22;
+ tl_vf_P[1][2] = P23;
+ tl_vf_P[2][0] = P31;
+ tl_vf_P[2][1] = P32;
+ tl_vf_P[2][2] = P33;
+#endif
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h (from
rev 6001, paparazzi3/trunk/sw/airborne/booz/ins/booz2_vf_int.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
2010-09-28 14:04:22 UTC (rev 6002)
@@ -0,0 +1,75 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#ifndef BOOZ2_VF_INT_H
+#define BOOZ2_VF_INT_H
+
+#include "std.h"
+#include "booz_geometry_int.h"
+
+extern void booz2_vfi_init( int32_t z0, int32_t zd0, int32_t bias0 );
+extern void booz2_vfi_propagate( int32_t accel_reading );
+
+/* z_meas : altitude measurement in meter */
+/* Q23.8 : accuracy 0.004m range 8388km */
+extern void booz2_vfi_update( int32_t z_meas );
+#define B2_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)
+
+/* 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
+
+/* 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
+
+/* 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)
+
+/* 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)
+
+/* 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
+/* 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
+
+
+
+
+#endif /* BOOZ2_VF_INT_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c (from rev 6001,
paparazzi3/trunk/sw/airborne/booz/booz2_ins.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-09-28
14:04:22 UTC (rev 6002)
@@ -0,0 +1,282 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * Copyright (C) 2009 Felix Ruess <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#include "booz2_ins.h"
+
+#include <firmwares/rotorcraft/imu.h>
+#include "firmwares/rotorcraft/baro.h"
+#include "booz_gps.h"
+
+#include "airframe.h"
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+
+#include "ahrs.h"
+
+#ifdef USE_VFF
+#include "ins/booz2_vf_float.h"
+#endif
+
+#ifdef USE_HFF
+#include "ins/booz2_hf_float.h"
+#endif
+
+#ifdef BOOZ2_SONAR
+#include "modules.h"
+#endif
+
+#ifdef SITL
+#include "nps_fdm.h"
+#include <stdio.h>
+#endif
+
+
+#include "math/pprz_geodetic_int.h"
+
+#include "flight_plan.h"
+
+/* gps transformed to LTP-NED */
+struct LtpDef_i booz_ins_ltp_def;
+ bool_t booz_ins_ltp_initialised;
+struct NedCoor_i booz_ins_gps_pos_cm_ned;
+struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
+#ifdef USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+struct FloatVect2 booz_ins_gps_pos_m_ned;
+struct FloatVect2 booz_ins_gps_speed_m_s_ned;
+#endif
+bool_t booz_ins_hf_realign;
+
+/* barometer */
+#ifdef USE_VFF
+int32_t booz_ins_qfe;
+bool_t booz_ins_baro_initialised;
+int32_t booz_ins_baro_alt;
+#ifdef BOOZ2_SONAR
+bool_t booz_ins_update_on_agl;
+int32_t booz_ins_sonar_offset;
+#endif
+#endif
+bool_t booz_ins_vf_realign;
+
+/* output */
+struct NedCoor_i booz_ins_ltp_pos;
+struct NedCoor_i booz_ins_ltp_speed;
+struct NedCoor_i booz_ins_ltp_accel;
+struct EnuCoor_i booz_ins_enu_pos;
+struct EnuCoor_i booz_ins_enu_speed;
+struct EnuCoor_i booz_ins_enu_accel;
+
+
+void booz_ins_init() {
+#ifdef USE_INS_NAV_INIT
+ booz_ins_ltp_initialised = TRUE;
+
+ /** FIXME: should use the same code than MOVE_WP in booz2_datalink.c */
+ struct LlaCoor_i llh; /* Height above the ellipsoid */
+ llh.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+ llh.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ //llh.alt = NAV_ALT0 - booz_ins_ltp_def.hmsl + booz_ins_ltp_def.lla.alt;
+ llh.alt = NAV_ALT0 + NAV_HMSL0;
+
+ struct EcefCoor_i nav_init;
+ ecef_of_lla_i(&nav_init, &llh);
+
+ ltp_def_from_ecef_i(&booz_ins_ltp_def, &nav_init);
+ booz_ins_ltp_def.hmsl = NAV_ALT0;
+#else
+ booz_ins_ltp_initialised = FALSE;
+#endif
+#ifdef USE_VFF
+ booz_ins_baro_initialised = FALSE;
+#ifdef BOOZ2_SONAR
+ booz_ins_update_on_agl = FALSE;
+#endif
+ b2_vff_init(0., 0., 0.);
+#endif
+ booz_ins_vf_realign = FALSE;
+ booz_ins_hf_realign = FALSE;
+#ifdef USE_HFF
+ b2_hff_init(0., 0., 0., 0.);
+#endif
+ INT32_VECT3_ZERO(booz_ins_ltp_pos);
+ INT32_VECT3_ZERO(booz_ins_ltp_speed);
+ INT32_VECT3_ZERO(booz_ins_ltp_accel);
+ INT32_VECT3_ZERO(booz_ins_enu_pos);
+ INT32_VECT3_ZERO(booz_ins_enu_speed);
+ INT32_VECT3_ZERO(booz_ins_enu_accel);
+}
+
+void booz_ins_periodic( void ) {
+}
+
+#ifdef USE_HFF
+void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
+ b2_hff_realign(pos, speed);
+}
+#else
+void booz_ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct
FloatVect2 speed __attribute__ ((unused))) {}
+#endif /* USE_HFF */
+
+
+void booz_ins_realign_v(float z) {
+#ifdef USE_VFF
+ b2_vff_realign(z);
+#endif
+}
+
+void booz_ins_propagate() {
+ /* untilt accels */
+ struct Int32Vect3 accel_body;
+ INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
+ struct Int32Vect3 accel_ltp;
+ INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
+ float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
+
+#ifdef USE_VFF
+ if (baro.status == BS_RUNNING && booz_ins_baro_initialised) {
+ b2_vff_propagate(z_accel_float);
+ booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
+ booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
+ booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ }
+ else { // feed accel from the sensors
+ booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
+ }
+#else
+ booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
+#endif /* USE_VFF */
+
+#ifdef USE_HFF
+ /* propagate horizontal filter */
+ b2_hff_propagate();
+#else
+ 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);
+}
+
+void booz_ins_update_baro() {
+#ifdef USE_VFF
+ if (baro.status == BS_RUNNING) {
+ if (!booz_ins_baro_initialised) {
+ booz_ins_qfe = baro.absolute;
+ booz_ins_baro_initialised = TRUE;
+ }
+ booz_ins_baro_alt = ((baro.absolute - booz_ins_qfe) *
BOOZ_INS_BARO_SENS_NUM)/BOOZ_INS_BARO_SENS_DEN;
+ float alt_float = POS_FLOAT_OF_BFP(booz_ins_baro_alt);
+ if (booz_ins_vf_realign) {
+ booz_ins_vf_realign = FALSE;
+ booz_ins_qfe = baro.absolute;
+#ifdef BOOZ2_SONAR
+ booz_ins_sonar_offset = sonar_meas;
+#endif
+ b2_vff_realign(0.);
+ booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
+ booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
+ booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
+ booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
+ booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
+ }
+ b2_vff_update(alt_float);
+ }
+#endif
+}
+
+
+void booz_ins_update_gps(void) {
+#ifdef USE_GPS
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
+ if (!booz_ins_ltp_initialised) {
+ ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos);
+ booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
+ booz_ins_ltp_def.hmsl = booz_gps_state.hmsl;
+ booz_ins_ltp_initialised = TRUE;
+ }
+ ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def,
&booz_gps_state.ecef_pos);
+ ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def,
&booz_gps_state.ecef_vel);
+#ifdef USE_HFF
+ VECT2_ASSIGN(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x,
booz_ins_gps_pos_cm_ned.y);
+ VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
+ VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x,
booz_ins_gps_speed_cm_s_ned.y);
+ VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
+ if (booz_ins_hf_realign) {
+ booz_ins_hf_realign = FALSE;
+#ifdef SITL
+ struct FloatVect2 true_pos, true_speed;
+ VECT2_COPY(true_pos, fdm.ltpprz_pos);
+ VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
+ b2_hff_realign(true_pos, true_speed);
+#else
+ const struct FloatVect2 zero = {0.0, 0.0};
+ b2_hff_realign(booz_ins_gps_pos_m_ned, zero);
+#endif
+ }
+ b2_hff_update_gps();
+#ifndef USE_VFF /* vff not used */
+ booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) /
INT32_POS_OF_CM_DEN;
+ booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z *
INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
+#endif /* vff not used */
+#endif /* hff used */
+
+
+#ifndef USE_HFF /* hff not used */
+#ifndef USE_VFF /* neither hf nor vf used */
+ INT32_VECT3_SCALE_3(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+ INT32_VECT3_SCALE_3(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#else /* only vff used */
+ INT32_VECT2_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+ INT32_VECT2_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);
+#endif
+
+#ifdef USE_GPS_LAG_HACK
+ 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 /* hff not used */
+
+ 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);
+ }
+#endif /* USE_GPS */
+}
+
+void booz_ins_update_sonar() {
+#if defined BOOZ2_SONAR && defined USE_VFF
+ static int32_t sonar_filtered = 0;
+ sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
+ /* update baro_qfe assuming a flat ground */
+ if (booz_ins_update_on_agl && booz2_analog_baro_status ==
BOOZ2_ANALOG_BARO_RUNNING) {
+ int32_t d_sonar = (((int32_t)sonar_filtered - booz_ins_sonar_offset) *
BOOZ_INS_SONAR_SENS_NUM) / BOOZ_INS_SONAR_SENS_DEN;
+ booz_ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar *
(BOOZ_INS_BARO_SENS_DEN))/BOOZ_INS_BARO_SENS_NUM;
+ }
+#endif
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h (from rev 6001,
paparazzi3/trunk/sw/airborne/booz/booz2_ins.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h 2010-09-28
14:04:22 UTC (rev 6002)
@@ -0,0 +1,75 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * 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.
+ */
+
+#ifndef BOOZ2_INS_H
+#define BOOZ2_INS_H
+
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_algebra_float.h"
+
+/* gps transformed to LTP-NED */
+extern struct LtpDef_i booz_ins_ltp_def;
+extern bool_t booz_ins_ltp_initialised;
+extern struct NedCoor_i booz_ins_gps_pos_cm_ned;
+extern struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
+
+/* barometer */
+#ifdef USE_VFF
+extern int32_t booz_ins_baro_alt;
+extern int32_t booz_ins_qfe;
+extern bool_t booz_ins_baro_initialised;
+#ifdef BOOZ2_SONAR
+extern bool_t booz_ins_update_on_agl; /* use sonar to update agl if available
*/
+extern int32_t booz_ins_sonar_offset;
+#endif
+#endif
+
+/* output LTP NED */
+extern struct NedCoor_i booz_ins_ltp_pos;
+extern struct NedCoor_i booz_ins_ltp_speed;
+extern struct NedCoor_i booz_ins_ltp_accel;
+/* output LTP ENU */
+extern struct EnuCoor_i booz_ins_enu_pos;
+extern struct EnuCoor_i booz_ins_enu_speed;
+extern struct EnuCoor_i booz_ins_enu_accel;
+#ifdef USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+extern struct FloatVect2 booz_ins_gps_pos_m_ned;
+extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
+#endif
+
+extern bool_t booz_ins_hf_realign;
+extern bool_t booz_ins_vf_realign;
+
+extern void booz_ins_init( void );
+extern void booz_ins_periodic( void );
+extern void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
+extern void booz_ins_realign_v(float z);
+extern void booz_ins_propagate( void );
+extern void booz_ins_update_baro( void );
+extern void booz_ins_update_gps( void );
+extern void booz_ins_update_sonar( void );
+
+
+#endif /* BOOZ2_INS_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6002] moving booz ins, still need to rename,
Felix Ruess <=