[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6274] moved ins to general subsystem
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6274] moved ins to general subsystem |
|
Date: |
Tue, 26 Oct 2010 18:03:28 +0000 |
Revision: 6274
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6274
Author: flixr
Date: 2010-10-26 18:03:28 +0000 (Tue, 26 Oct 2010)
Log Message:
-----------
moved ins to general subsystem
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/booz_stabilization_int.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
paparazzi3/trunk/conf/modules/ins_xsens.xml
paparazzi3/trunk/conf/modules/ins_xsens_MTiG_Uart0.xml
paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml
paparazzi3/trunk/conf/modules/ins_xsens_MTi_Uart0.xml
paparazzi3/trunk/conf/settings/settings_UofAdelaide.xml
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h
paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h
paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c
paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/subsystems/ins/
paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.c
paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.h
paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.c
paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.h
paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.c
paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.h
paparazzi3/trunk/sw/airborne/subsystems/ins.c
paparazzi3/trunk/sw/airborne/subsystems/ins.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-10-26 17:09:15 UTC
(rev 6273)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-10-26 18:03:28 UTC
(rev 6274)
@@ -196,7 +196,7 @@
ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
-ap.srcs += $(SRC_FIRMWARE)/ins.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
math/pprz_geodetic_double.c
#
@@ -208,7 +208,7 @@
#
# vertical filter float version
-ap.srcs += $(SRC_FIRMWARE)/ins/vf_float.c
+ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)'
ap.srcs += $(SRC_FIRMWARE)/navigation.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/booz_stabilization_int.makefile
===================================================================
---
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/booz_stabilization_int.makefile
2010-10-26 17:09:15 UTC (rev 6273)
+++
paparazzi3/trunk/conf/autopilot/subsystems/lisa_passthrough/booz_stabilization_int.makefile
2010-10-26 18:03:28 UTC (rev 6274)
@@ -8,10 +8,10 @@
stm_passthrough.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
stm_passthrough.CFLAGS += -DUSE_NAVIGATION
-stm_passthrough.srcs += $(SRC_FIRMWARE)/ins.c
+stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins.c
stm_passthrough.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
math/pprz_geodetic_double.c
stm_passthrough.srcs += $(SRC_FIRMWARE)/navigation.c
-stm_passthrough.srcs += $(SRC_FIRMWARE)/ins/vf_float.c
+stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
stm_passthrough.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)'
stm_passthrough.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
Modified: paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-10-26 18:03:28 UTC (rev 6274)
@@ -147,10 +147,10 @@
sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
sim.srcs += math/pprz_geodetic_int.c
-sim.srcs += $(SRC_FIRMWARE)/ins.c
+sim.srcs += $(SRC_SUBSYSTEMS)/ins.c
# vertical filter float version
-sim.srcs += $(SRC_FIRMWARE)/ins/vf_float.c
+sim.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
sim.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)'
#
Modified: paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
2010-10-26 18:03:28 UTC (rev 6274)
@@ -2,8 +2,5 @@
# simple horizontal filter for INS
#
-ap.CFLAGS += -DUSE_HFF
-ap.srcs += $(SRC_FIRMWARE)/ins/hf_float.c
-
-sim.CFLAGS += -DUSE_HFF
-sim.srcs += $(SRC_FIRMWARE)/ins/hf_float.c
+$(TARGET).CFLAGS += -DUSE_HFF
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/hf_float.c
Modified: paparazzi3/trunk/conf/modules/ins_xsens.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_xsens.xml 2010-10-26 17:09:15 UTC (rev
6273)
+++ paparazzi3/trunk/conf/modules/ins_xsens.xml 2010-10-26 18:03:28 UTC (rev
6274)
@@ -3,7 +3,7 @@
<module name="ins">
<!-- <depend conflict="ins" -->
<header>
- <file name="ins.h"/>
+ <file name="subsystems/ins.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="60"/>
Modified: paparazzi3/trunk/conf/modules/ins_xsens_MTiG_Uart0.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_xsens_MTiG_Uart0.xml 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/conf/modules/ins_xsens_MTiG_Uart0.xml 2010-10-26
18:03:28 UTC (rev 6274)
@@ -4,7 +4,7 @@
<!-- <depend conflict="ins" -->
<!-- <depend require="gps_xsens" -->
<header>
- <file name="ins.h"/>
+ <file name="subsystems/ins.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="60"/>
Modified: paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/conf/modules/ins_xsens_MTiG_fixedwing.xml 2010-10-26
18:03:28 UTC (rev 6274)
@@ -4,7 +4,7 @@
<!-- <depend conflict="ins" -->
<!-- <depend require="gps_xsens" -->
<header>
- <file name="ins.h"/>
+ <file name="subsystems/ins.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="60"/>
Modified: paparazzi3/trunk/conf/modules/ins_xsens_MTi_Uart0.xml
===================================================================
--- paparazzi3/trunk/conf/modules/ins_xsens_MTi_Uart0.xml 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/conf/modules/ins_xsens_MTi_Uart0.xml 2010-10-26
18:03:28 UTC (rev 6274)
@@ -3,7 +3,7 @@
<module name="ins">
<!-- <depend conflict="ins" -->
<header>
- <file name="ins.h"/>
+ <file name="subsystems/ins.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="60"/>
Modified: paparazzi3/trunk/conf/settings/settings_UofAdelaide.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_UofAdelaide.xml 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/conf/settings/settings_UofAdelaide.xml 2010-10-26
18:03:28 UTC (rev 6274)
@@ -59,7 +59,7 @@
<dl_setting var="guidance_v_kd" min="-600" step="1" max="0"
module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="-300" step="1" max="0"
module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3"
module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m"
alt_unit_coef="0.00390625"/>
- <dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="ins"
shortname="vf_realign" values="OFF|ON"/>
+ <dl_setting var="ins_vf_realign" min="0" step="1" max="1"
module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
@@ -71,7 +71,7 @@
<dl_setting var="guidance_h_igain" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="ka"/>
- <dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="ins"
shortname="hf_realign" values="OFF|ON"/>
+ <dl_setting var="ins_hf_realign" min="0" step="1" max="1"
module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="NAV">
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-10-26 17:09:15 UTC
(rev 6273)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-10-26 18:03:28 UTC
(rev 6274)
@@ -48,7 +48,7 @@
<dl_setting var="guidance_v_kd" min="-600" step="1" max="0"
module="guidance/guidance_v" shortname="kd"/>
<dl_setting var="guidance_v_ki" min="-300" step="1" max="0"
module="guidance/guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="guidance_v_z_sp" min="-5" step="0.5" max="3"
module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m"
alt_unit_coef="0.00390625"/>
- <dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="ins"
shortname="vf_realign" values="OFF|ON"/>
+ <dl_setting var="ins_vf_realign" min="0" step="1" max="1"
module="subsystems/ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
@@ -60,7 +60,7 @@
<dl_setting var="guidance_h_igain" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="guidance_h_ngain" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="kn"/>
<dl_setting var="guidance_h_again" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="ka"/>
- <dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="ins"
shortname="hf_realign" values="OFF|ON"/>
+ <dl_setting var="ins_hf_realign" min="0" step="1" max="1"
module="subsystems/ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="NAV">
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2010-10-26 17:09:15 UTC
(rev 6273)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2010-10-26 18:03:28 UTC
(rev 6274)
@@ -41,7 +41,7 @@
#include "firmwares/rotorcraft/navigation.h"
#include "math/pprz_geodetic_int.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#define IdOfMsg(x) (x[1])
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
2010-10-26 18:03:28 UTC (rev 6274)
@@ -30,7 +30,7 @@
#include "led.h"
#include "airframe.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#define AP_MODE_FAILSAFE 0
#define AP_MODE_KILL 1
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-10-26 18:03:28 UTC (rev 6274)
@@ -28,7 +28,7 @@
#include "subsystems/ahrs.h"
#include "firmwares/rotorcraft/stabilization.h"
// #include "booz_fms.h" FIXME
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "firmwares/rotorcraft/navigation.h"
#include "airframe.h"
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-10-26 18:03:28 UTC (rev 6274)
@@ -32,7 +32,7 @@
// #include "booz_fms.h" FIXME
#include "firmwares/rotorcraft/navigation.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "math/pprz_algebra_int.h"
#include "airframe.h"
Deleted: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,282 +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 "firmwares/rotorcraft/ins.h"
-
-#include "subsystems/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 "subsystems/ahrs.h"
-
-#ifdef USE_VFF
-#include "ins/vf_float.h"
-#endif
-
-#ifdef USE_HFF
-#include "ins/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 ins_ltp_def;
- bool_t ins_ltp_initialised;
-struct NedCoor_i ins_gps_pos_cm_ned;
-struct NedCoor_i ins_gps_speed_cm_s_ned;
-#ifdef USE_HFF
-/* horizontal gps transformed to NED in meters as float */
-struct FloatVect2 ins_gps_pos_m_ned;
-struct FloatVect2 ins_gps_speed_m_s_ned;
-#endif
-bool_t ins_hf_realign;
-
-/* barometer */
-#ifdef USE_VFF
-int32_t ins_qfe;
-bool_t ins_baro_initialised;
-int32_t ins_baro_alt;
-#ifdef BOOZ2_SONAR
-bool_t ins_update_on_agl;
-int32_t ins_sonar_offset;
-#endif
-#endif
-bool_t ins_vf_realign;
-
-/* output */
-struct NedCoor_i ins_ltp_pos;
-struct NedCoor_i ins_ltp_speed;
-struct NedCoor_i ins_ltp_accel;
-struct EnuCoor_i ins_enu_pos;
-struct EnuCoor_i ins_enu_speed;
-struct EnuCoor_i ins_enu_accel;
-
-
-void ins_init() {
-#ifdef USE_INS_NAV_INIT
- 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 - ins_ltp_def.hmsl + 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(&ins_ltp_def, &nav_init);
- ins_ltp_def.hmsl = NAV_ALT0;
-#else
- ins_ltp_initialised = FALSE;
-#endif
-#ifdef USE_VFF
- ins_baro_initialised = FALSE;
-#ifdef BOOZ2_SONAR
- ins_update_on_agl = FALSE;
-#endif
- vff_init(0., 0., 0.);
-#endif
- ins_vf_realign = FALSE;
- ins_hf_realign = FALSE;
-#ifdef USE_HFF
- b2_hff_init(0., 0., 0., 0.);
-#endif
- INT32_VECT3_ZERO(ins_ltp_pos);
- INT32_VECT3_ZERO(ins_ltp_speed);
- INT32_VECT3_ZERO(ins_ltp_accel);
- INT32_VECT3_ZERO(ins_enu_pos);
- INT32_VECT3_ZERO(ins_enu_speed);
- INT32_VECT3_ZERO(ins_enu_accel);
-}
-
-void ins_periodic( void ) {
-}
-
-#ifdef USE_HFF
-void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
- b2_hff_realign(pos, speed);
-}
-#else
-void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct
FloatVect2 speed __attribute__ ((unused))) {}
-#endif /* USE_HFF */
-
-
-void ins_realign_v(float z) {
-#ifdef USE_VFF
- vff_realign(z);
-#endif
-}
-
-void 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 && ins_baro_initialised) {
- vff_propagate(z_accel_float);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
- }
- else { // feed accel from the sensors
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
- }
-#else
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
-#endif /* USE_VFF */
-
-#ifdef USE_HFF
- /* propagate horizontal filter */
- b2_hff_propagate();
-#else
- ins_ltp_accel.x = accel_ltp.x;
- ins_ltp_accel.y = accel_ltp.y;
-#endif /* USE_HFF */
-
- INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
- INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
- INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
-}
-
-void ins_update_baro() {
-#ifdef USE_VFF
- if (baro.status == BS_RUNNING) {
- if (!ins_baro_initialised) {
- ins_qfe = baro.absolute;
- ins_baro_initialised = TRUE;
- }
- ins_baro_alt = ((baro.absolute - ins_qfe) *
INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
- float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
- if (ins_vf_realign) {
- ins_vf_realign = FALSE;
- ins_qfe = baro.absolute;
-#ifdef BOOZ2_SONAR
- ins_sonar_offset = sonar_meas;
-#endif
- vff_realign(0.);
- ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
- ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
- ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
- ins_enu_pos.z = -ins_ltp_pos.z;
- ins_enu_speed.z = -ins_ltp_speed.z;
- ins_enu_accel.z = -ins_ltp_accel.z;
- }
- vff_update(alt_float);
- }
-#endif
-}
-
-
-void ins_update_gps(void) {
-#ifdef USE_GPS
- if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
- if (!ins_ltp_initialised) {
- ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos);
- ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
- ins_ltp_def.hmsl = booz_gps_state.hmsl;
- ins_ltp_initialised = TRUE;
- }
- ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def,
&booz_gps_state.ecef_pos);
- ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def,
&booz_gps_state.ecef_vel);
-#ifdef USE_HFF
- VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x,
ins_gps_pos_cm_ned.y);
- VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
- VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x,
ins_gps_speed_cm_s_ned.y);
- VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
- if (ins_hf_realign) {
- 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(ins_gps_pos_m_ned, zero);
-#endif
- }
- b2_hff_update_gps();
-#ifndef USE_VFF /* vff not used */
- ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) /
INT32_POS_OF_CM_DEN;
- ins_ltp_speed.z = (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(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM,
INT32_POS_OF_CM_DEN);
- INT32_VECT3_SCALE_3(ins_ltp_speed, 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(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM,
INT32_POS_OF_CM_DEN);
- INT32_VECT2_SCALE_2(ins_ltp_speed, 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, ins_ltp_speed);
- INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
- VECT2_ADD(ins_ltp_pos, d_pos);
-#endif
-#endif /* hff not used */
-
- INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
- INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
- INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
- }
-#endif /* USE_GPS */
-}
-
-void 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 (ins_update_on_agl && booz2_analog_baro_status ==
BOOZ2_ANALOG_BARO_RUNNING) {
- int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) *
INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
- ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar *
(INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
- }
-#endif
-}
Deleted: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 INS_H
-#define 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 ins_ltp_def;
-extern bool_t ins_ltp_initialised;
-extern struct NedCoor_i ins_gps_pos_cm_ned;
-extern struct NedCoor_i ins_gps_speed_cm_s_ned;
-
-/* barometer */
-#ifdef USE_VFF
-extern int32_t ins_baro_alt;
-extern int32_t ins_qfe;
-extern bool_t ins_baro_initialised;
-#ifdef BOOZ2_SONAR
-extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
-extern int32_t ins_sonar_offset;
-#endif
-#endif
-
-/* output LTP NED */
-extern struct NedCoor_i ins_ltp_pos;
-extern struct NedCoor_i ins_ltp_speed;
-extern struct NedCoor_i ins_ltp_accel;
-/* output LTP ENU */
-extern struct EnuCoor_i ins_enu_pos;
-extern struct EnuCoor_i ins_enu_speed;
-extern struct EnuCoor_i ins_enu_accel;
-#ifdef USE_HFF
-/* horizontal gps transformed to NED in meters as float */
-extern struct FloatVect2 ins_gps_pos_m_ned;
-extern struct FloatVect2 ins_gps_speed_m_s_ned;
-#endif
-
-extern bool_t ins_hf_realign;
-extern bool_t ins_vf_realign;
-
-extern void ins_init( void );
-extern void ins_periodic( void );
-extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
-extern void ins_realign_v(float z);
-extern void ins_propagate( void );
-extern void ins_update_baro( void );
-extern void ins_update_gps( void );
-extern void ins_update_sonar( void );
-
-
-#endif /* INS_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -53,7 +53,7 @@
#include "firmwares/rotorcraft/guidance.h"
#include "subsystems/ahrs.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#if defined USE_CAM || USE_DROP
#include "booz2_pwm_hw.h"
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
2010-10-26 18:03:28 UTC (rev 6274)
@@ -27,7 +27,7 @@
#include "pprz_debug.h"
#include "booz_gps.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "modules.h"
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-10-26 18:03:28 UTC (rev 6274)
@@ -48,7 +48,7 @@
#include "firmwares/rotorcraft/battery.h"
#include "subsystems/imu.h"
#include "booz_gps.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "subsystems/ahrs.h"
#include "i2c_hw.h"
@@ -472,7 +472,7 @@
}
#ifdef USE_VFF
-#include "firmwares/rotorcraft/ins/vf_float.h"
+#include "subsystems/ins/vf_float.h"
#define PERIODIC_SEND_VFF(_chan) { \
DOWNLINK_SEND_VFF(_chan, \
&vff_z_meas, \
@@ -488,7 +488,7 @@
#endif
#ifdef USE_HFF
-#include "firmwares/rotorcraft/ins/hf_float.h"
+#include "subsystems/ins/hf_float.h"
#define PERIODIC_SEND_HFF(_chan) { \
DOWNLINK_SEND_HFF(_chan, \
&b2_hff_state.x, \
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -26,7 +26,7 @@
#include "booz2_pwm_hw.h"
#include "subsystems/ahrs.h"
#include "firmwares/rotorcraft/navigation.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "flight_plan.h"
uint8_t booz_cam_mode;
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-10-26 18:03:28 UTC (rev 6274)
@@ -24,11 +24,11 @@
#include "cam_track.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "subsystems/ahrs.h"
#ifdef USE_HFF
-#include "ins/hf_float.h"
+#include "subsystems/ins/hf_float.h"
#endif
struct FloatVect3 target_pos_ned;
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,7 +1,7 @@
-/*
+/*
C Datei für die Einbindung eines ArduIMU's
address@hidden: schmiemi
- chaneren
address@hidden: schmiemi
+ chaneren
*/
@@ -20,9 +20,9 @@
#ifndef ARDUIMU_I2C_DEV
#define ARDUIMU_I2C_DEV i2c0
#endif
-
-// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
-// einzugebende Adresse im ArduIMU ist 0000 1011
+
+// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write
+// einzugebende Adresse im ArduIMU ist 0000 1011
//da ArduIMU das Read/Write Bit selber anfügt.
#define ArduIMU_SLAVE_ADDR 0x22
@@ -67,152 +67,151 @@
void ArduIMU_periodicGPS( void ) {
- if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 ==
TRUE ) {
- gps_daten_abgespeichert = FALSE;
- }
+ if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 == TRUE
) {
+ gps_daten_abgespeichert = FALSE;
+ }
- if( imu_daten_angefordert == TRUE ){
- IMU_Daten_verarbeiten();
- }
+ if( imu_daten_angefordert == TRUE ){
+ IMU_Daten_verarbeiten();
+ }
- if ( gps_daten_abgespeichert == FALSE ) {
- //posllh
- GPS_Data [0] = gps_itow;
- GPS_Data [1] = gps_lon;
- GPS_Data [2] = gps_lat;
- GPS_Data [3] = gps_alt; //höhe über elipsoid
- GPS_Data [4] = gps_hmsl; //höhe über sea level
- //velend
- GPS_Data [5] = gps_speed_3d; //speed 3D
- GPS_Data [6] = gps_gspeed; //ground speed
- GPS_Data [7] = gps_course * 100000; //Kurs
- //status
- GPS_Data [8] = gps_mode; //fix
- GPS_Data [9] = gps_status_flags; //flags
- //sol
- GPS_Data [10] = gps_mode; //fix
- GPS_Data [11] = gps_sol_flags; //flags
- GPS_Data [12] = gps_ecefVZ; //ecefVZ
- GPS_Data [13] = gps_numSV;
-
- gps_daten_abgespeichert = TRUE;
- }
-
- if(messageNr == 0) {
+ if ( gps_daten_abgespeichert == FALSE ) {
+ //posllh
+ GPS_Data [0] = gps_itow;
+ GPS_Data [1] = gps_lon;
+ GPS_Data [2] = gps_lat;
+ GPS_Data [3] = gps_alt; //höhe über elipsoid
+ GPS_Data [4] = gps_hmsl; //höhe über sea level
+ //velend
+ GPS_Data [5] = gps_speed_3d; //speed 3D
+ GPS_Data [6] = gps_gspeed; //ground speed
+ GPS_Data [7] = gps_course * 100000; //Kurs
+ //status
+ GPS_Data [8] = gps_mode; //fix
+ GPS_Data [9] = gps_status_flags; //flags
+ //sol
+ GPS_Data [10] = gps_mode; //fix
+ GPS_Data [11] = gps_sol_flags; //flags
+ GPS_Data [12] = gps_ecefVZ; //ecefVZ
+ GPS_Data [13] = gps_numSV;
- //test für 32bit in byte packete abzupacken:
- //GPS_Data [0] = -1550138773;
+ gps_daten_abgespeichert = TRUE;
+ }
- ardu_gps_trans.buf[0] = 0; //message Nr =
0 --> itow bis ground speed
- ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
- ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
- ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
- ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
- ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
- ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
- ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
- ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
- ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
- ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
- ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
- ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
- ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
- ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
- ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
- ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
- ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
- ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
- ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
- ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
- ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
- ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
- ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
- ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
- ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
- ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
- ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
- ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
- I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28);
+ if(messageNr == 0) {
- gps_daten_versendet_msg1 = TRUE;
- messageNr =1;
- }
+ //test für 32bit in byte packete abzupacken:
+ //GPS_Data [0] = -1550138773;
+
+ ardu_gps_trans.buf[0] = 0; //message Nr =
0 --> itow bis ground speed
+ ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow
+ ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8);
+ ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16);
+ ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24);
+ ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon
+ ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8);
+ ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16);
+ ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24);
+ ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat
+ ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8);
+ ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16);
+ ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24);
+ ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height
+ ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8);
+ ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16);
+ ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24);
+ ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl
+ ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8);
+ ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16);
+ ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24);
+ ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed
+ ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8);
+ ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16);
+ ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24);
+ ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed
+ ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8);
+ ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16);
+ ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24);
+ I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28);
+
+ gps_daten_versendet_msg1 = TRUE;
+ messageNr =1;
+ }
else {
-
- ardu_gps_trans.buf[0] = 1; //message Nr = 1 -->
ground course, ecefVZ, numSV, Fix, flags, fix, flags
- ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
- ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
- ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
- ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
- ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
- ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
- ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
- ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
- ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
- ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
- ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
- ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
- ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
- I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13);
- gps_daten_versendet_msg2 = TRUE;
- messageNr = 0;
- }
+ ardu_gps_trans.buf[0] = 1; //message Nr = 1 -->
ground course, ecefVZ, numSV, Fix, flags, fix, flags
+ ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course
+ ardu_gps_trans.buf[2] = (GPS_Data[7] >>8);
+ ardu_gps_trans.buf[3] = (GPS_Data[7] >>16);
+ ardu_gps_trans.buf[4] = (GPS_Data[7] >>24);
+ ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ
+ ardu_gps_trans.buf[6] = (GPS_Data[12] >>8);
+ ardu_gps_trans.buf[7] = (GPS_Data[12] >>16);
+ ardu_gps_trans.buf[8] = (GPS_Data[12] >>24);
+ ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV
+ ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix
+ ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags
+ ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix
+ ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags
+ I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13);
+ gps_daten_versendet_msg2 = TRUE;
+ messageNr = 0;
+ }
+
//DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV
,&gps_alt , &gps_hmsl , &gps_itow, &gps_speed_3d);
}
void ArduIMU_periodic( void ) {
//Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt.
- //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte,
Status
- if (imu_daten_angefordert == TRUE) {
- IMU_Daten_verarbeiten();
- }
- I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, 12);
+ //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status
+ if (imu_daten_angefordert == TRUE) {
+ IMU_Daten_verarbeiten();
+ }
+ I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, 12);
- imu_daten_angefordert = TRUE;
- /*
- Buffer O: Roll
- Buffer 1: Pitch
- Buffer 2: Yaw
- Buffer 3: Beschleunigung X-Achse
- Buffer 4: Beschleunigung Y-Achse
- Buffer 5: Beschleunigung Z-Achse
- */
+ imu_daten_angefordert = TRUE;
+ /*
+ Buffer O: Roll
+ Buffer 1: Pitch
+ Buffer 2: Yaw
+ Buffer 3: Beschleunigung X-Achse
+ Buffer 4: Beschleunigung Y-Achse
+ Buffer 5: Beschleunigung Z-Achse
+ */
- //Nachricht zum GCS senden
- // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0],
&ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4],
&ArduIMU_data[5]);
+ //Nachricht zum GCS senden
+ // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0],
&ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4],
&ArduIMU_data[5]);
// DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &airspeed_mode ,
&altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert,
&amsys_baro_scaliert);
}
void IMU_Daten_verarbeiten( void ) {
- //Empfangene Byts zusammenfügen und bereitstellen
- recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
- recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
- recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
- recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
- recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
- recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
+ //Empfangene Byts zusammenfügen und bereitstellen
+ recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
+ recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
+ recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
+ recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
+ recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
+ recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
- //Floats zurück transformieren. Transformation ist auf ArduIMU
programmiert.
- ArduIMU_data[0] = (float) (recievedData[0] / (float)100);
- ArduIMU_data[1] = (float) (recievedData[1] / (float)100);
- ArduIMU_data[2] = (float) (recievedData[2] / (float)100);
- ArduIMU_data[3] = (float) (recievedData[3] / (float)100);
- ArduIMU_data[4] = (float) (recievedData[4] / (float)100);
- ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
+ //Floats zurück transformieren. Transformation ist auf ArduIMU
programmiert.
+ ArduIMU_data[0] = (float) (recievedData[0] / (float)100);
+ ArduIMU_data[1] = (float) (recievedData[1] / (float)100);
+ ArduIMU_data[2] = (float) (recievedData[2] / (float)100);
+ ArduIMU_data[3] = (float) (recievedData[3] / (float)100);
+ ArduIMU_data[4] = (float) (recievedData[4] / (float)100);
+ ArduIMU_data[5] = (float) (recievedData[5] / (float)100);
- // test
- estimator_phi = (float)ArduIMU_data[0]*0.01745329252 -
ins_roll_neutral;
- estimator_theta = (float)ArduIMU_data[1]*0.01745329252 -
ins_pitch_neutral;
- imu_daten_angefordert = FALSE;
+ // test
+ estimator_phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral;
+ estimator_theta = (float)ArduIMU_data[1]*0.01745329252 -
ins_pitch_neutral;
+ imu_daten_angefordert = FALSE;
- {
- float psi=0;
- RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel,
&estimator_phi, &estimator_theta, &psi));
- }
+ {
+ float psi=0;
+ RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &estimator_phi,
&estimator_theta, &psi));
+ }
}
-
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_arduimu.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,5 +1,3 @@
-
-
#ifndef ArduIMU_H
#define ArduIMU_H
@@ -16,7 +14,7 @@
extern float pitch_of_throttle_gain;
extern float throttle_slew;
-void ArduIMU_init( void );
+void ArduIMU_init( void );
void ArduIMU_periodic( void );
void ArduIMU_periodicGPS( void );
void IMU_Daten_verarbeiten( void );
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2003-2006 Haiyang Chao
*
* 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.
*
*/
@@ -40,11 +40,11 @@
#include "estimator.h"
#include "xsens_protocol.h"
-#define UNINIT 0
-#define GOT_SYNC1 1
-#define GOT_SYNC2 2
-#define GOT_ID 3
-#define GOT_LEN 4
+#define UNINIT 0
+#define GOT_SYNC1 1
+#define GOT_SYNC2 2
+#define GOT_ID 3
+#define GOT_LEN 4
#define GOT_PAYLOAD 5
#define GOT_CHECKSUM1 6
#define GOT_CHECKSUM2 7
@@ -107,7 +107,7 @@
int16_t ugear_phi;
int16_t ugear_psi;
-int16_t ugear_theta;
+int16_t ugear_theta;
int16_t gps_ve;
int16_t gps_vn;
@@ -115,7 +115,7 @@
/* added 20080522 for debugging*/
int16_t ugear_debug1;
int16_t ugear_debug2;
-int16_t ugear_debug3;
+int16_t ugear_debug3;
int32_t ugear_debug4;
int32_t ugear_debug5;
int32_t ugear_debug6;
@@ -130,8 +130,8 @@
};
struct gps {
- int32_t lon,lat,alt;
- int16_t ve,vn,vd;
+ int32_t lon,lat,alt;
+ int16_t ve,vn,vd;
};
struct imu imupacket;
@@ -175,8 +175,8 @@
ugear_status++;
//ugear_theta = 30; // for debug
if (ugear_type > 2)
- goto restart;
- break;
+ goto restart;
+ break;
case GOT_ID:
ugear_len = c;
ugear_msg_idx = 0;
@@ -203,7 +203,7 @@
break;
}
return;
-error:
+error:
restart:
ugear_status = UNINIT;
return;
@@ -219,77 +219,77 @@
*/
void parse_ugear_msg( void ){
- float ins_phi, ins_psi, ins_theta;
+ float ins_phi, ins_psi, ins_theta;
- switch (ugear_type){
- case 0: /*gps*/
- ugear_debug1 = ugear_debug1+1;
- gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
- gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
-
- nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
- latlong_utm_of(RadOfDeg(gps_lat/1e7),
RadOfDeg(gps_lon/1e7), nav_utm_zone0);
- gps_utm_east = latlong_utm_x * 100;
- gps_utm_north = latlong_utm_y * 100;
+ switch (ugear_type){
+ case 0: /*gps*/
+ ugear_debug1 = ugear_debug1+1;
+ gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
+ gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
- gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
- gps_utm_zone = nav_utm_zone0;
-
- gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
- gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
- gps_course =
UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000; /*in decdegree */
- gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
- gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
- gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
- gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
- gps_week = 0; // FIXME
- gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
+ nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
+ latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7),
nav_utm_zone0);
+ gps_utm_east = latlong_utm_x * 100;
+ gps_utm_north = latlong_utm_y * 100;
- //ugear_debug2 = gps_climb;
- //ugear_debug4 =
(int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
- //ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
- //ugear_debug6 = (int16_t)estimator_phi*100;
+ gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
+ gps_utm_zone = nav_utm_zone0;
- gps_mode = 3; /*force GPSfix to be valided*/
- gps_pos_available = TRUE; /* The 3 UBX messages are
sent in one rafale */
- break;
- case 1: /*IMU*/
- ugear_debug2 = ugear_debug2+1;
- ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
- ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
- ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
- ugear_debug4 = (int32_t)ugear_phi;
- ugear_debug5 = (int32_t)ugear_theta;
- ugear_debug6 = (int32_t)ugear_psi;
- ugear_debug3 = 333;
- ins_phi = (float)ugear_phi/10000 - ins_roll_neutral;
- ins_psi = 0;
- ins_theta = (float)ugear_theta/10000 -
ins_pitch_neutral;
+ gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
+ gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
+ gps_course = UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000;
/*in decdegree */
+ gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
+ gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
+ gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
+ gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
+ gps_week = 0; // FIXME
+ gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
+
+ //ugear_debug2 = gps_climb;
+ //ugear_debug4 = (int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
+ //ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
+ //ugear_debug6 = (int16_t)estimator_phi*100;
+
+ gps_mode = 3; /*force GPSfix to be valided*/
+ gps_pos_available = TRUE; /* The 3 UBX messages are sent in one
rafale */
+ break;
+ case 1: /*IMU*/
+ ugear_debug2 = ugear_debug2+1;
+ ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
+ ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
+ ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
+ ugear_debug4 = (int32_t)ugear_phi;
+ ugear_debug5 = (int32_t)ugear_theta;
+ ugear_debug6 = (int32_t)ugear_psi;
+ ugear_debug3 = 333;
+ ins_phi = (float)ugear_phi/10000 - ins_roll_neutral;
+ ins_psi = 0;
+ ins_theta = (float)ugear_theta/10000 - ins_pitch_neutral;
#ifndef INFRARED
- EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
+ EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
#endif
- break;
- case 2: /*GPS status*/
+ break;
+ case 2: /*GPS status*/
// ugear_debug1 = 2;
- gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
- uint8_t is;
- for(is = 0; is < Min(gps_nb_channels, 16); is++) {
- uint8_t ch =
XSENS_GPSStatus_chn(ugear_msg_buf,is);
- if (ch > 16) continue;
- gps_svinfos[ch].svid =
XSENS_GPSStatus_svid(ugear_msg_buf, is);
- gps_svinfos[ch].flags =
XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
- gps_svinfos[ch].qi =
XSENS_GPSStatus_qi(ugear_msg_buf, is);
- gps_svinfos[ch].cno =
XSENS_GPSStatus_cnr(ugear_msg_buf, is);
- gps_svinfos[ch].elev = 0;
- gps_svinfos[ch].azim = 0;
- }
- break;
- case 3: /*servo*/
- break;
- case 4: /*health*/
- break;
+ gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
+ uint8_t is;
+ for(is = 0; is < Min(gps_nb_channels, 16); is++) {
+ uint8_t ch = XSENS_GPSStatus_chn(ugear_msg_buf,is);
+ if (ch > 16) continue;
+ gps_svinfos[ch].svid = XSENS_GPSStatus_svid(ugear_msg_buf,
is);
+ gps_svinfos[ch].flags =
XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
+ gps_svinfos[ch].qi = XSENS_GPSStatus_qi(ugear_msg_buf, is);
+ gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(ugear_msg_buf,
is);
+ gps_svinfos[ch].elev = 0;
+ gps_svinfos[ch].azim = 0;
+ }
+ break;
+ case 3: /*servo*/
+ break;
+ case 4: /*health*/
+ break;
- }
+ }
}
@@ -298,32 +298,30 @@
}
void ugear_event( void ) {
- if (UgearBuffer()){
- ReadUgearBuffer();
- }
- if (ugear_msg_received){
- parse_ugear_msg();
- ugear_msg_received = FALSE;
- if (gps_pos_available){
- //gps_downlink();
- gps_verbose_downlink = !launch;
- UseGpsPosNoSend(estimator_update_state_gps);
- gps_msg_received_counter = gps_msg_received_counter+1;
- #ifdef GX2
- if (gps_msg_received_counter == 1){
- gps_send();
- gps_msg_received_counter = 0;
- }
- #endif
- #ifdef XSENSDL
- if (gps_msg_received_counter == 25){
- gps_send();
- gps_msg_received_counter = 0;
- }
- #endif
- gps_pos_available = FALSE;
- }
- }
+ if (UgearBuffer()){
+ ReadUgearBuffer();
+ }
+ if (ugear_msg_received){
+ parse_ugear_msg();
+ ugear_msg_received = FALSE;
+ if (gps_pos_available){
+ //gps_downlink();
+ gps_verbose_downlink = !launch;
+ UseGpsPosNoSend(estimator_update_state_gps);
+ gps_msg_received_counter = gps_msg_received_counter+1;
+ #ifdef GX2
+ if (gps_msg_received_counter == 1){
+ gps_send();
+ gps_msg_received_counter = 0;
+ }
+ #endif
+ #ifdef XSENSDL
+ if (gps_msg_received_counter == 25){
+ gps_send();
+ gps_msg_received_counter = 0;
+ }
+ #endif
+ gps_pos_available = FALSE;
+ }
+ }
}
-
-
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_osam_ugear.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,6 +1,6 @@
/*
* $Id$
- *
+ *
* Copyright (C) 2003-2006 Haiyang Chao
*
* 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.
*
*/
@@ -48,7 +48,7 @@
#endif
#define UGEAR_MAX_PAYLOAD 40
-/*#define UGEAR_MAX_PAYLOAD 24*/ /*Bugs!!! why work for the former version
??20080708*/
+/*#define UGEAR_MAX_PAYLOAD 24*/ /*Bugs!!! why work for the former version
??20080708*/
/*#define GPS_NB_CHANNELS 16*/
#define UGEAR_NAV_POSLLH_LAT(_ubx_payload)
(int32_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
@@ -76,15 +76,15 @@
/*define the following varables for communication with ugear by haiyang
20080508*/
extern int16_t ugear_phi;
extern int16_t ugear_psi;
-extern int16_t ugear_theta;
+extern int16_t ugear_theta;
/* added 20080522 for debugging*/
extern int16_t ugear_debug1;
extern int16_t ugear_debug2;
-extern int16_t ugear_debug3;
+extern int16_t ugear_debug3;
extern int32_t ugear_debug4;
extern int32_t ugear_debug5;
-extern int32_t ugear_debug6;
+extern int32_t ugear_debug6;
extern struct imu imupacket;
extern struct gps gpspacket;
@@ -92,4 +92,3 @@
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
-
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,6 +1,6 @@
/*
* Paparazzi $Id: ins_xsens.c 3872 2009-08-05 14:42:41Z mmm $
- *
+ *
* Copyright (C) 2010 ENAC
*
* 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.
*
*/
@@ -62,7 +62,7 @@
//TODO send error
return;
}
-
+
// parse message (will work only with read and write register)
switch (last_received_packet.RegID) {
case VN100_REG_ADOR :
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_vn100.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,6 +1,6 @@
/*
- * $Id: $
- *
+ * $Id: $
+ *
* Copyright (C) 2010 ENAC
*
* 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.
*
*/
Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -1,6 +1,6 @@
/*
* Paparazzi mcu0 $Id$
- *
+ *
* Copyright (C) 2003 Pascal Brisset, 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.
*
*/
@@ -26,7 +26,7 @@
* \brief Parser for the Xsens protocol
*/
-#include "ins/ins.h"
+#include "subsystems/ins.h"
#include <inttypes.h>
@@ -83,14 +83,14 @@
}
#define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }
-/** Includes macros generated from xsens_MTi-G.xml */
+/** Includes macros generated from xsens_MTi-G.xml */
#include "xsens_protocol.h"
-
+
#define XSENS_MAX_PAYLOAD 254
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
-/* output mode : calibrated, orientation, position, velocity, status
+/* output mode : calibrated, orientation, position, velocity, status
* -----------
*
* bit 0 temp
@@ -138,7 +138,7 @@
* bit 24-27 Reseverd
*
* bit 28-30 Reseverd
- * bit 31 0=X-North-Z-Up, 1=North-East-Down
+ * bit 31 0=X-North-Z-Up, 1=North-East-Down
*/
#ifndef XSENS_OUTPUT_SETTINGS
#define XSENS_OUTPUT_SETTINGS 0x80000C05
@@ -413,7 +413,7 @@
break;
}
return;
- error:
+ error:
restart:
xsens_status = UNINIT;
return;
Modified: paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
2010-10-26 18:03:28 UTC (rev 6274)
@@ -39,7 +39,7 @@
extern void maxbotix_init(void);
extern void maxbotix_read(void);
-#include "firmwares/rotorcraft/ins.h" // needed because ins is not a module
+#include "subsystems/ins.h" // needed because ins is not a module
#define SonarEvent(_handler) { \
if (sonar_data_available) { \
Modified:
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
2010-10-26 17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
2010-10-26 18:03:28 UTC (rev 6274)
@@ -23,7 +23,7 @@
#include "booz_fms.h"
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
#include "math/pprz_algebra_int.h"
#define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW
Copied: paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.c (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 "subsystems/ins/hf_float.h"
+#include "subsystems/ins.h"
+#include "subsystems/imu.h"
+#include "subsystems/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 HFF_ACCEL_NOISE
+#define HFF_ACCEL_NOISE 0.5
+#endif
+#define Q HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
+#define Qdotdot HFF_ACCEL_NOISE*DT_HFILTER
+
+//TODO: proper measurement noise
+#ifndef HFF_R_POS
+#define HFF_R_POS 8.
+#endif
+#ifndef HFF_R_POS_MIN
+#define HFF_R_POS_MIN 3.
+#endif
+
+#ifndef HFF_R_SPEED
+#define HFF_R_SPEED 2.
+#endif
+#ifndef HFF_R_SPEED_MIN
+#define 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 = HFF_R_POS;
+ Rgps_vel = 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 = 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<HFF_STATE_SIZE; i++) {
+ for (j=0; j<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<HFF_STATE_SIZE; i++) {
+ for (j=0; j<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 < HFF_STATE_SIZE; i++) {
+ for (int j=0; j < 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 */
+ ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ 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 < HFF_R_POS_MIN)
+ Rgps_pos = HFF_R_POS_MIN;
+
+ Rgps_vel = (float) booz_gps_state.sacc / 100.;
+ if (Rgps_vel < HFF_R_SPEED_MIN)
+ Rgps_vel = 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, ins_gps_pos_m_ned.x, Rgps_pos);
+ b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
+#ifdef HFF_UPDATE_SPEED
+ b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
+ b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
+#endif
+
+ /* update ins state */
+ ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+ ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+ ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+ ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+ ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
+ 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, ins_gps_pos_m_ned.x, Rgps_pos);
+ b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
+#ifdef HFF_UPDATE_SPEED
+ b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
+ b2_hff_update_ydot(b2_hff_rb_last, 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/subsystems/ins/hf_float.h (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins/hf_float.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 HF_FLOAT_H
+#define HF_FLOAT_H
+
+#include "std.h"
+#include "math/pprz_algebra_float.h"
+
+#define 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 HFF_UPDATE_SPEED
+
+struct HfilterFloat {
+ float x;
+ /* float xbias; */
+ float xdot;
+ float xdotdot;
+ float y;
+ /* float ybias; */
+ float ydot;
+ float ydotdot;
+ float xP[HFF_STATE_SIZE][HFF_STATE_SIZE];
+ float yP[HFF_STATE_SIZE][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 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 /* HF_FLOAT_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.c (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 "subsystems/ins/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 vff_z;
+float vff_bias;
+float vff_zdot;
+float vff_zdotdot;
+
+float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
+
+float vff_z_meas;
+
+void vff_init(float init_z, float init_zdot, float init_bias) {
+ vff_z = init_z;
+ vff_zdot = init_zdot;
+ vff_bias = init_bias;
+ int i, j;
+ for (i=0; i<VFF_STATE_SIZE; i++) {
+ for (j=0; j<VFF_STATE_SIZE; j++)
+ vff_P[i][j] = 0.;
+ 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 vff_propagate(float accel) {
+ /* update state */
+ vff_zdotdot = accel + 9.81 - vff_bias;
+ vff_z = vff_z + DT_VFILTER * vff_zdot;
+ vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
+ /* update covariance */
+ const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] +
DT_VFILTER * vff_P[1][1] );
+ const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] -
DT_VFILTER * vff_P[1][2] );
+ const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
+ const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] -
DT_VFILTER * vff_P[2][1] );
+ const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] +
DT_VFILTER * vff_P[2][2] );
+ const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
+ const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
+ const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
+ const float FPF22 = vff_P[2][2];
+
+ vff_P[0][0] = FPF00 + Qzz;
+ vff_P[0][1] = FPF01;
+ vff_P[0][2] = FPF02;
+ vff_P[1][0] = FPF10;
+ vff_P[1][1] = FPF11 + Qzdotzdot;
+ vff_P[1][2] = FPF12;
+ vff_P[2][0] = FPF20;
+ vff_P[2][1] = FPF21;
+ vff_P[2][2] = FPF22 + Qbiasbias;
+
+}
+/*
+ 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) {
+ vff_z_meas = z_meas;
+
+ const float y = z_meas - vff_z;
+ const float S = vff_P[0][0] + conf;
+ const float K1 = vff_P[0][0] * 1/S;
+ const float K2 = vff_P[1][0] * 1/S;
+ const float K3 = vff_P[2][0] * 1/S;
+
+ vff_z = vff_z + K1 * y;
+ vff_zdot = vff_zdot + K2 * y;
+ vff_bias = vff_bias + K3 * y;
+
+ const float P11 = (1. - K1) * vff_P[0][0];
+ const float P12 = (1. - K1) * vff_P[0][1];
+ const float P13 = (1. - K1) * vff_P[0][2];
+ const float P21 = -K2 * vff_P[0][0] + vff_P[1][0];
+ const float P22 = -K2 * vff_P[0][1] + vff_P[1][1];
+ const float P23 = -K2 * vff_P[0][2] + vff_P[1][2];
+ const float P31 = -K3 * vff_P[0][0] + vff_P[2][0];
+ const float P32 = -K3 * vff_P[0][1] + vff_P[2][1];
+ const float P33 = -K3 * vff_P[0][2] + vff_P[2][2];
+
+ vff_P[0][0] = P11;
+ vff_P[0][1] = P12;
+ vff_P[0][2] = P13;
+ vff_P[1][0] = P21;
+ vff_P[1][1] = P22;
+ vff_P[1][2] = P23;
+ vff_P[2][0] = P31;
+ vff_P[2][1] = P32;
+ vff_P[2][2] = P33;
+
+}
+
+void vff_update(float z_meas) {
+ update_z_conf(z_meas, R);
+}
+
+void 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 - vff_zdot;
+ const float S = vff_P[1][1] + conf;
+ const float K1 = vff_P[0][1] * 1/S;
+ const float K2 = vff_P[1][1] * 1/S;
+ const float K3 = vff_P[2][1] * 1/S;
+
+ vff_z = vff_z + K1 * yd;
+ vff_zdot = vff_zdot + K2 * yd;
+ vff_bias = vff_bias + K3 * yd;
+
+ const float P11 = -K1 * vff_P[1][0] + vff_P[0][0];
+ const float P12 = -K1 * vff_P[1][1] + vff_P[0][1];
+ const float P13 = -K1 * vff_P[1][2] + vff_P[0][2];
+ const float P21 = (1. - K2) * vff_P[1][0];
+ const float P22 = (1. - K2) * vff_P[1][1];
+ const float P23 = (1. - K2) * vff_P[1][2];
+ const float P31 = -K3 * vff_P[1][0] + vff_P[2][0];
+ const float P32 = -K3 * vff_P[1][1] + vff_P[2][1];
+ const float P33 = -K3 * vff_P[1][2] + vff_P[2][2];
+
+ vff_P[0][0] = P11;
+ vff_P[0][1] = P12;
+ vff_P[0][2] = P13;
+ vff_P[1][0] = P21;
+ vff_P[1][1] = P22;
+ vff_P[1][2] = P23;
+ vff_P[2][0] = P31;
+ vff_P[2][1] = P32;
+ vff_P[2][2] = P33;
+
+}
+
+void vff_update_vz_conf(float vz_meas, float conf) {
+ update_vz_conf(vz_meas, conf);
+}
+
+void vff_realign(float z_meas) {
+ vff_z = z_meas;
+ vff_zdot = 0;
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.h (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins/vf_float.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 VF_FLOAT_H
+#define VF_FLOAT_H
+
+#define VFF_STATE_SIZE 3
+
+extern float vff_z;
+extern float vff_zdot;
+extern float vff_bias;
+extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
+extern float vff_zdotdot;
+
+extern float vff_z_meas;
+
+extern void vff_init(float z, float zdot, float bias);
+extern void vff_propagate(float accel);
+extern void vff_update(float z_meas);
+extern void vff_update_z_conf(float z_meas, float conf);
+extern void vff_update_vz_conf(float vz_meas, float conf);
+extern void vff_realign(float z_meas);
+
+#endif /* VF_FLOAT_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.c (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 "subsystems/ins/vf_int.h"
+
+#include "booz_geometry_mixed.h"
+
+int64_t vfi_z;
+int32_t vfi_zd;
+int32_t vfi_abias;
+int32_t vfi_zdd;
+int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE];
+
+/* initial covariance */
+#define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
+#define VFI_INIT_PZDZD BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
+#define VFI_INIT_PABAB BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
+
+/* system and measurement noise */
+#define VFI_ACCEL_NOISE 0.1
+#define VFI_DT2_2 (1./(512.*512.)/2.)
+#define VFI_DT (1./512.)
+#define VFI_QZZ BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT2_2,
VFI_P_FRAC)
+#define VFI_QZDZD BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT, VFI_P_FRAC)
+#define VFI_QABAB BOOZ_INT_OF_FLOAT(1e-7, VFI_P_FRAC)
+#define VFI_R BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
+
+
+void vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
+
+ // initialize state vector
+ vfi_z = z0;
+ vfi_zd = zd0;
+ vfi_abias = bias0;
+ vfi_zdd = 0;
+ // initialize covariance
+ int i, j;
+ for (i=0; i<VFI_S_SIZE; i++)
+ for (j=0; j<VFI_S_SIZE; j++)
+ vfi_P[i][j] = 0;
+ vfi_P[VFI_S_Z][VFI_S_Z] = VFI_INIT_PZZ;
+ vfi_P[VFI_S_ZD][VFI_S_ZD] = VFI_INIT_PZDZD;
+ vfi_P[VFI_S_AB][VFI_S_AB] = VFI_INIT_PABAB;
+
+}
+
+/*
+
+ 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 vfi_propagate( int32_t accel_reading ) {
+
+ // compute unbiased vertical acceleration
+ vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, VFI_ZDD_FRAC) - vfi_abias;
+ // propagate state
+ const int32_t dz = vfi_zd >> ( VFI_F_UPDATE_FRAC + VFI_ZD_FRAC -
VFI_Z_FRAC);
+ vfi_z += dz;
+ const int32_t dzd = vfi_zdd >> ( VFI_F_UPDATE_FRAC + VFI_ZDD_FRAC -
VFI_ZD_FRAC);
+ vfi_zd += dzd;
+
+ // propagate covariance
+ const int32_t tmp1 = vfi_P[1][0] + vfi_P[0][1] +
(vfi_P[1][1]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF00 = vfi_P[0][0] + (tmp1>>VFI_F_UPDATE_FRAC);
+ const int32_t tmp2 = vfi_P[1][1] - vfi_P[0][2] -
(vfi_P[1][2]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF01 = vfi_P[0][1] + (tmp2>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF02 = vfi_P[0][2] + (vfi_P[1][2] >> VFI_F_UPDATE_FRAC);;
+ const int32_t tmp3 = -vfi_P[2][0] + vfi_P[1][1] -
(vfi_P[2][1]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF10 = vfi_P[1][0] + (tmp3>>VFI_F_UPDATE_FRAC);
+ const int32_t tmp4 = -vfi_P[2][1] - vfi_P[1][2] +
(vfi_P[2][2]>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF11 = vfi_P[1][1] + (tmp4>>VFI_F_UPDATE_FRAC);
+ const int32_t FPF12 = vfi_P[1][2] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF20 = vfi_P[2][0] + (vfi_P[2][1] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF21 = vfi_P[2][1] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
+ const int32_t FPF22 = vfi_P[2][2];
+
+ vfi_P[0][0] = FPF00 + VFI_QZZ;
+ vfi_P[0][1] = FPF01;
+ vfi_P[0][2] = FPF02;
+ vfi_P[1][0] = FPF10;
+ vfi_P[1][1] = FPF11 + VFI_QZDZD;
+ vfi_P[1][2] = FPF12;
+ vfi_P[2][0] = FPF20;
+ vfi_P[2][1] = FPF21;
+ vfi_P[2][2] = FPF22 + VFI_QABAB;
+
+}
+
+
+void vfi_update( int32_t z_meas ) {
+
+ const int64_t y = (z_meas<<(VFI_Z_FRAC-VFI_MEAS_Z_FRAC)) - vfi_z;
+ const int32_t S = vfi_P[0][0] + VFI_R;
+
+ const int32_t K1 = vfi_P[0][0] / S;
+ const int32_t K2 = vfi_P[1][0] / S;
+ const int32_t K3 = vfi_P[2][0] / S;
+
+ vfi_z = vfi_z + ((K1 * y)>>VFI_P_FRAC);
+ vfi_zd = vfi_zd + ((K2 * y)>>VFI_P_FRAC);
+ vfi_abias = vfi_abias + ((K3 * y)>>VFI_P_FRAC);
+
+#if 0
+
+ const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) *
vfi_P[0][0])>>VFI_P_RES;
+ const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][1];
+ const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][2];
+ const int32_t P21 = -K2 * vfi_P[0][0] + vfi_P[1][0];
+ const int32_t P22 = -K2 * vfi_P[0][1] + vfi_P[1][1];
+ const int32_t P23 = -K2 * vfi_P[0][2] + vfi_P[1][2];
+ const int32_t P31 = -K3 * vfi_P[0][0] + vfi_P[2][0];
+ const int32_t P32 = -K3 * vfi_P[0][1] + vfi_P[2][1];
+ const int32_t P33 = -K3 * vfi_P[0][2] + vfi_P[2][2];
+
+ tl_vf_P[0][0] = P11;
+ tl_vf_P[0][1] = P12;
+ 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/subsystems/ins/vf_int.h (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins/vf_int.h 2010-10-26
18:03:28 UTC (rev 6274)
@@ -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 VF_INT_H
+#define VF_INT_H
+
+#include "std.h"
+#include "booz_geometry_int.h"
+
+extern void vfi_init( int32_t z0, int32_t zd0, int32_t bias0 );
+extern void vfi_propagate( int32_t accel_reading );
+
+/* z_meas : altitude measurement in meter */
+/* Q23.8 : accuracy 0.004m range 8388km */
+extern void vfi_update( int32_t z_meas );
+#define VFI_Z_MEAS_FRAC IPOS_FRAC
+
+/* propagate frequency : 512 Hz */
+#define VFI_F_UPDATE_FRAC 9
+#define VFI_F_UPDATE (1<<VFI_F_UPDATE_RES)
+
+/* vertical acceleration in m/s^2 */
+/* Q21.10 : accuracy 0.001m/s^2, range 2097km/s2 */
+extern int32_t vfi_zdd;
+#define VFI_ZDD_FRAC IACCEL_RES
+
+/* vertical accelerometer bias in m/s^2 */
+/* Q21.10 : accuracy 0.001m/s^2, range 2097km/s2 */
+extern int32_t vfi_abias;
+#define VFI_BIAS_FRAC IACCEL_RES
+
+/* vertical speed in m/s */
+/* Q12.19 : accuracy 0.000002 , range 4096m/s2 */
+extern int32_t vfi_zd;
+#define VFI_ZD_FRAC (VFI_ZDD_FRAC + VFI_F_UPDATE_FRAC)
+
+/* altitude in m */
+/* Q35.28 : accuracy 3.7e-9 , range 3.4e10m */
+extern int64_t vfi_z;
+#define VFI_Z_FRAC (VFI_ZD_FRAC + VFI_F_UPDATE_FRAC)
+
+/* Kalman filter state */
+#define VFI_S_Z 0
+#define VFI_S_ZD 1
+#define VFI_S_AB 2
+#define VFI_S_SIZE 3
+/* Kalman filter covariance */
+/* Q3.28 */
+extern int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE];
+#define VFI_P_FRAC 28
+
+
+
+
+#endif /* VF_INT_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/ins.c (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins.c 2010-10-26 18:03:28 UTC
(rev 6274)
@@ -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 "subsystems/ins.h"
+
+#include "subsystems/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 "subsystems/ahrs.h"
+
+#ifdef USE_VFF
+#include "subsystems/ins/vf_float.h"
+#endif
+
+#ifdef USE_HFF
+#include "subsystems/ins/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 ins_ltp_def;
+ bool_t ins_ltp_initialised;
+struct NedCoor_i ins_gps_pos_cm_ned;
+struct NedCoor_i ins_gps_speed_cm_s_ned;
+#ifdef USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+struct FloatVect2 ins_gps_pos_m_ned;
+struct FloatVect2 ins_gps_speed_m_s_ned;
+#endif
+bool_t ins_hf_realign;
+
+/* barometer */
+#ifdef USE_VFF
+int32_t ins_qfe;
+bool_t ins_baro_initialised;
+int32_t ins_baro_alt;
+#ifdef BOOZ2_SONAR
+bool_t ins_update_on_agl;
+int32_t ins_sonar_offset;
+#endif
+#endif
+bool_t ins_vf_realign;
+
+/* output */
+struct NedCoor_i ins_ltp_pos;
+struct NedCoor_i ins_ltp_speed;
+struct NedCoor_i ins_ltp_accel;
+struct EnuCoor_i ins_enu_pos;
+struct EnuCoor_i ins_enu_speed;
+struct EnuCoor_i ins_enu_accel;
+
+
+void ins_init() {
+#ifdef USE_INS_NAV_INIT
+ 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 - ins_ltp_def.hmsl + 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(&ins_ltp_def, &nav_init);
+ ins_ltp_def.hmsl = NAV_ALT0;
+#else
+ ins_ltp_initialised = FALSE;
+#endif
+#ifdef USE_VFF
+ ins_baro_initialised = FALSE;
+#ifdef BOOZ2_SONAR
+ ins_update_on_agl = FALSE;
+#endif
+ vff_init(0., 0., 0.);
+#endif
+ ins_vf_realign = FALSE;
+ ins_hf_realign = FALSE;
+#ifdef USE_HFF
+ b2_hff_init(0., 0., 0., 0.);
+#endif
+ INT32_VECT3_ZERO(ins_ltp_pos);
+ INT32_VECT3_ZERO(ins_ltp_speed);
+ INT32_VECT3_ZERO(ins_ltp_accel);
+ INT32_VECT3_ZERO(ins_enu_pos);
+ INT32_VECT3_ZERO(ins_enu_speed);
+ INT32_VECT3_ZERO(ins_enu_accel);
+}
+
+void ins_periodic( void ) {
+}
+
+#ifdef USE_HFF
+void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
+ b2_hff_realign(pos, speed);
+}
+#else
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct
FloatVect2 speed __attribute__ ((unused))) {}
+#endif /* USE_HFF */
+
+
+void ins_realign_v(float z) {
+#ifdef USE_VFF
+ vff_realign(z);
+#endif
+}
+
+void 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 && ins_baro_initialised) {
+ vff_propagate(z_accel_float);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
+ }
+ else { // feed accel from the sensors
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
+ }
+#else
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
+#endif /* USE_VFF */
+
+#ifdef USE_HFF
+ /* propagate horizontal filter */
+ b2_hff_propagate();
+#else
+ ins_ltp_accel.x = accel_ltp.x;
+ ins_ltp_accel.y = accel_ltp.y;
+#endif /* USE_HFF */
+
+ INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
+ INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
+ INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
+}
+
+void ins_update_baro() {
+#ifdef USE_VFF
+ if (baro.status == BS_RUNNING) {
+ if (!ins_baro_initialised) {
+ ins_qfe = baro.absolute;
+ ins_baro_initialised = TRUE;
+ }
+ ins_baro_alt = ((baro.absolute - ins_qfe) *
INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
+ float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
+ if (ins_vf_realign) {
+ ins_vf_realign = FALSE;
+ ins_qfe = baro.absolute;
+#ifdef BOOZ2_SONAR
+ ins_sonar_offset = sonar_meas;
+#endif
+ vff_realign(0.);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
+ ins_enu_pos.z = -ins_ltp_pos.z;
+ ins_enu_speed.z = -ins_ltp_speed.z;
+ ins_enu_accel.z = -ins_ltp_accel.z;
+ }
+ vff_update(alt_float);
+ }
+#endif
+}
+
+
+void ins_update_gps(void) {
+#ifdef USE_GPS
+ if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
+ if (!ins_ltp_initialised) {
+ ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos);
+ ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
+ ins_ltp_def.hmsl = booz_gps_state.hmsl;
+ ins_ltp_initialised = TRUE;
+ }
+ ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def,
&booz_gps_state.ecef_pos);
+ ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def,
&booz_gps_state.ecef_vel);
+#ifdef USE_HFF
+ VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x,
ins_gps_pos_cm_ned.y);
+ VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
+ VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x,
ins_gps_speed_cm_s_ned.y);
+ VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
+ if (ins_hf_realign) {
+ 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(ins_gps_pos_m_ned, zero);
+#endif
+ }
+ b2_hff_update_gps();
+#ifndef USE_VFF /* vff not used */
+ ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) /
INT32_POS_OF_CM_DEN;
+ ins_ltp_speed.z = (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(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM,
INT32_POS_OF_CM_DEN);
+ INT32_VECT3_SCALE_3(ins_ltp_speed, 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(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM,
INT32_POS_OF_CM_DEN);
+ INT32_VECT2_SCALE_2(ins_ltp_speed, 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, ins_ltp_speed);
+ INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
+ VECT2_ADD(ins_ltp_pos, d_pos);
+#endif
+#endif /* hff not used */
+
+ INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
+ INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
+ INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
+ }
+#endif /* USE_GPS */
+}
+
+void 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 (ins_update_on_agl && booz2_analog_baro_status ==
BOOZ2_ANALOG_BARO_RUNNING) {
+ int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) *
INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
+ ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar *
(INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
+ }
+#endif
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/ins.h (from rev 6273,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ins.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/ins.h 2010-10-26 18:03:28 UTC
(rev 6274)
@@ -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 INS_H
+#define 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 ins_ltp_def;
+extern bool_t ins_ltp_initialised;
+extern struct NedCoor_i ins_gps_pos_cm_ned;
+extern struct NedCoor_i ins_gps_speed_cm_s_ned;
+
+/* barometer */
+#ifdef USE_VFF
+extern int32_t ins_baro_alt;
+extern int32_t ins_qfe;
+extern bool_t ins_baro_initialised;
+#ifdef BOOZ2_SONAR
+extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
+extern int32_t ins_sonar_offset;
+#endif
+#endif
+
+/* output LTP NED */
+extern struct NedCoor_i ins_ltp_pos;
+extern struct NedCoor_i ins_ltp_speed;
+extern struct NedCoor_i ins_ltp_accel;
+/* output LTP ENU */
+extern struct EnuCoor_i ins_enu_pos;
+extern struct EnuCoor_i ins_enu_speed;
+extern struct EnuCoor_i ins_enu_accel;
+#ifdef USE_HFF
+/* horizontal gps transformed to NED in meters as float */
+extern struct FloatVect2 ins_gps_pos_m_ned;
+extern struct FloatVect2 ins_gps_speed_m_s_ned;
+#endif
+
+extern bool_t ins_hf_realign;
+extern bool_t ins_vf_realign;
+
+extern void ins_init( void );
+extern void ins_periodic( void );
+extern void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
+extern void ins_realign_v(float z);
+extern void ins_propagate( void );
+extern void ins_update_baro( void );
+extern void ins_update_gps( void );
+extern void ins_update_sonar( void );
+
+
+#endif /* INS_H */
Modified: paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
===================================================================
--- paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-10-26
17:09:15 UTC (rev 6273)
+++ paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-10-26
18:03:28 UTC (rev 6274)
@@ -215,7 +215,7 @@
#ifdef BYPASS_INS
-#include "firmwares/rotorcraft/ins.h"
+#include "subsystems/ins.h"
static void sim_overwrite_ins(void) {
ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]);
ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]);
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6274] moved ins to general subsystem,
Felix Ruess <=