[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6003] rename booz ins
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6003] rename booz ins |
|
Date: |
Tue, 28 Sep 2010 14:04:33 +0000 |
Revision: 6003
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6003
Author: flixr
Date: 2010-09-28 14:04:33 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
rename booz ins
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml
paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml
paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml
paparazzi3/trunk/conf/airframes/booz2_NoVa.xml
paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml
paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
paparazzi3/trunk/conf/flight_plans/basic_booz.xml
paparazzi3/trunk/conf/flight_plans/booz2_nova.xml
paparazzi3/trunk/conf/flight_plans/booz_test_drop.xml
paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/modules/sonar_maxbotix_booz.xml
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/conf/telemetry/booz_minimal.xml
paparazzi3/trunk/conf/telemetry/csc_ap.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
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/sonar/sonar_maxbotix_booz.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
paparazzi3/trunk/sw/simulator/nps/nps_ivy.c
paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -342,7 +342,7 @@
ap.CFLAGS += -DFAILSAFE_GROUND_DETECT
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
ap.CFLAGS += -DGPS_USE_LATLONG
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -291,7 +291,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8
#-DUSE_GPS_ACC4R
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
2010-09-28 14:04:33 UTC (rev 6003)
@@ -247,7 +247,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8
#-DUSE_GPS_ACC4R
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -303,7 +303,7 @@
ap.CFLAGS += -DFAILSAFE_GROUND_DETECT
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
ap.CFLAGS += -DGPS_USE_LATLONG
Modified: paparazzi3/trunk/conf/airframes/booz2_NoVa.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_NoVa.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/airframes/booz2_NoVa.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -243,7 +243,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
ap.CFLAGS += -DGPS_USE_LATLONG
Modified: paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -243,7 +243,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
ap.CFLAGS += -DGPS_USE_LATLONG
Modified: paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -244,7 +244,7 @@
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
-include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
+include $(CFG_BOOZ)/subsystems/ins_hff.makefile
#set GPS lag for horizontal filter
ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
ap.CFLAGS += -DGPS_USE_LATLONG
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -195,7 +195,7 @@
ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c
ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
-ap.srcs += $(SRC_BOOZ)/booz2_ins.c
+ap.srcs += $(SRC_FIRMWARE)/ins.c
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
math/pprz_geodetic_double.c
#
@@ -207,7 +207,7 @@
#
# vertical filter float version
-ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
+ap.srcs += $(SRC_FIRMWARE)/ins/vf_float.c
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)'
ap.srcs += $(SRC_BOOZ)/booz2_navigation.c
Modified: paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-28 14:04:33 UTC (rev 6003)
@@ -136,16 +136,16 @@
sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c
sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
sim.srcs += math/pprz_geodetic_int.c
-sim.srcs += $(SRC_BOOZ)/booz2_ins.c
+sim.srcs += $(SRC_FIRMWARE)/ins.c
# vertical filter float version
-sim.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
+sim.srcs += $(SRC_FIRMWARE)/ins/vf_float.c
sim.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)'
#
# INS choice
#
-# include booz2_ins_hff.makefile
+# include ins_hff.makefile
# or
# nothing
#
Modified: paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/ins_hff.makefile
2010-09-28 14:04:33 UTC (rev 6003)
@@ -3,7 +3,7 @@
#
ap.CFLAGS += -DUSE_HFF
-ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
+ap.srcs += $(SRC_FIRMWARE)/ins/hf_float.c
sim.CFLAGS += -DUSE_HFF
-sim.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
+sim.srcs += $(SRC_FIRMWARE)/ins/hf_float.c
Modified: paparazzi3/trunk/conf/flight_plans/basic_booz.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/basic_booz.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/flight_plans/basic_booz.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -34,7 +34,7 @@
<attitude pitch="0" roll="0" throttle="0" vmode="throttle"
until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
- <exception cond="booz_ins_enu_pos.z > POS_BFP_OF_REAL(2.)"
deroute="Standby"/>
+ <exception cond="ins_enu_pos.z > POS_BFP_OF_REAL(2.)" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="0.5" wp="CLIMB"/>
</block>
Modified: paparazzi3/trunk/conf/flight_plans/booz2_nova.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/booz2_nova.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/flight_plans/booz2_nova.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -23,7 +23,7 @@
<attitude pitch="0" roll="0" throttle="0" vmode="throttle"
until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
- <exception cond="booz_ins_enu_pos.z > POS_BFP_OF_REAL(2.)"
deroute="Standby"/>
+ <exception cond="ins_enu_pos.z > POS_BFP_OF_REAL(2.)" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<call fun="NavResurrect()"/>
<while cond="LessThan(NavBlockTime(), 3)"/>
Modified: paparazzi3/trunk/conf/flight_plans/booz_test_drop.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/booz_test_drop.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/flight_plans/booz_test_drop.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -26,7 +26,7 @@
<attitude pitch="0" roll="0" throttle="0" vmode="throttle"
until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
- <exception cond="booz_ins_enu_pos.z > POS_BFP_OF_REAL(2.)"
deroute="Standby"/>
+ <exception cond="ins_enu_pos.z > POS_BFP_OF_REAL(2.)" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<call fun="NavResurrect()"/>
<while cond="LessThan(NavBlockTime(), 3)"/>
Modified: paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/flight_plans/booz_test_sim.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -31,7 +31,7 @@
<attitude pitch="0" roll="0" throttle="0" vmode="throttle"
until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
- <exception cond="booz_ins_enu_pos.z > POS_BFP_OF_REAL(2.)"
deroute="Standby"/>
+ <exception cond="ins_enu_pos.z > POS_BFP_OF_REAL(2.)" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<while cond="LessThan(NavBlockTime(), 3)"/>
<stay height="4.0" vmode="alt" wp="CLIMB"/>
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/messages.xml 2010-09-28 14:04:33 UTC (rev 6003)
@@ -1036,14 +1036,14 @@
<field name="body_psi" type="int32" alt_unit="degres"
alt_unit_coef="0.0139882"/>
</message>
- <message name="BOOZ2_INS" id="151">
+ <message name="INS" id="151">
<field name="baro_alt" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
<field name="ins_z" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
<field name="ins_zd" type="int32" alt_unit="m/s"
alt_unit_coef="0.0000019"/>
<field name="ins_zdd" type="int32" alt_unit="m/s2"
alt_unit_coef="0.0009766"/>
</message>
- <message name="BOOZ2_INS2" id="152">
+ <message name="INS2" id="152">
<field name="xdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
<field name="ydd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
<field name="zdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
@@ -1055,7 +1055,7 @@
<field name="z" type="int32" alt_unit="m" alt_unit_coef="0.0039"/>
</message>
- <message name="BOOZ2_INS3" id="153">
+ <message name="INS3" id="153">
<field name="gps_x_ned" type="int32" alt_unit="m"
alt_unit_coef="0.00390625"/>
<field name="gps_y_ned" type="int32" alt_unit="m"
alt_unit_coef="0.00390625"/>
<field name="gps_z_ned" type="int32" alt_unit="m"
alt_unit_coef="0.00390625"/>
@@ -1064,7 +1064,7 @@
<field name="gps_zd_ned" type="int32" alt_unit="m/s"
alt_unit_coef="0.0000019073"/>
</message>
- <message name="BOOZ2_INS_REF" id="154">
+ <message name="INS_REF" id="154">
<field name="ecef_x0" type="int32" alt_unit="m" alt_unit_coef="0.01"/>
<field name="ecef_y0" type="int32" alt_unit="m" alt_unit_coef="0.01"/>
<field name="ecef_z0" type="int32" alt_unit="m" alt_unit_coef="0.01"/>
Modified: paparazzi3/trunk/conf/modules/sonar_maxbotix_booz.xml
===================================================================
--- paparazzi3/trunk/conf/modules/sonar_maxbotix_booz.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/modules/sonar_maxbotix_booz.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -6,7 +6,7 @@
</header>
<init fun="maxbotix_init()"/>
<periodic fun="maxbotix_read()" freq="10"/>
- <event fun="SonarEvent(booz_ins_update_sonar)"/>
+ <event fun="SonarEvent(ins_update_sonar)"/>
<makefile target="ap">
<flag name="USE_EXTRA_ADC"/>
<flag name="USE_ADC_1"/>
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -48,7 +48,7 @@
<dl_setting var="booz2_guidance_v_kd" min="-600" step="1" max="0"
module="guidance/booz2_guidance_v" shortname="kd"/>
<dl_setting var="booz2_guidance_v_ki" min="-300" step="1" max="0"
module="guidance/booz2_guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="booz2_guidance_v_z_sp" min="-5" step="0.5" max="3"
module="guidance/booz2_guidance_v" shortname="sp" unit="2e-8m" alt_unit="m"
alt_unit_coef="0.00390625"/>
- <dl_setting var="booz_ins_vf_realign" min="0" step="1" max="1"
module="booz2_ins" shortname="vf_realign" values="OFF|ON"/>
+ <dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="ins"
shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
@@ -60,7 +60,7 @@
<dl_setting var="booz2_guidance_h_igain" min="-400" step="1" max="0"
module="guidance/booz2_guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="booz2_guidance_h_ngain" min="-400" step="1" max="0"
module="guidance/booz2_guidance_h" shortname="kn"/>
<dl_setting var="booz2_guidance_h_again" min="-400" step="1" max="0"
module="guidance/booz2_guidance_h" shortname="ka"/>
- <dl_setting var="booz_ins_hf_realign" min="0" step="1" max="1"
module="booz2_ins" shortname="hf_realign" values="OFF|ON"/>
+ <dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="ins"
shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="NAV">
Modified: paparazzi3/trunk/conf/telemetry/booz_minimal.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/booz_minimal.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/telemetry/booz_minimal.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -10,7 +10,7 @@
<message name="BOOZ_STATUS" period="1.4"/>
<message name="BOOZ2_FP" period="1.2"/>
<message name="ALIVE" period="2.1"/>
- <message name="BOOZ2_INS_REF" period="10.1"/>
+ <message name="INS_REF" period="10.1"/>
<message name="BOOZ2_NAV_STATUS" period="2.6"/>
<message name="WP_MOVED" period="6.3"/>
</mode>
@@ -20,7 +20,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS_REF" period="5.1"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
Modified: paparazzi3/trunk/conf/telemetry/csc_ap.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/csc_ap.xml 2010-09-28 14:04:22 UTC (rev
6002)
+++ paparazzi3/trunk/conf/telemetry/csc_ap.xml 2010-09-28 14:04:33 UTC (rev
6003)
@@ -5,7 +5,7 @@
<mode name="default">
<message name="ALIVE" period="5"/>
<message name="ATTITUDE" period="0.1"/>
- <message name="BOOZ2_INS3" period="0.25"/>
+ <message name="INS3" period="0.25"/>
<message name="GPS_SOL" period="1.5"/>
<message name="VANE_SENSOR" period="0.2"/>
<message name="SIMPLE_COMMANDS" period="0.2"/>
@@ -28,7 +28,7 @@
<message name="DL_VALUE" period="0.5"/>
<message name="RMAT_DEBUG" period="1"/>
<message name="GPS_SOL" period="1.5"/>
- <message name="BOOZ2_INS3" period="0.25"/>
+ <message name="INS3" period="0.25"/>
</mode>
</process>
</telemetry>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -10,7 +10,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS_REF" period="5.1"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
@@ -78,8 +78,8 @@
<message name="BOOZ2_VFF" period=".05"/>
<message name="BOOZ2_VERT_LOOP" period=".05"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
- <message name="BOOZ2_INS" period=".05"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS" period=".05"/>
+ <message name="INS_REF" period="5.1"/>
</mode>
<mode name="h_loop">
@@ -92,7 +92,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="BOOZ2_HFF_GPS" period=".03"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS_REF" period="5.1"/>
</mode>
<mode name="aligner">
@@ -114,9 +114,9 @@
<!--<message name="BOOZ2_SONAR" period="0.1"/>-->
<!--<message name="BOOZ2_TUNE_HOVER" period=".1"/>-->
<!-- <message name="BOOZ2_GPS" period=".20"/> -->
- <!--<message name="BOOZ2_INS2" period=".05"/>
- <message name="BOOZ2_INS3" period=".20"/>-->
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <!--<message name="INS2" period=".05"/>
+ <message name="INS3" period=".20"/>-->
+ <message name="INS_REF" period="5.1"/>
</mode>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2010-09-28
14:04:33 UTC (rev 6003)
@@ -10,7 +10,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS_REF" period="5.1"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
@@ -77,8 +77,8 @@
<message name="BOOZ2_VFF" period=".1"/>
<message name="BOOZ2_VERT_LOOP" period=".1"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
- <message name="BOOZ2_INS" period=".2"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS" period=".2"/>
+ <message name="INS_REF" period="5.1"/>
</mode>
<mode name="h_loop">
@@ -91,7 +91,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="BOOZ2_HFF_GPS" period=".1"/>
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <message name="INS_REF" period="5.1"/>
</mode>
<mode name="aligner">
@@ -113,9 +113,9 @@
<!--<message name="BOOZ2_SONAR" period="0.1"/>-->
<!--<message name="BOOZ2_TUNE_HOVER" period=".1"/>-->
<!-- <message name="BOOZ2_GPS" period=".20"/> -->
- <!--<message name="BOOZ2_INS2" period=".05"/>
- <message name="BOOZ2_INS3" period=".20"/>-->
- <message name="BOOZ2_INS_REF" period="5.1"/>
+ <!--<message name="INS2" period=".05"/>
+ <message name="INS3" period=".20"/>-->
+ <message name="INS_REF" period="5.1"/>
</mode>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -41,7 +41,7 @@
#include "booz2_navigation.h"
#include "math/pprz_geodetic_int.h"
-#include "booz2_ins.h"
+#include "ins.h"
#define IdOfMsg(x) (x[1])
@@ -94,8 +94,8 @@
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
- lla.alt = DL_MOVE_WP_alt(dl_buffer) - booz_ins_ltp_def.hmsl +
booz_ins_ltp_def.lla.alt;
- enu_of_lla_point_i(&enu,&booz_ins_ltp_def,&lla);
+ lla.alt = DL_MOVE_WP_alt(dl_buffer) - ins_ltp_def.hmsl +
ins_ltp_def.lla.alt;
+ enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-28
14:04:33 UTC (rev 6003)
@@ -27,7 +27,7 @@
#include "booz/booz2_debug.h"
#include "booz_gps.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "autopilot.h"
#include "modules.h"
@@ -110,7 +110,7 @@
/* compute a vector to the waypoint */
struct Int32Vect2 path_to_waypoint;
- VECT2_DIFF(path_to_waypoint, booz2_navigation_target, booz_ins_enu_pos);
+ VECT2_DIFF(path_to_waypoint, booz2_navigation_target, ins_enu_pos);
/* saturate it */
VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
@@ -126,7 +126,7 @@
struct Int32Vect2 path_to_carrot;
VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
- VECT2_SUM(booz2_navigation_carrot, path_to_carrot, booz_ins_enu_pos);
+ VECT2_SUM(booz2_navigation_carrot, path_to_carrot, ins_enu_pos);
}
#else
// if H_REF is used, CARROT_DIST is not used
@@ -142,7 +142,7 @@
}
else {
struct Int32Vect2 pos_diff;
- VECT2_DIFF(pos_diff, booz_ins_enu_pos,waypoints[wp_center]);
+ VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_center]);
// go back to half metric precision or values are too large
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
@@ -186,7 +186,7 @@
void nav_route(uint8_t wp_start, uint8_t wp_end) {
struct Int32Vect2 wp_diff,pos_diff;
VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
- VECT2_DIFF(pos_diff, booz_ins_enu_pos,waypoints[wp_start]);
+ VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_start]);
// go back to metric precision or values are too large
INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
@@ -222,7 +222,7 @@
int32_t dist_to_point;
struct Int32Vect2 diff;
static uint8_t time_at_wp = 0;
- VECT2_DIFF(diff, waypoints[wp_idx], booz_ins_enu_pos);
+ VECT2_DIFF(diff, waypoints[wp_idx], ins_enu_pos);
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_VECT2_NORM(dist_to_point, diff);
//printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
@@ -250,25 +250,25 @@
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
- booz_ins_ltp_initialised = FALSE;
- booz_ins_hf_realign = TRUE;
- booz_ins_vf_realign = TRUE;
+ ins_ltp_initialised = FALSE;
+ ins_hf_realign = TRUE;
+ ins_vf_realign = TRUE;
return 0;
}
unit_t nav_reset_alt( void ) {
- booz_ins_vf_realign = TRUE;
+ ins_vf_realign = TRUE;
#ifdef USE_GPS
- booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
- booz_ins_ltp_def.hmsl = booz_gps_state.hmsl;
+ ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
+ ins_ltp_def.hmsl = booz_gps_state.hmsl;
#endif
return 0;
}
void nav_init_stage( void ) {
- INT32_VECT3_COPY(nav_last_point, booz_ins_enu_pos);
+ INT32_VECT3_COPY(nav_last_point, ins_enu_pos);
stage_time = 0;
nav_circle_radians = 0;
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
@@ -300,7 +300,7 @@
/* run carrot loop */
booz2_nav_run();
- ground_alt = POS_BFP_OF_REAL((float)booz_ins_ltp_def.hmsl / 100.);
+ ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.);
}
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2010-09-28
14:04:33 UTC (rev 6003)
@@ -112,7 +112,7 @@
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
-#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp],
booz_ins_enu_pos); FALSE; })
+#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], ins_enu_pos);
FALSE; })
#define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1],
waypoints[_wp2]); FALSE; })
#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
@@ -212,9 +212,9 @@
}
-#define GetPosX() POS_FLOAT_OF_BFP(booz_ins_enu_pos.x)
-#define GetPosY() POS_FLOAT_OF_BFP(booz_ins_enu_pos.y)
-#define GetPosAlt() (POS_FLOAT_OF_BFP(booz_ins_enu_pos.z+ground_alt))
+#define GetPosX() POS_FLOAT_OF_BFP(ins_enu_pos.x)
+#define GetPosY() POS_FLOAT_OF_BFP(ins_enu_pos.y)
+#define GetPosAlt() (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))
extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3
speed_sp, int16_t heading_rate_sp );
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -48,7 +48,7 @@
#include "booz2_battery.h"
#include <firmwares/rotorcraft/imu.h>
#include "booz_gps.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "ahrs.h"
#include "i2c_hw.h"
@@ -472,7 +472,7 @@
}
#ifdef USE_VFF
-#include "ins/booz2_vf_float.h"
+#include "ins/vf_float.h"
#define PERIODIC_SEND_BOOZ2_VFF(_chan) { \
DOWNLINK_SEND_BOOZ2_VFF(_chan, \
&b2_vff_z_meas, \
@@ -488,7 +488,7 @@
#endif
#ifdef USE_HFF
-#include "ins/booz2_hf_float.h"
+#include "ins/hf_float.h"
#define PERIODIC_SEND_BOOZ2_HFF(_chan) { \
DOWNLINK_SEND_BOOZ2_HFF(_chan, \
&b2_hff_state.x, \
@@ -533,21 +533,21 @@
&booz2_guidance_h_held_pos.y); \
}
-#define PERIODIC_SEND_BOOZ2_INS(_chan) { \
- DOWNLINK_SEND_BOOZ2_INS(_chan, \
- &booz_ins_baro_alt, \
- &booz_ins_ltp_pos.z, \
- &booz_ins_ltp_speed.z, \
- &booz_ins_ltp_accel.z); \
+#define PERIODIC_SEND_INS(_chan) { \
+ DOWNLINK_SEND_INS(_chan, \
+ &ins_baro_alt, \
+ &ins_ltp_pos.z, \
+ &ins_ltp_speed.z, \
+ &ins_ltp_accel.z); \
}
-#define PERIODIC_SEND_BOOZ2_INS2(_chan) { \
+#define PERIODIC_SEND_INS2(_chan) { \
struct Int32Vect3 pos_low_res; \
pos_low_res.x = (int32_t)(b2ins_pos_ltp.x>>20); \
pos_low_res.y = (int32_t)(b2ins_pos_ltp.y>>20); \
pos_low_res.z = (int32_t)(b2ins_pos_ltp.z>>20); \
- DOWNLINK_SEND_BOOZ2_INS2(_chan, \
+ DOWNLINK_SEND_INS2(_chan, \
&b2ins_accel_ltp.x, \
&b2ins_accel_ltp.y, \
&b2ins_accel_ltp.z, \
@@ -561,9 +561,9 @@
}
#ifdef USE_GPS
-#include "ins/booz2_hf_float.h"
-#define PERIODIC_SEND_BOOZ2_INS3(_chan) { \
- DOWNLINK_SEND_BOOZ2_INS3(_chan, \
+#include "ins/hf_float.h"
+#define PERIODIC_SEND_INS3(_chan) { \
+ DOWNLINK_SEND_INS3(_chan, \
&b2ins_meas_gps_pos_ned.x, \
&b2ins_meas_gps_pos_ned.y, \
&b2ins_meas_gps_pos_ned.z, \
@@ -573,32 +573,32 @@
); \
}
#else /* !USE_GPS */
-#define PERIODIC_SEND_BOOZ2_INS3(_chan) {}
+#define PERIODIC_SEND_INS3(_chan) {}
#endif /* USE_GPS */
#define PERIODIC_SEND_BOOZ_INS(_chan) { \
DOWNLINK_SEND_BOOZ_INS(_chan, \
- &booz_ins_ltp_pos.x, \
- &booz_ins_ltp_pos.y, \
- &booz_ins_ltp_pos.z, \
- &booz_ins_ltp_speed.x, \
- &booz_ins_ltp_speed.y, \
- &booz_ins_ltp_speed.z, \
- &booz_ins_ltp_accel.x, \
- &booz_ins_ltp_accel.y, \
- &booz_ins_ltp_accel.z); \
+ &ins_ltp_pos.x, \
+ &ins_ltp_pos.y, \
+ &ins_ltp_pos.z, \
+ &ins_ltp_speed.x, \
+ &ins_ltp_speed.y, \
+ &ins_ltp_speed.z, \
+ &ins_ltp_accel.x, \
+ &ins_ltp_accel.y, \
+ &ins_ltp_accel.z); \
}
-#define PERIODIC_SEND_BOOZ2_INS_REF(_chan) { \
- DOWNLINK_SEND_BOOZ2_INS_REF(_chan, \
- &booz_ins_ltp_def.ecef.x, \
- &booz_ins_ltp_def.ecef.y, \
- &booz_ins_ltp_def.ecef.z, \
- &booz_ins_ltp_def.lla.lat, \
- &booz_ins_ltp_def.lla.lon, \
- &booz_ins_ltp_def.lla.alt, \
- &booz_ins_ltp_def.hmsl, \
- &booz_ins_qfe); \
+#define PERIODIC_SEND_INS_REF(_chan) { \
+ DOWNLINK_SEND_INS_REF(_chan, \
+ &ins_ltp_def.ecef.x, \
+ &ins_ltp_def.ecef.y, \
+ &ins_ltp_def.ecef.z, \
+ &ins_ltp_def.lla.lat, \
+ &ins_ltp_def.lla.lon, \
+ &ins_ltp_def.lla.alt, \
+ &ins_ltp_def.hmsl, \
+ &ins_qfe); \
}
@@ -607,9 +607,9 @@
DOWNLINK_SEND_BOOZ2_VERT_LOOP(_chan, \
&booz2_guidance_v_z_sp, \
&booz2_guidance_v_zd_sp, \
- &booz_ins_ltp_pos.z, \
- &booz_ins_ltp_speed.z, \
- &booz_ins_ltp_accel.z, \
+ &ins_ltp_pos.z, \
+ &ins_ltp_speed.z, \
+ &ins_ltp_accel.z, \
&booz2_guidance_v_z_ref, \
&booz2_guidance_v_zd_ref, \
&booz2_guidance_v_zdd_ref, \
@@ -626,12 +626,12 @@
DOWNLINK_SEND_BOOZ2_HOVER_LOOP(_chan, \
&booz2_guidance_h_pos_sp.x, \
&booz2_guidance_h_pos_sp.y, \
- &booz_ins_ltp_pos.x, \
- &booz_ins_ltp_pos.y, \
- &booz_ins_ltp_speed.x, \
- &booz_ins_ltp_speed.y, \
- &booz_ins_ltp_accel.x, \
- &booz_ins_ltp_accel.y, \
+ &ins_ltp_pos.x, \
+ &ins_ltp_pos.y, \
+ &ins_ltp_speed.x, \
+ &ins_ltp_speed.y, \
+ &ins_ltp_accel.x, \
+ &ins_ltp_accel.y, \
&booz2_guidance_h_pos_err.x, \
&booz2_guidance_h_pos_err.y, \
&booz2_guidance_h_speed_err.x, \
@@ -664,12 +664,12 @@
#define PERIODIC_SEND_BOOZ2_FP(_chan) {
\
int32_t carrot_up = -booz2_guidance_v_z_sp;
\
DOWNLINK_SEND_BOOZ2_FP( _chan, \
- &booz_ins_enu_pos.x, \
- &booz_ins_enu_pos.y, \
- &booz_ins_enu_pos.z, \
- &booz_ins_enu_speed.x, \
- &booz_ins_enu_speed.y, \
- &booz_ins_enu_speed.z, \
+ &ins_enu_pos.x, \
+ &ins_enu_pos.y, \
+ &ins_enu_pos.z, \
+ &ins_enu_speed.x, \
+ &ins_enu_speed.y, \
+ &ins_enu_speed.z, \
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi, \
Modified: paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -23,7 +23,7 @@
#include "booz_fms.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "math/pprz_algebra_int.h"
#define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW
@@ -69,7 +69,7 @@
#if 0
case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)
- booz_fms_test_signal_start_z = booz_ins_ltp_pos.z;
+ booz_fms_test_signal_start_z = ins_ltp_pos.z;
else {
booz_fms_input.v_sp.height = (booz_fms_test_signal_counter <
booz_fms_test_signal_period) ?
booz_fms_test_signal_start_z :
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_h.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -28,7 +28,7 @@
#include "ahrs.h"
#include "booz_stabilization.h"
#include "booz_fms.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "booz2_navigation.h"
#include "airframe.h"
@@ -224,12 +224,12 @@
static inline void booz2_guidance_h_hover_run(void) {
/* compute position error */
- VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos,
booz2_guidance_h_pos_sp);
+ VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_sp);
/* saturate it */
VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
- VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
+ VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed);
/* saturate it */
VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -293,13 +293,13 @@
#endif
/* compute position error */
- VECT2_DIFF(booz2_guidance_h_pos_err, booz_ins_ltp_pos,
booz2_guidance_h_pos_ref);
+ VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_ref);
/* saturate it */
VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
- //VECT2_COPY(booz2_guidance_h_speed_err, booz_ins_ltp_speed);
- VECT2_DIFF(booz2_guidance_h_speed_err, booz_ins_ltp_speed,
booz2_guidance_h_speed_ref);
+ //VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed);
+ VECT2_DIFF(booz2_guidance_h_speed_err, ins_ltp_speed,
booz2_guidance_h_speed_ref);
/* saturate it */
VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
@@ -313,16 +313,16 @@
}
else { // Tracking algorithm, no integral
int32_t vect_prod = 0;
- int32_t scal_prod = booz_ins_ltp_speed.x * booz2_guidance_h_pos_err.x +
booz_ins_ltp_speed.y * booz2_guidance_h_pos_err.y;
+ int32_t scal_prod = ins_ltp_speed.x * booz2_guidance_h_pos_err.x +
ins_ltp_speed.y * booz2_guidance_h_pos_err.y;
// compute vectorial product only if angle < pi/2 (scalar product > 0)
if (scal_prod >= 0) {
- vect_prod = ((booz_ins_ltp_speed.x * booz2_guidance_h_pos_err.y) >>
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10))
- - ((booz_ins_ltp_speed.y * booz2_guidance_h_pos_err.x) >>
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10));
+ vect_prod = ((ins_ltp_speed.x * booz2_guidance_h_pos_err.y) >>
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10))
+ - ((ins_ltp_speed.y * booz2_guidance_h_pos_err.x) >>
(INT32_POS_FRAC + INT32_SPEED_FRAC - 10));
}
// multiply by vector orthogonal to speed
VECT2_ASSIGN(booz2_guidance_h_nav_err,
- vect_prod * (-booz_ins_ltp_speed.y),
- vect_prod * booz_ins_ltp_speed.x);
+ vect_prod * (-ins_ltp_speed.y),
+ vect_prod * ins_ltp_speed.x);
// divide by 2 times dist ( >> 16 )
VECT2_SDIV(booz2_guidance_h_nav_err, booz2_guidance_h_nav_err, dist*dist);
// *2 ??
@@ -370,7 +370,7 @@
static inline void booz2_guidance_h_hover_enter(void) {
- VECT2_COPY(booz2_guidance_h_pos_sp, booz_ins_ltp_pos);
+ VECT2_COPY(booz2_guidance_h_pos_sp, ins_ltp_pos);
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz2_guidance_h_rc_sp );
@@ -383,8 +383,8 @@
INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
struct Int32Vect2 pos,speed,zero;
INT_VECT2_ZERO(zero);
- VECT2_COPY(pos, booz_ins_ltp_pos);
- VECT2_COPY(speed, booz_ins_ltp_speed);
+ VECT2_COPY(pos, ins_ltp_pos);
+ VECT2_COPY(speed, ins_ltp_speed);
Booz2GuidanceHSetRef(pos, speed, zero);
struct Int32Eulers tmp_sp;
Modified: paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/booz/guidance/booz2_guidance_v.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -32,7 +32,7 @@
#include "booz_fms.h"
#include "booz2_navigation.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "math/pprz_algebra_int.h"
#include "airframe.h"
@@ -126,7 +126,7 @@
case BOOZ2_GUIDANCE_V_MODE_HOVER:
case BOOZ2_GUIDANCE_V_MODE_NAV:
booz2_guidance_v_z_sum_err = 0;
- Booz2GuidanceVSetRef(booz_ins_ltp_pos.z, booz_ins_ltp_speed.z, 0);
+ Booz2GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
break;
default:
break;
@@ -150,18 +150,18 @@
if (in_flight) {
// we should use something after the supervision!!! fuck!!!
int32_t cmd_hack = Chop(booz_stabilization_cmd[COMMAND_THRUST], 1, 200);
- b2_gv_adapt_run(booz_ins_ltp_accel.z, cmd_hack);
+ b2_gv_adapt_run(ins_ltp_accel.z, cmd_hack);
}
else {
// reset vertical filter until takeoff
- //booz_ins_vf_realign = TRUE;
+ //ins_vf_realign = TRUE;
}
switch (booz2_guidance_v_mode) {
case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT:
- booz2_guidance_v_z_sp = booz_ins_ltp_pos.z; // not sure why we do that
- Booz2GuidanceVSetRef(booz_ins_ltp_pos.z, 0, 0); // or that - mode enter
should take care of it ?
+ booz2_guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that
+ Booz2GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should
take care of it ?
booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t;
break;
@@ -236,9 +236,9 @@
booz2_guidance_v_zd_ref = b2_gv_zd_ref<<(INT32_SPEED_FRAC -
B2_GV_ZD_REF_FRAC);
booz2_guidance_v_zdd_ref = b2_gv_zdd_ref<<(INT32_ACCEL_FRAC -
B2_GV_ZDD_REF_FRAC);
/* compute the error to our reference */
- int32_t err_z = booz_ins_ltp_pos.z - booz2_guidance_v_z_ref;
+ int32_t err_z = ins_ltp_pos.z - booz2_guidance_v_z_ref;
Bound(err_z, BOOZ2_GUIDANCE_V_MIN_ERR_Z, BOOZ2_GUIDANCE_V_MAX_ERR_Z);
- int32_t err_zd = booz_ins_ltp_speed.z - booz2_guidance_v_zd_ref;
+ int32_t err_zd = ins_ltp_speed.z - booz2_guidance_v_zd_ref;
Bound(err_zd, BOOZ2_GUIDANCE_V_MIN_ERR_ZD, BOOZ2_GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) {
Modified: paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h 2010-09-28 14:04:22 UTC
(rev 6002)
+++ paparazzi3/trunk/sw/airborne/csc/csc_telemetry.h 2010-09-28 14:04:33 UTC
(rev 6003)
@@ -106,8 +106,8 @@
&csc_gps_errors.rate.z) \
}
-#define PERIODIC_SEND_BOOZ2_INS3(_chan) { \
-DOWNLINK_SEND_BOOZ2_INS3(_chan, \
+#define PERIODIC_SEND_INS3(_chan) { \
+DOWNLINK_SEND_INS3(_chan, \
&booz_ins_gps_pos_cm_ned.x, \
&booz_ins_gps_pos_cm_ned.y, \
&booz_ins_gps_pos_cm_ned.z, \
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.h
2010-09-28 14:04:33 UTC (rev 6003)
@@ -30,7 +30,7 @@
#include "led.h"
#include "airframe.h"
-#include "booz2_ins.h"
+#include "ins.h"
#define AP_MODE_FAILSAFE 0
#define AP_MODE_KILL 1
@@ -106,8 +106,8 @@
#endif
static inline void DetectGroundEvent(void) {
if (autopilot_mode == AP_MODE_FAILSAFE || autopilot_detect_ground_once) {
- if (booz_ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT ||
- booz_ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) {
+ if (ins_ltp_accel.z < -TRESHOLD_GROUND_DETECT ||
+ ins_ltp_accel.z > TRESHOLD_GROUND_DETECT) {
autopilot_detect_ground = TRUE;
autopilot_detect_ground_once = FALSE;
}
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float-old.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_hf_float.h"
-#include "booz2_ins.h"
+#include "hf_float.h"
+#include "ins.h"
#include <firmwares/rotorcraft/imu.h>
#include "ahrs.h"
@@ -79,7 +79,7 @@
#ifdef UPDATE_FROM_POS
struct Int64Vect2 scaled_pos_meas;
/* INT32 pos in cm -> INT64 pos in cm*/
- VECT2_COPY(scaled_pos_meas, booz_ins_gps_pos_cm_ned);
+ VECT2_COPY(scaled_pos_meas, ins_gps_pos_cm_ned);
/* to BFP but still in cm */
VECT2_SMUL(scaled_pos_meas, scaled_pos_meas, (1<<B2INS_POS_LTP_FRAC));
/* INT64 BFP pos in cm -> INT64 BFP pos in m */
@@ -95,7 +95,7 @@
#endif /* UPDATE_FROM_POS */
#ifdef UPDATE_FROM_SPEED
- INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+ INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
struct Int32Vect2 scaled_speed_meas;
VECT2_SMUL(scaled_speed_meas, b2ins_meas_gps_speed_ned,
(1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)));
struct Int32Vect2 speed_residual;
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/hf_float.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -22,8 +22,8 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_hf_float.h"
-#include "booz2_ins.h"
+#include "hf_float.h"
+#include "ins.h"
#include <firmwares/rotorcraft/imu.h>
#include "ahrs.h"
#include "booz_gps.h"
@@ -454,12 +454,12 @@
b2_hff_propagate_y(&b2_hff_state);
/* update ins state from horizontal filter */
- booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
- booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
- booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
- booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
- booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
- booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+ 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 */
@@ -502,20 +502,20 @@
#endif
/* update filter state with measurement */
- b2_hff_update_x(&b2_hff_state, booz_ins_gps_pos_m_ned.x, Rgps_pos);
- b2_hff_update_y(&b2_hff_state, booz_ins_gps_pos_m_ned.y, Rgps_pos);
+ 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 B2_HFF_UPDATE_SPEED
- b2_hff_update_xdot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.x, Rgps_vel);
- b2_hff_update_ydot(&b2_hff_state, booz_ins_gps_speed_m_s_ned.y, Rgps_vel);
+ 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 */
- booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
- booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
- booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
- booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
- booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
- booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+ 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) {
@@ -524,11 +524,11 @@
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n",
b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = TRUE;
- b2_hff_update_x(b2_hff_rb_last, booz_ins_gps_pos_m_ned.x, Rgps_pos);
- b2_hff_update_y(b2_hff_rb_last, booz_ins_gps_pos_m_ned.y, Rgps_pos);
+ 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 B2_HFF_UPDATE_SPEED
- b2_hff_update_xdot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.x,
Rgps_vel);
- b2_hff_update_ydot(b2_hff_rb_last, booz_ins_gps_speed_m_s_ned.y,
Rgps_vel);
+ 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));
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_float.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_vf_float.h"
+#include "vf_float.h"
/*
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_vf_int.h"
+#include "vf_int.h"
#include "booz_geometry_mixed.h"
@@ -46,7 +46,7 @@
#define VFI_R BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
-void booz2_vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
+void vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
// initialize state vector
b2_vfi_z = z0;
@@ -82,7 +82,7 @@
*/
-void booz2_vfi_propagate( int32_t accel_reading ) {
+void vfi_propagate( int32_t accel_reading ) {
// compute unbiased vertical acceleration
b2_vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, B2_VFI_ZDD_FRAC) -
b2_vfi_abias;
@@ -120,7 +120,7 @@
}
-void booz2_vfi_update( int32_t z_meas ) {
+void vfi_update( int32_t z_meas ) {
const int64_t y = (z_meas<<(B2_VFI_Z_FRAC-B2_VFI_MEAS_Z_FRAC)) - b2_vfi_z;
const int32_t S = b2_vfi_P[0][0] + VFI_R;
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins/vf_int.h
2010-09-28 14:04:33 UTC (rev 6003)
@@ -27,12 +27,12 @@
#include "std.h"
#include "booz_geometry_int.h"
-extern void booz2_vfi_init( int32_t z0, int32_t zd0, int32_t bias0 );
-extern void booz2_vfi_propagate( int32_t accel_reading );
+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 booz2_vfi_update( int32_t z_meas );
+extern void vfi_update( int32_t z_meas );
#define B2_VFI_Z_MEAS_FRAC IPOS_FRAC
/* propagate frequency : 512 Hz */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.c 2010-09-28
14:04:33 UTC (rev 6003)
@@ -22,7 +22,7 @@
* Boston, MA 02111-1307, USA.
*/
-#include "booz2_ins.h"
+#include "ins.h"
#include <firmwares/rotorcraft/imu.h>
#include "firmwares/rotorcraft/baro.h"
@@ -35,11 +35,11 @@
#include "ahrs.h"
#ifdef USE_VFF
-#include "ins/booz2_vf_float.h"
+#include "ins/vf_float.h"
#endif
#ifdef USE_HFF
-#include "ins/booz2_hf_float.h"
+#include "ins/hf_float.h"
#endif
#ifdef BOOZ2_SONAR
@@ -57,96 +57,96 @@
#include "flight_plan.h"
/* gps transformed to LTP-NED */
-struct LtpDef_i booz_ins_ltp_def;
- bool_t booz_ins_ltp_initialised;
-struct NedCoor_i booz_ins_gps_pos_cm_ned;
-struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
+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 booz_ins_gps_pos_m_ned;
-struct FloatVect2 booz_ins_gps_speed_m_s_ned;
+struct FloatVect2 ins_gps_pos_m_ned;
+struct FloatVect2 ins_gps_speed_m_s_ned;
#endif
-bool_t booz_ins_hf_realign;
+bool_t ins_hf_realign;
/* barometer */
#ifdef USE_VFF
-int32_t booz_ins_qfe;
-bool_t booz_ins_baro_initialised;
-int32_t booz_ins_baro_alt;
+int32_t ins_qfe;
+bool_t ins_baro_initialised;
+int32_t ins_baro_alt;
#ifdef BOOZ2_SONAR
-bool_t booz_ins_update_on_agl;
-int32_t booz_ins_sonar_offset;
+bool_t ins_update_on_agl;
+int32_t ins_sonar_offset;
#endif
#endif
-bool_t booz_ins_vf_realign;
+bool_t ins_vf_realign;
/* output */
-struct NedCoor_i booz_ins_ltp_pos;
-struct NedCoor_i booz_ins_ltp_speed;
-struct NedCoor_i booz_ins_ltp_accel;
-struct EnuCoor_i booz_ins_enu_pos;
-struct EnuCoor_i booz_ins_enu_speed;
-struct EnuCoor_i booz_ins_enu_accel;
+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 booz_ins_init() {
+void ins_init() {
#ifdef USE_INS_NAV_INIT
- booz_ins_ltp_initialised = TRUE;
+ ins_ltp_initialised = TRUE;
/** FIXME: should use the same code than MOVE_WP in booz2_datalink.c */
struct LlaCoor_i llh; /* Height above the ellipsoid */
llh.lat = INT32_RAD_OF_DEG(NAV_LAT0);
llh.lon = INT32_RAD_OF_DEG(NAV_LON0);
- //llh.alt = NAV_ALT0 - booz_ins_ltp_def.hmsl + booz_ins_ltp_def.lla.alt;
+ //llh.alt = NAV_ALT0 - 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(&booz_ins_ltp_def, &nav_init);
- booz_ins_ltp_def.hmsl = NAV_ALT0;
+ ltp_def_from_ecef_i(&ins_ltp_def, &nav_init);
+ ins_ltp_def.hmsl = NAV_ALT0;
#else
- booz_ins_ltp_initialised = FALSE;
+ ins_ltp_initialised = FALSE;
#endif
#ifdef USE_VFF
- booz_ins_baro_initialised = FALSE;
+ ins_baro_initialised = FALSE;
#ifdef BOOZ2_SONAR
- booz_ins_update_on_agl = FALSE;
+ ins_update_on_agl = FALSE;
#endif
b2_vff_init(0., 0., 0.);
#endif
- booz_ins_vf_realign = FALSE;
- booz_ins_hf_realign = FALSE;
+ ins_vf_realign = FALSE;
+ ins_hf_realign = FALSE;
#ifdef USE_HFF
b2_hff_init(0., 0., 0., 0.);
#endif
- INT32_VECT3_ZERO(booz_ins_ltp_pos);
- INT32_VECT3_ZERO(booz_ins_ltp_speed);
- INT32_VECT3_ZERO(booz_ins_ltp_accel);
- INT32_VECT3_ZERO(booz_ins_enu_pos);
- INT32_VECT3_ZERO(booz_ins_enu_speed);
- INT32_VECT3_ZERO(booz_ins_enu_accel);
+ 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 booz_ins_periodic( void ) {
+void ins_periodic( void ) {
}
#ifdef USE_HFF
-void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
+void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
b2_hff_realign(pos, speed);
}
#else
-void booz_ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct
FloatVect2 speed __attribute__ ((unused))) {}
+void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct
FloatVect2 speed __attribute__ ((unused))) {}
#endif /* USE_HFF */
-void booz_ins_realign_v(float z) {
+void ins_realign_v(float z) {
#ifdef USE_VFF
b2_vff_realign(z);
#endif
}
-void booz_ins_propagate() {
+void ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_body;
INT32_RMAT_TRANSP_VMULT(accel_body, imu.body_to_imu_rmat, imu.accel);
@@ -155,54 +155,54 @@
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
#ifdef USE_VFF
- if (baro.status == BS_RUNNING && booz_ins_baro_initialised) {
+ if (baro.status == BS_RUNNING && ins_baro_initialised) {
b2_vff_propagate(z_accel_float);
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
}
else { // feed accel from the sensors
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
}
#else
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
#endif /* USE_VFF */
#ifdef USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
#else
- booz_ins_ltp_accel.x = accel_ltp.x;
- booz_ins_ltp_accel.y = accel_ltp.y;
+ ins_ltp_accel.x = accel_ltp.x;
+ ins_ltp_accel.y = accel_ltp.y;
#endif /* USE_HFF */
- INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
- INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
- INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
+ 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 booz_ins_update_baro() {
+void ins_update_baro() {
#ifdef USE_VFF
if (baro.status == BS_RUNNING) {
- if (!booz_ins_baro_initialised) {
- booz_ins_qfe = baro.absolute;
- booz_ins_baro_initialised = TRUE;
+ if (!ins_baro_initialised) {
+ ins_qfe = baro.absolute;
+ ins_baro_initialised = TRUE;
}
- booz_ins_baro_alt = ((baro.absolute - booz_ins_qfe) *
BOOZ_INS_BARO_SENS_NUM)/BOOZ_INS_BARO_SENS_DEN;
- float alt_float = POS_FLOAT_OF_BFP(booz_ins_baro_alt);
- if (booz_ins_vf_realign) {
- booz_ins_vf_realign = FALSE;
- booz_ins_qfe = baro.absolute;
+ ins_baro_alt = ((baro.absolute - ins_qfe) *
BOOZ_INS_BARO_SENS_NUM)/BOOZ_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
- booz_ins_sonar_offset = sonar_meas;
+ ins_sonar_offset = sonar_meas;
#endif
b2_vff_realign(0.);
- booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
- booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
- booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
- booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
- booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
- booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
+ ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
+ ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
+ ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
+ ins_enu_pos.z = -ins_ltp_pos.z;
+ ins_enu_speed.z = -ins_ltp_speed.z;
+ ins_enu_accel.z = -ins_ltp_accel.z;
}
b2_vff_update(alt_float);
}
@@ -210,24 +210,24 @@
}
-void booz_ins_update_gps(void) {
+void ins_update_gps(void) {
#ifdef USE_GPS
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
- if (!booz_ins_ltp_initialised) {
- ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos);
- booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
- booz_ins_ltp_def.hmsl = booz_gps_state.hmsl;
- booz_ins_ltp_initialised = TRUE;
+ 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(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def,
&booz_gps_state.ecef_pos);
- ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def,
&booz_gps_state.ecef_vel);
+ 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(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_cm_ned.x,
booz_ins_gps_pos_cm_ned.y);
- VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
- VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x,
booz_ins_gps_speed_cm_s_ned.y);
- VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
- if (booz_ins_hf_realign) {
- booz_ins_hf_realign = FALSE;
+ 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);
@@ -235,48 +235,48 @@
b2_hff_realign(true_pos, true_speed);
#else
const struct FloatVect2 zero = {0.0, 0.0};
- b2_hff_realign(booz_ins_gps_pos_m_ned, zero);
+ b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
#ifndef USE_VFF /* vff not used */
- booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) /
INT32_POS_OF_CM_DEN;
- booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z *
INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
+ 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(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
- INT32_VECT3_SCALE_3(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+ 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(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
- INT32_VECT2_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+ 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, booz_ins_ltp_speed);
+ VECT2_COPY(d_pos, ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
- VECT2_ADD(booz_ins_ltp_pos, d_pos);
+ VECT2_ADD(ins_ltp_pos, d_pos);
#endif
#endif /* hff not used */
- INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
- INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
- INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
+ 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 booz_ins_update_sonar() {
+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 (booz_ins_update_on_agl && booz2_analog_baro_status ==
BOOZ2_ANALOG_BARO_RUNNING) {
- int32_t d_sonar = (((int32_t)sonar_filtered - booz_ins_sonar_offset) *
BOOZ_INS_SONAR_SENS_NUM) / BOOZ_INS_SONAR_SENS_DEN;
- booz_ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar *
(BOOZ_INS_BARO_SENS_DEN))/BOOZ_INS_BARO_SENS_NUM;
+ if (ins_update_on_agl && booz2_analog_baro_status ==
BOOZ2_ANALOG_BARO_RUNNING) {
+ int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) *
BOOZ_INS_SONAR_SENS_NUM) / BOOZ_INS_SONAR_SENS_DEN;
+ ins_qfe = (int32_t)booz2_analog_baro_value + (d_sonar *
(BOOZ_INS_BARO_SENS_DEN))/BOOZ_INS_BARO_SENS_NUM;
}
#endif
}
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/ins.h 2010-09-28
14:04:33 UTC (rev 6003)
@@ -21,55 +21,55 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_INS_H
-#define BOOZ2_INS_H
+#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 booz_ins_ltp_def;
-extern bool_t booz_ins_ltp_initialised;
-extern struct NedCoor_i booz_ins_gps_pos_cm_ned;
-extern struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
+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 booz_ins_baro_alt;
-extern int32_t booz_ins_qfe;
-extern bool_t booz_ins_baro_initialised;
+extern int32_t ins_baro_alt;
+extern int32_t ins_qfe;
+extern bool_t ins_baro_initialised;
#ifdef BOOZ2_SONAR
-extern bool_t booz_ins_update_on_agl; /* use sonar to update agl if available
*/
-extern int32_t booz_ins_sonar_offset;
+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 booz_ins_ltp_pos;
-extern struct NedCoor_i booz_ins_ltp_speed;
-extern struct NedCoor_i booz_ins_ltp_accel;
+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 booz_ins_enu_pos;
-extern struct EnuCoor_i booz_ins_enu_speed;
-extern struct EnuCoor_i booz_ins_enu_accel;
+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 booz_ins_gps_pos_m_ned;
-extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
+extern struct FloatVect2 ins_gps_pos_m_ned;
+extern struct FloatVect2 ins_gps_speed_m_s_ned;
#endif
-extern bool_t booz_ins_hf_realign;
-extern bool_t booz_ins_vf_realign;
+extern bool_t ins_hf_realign;
+extern bool_t ins_vf_realign;
-extern void booz_ins_init( void );
-extern void booz_ins_periodic( void );
-extern void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
-extern void booz_ins_realign_v(float z);
-extern void booz_ins_propagate( void );
-extern void booz_ins_update_baro( void );
-extern void booz_ins_update_gps( void );
-extern void booz_ins_update_sonar( void );
+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 /* BOOZ2_INS_H */
+#endif /* INS_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-28
14:04:33 UTC (rev 6003)
@@ -53,7 +53,7 @@
#include "booz_guidance.h"
#include "ahrs.h"
-#include "booz2_ins.h"
+#include "ins.h"
#if defined USE_CAM || USE_DROP
#include "booz2_pwm_hw.h"
@@ -122,7 +122,7 @@
ahrs_aligner_init();
ahrs_init();
- booz_ins_init();
+ ins_init();
#ifdef USE_GPS
booz_gps_init();
@@ -231,12 +231,12 @@
#ifdef SITL
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
- booz_ins_propagate();
+ ins_propagate();
}
}
static inline void on_baro_abs_event( void ) {
- booz_ins_update_baro();
+ ins_update_baro();
}
static inline void on_baro_dif_event( void ) {
@@ -244,7 +244,7 @@
}
static inline void on_gps_event(void) {
- booz_ins_update_gps();
+ ins_update_gps();
}
static inline void on_mag_event(void) {
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-09-28
14:04:33 UTC (rev 6003)
@@ -26,7 +26,7 @@
#include "booz2_pwm_hw.h"
#include "ahrs.h"
#include "booz2_navigation.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "flight_plan.h"
uint8_t booz_cam_mode;
@@ -116,14 +116,14 @@
#ifdef WP_CAM
{
struct Int32Vect2 diff;
- VECT2_DIFF(diff, waypoints[WP_CAM], booz_ins_enu_pos);
+ VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos);
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
nav_heading = booz_cam_pan;
#ifdef BOOZ_CAM_USE_TILT_ANGLES
int32_t dist, height;
INT32_VECT2_NORM(dist, diff);
- height = (waypoints[WP_CAM].z - booz_ins_enu_pos.z) >> INT32_POS_FRAC;
+ height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC;
INT32_ATAN2(booz_cam_tilt, height, dist);
Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX -
BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/cam_track.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -24,11 +24,11 @@
#include "cam_track.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "ahrs.h"
#ifdef USE_HFF
-#include "ins/booz2_hf_float.h"
+#include "ins/hf_float.h"
#endif
struct FloatVect3 target_pos_ned;
@@ -57,8 +57,8 @@
uint8_t cam_data_len;
void track_init(void) {
- booz_ins_ltp_initialised = TRUE; // ltp is initialized and centered on the
target
- booz_ins_update_on_agl = TRUE; // use sonar to update agl (assume flat
ground)
+ ins_ltp_initialised = TRUE; // ltp is initialized and centered on the target
+ ins_update_on_agl = TRUE; // use sonar to update agl (assume flat ground)
cam_status = UNINIT;
cam_data_len = CAM_DATA_LEN;
@@ -99,7 +99,7 @@
cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10);
cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10);
cmd_msg[c++] = ' ';
- float alt = -POS_FLOAT_OF_BFP(booz_ins_ltp_pos.z);
+ float alt = -POS_FLOAT_OF_BFP(ins_ltp_pos.z);
//alt = 0.40;
if (alt > 0) cmd_msg[c++] = ' ';
else { cmd_msg[c++] = '-'; alt = -alt; }
@@ -120,31 +120,31 @@
}
void track_event(void) {
- if (!booz_ins_ltp_initialised) {
- booz_ins_ltp_initialised = TRUE;
- booz_ins_hf_realign = TRUE;
+ if (!ins_ltp_initialised) {
+ ins_ltp_initialised = TRUE;
+ ins_hf_realign = TRUE;
}
#ifdef USE_HFF
- if (booz_ins_hf_realign) {
- booz_ins_hf_realign = FALSE;
+ if (ins_hf_realign) {
+ ins_hf_realign = FALSE;
struct FloatVect2 pos, zero;
pos.x = -target_pos_ned.x;
pos.y = -target_pos_ned.y;
- booz_ins_realign_h(pos, zero);
+ ins_realign_h(pos, zero);
}
const stuct FlotVect2 measuremet_noise = { 10.0, 10.0);
b2_hff_update_pos(-target_pos_ned, measurement_noise);
- booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
- booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
- booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
- booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
- booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
- booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
+ 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);
#else
// store pos in ins
- booz_ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x));
- booz_ins_ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y));
+ ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x));
+ ins_ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y));
// compute speed from last pos
// TODO get delta T
// store last pos
Modified: paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
2010-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/modules/sonar/sonar_maxbotix_booz.h
2010-09-28 14:04:33 UTC (rev 6003)
@@ -39,7 +39,7 @@
extern void maxbotix_init(void);
extern void maxbotix_read(void);
-#include "booz2_ins.h" // needed because booz_ins is not a module
+#include "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-09-28 14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
2010-09-28 14:04:33 UTC (rev 6003)
@@ -23,7 +23,7 @@
#include "booz_fms.h"
-#include "booz2_ins.h"
+#include "ins.h"
#include "math/pprz_algebra_int.h"
#define FMS_TEST_SIGNAL_DEFAULT_MODE STEP_YAW
@@ -69,7 +69,7 @@
#if 0
case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)
- booz_fms_test_signal_start_z = booz_ins_ltp_pos.z;
+ booz_fms_test_signal_start_z = ins_ltp_pos.z;
else {
booz_fms_input.v_sp.height = (booz_fms_test_signal_counter <
booz_fms_test_signal_period) ?
booz_fms_test_signal_start_z :
Modified: paparazzi3/trunk/sw/simulator/nps/nps_ivy.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_ivy.c 2010-09-28 14:04:22 UTC (rev
6002)
+++ paparazzi3/trunk/sw/simulator/nps/nps_ivy.c 2010-09-28 14:04:33 UTC (rev
6003)
@@ -94,8 +94,8 @@
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
- lla.alt = atoi(argv[5]) - booz_ins_ltp_def.hmsl + booz_ins_ltp_def.lla.alt;
- enu_of_lla_point_i(&enu,&booz_ins_ltp_def,&lla);
+ lla.alt = atoi(argv[5]) - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
+ enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
Modified: paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c
===================================================================
--- paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-09-28
14:04:22 UTC (rev 6002)
+++ paparazzi3/trunk/sw/simulator/old_booz/booz2_sim_main.c 2010-09-28
14:04:33 UTC (rev 6003)
@@ -215,11 +215,11 @@
#ifdef BYPASS_INS
-#include "booz2_ins.h"
+#include "ins.h"
static void sim_overwrite_ins(void) {
- booz_ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]);
- booz_ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]);
- booz_ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(bfm.accel_ltp->ve[AXIS_Z]);
+ 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]);
+ ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(bfm.accel_ltp->ve[AXIS_Z]);
}
#endif /* BYPASS_INS */
@@ -358,9 +358,9 @@
int alt = atoi(argv[5]);
lla.lat = INT32_RAD_OF_DEG(lat);
lla.lon = INT32_RAD_OF_DEG(lon);
- lla.alt = alt+booz_ins_ltp_def.lla.alt;
- //printf("move rad %d %d %d
(%d)\n",lla.lat,lla.lon,lla.alt,booz_ins_ltp_def.lla.alt);
- enu_of_lla_point_i(&enu,&booz_ins_ltp_def,&lla);
+ lla.alt = alt+ins_ltp_def.lla.alt;
+ //printf("move rad %d %d %d
(%d)\n",lla.lat,lla.lon,lla.alt,ins_ltp_def.lla.alt);
+ enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6003] rename booz ins,
Felix Ruess <=