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 29 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>
<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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-140"/> <define name="PGAIN" value="-140"/>
<define name="DGAIN" value="-90"/> <define name="DGAIN" value="-90"/>
<define name="IGAIN" value="-30"/> <define name="IGAIN" value="-30"/>
@@ -230,7 +230,7 @@
<makefile location="after"> <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 += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3 -DUSE_PWM1
ap.CFLAGS += -DBOOZ_ACTUATORS_MAX_THRUST=160 -DBOOZ_THRUST_LOWPASS=17 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 += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\"
sim.CFLAGS += -DNPS_NO_SUPERVISION 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.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
sim.srcs += $(SRC_BOOZ_SIM)/booz2_pwm_hw.c 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"/> <define name="SONAR_SENS" value="2.146" integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -166,7 +166,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/> <define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-15"/> <define name="IGAIN" value="-15"/>
@@ -223,14 +223,14 @@
<makefile location="after"> <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.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
ap.srcs += $(SRC_BOOZ_ARCH)/booz2_pwm_hw.c ap.srcs += $(SRC_BOOZ_ARCH)/booz2_pwm_hw.c
sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\" sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\"
sim.CFLAGS += -DNPS_NO_SUPERVISION 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.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
sim.srcs += $(SRC_BOOZ_SIM)/booz2_pwm_hw.c 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"/> <define name="UNTILT_ACCEL" value="1"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -215,7 +215,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-60"/> <define name="PGAIN" value="-60"/>
<define name="DGAIN" value="-120"/> <define name="DGAIN" value="-120"/>
<define name="IGAIN" value="-10"/> <define name="IGAIN" value="-10"/>
@@ -260,10 +260,10 @@
#set GPS lag for horizontal filter #set GPS lag for horizontal filter
#ap.CFLAGS += -DUSE_GPS_ACC4R #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 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 sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT
</makefile> </makefile>
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+2 -2
View File
@@ -153,7 +153,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -162,7 +162,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -166,7 +166,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <define name="IGAIN" value="-0"/>
conf/airframes/Poine/booz2_a2.xml
+2 -2
View File
@@ -138,7 +138,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <define name="IGAIN" value="-0"/>
conf/airframes/Poine/booz2_a3.xml
+2 -2
View File
@@ -125,7 +125,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <define name="IGAIN" value="-0"/>
conf/airframes/Poine/booz2_a4.xml
+2 -2
View File
@@ -103,7 +103,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -158,7 +158,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -159,7 +159,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="10." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -159,7 +159,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="14.9" integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -235,7 +235,7 @@ second attempt
<!-- <define name="INV_M" value ="0.2"/> --> <!-- <define name="INV_M" value ="0.2"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -146,7 +146,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <define name="IGAIN" value="-0"/>
conf/airframes/UofAdelaide/A1000_NOVA.xml
+2 -2
View File
@@ -175,7 +175,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> --> <!-- <define name="INV_M" value="0.118"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/> <define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/> <define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/> <define name="IGAIN" value="-15"/>
conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+2 -2
View File
@@ -175,7 +175,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> --> <!-- <define name="INV_M" value="0.118"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/> <define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/> <define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/> <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"/> <define name="BARO_SENS" value="14.9" integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -237,7 +237,7 @@ second attempt
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -154,7 +154,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <define name="IGAIN" value="-0"/>
conf/airframes/booz2_Aron.xml
+2 -2
View File
@@ -138,7 +138,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <define name="IGAIN" value="-0"/>
conf/airframes/booz2_NoVa.xml
+2 -2
View File
@@ -175,7 +175,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> --> <!-- <define name="INV_M" value="0.118"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/> <define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/> <define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/> <define name="IGAIN" value="-15"/>
conf/airframes/booz2_NoVa_001.xml
+2 -2
View File
@@ -175,7 +175,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> --> <!-- <define name="INV_M" value="0.118"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/> <define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/> <define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/> <define name="IGAIN" value="-15"/>
conf/airframes/booz2_NoVa_002.xml
+2 -2
View File
@@ -175,7 +175,7 @@
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -195,7 +195,7 @@
<!-- <define name="INV_M" value="0.118"/> --> <!-- <define name="INV_M" value="0.118"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/> <define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/> <define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/> <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" /> <define name="BARO_SENS" value="15." integer="16" />
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)" />
<define name="MAX_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.)" /> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)" />
@@ -180,7 +180,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-70"/> <define name="PGAIN" value="-70"/>
<define name="DGAIN" value="-150"/> <define name="DGAIN" value="-150"/>
<define name="IGAIN" value="-15"/> <define name="IGAIN" value="-15"/>
@@ -227,12 +227,12 @@
<!--<define name="RADIO_CONTROL_KILL_SWITCH" value="RADIO_CONTROL_SWITCH1"/>--> <!--<define name="RADIO_CONTROL_KILL_SWITCH" value="RADIO_CONTROL_SWITCH1"/>-->
<define name="USE_ADAPT_HOVER"/> <define name="USE_ADAPT_HOVER"/>
<define name="USE_GPS_ACC4R"/> <define name="USE_GPS_ACC4R"/>
<define name="B2_GUIDANCE_H_USE_REF"/> <define name="GUIDANCE_H_USE_REF"/>
<define name="USE_INS_NAV_INIT"/> <define name="USE_INS_NAV_INIT"/>
</target> </target>
<target name="sim" board="pc"> <target name="sim" board="pc">
<define name="USE_ADAPT_HOVER"/> <define name="USE_ADAPT_HOVER"/>
<define name="B2_GUIDANCE_H_USE_REF"/> <define name="GUIDANCE_H_USE_REF"/>
<define name="USE_INS_NAV_INIT"/> <define name="USE_INS_NAV_INIT"/>
<subsystem name="fdm" type="nps"/> <subsystem name="fdm" type="nps"/>
</target> </target>
conf/airframes/booz2_ppzuav.xml
+2 -2
View File
@@ -150,7 +150,7 @@
<define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/> <define name="THRUST_COEF" value="{ 256, 256, 256, 256}"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -170,7 +170,7 @@
<!-- <define name="INV_M" value="0.118"/> --> <!-- <define name="INV_M" value="0.118"/> -->
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-50"/> <define name="PGAIN" value="-50"/>
<define name="DGAIN" value="-60"/> <define name="DGAIN" value="-60"/>
<define name="IGAIN" value="-15"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -150,7 +150,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="19." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -160,7 +160,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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"/> <define name="BARO_SENS" value="15." integer="16"/>
</section> </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="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
<define name="MAX_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.)"/> <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
@@ -154,7 +154,7 @@
</section> </section>
<section name="GUIDANCE_H" prefix="BOOZ2_GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/> <define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/> <define name="DGAIN" value="-100"/>
<define name="IGAIN" value="-0"/> <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.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
ap.CFLAGS += -DUSE_NAVIGATION ap.CFLAGS += -DUSE_NAVIGATION
ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
ap.srcs += $(SRC_FIRMWARE)/ins.c ap.srcs += $(SRC_FIRMWARE)/ins.c
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c 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 endif
sim.CFLAGS += -DUSE_NAVIGATION sim.CFLAGS += -DUSE_NAVIGATION
sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
sim.srcs += math/pprz_geodetic_int.c sim.srcs += math/pprz_geodetic_int.c
sim.srcs += $(SRC_FIRMWARE)/ins.c sim.srcs += $(SRC_FIRMWARE)/ins.c
conf/messages.xml
+2 -2
View File
@@ -936,7 +936,7 @@
<field name="cmd_thrust" type="int32"/> <field name="cmd_thrust" type="int32"/>
</message> </message>
<message name="BOOZ2_GUIDANCE" id="144"> <message name="GUIDANCE" id="144">
<field name="pos_n" type="int32"/> <field name="pos_n" type="int32"/>
<field name="pos_e" type="int32"/> <field name="pos_e" type="int32"/>
<field name="sp_pos_n" type="int32"/> <field name="sp_pos_n" type="int32"/>
@@ -1009,7 +1009,7 @@
<field name="tbd" type="float"/> <field name="tbd" type="float"/>
</message> </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="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_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"/> <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>
<dl_settings NAME="Vert Loop"> <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="guidance_v_kp" min="-600" step="1" max="0" module="guidance/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="guidance_v_kd" min="-600" step="1" max="0" module="guidance/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="guidance_v_ki" min="-300" step="1" max="0" module="guidance/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_z_sp" min="-5" step="0.5" max="3" module="guidance/guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="ins" shortname="vf_realign" values="OFF|ON"/> <dl_setting var="ins_vf_realign" min="0" step="1" max="1" module="ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings> </dl_settings>
<dl_settings NAME="Horiz Loop"> <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="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="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="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="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="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="booz2_guidance_h_pgain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="kp"/> <dl_setting var="guidance_h_pgain" min="-400" step="1" max="0" module="guidance/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="guidance_h_dgain" min="-400" step="1" max="0" module="guidance/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="guidance_h_igain" min="-400" step="1" max="0" module="guidance/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="guidance_h_ngain" min="-400" step="1" max="0" module="guidance/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_again" min="-400" step="1" max="0" module="guidance/guidance_h" shortname="ka"/>
<dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="ins" shortname="hf_realign" values="OFF|ON"/> <dl_setting var="ins_hf_realign" min="0" step="1" max="1" module="ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings> </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_t dist_to_waypoint;
INT32_VECT2_NORM(dist_to_waypoint, path_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) { if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
VECT2_COPY(booz2_navigation_carrot, booz2_navigation_target); VECT2_COPY(booz2_navigation_carrot, booz2_navigation_target);
} }
sw/airborne/booz/booz2_telemetry.h
+49 -49
View File
@@ -35,7 +35,7 @@
#endif #endif
#include "autopilot.h" #include "autopilot.h"
#include "booz_guidance.h" #include "guidance.h"
#include "actuators.h" #include "actuators.h"
@@ -67,8 +67,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&autopilot_mode, \ &autopilot_mode, \
&autopilot_in_flight, \ &autopilot_in_flight, \
&autopilot_motors_on, \ &autopilot_motors_on, \
&booz2_guidance_h_mode, \ &guidance_h_mode, \
&booz2_guidance_v_mode, \ &guidance_v_mode, \
&booz2_battery_voltage, \ &booz2_battery_voltage, \
&cpu_time_sec \ &cpu_time_sec \
); \ ); \
@@ -86,8 +86,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&autopilot_mode, \ &autopilot_mode, \
&autopilot_in_flight, \ &autopilot_in_flight, \
&autopilot_motors_on, \ &autopilot_motors_on, \
&booz2_guidance_h_mode, \ &guidance_h_mode, \
&booz2_guidance_v_mode, \ &guidance_v_mode, \
&booz2_battery_voltage, \ &booz2_battery_voltage, \
&cpu_time_sec \ &cpu_time_sec \
); \ ); \
@@ -525,12 +525,12 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {} #define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {}
#endif #endif
#define PERIODIC_SEND_BOOZ2_GUIDANCE(_chan) { \ #define PERIODIC_SEND_GUIDANCE(_chan) { \
DOWNLINK_SEND_BOOZ2_GUIDANCE(_chan, \ DOWNLINK_SEND_GUIDANCE(_chan, \
&booz2_guidance_h_cur_pos.x, \ &guidance_h_cur_pos.x, \
&booz2_guidance_h_cur_pos.y, \ &guidance_h_cur_pos.y, \
&booz2_guidance_h_held_pos.x, \ &guidance_h_held_pos.x, \
&booz2_guidance_h_held_pos.y); \ &guidance_h_held_pos.y); \
} }
#define PERIODIC_SEND_INS(_chan) { \ #define PERIODIC_SEND_INS(_chan) { \
@@ -605,64 +605,64 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_BOOZ2_VERT_LOOP(_chan) { \ #define PERIODIC_SEND_BOOZ2_VERT_LOOP(_chan) { \
DOWNLINK_SEND_BOOZ2_VERT_LOOP(_chan, \ DOWNLINK_SEND_BOOZ2_VERT_LOOP(_chan, \
&booz2_guidance_v_z_sp, \ &guidance_v_z_sp, \
&booz2_guidance_v_zd_sp, \ &guidance_v_zd_sp, \
&ins_ltp_pos.z, \ &ins_ltp_pos.z, \
&ins_ltp_speed.z, \ &ins_ltp_speed.z, \
&ins_ltp_accel.z, \ &ins_ltp_accel.z, \
&booz2_guidance_v_z_ref, \ &guidance_v_z_ref, \
&booz2_guidance_v_zd_ref, \ &guidance_v_zd_ref, \
&booz2_guidance_v_zdd_ref, \ &guidance_v_zdd_ref, \
&b2_gv_adapt_X, \ &b2_gv_adapt_X, \
&b2_gv_adapt_P, \ &b2_gv_adapt_P, \
&b2_gv_adapt_Xmeas, \ &b2_gv_adapt_Xmeas, \
&booz2_guidance_v_z_sum_err, \ &guidance_v_z_sum_err, \
&booz2_guidance_v_ff_cmd, \ &guidance_v_ff_cmd, \
&booz2_guidance_v_fb_cmd, \ &guidance_v_fb_cmd, \
&booz2_guidance_v_delta_t); \ &guidance_v_delta_t); \
} }
#define PERIODIC_SEND_BOOZ2_HOVER_LOOP(_chan) { \ #define PERIODIC_SEND_BOOZ2_HOVER_LOOP(_chan) { \
DOWNLINK_SEND_BOOZ2_HOVER_LOOP(_chan, \ DOWNLINK_SEND_BOOZ2_HOVER_LOOP(_chan, \
&booz2_guidance_h_pos_sp.x, \ &guidance_h_pos_sp.x, \
&booz2_guidance_h_pos_sp.y, \ &guidance_h_pos_sp.y, \
&ins_ltp_pos.x, \ &ins_ltp_pos.x, \
&ins_ltp_pos.y, \ &ins_ltp_pos.y, \
&ins_ltp_speed.x, \ &ins_ltp_speed.x, \
&ins_ltp_speed.y, \ &ins_ltp_speed.y, \
&ins_ltp_accel.x, \ &ins_ltp_accel.x, \
&ins_ltp_accel.y, \ &ins_ltp_accel.y, \
&booz2_guidance_h_pos_err.x, \ &guidance_h_pos_err.x, \
&booz2_guidance_h_pos_err.y, \ &guidance_h_pos_err.y, \
&booz2_guidance_h_speed_err.x, \ &guidance_h_speed_err.x, \
&booz2_guidance_h_speed_err.y, \ &guidance_h_speed_err.y, \
&booz2_guidance_h_pos_err_sum.x, \ &guidance_h_pos_err_sum.x, \
&booz2_guidance_h_pos_err_sum.y, \ &guidance_h_pos_err_sum.y, \
&booz2_guidance_h_nav_err.x, \ &guidance_h_nav_err.x, \
&booz2_guidance_h_nav_err.y, \ &guidance_h_nav_err.y, \
&booz2_guidance_h_command_earth.x, \ &guidance_h_command_earth.x, \
&booz2_guidance_h_command_earth.y, \ &guidance_h_command_earth.y, \
&booz2_guidance_h_command_body.phi, \ &guidance_h_command_body.phi, \
&booz2_guidance_h_command_body.theta, \ &guidance_h_command_body.theta, \
&booz2_guidance_h_command_body.psi); \ &guidance_h_command_body.psi); \
} }
#define PERIODIC_SEND_BOOZ2_GUIDANCE_H_REF(_chan) { \ #define PERIODIC_SEND_GUIDANCE_H_REF(_chan) { \
DOWNLINK_SEND_BOOZ2_GUIDANCE_H_REF_INT(_chan, \ DOWNLINK_SEND_GUIDANCE_H_REF_INT(_chan, \
&booz2_guidance_h_pos_sp.x, \ &guidance_h_pos_sp.x, \
&booz2_guidance_h_pos_ref.x, \ &guidance_h_pos_ref.x, \
&booz2_guidance_h_speed_ref.x, \ &guidance_h_speed_ref.x, \
&booz2_guidance_h_accel_ref.x, \ &guidance_h_accel_ref.x, \
&booz2_guidance_h_pos_sp.y, \ &guidance_h_pos_sp.y, \
&booz2_guidance_h_pos_ref.y, \ &guidance_h_pos_ref.y, \
&booz2_guidance_h_speed_ref.y, \ &guidance_h_speed_ref.y, \
&booz2_guidance_h_accel_ref.y); \ &guidance_h_accel_ref.y); \
} }
#include "booz_gps.h" #include "booz_gps.h"
#include "booz2_navigation.h" #include "booz2_navigation.h"
#define PERIODIC_SEND_BOOZ2_FP(_chan) { \ #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, \ DOWNLINK_SEND_BOOZ2_FP( _chan, \
&ins_enu_pos.x, \ &ins_enu_pos.x, \
&ins_enu_pos.y, \ &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.phi, \
&ahrs.ltp_to_body_euler.theta, \ &ahrs.ltp_to_body_euler.theta, \
&ahrs.ltp_to_body_euler.psi, \ &ahrs.ltp_to_body_euler.psi, \
&booz2_guidance_h_pos_sp.y, \ &guidance_h_pos_sp.y, \
&booz2_guidance_h_pos_sp.x, \ &guidance_h_pos_sp.x, \
&carrot_up, \ &carrot_up, \
&booz2_guidance_h_command_body.psi, \ &guidance_h_command_body.psi, \
&booz_stabilization_cmd[COMMAND_THRUST], \ &booz_stabilization_cmd[COMMAND_THRUST], \
&autopilot_flight_time); \ &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.timeouted = TRUE;
fms.last_msg = BOOZ_FMS_TIMEOUT; 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); 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; fms.input.v_sp.climb = 0;
booz_fms_impl_init(); booz_fms_impl_init();
@@ -55,9 +55,9 @@ void booz_fms_periodic(void) {
fms.last_msg++; fms.last_msg++;
else { else {
fms.timeouted = TRUE; 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); 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; fms.input.v_sp.climb = 0;
} }
#endif #endif
sw/airborne/booz/booz_fms.h
+1 -1
View File
@@ -27,7 +27,7 @@
#include "std.h" #include "std.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
#include "autopilot.h" #include "autopilot.h"
#include "booz_guidance.h" #include "guidance.h"
struct Booz_fms_imu_info { struct Booz_fms_imu_info {
struct Int16Vect3 gyro; 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.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \
fms.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ fms.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \
switch (fms.input.h_mode) { \ switch (fms.input.h_mode) { \
case BOOZ2_GUIDANCE_H_MODE_KILL: \ case GUIDANCE_H_MODE_KILL: \
case BOOZ2_GUIDANCE_H_MODE_RATE : \ case GUIDANCE_H_MODE_RATE : \
break; \ 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.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); \ 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); \ BOOZ_FMS_LIMIT_ATTITUDE(fms.input.h_sp.attitude); \
} \ } \
break; \ 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.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); \ fms.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
} \ } \
break; \ 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.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); \ fms.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \
@@ -64,17 +64,17 @@
break; \ break; \
} \ } \
switch (fms.input.v_mode) { \ switch (fms.input.v_mode) { \
case BOOZ2_GUIDANCE_V_MODE_KILL: \ case GUIDANCE_V_MODE_KILL: \
case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: \ case GUIDANCE_V_MODE_RC_DIRECT: \
case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: \ case GUIDANCE_V_MODE_RC_CLIMB: \
break; \ 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); \ fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \ 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); \ fms.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \ 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); \ fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \
break; \ break; \
} \ } \
@@ -82,8 +82,8 @@
#define BOOZ_FMS_NAV_STICK_PARSE_DL(_dl_buffer) { \ #define BOOZ_FMS_NAV_STICK_PARSE_DL(_dl_buffer) { \
fms.last_msg = 0; \ fms.last_msg = 0; \
fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_NAV; \ fms.input.h_mode = GUIDANCE_H_MODE_NAV; \
fms.input.v_mode = BOOZ2_GUIDANCE_V_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.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.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \
fms.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_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.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD;
fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE;
fms_test_signal.counter = 0; fms_test_signal.counter = 0;
fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE;
fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; fms.input.v_mode = GUIDANCE_V_MODE_HOVER;
} }
void booz_fms_impl_periodic(void) { void booz_fms_impl_periodic(void) {
@@ -68,7 +68,7 @@ void booz_fms_impl_periodic(void) {
break; break;
#if 0 #if 0
case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { 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; booz_fms_test_signal_start_z = ins_ltp_pos.z;
else { else {
booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? 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 "std.h"
#include "booz_geometry_mixed.h" #include "booz_geometry_mixed.h"
#define B2_GUIDANCE_V_C #define GUIDANCE_V_C
#include "booz2_guidance_v_adpt.h" #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 <stdio.h>
#include "booz_geometry_mixed.h" #include "booz_geometry_mixed.h"
#define B2_GUIDANCE_V_C #define GUIDANCE_V_C
#include "booz2_guidance_v_ref.h" #include "guidance_v_ref.h"
#define NB_STEP 10000 #define NB_STEP 10000
#define DT (1./B2_GV_FREQ) #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 "booz_radio_control.h"
#include "booz2_commands.h" #include "booz2_commands.h"
#include "booz2_navigation.h" #include "booz2_navigation.h"
#include "booz_guidance.h" #include "guidance.h"
#include "booz_stabilization.h" #include "booz_stabilization.h"
#include "led.h" #include "led.h"
@@ -87,8 +87,8 @@ void autopilot_periodic(void) {
autopilot_in_flight, autopilot_motors_on); autopilot_in_flight, autopilot_motors_on);
} }
else { else {
booz2_guidance_v_run( autopilot_in_flight ); guidance_v_run( autopilot_in_flight );
booz2_guidance_h_run( autopilot_in_flight ); guidance_h_run( autopilot_in_flight );
SetCommands(booz_stabilization_cmd, SetCommands(booz_stabilization_cmd,
autopilot_in_flight, autopilot_motors_on); autopilot_in_flight, autopilot_motors_on);
} }
@@ -105,29 +105,29 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
#ifndef KILL_AS_FAILSAFE #ifndef KILL_AS_FAILSAFE
booz_stab_att_sp_euler.phi = 0; booz_stab_att_sp_euler.phi = 0;
booz_stab_att_sp_euler.theta = 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; break;
#endif #endif
case AP_MODE_KILL: case AP_MODE_KILL:
autopilot_motors_on = FALSE; autopilot_motors_on = FALSE;
booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL); guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
break; break;
case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_Z_HOLD: 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; break;
case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD: 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; break;
case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD: 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; break;
case AP_MODE_NAV: case AP_MODE_NAV:
booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV); guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break; break;
default: default:
break; break;
@@ -136,33 +136,33 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
switch (new_autopilot_mode) { switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE: case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE #ifndef KILL_AS_FAILSAFE
booz2_guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5);
booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB); guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
break; break;
#endif #endif
case AP_MODE_KILL: case AP_MODE_KILL:
booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL); guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break; break;
case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_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; break;
case AP_MODE_RATE_RC_CLIMB: case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_ATTITUDE_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; break;
case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_HOVER_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; break;
case AP_MODE_RATE_Z_HOLD: case AP_MODE_RATE_Z_HOLD:
case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD:
case AP_MODE_HOVER_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; break;
case AP_MODE_NAV: case AP_MODE_NAV:
booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_NAV); guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break; break;
default: default:
break; break;
@@ -252,8 +252,8 @@ void autopilot_on_rc_frame(void) {
kill_throttle = !autopilot_motors_on; kill_throttle = !autopilot_motors_on;
if (autopilot_mode > AP_MODE_FAILSAFE) { if (autopilot_mode > AP_MODE_FAILSAFE) {
booz2_guidance_v_read_rc(); guidance_v_read_rc();
booz2_guidance_h_read_rc(autopilot_in_flight); 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 #ifndef GUIDANCE_H
#define BOOZ_GUIDANCE_H #define GUIDANCE_H
#include "guidance/booz2_guidance_h.h" #include "guidance/guidance_h.h"
#include "guidance/booz2_guidance_v.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. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ2_GUIDANCE_H_H #ifndef GUIDANCE_H_H
#define BOOZ2_GUIDANCE_H_H #define GUIDANCE_H_H
#include "math/pprz_algebra_int.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 GUIDANCE_H_MODE_KILL 0
#define BOOZ2_GUIDANCE_H_MODE_RATE 1 #define GUIDANCE_H_MODE_RATE 1
#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2 #define GUIDANCE_H_MODE_ATTITUDE 2
#define BOOZ2_GUIDANCE_H_MODE_HOVER 3 #define GUIDANCE_H_MODE_HOVER 3
#define BOOZ2_GUIDANCE_H_MODE_NAV 4 #define GUIDANCE_H_MODE_NAV 4
extern uint8_t booz2_guidance_h_mode; extern uint8_t guidance_h_mode;
/* horizontal setpoint in NED */ /* horizontal setpoint in NED */
/* Q_int32_xx_8 */ /* Q_int32_xx_8 */
extern struct Int32Vect2 booz2_guidance_h_pos_sp; extern struct Int32Vect2 guidance_h_pos_sp;
extern int32_t booz2_guidance_h_psi_sp; extern int32_t guidance_h_psi_sp;
extern struct Int32Vect2 booz2_guidance_h_pos_ref; extern struct Int32Vect2 guidance_h_pos_ref;
extern struct Int32Vect2 booz2_guidance_h_speed_ref; extern struct Int32Vect2 guidance_h_speed_ref;
extern struct Int32Vect2 booz2_guidance_h_accel_ref; extern struct Int32Vect2 guidance_h_accel_ref;
extern struct Int32Vect2 booz2_guidance_h_pos_err; extern struct Int32Vect2 guidance_h_pos_err;
extern struct Int32Vect2 booz2_guidance_h_speed_err; extern struct Int32Vect2 guidance_h_speed_err;
extern struct Int32Vect2 booz2_guidance_h_pos_err_sum; extern struct Int32Vect2 guidance_h_pos_err_sum;
extern struct Int32Vect2 booz2_guidance_h_nav_err; extern struct Int32Vect2 guidance_h_nav_err;
extern struct Int32Eulers booz2_guidance_h_rc_sp; extern struct Int32Eulers guidance_h_rc_sp;
extern struct Int32Vect2 booz2_guidance_h_command_earth; extern struct Int32Vect2 guidance_h_command_earth;
extern struct Int32Eulers booz2_guidance_h_command_body; extern struct Int32Eulers guidance_h_command_body;
extern int32_t booz2_guidance_h_pgain; extern int32_t guidance_h_pgain;
extern int32_t booz2_guidance_h_dgain; extern int32_t guidance_h_dgain;
extern int32_t booz2_guidance_h_igain; extern int32_t guidance_h_igain;
extern int32_t booz2_guidance_h_ngain; extern int32_t guidance_h_ngain;
extern int32_t booz2_guidance_h_again; extern int32_t guidance_h_again;
extern void booz2_guidance_h_init(void); extern void guidance_h_init(void);
extern void booz2_guidance_h_mode_changed(uint8_t new_mode); extern void guidance_h_mode_changed(uint8_t new_mode);
extern void booz2_guidance_h_read_rc(bool_t in_flight); extern void guidance_h_read_rc(bool_t in_flight);
extern void booz2_guidance_h_run(bool_t in_flight); extern void guidance_h_run(bool_t in_flight);
#define booz2_guidance_h_SetKi(_val) { \ #define guidance_h_SetKi(_val) { \
booz2_guidance_h_igain = _val; \ guidance_h_igain = _val; \
INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); \ 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> * Copyright (C) 2008-2009 ENAC <poinix@gmail.com>
* *
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ2_GUIDANCE_H_REF_H #ifndef GUIDANCE_H_REF_H
#define BOOZ2_GUIDANCE_H_REF_H #define GUIDANCE_H_REF_H
#include "airframe.h" #include "airframe.h"
#include "inttypes.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) #define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
/* Saturations definition */ /* Saturations definition */
#ifndef BOOZ2_GUIDANCE_H_REF_MAX_ACCEL #ifndef GUIDANCE_H_REF_MAX_ACCEL
#define BOOZ2_GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 ) #define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
#endif #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 #ifndef GUIDANCE_H_REF_MAX_SPEED
#define BOOZ2_GUIDANCE_H_REF_MAX_SPEED ( 5. ) #define GUIDANCE_H_REF_MAX_SPEED ( 5. )
#endif #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 */ /* second order model natural frequency and damping */
#ifndef BOOZ2_GUIDANCE_H_REF_OMEGA #ifndef GUIDANCE_H_REF_OMEGA
#define BOOZ2_GUIDANCE_H_REF_OMEGA RadOfDeg(67.) #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
#endif #endif
#ifndef BOOZ2_GUIDANCE_H_REF_ZETA #ifndef GUIDANCE_H_REF_ZETA
#define BOOZ2_GUIDANCE_H_REF_ZETA 0.85 #define GUIDANCE_H_REF_ZETA 0.85
#endif #endif
#define B2_GH_ZETA_OMEGA_FRAC 10 #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_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 */ /* first order time constant */
#define B2_GH_REF_THAU_F 0.5 #define B2_GH_REF_THAU_F 0.5
#define B2_GH_REF_INV_THAU_FRAC 16 #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) #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_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_pos_sp(struct Int32Vect2 pos_sp);
static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_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. * Boston, MA 02111-1307, USA.
*/ */
#define B2_GUIDANCE_V_C #define GUIDANCE_V_C
#define B2_GUIDANCE_V_USE_REF #define GUIDANCE_V_USE_REF
#include "booz2_guidance_v.h" #include "guidance_v.h"
#include "booz_radio_control.h" #include "booz_radio_control.h"
@@ -37,95 +37,95 @@
#include "airframe.h" #include "airframe.h"
uint8_t booz2_guidance_v_mode; uint8_t guidance_v_mode;
int32_t booz2_guidance_v_ff_cmd; int32_t guidance_v_ff_cmd;
int32_t booz2_guidance_v_fb_cmd; int32_t guidance_v_fb_cmd;
/* command output */ /* command output */
int32_t booz2_guidance_v_delta_t; int32_t guidance_v_delta_t;
/* direct throttle from radio control */ /* direct throttle from radio control */
/* range 0:200 */ /* range 0:200 */
int32_t booz2_guidance_v_rc_delta_t; int32_t guidance_v_rc_delta_t;
/* vertical speed setpoint from radio control */ /* vertical speed setpoint from radio control */
/* Q12.19 : accuracy 0.0000019, range +/-4096 */ /* 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) */ /* altitude setpoint in meter (input) */
/* Q23.8 : accuracy 0.0039, range 8388km */ /* 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) */ /* vertical speed setpoint in meter/s (input) */
/* Q12.19 : accuracy 0.0000019, range +/-4096 */ /* Q12.19 : accuracy 0.0000019, range +/-4096 */
int32_t booz2_guidance_v_zd_sp; int32_t guidance_v_zd_sp;
#define BOOZ2_GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC #define GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC
/* altitude reference in meter */ /* altitude reference in meter */
/* Q23.8 : accuracy 0.0039, range 8388km */ /* 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 */ /* vertical speed reference in meter/s */
/* Q12.19 : accuracy 0.0000038, range 4096 */ /* 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 */ /* vertical acceleration reference in meter/s^2 */
/* Q21.10 : accuracy 0.0009766, range 2097152 */ /* 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 guidance_v_kp;
int32_t booz2_guidance_v_kd; int32_t guidance_v_kd;
int32_t booz2_guidance_v_ki; 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) { \ #define Booz2GuidanceVSetRef(_pos, _speed, _accel) { \
b2_gv_set_ref(_pos, _speed, _accel); \ b2_gv_set_ref(_pos, _speed, _accel); \
booz2_guidance_v_z_ref = _pos; \ guidance_v_z_ref = _pos; \
booz2_guidance_v_zd_ref = _speed; \ guidance_v_zd_ref = _speed; \
booz2_guidance_v_zdd_ref = _accel; \ guidance_v_zdd_ref = _accel; \
} }
static inline void run_hover_loop(bool_t in_flight); 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; guidance_v_kp = GUIDANCE_V_HOVER_KP;
booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD; guidance_v_kd = GUIDANCE_V_HOVER_KD;
booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI; guidance_v_ki = GUIDANCE_V_HOVER_KI;
booz2_guidance_v_z_sum_err = 0; guidance_v_z_sum_err = 0;
b2_gv_adapt_init(); 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 // 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 // used in RC_CLIMB
booz2_guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) * guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) *
BOOZ2_GUIDANCE_V_RC_CLIMB_COEF; GUIDANCE_V_RC_CLIMB_COEF;
DeadBand(booz2_guidance_v_rc_zd_sp, BOOZ2_GUIDANCE_V_RC_CLIMB_DEAD_BAND); 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; return;
// switch ( booz2_guidance_v_mode ) { // switch ( guidance_v_mode ) {
// //
// } // }
switch (new_mode) { switch (new_mode) {
case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: case GUIDANCE_V_MODE_RC_CLIMB:
case BOOZ2_GUIDANCE_V_MODE_CLIMB: case GUIDANCE_V_MODE_CLIMB:
case BOOZ2_GUIDANCE_V_MODE_HOVER: case GUIDANCE_V_MODE_HOVER:
case BOOZ2_GUIDANCE_V_MODE_NAV: case GUIDANCE_V_MODE_NAV:
booz2_guidance_v_z_sum_err = 0; guidance_v_z_sum_err = 0;
Booz2GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0); Booz2GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0);
break; break;
default: 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) if (in_flight)
b2_gv_adapt_init(); 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 // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT
// AKA SUPERVISION and co // AKA SUPERVISION and co
@@ -157,65 +157,65 @@ void booz2_guidance_v_run(bool_t in_flight) {
//ins_vf_realign = TRUE; //ins_vf_realign = TRUE;
} }
switch (booz2_guidance_v_mode) { switch (guidance_v_mode) {
case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: case GUIDANCE_V_MODE_RC_DIRECT:
booz2_guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that 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 ? 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; break;
case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: case GUIDANCE_V_MODE_RC_CLIMB:
booz2_guidance_v_zd_sp = booz2_guidance_v_rc_zd_sp; guidance_v_zd_sp = guidance_v_rc_zd_sp;
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); 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; break;
case BOOZ2_GUIDANCE_V_MODE_CLIMB: case GUIDANCE_V_MODE_CLIMB:
#ifdef USE_FMS #ifdef USE_FMS
if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_CLIMB) if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB)
booz2_guidance_v_zd_sp = fms.input.v_sp.climb; guidance_v_zd_sp = fms.input.v_sp.climb;
#endif #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); run_hover_loop(in_flight);
// saturate max authority with RC stick // 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; break;
case BOOZ2_GUIDANCE_V_MODE_HOVER: case GUIDANCE_V_MODE_HOVER:
#ifdef USE_FMS #ifdef USE_FMS
if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_HOVER) if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
booz2_guidance_v_z_sp = fms.input.v_sp.height; guidance_v_z_sp = fms.input.v_sp.height;
#endif #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); run_hover_loop(in_flight);
// saturate max authority with RC stick // 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; break;
case BOOZ2_GUIDANCE_V_MODE_NAV: case GUIDANCE_V_MODE_NAV:
{ {
if (vertical_mode == VERTICAL_MODE_ALT) { if (vertical_mode == VERTICAL_MODE_ALT) {
booz2_guidance_v_z_sp = -nav_flight_altitude; guidance_v_z_sp = -nav_flight_altitude;
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); run_hover_loop(in_flight);
} }
else if (vertical_mode == VERTICAL_MODE_CLIMB) { else if (vertical_mode == VERTICAL_MODE_CLIMB) {
booz2_guidance_v_zd_sp = -nav_climb; guidance_v_zd_sp = -nav_climb;
b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp); b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp);
nav_flight_altitude = -booz2_guidance_v_z_sp; nav_flight_altitude = -guidance_v_z_sp;
run_hover_loop(in_flight); run_hover_loop(in_flight);
} }
else if (vertical_mode == VERTICAL_MODE_MANUAL) { else if (vertical_mode == VERTICAL_MODE_MANUAL) {
booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only guidance_v_z_sp = -nav_flight_altitude; // For display only
booz2_guidance_v_delta_t = nav_throttle; guidance_v_delta_t = nav_throttle;
} }
/* use rc limitation if available */ /* use rc limitation if available */
if (radio_control.status == RADIO_CONTROL_OK) 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 else
booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t; booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break; break;
} }
default: default:
@@ -232,52 +232,52 @@ static inline void run_hover_loop(bool_t in_flight) {
/* convert our reference to generic representation */ /* convert our reference to generic representation */
int64_t tmp = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - INT32_POS_FRAC); int64_t tmp = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - INT32_POS_FRAC);
booz2_guidance_v_z_ref = (int32_t)tmp; guidance_v_z_ref = (int32_t)tmp;
booz2_guidance_v_zd_ref = b2_gv_zd_ref<<(INT32_SPEED_FRAC - B2_GV_ZD_REF_FRAC); 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_zdd_ref = b2_gv_zdd_ref<<(INT32_ACCEL_FRAC - B2_GV_ZDD_REF_FRAC);
/* compute the error to our reference */ /* compute the error to our reference */
int32_t err_z = ins_ltp_pos.z - booz2_guidance_v_z_ref; int32_t err_z = ins_ltp_pos.z - guidance_v_z_ref;
Bound(err_z, BOOZ2_GUIDANCE_V_MIN_ERR_Z, BOOZ2_GUIDANCE_V_MAX_ERR_Z); Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
int32_t err_zd = ins_ltp_speed.z - booz2_guidance_v_zd_ref; int32_t err_zd = ins_ltp_speed.z - guidance_v_zd_ref;
Bound(err_zd, BOOZ2_GUIDANCE_V_MIN_ERR_ZD, BOOZ2_GUIDANCE_V_MAX_ERR_ZD); Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
if (in_flight) { if (in_flight) {
booz2_guidance_v_z_sum_err += err_z; 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); Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
} }
else else
booz2_guidance_v_z_sum_err = 0; guidance_v_z_sum_err = 0;
/* our nominal command : (g + zdd)*m */ /* our nominal command : (g + zdd)*m */
#ifdef BOOZ2_GUIDANCE_V_INV_M #ifdef GUIDANCE_V_INV_M
const int32_t inv_m = BFP_OF_REAL(BOOZ2_GUIDANCE_V_INV_M, B2_GV_ADAPT_X_FRAC); const int32_t inv_m = BFP_OF_REAL(GUIDANCE_V_INV_M, B2_GV_ADAPT_X_FRAC);
#else #else
const int32_t inv_m = b2_gv_adapt_X>>(B2_GV_ADAPT_X_FRAC - FF_CMD_FRAC); const int32_t inv_m = b2_gv_adapt_X>>(B2_GV_ADAPT_X_FRAC - FF_CMD_FRAC);
#endif #endif
const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) - 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 0
if (g_m_zdd > 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 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 #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; int32_t cphi,ctheta,cphitheta;
PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi); PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi);
PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta); PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC; cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF; 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 #endif
/* our error command */ /* our error command */
booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) + guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) +
((-booz2_guidance_v_kd * err_zd) >> 21) + ((-guidance_v_kd * err_zd) >> 21) +
((-booz2_guidance_v_ki * booz2_guidance_v_z_sum_err) >> 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; guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
// booz2_guidance_v_delta_t = booz2_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. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ2_GUIDANCE_V #ifndef GUIDANCE_V
#define BOOZ2_GUIDANCE_V #define GUIDANCE_V
#include "std.h" #include "std.h"
#include "booz2_guidance_v_ref.h" #include "guidance_v_ref.h"
#include "booz2_guidance_v_adpt.h" #include "guidance_v_adpt.h"
#define BOOZ2_GUIDANCE_V_MODE_KILL 0 #define GUIDANCE_V_MODE_KILL 0
#define BOOZ2_GUIDANCE_V_MODE_RC_DIRECT 1 #define GUIDANCE_V_MODE_RC_DIRECT 1
#define BOOZ2_GUIDANCE_V_MODE_RC_CLIMB 2 #define GUIDANCE_V_MODE_RC_CLIMB 2
#define BOOZ2_GUIDANCE_V_MODE_CLIMB 3 #define GUIDANCE_V_MODE_CLIMB 3
#define BOOZ2_GUIDANCE_V_MODE_HOVER 4 #define GUIDANCE_V_MODE_HOVER 4
#define BOOZ2_GUIDANCE_V_MODE_NAV 5 #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 guidance_v_z_sp;
extern int32_t booz2_guidance_v_zd_sp; extern int32_t guidance_v_zd_sp;
extern int32_t booz2_guidance_v_z_ref; extern int32_t guidance_v_z_ref;
extern int32_t booz2_guidance_v_zd_ref; extern int32_t guidance_v_zd_ref;
extern int32_t booz2_guidance_v_zdd_ref; extern int32_t guidance_v_zdd_ref;
extern int32_t booz2_guidance_v_z_sum_err; extern int32_t guidance_v_z_sum_err;
extern int32_t booz2_guidance_v_ff_cmd; extern int32_t guidance_v_ff_cmd;
extern int32_t booz2_guidance_v_fb_cmd; extern int32_t guidance_v_fb_cmd;
extern int32_t booz2_guidance_v_delta_t; extern int32_t guidance_v_delta_t;
extern int32_t booz2_guidance_v_kp; extern int32_t guidance_v_kp;
extern int32_t booz2_guidance_v_kd; extern int32_t guidance_v_kd;
extern int32_t booz2_guidance_v_ki; extern int32_t guidance_v_ki;
extern void booz2_guidance_v_init(void); extern void guidance_v_init(void);
extern void booz2_guidance_v_read_rc(void); extern void guidance_v_read_rc(void);
extern void booz2_guidance_v_mode_changed(uint8_t new_mode); extern void guidance_v_mode_changed(uint8_t new_mode);
extern void booz2_guidance_v_notify_in_flight(bool_t in_flight); extern void guidance_v_notify_in_flight(bool_t in_flight);
extern void booz2_guidance_v_run(bool_t in_flight); extern void guidance_v_run(bool_t in_flight);
#define booz2_guidance_v_SetKi(_val) { \ #define guidance_v_SetKi(_val) { \
booz2_guidance_v_ki = _val; \ guidance_v_ki = _val; \
booz2_guidance_v_z_sum_err = 0; \ 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 * needed by the invert dynamic model to produce a nominal command
*/ */
#ifndef BOOZ2_GUIDANCE_V_ADPT #ifndef GUIDANCE_V_ADPT
#define BOOZ2_GUIDANCE_V_ADPT #define GUIDANCE_V_ADPT
extern int32_t b2_gv_adapt_X; extern int32_t b2_gv_adapt_X;
extern int32_t b2_gv_adapt_P; extern int32_t b2_gv_adapt_P;
extern int32_t b2_gv_adapt_Xmeas; extern int32_t b2_gv_adapt_Xmeas;
#ifdef B2_GUIDANCE_V_C #ifdef GUIDANCE_V_C
/* Our State /* Our State
Q13.18 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. * Boston, MA 02111-1307, USA.
*/ */
#ifndef BOOZ2_GUIDANCE_V_REF_H #ifndef GUIDANCE_V_REF_H
#define BOOZ2_GUIDANCE_V_REF_H #define GUIDANCE_V_REF_H
#include "airframe.h" #include "airframe.h"
#include "inttypes.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) #define B2_GV_Z_REF_FRAC (B2_GV_ZD_REF_FRAC + B2_GV_FREQ_FRAC)
/* Saturations definition */ /* Saturations definition */
#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZDD #ifndef GUIDANCE_V_REF_MIN_ZDD
#define BOOZ2_GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81) #define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
#endif #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 #ifndef GUIDANCE_V_REF_MAX_ZDD
#define BOOZ2_GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81) #define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
#endif #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 #ifndef GUIDANCE_V_REF_MIN_ZD
#define BOOZ2_GUIDANCE_V_REF_MIN_ZD (-3.) #define GUIDANCE_V_REF_MIN_ZD (-3.)
#endif #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 #ifndef GUIDANCE_V_REF_MAX_ZD
#define BOOZ2_GUIDANCE_V_REF_MAX_ZD ( 3.) #define GUIDANCE_V_REF_MAX_ZD ( 3.)
#endif #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 */ /* second order model natural frequency and damping */
#ifndef BOOZ2_GUIDANCE_V_REF_OMEGA #ifndef GUIDANCE_V_REF_OMEGA
#define BOOZ2_GUIDANCE_V_REF_OMEGA RadOfDeg(100.) #define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
#endif #endif
#ifndef BOOZ2_GUIDANCE_V_REF_ZETA #ifndef GUIDANCE_V_REF_ZETA
#define BOOZ2_GUIDANCE_V_REF_ZETA 0.85 #define GUIDANCE_V_REF_ZETA 0.85
#endif #endif
#define B2_GV_ZETA_OMEGA_FRAC 10 #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_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 */ /* first order time constant */
#define B2_GV_REF_THAU_F 0.25 #define B2_GV_REF_THAU_F 0.25
#define B2_GV_REF_INV_THAU_FRAC 16 #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) #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_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_z_sp(int32_t z_sp);
static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_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 "autopilot.h"
#include "booz_stabilization.h" #include "booz_stabilization.h"
#include "booz_guidance.h" #include "guidance.h"
#include "ahrs.h" #include "ahrs.h"
#include "ins.h" #include "ins.h"
@@ -115,8 +115,8 @@ STATIC_INLINE void main_init( void ) {
booz_fms_init(); booz_fms_init();
autopilot_init(); autopilot_init();
booz2_nav_init(); booz2_nav_init();
booz2_guidance_h_init(); guidance_h_init();
booz2_guidance_v_init(); guidance_v_init();
booz_stabilization_init(); booz_stabilization_init();
ahrs_aligner_init(); ahrs_aligner_init();

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