Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 15:30:08 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

rename booz2_guidance/BOOZ2_GUIDANCE and B2_GUIDANCE to guidance/GUIDANCE

Browse Source
This commit is contained in:
Felix Ruess
2010-09-28 14:04:56 +00:00
parent 0891f38a48
commit 6579e914c2
54 changed files with 569 additions and 569 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/blender.xml
+4 -4
View File
@@ -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 @@ 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
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+4 -4
View File
@@ -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
conf/airframes/ENAC/quadrotor/mkk1.xml
+4 -4
View File
@@ -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>
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+2 -2
View File
@@ -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"/>
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a1.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a1p.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a2.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a3.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a4.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a5.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a6.xml
+2 -2
View File
@@ -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"/>
conf/airframes/Poine/booz2_a7.xml
+2 -2
View File
@@ -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"/>
conf/airframes/UofAdelaide/A1000_BOOZ.xml
+2 -2
View File
@@ -219,7 +219,7 @@ second attempt
<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 @@ second attempt
<!-- <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"/>
conf/airframes/UofAdelaide/A1000_LISA.xml
+2 -2
View File
@@ -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"/>
conf/airframes/UofAdelaide/A1000_NOVA.xml
+2 -2
View File
@@ -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"/>
conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+2 -2
View File
@@ -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"/>
conf/airframes/UofAdelaide/booz2_a1000.xml
+2 -2
View File
@@ -220,7 +220,7 @@ second attempt
<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 @@ second attempt
</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"/>
conf/airframes/UofAdelaide/lisa_a1000.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_Aron.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_NoVa.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_NoVa_001.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_NoVa_002.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_flixr.xml
+4 -4
View File
@@ -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>
conf/airframes/booz2_ppzuav.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_s1.xml
+2 -2
View File
@@ -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"/>
conf/airframes/booz2_x1.xml
+2 -2
View File
@@ -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"/>
conf/airframes/esden/lisa_asctec.xml
+2 -2
View File
@@ -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"/>
conf/autopilot/rotorcraft.makefile
+2 -2
View File
@@ -192,8 +192,8 @@ ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
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
conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
+2 -2
View File
@@ -133,8 +133,8 @@ else ifeq ($(NUM_TYPE), float)
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
conf/messages.xml
+2 -2
View File
@@ -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"/>
conf/settings/settings_booz2.xml
+12 -12
View File
@@ -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>
sw/airborne/booz/booz2_navigation.c
+1 -1
View File
@@ -118,7 +118,7 @@ void booz2_nav_run(void) {
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);
}
sw/airborne/booz/booz2_telemetry.h
+49 -49
View File
@@ -35,7 +35,7 @@
#endif
#include "autopilot.h"
#include "booz_guidance.h"
#include "guidance.h"
#include "actuators.h"
@@ -67,8 +67,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&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 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&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 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#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 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#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 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&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); \
}
sw/airborne/booz/booz_fms.c
+4 -4
View File
@@ -41,9 +41,9 @@ void booz_fms_init(void) {
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 @@ void booz_fms_periodic(void) {
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
sw/airborne/booz/booz_fms.h
+1 -1
View File
@@ -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;
sw/airborne/booz/fms/booz_fms_datalink.h
+13 -13
View File
@@ -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); \
sw/airborne/booz/fms/booz_fms_test_signal.c
+3 -3
View File
@@ -37,8 +37,8 @@ void booz_fms_impl_init(void) {
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 @@ void booz_fms_impl_periodic(void) {
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) ?
sw/airborne/booz/test/test_vg_adpt.c
+2 -2
View File
@@ -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"
sw/airborne/booz/test/test_vg_ref.c
+2 -2
View File
@@ -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)
sw/airborne/firmwares/rotorcraft/autopilot.c
+19 -19
View File
@@ -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 @@ void autopilot_periodic(void) {
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 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
#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 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
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 @@ void autopilot_on_rc_frame(void) {
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);
}
}
sw/airborne/firmwares/rotorcraft/guidance.h
+5 -5
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+158 -158
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+34 -34
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+18 -18
View File
@@ -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 <poinix@gmail.com>
*
@@ -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 @@ extern struct Int64Vect2 b2_gh_pos_ref;
#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 @@ static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
}
}
#endif /* B2_GUIDANCE_H_C */
#endif /* GUIDANCE_H_C */
#endif /* BOOZ2_GUIDANCE_H_REF_H */
#endif /* GUIDANCE_H_REF_H */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+94 -94
View File
@@ -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 @@ void booz2_guidance_v_mode_changed(uint8_t new_mode) {
}
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 @@ void booz2_guidance_v_run(bool_t in_flight) {
//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 @@ static inline void run_hover_loop(bool_t in_flight) {
/* 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;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+32 -32
View File
@@ -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 */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+5 -5
View File
@@ -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 @@ static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
}
#endif /* B2_GUIDANCE_V_C */
#endif /* GUIDANCE_V_C */
#endif /* BOOZ2_GUIDANCE_V_ADPT */
#endif /* GUIDANCE_V_ADPT */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
+23 -23
View File
@@ -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 @@ extern int64_t b2_gv_z_ref;
#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 @@ static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_sp) {
}
}
#endif /* B2_GUIDANCE_V_C */
#endif /* GUIDANCE_V_C */
#endif /* BOOZ2_GUIDANCE_V_REF_H */
#endif /* GUIDANCE_V_REF_H */
sw/airborne/firmwares/rotorcraft/main.c
+3 -3
View File
@@ -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 @@ STATIC_INLINE void main_init( void ) {
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();

Some files were not shown because too many files have changed in this diff Show More