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 31 Packages Projects Releases Wiki Activity

rename booz_stabilization to stabilization

Browse Source
This commit is contained in:
Felix Ruess
2010-09-28 15:47:38 +00:00
parent e68d6b372e
commit 963450f6a0
60 changed files with 718 additions and 728 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/blender.xml
+2 -2
View File
@@ -87,7 +87,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -99,7 +99,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+2 -2
View File
@@ -83,7 +83,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -95,7 +95,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/ENAC/quadrotor/mkk1.xml
+3 -3
View File
@@ -121,7 +121,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -133,7 +133,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -146,7 +146,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+2 -2
View File
@@ -70,7 +70,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -82,7 +82,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
+2 -2
View File
@@ -70,7 +70,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -83,7 +83,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a1.xml
+2 -2
View File
@@ -84,7 +84,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -97,7 +97,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a1p.xml
+2 -2
View File
@@ -82,7 +82,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -95,7 +95,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a2.xml
+2 -2
View File
@@ -71,7 +71,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -83,7 +83,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a3.xml
+2 -2
View File
@@ -58,7 +58,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -70,7 +70,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a4.xml
+2 -2
View File
@@ -60,7 +60,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -72,7 +72,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<define name="SP_MAX_PHI" value="3000"/>
<define name="SP_MAX_THETA" value="6000"/>
conf/airframes/Poine/booz2_a5.xml
+2 -2
View File
@@ -81,7 +81,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -93,7 +93,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a6.xml
+2 -2
View File
@@ -82,7 +82,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -94,7 +94,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/Poine/booz2_a7.xml
+2 -2
View File
@@ -81,7 +81,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -93,7 +93,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/UofAdelaide/A1000_BOOZ.xml
+2 -2
View File
@@ -138,7 +138,7 @@ second attempt
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -171,7 +171,7 @@ second attempt
<define name="GAIN_R" value="-350"/> -->
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/UofAdelaide/A1000_LISA.xml
+2 -2
View File
@@ -69,7 +69,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -81,7 +81,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/UofAdelaide/A1000_NOVA.xml
+2 -2
View File
@@ -68,7 +68,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -93,7 +93,7 @@
<define name="DDGAIN_R" value="300" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+2 -2
View File
@@ -68,7 +68,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -93,7 +93,7 @@
<define name="DDGAIN_R" value="300" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/UofAdelaide/booz2_a1000.xml
+2 -2
View File
@@ -137,7 +137,7 @@ second attempt
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -171,7 +171,7 @@ second attempt
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/UofAdelaide/lisa_a1000.xml
+2 -2
View File
@@ -77,7 +77,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -89,7 +89,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_Aron.xml
+2 -2
View File
@@ -77,7 +77,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -89,7 +89,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_NoVa.xml
+2 -2
View File
@@ -68,7 +68,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -93,7 +93,7 @@
<define name="DDGAIN_R" value="300" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_NoVa_001.xml
+2 -2
View File
@@ -68,7 +68,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -93,7 +93,7 @@
<define name="DDGAIN_R" value="300" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_NoVa_002.xml
+2 -2
View File
@@ -68,7 +68,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -93,7 +93,7 @@
<define name="DDGAIN_R" value="300" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_flixr.xml
+2 -2
View File
@@ -90,7 +90,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -116,7 +116,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)" />
conf/airframes/booz2_ppzuav.xml
+2 -2
View File
@@ -53,7 +53,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000" />
<define name="SP_MAX_Q" value="10000" />
@@ -78,7 +78,7 @@
<define name="DDGAIN_R" value="300" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_s1.xml
+2 -2
View File
@@ -72,7 +72,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -85,7 +85,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/booz2_x1.xml
+2 -2
View File
@@ -83,7 +83,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -96,7 +96,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/airframes/esden/lisa_asctec.xml
+2 -2
View File
@@ -77,7 +77,7 @@
</section>
<section name="STABILIZATION_RATE" prefix="BOOZ_STABILIZATION_RATE_">
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
@@ -89,7 +89,7 @@
</section>
<section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="RadOfDeg(45.)"/>
conf/autopilot/mercury.makefile
+6 -6
View File
@@ -76,14 +76,14 @@ ap.CFLAGS += -DXSENS1_LINK=Uart0 -DIMU_TYPE_H=\"mercury_xsens.h\"
ap.srcs += $(SRC_BOOZ)/ahrs/ahrs_cmpl_euler.c $(SRC_BOOZ)/ahrs/ahrs_aligner.c
ap.CFLAGS += -DFLOAT_T=float
ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
ap.srcs += $(SRC_FIRMWARE)/stabilization.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
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 += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
# AHI copied from booz w/ light modifications for vertical control
conf/autopilot/rotorcraft.makefile
+6 -6
View File
@@ -181,15 +181,15 @@ endif
ap.srcs += $(SRC_FIRMWARE)/autopilot.c
ap.srcs += math/pprz_trig_int.c
ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
ap.srcs += $(SRC_FIRMWARE)/stabilization.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
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 += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
ap.CFLAGS += -DUSE_NAVIGATION
ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
+16 -16
View File
@@ -97,8 +97,8 @@ sim.srcs += $(SRC_FIRMWARE)/autopilot.c
# include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
#
sim.srcs += $(SRC_BOOZ)/booz_stabilization.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
sim.srcs += $(SRC_FIRMWARE)/stabilization.c
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
NUM_TYPE=integer
#NUM_TYPE=float
@@ -108,27 +108,27 @@ STAB_TYPE=euler
ifeq ($(NUM_TYPE), integer)
sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ifeq ($(STAB_TYPE), euler)
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
else ifeq ($(STAB_TYPE), quaternion)
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_int.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_quat_int.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_int.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
endif
else ifeq ($(NUM_TYPE), float)
sim.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_FLOAT
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_float.h\"
sim.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_float.h\"
ifeq ($(STAB_TYPE), euler)
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_float.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_float.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_float.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_float.h\"
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_float.c
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_float.c
else ifeq ($(STAB_TYPE), quaternion)
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_float.h\"
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_quat_float.c
sim.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_quat_float.c
sim.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_float.h\"
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_float.c
sim.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_float.c
endif
endif
conf/settings/settings_booz2.xml
+15 -15
View File
@@ -22,25 +22,25 @@
</dl_settings>
<dl_settings NAME="Rate Loop">
<dl_setting var="booz_stabilization_rate_gain.p" min="-1000" step="1" max="-1" module="stabilization/booz_stabilization_rate" shortname="gain p"/>
<dl_setting var="booz_stabilization_rate_gain.q" min="-1000" step="1" max="-1" module="stabilization/booz_stabilization_rate" shortname="gain q"/>
<dl_setting var="booz_stabilization_rate_gain.r" min="-1000" step="1" max="-1" module="stabilization/booz_stabilization_rate" shortname="gain r"/>
<dl_setting var="stabilization_rate_gain.p" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain p"/>
<dl_setting var="stabilization_rate_gain.q" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain q"/>
<dl_setting var="stabilization_rate_gain.r" min="-1000" step="1" max="-1" module="stabilization/stabilization_rate" shortname="gain r"/>
</dl_settings>
<dl_settings NAME="Att Loop">
<dl_setting var="booz_stabilization_gains.p.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain phi" />
<dl_setting var="booz_stabilization_gains.d.x" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain p"/>
<dl_setting var="booz_stabilization_gains.i.x" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain phi" handler="SetKiPhi"/>
<dl_setting var="booz_stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/booz_stabilization_attitude" shortname="ddgain p"/>
<dl_setting var="booz_stabilization_gains.p.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain theta"/>
<dl_setting var="booz_stabilization_gains.d.y" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain q"/>
<dl_setting var="booz_stabilization_gains.i.y" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain theta"/>
<dl_setting var="booz_stabilization_gains.dd.y" min="0" step="1" max="500" module="stabilization/booz_stabilization_attitude" shortname="ddgain q"/>
<dl_setting var="booz_stabilization_gains.p.z" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="pgain psi"/>
<dl_setting var="booz_stabilization_gains.d.z" min="-4000" step="1" max="-1" module="stabilization/booz_stabilization_attitude" shortname="dgain r"/>
<dl_setting var="booz_stabilization_gains.i.z" min="-300" step="1" max="0" module="stabilization/booz_stabilization_attitude" shortname="igain psi"/>
<dl_setting var="booz_stabilization_gains.dd.z" min="0" step="1" max="2000" module="stabilization/booz_stabilization_attitude" shortname="ddgain r"/>
<dl_setting var="stabilization_gains.p.x" min="-4000" step="1" max="-1" module="stabilization/stabilization_attitude" shortname="pgain phi" />
<dl_setting var="stabilization_gains.d.x" min="-4000" step="1" max="-1" module="stabilization/stabilization_attitude" shortname="dgain p"/>
<dl_setting var="stabilization_gains.i.x" min="-300" step="1" max="0" module="stabilization/stabilization_attitude" shortname="igain phi" handler="SetKiPhi"/>
<dl_setting var="stabilization_gains.dd.x" min="0" step="1" max="1000" module="stabilization/stabilization_attitude" shortname="ddgain p"/>
<dl_setting var="stabilization_gains.p.y" min="-4000" step="1" max="-1" module="stabilization/stabilization_attitude" shortname="pgain theta"/>
<dl_setting var="stabilization_gains.d.y" min="-4000" step="1" max="-1" module="stabilization/stabilization_attitude" shortname="dgain q"/>
<dl_setting var="stabilization_gains.i.y" min="-300" step="1" max="0" module="stabilization/stabilization_attitude" shortname="igain theta"/>
<dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="500" module="stabilization/stabilization_attitude" shortname="ddgain q"/>
<dl_setting var="stabilization_gains.p.z" min="-4000" step="1" max="-1" module="stabilization/stabilization_attitude" shortname="pgain psi"/>
<dl_setting var="stabilization_gains.d.z" min="-4000" step="1" max="-1" module="stabilization/stabilization_attitude" shortname="dgain r"/>
<dl_setting var="stabilization_gains.i.z" min="-300" step="1" max="0" module="stabilization/stabilization_attitude" shortname="igain psi"/>
<dl_setting var="stabilization_gains.dd.z" min="0" step="1" max="2000" module="stabilization/stabilization_attitude" shortname="ddgain r"/>
</dl_settings>
<dl_settings NAME="Vert Loop">
sw/airborne/booz/booz2_telemetry.h
+53 -53
View File
@@ -188,28 +188,28 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#define PERIODIC_SEND_BOOZ2_RATE_LOOP(_chan) { \
DOWNLINK_SEND_BOOZ2_RATE_LOOP(_chan, \
&booz_stabilization_rate_sp.p, \
&booz_stabilization_rate_sp.q, \
&booz_stabilization_rate_sp.r, \
&booz_stabilization_rate_ref.p, \
&booz_stabilization_rate_ref.q, \
&booz_stabilization_rate_ref.r, \
&booz_stabilization_rate_refdot.p, \
&booz_stabilization_rate_refdot.q, \
&booz_stabilization_rate_refdot.r, \
&booz_stabilization_rate_sum_err.p, \
&booz_stabilization_rate_sum_err.q, \
&booz_stabilization_rate_sum_err.r, \
&booz_stabilization_rate_ff_cmd.p, \
&booz_stabilization_rate_ff_cmd.q, \
&booz_stabilization_rate_ff_cmd.r, \
&booz_stabilization_rate_fb_cmd.p, \
&booz_stabilization_rate_fb_cmd.q, \
&booz_stabilization_rate_fb_cmd.r, \
&booz_stabilization_cmd[COMMAND_THRUST]); \
&stabilization_rate_sp.p, \
&stabilization_rate_sp.q, \
&stabilization_rate_sp.r, \
&stabilization_rate_ref.p, \
&stabilization_rate_ref.q, \
&stabilization_rate_ref.r, \
&stabilization_rate_refdot.p, \
&stabilization_rate_refdot.q, \
&stabilization_rate_refdot.r, \
&stabilization_rate_sum_err.p, \
&stabilization_rate_sum_err.q, \
&stabilization_rate_sum_err.r, \
&stabilization_rate_ff_cmd.p, \
&stabilization_rate_ff_cmd.q, \
&stabilization_rate_ff_cmd.r, \
&stabilization_rate_fb_cmd.p, \
&stabilization_rate_fb_cmd.q, \
&stabilization_rate_fb_cmd.r, \
&stabilization_cmd[COMMAND_THRUST]); \
}
#ifdef STABILISATION_ATTITUDE_TYPE_INT
@@ -224,18 +224,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&booz_stab_att_sp_euler.phi, \
&booz_stab_att_sp_euler.theta, \
&booz_stab_att_sp_euler.psi, \
&booz_stabilization_att_sum_err.phi, \
&booz_stabilization_att_sum_err.theta, \
&booz_stabilization_att_sum_err.psi, \
&booz_stabilization_att_fb_cmd[COMMAND_ROLL], \
&booz_stabilization_att_fb_cmd[COMMAND_PITCH], \
&booz_stabilization_att_fb_cmd[COMMAND_YAW], \
&booz_stabilization_att_ff_cmd[COMMAND_ROLL], \
&booz_stabilization_att_ff_cmd[COMMAND_PITCH], \
&booz_stabilization_att_ff_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW]); \
&stabilization_att_sum_err.phi, \
&stabilization_att_sum_err.theta, \
&stabilization_att_sum_err.psi, \
&stabilization_att_fb_cmd[COMMAND_ROLL], \
&stabilization_att_fb_cmd[COMMAND_PITCH], \
&stabilization_att_fb_cmd[COMMAND_YAW], \
&stabilization_att_ff_cmd[COMMAND_ROLL], \
&stabilization_att_ff_cmd[COMMAND_PITCH], \
&stabilization_att_ff_cmd[COMMAND_YAW], \
&stabilization_cmd[COMMAND_ROLL], \
&stabilization_cmd[COMMAND_PITCH], \
&stabilization_cmd[COMMAND_YAW]); \
}
@@ -268,18 +268,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&booz_stab_att_ref_euler.phi, \
&booz_stab_att_ref_euler.theta, \
&booz_stab_att_ref_euler.psi, \
&booz_stabilization_att_sum_err.phi, \
&booz_stabilization_att_sum_err.theta, \
&booz_stabilization_att_sum_err.psi, \
&booz_stabilization_att_fb_cmd[COMMAND_ROLL], \
&booz_stabilization_att_fb_cmd[COMMAND_PITCH], \
&booz_stabilization_att_fb_cmd[COMMAND_YAW], \
&booz_stabilization_att_ff_cmd[COMMAND_ROLL], \
&booz_stabilization_att_ff_cmd[COMMAND_PITCH], \
&booz_stabilization_att_ff_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW]); \
&stabilization_att_sum_err.phi, \
&stabilization_att_sum_err.theta, \
&stabilization_att_sum_err.psi, \
&stabilization_att_fb_cmd[COMMAND_ROLL], \
&stabilization_att_fb_cmd[COMMAND_PITCH], \
&stabilization_att_fb_cmd[COMMAND_YAW], \
&stabilization_att_ff_cmd[COMMAND_ROLL], \
&stabilization_att_ff_cmd[COMMAND_PITCH], \
&stabilization_att_ff_cmd[COMMAND_YAW], \
&stabilization_cmd[COMMAND_ROLL], \
&stabilization_cmd[COMMAND_PITCH], \
&stabilization_cmd[COMMAND_YAW]); \
}
#define PERIODIC_SEND_BOOZ2_STAB_ATTITUDE_REF(_chan) { \
@@ -317,10 +317,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#define PERIODIC_SEND_BOOZ2_CMD(_chan) { \
DOWNLINK_SEND_BOOZ2_CMD(_chan, \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_THRUST]); \
&stabilization_cmd[COMMAND_ROLL], \
&stabilization_cmd[COMMAND_PITCH], \
&stabilization_cmd[COMMAND_YAW], \
&stabilization_cmd[COMMAND_THRUST]); \
}
@@ -677,7 +677,7 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&guidance_h_pos_sp.x, \
&carrot_up, \
&guidance_h_command_body.psi, \
&booz_stabilization_cmd[COMMAND_THRUST], \
&stabilization_cmd[COMMAND_THRUST], \
&autopilot_flight_time); \
}
@@ -748,10 +748,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&radio_control.values[RADIO_CONTROL_ROLL], \
&radio_control.values[RADIO_CONTROL_PITCH], \
&radio_control.values[RADIO_CONTROL_YAW], \
&booz_stabilization_cmd[COMMAND_ROLL], \
&booz_stabilization_cmd[COMMAND_PITCH], \
&booz_stabilization_cmd[COMMAND_YAW], \
&booz_stabilization_cmd[COMMAND_THRUST], \
&stabilization_cmd[COMMAND_ROLL], \
&stabilization_cmd[COMMAND_PITCH], \
&stabilization_cmd[COMMAND_YAW], \
&stabilization_cmd[COMMAND_THRUST], \
&ahrs.ltp_to_imu_euler.phi, \
&ahrs.ltp_to_imu_euler.theta, \
&ahrs.ltp_to_imu_euler.psi, \
sw/airborne/csc/mercury_ap.c
+9 -9
View File
@@ -27,8 +27,8 @@
#include "commands.h"
#include "mercury_xsens.h"
#include <firmwares/rotorcraft/autopilot.h>
#include "booz_stabilization.h"
#include "stabilization/booz_stabilization_attitude.h"
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/stabilization/stabilization_attitude.h>
#include "led.h"
#include "math/pprz_algebra_float.h"
#include "string.h"
@@ -148,14 +148,14 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
if(kill) booz2_autopilot_motors_on = FALSE;
booz2_autopilot_in_flight = _in_flight;
booz_stabilization_attitude_read_rc(booz2_autopilot_in_flight);
booz_stabilization_attitude_run(booz2_autopilot_in_flight);
stabilization_attitude_read_rc(booz2_autopilot_in_flight);
stabilization_attitude_run(booz2_autopilot_in_flight);
booz2_guidance_v_run(booz2_autopilot_in_flight);
booz_stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 105 / 7200 + 95;
stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 105 / 7200 + 95;
CscSetCommands(booz_stabilization_cmd,
CscSetCommands(stabilization_cmd,
booz2_autopilot_in_flight,booz2_autopilot_motors_on);
@@ -163,9 +163,9 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
if(booz2_autopilot_motors_on && props_throttle_pass){
Bound(booz_stabilization_cmd[COMMAND_THRUST],0,255);
Bound(stabilization_cmd[COMMAND_THRUST],0,255);
for(uint8_t i = 0; i < PROPS_NB; i++)
mixed_commands[i] = booz_stabilization_cmd[COMMAND_THRUST];
mixed_commands[i] = stabilization_cmd[COMMAND_THRUST];
}
@@ -180,7 +180,7 @@ void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
MERCURY_SURFACES_SUPERVISION_RUN(Actuator,
booz_stabilization_cmd,
stabilization_cmd,
mixed_commands,
(!booz2_autopilot_in_flight));
ActuatorsCommit();
sw/airborne/csc/mercury_main.c
+3 -3
View File
@@ -53,7 +53,7 @@
#include "csc_baro.h"
#include "booz_radio_control.h"
#include "booz/stabilization/booz_stabilization_attitude.h"
#include "booz/stabilization/stabilization_attitude.h"
extern uint8_t vsupply;
@@ -119,7 +119,7 @@ static inline void csc_main_init( void ) {
xsens_init();
booz_stabilization_attitude_init();
stabilization_attitude_init();
booz2_guidance_v_init();
booz_ins_init();
@@ -138,7 +138,7 @@ static inline void csc_main_init( void ) {
csc_ap_init();
int_enable();
booz_stabilization_attitude_enter();
stabilization_attitude_enter();
}
sw/airborne/firmwares/rotorcraft/autopilot.c
+2 -2
View File
@@ -28,7 +28,7 @@
#include "booz2_commands.h"
#include "booz2_navigation.h"
#include <firmwares/rotorcraft/guidance.h>
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include "led.h"
uint8_t autopilot_mode;
@@ -89,7 +89,7 @@ void autopilot_periodic(void) {
else {
guidance_v_run( autopilot_in_flight );
guidance_h_run( autopilot_in_flight );
SetCommands(booz_stabilization_cmd,
SetCommands(stabilization_cmd,
autopilot_in_flight, autopilot_motors_on);
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+14 -14
View File
@@ -26,7 +26,7 @@
#include <firmwares/rotorcraft/guidance/guidance_h.h>
#include <firmwares/rotorcraft/ahrs.h>
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include "booz_fms.h"
#include <firmwares/rotorcraft/ins.h>
#include "booz2_navigation.h"
@@ -101,7 +101,7 @@ void guidance_h_mode_changed(uint8_t new_mode) {
switch ( guidance_h_mode ) {
// case GUIDANCE_H_MODE_RATE:
// booz_stabilization_rate_exit();
// stabilization_rate_exit();
// break;
default:
break;
@@ -110,11 +110,11 @@ void guidance_h_mode_changed(uint8_t new_mode) {
switch (new_mode) {
case GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_enter();
stabilization_rate_enter();
break;
case GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_enter();
stabilization_attitude_enter();
break;
case GUIDANCE_H_MODE_HOVER:
@@ -138,20 +138,20 @@ void guidance_h_read_rc(bool_t in_flight) {
switch ( guidance_h_mode ) {
case GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_read_rc();
stabilization_rate_read_rc();
break;
case GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_read_rc(in_flight);
stabilization_attitude_read_rc(in_flight);
break;
case GUIDANCE_H_MODE_HOVER:
BOOZ_STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
break;
case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RADIO_CONTROL_OK) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight);
guidance_h_rc_sp.psi = 0;
}
else {
@@ -169,16 +169,16 @@ void guidance_h_run(bool_t in_flight) {
switch ( guidance_h_mode ) {
case GUIDANCE_H_MODE_RATE:
booz_stabilization_rate_run(in_flight);
stabilization_rate_run(in_flight);
break;
case GUIDANCE_H_MODE_ATTITUDE:
booz_stabilization_attitude_run(in_flight);
stabilization_attitude_run(in_flight);
break;
case GUIDANCE_H_MODE_HOVER:
guidance_h_hover_run();
booz_stabilization_attitude_run(in_flight);
stabilization_attitude_run(in_flight);
break;
case GUIDANCE_H_MODE_NAV:
@@ -202,7 +202,7 @@ void guidance_h_run(bool_t in_flight) {
//guidance_h_hover_run();
guidance_h_nav_run(in_flight);
}
booz_stabilization_attitude_run(in_flight);
stabilization_attitude_run(in_flight);
break;
}
default:
@@ -372,7 +372,7 @@ static inline void guidance_h_hover_enter(void) {
VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos);
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( guidance_h_rc_sp );
STABILIZATION_ATTITUDE_RESET_PSI_REF( guidance_h_rc_sp );
INT_VECT2_ZERO(guidance_h_pos_err_sum);
@@ -388,7 +388,7 @@ static inline void guidance_h_nav_enter(void) {
GuidanceHSetRef(pos, speed, zero);
struct Int32Eulers tmp_sp;
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );
STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp );
guidance_h_psi_sp = tmp_sp.psi;
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
nav_heading = (guidance_h_psi_sp >> (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+8 -8
View File
@@ -27,7 +27,7 @@
#include "booz_radio_control.h"
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/ahrs.h>
#include "booz_fms.h"
#include "booz2_navigation.h"
@@ -149,7 +149,7 @@ void guidance_v_run(bool_t in_flight) {
// AKA SUPERVISION and co
if (in_flight) {
// we should use something after the supervision!!! fuck!!!
int32_t cmd_hack = Chop(booz_stabilization_cmd[COMMAND_THRUST], 1, 200);
int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], 1, 200);
gv_adapt_run(ins_ltp_accel.z, cmd_hack);
}
else {
@@ -162,14 +162,14 @@ void guidance_v_run(bool_t in_flight) {
case GUIDANCE_V_MODE_RC_DIRECT:
guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that
GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should take care of it ?
booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
break;
case GUIDANCE_V_MODE_RC_CLIMB:
guidance_v_zd_sp = guidance_v_rc_zd_sp;
gv_update_ref_from_zd_sp(guidance_v_zd_sp);
run_hover_loop(in_flight);
booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break;
case GUIDANCE_V_MODE_CLIMB:
@@ -180,7 +180,7 @@ void guidance_v_run(bool_t in_flight) {
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( guidance_v_rc_delta_t, guidance_v_delta_t);
stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t);
break;
case GUIDANCE_V_MODE_HOVER:
@@ -191,7 +191,7 @@ void guidance_v_run(bool_t in_flight) {
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( guidance_v_rc_delta_t, guidance_v_delta_t);
stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t);
break;
case GUIDANCE_V_MODE_NAV:
@@ -213,9 +213,9 @@ void guidance_v_run(bool_t in_flight) {
}
/* use rc limitation if available */
if (radio_control.status == RADIO_CONTROL_OK)
booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t);
stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t);
else
booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break;
}
default:
sw/airborne/firmwares/rotorcraft/main.c
+2 -2
View File
@@ -49,7 +49,7 @@
#include "booz_fms.h"
#include <firmwares/rotorcraft/autopilot.h>
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/guidance.h>
#include <firmwares/rotorcraft/ahrs.h>
@@ -117,7 +117,7 @@ STATIC_INLINE void main_init( void ) {
booz2_nav_init();
guidance_h_init();
guidance_v_init();
booz_stabilization_init();
stabilization_init();
ahrs_aligner_init();
ahrs_init();
sw/airborne/firmwares/rotorcraft/stabilization.c
+6 -8
View File
@@ -21,16 +21,14 @@
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
int32_t booz_stabilization_cmd[COMMANDS_NB];
int32_t stabilization_cmd[COMMANDS_NB];
void booz_stabilization_init(void) {
#ifndef BOOZ_STABILIZATION_SKIP_RATE
booz_stabilization_rate_init();
void stabilization_init(void) {
#ifndef STABILIZATION_SKIP_RATE
stabilization_rate_init();
#endif
booz_stabilization_attitude_init();
stabilization_attitude_init();
}
sw/airborne/firmwares/rotorcraft/stabilization.h
+7 -7
View File
@@ -21,18 +21,18 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_H
#define BOOZ_STABILIZATION_H
#ifndef STABILIZATION_H
#define STABILIZATION_H
#include "std.h"
#include "airframe.h"
#include "stabilization/booz_stabilization_rate.h"
#include "stabilization/booz_stabilization_attitude.h"
#include <firmwares/rotorcraft/stabilization/stabilization_rate.h>
#include <firmwares/rotorcraft/stabilization/stabilization_attitude.h>
extern void booz_stabilization_init(void);
extern void stabilization_init(void);
extern int32_t booz_stabilization_cmd[COMMANDS_NB];
extern int32_t stabilization_cmd[COMMANDS_NB];
#endif /* BOOZ_STABILIZATION_H */
#endif /* STABILIZATION_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+16 -17
View File
@@ -1,7 +1,6 @@
/*
* $Id$
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -19,29 +18,29 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_H
#define BOOZ_STABILIZATION_ATTITUDE_H
#ifndef STABILIZATION_ATTITUDE_H
#define STABILIZATION_ATTITUDE_H
#include STABILISATION_ATTITUDE_H
extern void booz_stabilization_attitude_init(void);
extern void booz_stabilization_attitude_read_rc(bool_t in_flight);
extern void booz_stabilization_attitude_read_beta_vane(float beta);
extern void booz_stabilization_attitude_read_alpha_vane(float alpha);
extern void booz_stabilization_attitude_enter(void);
extern void booz_stabilization_attitude_run(bool_t in_flight);
extern void stabilization_attitude_init(void);
extern void stabilization_attitude_read_rc(bool_t in_flight);
extern void stabilization_attitude_read_beta_vane(float beta);
extern void stabilization_attitude_read_alpha_vane(float alpha);
extern void stabilization_attitude_enter(void);
extern void stabilization_attitude_run(bool_t in_flight);
#include "stabilization/booz_stabilization_attitude_ref.h"
#include "stabilization/stabilization_attitude_ref.h"
#include STABILISATION_ATTITUDE_REF_H
extern void booz_stabilization_attitude_ref_init(void);
extern void booz_stabilization_attitude_ref_update(void);
extern void stabilization_attitude_ref_init(void);
extern void stabilization_attitude_ref_update(void);
#define booz_stabilization_attitude_SetKiPhi(_val) { \
booz_stabilization_gains.i.x = _val; \
booz_stabilization_att_sum_err.phi = 0; \
#define stabilization_attitude_SetKiPhi(_val) { \
stabilization_gains.i.x = _val; \
stabilization_att_sum_err.phi = 0; \
}
#endif /* BOOZ2_STABILIZATION_ATTITUDE_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+66 -68
View File
@@ -1,6 +1,6 @@
/*
* $Id: booz_stabilization_attitude_euler.c 3795 2009-07-24 23:43:02Z poine $
*
* $Id: stabilization_attitude_euler.c 3795 2009-07-24 23:43:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,10 +18,10 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include "math/pprz_algebra_float.h"
#include <firmwares/rotorcraft/ahrs.h>
@@ -30,70 +30,70 @@
#include "airframe.h"
struct FloatAttitudeGains booz_stabilization_gains;
struct FloatEulers booz_stabilization_att_sum_err;
struct FloatAttitudeGains stabilization_gains;
struct FloatEulers stabilization_att_sum_err;
float booz_stabilization_att_fb_cmd[COMMANDS_NB];
float booz_stabilization_att_ff_cmd[COMMANDS_NB];
float stabilization_att_fb_cmd[COMMANDS_NB];
float stabilization_att_ff_cmd[COMMANDS_NB];
void booz_stabilization_attitude_init(void) {
void stabilization_attitude_init(void) {
booz_stabilization_attitude_ref_init();
stabilization_attitude_ref_init();
VECT3_ASSIGN(booz_stabilization_gains.p,
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(booz_stabilization_gains.d,
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(booz_stabilization_gains.i,
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(booz_stabilization_gains.dd,
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
}
void booz_stabilization_attitude_read_rc(bool_t in_flight) {
void stabilization_attitude_read_rc(bool_t in_flight) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
}
void booz_stabilization_attitude_enter(void) {
void stabilization_attitude_enter(void) {
STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
FLOAT_EULERS_ZERO( stabilization_att_sum_err );
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
#define MAX_SUM_ERR RadOfDeg(56000)
void booz_stabilization_attitude_run(bool_t in_flight) {
void stabilization_attitude_run(bool_t in_flight) {
booz_stabilization_attitude_ref_update();
stabilization_attitude_ref_update();
/* Compute feedforward */
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
booz_stabilization_gains.dd.x * booz_stab_att_ref_accel.p / 32.;
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
booz_stabilization_gains.dd.y * booz_stab_att_ref_accel.q / 32.;
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
booz_stabilization_gains.dd.z * booz_stab_att_ref_accel.r / 32.;
stabilization_att_ff_cmd[COMMAND_ROLL] =
stabilization_gains.dd.x * booz_stab_att_ref_accel.p / 32.;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * booz_stab_att_ref_accel.q / 32.;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * booz_stab_att_ref_accel.r / 32.;
/* Compute feedback */
/* attitude error */
@@ -105,13 +105,13 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
if (in_flight) {
/* update integrator */
EULERS_ADD(booz_stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
FLOAT_EULERS_ZERO(booz_stabilization_att_sum_err);
FLOAT_EULERS_ZERO(stabilization_att_sum_err);
}
/* rate error */
struct FloatRates rate_float;
RATES_FLOAT_OF_BFP(rate_float, ahrs.body_rate);
@@ -120,29 +120,27 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
/* PID */
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
booz_stabilization_gains.p.x * att_err.phi +
booz_stabilization_gains.d.x * rate_err.p +
booz_stabilization_gains.i.x * booz_stabilization_att_sum_err.phi / 1024.;
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
stabilization_gains.i.x * stabilization_att_sum_err.phi / 1024.;
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
booz_stabilization_gains.p.y * att_err.theta +
booz_stabilization_gains.d.y * rate_err.q +
booz_stabilization_gains.i.y * booz_stabilization_att_sum_err.theta / 1024.;
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
stabilization_gains.i.y * stabilization_att_sum_err.theta / 1024.;
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
booz_stabilization_gains.p.z * att_err.psi +
booz_stabilization_gains.d.z * rate_err.r +
booz_stabilization_gains.i.z * booz_stabilization_att_sum_err.psi / 1024.;
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
stabilization_gains.i.z * stabilization_att_sum_err.psi / 1024.;
booz_stabilization_cmd[COMMAND_ROLL] =
(booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
booz_stabilization_cmd[COMMAND_PITCH] =
(booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
booz_stabilization_cmd[COMMAND_YAW] =
(booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW])/16.;
stabilization_cmd[COMMAND_ROLL] =
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
stabilization_cmd[COMMAND_PITCH] =
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
stabilization_cmd[COMMAND_YAW] =
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+72 -74
View File
@@ -1,6 +1,6 @@
/*
* $Id$
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,10 +18,10 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/ahrs.h>
#include "booz_radio_control.h"
@@ -29,77 +29,77 @@
#include "airframe.h"
struct Int32AttitudeGains booz_stabilization_gains;
struct Int32AttitudeGains stabilization_gains;
struct Int32Eulers booz_stabilization_att_sum_err;
struct Int32Eulers stabilization_att_sum_err;
int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
int32_t stabilization_att_fb_cmd[COMMANDS_NB];
int32_t stabilization_att_ff_cmd[COMMANDS_NB];
void booz_stabilization_attitude_init(void) {
void stabilization_attitude_init(void) {
booz_stabilization_attitude_ref_init();
VECT3_ASSIGN(booz_stabilization_gains.p,
BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(booz_stabilization_gains.d,
BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(booz_stabilization_gains.i,
BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN);
stabilization_attitude_ref_init();
VECT3_ASSIGN(booz_stabilization_gains.dd,
BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN,
BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN);
INT_EULERS_ZERO( booz_stabilization_att_sum_err );
VECT3_ASSIGN(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
INT_EULERS_ZERO( stabilization_att_sum_err );
}
void booz_stabilization_attitude_read_rc(bool_t in_flight) {
void stabilization_attitude_read_rc(bool_t in_flight) {
BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
STABILIZATION_ATTITUDE_READ_RC(booz_stab_att_sp_euler, in_flight);
}
void booz_stabilization_attitude_enter(void) {
void stabilization_attitude_enter(void) {
STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
INT_EULERS_ZERO( stabilization_att_sum_err );
BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz_stab_att_sp_euler );
INT_EULERS_ZERO( booz_stabilization_att_sum_err );
}
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
#define MAX_SUM_ERR 4000000
void booz_stabilization_attitude_run(bool_t in_flight) {
void stabilization_attitude_run(bool_t in_flight) {
/* update reference */
booz_stabilization_attitude_ref_update();
stabilization_attitude_ref_update();
/* compute feedforward command */
booz_stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(booz_stabilization_gains.dd.x * booz_stab_att_ref_accel.p, 5);
booz_stabilization_att_ff_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND(booz_stabilization_gains.dd.y * booz_stab_att_ref_accel.q, 5);
booz_stabilization_att_ff_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND(booz_stabilization_gains.dd.z * booz_stab_att_ref_accel.r, 5);
stabilization_att_ff_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND(stabilization_gains.dd.x * booz_stab_att_ref_accel.p, 5);
stabilization_att_ff_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND(stabilization_gains.dd.y * booz_stab_att_ref_accel.q, 5);
stabilization_att_ff_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND(stabilization_gains.dd.z * booz_stab_att_ref_accel.r, 5);
/* compute feedback command */
/* attitude error */
@@ -113,13 +113,13 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
if (in_flight) {
/* update integrator */
EULERS_ADD(booz_stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(booz_stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
INT_EULERS_ZERO(booz_stabilization_att_sum_err);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
/* rate error */
const struct Int32Rates rate_ref_scaled = {
OFFSET_AND_ROUND(booz_stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),
@@ -129,31 +129,29 @@ void booz_stabilization_attitude_run(bool_t in_flight) {
RATES_DIFF(rate_err, ahrs.body_rate, rate_ref_scaled);
/* PID */
booz_stabilization_att_fb_cmd[COMMAND_ROLL] =
booz_stabilization_gains.p.x * att_err.phi +
booz_stabilization_gains.d.x * rate_err.p +
OFFSET_AND_ROUND2((booz_stabilization_gains.i.x * booz_stabilization_att_sum_err.phi), 10);
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
OFFSET_AND_ROUND2((stabilization_gains.i.x * stabilization_att_sum_err.phi), 10);
booz_stabilization_att_fb_cmd[COMMAND_PITCH] =
booz_stabilization_gains.p.y * att_err.theta +
booz_stabilization_gains.d.y * rate_err.q +
OFFSET_AND_ROUND2((booz_stabilization_gains.i.y * booz_stabilization_att_sum_err.theta), 10);
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
OFFSET_AND_ROUND2((stabilization_gains.i.y * stabilization_att_sum_err.theta), 10);
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
OFFSET_AND_ROUND2((stabilization_gains.i.z * stabilization_att_sum_err.psi), 10);
booz_stabilization_att_fb_cmd[COMMAND_YAW] =
booz_stabilization_gains.p.z * att_err.psi +
booz_stabilization_gains.d.z * rate_err.r +
OFFSET_AND_ROUND2((booz_stabilization_gains.i.z * booz_stabilization_att_sum_err.psi), 10);
/* sum feedforward and feedback */
booz_stabilization_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_ROLL]+booz_stabilization_att_ff_cmd[COMMAND_ROLL]), 16);
booz_stabilization_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_PITCH]+booz_stabilization_att_ff_cmd[COMMAND_PITCH]), 16);
booz_stabilization_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND((booz_stabilization_att_fb_cmd[COMMAND_YAW]+booz_stabilization_att_ff_cmd[COMMAND_YAW]), 16);
stabilization_cmd[COMMAND_ROLL] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), 16);
stabilization_cmd[COMMAND_PITCH] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), 16);
stabilization_cmd[COMMAND_YAW] =
OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), 16);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h
+20 -21
View File
@@ -1,6 +1,6 @@
/*
* $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
*
* $Id: stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,35 +18,34 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_FLOAT_H
#define BOOZ_STABILIZATION_ATTITUDE_FLOAT_H
#ifndef STABILIZATION_ATTITUDE_FLOAT_H
#define STABILIZATION_ATTITUDE_FLOAT_H
#include "math/pprz_algebra_float.h"
#include "airframe.h"
struct FloatAttitudeGains {
struct FloatVect3 p;
struct FloatVect3 d;
struct FloatVect3 dd;
struct FloatVect3 rates_d;
struct FloatVect3 i;
struct FloatVect3 surface_p;
struct FloatVect3 surface_d;
struct FloatVect3 surface_dd;
struct FloatVect3 surface_i;
struct FloatVect3 p;
struct FloatVect3 d;
struct FloatVect3 dd;
struct FloatVect3 rates_d;
struct FloatVect3 i;
struct FloatVect3 surface_p;
struct FloatVect3 surface_d;
struct FloatVect3 surface_dd;
struct FloatVect3 surface_i;
};
extern struct FloatAttitudeGains booz_stabilization_gains[];
extern struct FloatEulers booz_stabilization_att_sum_err_eulers;
extern struct FloatAttitudeGains stabilization_gains[];
extern struct FloatEulers stabilization_att_sum_err_eulers;
extern float booz_stabilization_att_fb_cmd[COMMANDS_NB];
extern float booz_stabilization_att_ff_cmd[COMMANDS_NB];
extern float stabilization_att_fb_cmd[COMMANDS_NB];
extern float stabilization_att_ff_cmd[COMMANDS_NB];
void booz_stabilization_attitude_gain_schedule(uint8_t idx);
#endif /* BOOZ_STABILIZATION_ATTITUDE_FLOAT_H */
void stabilization_attitude_gain_schedule(uint8_t idx);
#endif /* STABILIZATION_ATTITUDE_FLOAT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h
+14 -15
View File
@@ -1,6 +1,6 @@
/*
* $Id: booz_stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
*
* $Id: stabilization_attitude.h 3794 2009-07-24 22:01:51Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,28 +18,27 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_INT_H
#define BOOZ_STABILIZATION_ATTITUDE_INT_H
#ifndef STABILIZATION_ATTITUDE_INT_H
#define STABILIZATION_ATTITUDE_INT_H
#include "math/pprz_algebra_int.h"
#include "airframe.h"
struct Int32AttitudeGains {
struct Int32Vect3 p;
struct Int32Vect3 d;
struct Int32Vect3 dd;
struct Int32Vect3 i;
struct Int32Vect3 p;
struct Int32Vect3 d;
struct Int32Vect3 dd;
struct Int32Vect3 i;
};
extern struct Int32AttitudeGains booz_stabilization_gains;
extern struct Int32Eulers booz_stabilization_att_sum_err;
extern struct Int32AttitudeGains stabilization_gains;
extern struct Int32Eulers stabilization_att_sum_err;
extern int32_t booz_stabilization_att_fb_cmd[COMMANDS_NB];
extern int32_t booz_stabilization_att_ff_cmd[COMMANDS_NB];
#endif /* BOOZ_STABILIZATION_ATTITUDE_INT_H */
extern int32_t stabilization_att_fb_cmd[COMMANDS_NB];
extern int32_t stabilization_att_ff_cmd[COMMANDS_NB];
#endif /* STABILIZATION_ATTITUDE_INT_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+90 -90
View File
@@ -1,6 +1,6 @@
/*
* $Id: booz_stabilization_attitude_quat_float.c 3787 2009-07-24 15:33:54Z poine $
*
* $Id: stabilization_attitude_quat_float.c 3787 2009-07-24 15:33:54Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,14 +18,14 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
/** \file booz_stabilization_attitude_quat_float.c
/** \file stabilization_attitude_quat_float.c
* \brief Booz quaternion attitude stabilization
*/
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
#include <stdio.h>
#include "math/pprz_algebra_float.h"
@@ -33,90 +33,90 @@
#include <firmwares/rotorcraft/ahrs.h>
#include "airframe.h"
struct FloatAttitudeGains booz_stabilization_gains[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB];
struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB];
struct FloatQuat booz_stabilization_att_sum_err_quat;
struct FloatEulers booz_stabilization_att_sum_err_eulers;
struct FloatQuat stabilization_att_sum_err_quat;
struct FloatEulers stabilization_att_sum_err_eulers;
float booz_stabilization_att_fb_cmd[COMMANDS_NB];
float booz_stabilization_att_ff_cmd[COMMANDS_NB];
float stabilization_att_fb_cmd[COMMANDS_NB];
float stabilization_att_ff_cmd[COMMANDS_NB];
static int gain_idx = BOOZ_STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
static int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT;
static const float phi_pgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN;
static const float theta_pgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN;
static const float psi_pgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN;
static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN;
static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN;
static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN;
static const float phi_dgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN;
static const float theta_dgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN;
static const float psi_dgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN;
static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN;
static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN;
static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN;
static const float phi_igain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN;
static const float theta_igain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN;
static const float psi_igain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN;
static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN;
static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN;
static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN;
static const float phi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN;
static const float theta_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN;
static const float psi_ddgain[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN;
static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN;
static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN;
static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN;
static const float phi_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN_D;
static const float theta_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN_D;
static const float psi_dgain_d[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN_D;
static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D;
static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D;
static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D;
static const float phi_pgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
static const float theta_pgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
static const float psi_pgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
static const float phi_dgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
static const float theta_dgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
static const float psi_dgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
static const float phi_igain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
static const float theta_igain_surface[] = BOOZ_STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
static const float psi_igain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
static const float phi_ddgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
static const float theta_ddgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
static const float psi_ddgain_surface[] = BOOZ_STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
#define IERROR_SCALE 1024
void booz_stabilization_attitude_init(void) {
void stabilization_attitude_init(void) {
booz_stabilization_attitude_ref_init();
stabilization_attitude_ref_init();
for (int i = 0; i < BOOZ_STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(booz_stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(booz_stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
void booz_stabilization_attitude_gain_schedule(uint8_t idx)
void stabilization_attitude_gain_schedule(uint8_t idx)
{
if (gain_idx >= BOOZ_STABILIZATION_ATTITUDE_GAIN_NB) {
// This could be bad -- Just say no.
return;
}
gain_idx = idx;
booz_stabilization_attitude_ref_schedule(idx);
if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) {
// This could be bad -- Just say no.
return;
}
gain_idx = idx;
stabilization_attitude_ref_schedule(idx);
}
void booz_stabilization_attitude_enter(void) {
void stabilization_attitude_enter(void) {
booz_stabilization_attitude_ref_enter();
FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
stabilization_attitude_ref_enter();
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
@@ -132,60 +132,60 @@ static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gain
}
static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err,
struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * -gains->p.x * att_err->qx +
GAIN_PRESCALER_D * gains->d.x * rate_err->p +
GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
GAIN_PRESCALER_D * gains->d.y * rate_err->q +
GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
fb_commands[COMMAND_YAW] =
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * -gains->p.z * att_err->qz +
GAIN_PRESCALER_D * gains->d.z * rate_err->r +
GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
fb_commands[COMMAND_ROLL_SURFACE] =
fb_commands[COMMAND_ROLL_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.x * att_err->qx +
GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
fb_commands[COMMAND_PITCH_SURFACE] =
fb_commands[COMMAND_PITCH_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.y * att_err->qy +
GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
fb_commands[COMMAND_YAW_SURFACE] =
fb_commands[COMMAND_YAW_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.z * att_err->qz +
GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
}
void booz_stabilization_attitude_run(bool_t enable_integrator) {
/*
* Update reference
*/
booz_stabilization_attitude_ref_update();
void stabilization_attitude_run(bool_t enable_integrator) {
/*
* Compute errors for feedback
* Update reference
*/
stabilization_attitude_ref_update();
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat att_err;
FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, booz_stab_att_ref_quat);
/* wrap it in the shortest direction */
FLOAT_QUAT_WRAP_SHORTEST(att_err);
FLOAT_QUAT_WRAP_SHORTEST(att_err);
/* rate error */
struct FloatRates rate_err;
@@ -199,21 +199,21 @@ void booz_stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP_INV(new_sum_err, booz_stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALISE(new_sum_err);
FLOAT_QUAT_COPY(booz_stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(booz_stabilization_att_sum_err_eulers, booz_stabilization_att_sum_err_quat);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
} else {
/* reset accumulator */
FLOAT_QUAT_ZERO( booz_stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( booz_stabilization_att_sum_err_eulers );
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
}
attitude_run_ff(booz_stabilization_att_ff_cmd, &booz_stabilization_gains[gain_idx], &booz_stab_att_ref_accel);
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &booz_stab_att_ref_accel);
attitude_run_fb(booz_stabilization_att_fb_cmd, &booz_stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &booz_stabilization_att_sum_err_quat);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat);
for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) {
booz_stabilization_cmd[i] = booz_stabilization_att_fb_cmd[i]+booz_stabilization_att_ff_cmd[i];
stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i];
}
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h
+3 -3
View File
@@ -1,5 +1,5 @@
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_H
#ifndef STABILIZATION_ATTITUDE_REF_H
#define STABILIZATION_ATTITUDE_REF_H
#define SATURATE_SPEED_TRIM_ACCEL() { \
if (booz_stab_att_ref_rate.p >= REF_RATE_MAX_P) { \
@@ -34,4 +34,4 @@
} \
}
#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_H */
#endif /* STABILIZATION_ATTITUDE_REF_H */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
+16 -16
View File
@@ -1,4 +1,4 @@
#include "booz_stabilization.h"
#include <firmwares/rotorcraft/stabilization.h>
struct FloatEulers booz_stab_att_sp_euler;
@@ -6,7 +6,7 @@ struct FloatEulers booz_stab_att_ref_euler;
struct FloatRates booz_stab_att_ref_rate;
struct FloatRates booz_stab_att_ref_accel;
void booz_stabilization_attitude_ref_init(void) {
void stabilization_attitude_ref_init(void) {
FLOAT_EULERS_ZERO(booz_stab_att_sp_euler);
FLOAT_EULERS_ZERO(booz_stab_att_ref_euler);
@@ -21,26 +21,26 @@ void booz_stabilization_attitude_ref_init(void) {
*/
#define DT_UPDATE (1./512.)
#define REF_ACCEL_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define REF_ACCEL_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define REF_ACCEL_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_RDOT
#define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
#define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
#define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
#define REF_RATE_MAX_P BOOZ_STABILIZATION_ATTITUDE_REF_MAX_P
#define REF_RATE_MAX_Q BOOZ_STABILIZATION_ATTITUDE_REF_MAX_Q
#define REF_RATE_MAX_R BOOZ_STABILIZATION_ATTITUDE_REF_MAX_R
#define REF_RATE_MAX_P STABILIZATION_ATTITUDE_REF_MAX_P
#define REF_RATE_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_Q
#define REF_RATE_MAX_R STABILIZATION_ATTITUDE_REF_MAX_R
#define OMEGA_P BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P
#define OMEGA_Q BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define OMEGA_R BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R
#define OMEGA_P STABILIZATION_ATTITUDE_REF_OMEGA_P
#define OMEGA_Q STABILIZATION_ATTITUDE_REF_OMEGA_Q
#define OMEGA_R STABILIZATION_ATTITUDE_REF_OMEGA_R
#define ZETA_P BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_Q BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_R BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R
#define ZETA_P STABILIZATION_ATTITUDE_REF_ZETA_P
#define ZETA_Q STABILIZATION_ATTITUDE_REF_ZETA_Q
#define ZETA_R STABILIZATION_ATTITUDE_REF_ZETA_R
#define USE_REF 1
void booz_stabilization_attitude_ref_update() {
void stabilization_attitude_ref_update() {
#ifdef USE_REF
@@ -77,7 +77,7 @@ void booz_stabilization_attitude_ref_update() {
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(booz_stab_att_ref_euler, booz_stabilization_att_sp);
EULERS_COPY(booz_stab_att_ref_euler, stabilization_att_sp);
FLOAT_RATES_ZERO(booz_stab_att_ref_rate);
FLOAT_RATES_ZERO(booz_stab_att_ref_accel);
#endif /* USE_REF */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h
+19 -19
View File
@@ -1,6 +1,6 @@
/*
* $Id: booz_stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
*
* $Id: stabilization_attitude_ref_traj_euler.h 3796 2009-07-25 00:01:02Z poine $
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,24 +18,24 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#define BOOZ_STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#ifndef STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#define STABILIZATION_ATTITUDE_REF_EULER_FLOAT_H
#include "booz_radio_control.h"
#include "math/pprz_algebra_float.h"
#include "stabilization/booz_stabilization_attitude_ref_float.h"
#include "stabilization_attitude_ref_float.h"
/*
* Radio Control
*/
#define SP_MAX_PHI BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI
#define SP_MAX_THETA BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA
#define SP_MAX_R BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R
#define SP_MAX_PHI STABILIZATION_ATTITUDE_SP_MAX_PHI
#define SP_MAX_THETA STABILIZATION_ATTITUDE_SP_MAX_THETA
#define SP_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
@@ -44,20 +44,20 @@
#define RC_UPDATE_FREQ 40.
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_R)
(radio_control.values[RADIO_CONTROL_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_CONTROL_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
#define BOOZ_STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\
#define STABILIZATION_ATTITUDE_READ_RC(_sp, _inflight) { \
\
_sp.phi = \
(-radio_control.values[RADIO_CONTROL_ROLL] * SP_MAX_PHI / MAX_PPRZ); \
_sp.theta = \
( radio_control.values[RADIO_CONTROL_PITCH] * SP_MAX_THETA / MAX_PPRZ); \
if (_inflight) { \
if (YAW_DEADBAND_EXCEEDED()) { \
_sp.psi += \
(-radio_control.values[RADIO_CONTROL_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \
FLOAT_ANGLE_NORMALIZE(_sp.psi); \
_sp.psi += \
(-radio_control.values[RADIO_CONTROL_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); \
FLOAT_ANGLE_NORMALIZE(_sp.psi); \
} \
} \
else { /* if not flying, use current yaw as setpoint */ \
@@ -65,11 +65,11 @@
} \
}
#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
#define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \
struct FloatEulers add_sp_float; \
EULERS_FLOAT_OF_BFP(add_sp_float, (_add_sp)); \
EULERS_ADD(booz_stabilization_att_sp,add_sp_float); \
FLOAT_ANGLE_NORMALIZE(booz_stabilization_att_sp.psi); \
EULERS_ADD(stabilization_att_sp,add_sp_float); \
FLOAT_ANGLE_NORMALIZE(stabilization_att_sp.psi); \
}

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