From 963450f6a0fb8cc0867d440e6db57a7a55460bba Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 28 Sep 2010 15:47:38 +0000 Subject: [PATCH] rename booz_stabilization to stabilization --- conf/airframes/ENAC/quadrotor/blender.xml | 4 +- conf/airframes/ENAC/quadrotor/booz2_g1.xml | 4 +- conf/airframes/ENAC/quadrotor/mkk1.xml | 6 +- .../quadrotor/ppzuav_booz2_asctec_example.xml | 4 +- .../quadrotor/ppzuav_booz2_mkk_example.xml | 4 +- conf/airframes/Poine/booz2_a1.xml | 4 +- conf/airframes/Poine/booz2_a1p.xml | 4 +- conf/airframes/Poine/booz2_a2.xml | 4 +- conf/airframes/Poine/booz2_a3.xml | 4 +- conf/airframes/Poine/booz2_a4.xml | 4 +- conf/airframes/Poine/booz2_a5.xml | 4 +- conf/airframes/Poine/booz2_a6.xml | 4 +- conf/airframes/Poine/booz2_a7.xml | 4 +- conf/airframes/UofAdelaide/A1000_BOOZ.xml | 4 +- conf/airframes/UofAdelaide/A1000_LISA.xml | 4 +- conf/airframes/UofAdelaide/A1000_NOVA.xml | 4 +- .../UofAdelaide/booz2_NoVa_001_1000.xml | 4 +- conf/airframes/UofAdelaide/booz2_a1000.xml | 4 +- conf/airframes/UofAdelaide/lisa_a1000.xml | 4 +- conf/airframes/booz2_Aron.xml | 4 +- conf/airframes/booz2_NoVa.xml | 4 +- conf/airframes/booz2_NoVa_001.xml | 4 +- conf/airframes/booz2_NoVa_002.xml | 4 +- conf/airframes/booz2_flixr.xml | 4 +- conf/airframes/booz2_ppzuav.xml | 4 +- conf/airframes/booz2_s1.xml | 4 +- conf/airframes/booz2_x1.xml | 4 +- conf/airframes/esden/lisa_asctec.xml | 4 +- conf/autopilot/mercury.makefile | 12 +- conf/autopilot/rotorcraft.makefile | 12 +- .../subsystems/rotorcraft/fdm_nps.makefile | 32 ++-- conf/settings/settings_booz2.xml | 30 +-- sw/airborne/booz/booz2_telemetry.h | 106 +++++------ sw/airborne/csc/mercury_ap.c | 18 +- sw/airborne/csc/mercury_main.c | 6 +- sw/airborne/firmwares/rotorcraft/autopilot.c | 4 +- .../rotorcraft/guidance/guidance_h.c | 28 +-- .../rotorcraft/guidance/guidance_v.c | 16 +- sw/airborne/firmwares/rotorcraft/main.c | 4 +- .../firmwares/rotorcraft/stabilization.c | 14 +- .../firmwares/rotorcraft/stabilization.h | 14 +- .../stabilization/stabilization_attitude.h | 33 ++-- .../stabilization_attitude_euler_float.c | 134 +++++++------ .../stabilization_attitude_euler_int.c | 146 +++++++------- .../stabilization_attitude_float.h | 41 ++-- .../stabilization_attitude_int.h | 29 ++- .../stabilization_attitude_quat_float.c | 180 +++++++++--------- .../stabilization_attitude_ref.h | 6 +- .../stabilization_attitude_ref_euler_float.c | 32 ++-- .../stabilization_attitude_ref_euler_float.h | 38 ++-- .../stabilization_attitude_ref_euler_int.c | 34 ++-- .../stabilization_attitude_ref_euler_int.h | 36 ++-- .../stabilization_attitude_ref_float.h | 5 +- .../stabilization_attitude_ref_int.h | 4 +- .../stabilization_attitude_ref_quat_float.c | 54 +++--- .../stabilization_attitude_ref_quat_float.h | 46 ++--- .../stabilization/stabilization_rate.c | 178 ++++++++--------- .../stabilization/stabilization_rate.h | 36 ++-- sw/airborne/modules/vehicle_interface/vi.c | 2 +- sw/airborne/modules/vehicle_interface/vi.h | 2 +- 60 files changed, 718 insertions(+), 728 deletions(-) diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index acc5acd53f..61f23293b7 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -87,7 +87,7 @@ -
+
@@ -99,7 +99,7 @@
-
+
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 07f63b8c26..c822097688 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -83,7 +83,7 @@
-
+
@@ -95,7 +95,7 @@
-
+
diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml index 2cb6f2141f..5c951416f3 100644 --- a/conf/airframes/ENAC/quadrotor/mkk1.xml +++ b/conf/airframes/ENAC/quadrotor/mkk1.xml @@ -121,7 +121,7 @@
-
+
@@ -133,7 +133,7 @@
-
+
@@ -146,7 +146,7 @@
-
+
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index 7f050a167e..bad4fb193a 100644 --- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -70,7 +70,7 @@
-
+
@@ -82,7 +82,7 @@
-
+
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index d36e2b83b1..30b5a79bd1 100644 --- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -70,7 +70,7 @@
-
+
@@ -83,7 +83,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml index 30cb635e5a..1f7ff99a54 100644 --- a/conf/airframes/Poine/booz2_a1.xml +++ b/conf/airframes/Poine/booz2_a1.xml @@ -84,7 +84,7 @@
-
+
@@ -97,7 +97,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml index 9bd3250ff1..d37b1d54c4 100644 --- a/conf/airframes/Poine/booz2_a1p.xml +++ b/conf/airframes/Poine/booz2_a1p.xml @@ -82,7 +82,7 @@
-
+
@@ -95,7 +95,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a2.xml b/conf/airframes/Poine/booz2_a2.xml index bfd36641e0..3ae6d2cfd0 100644 --- a/conf/airframes/Poine/booz2_a2.xml +++ b/conf/airframes/Poine/booz2_a2.xml @@ -71,7 +71,7 @@
-
+
@@ -83,7 +83,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a3.xml b/conf/airframes/Poine/booz2_a3.xml index 5126e1942a..b5fa5bed94 100644 --- a/conf/airframes/Poine/booz2_a3.xml +++ b/conf/airframes/Poine/booz2_a3.xml @@ -58,7 +58,7 @@
-
+
@@ -70,7 +70,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a4.xml b/conf/airframes/Poine/booz2_a4.xml index 51ee06f94e..1d3d111860 100644 --- a/conf/airframes/Poine/booz2_a4.xml +++ b/conf/airframes/Poine/booz2_a4.xml @@ -60,7 +60,7 @@
-
+
@@ -72,7 +72,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a5.xml b/conf/airframes/Poine/booz2_a5.xml index af30ac2e86..4320b5a8df 100644 --- a/conf/airframes/Poine/booz2_a5.xml +++ b/conf/airframes/Poine/booz2_a5.xml @@ -81,7 +81,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a6.xml b/conf/airframes/Poine/booz2_a6.xml index 7de5e35450..d57d2d95b0 100644 --- a/conf/airframes/Poine/booz2_a6.xml +++ b/conf/airframes/Poine/booz2_a6.xml @@ -82,7 +82,7 @@
-
+
@@ -94,7 +94,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 72999be6ac..0d5f1555fa 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -81,7 +81,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml index 8cd3c75376..76c65bc914 100644 --- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml @@ -138,7 +138,7 @@ second attempt
-
+
@@ -171,7 +171,7 @@ second attempt -->
-
+
diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml index 479094d63c..87bcdde037 100644 --- a/conf/airframes/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/UofAdelaide/A1000_LISA.xml @@ -69,7 +69,7 @@
-
+
@@ -81,7 +81,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml index 40409d086f..6401d96651 100644 --- a/conf/airframes/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml @@ -68,7 +68,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml index 9f9dc1bdf1..9fb2a4b39f 100644 --- a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml @@ -68,7 +68,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/UofAdelaide/booz2_a1000.xml index e1ca1c69af..fb85010b66 100755 --- a/conf/airframes/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/UofAdelaide/booz2_a1000.xml @@ -137,7 +137,7 @@ second attempt
-
+
@@ -171,7 +171,7 @@ second attempt
-
+
diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/UofAdelaide/lisa_a1000.xml index 5ee9425f5d..e6ce19c1a3 100644 --- a/conf/airframes/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/UofAdelaide/lisa_a1000.xml @@ -77,7 +77,7 @@
-
+
@@ -89,7 +89,7 @@
-
+
diff --git a/conf/airframes/booz2_Aron.xml b/conf/airframes/booz2_Aron.xml index 267237cb3d..34bd52d397 100644 --- a/conf/airframes/booz2_Aron.xml +++ b/conf/airframes/booz2_Aron.xml @@ -77,7 +77,7 @@
-
+
@@ -89,7 +89,7 @@
-
+
diff --git a/conf/airframes/booz2_NoVa.xml b/conf/airframes/booz2_NoVa.xml index 6a4a12a2b4..4738f4a333 100644 --- a/conf/airframes/booz2_NoVa.xml +++ b/conf/airframes/booz2_NoVa.xml @@ -68,7 +68,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/booz2_NoVa_001.xml b/conf/airframes/booz2_NoVa_001.xml index d3f25a9330..f2b27b9980 100644 --- a/conf/airframes/booz2_NoVa_001.xml +++ b/conf/airframes/booz2_NoVa_001.xml @@ -68,7 +68,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/booz2_NoVa_002.xml b/conf/airframes/booz2_NoVa_002.xml index 5150b0b77a..8745caa057 100644 --- a/conf/airframes/booz2_NoVa_002.xml +++ b/conf/airframes/booz2_NoVa_002.xml @@ -68,7 +68,7 @@
-
+
@@ -93,7 +93,7 @@
-
+
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 2b4bf28327..e44f1c4e20 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -90,7 +90,7 @@
-
+
@@ -116,7 +116,7 @@
-
+
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml index 67cf2eedf1..cb34c0b87c 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/booz2_ppzuav.xml @@ -53,7 +53,7 @@
-
+
@@ -78,7 +78,7 @@
-
+
diff --git a/conf/airframes/booz2_s1.xml b/conf/airframes/booz2_s1.xml index 496c6a92fe..02b6f2fc6e 100644 --- a/conf/airframes/booz2_s1.xml +++ b/conf/airframes/booz2_s1.xml @@ -72,7 +72,7 @@
-
+
@@ -85,7 +85,7 @@
-
+
diff --git a/conf/airframes/booz2_x1.xml b/conf/airframes/booz2_x1.xml index 854f36b511..b06ab1d44e 100644 --- a/conf/airframes/booz2_x1.xml +++ b/conf/airframes/booz2_x1.xml @@ -83,7 +83,7 @@
-
+
@@ -96,7 +96,7 @@
-
+
diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml index ff367f1268..50f8e48d67 100644 --- a/conf/airframes/esden/lisa_asctec.xml +++ b/conf/airframes/esden/lisa_asctec.xml @@ -77,7 +77,7 @@
-
+
@@ -89,7 +89,7 @@
-
+
diff --git a/conf/autopilot/mercury.makefile b/conf/autopilot/mercury.makefile index 4af31ea6ee..627d7648d0 100644 --- a/conf/autopilot/mercury.makefile +++ b/conf/autopilot/mercury.makefile @@ -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 diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 1a2b04006d..3bc9d22546 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -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 diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index a6615c096f..4a4d8f23e4 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -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 diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index 0c8fa57b09..33d72fecb3 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -22,25 +22,25 @@ - - - + + + - - - - - - - - - - - - + + + + + + + + + + + + diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index 1aa68a2d3b..482f8e9448 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -188,28 +188,28 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; -#include "booz_stabilization.h" +#include #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, \ diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c index 7ee0c72034..31ff348a59 100644 --- a/sw/airborne/csc/mercury_ap.c +++ b/sw/airborne/csc/mercury_ap.c @@ -27,8 +27,8 @@ #include "commands.h" #include "mercury_xsens.h" #include -#include "booz_stabilization.h" -#include "stabilization/booz_stabilization_attitude.h" +#include +#include #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(); diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c index fb1b47c060..ee02e0694c 100644 --- a/sw/airborne/csc/mercury_main.c +++ b/sw/airborne/csc/mercury_main.c @@ -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(); } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index f269c82ad3..5099a85eae 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -28,7 +28,7 @@ #include "booz2_commands.h" #include "booz2_navigation.h" #include -#include "booz_stabilization.h" +#include #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); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index adc9fdd908..ffae6fdb21 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -26,7 +26,7 @@ #include #include -#include "booz_stabilization.h" +#include #include "booz_fms.h" #include #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)); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index f4db460756..7d036223ed 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -27,7 +27,7 @@ #include "booz_radio_control.h" -#include "booz_stabilization.h" +#include #include #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: diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index abeee7d4b3..4affe0d9a5 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -49,7 +49,7 @@ #include "booz_fms.h" #include -#include "booz_stabilization.h" +#include #include #include @@ -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(); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.c b/sw/airborne/firmwares/rotorcraft/stabilization.c index bfbc660ad1..d95049f064 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization.c @@ -21,16 +21,14 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_stabilization.h" +#include -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(); } - - diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.h b/sw/airborne/firmwares/rotorcraft/stabilization.h index e9e69534c1..ee4ed0a1e6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization.h @@ -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 +#include -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index ded172f0dc..d197bf70a9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -1,7 +1,6 @@ - /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index 2586669eff..b82efbb0d9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -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 * * 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 #include "math/pprz_algebra_float.h" #include @@ -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.; + } - - diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index be03842a13..cf17314ef1 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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 #include #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); + } - - diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h index 92b3338144..fe8cb92a89 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_float.h @@ -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 * * 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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h index e95ff10549..cd6d1db2a3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_int.h @@ -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 * * 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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index 5e1e6df30d..117416d80b 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -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 * * 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 #include #include "math/pprz_algebra_float.h" @@ -33,90 +33,90 @@ #include #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]; } } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h index 3df59d5d0f..6d8d2587ef 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref.h @@ -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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index 02c82e7faf..1af0c04296 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -1,4 +1,4 @@ -#include "booz_stabilization.h" +#include 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 */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h index 72bb4d05d3..9f68d4c006 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.h @@ -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 * * 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); \ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index 8f7abc18cf..0e897a91b4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -1,5 +1,5 @@ /* - * $Id: booz_stabilization_attitude_ref_euler_int.h -1 $ + * $Id: stabilization_attitude_ref_euler_int.h -1 $ * * Copyright (C) 2008-2009 Antoine Drouin * @@ -21,14 +21,14 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_stabilization.h" +#include struct Int32Eulers booz_stab_att_sp_euler; struct Int32Eulers booz_stab_att_ref_euler; struct Int32Rates booz_stab_att_ref_rate; struct Int32Rates booz_stab_att_ref_accel; -void booz_stabilization_attitude_ref_init(void) { +void stabilization_attitude_ref_init(void) { INT_EULERS_ZERO(booz_stab_att_sp_euler); INT_EULERS_ZERO(booz_stab_att_ref_euler); @@ -40,30 +40,30 @@ void booz_stabilization_attitude_ref_init(void) { #define F_UPDATE_RES 9 #define F_UPDATE (1< * * This file is part of paparazzi. @@ -18,13 +18,13 @@ * 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_INT_H -#define BOOZ_STABILIZATION_ATTITUDE_REF_EULER_INT_H +#ifndef STABILIZATION_ATTITUDE_REF_EULER_INT_H +#define STABILIZATION_ATTITUDE_REF_EULER_INT_H -#include "stabilization/booz_stabilization_attitude_ref_int.h" +#include "stabilization_attitude_ref_int.h" #include "booz_radio_control.h" @@ -44,9 +44,9 @@ /* * Radio Control */ -#define SP_MAX_PHI ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PHI) -#define SP_MAX_THETA ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA) -#define SP_MAX_R ANGLE_BFP_OF_REAL(BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R) +#define SP_MAX_PHI ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI) +#define SP_MAX_THETA ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA) +#define SP_MAX_R ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R) @@ -55,11 +55,11 @@ #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 = \ ((int32_t)-radio_control.values[RADIO_CONTROL_ROLL] * (int32_t)SP_MAX_PHI / MAX_PPRZ) \ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ @@ -68,10 +68,10 @@ << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ if (_inflight) { \ if (YAW_DEADBAND_EXCEEDED()) { \ - _sp.psi += \ - ((int32_t)-radio_control.values[RADIO_CONTROL_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ - << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ - ANGLE_REF_NORMALIZE(_sp.psi); \ + _sp.psi += \ + ((int32_t)-radio_control.values[RADIO_CONTROL_YAW] * (int32_t)SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ) \ + << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ + ANGLE_REF_NORMALIZE(_sp.psi); \ } \ } \ else { /* if not flying, use current yaw as setpoint */ \ @@ -79,12 +79,12 @@ } \ } -#define BOOZ_STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ +#define STABILIZATION_ATTITUDE_ADD_SP(_add_sp) { \ EULERS_ADD(booz_stab_att_sp_euler,_add_sp); \ ANGLE_REF_NORMALIZE(booz_stab_att_sp_euler.psi); \ } -#define BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ +#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) { \ _sp.psi = ahrs.ltp_to_body_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); \ booz_stab_att_ref_euler.psi = _sp.psi; \ booz_stab_att_ref_rate.r = 0; \ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h index 8358516073..915f5e8117 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_float.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * 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_STABILISATION_ATTITUDE_REF_FLOAT_H #define BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H @@ -40,4 +40,3 @@ struct FloatRefModel { extern struct FloatRefModel booz_stab_att_ref_model[]; #endif /* BOOZ_STABILISATION_ATTITUDE_REF_FLOAT_H */ - diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h index 4fcb5506b9..951a547181 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_int.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2010 The Paparazzi Team * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * 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_STABILISATION_ATTITUDE_REF_INT_H #define BOOZ_STABILISATION_ATTITUDE_REF_INT_H diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index 953730dfc2..69a5bfe7e9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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. */ -/** \file booz_stabilization_attitude_ref_float.c +/** \file stabilization_attitude_ref_float.c * \brief Booz attitude reference generation (quaternion float version) * */ #include "airframe.h" -#include "booz_stabilization.h" +#include #include -#include "booz_stabilization_attitude_ref_float.h" +#include "stabilization_attitude_ref_float.h" #include "quat_setpoint.h" -#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 struct FloatEulers booz_stab_att_sp_euler; struct FloatQuat booz_stab_att_sp_quat; @@ -44,16 +44,16 @@ struct FloatQuat booz_stab_att_ref_quat; struct FloatRates booz_stab_att_ref_rate; struct FloatRates booz_stab_att_ref_accel; -struct FloatRefModel booz_stab_att_ref_model[BOOZ_STABILIZATION_ATTITUDE_GAIN_NB]; +struct FloatRefModel booz_stab_att_ref_model[STABILIZATION_ATTITUDE_GAIN_NB]; -static int ref_idx = BOOZ_STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT; +static int ref_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT; -static const float omega_p[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_P; -static const float zeta_p[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_P; -static const float omega_q[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_Q; -static const float zeta_q[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_Q; -static const float omega_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_OMEGA_R; -static const float zeta_r[] = BOOZ_STABILIZATION_ATTITUDE_REF_ZETA_R; +static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P; +static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P; +static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q; +static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q; +static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R; +static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; static void reset_psi_ref_from_body(void) { booz_stab_att_ref_euler.psi = ahrs_float.ltp_to_body_euler.psi; @@ -73,7 +73,7 @@ static void update_ref_quat_from_eulers(void) { FLOAT_QUAT_WRAP_SHORTEST(booz_stab_att_ref_quat); } -void booz_stabilization_attitude_ref_init(void) { +void stabilization_attitude_ref_init(void) { FLOAT_EULERS_ZERO(booz_stab_att_sp_euler); FLOAT_QUAT_ZERO( booz_stab_att_sp_quat); @@ -82,22 +82,22 @@ void booz_stabilization_attitude_ref_init(void) { FLOAT_RATES_ZERO( booz_stab_att_ref_rate); FLOAT_RATES_ZERO( booz_stab_att_ref_accel); - for (int i = 0; i < BOOZ_STABILIZATION_ATTITUDE_GAIN_NB; i++) { + for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { RATES_ASSIGN(booz_stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]); RATES_ASSIGN(booz_stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]); } } -void booz_stabilization_attitude_ref_schedule(uint8_t idx) +void stabilization_attitude_ref_schedule(uint8_t idx) { ref_idx = idx; } -void booz_stabilization_attitude_ref_enter() +void stabilization_attitude_ref_enter() { reset_psi_ref_from_body(); - booz_stabilization_attitude_sp_enter(); + stabilization_attitude_sp_enter(); update_ref_quat_from_eulers(); } @@ -110,7 +110,7 @@ void booz_stabilization_attitude_ref_enter() #define DT_UPDATE (1./512.) #endif -void booz_stabilization_attitude_ref_update() { +void stabilization_attitude_ref_update() { /* integrate reference attitude */ struct FloatQuat qdot; @@ -125,22 +125,22 @@ void booz_stabilization_attitude_ref_update() { RATES_ADD(booz_stab_att_ref_rate, delta_rate); /* compute reference angular accelerations */ - struct FloatQuat err; + struct FloatQuat err; /* compute reference attitude error */ FLOAT_QUAT_INV_COMP(err, booz_stab_att_sp_quat, booz_stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(err); /* propagate the 2nd order linear model */ - booz_stab_att_ref_accel.p = -2.*booz_stab_att_ref_model[ref_idx].zeta.p*booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_rate.p + booz_stab_att_ref_accel.p = -2.*booz_stab_att_ref_model[ref_idx].zeta.p*booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_rate.p - booz_stab_att_ref_model[ref_idx].omega.p*booz_stab_att_ref_model[ref_idx].omega.p*err.qx; - booz_stab_att_ref_accel.q = -2.*booz_stab_att_ref_model[ref_idx].zeta.q*booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_rate.q + booz_stab_att_ref_accel.q = -2.*booz_stab_att_ref_model[ref_idx].zeta.q*booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_rate.q - booz_stab_att_ref_model[ref_idx].omega.q*booz_stab_att_ref_model[ref_idx].omega.q*err.qy; - booz_stab_att_ref_accel.r = -2.*booz_stab_att_ref_model[ref_idx].zeta.r*booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_rate.r + booz_stab_att_ref_accel.r = -2.*booz_stab_att_ref_model[ref_idx].zeta.r*booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_rate.r - booz_stab_att_ref_model[ref_idx].omega.r*booz_stab_att_ref_model[ref_idx].omega.r*err.qz; /* saturate acceleration */ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; - const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; + const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(booz_stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); /* compute ref_euler */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h index fc96a021d0..75ba7be0a4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.h @@ -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 * * This file is part of paparazzi. @@ -18,42 +18,42 @@ * 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_QUAT_FLOAT_H -#define BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H +#ifndef STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H +#define STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H -#include "booz_stabilization.h" +#include #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" #define RC_UPDATE_FREQ 40. -#define ROLL_COEF (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ) -#define ROLL_COEF_H (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ) -#define PITCH_COEF ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) -#define YAW_COEF (BOOZ_STABILIZATION_ATTITUDE_SP_MAX_PSI / MAX_PPRZ) +#define ROLL_COEF (STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ) +#define ROLL_COEF_H (STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ) +#define PITCH_COEF ( STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) +#define YAW_COEF (STABILIZATION_ATTITUDE_SP_MAX_PSI / MAX_PPRZ) -#define ROLL_COEF_RATE (-BOOZ_STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ) -#define PITCH_COEF_RATE ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_Q / MAX_PPRZ) -#define YAW_COEF_RATE ( BOOZ_STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ) +#define ROLL_COEF_RATE (-STABILIZATION_ATTITUDE_SP_MAX_P / MAX_PPRZ) +#define PITCH_COEF_RATE ( STABILIZATION_ATTITUDE_SP_MAX_Q / MAX_PPRZ) +#define YAW_COEF_RATE ( STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ) #define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE)) #define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0) #define ROLL_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_CONTROL_ROLL] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A || \ - radio_control.values[RADIO_CONTROL_ROLL] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_A) + (radio_control.values[RADIO_CONTROL_ROLL] > STABILIZATION_ATTITUDE_DEADBAND_A || \ + radio_control.values[RADIO_CONTROL_ROLL] < -STABILIZATION_ATTITUDE_DEADBAND_A) #define PITCH_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_CONTROL_PITCH] > BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E || \ - radio_control.values[RADIO_CONTROL_PITCH] < -BOOZ_STABILIZATION_ATTITUDE_DEADBAND_E) + (radio_control.values[RADIO_CONTROL_PITCH] > STABILIZATION_ATTITUDE_DEADBAND_E || \ + radio_control.values[RADIO_CONTROL_PITCH] < -STABILIZATION_ATTITUDE_DEADBAND_E) #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) -void booz_stabilization_attitude_ref_enter(void); -void booz_stabilization_attitude_ref_schedule(uint8_t idx); +void stabilization_attitude_ref_enter(void); +void stabilization_attitude_ref_schedule(uint8_t idx); -#endif /* BOOZ_STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */ +#endif /* STABILIZATION_ATTITUDE_REF_QUAT_FLOAT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 81ebbbcd8a..220210157e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -22,7 +22,7 @@ * Boston, MA 02111-1307, USA. */ -#include "booz_stabilization.h" +#include #include @@ -36,163 +36,163 @@ #define MAX_SUM_ERR 4000000 -#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P -#define BOOZ_STABILIZATION_RATE_DDGAIN_P 0 +#ifndef STABILIZATION_RATE_DDGAIN_P +#define STABILIZATION_RATE_DDGAIN_P 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_Q -#define BOOZ_STABILIZATION_RATE_DDGAIN_Q 0 +#ifndef STABILIZATION_RATE_DDGAIN_Q +#define STABILIZATION_RATE_DDGAIN_Q 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_DDGAIN_R -#define BOOZ_STABILIZATION_RATE_DDGAIN_R 0 +#ifndef STABILIZATION_RATE_DDGAIN_R +#define STABILIZATION_RATE_DDGAIN_R 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_IGAIN_P -#define BOOZ_STABILIZATION_RATE_IGAIN_P 0 +#ifndef STABILIZATION_RATE_IGAIN_P +#define STABILIZATION_RATE_IGAIN_P 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_IGAIN_Q -#define BOOZ_STABILIZATION_RATE_IGAIN_Q 0 +#ifndef STABILIZATION_RATE_IGAIN_Q +#define STABILIZATION_RATE_IGAIN_Q 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_IGAIN_R -#define BOOZ_STABILIZATION_RATE_IGAIN_R 0 +#ifndef STABILIZATION_RATE_IGAIN_R +#define STABILIZATION_RATE_IGAIN_R 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_REF_TAU -#define BOOZ_STABILIZATION_RATE_REF_TAU 4 +#ifndef STABILIZATION_RATE_REF_TAU +#define STABILIZATION_RATE_REF_TAU 4 #endif #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)) -struct Int32Rates booz_stabilization_rate_sp; -struct Int32Rates booz_stabilization_rate_gain; -struct Int32Rates booz_stabilization_rate_igain; -struct Int32Rates booz_stabilization_rate_ddgain; -struct Int32Rates booz_stabilization_rate_ref; -struct Int32Rates booz_stabilization_rate_refdot; -struct Int32Rates booz_stabilization_rate_sum_err; +struct Int32Rates stabilization_rate_sp; +struct Int32Rates stabilization_rate_gain; +struct Int32Rates stabilization_rate_igain; +struct Int32Rates stabilization_rate_ddgain; +struct Int32Rates stabilization_rate_ref; +struct Int32Rates stabilization_rate_refdot; +struct Int32Rates stabilization_rate_sum_err; -struct Int32Rates booz_stabilization_rate_fb_cmd; -struct Int32Rates booz_stabilization_rate_ff_cmd; +struct Int32Rates stabilization_rate_fb_cmd; +struct Int32Rates stabilization_rate_ff_cmd; -#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_P -#define BOOZ_STABILIZATION_RATE_DEADBAND_P 0 +#ifndef STABILIZATION_RATE_DEADBAND_P +#define STABILIZATION_RATE_DEADBAND_P 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_Q -#define BOOZ_STABILIZATION_RATE_DEADBAND_Q 0 +#ifndef STABILIZATION_RATE_DEADBAND_Q +#define STABILIZATION_RATE_DEADBAND_Q 0 #endif -#ifndef BOOZ_STABILIZATION_RATE_DEADBAND_R -#define BOOZ_STABILIZATION_RATE_DEADBAND_R 200 +#ifndef STABILIZATION_RATE_DEADBAND_R +#define STABILIZATION_RATE_DEADBAND_R 200 #endif #define ROLL_RATE_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_CONTROL_ROLL] > BOOZ_STABILIZATION_RATE_DEADBAND_P || \ - radio_control.values[RADIO_CONTROL_ROLL] < -BOOZ_STABILIZATION_RATE_DEADBAND_P) + (radio_control.values[RADIO_CONTROL_ROLL] > STABILIZATION_RATE_DEADBAND_P || \ + radio_control.values[RADIO_CONTROL_ROLL] < -STABILIZATION_RATE_DEADBAND_P) #define PITCH_RATE_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_CONTROL_PITCH] > BOOZ_STABILIZATION_RATE_DEADBAND_Q || \ - radio_control.values[RADIO_CONTROL_PITCH] < -BOOZ_STABILIZATION_RATE_DEADBAND_Q) + (radio_control.values[RADIO_CONTROL_PITCH] > STABILIZATION_RATE_DEADBAND_Q || \ + radio_control.values[RADIO_CONTROL_PITCH] < -STABILIZATION_RATE_DEADBAND_Q) #define YAW_RATE_DEADBAND_EXCEEDED() \ - (radio_control.values[RADIO_CONTROL_YAW] > BOOZ_STABILIZATION_RATE_DEADBAND_R || \ - radio_control.values[RADIO_CONTROL_YAW] < -BOOZ_STABILIZATION_RATE_DEADBAND_R) + (radio_control.values[RADIO_CONTROL_YAW] > STABILIZATION_RATE_DEADBAND_R || \ + radio_control.values[RADIO_CONTROL_YAW] < -STABILIZATION_RATE_DEADBAND_R) -void booz_stabilization_rate_init(void) { +void stabilization_rate_init(void) { - INT_RATES_ZERO(booz_stabilization_rate_sp); + INT_RATES_ZERO(stabilization_rate_sp); - RATES_ASSIGN(booz_stabilization_rate_gain, - BOOZ_STABILIZATION_RATE_GAIN_P, - BOOZ_STABILIZATION_RATE_GAIN_Q, - BOOZ_STABILIZATION_RATE_GAIN_R); - RATES_ASSIGN(booz_stabilization_rate_igain, - BOOZ_STABILIZATION_RATE_IGAIN_P, - BOOZ_STABILIZATION_RATE_IGAIN_Q, - BOOZ_STABILIZATION_RATE_IGAIN_R); - RATES_ASSIGN(booz_stabilization_rate_ddgain, - BOOZ_STABILIZATION_RATE_DDGAIN_P, - BOOZ_STABILIZATION_RATE_DDGAIN_Q, - BOOZ_STABILIZATION_RATE_DDGAIN_R); + RATES_ASSIGN(stabilization_rate_gain, + STABILIZATION_RATE_GAIN_P, + STABILIZATION_RATE_GAIN_Q, + STABILIZATION_RATE_GAIN_R); + RATES_ASSIGN(stabilization_rate_igain, + STABILIZATION_RATE_IGAIN_P, + STABILIZATION_RATE_IGAIN_Q, + STABILIZATION_RATE_IGAIN_R); + RATES_ASSIGN(stabilization_rate_ddgain, + STABILIZATION_RATE_DDGAIN_P, + STABILIZATION_RATE_DDGAIN_Q, + STABILIZATION_RATE_DDGAIN_R); - INT_RATES_ZERO(booz_stabilization_rate_ref); - INT_RATES_ZERO(booz_stabilization_rate_refdot); - INT_RATES_ZERO(booz_stabilization_rate_sum_err); + INT_RATES_ZERO(stabilization_rate_ref); + INT_RATES_ZERO(stabilization_rate_refdot); + INT_RATES_ZERO(stabilization_rate_sum_err); } -void booz_stabilization_rate_read_rc( void ) { +void stabilization_rate_read_rc( void ) { if(ROLL_RATE_DEADBAND_EXCEEDED()) - booz_stabilization_rate_sp.p = (int32_t)-radio_control.values[RADIO_CONTROL_ROLL] * BOOZ_STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; + stabilization_rate_sp.p = (int32_t)-radio_control.values[RADIO_CONTROL_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; else - booz_stabilization_rate_sp.p = 0; + stabilization_rate_sp.p = 0; if(PITCH_RATE_DEADBAND_EXCEEDED()) - booz_stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_CONTROL_PITCH] * BOOZ_STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; + stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_CONTROL_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; else - booz_stabilization_rate_sp.q = 0; + stabilization_rate_sp.q = 0; if(YAW_RATE_DEADBAND_EXCEEDED()) - booz_stabilization_rate_sp.r = (int32_t)-radio_control.values[RADIO_CONTROL_YAW] * BOOZ_STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; + stabilization_rate_sp.r = (int32_t)-radio_control.values[RADIO_CONTROL_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; else - booz_stabilization_rate_sp.r = 0; + stabilization_rate_sp.r = 0; } -void booz_stabilization_rate_enter(void) { - RATES_COPY(booz_stabilization_rate_ref, booz_stabilization_rate_sp); - INT_RATES_ZERO(booz_stabilization_rate_sum_err); +void stabilization_rate_enter(void) { + RATES_COPY(stabilization_rate_ref, stabilization_rate_sp); + INT_RATES_ZERO(stabilization_rate_sum_err); } -void booz_stabilization_rate_run(bool_t in_flight) { +void stabilization_rate_run(bool_t in_flight) { /* reference */ struct Int32Rates _r; - RATES_DIFF(_r, booz_stabilization_rate_sp, booz_stabilization_rate_ref); - RATES_SDIV(booz_stabilization_rate_refdot, _r, BOOZ_STABILIZATION_RATE_REF_TAU); + RATES_DIFF(_r, stabilization_rate_sp, stabilization_rate_ref); + RATES_SDIV(stabilization_rate_refdot, _r, STABILIZATION_RATE_REF_TAU); /* integrate ref */ const struct Int32Rates _delta_ref = { - booz_stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), - booz_stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), - booz_stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)}; - RATES_ADD(booz_stabilization_rate_ref, _delta_ref); + stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), + stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), + stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)}; + RATES_ADD(stabilization_rate_ref, _delta_ref); /* compute feed-forward command */ - RATES_EWMULT_RSHIFT(booz_stabilization_rate_ff_cmd, booz_stabilization_rate_ddgain, booz_stabilization_rate_refdot, 14); + RATES_EWMULT_RSHIFT(stabilization_rate_ff_cmd, stabilization_rate_ddgain, stabilization_rate_refdot, 14); /* compute feed-back command */ /* error for feedback */ const struct Int32Rates _ref_scaled = { - OFFSET_AND_ROUND(booz_stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)), - OFFSET_AND_ROUND(booz_stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)), - OFFSET_AND_ROUND(booz_stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) }; + OFFSET_AND_ROUND(stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)), + OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)), + OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) }; struct Int32Rates _error; RATES_DIFF(_error, ahrs.body_rate, _ref_scaled); if (in_flight) { /* update integrator */ - RATES_ADD(booz_stabilization_rate_sum_err, _error); - RATES_BOUND_CUBE(booz_stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); + RATES_ADD(stabilization_rate_sum_err, _error); + RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); } else { - INT_RATES_ZERO(booz_stabilization_rate_sum_err); + INT_RATES_ZERO(stabilization_rate_sum_err); } /* PI */ - booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_gain.p * _error.p + - OFFSET_AND_ROUND2((booz_stabilization_rate_igain.p * booz_stabilization_rate_sum_err.p), 10); + stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p + + OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 10); - booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_gain.q * _error.q + - OFFSET_AND_ROUND2((booz_stabilization_rate_igain.q * booz_stabilization_rate_sum_err.q), 10); + stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + + OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10); - booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_gain.r * _error.r + - OFFSET_AND_ROUND2((booz_stabilization_rate_igain.r * booz_stabilization_rate_sum_err.r), 10); + stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); - booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_fb_cmd.p >> 16; - booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_fb_cmd.q >> 16; - booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_fb_cmd.r >> 16; + stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 16; + stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 16; + stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 16; /* sum to final command */ - booz_stabilization_cmd[COMMAND_ROLL] = booz_stabilization_rate_ff_cmd.p + booz_stabilization_rate_fb_cmd.p; - booz_stabilization_cmd[COMMAND_PITCH] = booz_stabilization_rate_ff_cmd.q + booz_stabilization_rate_fb_cmd.q; - booz_stabilization_cmd[COMMAND_YAW] = booz_stabilization_rate_ff_cmd.r + booz_stabilization_rate_fb_cmd.r; + stabilization_cmd[COMMAND_ROLL] = stabilization_rate_ff_cmd.p + stabilization_rate_fb_cmd.p; + stabilization_cmd[COMMAND_PITCH] = stabilization_rate_ff_cmd.q + stabilization_rate_fb_cmd.q; + stabilization_cmd[COMMAND_YAW] = stabilization_rate_ff_cmd.r + stabilization_rate_fb_cmd.r; } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h index f20fb43060..741b3c4fb0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -18,28 +18,28 @@ * 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_RATE -#define BOOZ_STABILIZATION_RATE +#ifndef STABILIZATION_RATE +#define STABILIZATION_RATE #include "math/pprz_algebra_int.h" -extern void booz_stabilization_rate_init(void); -extern void booz_stabilization_rate_read_rc(void); -extern void booz_stabilization_rate_run(bool_t in_flight); -extern void booz_stabilization_rate_enter(void); +extern void stabilization_rate_init(void); +extern void stabilization_rate_read_rc(void); +extern void stabilization_rate_run(bool_t in_flight); +extern void stabilization_rate_enter(void); -extern struct Int32Rates booz_stabilization_rate_sp; -extern struct Int32Rates booz_stabilization_rate_gain; -extern struct Int32Rates booz_stabilization_rate_igain; -extern struct Int32Rates booz_stabilization_rate_ddgain; -extern struct Int32Rates booz_stabilization_rate_ref; -extern struct Int32Rates booz_stabilization_rate_refdot; -extern struct Int32Rates booz_stabilization_rate_sum_err; +extern struct Int32Rates stabilization_rate_sp; +extern struct Int32Rates stabilization_rate_gain; +extern struct Int32Rates stabilization_rate_igain; +extern struct Int32Rates stabilization_rate_ddgain; +extern struct Int32Rates stabilization_rate_ref; +extern struct Int32Rates stabilization_rate_refdot; +extern struct Int32Rates stabilization_rate_sum_err; -extern struct Int32Rates booz_stabilization_rate_fb_cmd; -extern struct Int32Rates booz_stabilization_rate_ff_cmd; +extern struct Int32Rates stabilization_rate_fb_cmd; +extern struct Int32Rates stabilization_rate_ff_cmd; -#endif /* BOOZ_STABILIZATION_RATE */ +#endif /* STABILIZATION_RATE */ diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index 30404df190..dd5df99772 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -24,8 +24,8 @@ #include "vehicle_interface/vi.h" #include -#include "booz/booz_gps.h" #include +#include "booz/booz_gps.h" #include "airframe.h" diff --git a/sw/airborne/modules/vehicle_interface/vi.h b/sw/airborne/modules/vehicle_interface/vi.h index 0c6c804d86..32d22474e6 100644 --- a/sw/airborne/modules/vehicle_interface/vi.h +++ b/sw/airborne/modules/vehicle_interface/vi.h @@ -31,7 +31,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" #include -#include "booz/booz_stabilization.h" +#include #include #include "booz/booz2_navigation.h"