[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6005] rename booz2_guidance/BOOZ2_GUIDANCE and B2_G
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6005] rename booz2_guidance/BOOZ2_GUIDANCE and B2_GUIDANCE to guidance/ GUIDANCE |
|
Date: |
Tue, 28 Sep 2010 14:04:56 +0000 |
Revision: 6005
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6005
Author: flixr
Date: 2010-09-28 14:04:56 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
rename booz2_guidance/BOOZ2_GUIDANCE and B2_GUIDANCE to guidance/GUIDANCE
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/ENAC/quadrotor/blender.xml
paparazzi3/trunk/conf/airframes/ENAC/quadrotor/booz2_g1.xml
paparazzi3/trunk/conf/airframes/ENAC/quadrotor/mkk1.xml
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a2.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a3.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a4.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a5.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a6.xml
paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml
paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml
paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.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/UofAdelaide/lisa_a1000.xml
paparazzi3/trunk/conf/airframes/booz2_Aron.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/airframes/booz2_flixr.xml
paparazzi3/trunk/conf/airframes/booz2_ppzuav.xml
paparazzi3/trunk/conf/airframes/booz2_s1.xml
paparazzi3/trunk/conf/airframes/booz2_x1.xml
paparazzi3/trunk/conf/airframes/esden/lisa_asctec.xml
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
paparazzi3/trunk/sw/airborne/booz/booz_fms.c
paparazzi3/trunk/sw/airborne/booz/booz_fms.h
paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h
paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
paparazzi3/trunk/sw/airborne/booz/test/test_vg_adpt.c
paparazzi3/trunk/sw/airborne/booz/test/test_vg_ref.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
Modified: paparazzi3/trunk/conf/airframes/ENAC/quadrotor/blender.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/ENAC/quadrotor/blender.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/ENAC/quadrotor/blender.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -155,7 +155,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -175,7 +175,7 @@
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-140"/>
<define name="DGAIN" value="-90"/>
<define name="IGAIN" value="-30"/>
@@ -230,7 +230,7 @@
<makefile location="after">
-ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF
+ap.CFLAGS += -DGUIDANCE_H_USE_REF
ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
-DUSE_PWM1
ap.CFLAGS += -DBOOZ_ACTUATORS_MAX_THRUST=160 -DBOOZ_THRUST_LOWPASS=17
@@ -240,7 +240,7 @@
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\"
sim.CFLAGS += -DNPS_NO_SUPERVISION
-sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF
+sim.CFLAGS += -DGUIDANCE_H_USE_REF
sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
sim.srcs += $(SRC_BOOZ_SIM)/booz2_pwm_hw.c
Modified: paparazzi3/trunk/conf/airframes/ENAC/quadrotor/booz2_g1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/ENAC/quadrotor/booz2_g1.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/ENAC/quadrotor/booz2_g1.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -145,7 +145,7 @@
<define name="SONAR_SENS" value="2.146" integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -166,7 +166,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-15"/>
@@ -223,14 +223,14 @@
<makefile location="after">
-ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF
+ap.CFLAGS += -DGUIDANCE_H_USE_REF
ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
ap.srcs += $(SRC_BOOZ_ARCH)/booz2_pwm_hw.c
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\"
sim.CFLAGS += -DNPS_NO_SUPERVISION
-sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF
+sim.CFLAGS += -DGUIDANCE_H_USE_REF
sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
sim.srcs += $(SRC_BOOZ_SIM)/booz2_pwm_hw.c
Modified: paparazzi3/trunk/conf/airframes/ENAC/quadrotor/mkk1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/ENAC/quadrotor/mkk1.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/ENAC/quadrotor/mkk1.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -195,7 +195,7 @@
<define name="UNTILT_ACCEL" value="1"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -215,7 +215,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-60"/>
<define name="DGAIN" value="-120"/>
<define name="IGAIN" value="-10"/>
@@ -260,10 +260,10 @@
#set GPS lag for horizontal filter
#ap.CFLAGS += -DUSE_GPS_ACC4R
-#ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF
+#ap.CFLAGS += -DGUIDANCE_H_USE_REF
ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT
-#sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF
+#sim.CFLAGS += -DGUIDANCE_H_USE_REF
sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT
</makefile>
Modified:
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
===================================================================
---
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
2010-09-28 14:04:47 UTC (rev 6004)
+++
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
2010-09-28 14:04:56 UTC (rev 6005)
@@ -153,7 +153,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -170,7 +170,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified:
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
===================================================================
---
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
2010-09-28 14:04:47 UTC (rev 6004)
+++
paparazzi3/trunk/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
2010-09-28 14:04:56 UTC (rev 6005)
@@ -171,7 +171,7 @@
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -188,7 +188,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a1.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -145,7 +145,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -162,7 +162,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a1p.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -149,7 +149,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -166,7 +166,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a2.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a2.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a2.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -138,7 +138,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -154,7 +154,7 @@
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a3.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a3.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a3.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -125,7 +125,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -139,7 +139,7 @@
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a4.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a4.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a4.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -103,7 +103,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -117,7 +117,7 @@
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a5.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a5.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a5.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -141,7 +141,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -158,7 +158,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a6.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a6.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a6.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -142,7 +142,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -159,7 +159,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/Poine/booz2_a7.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -142,7 +142,7 @@
<define name="BARO_SENS" value="10." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -159,7 +159,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_BOOZ.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -219,7 +219,7 @@
<define name="BARO_SENS" value="14.9" integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -235,7 +235,7 @@
<!-- <define name="INV_M" value ="0.2"/> -->
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_LISA.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -129,7 +129,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -146,7 +146,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/A1000_NOVA.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -175,7 +175,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> -->
</section>
-<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
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:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
2010-09-28 14:04:56 UTC (rev 6005)
@@ -175,7 +175,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> -->
</section>
-<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/booz2_a1000.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -220,7 +220,7 @@
<define name="BARO_SENS" value="14.9" integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -237,7 +237,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/UofAdelaide/lisa_a1000.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/UofAdelaide/lisa_a1000.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/UofAdelaide/lisa_a1000.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -137,7 +137,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -154,7 +154,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_Aron.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_Aron.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_Aron.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -138,7 +138,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
@@ -154,7 +154,7 @@
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_NoVa.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_NoVa.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_NoVa.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -175,7 +175,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> -->
</section>
-<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_NoVa_001.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -175,7 +175,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> -->
</section>
-<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_NoVa_002.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -175,7 +175,7 @@
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> -->
</section>
-<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_flixr.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -164,7 +164,7 @@
<define name="BARO_SENS" value="15." integer="16" />
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)" />
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)" />
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)" />
@@ -180,7 +180,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/>
@@ -227,12 +227,12 @@
<!--<define name="RADIO_CONTROL_KILL_SWITCH"
value="RADIO_CONTROL_SWITCH1"/>-->
<define name="USE_ADAPT_HOVER"/>
<define name="USE_GPS_ACC4R"/>
- <define name="B2_GUIDANCE_H_USE_REF"/>
+ <define name="GUIDANCE_H_USE_REF"/>
<define name="USE_INS_NAV_INIT"/>
</target>
<target name="sim" board="pc">
<define name="USE_ADAPT_HOVER"/>
- <define name="B2_GUIDANCE_H_USE_REF"/>
+ <define name="GUIDANCE_H_USE_REF"/>
<define name="USE_INS_NAV_INIT"/>
<subsystem name="fdm" type="nps"/>
</target>
Modified: paparazzi3/trunk/conf/airframes/booz2_ppzuav.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_ppzuav.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_ppzuav.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -150,7 +150,7 @@
<define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -170,7 +170,7 @@
<!-- <define name="INV_M" value="0.118"/> -->
</section>
-<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_s1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_s1.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_s1.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -133,7 +133,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -150,7 +150,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/booz2_x1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_x1.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/airframes/booz2_x1.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -144,7 +144,7 @@
<define name="BARO_SENS" value="19." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -160,7 +160,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/airframes/esden/lisa_asctec.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/esden/lisa_asctec.xml 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/airframes/esden/lisa_asctec.xml 2010-09-28
14:04:56 UTC (rev 6005)
@@ -137,7 +137,7 @@
<define name="BARO_SENS" value="15." integer="16"/>
</section>
- <section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
+ <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -154,7 +154,7 @@
</section>
- <section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_">
+ <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/>
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -192,8 +192,8 @@
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
ap.CFLAGS += -DUSE_NAVIGATION
-ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c
-ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
+ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
+ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
ap.srcs += $(SRC_FIRMWARE)/ins.c
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
math/pprz_geodetic_double.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:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-28 14:04:56 UTC (rev 6005)
@@ -133,8 +133,8 @@
endif
sim.CFLAGS += -DUSE_NAVIGATION
-sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c
-sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
+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
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/conf/messages.xml 2010-09-28 14:04:56 UTC (rev 6005)
@@ -936,7 +936,7 @@
<field name="cmd_thrust" type="int32"/>
</message>
- <message name="BOOZ2_GUIDANCE" id="144">
+ <message name="GUIDANCE" id="144">
<field name="pos_n" type="int32"/>
<field name="pos_e" type="int32"/>
<field name="sp_pos_n" type="int32"/>
@@ -1009,7 +1009,7 @@
<field name="tbd" type="float"/>
</message>
- <message name="BOOZ2_GUIDANCE_H_REF_INT" id="149">
+ <message name="GUIDANCE_H_REF_INT" id="149">
<field name="sp_x" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
<field name="ref_x" type="int32" alt_unit="m"
alt_unit_coef="0.0039063"/>
<field name="ref_xd" type="int32" alt_unit="m/s"
alt_unit_coef="0.0000019"/>
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -44,22 +44,22 @@
</dl_settings>
<dl_settings NAME="Vert Loop">
- <dl_setting var="booz2_guidance_v_kp" min="-600" step="1" max="0"
module="guidance/booz2_guidance_v" shortname="kp"/>
- <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="guidance_v_kp" min="-600" step="1" max="0"
module="guidance/guidance_v" shortname="kp"/>
+ <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_settings>
<dl_settings NAME="Horiz Loop">
- <dl_setting var="booz2_guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1"
module="guidance/booz2_guidance_h" shortname="sp_x_ned" unit="1/2^8m"
alt_unit="m" alt_unit_coef="0.00390625"/>
- <dl_setting var="booz2_guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1"
module="guidance/booz2_guidance_h" shortname="sp_y_ned" unit="1/2^8m"
alt_unit="m" alt_unit_coef="0.00390625"/>
- <dl_setting var="booz2_guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5"
module="guidance/booz2_guidance_h" shortname="sp_psi" unit="1/2^20r"
alt_unit="deg" alt_unit_coef="0.000054641513360"/>
- <dl_setting var="booz2_guidance_h_pgain" min="-400" step="1" max="0"
module="guidance/booz2_guidance_h" shortname="kp"/>
- <dl_setting var="booz2_guidance_h_dgain" min="-400" step="1" max="0"
module="guidance/booz2_guidance_h" shortname="kd"/>
- <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="guidance_h_pos_sp.x" MIN="-10" MAX="10" STEP="1"
module="guidance/guidance_h" shortname="sp_x_ned" unit="1/2^8m" alt_unit="m"
alt_unit_coef="0.00390625"/>
+ <dl_setting var="guidance_h_pos_sp.y" MIN="-10" MAX="10" STEP="1"
module="guidance/guidance_h" shortname="sp_y_ned" unit="1/2^8m" alt_unit="m"
alt_unit_coef="0.00390625"/>
+ <dl_setting var="guidance_h_psi_sp" MIN="-180" MAX="180" STEP="5"
module="guidance/guidance_h" shortname="sp_psi" unit="1/2^20r" alt_unit="deg"
alt_unit_coef="0.000054641513360"/>
+ <dl_setting var="guidance_h_pgain" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="kp"/>
+ <dl_setting var="guidance_h_dgain" min="-400" step="1" max="0"
module="guidance/guidance_h" shortname="kd"/>
+ <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_settings>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-28
14:04:56 UTC (rev 6005)
@@ -118,7 +118,7 @@
int32_t dist_to_waypoint;
INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
-#ifndef B2_GUIDANCE_H_USE_REF
+#ifndef GUIDANCE_H_USE_REF
if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
VECT2_COPY(booz2_navigation_carrot, booz2_navigation_target);
}
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -35,7 +35,7 @@
#endif
#include "autopilot.h"
-#include "booz_guidance.h"
+#include "guidance.h"
#include "actuators.h"
@@ -67,8 +67,8 @@
&autopilot_mode, \
&autopilot_in_flight, \
&autopilot_motors_on, \
- &booz2_guidance_h_mode, \
- &booz2_guidance_v_mode, \
+ &guidance_h_mode, \
+ &guidance_v_mode, \
&booz2_battery_voltage, \
&cpu_time_sec \
); \
@@ -86,8 +86,8 @@
&autopilot_mode, \
&autopilot_in_flight, \
&autopilot_motors_on, \
- &booz2_guidance_h_mode, \
- &booz2_guidance_v_mode, \
+ &guidance_h_mode, \
+ &guidance_v_mode, \
&booz2_battery_voltage, \
&cpu_time_sec \
); \
@@ -525,12 +525,12 @@
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
#endif
-#define PERIODIC_SEND_BOOZ2_GUIDANCE(_chan) { \
- DOWNLINK_SEND_BOOZ2_GUIDANCE(_chan,
\
- &booz2_guidance_h_cur_pos.x, \
- &booz2_guidance_h_cur_pos.y, \
- &booz2_guidance_h_held_pos.x, \
- &booz2_guidance_h_held_pos.y); \
+#define PERIODIC_SEND_GUIDANCE(_chan) { \
+ DOWNLINK_SEND_GUIDANCE(_chan, \
+ &guidance_h_cur_pos.x, \
+ &guidance_h_cur_pos.y, \
+ &guidance_h_held_pos.x, \
+ &guidance_h_held_pos.y); \
}
#define PERIODIC_SEND_INS(_chan) { \
@@ -605,64 +605,64 @@
#define PERIODIC_SEND_BOOZ2_VERT_LOOP(_chan) { \
DOWNLINK_SEND_BOOZ2_VERT_LOOP(_chan, \
- &booz2_guidance_v_z_sp, \
- &booz2_guidance_v_zd_sp, \
+ &guidance_v_z_sp, \
+ &guidance_v_zd_sp, \
&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, \
+ &guidance_v_z_ref, \
+ &guidance_v_zd_ref, \
+ &guidance_v_zdd_ref, \
&b2_gv_adapt_X, \
&b2_gv_adapt_P, \
&b2_gv_adapt_Xmeas, \
- &booz2_guidance_v_z_sum_err, \
- &booz2_guidance_v_ff_cmd, \
- &booz2_guidance_v_fb_cmd, \
- &booz2_guidance_v_delta_t); \
+ &guidance_v_z_sum_err, \
+ &guidance_v_ff_cmd, \
+ &guidance_v_fb_cmd, \
+ &guidance_v_delta_t); \
}
#define PERIODIC_SEND_BOOZ2_HOVER_LOOP(_chan) {
\
DOWNLINK_SEND_BOOZ2_HOVER_LOOP(_chan, \
- &booz2_guidance_h_pos_sp.x, \
- &booz2_guidance_h_pos_sp.y, \
+ &guidance_h_pos_sp.x, \
+ &guidance_h_pos_sp.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, \
- &booz2_guidance_h_speed_err.y, \
- &booz2_guidance_h_pos_err_sum.x, \
- &booz2_guidance_h_pos_err_sum.y, \
- &booz2_guidance_h_nav_err.x, \
- &booz2_guidance_h_nav_err.y, \
- &booz2_guidance_h_command_earth.x, \
- &booz2_guidance_h_command_earth.y, \
- &booz2_guidance_h_command_body.phi, \
- &booz2_guidance_h_command_body.theta, \
- &booz2_guidance_h_command_body.psi); \
+ &guidance_h_pos_err.x, \
+ &guidance_h_pos_err.y, \
+ &guidance_h_speed_err.x, \
+ &guidance_h_speed_err.y, \
+ &guidance_h_pos_err_sum.x, \
+ &guidance_h_pos_err_sum.y, \
+ &guidance_h_nav_err.x, \
+ &guidance_h_nav_err.y, \
+ &guidance_h_command_earth.x, \
+ &guidance_h_command_earth.y, \
+ &guidance_h_command_body.phi, \
+ &guidance_h_command_body.theta, \
+ &guidance_h_command_body.psi); \
}
-#define PERIODIC_SEND_BOOZ2_GUIDANCE_H_REF(_chan) { \
- DOWNLINK_SEND_BOOZ2_GUIDANCE_H_REF_INT(_chan, \
- &booz2_guidance_h_pos_sp.x, \
- &booz2_guidance_h_pos_ref.x, \
- &booz2_guidance_h_speed_ref.x, \
- &booz2_guidance_h_accel_ref.x, \
- &booz2_guidance_h_pos_sp.y, \
- &booz2_guidance_h_pos_ref.y, \
- &booz2_guidance_h_speed_ref.y, \
- &booz2_guidance_h_accel_ref.y); \
+#define PERIODIC_SEND_GUIDANCE_H_REF(_chan) { \
+ DOWNLINK_SEND_GUIDANCE_H_REF_INT(_chan, \
+ &guidance_h_pos_sp.x, \
+ &guidance_h_pos_ref.x, \
+ &guidance_h_speed_ref.x, \
+ &guidance_h_accel_ref.x, \
+ &guidance_h_pos_sp.y, \
+ &guidance_h_pos_ref.y, \
+ &guidance_h_speed_ref.y, \
+ &guidance_h_accel_ref.y); \
}
#include "booz_gps.h"
#include "booz2_navigation.h"
#define PERIODIC_SEND_BOOZ2_FP(_chan) {
\
- int32_t carrot_up = -booz2_guidance_v_z_sp;
\
+ int32_t carrot_up = -guidance_v_z_sp; \
DOWNLINK_SEND_BOOZ2_FP( _chan, \
&ins_enu_pos.x, \
&ins_enu_pos.y, \
@@ -673,10 +673,10 @@
&ahrs.ltp_to_body_euler.phi, \
&ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi, \
- &booz2_guidance_h_pos_sp.y, \
- &booz2_guidance_h_pos_sp.x, \
+ &guidance_h_pos_sp.y, \
+ &guidance_h_pos_sp.x, \
&carrot_up, \
- &booz2_guidance_h_command_body.psi, \
+ &guidance_h_command_body.psi, \
&booz_stabilization_cmd[COMMAND_THRUST], \
&autopilot_flight_time); \
}
Modified: paparazzi3/trunk/sw/airborne/booz/booz_fms.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_fms.c 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/booz_fms.c 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -41,9 +41,9 @@
fms.timeouted = TRUE;
fms.last_msg = BOOZ_FMS_TIMEOUT;
- fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+ fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(fms.input.h_sp.attitude);
- fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB;
+ fms.input.v_mode = GUIDANCE_V_MODE_CLIMB;
fms.input.v_sp.climb = 0;
booz_fms_impl_init();
@@ -55,9 +55,9 @@
fms.last_msg++;
else {
fms.timeouted = TRUE;
- fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+ fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(fms.input.h_sp.attitude);
- fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB;
+ fms.input.v_mode = GUIDANCE_V_MODE_CLIMB;
fms.input.v_sp.climb = 0;
}
#endif
Modified: paparazzi3/trunk/sw/airborne/booz/booz_fms.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_fms.h 2010-09-28 14:04:47 UTC
(rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/booz_fms.h 2010-09-28 14:04:56 UTC
(rev 6005)
@@ -27,7 +27,7 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "autopilot.h"
-#include "booz_guidance.h"
+#include "guidance.h"
struct Booz_fms_imu_info {
struct Int16Vect3 gyro;
Modified: paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_datalink.h 2010-09-28
14:04:56 UTC (rev 6005)
@@ -37,10 +37,10 @@
fms.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \
fms.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \
switch (fms.input.h_mode) { \
- case BOOZ2_GUIDANCE_H_MODE_KILL: \
- case BOOZ2_GUIDANCE_H_MODE_RATE : \
+ case GUIDANCE_H_MODE_KILL: \
+ case GUIDANCE_H_MODE_RATE : \
break; \
- case BOOZ2_GUIDANCE_H_MODE_ATTITUDE : \
+ case GUIDANCE_H_MODE_ATTITUDE : \
{
\
fms.input.h_sp.attitude.phi =
DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
fms.input.h_sp.attitude.theta =
DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
@@ -49,13 +49,13 @@
BOOZ_FMS_LIMIT_ATTITUDE(fms.input.h_sp.attitude); \
}
\
break; \
- case BOOZ2_GUIDANCE_H_MODE_HOVER : \
+ case GUIDANCE_H_MODE_HOVER : \
{
\
fms.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);
\
fms.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer);
\
}
\
break; \
- case BOOZ2_GUIDANCE_H_MODE_NAV : \
+ case GUIDANCE_H_MODE_NAV : \
{ \
fms.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
fms.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
@@ -64,17 +64,17 @@
break; \
} \
switch (fms.input.v_mode) { \
- case BOOZ2_GUIDANCE_V_MODE_KILL: \
- case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: \
- case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: \
+ case GUIDANCE_V_MODE_KILL: \
+ case GUIDANCE_V_MODE_RC_DIRECT: \
+ case GUIDANCE_V_MODE_RC_CLIMB: \
break; \
- case BOOZ2_GUIDANCE_V_MODE_CLIMB : \
+ case GUIDANCE_V_MODE_CLIMB : \
fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \
- case BOOZ2_GUIDANCE_V_MODE_HOVER : \
+ case GUIDANCE_V_MODE_HOVER : \
fms.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \
- case BOOZ2_GUIDANCE_V_MODE_NAV : \
+ case GUIDANCE_V_MODE_NAV : \
fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \
} \
@@ -82,8 +82,8 @@
#define BOOZ_FMS_NAV_STICK_PARSE_DL(_dl_buffer) { \
fms.last_msg = 0; \
- fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_NAV; \
- fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_NAV; \
+ fms.input.h_mode = GUIDANCE_H_MODE_NAV; \
+ fms.input.v_mode = GUIDANCE_V_MODE_NAV; \
fms.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \
fms.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \
fms.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \
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:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
2010-09-28 14:04:56 UTC (rev 6005)
@@ -37,8 +37,8 @@
fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD;
fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE;
fms_test_signal.counter = 0;
- fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
- fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER;
+ fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
+ fms.input.v_mode = GUIDANCE_V_MODE_HOVER;
}
void booz_fms_impl_periodic(void) {
@@ -68,7 +68,7 @@
break;
#if 0
case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
- if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)
+ if (guidance_v_mode < GUIDANCE_V_MODE_HOVER)
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) ?
Modified: paparazzi3/trunk/sw/airborne/booz/test/test_vg_adpt.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/test_vg_adpt.c 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/test/test_vg_adpt.c 2010-09-28
14:04:56 UTC (rev 6005)
@@ -27,8 +27,8 @@
#include "std.h"
#include "booz_geometry_mixed.h"
-#define B2_GUIDANCE_V_C
-#include "booz2_guidance_v_adpt.h"
+#define GUIDANCE_V_C
+#include "guidance_v_adpt.h"
Modified: paparazzi3/trunk/sw/airborne/booz/test/test_vg_ref.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/test_vg_ref.c 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/booz/test/test_vg_ref.c 2010-09-28
14:04:56 UTC (rev 6005)
@@ -24,8 +24,8 @@
#include <stdio.h>
#include "booz_geometry_mixed.h"
-#define B2_GUIDANCE_V_C
-#include "booz2_guidance_v_ref.h"
+#define GUIDANCE_V_C
+#include "guidance_v_ref.h"
#define NB_STEP 10000
#define DT (1./B2_GV_FREQ)
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
2010-09-28 14:04:56 UTC (rev 6005)
@@ -27,7 +27,7 @@
#include "booz_radio_control.h"
#include "booz2_commands.h"
#include "booz2_navigation.h"
-#include "booz_guidance.h"
+#include "guidance.h"
#include "booz_stabilization.h"
#include "led.h"
@@ -87,8 +87,8 @@
autopilot_in_flight, autopilot_motors_on);
}
else {
- booz2_guidance_v_run( autopilot_in_flight );
- booz2_guidance_h_run( autopilot_in_flight );
+ guidance_v_run( autopilot_in_flight );
+ guidance_h_run( autopilot_in_flight );
SetCommands(booz_stabilization_cmd,
autopilot_in_flight, autopilot_motors_on);
}
@@ -105,29 +105,29 @@
#ifndef KILL_AS_FAILSAFE
booz_stab_att_sp_euler.phi = 0;
booz_stab_att_sp_euler.theta = 0;
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
+ guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
#endif
case AP_MODE_KILL:
autopilot_motors_on = FALSE;
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL);
+ guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_Z_HOLD:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_RATE);
+ guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
break;
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE);
+ guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
break;
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_HOVER);
+ guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
break;
case AP_MODE_NAV:
- booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV);
+ guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
default:
break;
@@ -136,33 +136,33 @@
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
- booz2_guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB);
+ guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
#endif
case AP_MODE_KILL:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_DIRECT);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_RC_CLIMB:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_CLIMB);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB);
break;
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_CLIMB:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break;
case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_Z_HOLD:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_HOVER);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER);
break;
case AP_MODE_NAV:
- booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_NAV);
+ guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
default:
break;
@@ -252,8 +252,8 @@
kill_throttle = !autopilot_motors_on;
if (autopilot_mode > AP_MODE_FAILSAFE) {
- booz2_guidance_v_read_rc();
- booz2_guidance_h_read_rc(autopilot_in_flight);
+ guidance_v_read_rc();
+ guidance_h_read_rc(autopilot_in_flight);
}
}
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-09-28 14:04:56 UTC (rev 6005)
@@ -21,9 +21,9 @@
* Boston, MA 02111-1307, USA.
*/
-#define B2_GUIDANCE_H_C
-//#define B2_GUIDANCE_H_USE_REF
-#include "booz2_guidance_h.h"
+#define GUIDANCE_H_C
+//#define GUIDANCE_H_USE_REF
+#include "guidance_h.h"
#include "ahrs.h"
#include "booz_stabilization.h"
@@ -33,74 +33,74 @@
#include "airframe.h"
-uint8_t booz2_guidance_h_mode;
+uint8_t guidance_h_mode;
-struct Int32Vect2 booz2_guidance_h_pos_sp;
-int32_t booz2_guidance_h_psi_sp;
-struct Int32Vect2 booz2_guidance_h_pos_ref;
-struct Int32Vect2 booz2_guidance_h_speed_ref;
-struct Int32Vect2 booz2_guidance_h_accel_ref;
+struct Int32Vect2 guidance_h_pos_sp;
+int32_t guidance_h_psi_sp;
+struct Int32Vect2 guidance_h_pos_ref;
+struct Int32Vect2 guidance_h_speed_ref;
+struct Int32Vect2 guidance_h_accel_ref;
-struct Int32Vect2 booz2_guidance_h_pos_err;
-struct Int32Vect2 booz2_guidance_h_speed_err;
-struct Int32Vect2 booz2_guidance_h_pos_err_sum;
-struct Int32Vect2 booz2_guidance_h_nav_err;
+struct Int32Vect2 guidance_h_pos_err;
+struct Int32Vect2 guidance_h_speed_err;
+struct Int32Vect2 guidance_h_pos_err_sum;
+struct Int32Vect2 guidance_h_nav_err;
-struct Int32Eulers booz2_guidance_h_rc_sp;
-struct Int32Vect2 booz2_guidance_h_command_earth;
-struct Int32Vect2 booz2_guidance_h_stick_earth_sp;
-struct Int32Eulers booz2_guidance_h_command_body;
+struct Int32Eulers guidance_h_rc_sp;
+struct Int32Vect2 guidance_h_command_earth;
+struct Int32Vect2 guidance_h_stick_earth_sp;
+struct Int32Eulers guidance_h_command_body;
-int32_t booz2_guidance_h_pgain;
-int32_t booz2_guidance_h_dgain;
-int32_t booz2_guidance_h_igain;
-int32_t booz2_guidance_h_ngain;
-int32_t booz2_guidance_h_again;
+int32_t guidance_h_pgain;
+int32_t guidance_h_dgain;
+int32_t guidance_h_igain;
+int32_t guidance_h_ngain;
+int32_t guidance_h_again;
-#ifndef BOOZ2_GUIDANCE_H_NGAIN
-#define BOOZ2_GUIDANCE_H_NGAIN 0
+#ifndef GUIDANCE_H_NGAIN
+#define GUIDANCE_H_NGAIN 0
#endif
-#ifndef BOOZ2_GUIDANCE_H_AGAIN
-#define BOOZ2_GUIDANCE_H_AGAIN 0
+#ifndef GUIDANCE_H_AGAIN
+#define GUIDANCE_H_AGAIN 0
#endif
-static inline void booz2_guidance_h_hover_run(void);
-static inline void booz2_guidance_h_nav_run(bool_t in_flight);
-static inline void booz2_guidance_h_hover_enter(void);
-static inline void booz2_guidance_h_nav_enter(void);
+static inline void guidance_h_hover_run(void);
+static inline void guidance_h_nav_run(bool_t in_flight);
+static inline void guidance_h_hover_enter(void);
+static inline void guidance_h_nav_enter(void);
#define Booz2GuidanceHSetRef(_pos, _speed, _accel) { \
b2_gh_set_ref(_pos, _speed, _accel); \
- VECT2_COPY(booz2_guidance_h_pos_ref, _pos); \
- VECT2_COPY(booz2_guidance_h_speed_ref, _speed); \
- VECT2_COPY(booz2_guidance_h_accel_ref, _accel); \
+ VECT2_COPY(guidance_h_pos_ref, _pos); \
+ VECT2_COPY(guidance_h_speed_ref, _speed); \
+ VECT2_COPY(guidance_h_accel_ref, _accel); \
}
-void booz2_guidance_h_init(void) {
+void guidance_h_init(void) {
- booz2_guidance_h_mode = BOOZ2_GUIDANCE_H_MODE_KILL;
- booz2_guidance_h_psi_sp = 0;
- INT_VECT2_ZERO(booz2_guidance_h_pos_sp);
- INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
- INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
- INT_EULERS_ZERO(booz2_guidance_h_command_body);
- booz2_guidance_h_pgain = BOOZ2_GUIDANCE_H_PGAIN;
- booz2_guidance_h_igain = BOOZ2_GUIDANCE_H_IGAIN;
- booz2_guidance_h_dgain = BOOZ2_GUIDANCE_H_DGAIN;
- booz2_guidance_h_ngain = BOOZ2_GUIDANCE_H_NGAIN;
- booz2_guidance_h_again = BOOZ2_GUIDANCE_H_AGAIN;
+ guidance_h_mode = GUIDANCE_H_MODE_KILL;
+ guidance_h_psi_sp = 0;
+ INT_VECT2_ZERO(guidance_h_pos_sp);
+ INT_VECT2_ZERO(guidance_h_pos_err_sum);
+ INT_EULERS_ZERO(guidance_h_rc_sp);
+ INT_EULERS_ZERO(guidance_h_command_body);
+ guidance_h_pgain = GUIDANCE_H_PGAIN;
+ guidance_h_igain = GUIDANCE_H_IGAIN;
+ guidance_h_dgain = GUIDANCE_H_DGAIN;
+ guidance_h_ngain = GUIDANCE_H_NGAIN;
+ guidance_h_again = GUIDANCE_H_AGAIN;
}
-void booz2_guidance_h_mode_changed(uint8_t new_mode) {
- if (new_mode == booz2_guidance_h_mode)
+void guidance_h_mode_changed(uint8_t new_mode) {
+ if (new_mode == guidance_h_mode)
return;
- switch ( booz2_guidance_h_mode ) {
- // case BOOZ2_GUIDANCE_H_MODE_RATE:
+ switch ( guidance_h_mode ) {
+ // case GUIDANCE_H_MODE_RATE:
// booz_stabilization_rate_exit();
// break;
default:
@@ -109,53 +109,53 @@
switch (new_mode) {
- case BOOZ2_GUIDANCE_H_MODE_RATE:
+ case GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_enter();
break;
- case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+ case GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_enter();
break;
- case BOOZ2_GUIDANCE_H_MODE_HOVER:
- booz2_guidance_h_hover_enter();
+ case GUIDANCE_H_MODE_HOVER:
+ guidance_h_hover_enter();
break;
- case BOOZ2_GUIDANCE_H_MODE_NAV:
- booz2_guidance_h_nav_enter();
+ case GUIDANCE_H_MODE_NAV:
+ guidance_h_nav_enter();
break;
default:
break;
}
- booz2_guidance_h_mode = new_mode;
+ guidance_h_mode = new_mode;
}
-void booz2_guidance_h_read_rc(bool_t in_flight) {
+void guidance_h_read_rc(bool_t in_flight) {
- switch ( booz2_guidance_h_mode ) {
+ switch ( guidance_h_mode ) {
- case BOOZ2_GUIDANCE_H_MODE_RATE:
+ case GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_read_rc();
break;
- case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+ case GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_read_rc(in_flight);
break;
- case BOOZ2_GUIDANCE_H_MODE_HOVER:
- BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
+ case GUIDANCE_H_MODE_HOVER:
+ BOOZ_STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
break;
- case BOOZ2_GUIDANCE_H_MODE_NAV:
+ case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RADIO_CONTROL_OK) {
- BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight);
- booz2_guidance_h_rc_sp.psi = 0;
+ BOOZ_STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
+ guidance_h_rc_sp.psi = 0;
}
else {
- INT_EULERS_ZERO(booz2_guidance_h_rc_sp);
+ INT_EULERS_ZERO(guidance_h_rc_sp);
}
break;
default:
@@ -165,25 +165,25 @@
}
-void booz2_guidance_h_run(bool_t in_flight) {
- switch ( booz2_guidance_h_mode ) {
+void guidance_h_run(bool_t in_flight) {
+ switch ( guidance_h_mode ) {
- case BOOZ2_GUIDANCE_H_MODE_RATE:
+ case GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_run(in_flight);
break;
- case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
+ case GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_run(in_flight);
break;
- case BOOZ2_GUIDANCE_H_MODE_HOVER:
- booz2_guidance_h_hover_run();
+ case GUIDANCE_H_MODE_HOVER:
+ guidance_h_hover_run();
booz_stabilization_attitude_run(in_flight);
break;
- case BOOZ2_GUIDANCE_H_MODE_NAV:
+ case GUIDANCE_H_MODE_NAV:
{
- if (!in_flight) booz2_guidance_h_nav_enter();
+ if (!in_flight) guidance_h_nav_enter();
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
@@ -192,15 +192,15 @@
#endif
}
else {
- INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp,
booz2_navigation_carrot);
-#ifdef B2_GUIDANCE_H_USE_REF
- b2_gh_update_ref_from_pos_sp(booz2_guidance_h_pos_sp);
+ INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, booz2_navigation_carrot);
+#ifdef GUIDANCE_H_USE_REF
+ b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp);
#endif
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
- booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC));
+ guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC));
#endif
- //booz2_guidance_h_hover_run();
- booz2_guidance_h_nav_run(in_flight);
+ //guidance_h_hover_run();
+ guidance_h_nav_run(in_flight);
}
booz_stabilization_attitude_run(in_flight);
break;
@@ -221,33 +221,33 @@
//#define MAX_BANK (65536)
#define MAX_BANK (98000)
-static inline void booz2_guidance_h_hover_run(void) {
+static inline void guidance_h_hover_run(void) {
/* compute position error */
- VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_sp);
+ VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_sp);
/* saturate it */
- VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+ VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
- VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed);
+ VECT2_COPY(guidance_h_speed_err, ins_ltp_speed);
/* saturate it */
- VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+ VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
/* update pos error integral */
- VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err);
+ VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err);
/* saturate it */
- VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM);
+ VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM);
/* run PID */
// cmd_earth < 15.17
- booz2_guidance_h_command_earth.x = (booz2_guidance_h_pgain<<1) *
booz2_guidance_h_pos_err.x +
- booz2_guidance_h_dgain *
(booz2_guidance_h_speed_err.x>>9) +
- booz2_guidance_h_igain *
(booz2_guidance_h_pos_err_sum.x >> 12);
- booz2_guidance_h_command_earth.y = (booz2_guidance_h_pgain<<1) *
booz2_guidance_h_pos_err.y +
- booz2_guidance_h_dgain *(
booz2_guidance_h_speed_err.y>>9) +
- booz2_guidance_h_igain *
(booz2_guidance_h_pos_err_sum.y >> 12);
+ guidance_h_command_earth.x = (guidance_h_pgain<<1) * guidance_h_pos_err.x +
+ guidance_h_dgain *
(guidance_h_speed_err.x>>9) +
+ guidance_h_igain *
(guidance_h_pos_err_sum.x >> 12);
+ guidance_h_command_earth.y = (guidance_h_pgain<<1) * guidance_h_pos_err.y +
+ guidance_h_dgain *(
guidance_h_speed_err.y>>9) +
+ guidance_h_igain *
(guidance_h_pos_err_sum.y >> 12);
- VECT2_STRIM(booz2_guidance_h_command_earth, -MAX_BANK, MAX_BANK);
+ VECT2_STRIM(guidance_h_command_earth, -MAX_BANK, MAX_BANK);
/* Rotate to body frame */
int32_t s_psi, c_psi;
@@ -256,22 +256,22 @@
// INT32_TRIG_FRAC - 2: 100mm erreur, gain 100 -> 10000 command | 2 degres =
36000, so multiply by 4
- booz2_guidance_h_command_body.phi =
- ( - s_psi * booz2_guidance_h_command_earth.x + c_psi *
booz2_guidance_h_command_earth.y)
+ guidance_h_command_body.phi =
+ ( - s_psi * guidance_h_command_earth.x + c_psi *
guidance_h_command_earth.y)
>> (INT32_TRIG_FRAC - 2);
- booz2_guidance_h_command_body.theta =
- - ( c_psi * booz2_guidance_h_command_earth.x + s_psi *
booz2_guidance_h_command_earth.y)
+ guidance_h_command_body.theta =
+ - ( c_psi * guidance_h_command_earth.x + s_psi *
guidance_h_command_earth.y)
>> (INT32_TRIG_FRAC - 2);
- booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
- booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
- booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp +
booz2_guidance_h_rc_sp.psi;
+ guidance_h_command_body.phi += guidance_h_rc_sp.phi;
+ guidance_h_command_body.theta += guidance_h_rc_sp.theta;
+ guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi;
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
- ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
+ ANGLE_REF_NORMALIZE(guidance_h_command_body.psi);
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
- EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
+ EULERS_COPY(booz_stab_att_sp_euler, guidance_h_command_body);
}
@@ -279,72 +279,72 @@
#define NAV_MAX_BANK BFP_OF_REAL(0.35,REF_ANGLE_FRAC)
#define HOLD_DISTANCE POS_BFP_OF_REAL(10.)
-static inline void booz2_guidance_h_nav_run(bool_t in_flight) {
+static inline void guidance_h_nav_run(bool_t in_flight) {
/* convert our reference to generic representation */
-#ifdef B2_GUIDANCE_H_USE_REF
- INT32_VECT2_RSHIFT(booz2_guidance_h_pos_ref, b2_gh_pos_ref,
(B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
- INT32_VECT2_LSHIFT(booz2_guidance_h_speed_ref, b2_gh_speed_ref,
(INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
- INT32_VECT2_LSHIFT(booz2_guidance_h_accel_ref, b2_gh_accel_ref,
(INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
+#ifdef GUIDANCE_H_USE_REF
+ INT32_VECT2_RSHIFT(guidance_h_pos_ref, b2_gh_pos_ref,
(B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
+ INT32_VECT2_LSHIFT(guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC
- B2_GH_SPEED_REF_FRAC));
+ INT32_VECT2_LSHIFT(guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC
- B2_GH_ACCEL_REF_FRAC));
#else
- VECT2_COPY(booz2_guidance_h_pos_ref, booz2_guidance_h_pos_sp);
- INT_VECT2_ZERO(booz2_guidance_h_speed_ref);
- INT_VECT2_ZERO(booz2_guidance_h_accel_ref);
+ VECT2_COPY(guidance_h_pos_ref, guidance_h_pos_sp);
+ INT_VECT2_ZERO(guidance_h_speed_ref);
+ INT_VECT2_ZERO(guidance_h_accel_ref);
#endif
/* compute position error */
- VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_ref);
+ VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_ref);
/* saturate it */
- VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+ VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
/* compute speed error */
- //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);
+ //VECT2_COPY(guidance_h_speed_err, ins_ltp_speed);
+ VECT2_DIFF(guidance_h_speed_err, ins_ltp_speed, guidance_h_speed_ref);
/* saturate it */
- VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+ VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
int32_t dist;
- INT32_VECT2_NORM(dist, booz2_guidance_h_pos_err);
+ INT32_VECT2_NORM(dist, guidance_h_pos_err);
if ( dist < HOLD_DISTANCE ) { // Hold position
/* update pos error integral */
- VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err);
+ VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err);
/* saturate it */
- VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM,
MAX_POS_ERR_SUM);
+ VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM);
}
else { // Tracking algorithm, no integral
int32_t vect_prod = 0;
- int32_t scal_prod = ins_ltp_speed.x * booz2_guidance_h_pos_err.x +
ins_ltp_speed.y * booz2_guidance_h_pos_err.y;
+ int32_t scal_prod = ins_ltp_speed.x * guidance_h_pos_err.x +
ins_ltp_speed.y * guidance_h_pos_err.y;
// compute vectorial product only if angle < pi/2 (scalar product > 0)
if (scal_prod >= 0) {
- 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));
+ vect_prod = ((ins_ltp_speed.x * guidance_h_pos_err.y) >> (INT32_POS_FRAC
+ INT32_SPEED_FRAC - 10))
+ - ((ins_ltp_speed.y * 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,
+ VECT2_ASSIGN(guidance_h_nav_err,
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);
+ VECT2_SDIV(guidance_h_nav_err, guidance_h_nav_err, dist*dist);
// *2 ??
}
- if (!in_flight) { INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); }
+ if (!in_flight) { INT_VECT2_ZERO(guidance_h_pos_err_sum); }
/* run PID */
- booz2_guidance_h_command_earth.x =
- booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.x << (10 -
INT32_POS_FRAC)) +
- booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >>
(INT32_SPEED_FRAC - 10)) +
- booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 +
INT32_POS_FRAC - 10)) +
- booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x +
- booz2_guidance_h_again * booz2_guidance_h_accel_ref.x; /* feedforward gain
*/
- booz2_guidance_h_command_earth.y =
- booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 -
INT32_POS_FRAC)) +
- booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >>
(INT32_SPEED_FRAC - 10)) +
- booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 +
INT32_POS_FRAC - 10)) +
- booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y +
- booz2_guidance_h_again * booz2_guidance_h_accel_ref.y; /* feedforward gain
*/
+ guidance_h_command_earth.x =
+ guidance_h_pgain * (guidance_h_pos_err.x << (10 - INT32_POS_FRAC)) +
+ guidance_h_dgain * (guidance_h_speed_err.x >> (INT32_SPEED_FRAC - 10)) +
+ guidance_h_igain * (guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC -
10)) +
+ guidance_h_ngain * guidance_h_nav_err.x +
+ guidance_h_again * guidance_h_accel_ref.x; /* feedforward gain */
+ guidance_h_command_earth.y =
+ guidance_h_pgain * (guidance_h_pos_err.y << (10 - INT32_POS_FRAC)) +
+ guidance_h_dgain * (guidance_h_speed_err.y >> (INT32_SPEED_FRAC - 10)) +
+ guidance_h_igain * (guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC -
10)) +
+ guidance_h_ngain * guidance_h_nav_err.y +
+ guidance_h_again * guidance_h_accel_ref.y; /* feedforward gain */
- VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK);
- INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth,
booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for
trigo operation
+ VECT2_STRIM(guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK);
+ INT32_VECT2_RSHIFT(guidance_h_command_earth, guidance_h_command_earth,
REF_ANGLE_FRAC - 16); // Reduice to 16 for trigo operation
/* Rotate to body frame */
int32_t s_psi, c_psi;
@@ -352,35 +352,35 @@
PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi);
// Restore angle ref resolution after rotation
- booz2_guidance_h_command_body.phi =
- ( - s_psi * booz2_guidance_h_command_earth.x + c_psi *
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
- booz2_guidance_h_command_body.theta =
- - ( c_psi * booz2_guidance_h_command_earth.x + s_psi *
booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
+ guidance_h_command_body.phi =
+ ( - s_psi * guidance_h_command_earth.x + c_psi *
guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
+ guidance_h_command_body.theta =
+ - ( c_psi * guidance_h_command_earth.x + s_psi *
guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16));
// Add RC setpoint
- booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi;
- booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta;
- booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp +
booz2_guidance_h_rc_sp.psi;
- ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi);
+ guidance_h_command_body.phi += guidance_h_rc_sp.phi;
+ guidance_h_command_body.theta += guidance_h_rc_sp.theta;
+ guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi;
+ ANGLE_REF_NORMALIZE(guidance_h_command_body.psi);
// Set attitude setpoint
- EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body);
+ EULERS_COPY(booz_stab_att_sp_euler, guidance_h_command_body);
}
-static inline void booz2_guidance_h_hover_enter(void) {
+static inline void guidance_h_hover_enter(void) {
- VECT2_COPY(booz2_guidance_h_pos_sp, ins_ltp_pos);
+ VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos);
- BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz2_guidance_h_rc_sp );
+ BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( guidance_h_rc_sp );
- INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
+ INT_VECT2_ZERO(guidance_h_pos_err_sum);
}
-static inline void booz2_guidance_h_nav_enter(void) {
+static inline void guidance_h_nav_enter(void) {
- INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot);
+ INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, booz2_navigation_carrot);
struct Int32Vect2 pos,speed,zero;
INT_VECT2_ZERO(zero);
VECT2_COPY(pos, ins_ltp_pos);
@@ -389,12 +389,12 @@
struct Int32Eulers tmp_sp;
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );
- booz2_guidance_h_psi_sp = tmp_sp.psi;
+ guidance_h_psi_sp = tmp_sp.psi;
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
- nav_heading = (booz2_guidance_h_psi_sp >> (REF_ANGLE_FRAC -
INT32_ANGLE_FRAC));
+ nav_heading = (guidance_h_psi_sp >> (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
- booz2_guidance_h_rc_sp.psi = 0;
+ guidance_h_rc_sp.psi = 0;
- INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum);
+ INT_VECT2_ZERO(guidance_h_pos_err_sum);
}
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -21,56 +21,56 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_GUIDANCE_H_H
-#define BOOZ2_GUIDANCE_H_H
+#ifndef GUIDANCE_H_H
+#define GUIDANCE_H_H
#include "math/pprz_algebra_int.h"
-#include "booz2_guidance_h_ref.h"
+#include "guidance_h_ref.h"
-#define BOOZ2_GUIDANCE_H_MODE_KILL 0
-#define BOOZ2_GUIDANCE_H_MODE_RATE 1
-#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2
-#define BOOZ2_GUIDANCE_H_MODE_HOVER 3
-#define BOOZ2_GUIDANCE_H_MODE_NAV 4
+#define GUIDANCE_H_MODE_KILL 0
+#define GUIDANCE_H_MODE_RATE 1
+#define GUIDANCE_H_MODE_ATTITUDE 2
+#define GUIDANCE_H_MODE_HOVER 3
+#define GUIDANCE_H_MODE_NAV 4
-extern uint8_t booz2_guidance_h_mode;
+extern uint8_t guidance_h_mode;
/* horizontal setpoint in NED */
/* Q_int32_xx_8 */
-extern struct Int32Vect2 booz2_guidance_h_pos_sp;
-extern int32_t booz2_guidance_h_psi_sp;
-extern struct Int32Vect2 booz2_guidance_h_pos_ref;
-extern struct Int32Vect2 booz2_guidance_h_speed_ref;
-extern struct Int32Vect2 booz2_guidance_h_accel_ref;
+extern struct Int32Vect2 guidance_h_pos_sp;
+extern int32_t guidance_h_psi_sp;
+extern struct Int32Vect2 guidance_h_pos_ref;
+extern struct Int32Vect2 guidance_h_speed_ref;
+extern struct Int32Vect2 guidance_h_accel_ref;
-extern struct Int32Vect2 booz2_guidance_h_pos_err;
-extern struct Int32Vect2 booz2_guidance_h_speed_err;
-extern struct Int32Vect2 booz2_guidance_h_pos_err_sum;
-extern struct Int32Vect2 booz2_guidance_h_nav_err;
+extern struct Int32Vect2 guidance_h_pos_err;
+extern struct Int32Vect2 guidance_h_speed_err;
+extern struct Int32Vect2 guidance_h_pos_err_sum;
+extern struct Int32Vect2 guidance_h_nav_err;
-extern struct Int32Eulers booz2_guidance_h_rc_sp;
-extern struct Int32Vect2 booz2_guidance_h_command_earth;
-extern struct Int32Eulers booz2_guidance_h_command_body;
+extern struct Int32Eulers guidance_h_rc_sp;
+extern struct Int32Vect2 guidance_h_command_earth;
+extern struct Int32Eulers guidance_h_command_body;
-extern int32_t booz2_guidance_h_pgain;
-extern int32_t booz2_guidance_h_dgain;
-extern int32_t booz2_guidance_h_igain;
-extern int32_t booz2_guidance_h_ngain;
-extern int32_t booz2_guidance_h_again;
+extern int32_t guidance_h_pgain;
+extern int32_t guidance_h_dgain;
+extern int32_t guidance_h_igain;
+extern int32_t guidance_h_ngain;
+extern int32_t guidance_h_again;
-extern void booz2_guidance_h_init(void);
-extern void booz2_guidance_h_mode_changed(uint8_t new_mode);
-extern void booz2_guidance_h_read_rc(bool_t in_flight);
-extern void booz2_guidance_h_run(bool_t in_flight);
+extern void guidance_h_init(void);
+extern void guidance_h_mode_changed(uint8_t new_mode);
+extern void guidance_h_read_rc(bool_t in_flight);
+extern void guidance_h_run(bool_t in_flight);
-#define booz2_guidance_h_SetKi(_val) { \
- booz2_guidance_h_igain = _val; \
- INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); \
+#define guidance_h_SetKi(_val) { \
+ guidance_h_igain = _val; \
+ INT_VECT2_ZERO(guidance_h_pos_err_sum); \
}
-#endif /* BOOZ2_GUIDANCE_H_H */
+#endif /* GUIDANCE_H_H */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -1,5 +1,5 @@
/*
- * $Id: booz2_guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $
+ * $Id: guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $
*
* Copyright (C) 2008-2009 ENAC <address@hidden>
*
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_GUIDANCE_H_REF_H
-#define BOOZ2_GUIDANCE_H_REF_H
+#ifndef GUIDANCE_H_REF_H
+#define GUIDANCE_H_REF_H
#include "airframe.h"
#include "inttypes.h"
@@ -50,34 +50,34 @@
#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
/* Saturations definition */
-#ifndef BOOZ2_GUIDANCE_H_REF_MAX_ACCEL
-#define BOOZ2_GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
+#ifndef GUIDANCE_H_REF_MAX_ACCEL
+#define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
#endif
-#define B2_GH_MAX_ACCEL BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_ACCEL,
B2_GH_ACCEL_REF_FRAC)
+#define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL,
B2_GH_ACCEL_REF_FRAC)
-#ifndef BOOZ2_GUIDANCE_H_REF_MAX_SPEED
-#define BOOZ2_GUIDANCE_H_REF_MAX_SPEED ( 5. )
+#ifndef GUIDANCE_H_REF_MAX_SPEED
+#define GUIDANCE_H_REF_MAX_SPEED ( 5. )
#endif
-#define B2_GH_MAX_SPEED BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_SPEED,
B2_GH_SPEED_REF_FRAC)
+#define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED,
B2_GH_SPEED_REF_FRAC)
/* second order model natural frequency and damping */
-#ifndef BOOZ2_GUIDANCE_H_REF_OMEGA
-#define BOOZ2_GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
+#ifndef GUIDANCE_H_REF_OMEGA
+#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
#endif
-#ifndef BOOZ2_GUIDANCE_H_REF_ZETA
-#define BOOZ2_GUIDANCE_H_REF_ZETA 0.85
+#ifndef GUIDANCE_H_REF_ZETA
+#define GUIDANCE_H_REF_ZETA 0.85
#endif
#define B2_GH_ZETA_OMEGA_FRAC 10
-#define B2_GH_ZETA_OMEGA
BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_ZETA*BOOZ2_GUIDANCE_H_REF_OMEGA),
B2_GH_ZETA_OMEGA_FRAC)
+#define B2_GH_ZETA_OMEGA
BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC)
#define B2_GH_OMEGA_2_FRAC 7
-#define B2_GH_OMEGA_2
BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_OMEGA*BOOZ2_GUIDANCE_H_REF_OMEGA),
B2_GH_OMEGA_2_FRAC)
+#define B2_GH_OMEGA_2
BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC)
/* first order time constant */
#define B2_GH_REF_THAU_F 0.5
#define B2_GH_REF_INV_THAU_FRAC 16
#define B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F),
B2_GH_REF_INV_THAU_FRAC)
-#ifdef B2_GUIDANCE_H_C
+#ifdef GUIDANCE_H_C
static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2
speed, struct Int32Vect2 accel);
static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
@@ -186,6 +186,6 @@
}
}
-#endif /* B2_GUIDANCE_H_C */
+#endif /* GUIDANCE_H_C */
-#endif /* BOOZ2_GUIDANCE_H_REF_H */
+#endif /* GUIDANCE_H_REF_H */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-09-28 14:04:56 UTC (rev 6005)
@@ -21,9 +21,9 @@
* Boston, MA 02111-1307, USA.
*/
-#define B2_GUIDANCE_V_C
-#define B2_GUIDANCE_V_USE_REF
-#include "booz2_guidance_v.h"
+#define GUIDANCE_V_C
+#define GUIDANCE_V_USE_REF
+#include "guidance_v.h"
#include "booz_radio_control.h"
@@ -37,95 +37,95 @@
#include "airframe.h"
-uint8_t booz2_guidance_v_mode;
-int32_t booz2_guidance_v_ff_cmd;
-int32_t booz2_guidance_v_fb_cmd;
+uint8_t guidance_v_mode;
+int32_t guidance_v_ff_cmd;
+int32_t guidance_v_fb_cmd;
/* command output */
-int32_t booz2_guidance_v_delta_t;
+int32_t guidance_v_delta_t;
/* direct throttle from radio control */
/* range 0:200 */
-int32_t booz2_guidance_v_rc_delta_t;
+int32_t guidance_v_rc_delta_t;
/* vertical speed setpoint from radio control */
/* Q12.19 : accuracy 0.0000019, range +/-4096 */
-int32_t booz2_guidance_v_rc_zd_sp;
+int32_t guidance_v_rc_zd_sp;
/* altitude setpoint in meter (input) */
/* Q23.8 : accuracy 0.0039, range 8388km */
-int32_t booz2_guidance_v_z_sp;
+int32_t guidance_v_z_sp;
/* vertical speed setpoint in meter/s (input) */
/* Q12.19 : accuracy 0.0000019, range +/-4096 */
-int32_t booz2_guidance_v_zd_sp;
-#define BOOZ2_GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC
+int32_t guidance_v_zd_sp;
+#define GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC
/* altitude reference in meter */
/* Q23.8 : accuracy 0.0039, range 8388km */
-int32_t booz2_guidance_v_z_ref;
+int32_t guidance_v_z_ref;
/* vertical speed reference in meter/s */
/* Q12.19 : accuracy 0.0000038, range 4096 */
-int32_t booz2_guidance_v_zd_ref;
+int32_t guidance_v_zd_ref;
/* vertical acceleration reference in meter/s^2 */
/* Q21.10 : accuracy 0.0009766, range 2097152 */
-int32_t booz2_guidance_v_zdd_ref;
+int32_t guidance_v_zdd_ref;
-int32_t booz2_guidance_v_kp;
-int32_t booz2_guidance_v_kd;
-int32_t booz2_guidance_v_ki;
+int32_t guidance_v_kp;
+int32_t guidance_v_kd;
+int32_t guidance_v_ki;
-int32_t booz2_guidance_v_z_sum_err;
+int32_t guidance_v_z_sum_err;
#define Booz2GuidanceVSetRef(_pos, _speed, _accel) { \
b2_gv_set_ref(_pos, _speed, _accel); \
- booz2_guidance_v_z_ref = _pos; \
- booz2_guidance_v_zd_ref = _speed; \
- booz2_guidance_v_zdd_ref = _accel; \
+ guidance_v_z_ref = _pos; \
+ guidance_v_zd_ref = _speed; \
+ guidance_v_zdd_ref = _accel; \
}
static inline void run_hover_loop(bool_t in_flight);
-void booz2_guidance_v_init(void) {
+void guidance_v_init(void) {
- booz2_guidance_v_mode = BOOZ2_GUIDANCE_V_MODE_KILL;
+ guidance_v_mode = GUIDANCE_V_MODE_KILL;
- booz2_guidance_v_kp = BOOZ2_GUIDANCE_V_HOVER_KP;
- booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD;
- booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI;
+ guidance_v_kp = GUIDANCE_V_HOVER_KP;
+ guidance_v_kd = GUIDANCE_V_HOVER_KD;
+ guidance_v_ki = GUIDANCE_V_HOVER_KI;
- booz2_guidance_v_z_sum_err = 0;
+ guidance_v_z_sum_err = 0;
b2_gv_adapt_init();
}
-void booz2_guidance_v_read_rc(void) {
+void guidance_v_read_rc(void) {
// used in RC_DIRECT directly and as saturation in CLIMB and HOVER
- booz2_guidance_v_rc_delta_t =
(int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 200 / MAX_PPRZ;
+ guidance_v_rc_delta_t =
(int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 200 / MAX_PPRZ;
// used in RC_CLIMB
- booz2_guidance_v_rc_zd_sp = ((MAX_PPRZ/2) -
(int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) *
- BOOZ2_GUIDANCE_V_RC_CLIMB_COEF;
- DeadBand(booz2_guidance_v_rc_zd_sp, BOOZ2_GUIDANCE_V_RC_CLIMB_DEAD_BAND);
+ guidance_v_rc_zd_sp = ((MAX_PPRZ/2) -
(int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) *
+ GUIDANCE_V_RC_CLIMB_COEF;
+ DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND);
}
-void booz2_guidance_v_mode_changed(uint8_t new_mode) {
+void guidance_v_mode_changed(uint8_t new_mode) {
- if (new_mode == booz2_guidance_v_mode)
+ if (new_mode == guidance_v_mode)
return;
- // switch ( booz2_guidance_v_mode ) {
+ // switch ( guidance_v_mode ) {
//
// }
switch (new_mode) {
- case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB:
- case BOOZ2_GUIDANCE_V_MODE_CLIMB:
- case BOOZ2_GUIDANCE_V_MODE_HOVER:
- case BOOZ2_GUIDANCE_V_MODE_NAV:
- booz2_guidance_v_z_sum_err = 0;
+ case GUIDANCE_V_MODE_RC_CLIMB:
+ case GUIDANCE_V_MODE_CLIMB:
+ case GUIDANCE_V_MODE_HOVER:
+ case GUIDANCE_V_MODE_NAV:
+ guidance_v_z_sum_err = 0;
Booz2GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
break;
default:
@@ -133,17 +133,17 @@
}
- booz2_guidance_v_mode = new_mode;
+ guidance_v_mode = new_mode;
}
-void booz2_guidance_v_notify_in_flight( bool_t in_flight) {
+void guidance_v_notify_in_flight( bool_t in_flight) {
if (in_flight)
b2_gv_adapt_init();
}
-void booz2_guidance_v_run(bool_t in_flight) {
+void guidance_v_run(bool_t in_flight) {
// FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co
@@ -157,65 +157,65 @@
//ins_vf_realign = TRUE;
}
- switch (booz2_guidance_v_mode) {
+ switch (guidance_v_mode) {
- case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT:
- booz2_guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that
+ case GUIDANCE_V_MODE_RC_DIRECT:
+ 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;
+ booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
break;
- case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB:
- booz2_guidance_v_zd_sp = booz2_guidance_v_rc_zd_sp;
- b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
+ case GUIDANCE_V_MODE_RC_CLIMB:
+ guidance_v_zd_sp = guidance_v_rc_zd_sp;
+ b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp);
run_hover_loop(in_flight);
- booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+ booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break;
- case BOOZ2_GUIDANCE_V_MODE_CLIMB:
+ case GUIDANCE_V_MODE_CLIMB:
#ifdef USE_FMS
- if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_CLIMB)
- booz2_guidance_v_zd_sp = fms.input.v_sp.climb;
+ if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB)
+ guidance_v_zd_sp = fms.input.v_sp.climb;
#endif
- b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
+ b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp);
run_hover_loop(in_flight);
// saturate max authority with RC stick
- booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t,
booz2_guidance_v_delta_t);
+ booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t,
guidance_v_delta_t);
break;
- case BOOZ2_GUIDANCE_V_MODE_HOVER:
+ case GUIDANCE_V_MODE_HOVER:
#ifdef USE_FMS
- if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_HOVER)
- booz2_guidance_v_z_sp = fms.input.v_sp.height;
+ if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
+ guidance_v_z_sp = fms.input.v_sp.height;
#endif
- b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp);
+ b2_gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
// saturate max authority with RC stick
- booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t,
booz2_guidance_v_delta_t);
+ booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t,
guidance_v_delta_t);
break;
- case BOOZ2_GUIDANCE_V_MODE_NAV:
+ case GUIDANCE_V_MODE_NAV:
{
if (vertical_mode == VERTICAL_MODE_ALT) {
- booz2_guidance_v_z_sp = -nav_flight_altitude;
- b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp);
+ guidance_v_z_sp = -nav_flight_altitude;
+ b2_gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_CLIMB) {
- booz2_guidance_v_zd_sp = -nav_climb;
- b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp);
- nav_flight_altitude = -booz2_guidance_v_z_sp;
+ guidance_v_zd_sp = -nav_climb;
+ b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp);
+ nav_flight_altitude = -guidance_v_z_sp;
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_MANUAL) {
- booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only
- booz2_guidance_v_delta_t = nav_throttle;
+ guidance_v_z_sp = -nav_flight_altitude; // For display only
+ guidance_v_delta_t = nav_throttle;
}
/* use rc limitation if available */
if (radio_control.status == RADIO_CONTROL_OK)
- booz_stabilization_cmd[COMMAND_THRUST] = Min(
booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t);
+ booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t,
guidance_v_delta_t);
else
- booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t;
+ booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break;
}
default:
@@ -232,52 +232,52 @@
/* convert our reference to generic representation */
int64_t tmp = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - INT32_POS_FRAC);
- booz2_guidance_v_z_ref = (int32_t)tmp;
- 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);
+ guidance_v_z_ref = (int32_t)tmp;
+ guidance_v_zd_ref = b2_gv_zd_ref<<(INT32_SPEED_FRAC - B2_GV_ZD_REF_FRAC);
+ 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 = 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 = ins_ltp_speed.z - booz2_guidance_v_zd_ref;
- Bound(err_zd, BOOZ2_GUIDANCE_V_MIN_ERR_ZD, BOOZ2_GUIDANCE_V_MAX_ERR_ZD);
+ int32_t err_z = ins_ltp_pos.z - guidance_v_z_ref;
+ Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
+ int32_t err_zd = ins_ltp_speed.z - guidance_v_zd_ref;
+ Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) {
- booz2_guidance_v_z_sum_err += err_z;
- Bound(booz2_guidance_v_z_sum_err, -BOOZ2_GUIDANCE_V_MAX_SUM_ERR,
BOOZ2_GUIDANCE_V_MAX_SUM_ERR);
+ guidance_v_z_sum_err += err_z;
+ Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR,
GUIDANCE_V_MAX_SUM_ERR);
}
else
- booz2_guidance_v_z_sum_err = 0;
+ guidance_v_z_sum_err = 0;
/* our nominal command : (g + zdd)*m */
-#ifdef BOOZ2_GUIDANCE_V_INV_M
- const int32_t inv_m = BFP_OF_REAL(BOOZ2_GUIDANCE_V_INV_M,
B2_GV_ADAPT_X_FRAC);
+#ifdef GUIDANCE_V_INV_M
+ const int32_t inv_m = BFP_OF_REAL(GUIDANCE_V_INV_M, B2_GV_ADAPT_X_FRAC);
#else
const int32_t inv_m = b2_gv_adapt_X>>(B2_GV_ADAPT_X_FRAC - FF_CMD_FRAC);
#endif
const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
- (booz2_guidance_v_zdd_ref<<(FF_CMD_FRAC -
INT32_ACCEL_FRAC));
+ (guidance_v_zdd_ref<<(FF_CMD_FRAC -
INT32_ACCEL_FRAC));
#if 0
if (g_m_zdd > 0)
- booz2_guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m;
+ guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m;
else
- booz2_guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
+ guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
#else
- booz2_guidance_v_ff_cmd = g_m_zdd / inv_m;
+ guidance_v_ff_cmd = g_m_zdd / inv_m;
int32_t cphi,ctheta,cphitheta;
PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF;
- booz2_guidance_v_ff_cmd = (booz2_guidance_v_ff_cmd << INT32_TRIG_FRAC) /
cphitheta;
+ guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
#endif
/* our error command */
- booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) +
- ((-booz2_guidance_v_kd * err_zd) >> 21) +
- ((-booz2_guidance_v_ki *
booz2_guidance_v_z_sum_err) >> 21);
+ guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) +
+ ((-guidance_v_kd * err_zd) >> 21) +
+ ((-guidance_v_ki * guidance_v_z_sum_err) >> 21);
- booz2_guidance_v_delta_t = booz2_guidance_v_ff_cmd + booz2_guidance_v_fb_cmd;
- // booz2_guidance_v_delta_t = booz2_guidance_v_fb_cmd;
+ guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
+ // guidance_v_delta_t = guidance_v_fb_cmd;
}
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -21,47 +21,47 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_GUIDANCE_V
-#define BOOZ2_GUIDANCE_V
+#ifndef GUIDANCE_V
+#define GUIDANCE_V
#include "std.h"
-#include "booz2_guidance_v_ref.h"
-#include "booz2_guidance_v_adpt.h"
+#include "guidance_v_ref.h"
+#include "guidance_v_adpt.h"
-#define BOOZ2_GUIDANCE_V_MODE_KILL 0
-#define BOOZ2_GUIDANCE_V_MODE_RC_DIRECT 1
-#define BOOZ2_GUIDANCE_V_MODE_RC_CLIMB 2
-#define BOOZ2_GUIDANCE_V_MODE_CLIMB 3
-#define BOOZ2_GUIDANCE_V_MODE_HOVER 4
-#define BOOZ2_GUIDANCE_V_MODE_NAV 5
+#define GUIDANCE_V_MODE_KILL 0
+#define GUIDANCE_V_MODE_RC_DIRECT 1
+#define GUIDANCE_V_MODE_RC_CLIMB 2
+#define GUIDANCE_V_MODE_CLIMB 3
+#define GUIDANCE_V_MODE_HOVER 4
+#define GUIDANCE_V_MODE_NAV 5
-extern uint8_t booz2_guidance_v_mode;
+extern uint8_t guidance_v_mode;
-extern int32_t booz2_guidance_v_z_sp;
-extern int32_t booz2_guidance_v_zd_sp;
-extern int32_t booz2_guidance_v_z_ref;
-extern int32_t booz2_guidance_v_zd_ref;
-extern int32_t booz2_guidance_v_zdd_ref;
-extern int32_t booz2_guidance_v_z_sum_err;
-extern int32_t booz2_guidance_v_ff_cmd;
-extern int32_t booz2_guidance_v_fb_cmd;
-extern int32_t booz2_guidance_v_delta_t;
+extern int32_t guidance_v_z_sp;
+extern int32_t guidance_v_zd_sp;
+extern int32_t guidance_v_z_ref;
+extern int32_t guidance_v_zd_ref;
+extern int32_t guidance_v_zdd_ref;
+extern int32_t guidance_v_z_sum_err;
+extern int32_t guidance_v_ff_cmd;
+extern int32_t guidance_v_fb_cmd;
+extern int32_t guidance_v_delta_t;
-extern int32_t booz2_guidance_v_kp;
-extern int32_t booz2_guidance_v_kd;
-extern int32_t booz2_guidance_v_ki;
+extern int32_t guidance_v_kp;
+extern int32_t guidance_v_kd;
+extern int32_t guidance_v_ki;
-extern void booz2_guidance_v_init(void);
-extern void booz2_guidance_v_read_rc(void);
-extern void booz2_guidance_v_mode_changed(uint8_t new_mode);
-extern void booz2_guidance_v_notify_in_flight(bool_t in_flight);
-extern void booz2_guidance_v_run(bool_t in_flight);
+extern void guidance_v_init(void);
+extern void guidance_v_read_rc(void);
+extern void guidance_v_mode_changed(uint8_t new_mode);
+extern void guidance_v_notify_in_flight(bool_t in_flight);
+extern void guidance_v_run(bool_t in_flight);
-#define booz2_guidance_v_SetKi(_val) { \
- booz2_guidance_v_ki = _val; \
- booz2_guidance_v_z_sum_err = 0; \
+#define guidance_v_SetKi(_val) { \
+ guidance_v_ki = _val; \
+ guidance_v_z_sum_err = 0; \
}
-#endif /* BOOZ2_GUIDANCE_V */
+#endif /* GUIDANCE_V */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
===================================================================
---
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
2010-09-28 14:04:47 UTC (rev 6004)
+++
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -27,15 +27,15 @@
* needed by the invert dynamic model to produce a nominal command
*/
-#ifndef BOOZ2_GUIDANCE_V_ADPT
-#define BOOZ2_GUIDANCE_V_ADPT
+#ifndef GUIDANCE_V_ADPT
+#define GUIDANCE_V_ADPT
extern int32_t b2_gv_adapt_X;
extern int32_t b2_gv_adapt_P;
extern int32_t b2_gv_adapt_Xmeas;
-#ifdef B2_GUIDANCE_V_C
+#ifdef GUIDANCE_V_C
/* Our State
Q13.18
@@ -142,6 +142,6 @@
}
-#endif /* B2_GUIDANCE_V_C */
+#endif /* GUIDANCE_V_C */
-#endif /* BOOZ2_GUIDANCE_V_ADPT */
+#endif /* GUIDANCE_V_ADPT */
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef BOOZ2_GUIDANCE_V_REF_H
-#define BOOZ2_GUIDANCE_V_REF_H
+#ifndef GUIDANCE_V_REF_H
+#define GUIDANCE_V_REF_H
#include "airframe.h"
#include "inttypes.h"
@@ -50,44 +50,44 @@
#define B2_GV_Z_REF_FRAC (B2_GV_ZD_REF_FRAC + B2_GV_FREQ_FRAC)
/* Saturations definition */
-#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZDD
-#define BOOZ2_GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
+#ifndef GUIDANCE_V_REF_MIN_ZDD
+#define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
#endif
-#define B2_GV_MIN_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZDD,
B2_GV_ZDD_REF_FRAC)
+#define B2_GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, B2_GV_ZDD_REF_FRAC)
-#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZDD
-#define BOOZ2_GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
+#ifndef GUIDANCE_V_REF_MAX_ZDD
+#define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
#endif
-#define B2_GV_MAX_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZDD,
B2_GV_ZDD_REF_FRAC)
+#define B2_GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, B2_GV_ZDD_REF_FRAC)
-#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZD
-#define BOOZ2_GUIDANCE_V_REF_MIN_ZD (-3.)
+#ifndef GUIDANCE_V_REF_MIN_ZD
+#define GUIDANCE_V_REF_MIN_ZD (-3.)
#endif
-#define B2_GV_MIN_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZD ,
B2_GV_ZD_REF_FRAC)
+#define B2_GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , B2_GV_ZD_REF_FRAC)
-#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZD
-#define BOOZ2_GUIDANCE_V_REF_MAX_ZD ( 3.)
+#ifndef GUIDANCE_V_REF_MAX_ZD
+#define GUIDANCE_V_REF_MAX_ZD ( 3.)
#endif
-#define B2_GV_MAX_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZD ,
B2_GV_ZD_REF_FRAC)
+#define B2_GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , B2_GV_ZD_REF_FRAC)
/* second order model natural frequency and damping */
-#ifndef BOOZ2_GUIDANCE_V_REF_OMEGA
-#define BOOZ2_GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
+#ifndef GUIDANCE_V_REF_OMEGA
+#define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
#endif
-#ifndef BOOZ2_GUIDANCE_V_REF_ZETA
-#define BOOZ2_GUIDANCE_V_REF_ZETA 0.85
+#ifndef GUIDANCE_V_REF_ZETA
+#define GUIDANCE_V_REF_ZETA 0.85
#endif
#define B2_GV_ZETA_OMEGA_FRAC 10
-#define B2_GV_ZETA_OMEGA
BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_ZETA*BOOZ2_GUIDANCE_V_REF_OMEGA),
B2_GV_ZETA_OMEGA_FRAC)
+#define B2_GV_ZETA_OMEGA
BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), B2_GV_ZETA_OMEGA_FRAC)
#define B2_GV_OMEGA_2_FRAC 7
-#define B2_GV_OMEGA_2
BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_OMEGA*BOOZ2_GUIDANCE_V_REF_OMEGA),
B2_GV_OMEGA_2_FRAC)
+#define B2_GV_OMEGA_2
BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), B2_GV_OMEGA_2_FRAC)
/* first order time constant */
#define B2_GV_REF_THAU_F 0.25
#define B2_GV_REF_INV_THAU_FRAC 16
#define B2_GV_REF_INV_THAU BFP_OF_REAL((1./0.25), B2_GV_REF_INV_THAU_FRAC)
-#ifdef B2_GUIDANCE_V_C
+#ifdef GUIDANCE_V_C
static inline void b2_gv_set_ref(int32_t alt, int32_t speed, int32_t accel);
static inline void b2_gv_update_ref_from_z_sp(int32_t z_sp);
static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_sp);
@@ -160,6 +160,6 @@
}
}
-#endif /* B2_GUIDANCE_V_C */
+#endif /* GUIDANCE_V_C */
-#endif /* BOOZ2_GUIDANCE_V_REF_H */
+#endif /* GUIDANCE_V_REF_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -1,7 +1,7 @@
-#ifndef BOOZ_GUIDANCE_H
-#define BOOZ_GUIDANCE_H
+#ifndef GUIDANCE_H
+#define GUIDANCE_H
-#include "guidance/booz2_guidance_h.h"
-#include "guidance/booz2_guidance_v.h"
+#include "guidance/guidance_h.h"
+#include "guidance/guidance_v.h"
-#endif /* BOOZ_GUIDANCE_H */
+#endif /* GUIDANCE_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-28
14:04:56 UTC (rev 6005)
@@ -50,7 +50,7 @@
#include "autopilot.h"
#include "booz_stabilization.h"
-#include "booz_guidance.h"
+#include "guidance.h"
#include "ahrs.h"
#include "ins.h"
@@ -115,8 +115,8 @@
booz_fms_init();
autopilot_init();
booz2_nav_init();
- booz2_guidance_h_init();
- booz2_guidance_v_init();
+ guidance_h_init();
+ guidance_v_init();
booz_stabilization_init();
ahrs_aligner_init();
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.c 2010-09-28
14:04:56 UTC (rev 6005)
@@ -41,9 +41,9 @@
vi.timeouted = TRUE;
vi.last_msg = VI_TIMEOUT;
- vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+ vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(vi.input.h_sp.attitude);
- vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB;
+ vi.input.v_mode = GUIDANCE_V_MODE_CLIMB;
vi.input.v_sp.climb = 0;
vi_impl_init();
@@ -55,9 +55,9 @@
vi.last_msg++;
else {
vi.timeouted = TRUE;
- vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
+ vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
INT_EULERS_ZERO(vi.input.h_sp.attitude);
- vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB;
+ vi.input.v_mode = GUIDANCE_V_MODE_CLIMB;
vi.input.v_sp.climb = 0;
}
#endif
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-09-28
14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-09-28
14:04:56 UTC (rev 6005)
@@ -32,7 +32,7 @@
#include "math/pprz_algebra_int.h"
#include "autopilot.h"
#include "booz/booz_stabilization.h"
-#include "booz/booz_guidance.h"
+#include "booz/guidance.h"
#include "booz/booz2_navigation.h"
struct Vi_imu_info {
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
2010-09-28 14:04:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_datalink.h
2010-09-28 14:04:56 UTC (rev 6005)
@@ -56,10 +56,10 @@
vi.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \
vi.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \
switch (vi.input.h_mode) { \
- case BOOZ2_GUIDANCE_H_MODE_KILL: \
- case BOOZ2_GUIDANCE_H_MODE_RATE : \
+ case GUIDANCE_H_MODE_KILL: \
+ case GUIDANCE_H_MODE_RATE : \
break; \
- case BOOZ2_GUIDANCE_H_MODE_ATTITUDE : \
+ case GUIDANCE_H_MODE_ATTITUDE : \
{
\
vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer);
\
vi.input.h_sp.attitude.theta =
DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
@@ -68,13 +68,13 @@
VI_LIMIT_ATTITUDE(vi.input.h_sp.attitude); \
}
\
break; \
- case BOOZ2_GUIDANCE_H_MODE_HOVER : \
+ case GUIDANCE_H_MODE_HOVER : \
{
\
vi.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
vi.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
}
\
break; \
- case BOOZ2_GUIDANCE_H_MODE_NAV : \
+ case GUIDANCE_H_MODE_NAV : \
{
\
vi.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \
vi.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
@@ -85,17 +85,17 @@
break; \
} \
switch (vi.input.v_mode) { \
- case BOOZ2_GUIDANCE_V_MODE_KILL: \
- case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: \
- case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: \
+ case GUIDANCE_V_MODE_KILL: \
+ case GUIDANCE_V_MODE_RC_DIRECT: \
+ case GUIDANCE_V_MODE_RC_CLIMB: \
break; \
- case BOOZ2_GUIDANCE_V_MODE_CLIMB : \
+ case GUIDANCE_V_MODE_CLIMB : \
vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \
- case BOOZ2_GUIDANCE_V_MODE_HOVER : \
+ case GUIDANCE_V_MODE_HOVER : \
vi.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \
- case BOOZ2_GUIDANCE_V_MODE_NAV : \
+ case GUIDANCE_V_MODE_NAV : \
vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \
default: \
@@ -105,8 +105,8 @@
#define VI_NAV_STICK_PARSE_DL(_dl_buffer) { \
vi.last_msg = 0; \
- vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_NAV; \
- vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_NAV; \
+ vi.input.h_mode = GUIDANCE_H_MODE_NAV; \
+ vi.input.v_mode = GUIDANCE_V_MODE_NAV; \
vi.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \
vi.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \
vi.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \
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:47 UTC (rev 6004)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi_test_signal.c
2010-09-28 14:04:56 UTC (rev 6005)
@@ -37,8 +37,8 @@
fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD;
fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE;
fms_test_signal.counter = 0;
- fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
- fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER;
+ fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
+ fms.input.v_mode = GUIDANCE_V_MODE_HOVER;
}
void booz_fms_impl_periodic(void) {
@@ -68,7 +68,7 @@
break;
#if 0
case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
- if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)
+ if (guidance_v_mode < GUIDANCE_V_MODE_HOVER)
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) ?
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6005] rename booz2_guidance/BOOZ2_GUIDANCE and B2_GUIDANCE to guidance/ GUIDANCE,
Felix Ruess <=