From 6579e914c26b647fee7dc4de43e18a360b817cd6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 28 Sep 2010 14:04:56 +0000 Subject: [PATCH] rename booz2_guidance/BOOZ2_GUIDANCE and B2_GUIDANCE to guidance/GUIDANCE --- conf/airframes/ENAC/quadrotor/blender.xml | 8 +- conf/airframes/ENAC/quadrotor/booz2_g1.xml | 8 +- conf/airframes/ENAC/quadrotor/mkk1.xml | 8 +- .../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 | 8 +- 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/rotorcraft.makefile | 4 +- .../subsystems/rotorcraft/fdm_nps.makefile | 4 +- conf/messages.xml | 4 +- conf/settings/settings_booz2.xml | 24 +- sw/airborne/booz/booz2_navigation.c | 2 +- sw/airborne/booz/booz2_telemetry.h | 98 +++--- sw/airborne/booz/booz_fms.c | 8 +- sw/airborne/booz/booz_fms.h | 2 +- sw/airborne/booz/fms/booz_fms_datalink.h | 26 +- sw/airborne/booz/fms/booz_fms_test_signal.c | 6 +- sw/airborne/booz/test/test_vg_adpt.c | 4 +- sw/airborne/booz/test/test_vg_ref.c | 4 +- sw/airborne/firmwares/rotorcraft/autopilot.c | 38 +-- sw/airborne/firmwares/rotorcraft/guidance.h | 10 +- .../rotorcraft/guidance/guidance_h.c | 316 +++++++++--------- .../rotorcraft/guidance/guidance_h.h | 68 ++-- .../rotorcraft/guidance/guidance_h_ref.h | 36 +- .../rotorcraft/guidance/guidance_v.c | 188 +++++------ .../rotorcraft/guidance/guidance_v.h | 64 ++-- .../rotorcraft/guidance/guidance_v_adpt.h | 10 +- .../rotorcraft/guidance/guidance_v_ref.h | 46 +-- sw/airborne/firmwares/rotorcraft/main.c | 6 +- sw/airborne/modules/vehicle_interface/vi.c | 8 +- sw/airborne/modules/vehicle_interface/vi.h | 2 +- .../modules/vehicle_interface/vi_datalink.h | 26 +- .../vehicle_interface/vi_test_signal.c | 6 +- 54 files changed, 569 insertions(+), 569 deletions(-) diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index b50fc8af7f..acc5acd53f 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -155,7 +155,7 @@ -
+
@@ -175,7 +175,7 @@
-
+
@@ -230,7 +230,7 @@ -ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF +ap.CFLAGS += -DGUIDANCE_H_USE_REF ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3 -DUSE_PWM1 ap.CFLAGS += -DBOOZ_ACTUATORS_MAX_THRUST=160 -DBOOZ_THRUST_LOWPASS=17 @@ -240,7 +240,7 @@ ap.srcs += $(SRC_BOOZ_ARCH)/booz2_pwm_hw.c sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\" sim.CFLAGS += -DNPS_NO_SUPERVISION -sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF +sim.CFLAGS += -DGUIDANCE_H_USE_REF sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3 sim.srcs += $(SRC_BOOZ_SIM)/booz2_pwm_hw.c diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 9797a69135..07f63b8c26 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -145,7 +145,7 @@
-
+
@@ -166,7 +166,7 @@
-
+
@@ -223,14 +223,14 @@ -ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF +ap.CFLAGS += -DGUIDANCE_H_USE_REF ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3 ap.srcs += $(SRC_BOOZ_ARCH)/booz2_pwm_hw.c sim.CFLAGS += -DBSM_PARAMS=\"booz_sensors_model_params_booz2_a2.h\" sim.CFLAGS += -DNPS_NO_SUPERVISION -sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF +sim.CFLAGS += -DGUIDANCE_H_USE_REF sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3 sim.srcs += $(SRC_BOOZ_SIM)/booz2_pwm_hw.c diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml index f9c9f4906f..2cb6f2141f 100644 --- a/conf/airframes/ENAC/quadrotor/mkk1.xml +++ b/conf/airframes/ENAC/quadrotor/mkk1.xml @@ -195,7 +195,7 @@
-
+
@@ -215,7 +215,7 @@
-
+
@@ -260,10 +260,10 @@ #set GPS lag for horizontal filter #ap.CFLAGS += -DUSE_GPS_ACC4R -#ap.CFLAGS += -DB2_GUIDANCE_H_USE_REF +#ap.CFLAGS += -DGUIDANCE_H_USE_REF ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -#sim.CFLAGS += -DB2_GUIDANCE_H_USE_REF +#sim.CFLAGS += -DGUIDANCE_H_USE_REF sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml index 2b356a3164..7f050a167e 100644 --- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml +++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml @@ -153,7 +153,7 @@
-
+
@@ -170,7 +170,7 @@
-
+
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml index 9680371be4..d36e2b83b1 100644 --- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml +++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml @@ -171,7 +171,7 @@ -
+
@@ -188,7 +188,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml index 52266793c4..30cb635e5a 100644 --- a/conf/airframes/Poine/booz2_a1.xml +++ b/conf/airframes/Poine/booz2_a1.xml @@ -145,7 +145,7 @@
-
+
@@ -162,7 +162,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml index 7b800642e4..9bd3250ff1 100644 --- a/conf/airframes/Poine/booz2_a1p.xml +++ b/conf/airframes/Poine/booz2_a1p.xml @@ -149,7 +149,7 @@
-
+
@@ -166,7 +166,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a2.xml b/conf/airframes/Poine/booz2_a2.xml index a7ad5b6e81..bfd36641e0 100644 --- a/conf/airframes/Poine/booz2_a2.xml +++ b/conf/airframes/Poine/booz2_a2.xml @@ -138,7 +138,7 @@
-
+
@@ -154,7 +154,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a3.xml b/conf/airframes/Poine/booz2_a3.xml index 0724249ee4..5126e1942a 100644 --- a/conf/airframes/Poine/booz2_a3.xml +++ b/conf/airframes/Poine/booz2_a3.xml @@ -125,7 +125,7 @@
-
+
@@ -139,7 +139,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a4.xml b/conf/airframes/Poine/booz2_a4.xml index 12027c1da3..51ee06f94e 100644 --- a/conf/airframes/Poine/booz2_a4.xml +++ b/conf/airframes/Poine/booz2_a4.xml @@ -103,7 +103,7 @@
-
+
@@ -117,7 +117,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a5.xml b/conf/airframes/Poine/booz2_a5.xml index 9d4810bff0..af30ac2e86 100644 --- a/conf/airframes/Poine/booz2_a5.xml +++ b/conf/airframes/Poine/booz2_a5.xml @@ -141,7 +141,7 @@
-
+
@@ -158,7 +158,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a6.xml b/conf/airframes/Poine/booz2_a6.xml index 4c7f7c0761..7de5e35450 100644 --- a/conf/airframes/Poine/booz2_a6.xml +++ b/conf/airframes/Poine/booz2_a6.xml @@ -142,7 +142,7 @@
-
+
@@ -159,7 +159,7 @@
-
+
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml index 34146a2cd7..72999be6ac 100644 --- a/conf/airframes/Poine/booz2_a7.xml +++ b/conf/airframes/Poine/booz2_a7.xml @@ -142,7 +142,7 @@
-
+
@@ -159,7 +159,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml index 88d4154819..8cd3c75376 100644 --- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml @@ -219,7 +219,7 @@ second attempt
-
+
@@ -235,7 +235,7 @@ second attempt
-
+
diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml index a7e94af2ab..479094d63c 100644 --- a/conf/airframes/UofAdelaide/A1000_LISA.xml +++ b/conf/airframes/UofAdelaide/A1000_LISA.xml @@ -129,7 +129,7 @@
-
+
@@ -146,7 +146,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml index 37c956f736..40409d086f 100644 --- a/conf/airframes/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml @@ -175,7 +175,7 @@
-
+
@@ -195,7 +195,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml index 167797303c..9f9dc1bdf1 100644 --- a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml +++ b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml @@ -175,7 +175,7 @@
-
+
@@ -195,7 +195,7 @@
-
+
diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/UofAdelaide/booz2_a1000.xml index 93e5014697..e1ca1c69af 100755 --- a/conf/airframes/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/UofAdelaide/booz2_a1000.xml @@ -220,7 +220,7 @@ second attempt
-
+
@@ -237,7 +237,7 @@ second attempt
-
+
diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/UofAdelaide/lisa_a1000.xml index 31f6609e0e..5ee9425f5d 100644 --- a/conf/airframes/UofAdelaide/lisa_a1000.xml +++ b/conf/airframes/UofAdelaide/lisa_a1000.xml @@ -137,7 +137,7 @@
-
+
@@ -154,7 +154,7 @@
-
+
diff --git a/conf/airframes/booz2_Aron.xml b/conf/airframes/booz2_Aron.xml index e3a131cb7f..267237cb3d 100644 --- a/conf/airframes/booz2_Aron.xml +++ b/conf/airframes/booz2_Aron.xml @@ -138,7 +138,7 @@
-
+
@@ -154,7 +154,7 @@
-
+
diff --git a/conf/airframes/booz2_NoVa.xml b/conf/airframes/booz2_NoVa.xml index e8a00740b4..6a4a12a2b4 100644 --- a/conf/airframes/booz2_NoVa.xml +++ b/conf/airframes/booz2_NoVa.xml @@ -175,7 +175,7 @@
-
+
@@ -195,7 +195,7 @@
-
+
diff --git a/conf/airframes/booz2_NoVa_001.xml b/conf/airframes/booz2_NoVa_001.xml index c2ebcf9c06..d3f25a9330 100644 --- a/conf/airframes/booz2_NoVa_001.xml +++ b/conf/airframes/booz2_NoVa_001.xml @@ -175,7 +175,7 @@
-
+
@@ -195,7 +195,7 @@
-
+
diff --git a/conf/airframes/booz2_NoVa_002.xml b/conf/airframes/booz2_NoVa_002.xml index bcc9a49d85..5150b0b77a 100644 --- a/conf/airframes/booz2_NoVa_002.xml +++ b/conf/airframes/booz2_NoVa_002.xml @@ -175,7 +175,7 @@
-
+
@@ -195,7 +195,7 @@
-
+
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 8f11b0b36e..2b4bf28327 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -164,7 +164,7 @@
-
+
@@ -180,7 +180,7 @@
-
+
@@ -227,12 +227,12 @@ - + - + diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml index f4e9edd2c3..67cf2eedf1 100644 --- a/conf/airframes/booz2_ppzuav.xml +++ b/conf/airframes/booz2_ppzuav.xml @@ -150,7 +150,7 @@
-
+
@@ -170,7 +170,7 @@
-
+
diff --git a/conf/airframes/booz2_s1.xml b/conf/airframes/booz2_s1.xml index d0b30abf58..496c6a92fe 100644 --- a/conf/airframes/booz2_s1.xml +++ b/conf/airframes/booz2_s1.xml @@ -133,7 +133,7 @@
-
+
@@ -150,7 +150,7 @@
-
+
diff --git a/conf/airframes/booz2_x1.xml b/conf/airframes/booz2_x1.xml index ed119aa5ba..854f36b511 100644 --- a/conf/airframes/booz2_x1.xml +++ b/conf/airframes/booz2_x1.xml @@ -144,7 +144,7 @@
-
+
@@ -160,7 +160,7 @@
-
+
diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml index 4cdf30b887..ff367f1268 100644 --- a/conf/airframes/esden/lisa_asctec.xml +++ b/conf/airframes/esden/lisa_asctec.xml @@ -137,7 +137,7 @@
-
+
@@ -154,7 +154,7 @@
-
+
diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile index 5b3519ae86..1a2b04006d 100644 --- a/conf/autopilot/rotorcraft.makefile +++ b/conf/autopilot/rotorcraft.makefile @@ -192,8 +192,8 @@ ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c ap.CFLAGS += -DUSE_NAVIGATION -ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c -ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c +ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c +ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c ap.srcs += $(SRC_FIRMWARE)/ins.c ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile index 874b7d03d5..a6615c096f 100644 --- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile +++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile @@ -133,8 +133,8 @@ else ifeq ($(NUM_TYPE), float) endif sim.CFLAGS += -DUSE_NAVIGATION -sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c -sim.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c +sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c +sim.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c sim.srcs += math/pprz_geodetic_int.c sim.srcs += $(SRC_FIRMWARE)/ins.c diff --git a/conf/messages.xml b/conf/messages.xml index f605d8af96..81997e986e 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -936,7 +936,7 @@ - + @@ -1009,7 +1009,7 @@ - + diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index 5d446a61f5..0c8fa57b09 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -44,22 +44,22 @@ - - - - + + + + - - - - - - - - + + + + + + + + diff --git a/sw/airborne/booz/booz2_navigation.c b/sw/airborne/booz/booz2_navigation.c index 60f5fe0574..9da7297a0d 100644 --- a/sw/airborne/booz/booz2_navigation.c +++ b/sw/airborne/booz/booz2_navigation.c @@ -118,7 +118,7 @@ void booz2_nav_run(void) { int32_t dist_to_waypoint; INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint); -#ifndef B2_GUIDANCE_H_USE_REF +#ifndef GUIDANCE_H_USE_REF if (dist_to_waypoint < CLOSE_TO_WAYPOINT) { VECT2_COPY(booz2_navigation_carrot, booz2_navigation_target); } diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index 0e39c06f95..5c9b6539e4 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -35,7 +35,7 @@ #endif #include "autopilot.h" -#include "booz_guidance.h" +#include "guidance.h" #include "actuators.h" @@ -67,8 +67,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &autopilot_mode, \ &autopilot_in_flight, \ &autopilot_motors_on, \ - &booz2_guidance_h_mode, \ - &booz2_guidance_v_mode, \ + &guidance_h_mode, \ + &guidance_v_mode, \ &booz2_battery_voltage, \ &cpu_time_sec \ ); \ @@ -86,8 +86,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &autopilot_mode, \ &autopilot_in_flight, \ &autopilot_motors_on, \ - &booz2_guidance_h_mode, \ - &booz2_guidance_v_mode, \ + &guidance_h_mode, \ + &guidance_v_mode, \ &booz2_battery_voltage, \ &cpu_time_sec \ ); \ @@ -525,12 +525,12 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_BOOZ2_HFF_GPS(_chan) {} #endif -#define PERIODIC_SEND_BOOZ2_GUIDANCE(_chan) { \ - DOWNLINK_SEND_BOOZ2_GUIDANCE(_chan, \ - &booz2_guidance_h_cur_pos.x, \ - &booz2_guidance_h_cur_pos.y, \ - &booz2_guidance_h_held_pos.x, \ - &booz2_guidance_h_held_pos.y); \ +#define PERIODIC_SEND_GUIDANCE(_chan) { \ + DOWNLINK_SEND_GUIDANCE(_chan, \ + &guidance_h_cur_pos.x, \ + &guidance_h_cur_pos.y, \ + &guidance_h_held_pos.x, \ + &guidance_h_held_pos.y); \ } #define PERIODIC_SEND_INS(_chan) { \ @@ -605,64 +605,64 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; #define PERIODIC_SEND_BOOZ2_VERT_LOOP(_chan) { \ DOWNLINK_SEND_BOOZ2_VERT_LOOP(_chan, \ - &booz2_guidance_v_z_sp, \ - &booz2_guidance_v_zd_sp, \ + &guidance_v_z_sp, \ + &guidance_v_zd_sp, \ &ins_ltp_pos.z, \ &ins_ltp_speed.z, \ &ins_ltp_accel.z, \ - &booz2_guidance_v_z_ref, \ - &booz2_guidance_v_zd_ref, \ - &booz2_guidance_v_zdd_ref, \ + &guidance_v_z_ref, \ + &guidance_v_zd_ref, \ + &guidance_v_zdd_ref, \ &b2_gv_adapt_X, \ &b2_gv_adapt_P, \ &b2_gv_adapt_Xmeas, \ - &booz2_guidance_v_z_sum_err, \ - &booz2_guidance_v_ff_cmd, \ - &booz2_guidance_v_fb_cmd, \ - &booz2_guidance_v_delta_t); \ + &guidance_v_z_sum_err, \ + &guidance_v_ff_cmd, \ + &guidance_v_fb_cmd, \ + &guidance_v_delta_t); \ } #define PERIODIC_SEND_BOOZ2_HOVER_LOOP(_chan) { \ DOWNLINK_SEND_BOOZ2_HOVER_LOOP(_chan, \ - &booz2_guidance_h_pos_sp.x, \ - &booz2_guidance_h_pos_sp.y, \ + &guidance_h_pos_sp.x, \ + &guidance_h_pos_sp.y, \ &ins_ltp_pos.x, \ &ins_ltp_pos.y, \ &ins_ltp_speed.x, \ &ins_ltp_speed.y, \ &ins_ltp_accel.x, \ &ins_ltp_accel.y, \ - &booz2_guidance_h_pos_err.x, \ - &booz2_guidance_h_pos_err.y, \ - &booz2_guidance_h_speed_err.x, \ - &booz2_guidance_h_speed_err.y, \ - &booz2_guidance_h_pos_err_sum.x, \ - &booz2_guidance_h_pos_err_sum.y, \ - &booz2_guidance_h_nav_err.x, \ - &booz2_guidance_h_nav_err.y, \ - &booz2_guidance_h_command_earth.x, \ - &booz2_guidance_h_command_earth.y, \ - &booz2_guidance_h_command_body.phi, \ - &booz2_guidance_h_command_body.theta, \ - &booz2_guidance_h_command_body.psi); \ + &guidance_h_pos_err.x, \ + &guidance_h_pos_err.y, \ + &guidance_h_speed_err.x, \ + &guidance_h_speed_err.y, \ + &guidance_h_pos_err_sum.x, \ + &guidance_h_pos_err_sum.y, \ + &guidance_h_nav_err.x, \ + &guidance_h_nav_err.y, \ + &guidance_h_command_earth.x, \ + &guidance_h_command_earth.y, \ + &guidance_h_command_body.phi, \ + &guidance_h_command_body.theta, \ + &guidance_h_command_body.psi); \ } -#define PERIODIC_SEND_BOOZ2_GUIDANCE_H_REF(_chan) { \ - DOWNLINK_SEND_BOOZ2_GUIDANCE_H_REF_INT(_chan, \ - &booz2_guidance_h_pos_sp.x, \ - &booz2_guidance_h_pos_ref.x, \ - &booz2_guidance_h_speed_ref.x, \ - &booz2_guidance_h_accel_ref.x, \ - &booz2_guidance_h_pos_sp.y, \ - &booz2_guidance_h_pos_ref.y, \ - &booz2_guidance_h_speed_ref.y, \ - &booz2_guidance_h_accel_ref.y); \ +#define PERIODIC_SEND_GUIDANCE_H_REF(_chan) { \ + DOWNLINK_SEND_GUIDANCE_H_REF_INT(_chan, \ + &guidance_h_pos_sp.x, \ + &guidance_h_pos_ref.x, \ + &guidance_h_speed_ref.x, \ + &guidance_h_accel_ref.x, \ + &guidance_h_pos_sp.y, \ + &guidance_h_pos_ref.y, \ + &guidance_h_speed_ref.y, \ + &guidance_h_accel_ref.y); \ } #include "booz_gps.h" #include "booz2_navigation.h" #define PERIODIC_SEND_BOOZ2_FP(_chan) { \ - int32_t carrot_up = -booz2_guidance_v_z_sp; \ + int32_t carrot_up = -guidance_v_z_sp; \ DOWNLINK_SEND_BOOZ2_FP( _chan, \ &ins_enu_pos.x, \ &ins_enu_pos.y, \ @@ -673,10 +673,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel; &ahrs.ltp_to_body_euler.phi, \ &ahrs.ltp_to_body_euler.theta, \ &ahrs.ltp_to_body_euler.psi, \ - &booz2_guidance_h_pos_sp.y, \ - &booz2_guidance_h_pos_sp.x, \ + &guidance_h_pos_sp.y, \ + &guidance_h_pos_sp.x, \ &carrot_up, \ - &booz2_guidance_h_command_body.psi, \ + &guidance_h_command_body.psi, \ &booz_stabilization_cmd[COMMAND_THRUST], \ &autopilot_flight_time); \ } diff --git a/sw/airborne/booz/booz_fms.c b/sw/airborne/booz/booz_fms.c index d711e38f46..d0757c5aa1 100644 --- a/sw/airborne/booz/booz_fms.c +++ b/sw/airborne/booz/booz_fms.c @@ -41,9 +41,9 @@ void booz_fms_init(void) { fms.timeouted = TRUE; fms.last_msg = BOOZ_FMS_TIMEOUT; - fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; INT_EULERS_ZERO(fms.input.h_sp.attitude); - fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + fms.input.v_mode = GUIDANCE_V_MODE_CLIMB; fms.input.v_sp.climb = 0; booz_fms_impl_init(); @@ -55,9 +55,9 @@ void booz_fms_periodic(void) { fms.last_msg++; else { fms.timeouted = TRUE; - fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; INT_EULERS_ZERO(fms.input.h_sp.attitude); - fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + fms.input.v_mode = GUIDANCE_V_MODE_CLIMB; fms.input.v_sp.climb = 0; } #endif diff --git a/sw/airborne/booz/booz_fms.h b/sw/airborne/booz/booz_fms.h index 4f188bb3e9..f9a37da452 100644 --- a/sw/airborne/booz/booz_fms.h +++ b/sw/airborne/booz/booz_fms.h @@ -27,7 +27,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" #include "autopilot.h" -#include "booz_guidance.h" +#include "guidance.h" struct Booz_fms_imu_info { struct Int16Vect3 gyro; diff --git a/sw/airborne/booz/fms/booz_fms_datalink.h b/sw/airborne/booz/fms/booz_fms_datalink.h index 2ba0aa9c6e..d3223b3f1b 100644 --- a/sw/airborne/booz/fms/booz_fms_datalink.h +++ b/sw/airborne/booz/fms/booz_fms_datalink.h @@ -37,10 +37,10 @@ fms.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \ fms.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ switch (fms.input.h_mode) { \ - case BOOZ2_GUIDANCE_H_MODE_KILL: \ - case BOOZ2_GUIDANCE_H_MODE_RATE : \ + case GUIDANCE_H_MODE_KILL: \ + case GUIDANCE_H_MODE_RATE : \ break; \ - case BOOZ2_GUIDANCE_H_MODE_ATTITUDE : \ + case GUIDANCE_H_MODE_ATTITUDE : \ { \ fms.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ fms.input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ @@ -49,13 +49,13 @@ BOOZ_FMS_LIMIT_ATTITUDE(fms.input.h_sp.attitude); \ } \ break; \ - case BOOZ2_GUIDANCE_H_MODE_HOVER : \ + case GUIDANCE_H_MODE_HOVER : \ { \ fms.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ fms.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ } \ break; \ - case BOOZ2_GUIDANCE_H_MODE_NAV : \ + case GUIDANCE_H_MODE_NAV : \ { \ fms.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ fms.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ @@ -64,17 +64,17 @@ break; \ } \ switch (fms.input.v_mode) { \ - case BOOZ2_GUIDANCE_V_MODE_KILL: \ - case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: \ - case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: \ + case GUIDANCE_V_MODE_KILL: \ + case GUIDANCE_V_MODE_RC_DIRECT: \ + case GUIDANCE_V_MODE_RC_CLIMB: \ break; \ - case BOOZ2_GUIDANCE_V_MODE_CLIMB : \ + case GUIDANCE_V_MODE_CLIMB : \ fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ break; \ - case BOOZ2_GUIDANCE_V_MODE_HOVER : \ + case GUIDANCE_V_MODE_HOVER : \ fms.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ break; \ - case BOOZ2_GUIDANCE_V_MODE_NAV : \ + case GUIDANCE_V_MODE_NAV : \ fms.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ break; \ } \ @@ -82,8 +82,8 @@ #define BOOZ_FMS_NAV_STICK_PARSE_DL(_dl_buffer) { \ fms.last_msg = 0; \ - fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_NAV; \ - fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_NAV; \ + fms.input.h_mode = GUIDANCE_H_MODE_NAV; \ + fms.input.v_mode = GUIDANCE_V_MODE_NAV; \ fms.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \ fms.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \ fms.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \ diff --git a/sw/airborne/booz/fms/booz_fms_test_signal.c b/sw/airborne/booz/fms/booz_fms_test_signal.c index 1f7f15b405..4ad3a3db51 100644 --- a/sw/airborne/booz/fms/booz_fms_test_signal.c +++ b/sw/airborne/booz/fms/booz_fms_test_signal.c @@ -37,8 +37,8 @@ void booz_fms_impl_init(void) { fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD; fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; fms_test_signal.counter = 0; - fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; - fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; + fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; + fms.input.v_mode = GUIDANCE_V_MODE_HOVER; } void booz_fms_impl_periodic(void) { @@ -68,7 +68,7 @@ void booz_fms_impl_periodic(void) { break; #if 0 case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { - if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER) + if (guidance_v_mode < GUIDANCE_V_MODE_HOVER) booz_fms_test_signal_start_z = ins_ltp_pos.z; else { booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? diff --git a/sw/airborne/booz/test/test_vg_adpt.c b/sw/airborne/booz/test/test_vg_adpt.c index 95fc23cfe3..3dc12b69d3 100644 --- a/sw/airborne/booz/test/test_vg_adpt.c +++ b/sw/airborne/booz/test/test_vg_adpt.c @@ -27,8 +27,8 @@ #include "std.h" #include "booz_geometry_mixed.h" -#define B2_GUIDANCE_V_C -#include "booz2_guidance_v_adpt.h" +#define GUIDANCE_V_C +#include "guidance_v_adpt.h" diff --git a/sw/airborne/booz/test/test_vg_ref.c b/sw/airborne/booz/test/test_vg_ref.c index ec00dcb129..9fc0c946ad 100644 --- a/sw/airborne/booz/test/test_vg_ref.c +++ b/sw/airborne/booz/test/test_vg_ref.c @@ -24,8 +24,8 @@ #include #include "booz_geometry_mixed.h" -#define B2_GUIDANCE_V_C -#include "booz2_guidance_v_ref.h" +#define GUIDANCE_V_C +#include "guidance_v_ref.h" #define NB_STEP 10000 #define DT (1./B2_GV_FREQ) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index d33e24eaa9..5168a12fd5 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -27,7 +27,7 @@ #include "booz_radio_control.h" #include "booz2_commands.h" #include "booz2_navigation.h" -#include "booz_guidance.h" +#include "guidance.h" #include "booz_stabilization.h" #include "led.h" @@ -87,8 +87,8 @@ void autopilot_periodic(void) { autopilot_in_flight, autopilot_motors_on); } else { - booz2_guidance_v_run( autopilot_in_flight ); - booz2_guidance_h_run( autopilot_in_flight ); + guidance_v_run( autopilot_in_flight ); + guidance_h_run( autopilot_in_flight ); SetCommands(booz_stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } @@ -105,29 +105,29 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { #ifndef KILL_AS_FAILSAFE booz_stab_att_sp_euler.phi = 0; booz_stab_att_sp_euler.theta = 0; - booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE); + guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; #endif case AP_MODE_KILL: autopilot_motors_on = FALSE; - booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_KILL); + guidance_h_mode_changed(GUIDANCE_H_MODE_KILL); break; case AP_MODE_RATE_DIRECT: case AP_MODE_RATE_Z_HOLD: - booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_RATE); + guidance_h_mode_changed(GUIDANCE_H_MODE_RATE); break; case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_ATTITUDE_Z_HOLD: - booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_ATTITUDE); + guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE); break; case AP_MODE_HOVER_DIRECT: case AP_MODE_HOVER_CLIMB: case AP_MODE_HOVER_Z_HOLD: - booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_HOVER); + guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER); break; case AP_MODE_NAV: - booz2_guidance_h_mode_changed(BOOZ2_GUIDANCE_H_MODE_NAV); + guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; default: break; @@ -136,33 +136,33 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { switch (new_autopilot_mode) { case AP_MODE_FAILSAFE: #ifndef KILL_AS_FAILSAFE - booz2_guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB); + guidance_v_zd_sp = SPEED_BFP_OF_REAL(0.5); + guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; #endif case AP_MODE_KILL: - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_KILL); + guidance_v_mode_changed(GUIDANCE_V_MODE_KILL); break; case AP_MODE_RATE_DIRECT: case AP_MODE_ATTITUDE_DIRECT: case AP_MODE_HOVER_DIRECT: - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_DIRECT); + guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT); break; case AP_MODE_RATE_RC_CLIMB: case AP_MODE_ATTITUDE_RC_CLIMB: - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_RC_CLIMB); + guidance_v_mode_changed(GUIDANCE_V_MODE_RC_CLIMB); break; case AP_MODE_ATTITUDE_CLIMB: case AP_MODE_HOVER_CLIMB: - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_CLIMB); + guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB); break; case AP_MODE_RATE_Z_HOLD: case AP_MODE_ATTITUDE_Z_HOLD: case AP_MODE_HOVER_Z_HOLD: - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_HOVER); + guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER); break; case AP_MODE_NAV: - booz2_guidance_v_mode_changed(BOOZ2_GUIDANCE_V_MODE_NAV); + guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; default: break; @@ -252,8 +252,8 @@ void autopilot_on_rc_frame(void) { kill_throttle = !autopilot_motors_on; if (autopilot_mode > AP_MODE_FAILSAFE) { - booz2_guidance_v_read_rc(); - booz2_guidance_h_read_rc(autopilot_in_flight); + guidance_v_read_rc(); + guidance_h_read_rc(autopilot_in_flight); } } diff --git a/sw/airborne/firmwares/rotorcraft/guidance.h b/sw/airborne/firmwares/rotorcraft/guidance.h index 5069c1d3a4..bd8ce81b08 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance.h +++ b/sw/airborne/firmwares/rotorcraft/guidance.h @@ -1,7 +1,7 @@ -#ifndef BOOZ_GUIDANCE_H -#define BOOZ_GUIDANCE_H +#ifndef GUIDANCE_H +#define GUIDANCE_H -#include "guidance/booz2_guidance_h.h" -#include "guidance/booz2_guidance_v.h" +#include "guidance/guidance_h.h" +#include "guidance/guidance_v.h" -#endif /* BOOZ_GUIDANCE_H */ +#endif /* GUIDANCE_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 5ab9eaab74..0baaa912d6 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -21,9 +21,9 @@ * Boston, MA 02111-1307, USA. */ -#define B2_GUIDANCE_H_C -//#define B2_GUIDANCE_H_USE_REF -#include "booz2_guidance_h.h" +#define GUIDANCE_H_C +//#define GUIDANCE_H_USE_REF +#include "guidance_h.h" #include "ahrs.h" #include "booz_stabilization.h" @@ -33,74 +33,74 @@ #include "airframe.h" -uint8_t booz2_guidance_h_mode; +uint8_t guidance_h_mode; -struct Int32Vect2 booz2_guidance_h_pos_sp; -int32_t booz2_guidance_h_psi_sp; -struct Int32Vect2 booz2_guidance_h_pos_ref; -struct Int32Vect2 booz2_guidance_h_speed_ref; -struct Int32Vect2 booz2_guidance_h_accel_ref; +struct Int32Vect2 guidance_h_pos_sp; +int32_t guidance_h_psi_sp; +struct Int32Vect2 guidance_h_pos_ref; +struct Int32Vect2 guidance_h_speed_ref; +struct Int32Vect2 guidance_h_accel_ref; -struct Int32Vect2 booz2_guidance_h_pos_err; -struct Int32Vect2 booz2_guidance_h_speed_err; -struct Int32Vect2 booz2_guidance_h_pos_err_sum; -struct Int32Vect2 booz2_guidance_h_nav_err; +struct Int32Vect2 guidance_h_pos_err; +struct Int32Vect2 guidance_h_speed_err; +struct Int32Vect2 guidance_h_pos_err_sum; +struct Int32Vect2 guidance_h_nav_err; -struct Int32Eulers booz2_guidance_h_rc_sp; -struct Int32Vect2 booz2_guidance_h_command_earth; -struct Int32Vect2 booz2_guidance_h_stick_earth_sp; -struct Int32Eulers booz2_guidance_h_command_body; +struct Int32Eulers guidance_h_rc_sp; +struct Int32Vect2 guidance_h_command_earth; +struct Int32Vect2 guidance_h_stick_earth_sp; +struct Int32Eulers guidance_h_command_body; -int32_t booz2_guidance_h_pgain; -int32_t booz2_guidance_h_dgain; -int32_t booz2_guidance_h_igain; -int32_t booz2_guidance_h_ngain; -int32_t booz2_guidance_h_again; +int32_t guidance_h_pgain; +int32_t guidance_h_dgain; +int32_t guidance_h_igain; +int32_t guidance_h_ngain; +int32_t guidance_h_again; -#ifndef BOOZ2_GUIDANCE_H_NGAIN -#define BOOZ2_GUIDANCE_H_NGAIN 0 +#ifndef GUIDANCE_H_NGAIN +#define GUIDANCE_H_NGAIN 0 #endif -#ifndef BOOZ2_GUIDANCE_H_AGAIN -#define BOOZ2_GUIDANCE_H_AGAIN 0 +#ifndef GUIDANCE_H_AGAIN +#define GUIDANCE_H_AGAIN 0 #endif -static inline void booz2_guidance_h_hover_run(void); -static inline void booz2_guidance_h_nav_run(bool_t in_flight); -static inline void booz2_guidance_h_hover_enter(void); -static inline void booz2_guidance_h_nav_enter(void); +static inline void guidance_h_hover_run(void); +static inline void guidance_h_nav_run(bool_t in_flight); +static inline void guidance_h_hover_enter(void); +static inline void guidance_h_nav_enter(void); #define Booz2GuidanceHSetRef(_pos, _speed, _accel) { \ b2_gh_set_ref(_pos, _speed, _accel); \ - VECT2_COPY(booz2_guidance_h_pos_ref, _pos); \ - VECT2_COPY(booz2_guidance_h_speed_ref, _speed); \ - VECT2_COPY(booz2_guidance_h_accel_ref, _accel); \ + VECT2_COPY(guidance_h_pos_ref, _pos); \ + VECT2_COPY(guidance_h_speed_ref, _speed); \ + VECT2_COPY(guidance_h_accel_ref, _accel); \ } -void booz2_guidance_h_init(void) { +void guidance_h_init(void) { - booz2_guidance_h_mode = BOOZ2_GUIDANCE_H_MODE_KILL; - booz2_guidance_h_psi_sp = 0; - INT_VECT2_ZERO(booz2_guidance_h_pos_sp); - INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); - INT_EULERS_ZERO(booz2_guidance_h_rc_sp); - INT_EULERS_ZERO(booz2_guidance_h_command_body); - booz2_guidance_h_pgain = BOOZ2_GUIDANCE_H_PGAIN; - booz2_guidance_h_igain = BOOZ2_GUIDANCE_H_IGAIN; - booz2_guidance_h_dgain = BOOZ2_GUIDANCE_H_DGAIN; - booz2_guidance_h_ngain = BOOZ2_GUIDANCE_H_NGAIN; - booz2_guidance_h_again = BOOZ2_GUIDANCE_H_AGAIN; + guidance_h_mode = GUIDANCE_H_MODE_KILL; + guidance_h_psi_sp = 0; + INT_VECT2_ZERO(guidance_h_pos_sp); + INT_VECT2_ZERO(guidance_h_pos_err_sum); + INT_EULERS_ZERO(guidance_h_rc_sp); + INT_EULERS_ZERO(guidance_h_command_body); + guidance_h_pgain = GUIDANCE_H_PGAIN; + guidance_h_igain = GUIDANCE_H_IGAIN; + guidance_h_dgain = GUIDANCE_H_DGAIN; + guidance_h_ngain = GUIDANCE_H_NGAIN; + guidance_h_again = GUIDANCE_H_AGAIN; } -void booz2_guidance_h_mode_changed(uint8_t new_mode) { - if (new_mode == booz2_guidance_h_mode) +void guidance_h_mode_changed(uint8_t new_mode) { + if (new_mode == guidance_h_mode) return; - switch ( booz2_guidance_h_mode ) { - // case BOOZ2_GUIDANCE_H_MODE_RATE: + switch ( guidance_h_mode ) { + // case GUIDANCE_H_MODE_RATE: // booz_stabilization_rate_exit(); // break; default: @@ -109,53 +109,53 @@ void booz2_guidance_h_mode_changed(uint8_t new_mode) { switch (new_mode) { - case BOOZ2_GUIDANCE_H_MODE_RATE: + case GUIDANCE_H_MODE_RATE: booz_stabilization_rate_enter(); break; - case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: + case GUIDANCE_H_MODE_ATTITUDE: booz_stabilization_attitude_enter(); break; - case BOOZ2_GUIDANCE_H_MODE_HOVER: - booz2_guidance_h_hover_enter(); + case GUIDANCE_H_MODE_HOVER: + guidance_h_hover_enter(); break; - case BOOZ2_GUIDANCE_H_MODE_NAV: - booz2_guidance_h_nav_enter(); + case GUIDANCE_H_MODE_NAV: + guidance_h_nav_enter(); break; default: break; } - booz2_guidance_h_mode = new_mode; + guidance_h_mode = new_mode; } -void booz2_guidance_h_read_rc(bool_t in_flight) { +void guidance_h_read_rc(bool_t in_flight) { - switch ( booz2_guidance_h_mode ) { + switch ( guidance_h_mode ) { - case BOOZ2_GUIDANCE_H_MODE_RATE: + case GUIDANCE_H_MODE_RATE: booz_stabilization_rate_read_rc(); break; - case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: + case GUIDANCE_H_MODE_ATTITUDE: booz_stabilization_attitude_read_rc(in_flight); break; - case BOOZ2_GUIDANCE_H_MODE_HOVER: - BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight); + case GUIDANCE_H_MODE_HOVER: + BOOZ_STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight); break; - case BOOZ2_GUIDANCE_H_MODE_NAV: + case GUIDANCE_H_MODE_NAV: if (radio_control.status == RADIO_CONTROL_OK) { - BOOZ_STABILIZATION_ATTITUDE_READ_RC(booz2_guidance_h_rc_sp, in_flight); - booz2_guidance_h_rc_sp.psi = 0; + BOOZ_STABILIZATION_ATTITUDE_READ_RC(guidance_h_rc_sp, in_flight); + guidance_h_rc_sp.psi = 0; } else { - INT_EULERS_ZERO(booz2_guidance_h_rc_sp); + INT_EULERS_ZERO(guidance_h_rc_sp); } break; default: @@ -165,25 +165,25 @@ void booz2_guidance_h_read_rc(bool_t in_flight) { } -void booz2_guidance_h_run(bool_t in_flight) { - switch ( booz2_guidance_h_mode ) { +void guidance_h_run(bool_t in_flight) { + switch ( guidance_h_mode ) { - case BOOZ2_GUIDANCE_H_MODE_RATE: + case GUIDANCE_H_MODE_RATE: booz_stabilization_rate_run(in_flight); break; - case BOOZ2_GUIDANCE_H_MODE_ATTITUDE: + case GUIDANCE_H_MODE_ATTITUDE: booz_stabilization_attitude_run(in_flight); break; - case BOOZ2_GUIDANCE_H_MODE_HOVER: - booz2_guidance_h_hover_run(); + case GUIDANCE_H_MODE_HOVER: + guidance_h_hover_run(); booz_stabilization_attitude_run(in_flight); break; - case BOOZ2_GUIDANCE_H_MODE_NAV: + case GUIDANCE_H_MODE_NAV: { - if (!in_flight) booz2_guidance_h_nav_enter(); + if (!in_flight) guidance_h_nav_enter(); if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) { #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT @@ -192,15 +192,15 @@ void booz2_guidance_h_run(bool_t in_flight) { #endif } else { - INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot); -#ifdef B2_GUIDANCE_H_USE_REF - b2_gh_update_ref_from_pos_sp(booz2_guidance_h_pos_sp); + INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, booz2_navigation_carrot); +#ifdef GUIDANCE_H_USE_REF + b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp); #endif #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT - booz2_guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); + guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); #endif - //booz2_guidance_h_hover_run(); - booz2_guidance_h_nav_run(in_flight); + //guidance_h_hover_run(); + guidance_h_nav_run(in_flight); } booz_stabilization_attitude_run(in_flight); break; @@ -221,33 +221,33 @@ void booz2_guidance_h_run(bool_t in_flight) { //#define MAX_BANK (65536) #define MAX_BANK (98000) -static inline void booz2_guidance_h_hover_run(void) { +static inline void guidance_h_hover_run(void) { /* compute position error */ - VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_sp); + VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_sp); /* saturate it */ - VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); + VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); /* compute speed error */ - VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed); + VECT2_COPY(guidance_h_speed_err, ins_ltp_speed); /* saturate it */ - VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); + VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); /* update pos error integral */ - VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err); + VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); /* saturate it */ - VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); + VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); /* run PID */ // cmd_earth < 15.17 - booz2_guidance_h_command_earth.x = (booz2_guidance_h_pgain<<1) * booz2_guidance_h_pos_err.x + - booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x>>9) + - booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> 12); - booz2_guidance_h_command_earth.y = (booz2_guidance_h_pgain<<1) * booz2_guidance_h_pos_err.y + - booz2_guidance_h_dgain *( booz2_guidance_h_speed_err.y>>9) + - booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> 12); + guidance_h_command_earth.x = (guidance_h_pgain<<1) * guidance_h_pos_err.x + + guidance_h_dgain * (guidance_h_speed_err.x>>9) + + guidance_h_igain * (guidance_h_pos_err_sum.x >> 12); + guidance_h_command_earth.y = (guidance_h_pgain<<1) * guidance_h_pos_err.y + + guidance_h_dgain *( guidance_h_speed_err.y>>9) + + guidance_h_igain * (guidance_h_pos_err_sum.y >> 12); - VECT2_STRIM(booz2_guidance_h_command_earth, -MAX_BANK, MAX_BANK); + VECT2_STRIM(guidance_h_command_earth, -MAX_BANK, MAX_BANK); /* Rotate to body frame */ int32_t s_psi, c_psi; @@ -256,22 +256,22 @@ static inline void booz2_guidance_h_hover_run(void) { // INT32_TRIG_FRAC - 2: 100mm erreur, gain 100 -> 10000 command | 2 degres = 36000, so multiply by 4 - booz2_guidance_h_command_body.phi = - ( - s_psi * booz2_guidance_h_command_earth.x + c_psi * booz2_guidance_h_command_earth.y) + guidance_h_command_body.phi = + ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - 2); - booz2_guidance_h_command_body.theta = - - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y) + guidance_h_command_body.theta = + - ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - 2); - booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi; - booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta; - booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi; + guidance_h_command_body.phi += guidance_h_rc_sp.phi; + guidance_h_command_body.theta += guidance_h_rc_sp.theta; + guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi; #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT - ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi); + ANGLE_REF_NORMALIZE(guidance_h_command_body.psi); #endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */ - EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body); + EULERS_COPY(booz_stab_att_sp_euler, guidance_h_command_body); } @@ -279,72 +279,72 @@ static inline void booz2_guidance_h_hover_run(void) { #define NAV_MAX_BANK BFP_OF_REAL(0.35,REF_ANGLE_FRAC) #define HOLD_DISTANCE POS_BFP_OF_REAL(10.) -static inline void booz2_guidance_h_nav_run(bool_t in_flight) { +static inline void guidance_h_nav_run(bool_t in_flight) { /* convert our reference to generic representation */ -#ifdef B2_GUIDANCE_H_USE_REF - INT32_VECT2_RSHIFT(booz2_guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC)); - INT32_VECT2_LSHIFT(booz2_guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC)); - INT32_VECT2_LSHIFT(booz2_guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC)); +#ifdef GUIDANCE_H_USE_REF + INT32_VECT2_RSHIFT(guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC)); + INT32_VECT2_LSHIFT(guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC)); + INT32_VECT2_LSHIFT(guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC)); #else - VECT2_COPY(booz2_guidance_h_pos_ref, booz2_guidance_h_pos_sp); - INT_VECT2_ZERO(booz2_guidance_h_speed_ref); - INT_VECT2_ZERO(booz2_guidance_h_accel_ref); + VECT2_COPY(guidance_h_pos_ref, guidance_h_pos_sp); + INT_VECT2_ZERO(guidance_h_speed_ref); + INT_VECT2_ZERO(guidance_h_accel_ref); #endif /* compute position error */ - VECT2_DIFF(booz2_guidance_h_pos_err, ins_ltp_pos, booz2_guidance_h_pos_ref); + VECT2_DIFF(guidance_h_pos_err, ins_ltp_pos, guidance_h_pos_ref); /* saturate it */ - VECT2_STRIM(booz2_guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); + VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR); /* compute speed error */ - //VECT2_COPY(booz2_guidance_h_speed_err, ins_ltp_speed); - VECT2_DIFF(booz2_guidance_h_speed_err, ins_ltp_speed, booz2_guidance_h_speed_ref); + //VECT2_COPY(guidance_h_speed_err, ins_ltp_speed); + VECT2_DIFF(guidance_h_speed_err, ins_ltp_speed, guidance_h_speed_ref); /* saturate it */ - VECT2_STRIM(booz2_guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); + VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR); int32_t dist; - INT32_VECT2_NORM(dist, booz2_guidance_h_pos_err); + INT32_VECT2_NORM(dist, guidance_h_pos_err); if ( dist < HOLD_DISTANCE ) { // Hold position /* update pos error integral */ - VECT2_ADD(booz2_guidance_h_pos_err_sum, booz2_guidance_h_pos_err); + VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err); /* saturate it */ - VECT2_STRIM(booz2_guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); + VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM); } else { // Tracking algorithm, no integral int32_t vect_prod = 0; - int32_t scal_prod = ins_ltp_speed.x * booz2_guidance_h_pos_err.x + ins_ltp_speed.y * booz2_guidance_h_pos_err.y; + int32_t scal_prod = ins_ltp_speed.x * guidance_h_pos_err.x + ins_ltp_speed.y * guidance_h_pos_err.y; // compute vectorial product only if angle < pi/2 (scalar product > 0) if (scal_prod >= 0) { - vect_prod = ((ins_ltp_speed.x * booz2_guidance_h_pos_err.y) >> (INT32_POS_FRAC + INT32_SPEED_FRAC - 10)) - - ((ins_ltp_speed.y * booz2_guidance_h_pos_err.x) >> (INT32_POS_FRAC + INT32_SPEED_FRAC - 10)); + vect_prod = ((ins_ltp_speed.x * guidance_h_pos_err.y) >> (INT32_POS_FRAC + INT32_SPEED_FRAC - 10)) + - ((ins_ltp_speed.y * guidance_h_pos_err.x) >> (INT32_POS_FRAC + INT32_SPEED_FRAC - 10)); } // multiply by vector orthogonal to speed - VECT2_ASSIGN(booz2_guidance_h_nav_err, + VECT2_ASSIGN(guidance_h_nav_err, vect_prod * (-ins_ltp_speed.y), vect_prod * ins_ltp_speed.x); // divide by 2 times dist ( >> 16 ) - VECT2_SDIV(booz2_guidance_h_nav_err, booz2_guidance_h_nav_err, dist*dist); + VECT2_SDIV(guidance_h_nav_err, guidance_h_nav_err, dist*dist); // *2 ?? } - if (!in_flight) { INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); } + if (!in_flight) { INT_VECT2_ZERO(guidance_h_pos_err_sum); } /* run PID */ - booz2_guidance_h_command_earth.x = - booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.x << (10 - INT32_POS_FRAC)) + - booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.x >> (INT32_SPEED_FRAC - 10)) + - booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - 10)) + - booz2_guidance_h_ngain * booz2_guidance_h_nav_err.x + - booz2_guidance_h_again * booz2_guidance_h_accel_ref.x; /* feedforward gain */ - booz2_guidance_h_command_earth.y = - booz2_guidance_h_pgain * (booz2_guidance_h_pos_err.y << (10 - INT32_POS_FRAC)) + - booz2_guidance_h_dgain * (booz2_guidance_h_speed_err.y >> (INT32_SPEED_FRAC - 10)) + - booz2_guidance_h_igain * (booz2_guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - 10)) + - booz2_guidance_h_ngain * booz2_guidance_h_nav_err.y + - booz2_guidance_h_again * booz2_guidance_h_accel_ref.y; /* feedforward gain */ + guidance_h_command_earth.x = + guidance_h_pgain * (guidance_h_pos_err.x << (10 - INT32_POS_FRAC)) + + guidance_h_dgain * (guidance_h_speed_err.x >> (INT32_SPEED_FRAC - 10)) + + guidance_h_igain * (guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - 10)) + + guidance_h_ngain * guidance_h_nav_err.x + + guidance_h_again * guidance_h_accel_ref.x; /* feedforward gain */ + guidance_h_command_earth.y = + guidance_h_pgain * (guidance_h_pos_err.y << (10 - INT32_POS_FRAC)) + + guidance_h_dgain * (guidance_h_speed_err.y >> (INT32_SPEED_FRAC - 10)) + + guidance_h_igain * (guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - 10)) + + guidance_h_ngain * guidance_h_nav_err.y + + guidance_h_again * guidance_h_accel_ref.y; /* feedforward gain */ - VECT2_STRIM(booz2_guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK); - INT32_VECT2_RSHIFT(booz2_guidance_h_command_earth, booz2_guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for trigo operation + VECT2_STRIM(guidance_h_command_earth, -NAV_MAX_BANK, NAV_MAX_BANK); + INT32_VECT2_RSHIFT(guidance_h_command_earth, guidance_h_command_earth, REF_ANGLE_FRAC - 16); // Reduice to 16 for trigo operation /* Rotate to body frame */ int32_t s_psi, c_psi; @@ -352,35 +352,35 @@ static inline void booz2_guidance_h_nav_run(bool_t in_flight) { PPRZ_ITRIG_COS(c_psi, ahrs.ltp_to_body_euler.psi); // Restore angle ref resolution after rotation - booz2_guidance_h_command_body.phi = - ( - s_psi * booz2_guidance_h_command_earth.x + c_psi * booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16)); - booz2_guidance_h_command_body.theta = - - ( c_psi * booz2_guidance_h_command_earth.x + s_psi * booz2_guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16)); + guidance_h_command_body.phi = + ( - s_psi * guidance_h_command_earth.x + c_psi * guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16)); + guidance_h_command_body.theta = + - ( c_psi * guidance_h_command_earth.x + s_psi * guidance_h_command_earth.y) >> (INT32_TRIG_FRAC - (REF_ANGLE_FRAC - 16)); // Add RC setpoint - booz2_guidance_h_command_body.phi += booz2_guidance_h_rc_sp.phi; - booz2_guidance_h_command_body.theta += booz2_guidance_h_rc_sp.theta; - booz2_guidance_h_command_body.psi = booz2_guidance_h_psi_sp + booz2_guidance_h_rc_sp.psi; - ANGLE_REF_NORMALIZE(booz2_guidance_h_command_body.psi); + guidance_h_command_body.phi += guidance_h_rc_sp.phi; + guidance_h_command_body.theta += guidance_h_rc_sp.theta; + guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi; + ANGLE_REF_NORMALIZE(guidance_h_command_body.psi); // Set attitude setpoint - EULERS_COPY(booz_stab_att_sp_euler, booz2_guidance_h_command_body); + EULERS_COPY(booz_stab_att_sp_euler, guidance_h_command_body); } -static inline void booz2_guidance_h_hover_enter(void) { +static inline void guidance_h_hover_enter(void) { - VECT2_COPY(booz2_guidance_h_pos_sp, ins_ltp_pos); + VECT2_COPY(guidance_h_pos_sp, ins_ltp_pos); - BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( booz2_guidance_h_rc_sp ); + BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( guidance_h_rc_sp ); - INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_pos_err_sum); } -static inline void booz2_guidance_h_nav_enter(void) { +static inline void guidance_h_nav_enter(void) { - INT32_VECT2_NED_OF_ENU(booz2_guidance_h_pos_sp, booz2_navigation_carrot); + INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, booz2_navigation_carrot); struct Int32Vect2 pos,speed,zero; INT_VECT2_ZERO(zero); VECT2_COPY(pos, ins_ltp_pos); @@ -389,12 +389,12 @@ static inline void booz2_guidance_h_nav_enter(void) { struct Int32Eulers tmp_sp; BOOZ_STABILIZATION_ATTITUDE_RESET_PSI_REF( tmp_sp ); - booz2_guidance_h_psi_sp = tmp_sp.psi; + guidance_h_psi_sp = tmp_sp.psi; #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT - nav_heading = (booz2_guidance_h_psi_sp >> (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); + nav_heading = (guidance_h_psi_sp >> (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)); #endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */ - booz2_guidance_h_rc_sp.psi = 0; + guidance_h_rc_sp.psi = 0; - INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); + INT_VECT2_ZERO(guidance_h_pos_err_sum); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 5ca8adcaf9..c074779a21 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -21,56 +21,56 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ2_GUIDANCE_H_H -#define BOOZ2_GUIDANCE_H_H +#ifndef GUIDANCE_H_H +#define GUIDANCE_H_H #include "math/pprz_algebra_int.h" -#include "booz2_guidance_h_ref.h" +#include "guidance_h_ref.h" -#define BOOZ2_GUIDANCE_H_MODE_KILL 0 -#define BOOZ2_GUIDANCE_H_MODE_RATE 1 -#define BOOZ2_GUIDANCE_H_MODE_ATTITUDE 2 -#define BOOZ2_GUIDANCE_H_MODE_HOVER 3 -#define BOOZ2_GUIDANCE_H_MODE_NAV 4 +#define GUIDANCE_H_MODE_KILL 0 +#define GUIDANCE_H_MODE_RATE 1 +#define GUIDANCE_H_MODE_ATTITUDE 2 +#define GUIDANCE_H_MODE_HOVER 3 +#define GUIDANCE_H_MODE_NAV 4 -extern uint8_t booz2_guidance_h_mode; +extern uint8_t guidance_h_mode; /* horizontal setpoint in NED */ /* Q_int32_xx_8 */ -extern struct Int32Vect2 booz2_guidance_h_pos_sp; -extern int32_t booz2_guidance_h_psi_sp; -extern struct Int32Vect2 booz2_guidance_h_pos_ref; -extern struct Int32Vect2 booz2_guidance_h_speed_ref; -extern struct Int32Vect2 booz2_guidance_h_accel_ref; +extern struct Int32Vect2 guidance_h_pos_sp; +extern int32_t guidance_h_psi_sp; +extern struct Int32Vect2 guidance_h_pos_ref; +extern struct Int32Vect2 guidance_h_speed_ref; +extern struct Int32Vect2 guidance_h_accel_ref; -extern struct Int32Vect2 booz2_guidance_h_pos_err; -extern struct Int32Vect2 booz2_guidance_h_speed_err; -extern struct Int32Vect2 booz2_guidance_h_pos_err_sum; -extern struct Int32Vect2 booz2_guidance_h_nav_err; +extern struct Int32Vect2 guidance_h_pos_err; +extern struct Int32Vect2 guidance_h_speed_err; +extern struct Int32Vect2 guidance_h_pos_err_sum; +extern struct Int32Vect2 guidance_h_nav_err; -extern struct Int32Eulers booz2_guidance_h_rc_sp; -extern struct Int32Vect2 booz2_guidance_h_command_earth; -extern struct Int32Eulers booz2_guidance_h_command_body; +extern struct Int32Eulers guidance_h_rc_sp; +extern struct Int32Vect2 guidance_h_command_earth; +extern struct Int32Eulers guidance_h_command_body; -extern int32_t booz2_guidance_h_pgain; -extern int32_t booz2_guidance_h_dgain; -extern int32_t booz2_guidance_h_igain; -extern int32_t booz2_guidance_h_ngain; -extern int32_t booz2_guidance_h_again; +extern int32_t guidance_h_pgain; +extern int32_t guidance_h_dgain; +extern int32_t guidance_h_igain; +extern int32_t guidance_h_ngain; +extern int32_t guidance_h_again; -extern void booz2_guidance_h_init(void); -extern void booz2_guidance_h_mode_changed(uint8_t new_mode); -extern void booz2_guidance_h_read_rc(bool_t in_flight); -extern void booz2_guidance_h_run(bool_t in_flight); +extern void guidance_h_init(void); +extern void guidance_h_mode_changed(uint8_t new_mode); +extern void guidance_h_read_rc(bool_t in_flight); +extern void guidance_h_run(bool_t in_flight); -#define booz2_guidance_h_SetKi(_val) { \ - booz2_guidance_h_igain = _val; \ - INT_VECT2_ZERO(booz2_guidance_h_pos_err_sum); \ +#define guidance_h_SetKi(_val) { \ + guidance_h_igain = _val; \ + INT_VECT2_ZERO(guidance_h_pos_err_sum); \ } -#endif /* BOOZ2_GUIDANCE_H_H */ +#endif /* GUIDANCE_H_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h index 8b13969ceb..6861767740 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h @@ -1,5 +1,5 @@ /* - * $Id: booz2_guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $ + * $Id: guidance_v_ref.h 4173 2009-09-18 11:57:21Z flixr $ * * Copyright (C) 2008-2009 ENAC * @@ -21,8 +21,8 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ2_GUIDANCE_H_REF_H -#define BOOZ2_GUIDANCE_H_REF_H +#ifndef GUIDANCE_H_REF_H +#define GUIDANCE_H_REF_H #include "airframe.h" #include "inttypes.h" @@ -50,34 +50,34 @@ extern struct Int64Vect2 b2_gh_pos_ref; #define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC) /* Saturations definition */ -#ifndef BOOZ2_GUIDANCE_H_REF_MAX_ACCEL -#define BOOZ2_GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 ) +#ifndef GUIDANCE_H_REF_MAX_ACCEL +#define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 ) #endif -#define B2_GH_MAX_ACCEL BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC) +#define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC) -#ifndef BOOZ2_GUIDANCE_H_REF_MAX_SPEED -#define BOOZ2_GUIDANCE_H_REF_MAX_SPEED ( 5. ) +#ifndef GUIDANCE_H_REF_MAX_SPEED +#define GUIDANCE_H_REF_MAX_SPEED ( 5. ) #endif -#define B2_GH_MAX_SPEED BFP_OF_REAL(BOOZ2_GUIDANCE_H_REF_MAX_SPEED, B2_GH_SPEED_REF_FRAC) +#define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, B2_GH_SPEED_REF_FRAC) /* second order model natural frequency and damping */ -#ifndef BOOZ2_GUIDANCE_H_REF_OMEGA -#define BOOZ2_GUIDANCE_H_REF_OMEGA RadOfDeg(67.) +#ifndef GUIDANCE_H_REF_OMEGA +#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.) #endif -#ifndef BOOZ2_GUIDANCE_H_REF_ZETA -#define BOOZ2_GUIDANCE_H_REF_ZETA 0.85 +#ifndef GUIDANCE_H_REF_ZETA +#define GUIDANCE_H_REF_ZETA 0.85 #endif #define B2_GH_ZETA_OMEGA_FRAC 10 -#define B2_GH_ZETA_OMEGA BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_ZETA*BOOZ2_GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC) +#define B2_GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC) #define B2_GH_OMEGA_2_FRAC 7 -#define B2_GH_OMEGA_2 BFP_OF_REAL((BOOZ2_GUIDANCE_H_REF_OMEGA*BOOZ2_GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC) +#define B2_GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC) /* first order time constant */ #define B2_GH_REF_THAU_F 0.5 #define B2_GH_REF_INV_THAU_FRAC 16 #define B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F), B2_GH_REF_INV_THAU_FRAC) -#ifdef B2_GUIDANCE_H_C +#ifdef GUIDANCE_H_C static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel); static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp); static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp); @@ -186,6 +186,6 @@ static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { } } -#endif /* B2_GUIDANCE_H_C */ +#endif /* GUIDANCE_H_C */ -#endif /* BOOZ2_GUIDANCE_H_REF_H */ +#endif /* GUIDANCE_H_REF_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 9f642ddbde..87593a2928 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -21,9 +21,9 @@ * Boston, MA 02111-1307, USA. */ -#define B2_GUIDANCE_V_C -#define B2_GUIDANCE_V_USE_REF -#include "booz2_guidance_v.h" +#define GUIDANCE_V_C +#define GUIDANCE_V_USE_REF +#include "guidance_v.h" #include "booz_radio_control.h" @@ -37,95 +37,95 @@ #include "airframe.h" -uint8_t booz2_guidance_v_mode; -int32_t booz2_guidance_v_ff_cmd; -int32_t booz2_guidance_v_fb_cmd; +uint8_t guidance_v_mode; +int32_t guidance_v_ff_cmd; +int32_t guidance_v_fb_cmd; /* command output */ -int32_t booz2_guidance_v_delta_t; +int32_t guidance_v_delta_t; /* direct throttle from radio control */ /* range 0:200 */ -int32_t booz2_guidance_v_rc_delta_t; +int32_t guidance_v_rc_delta_t; /* vertical speed setpoint from radio control */ /* Q12.19 : accuracy 0.0000019, range +/-4096 */ -int32_t booz2_guidance_v_rc_zd_sp; +int32_t guidance_v_rc_zd_sp; /* altitude setpoint in meter (input) */ /* Q23.8 : accuracy 0.0039, range 8388km */ -int32_t booz2_guidance_v_z_sp; +int32_t guidance_v_z_sp; /* vertical speed setpoint in meter/s (input) */ /* Q12.19 : accuracy 0.0000019, range +/-4096 */ -int32_t booz2_guidance_v_zd_sp; -#define BOOZ2_GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC +int32_t guidance_v_zd_sp; +#define GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC /* altitude reference in meter */ /* Q23.8 : accuracy 0.0039, range 8388km */ -int32_t booz2_guidance_v_z_ref; +int32_t guidance_v_z_ref; /* vertical speed reference in meter/s */ /* Q12.19 : accuracy 0.0000038, range 4096 */ -int32_t booz2_guidance_v_zd_ref; +int32_t guidance_v_zd_ref; /* vertical acceleration reference in meter/s^2 */ /* Q21.10 : accuracy 0.0009766, range 2097152 */ -int32_t booz2_guidance_v_zdd_ref; +int32_t guidance_v_zdd_ref; -int32_t booz2_guidance_v_kp; -int32_t booz2_guidance_v_kd; -int32_t booz2_guidance_v_ki; +int32_t guidance_v_kp; +int32_t guidance_v_kd; +int32_t guidance_v_ki; -int32_t booz2_guidance_v_z_sum_err; +int32_t guidance_v_z_sum_err; #define Booz2GuidanceVSetRef(_pos, _speed, _accel) { \ b2_gv_set_ref(_pos, _speed, _accel); \ - booz2_guidance_v_z_ref = _pos; \ - booz2_guidance_v_zd_ref = _speed; \ - booz2_guidance_v_zdd_ref = _accel; \ + guidance_v_z_ref = _pos; \ + guidance_v_zd_ref = _speed; \ + guidance_v_zdd_ref = _accel; \ } static inline void run_hover_loop(bool_t in_flight); -void booz2_guidance_v_init(void) { +void guidance_v_init(void) { - booz2_guidance_v_mode = BOOZ2_GUIDANCE_V_MODE_KILL; + guidance_v_mode = GUIDANCE_V_MODE_KILL; - booz2_guidance_v_kp = BOOZ2_GUIDANCE_V_HOVER_KP; - booz2_guidance_v_kd = BOOZ2_GUIDANCE_V_HOVER_KD; - booz2_guidance_v_ki = BOOZ2_GUIDANCE_V_HOVER_KI; + guidance_v_kp = GUIDANCE_V_HOVER_KP; + guidance_v_kd = GUIDANCE_V_HOVER_KD; + guidance_v_ki = GUIDANCE_V_HOVER_KI; - booz2_guidance_v_z_sum_err = 0; + guidance_v_z_sum_err = 0; b2_gv_adapt_init(); } -void booz2_guidance_v_read_rc(void) { +void guidance_v_read_rc(void) { // used in RC_DIRECT directly and as saturation in CLIMB and HOVER - booz2_guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 200 / MAX_PPRZ; + guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE] * 200 / MAX_PPRZ; // used in RC_CLIMB - booz2_guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) * - BOOZ2_GUIDANCE_V_RC_CLIMB_COEF; - DeadBand(booz2_guidance_v_rc_zd_sp, BOOZ2_GUIDANCE_V_RC_CLIMB_DEAD_BAND); + guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_CONTROL_THROTTLE]) * + GUIDANCE_V_RC_CLIMB_COEF; + DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND); } -void booz2_guidance_v_mode_changed(uint8_t new_mode) { +void guidance_v_mode_changed(uint8_t new_mode) { - if (new_mode == booz2_guidance_v_mode) + if (new_mode == guidance_v_mode) return; - // switch ( booz2_guidance_v_mode ) { + // switch ( guidance_v_mode ) { // // } switch (new_mode) { - case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: - case BOOZ2_GUIDANCE_V_MODE_CLIMB: - case BOOZ2_GUIDANCE_V_MODE_HOVER: - case BOOZ2_GUIDANCE_V_MODE_NAV: - booz2_guidance_v_z_sum_err = 0; + case GUIDANCE_V_MODE_RC_CLIMB: + case GUIDANCE_V_MODE_CLIMB: + case GUIDANCE_V_MODE_HOVER: + case GUIDANCE_V_MODE_NAV: + guidance_v_z_sum_err = 0; Booz2GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0); break; default: @@ -133,17 +133,17 @@ void booz2_guidance_v_mode_changed(uint8_t new_mode) { } - booz2_guidance_v_mode = new_mode; + guidance_v_mode = new_mode; } -void booz2_guidance_v_notify_in_flight( bool_t in_flight) { +void guidance_v_notify_in_flight( bool_t in_flight) { if (in_flight) b2_gv_adapt_init(); } -void booz2_guidance_v_run(bool_t in_flight) { +void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co @@ -157,65 +157,65 @@ void booz2_guidance_v_run(bool_t in_flight) { //ins_vf_realign = TRUE; } - switch (booz2_guidance_v_mode) { + switch (guidance_v_mode) { - case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: - booz2_guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that + case GUIDANCE_V_MODE_RC_DIRECT: + guidance_v_z_sp = ins_ltp_pos.z; // not sure why we do that Booz2GuidanceVSetRef(ins_ltp_pos.z, 0, 0); // or that - mode enter should take care of it ? - booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_rc_delta_t; + booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; break; - case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: - booz2_guidance_v_zd_sp = booz2_guidance_v_rc_zd_sp; - b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp); + case GUIDANCE_V_MODE_RC_CLIMB: + guidance_v_zd_sp = guidance_v_rc_zd_sp; + b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); - booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t; + booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; - case BOOZ2_GUIDANCE_V_MODE_CLIMB: + case GUIDANCE_V_MODE_CLIMB: #ifdef USE_FMS - if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_CLIMB) - booz2_guidance_v_zd_sp = fms.input.v_sp.climb; + if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB) + guidance_v_zd_sp = fms.input.v_sp.climb; #endif - b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp); + b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp); run_hover_loop(in_flight); // saturate max authority with RC stick - booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t); + booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t); break; - case BOOZ2_GUIDANCE_V_MODE_HOVER: + case GUIDANCE_V_MODE_HOVER: #ifdef USE_FMS - if (fms.enabled && fms.input.v_mode == BOOZ2_GUIDANCE_V_MODE_HOVER) - booz2_guidance_v_z_sp = fms.input.v_sp.height; + if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER) + guidance_v_z_sp = fms.input.v_sp.height; #endif - b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp); + b2_gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); // saturate max authority with RC stick - booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t); + booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t); break; - case BOOZ2_GUIDANCE_V_MODE_NAV: + case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { - booz2_guidance_v_z_sp = -nav_flight_altitude; - b2_gv_update_ref_from_z_sp(booz2_guidance_v_z_sp); + guidance_v_z_sp = -nav_flight_altitude; + b2_gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_CLIMB) { - booz2_guidance_v_zd_sp = -nav_climb; - b2_gv_update_ref_from_zd_sp(booz2_guidance_v_zd_sp); - nav_flight_altitude = -booz2_guidance_v_z_sp; + guidance_v_zd_sp = -nav_climb; + b2_gv_update_ref_from_zd_sp(guidance_v_zd_sp); + nav_flight_altitude = -guidance_v_z_sp; run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_MANUAL) { - booz2_guidance_v_z_sp = -nav_flight_altitude; // For display only - booz2_guidance_v_delta_t = nav_throttle; + guidance_v_z_sp = -nav_flight_altitude; // For display only + guidance_v_delta_t = nav_throttle; } /* use rc limitation if available */ if (radio_control.status == RADIO_CONTROL_OK) - booz_stabilization_cmd[COMMAND_THRUST] = Min( booz2_guidance_v_rc_delta_t, booz2_guidance_v_delta_t); + booz_stabilization_cmd[COMMAND_THRUST] = Min( guidance_v_rc_delta_t, guidance_v_delta_t); else - booz_stabilization_cmd[COMMAND_THRUST] = booz2_guidance_v_delta_t; + booz_stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; } default: @@ -232,52 +232,52 @@ static inline void run_hover_loop(bool_t in_flight) { /* convert our reference to generic representation */ int64_t tmp = b2_gv_z_ref>>(B2_GV_Z_REF_FRAC - INT32_POS_FRAC); - booz2_guidance_v_z_ref = (int32_t)tmp; - booz2_guidance_v_zd_ref = b2_gv_zd_ref<<(INT32_SPEED_FRAC - B2_GV_ZD_REF_FRAC); - booz2_guidance_v_zdd_ref = b2_gv_zdd_ref<<(INT32_ACCEL_FRAC - B2_GV_ZDD_REF_FRAC); + guidance_v_z_ref = (int32_t)tmp; + guidance_v_zd_ref = b2_gv_zd_ref<<(INT32_SPEED_FRAC - B2_GV_ZD_REF_FRAC); + guidance_v_zdd_ref = b2_gv_zdd_ref<<(INT32_ACCEL_FRAC - B2_GV_ZDD_REF_FRAC); /* compute the error to our reference */ - int32_t err_z = ins_ltp_pos.z - booz2_guidance_v_z_ref; - Bound(err_z, BOOZ2_GUIDANCE_V_MIN_ERR_Z, BOOZ2_GUIDANCE_V_MAX_ERR_Z); - int32_t err_zd = ins_ltp_speed.z - booz2_guidance_v_zd_ref; - Bound(err_zd, BOOZ2_GUIDANCE_V_MIN_ERR_ZD, BOOZ2_GUIDANCE_V_MAX_ERR_ZD); + int32_t err_z = ins_ltp_pos.z - guidance_v_z_ref; + Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z); + int32_t err_zd = ins_ltp_speed.z - guidance_v_zd_ref; + Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD); if (in_flight) { - booz2_guidance_v_z_sum_err += err_z; - Bound(booz2_guidance_v_z_sum_err, -BOOZ2_GUIDANCE_V_MAX_SUM_ERR, BOOZ2_GUIDANCE_V_MAX_SUM_ERR); + guidance_v_z_sum_err += err_z; + Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR); } else - booz2_guidance_v_z_sum_err = 0; + guidance_v_z_sum_err = 0; /* our nominal command : (g + zdd)*m */ -#ifdef BOOZ2_GUIDANCE_V_INV_M - const int32_t inv_m = BFP_OF_REAL(BOOZ2_GUIDANCE_V_INV_M, B2_GV_ADAPT_X_FRAC); +#ifdef GUIDANCE_V_INV_M + const int32_t inv_m = BFP_OF_REAL(GUIDANCE_V_INV_M, B2_GV_ADAPT_X_FRAC); #else const int32_t inv_m = b2_gv_adapt_X>>(B2_GV_ADAPT_X_FRAC - FF_CMD_FRAC); #endif const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) - - (booz2_guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC)); + (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC)); #if 0 if (g_m_zdd > 0) - booz2_guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m; + guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m; else - booz2_guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m; + guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m; #else - booz2_guidance_v_ff_cmd = g_m_zdd / inv_m; + guidance_v_ff_cmd = g_m_zdd / inv_m; int32_t cphi,ctheta,cphitheta; PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi); PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta); cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC; if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF; - booz2_guidance_v_ff_cmd = (booz2_guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta; + guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta; #endif /* our error command */ - booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) + - ((-booz2_guidance_v_kd * err_zd) >> 21) + - ((-booz2_guidance_v_ki * booz2_guidance_v_z_sum_err) >> 21); + guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) + + ((-guidance_v_kd * err_zd) >> 21) + + ((-guidance_v_ki * guidance_v_z_sum_err) >> 21); - booz2_guidance_v_delta_t = booz2_guidance_v_ff_cmd + booz2_guidance_v_fb_cmd; - // booz2_guidance_v_delta_t = booz2_guidance_v_fb_cmd; + guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd; + // guidance_v_delta_t = guidance_v_fb_cmd; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 8c7023390d..7b5c8a2cd1 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -21,47 +21,47 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ2_GUIDANCE_V -#define BOOZ2_GUIDANCE_V +#ifndef GUIDANCE_V +#define GUIDANCE_V #include "std.h" -#include "booz2_guidance_v_ref.h" -#include "booz2_guidance_v_adpt.h" +#include "guidance_v_ref.h" +#include "guidance_v_adpt.h" -#define BOOZ2_GUIDANCE_V_MODE_KILL 0 -#define BOOZ2_GUIDANCE_V_MODE_RC_DIRECT 1 -#define BOOZ2_GUIDANCE_V_MODE_RC_CLIMB 2 -#define BOOZ2_GUIDANCE_V_MODE_CLIMB 3 -#define BOOZ2_GUIDANCE_V_MODE_HOVER 4 -#define BOOZ2_GUIDANCE_V_MODE_NAV 5 +#define GUIDANCE_V_MODE_KILL 0 +#define GUIDANCE_V_MODE_RC_DIRECT 1 +#define GUIDANCE_V_MODE_RC_CLIMB 2 +#define GUIDANCE_V_MODE_CLIMB 3 +#define GUIDANCE_V_MODE_HOVER 4 +#define GUIDANCE_V_MODE_NAV 5 -extern uint8_t booz2_guidance_v_mode; +extern uint8_t guidance_v_mode; -extern int32_t booz2_guidance_v_z_sp; -extern int32_t booz2_guidance_v_zd_sp; -extern int32_t booz2_guidance_v_z_ref; -extern int32_t booz2_guidance_v_zd_ref; -extern int32_t booz2_guidance_v_zdd_ref; -extern int32_t booz2_guidance_v_z_sum_err; -extern int32_t booz2_guidance_v_ff_cmd; -extern int32_t booz2_guidance_v_fb_cmd; -extern int32_t booz2_guidance_v_delta_t; +extern int32_t guidance_v_z_sp; +extern int32_t guidance_v_zd_sp; +extern int32_t guidance_v_z_ref; +extern int32_t guidance_v_zd_ref; +extern int32_t guidance_v_zdd_ref; +extern int32_t guidance_v_z_sum_err; +extern int32_t guidance_v_ff_cmd; +extern int32_t guidance_v_fb_cmd; +extern int32_t guidance_v_delta_t; -extern int32_t booz2_guidance_v_kp; -extern int32_t booz2_guidance_v_kd; -extern int32_t booz2_guidance_v_ki; +extern int32_t guidance_v_kp; +extern int32_t guidance_v_kd; +extern int32_t guidance_v_ki; -extern void booz2_guidance_v_init(void); -extern void booz2_guidance_v_read_rc(void); -extern void booz2_guidance_v_mode_changed(uint8_t new_mode); -extern void booz2_guidance_v_notify_in_flight(bool_t in_flight); -extern void booz2_guidance_v_run(bool_t in_flight); +extern void guidance_v_init(void); +extern void guidance_v_read_rc(void); +extern void guidance_v_mode_changed(uint8_t new_mode); +extern void guidance_v_notify_in_flight(bool_t in_flight); +extern void guidance_v_run(bool_t in_flight); -#define booz2_guidance_v_SetKi(_val) { \ - booz2_guidance_v_ki = _val; \ - booz2_guidance_v_z_sum_err = 0; \ +#define guidance_v_SetKi(_val) { \ + guidance_v_ki = _val; \ + guidance_v_z_sum_err = 0; \ } -#endif /* BOOZ2_GUIDANCE_V */ +#endif /* GUIDANCE_V */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h index b7219e2b81..1bfc41fabe 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h @@ -27,15 +27,15 @@ * needed by the invert dynamic model to produce a nominal command */ -#ifndef BOOZ2_GUIDANCE_V_ADPT -#define BOOZ2_GUIDANCE_V_ADPT +#ifndef GUIDANCE_V_ADPT +#define GUIDANCE_V_ADPT extern int32_t b2_gv_adapt_X; extern int32_t b2_gv_adapt_P; extern int32_t b2_gv_adapt_Xmeas; -#ifdef B2_GUIDANCE_V_C +#ifdef GUIDANCE_V_C /* Our State Q13.18 @@ -142,6 +142,6 @@ static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) { } -#endif /* B2_GUIDANCE_V_C */ +#endif /* GUIDANCE_V_C */ -#endif /* BOOZ2_GUIDANCE_V_ADPT */ +#endif /* GUIDANCE_V_ADPT */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index acd87fa748..c15d1f8194 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -21,8 +21,8 @@ * Boston, MA 02111-1307, USA. */ -#ifndef BOOZ2_GUIDANCE_V_REF_H -#define BOOZ2_GUIDANCE_V_REF_H +#ifndef GUIDANCE_V_REF_H +#define GUIDANCE_V_REF_H #include "airframe.h" #include "inttypes.h" @@ -50,44 +50,44 @@ extern int64_t b2_gv_z_ref; #define B2_GV_Z_REF_FRAC (B2_GV_ZD_REF_FRAC + B2_GV_FREQ_FRAC) /* Saturations definition */ -#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZDD -#define BOOZ2_GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81) +#ifndef GUIDANCE_V_REF_MIN_ZDD +#define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81) #endif -#define B2_GV_MIN_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZDD, B2_GV_ZDD_REF_FRAC) +#define B2_GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, B2_GV_ZDD_REF_FRAC) -#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZDD -#define BOOZ2_GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81) +#ifndef GUIDANCE_V_REF_MAX_ZDD +#define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81) #endif -#define B2_GV_MAX_ZDD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZDD, B2_GV_ZDD_REF_FRAC) +#define B2_GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, B2_GV_ZDD_REF_FRAC) -#ifndef BOOZ2_GUIDANCE_V_REF_MIN_ZD -#define BOOZ2_GUIDANCE_V_REF_MIN_ZD (-3.) +#ifndef GUIDANCE_V_REF_MIN_ZD +#define GUIDANCE_V_REF_MIN_ZD (-3.) #endif -#define B2_GV_MIN_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MIN_ZD , B2_GV_ZD_REF_FRAC) +#define B2_GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , B2_GV_ZD_REF_FRAC) -#ifndef BOOZ2_GUIDANCE_V_REF_MAX_ZD -#define BOOZ2_GUIDANCE_V_REF_MAX_ZD ( 3.) +#ifndef GUIDANCE_V_REF_MAX_ZD +#define GUIDANCE_V_REF_MAX_ZD ( 3.) #endif -#define B2_GV_MAX_ZD BFP_OF_REAL(BOOZ2_GUIDANCE_V_REF_MAX_ZD , B2_GV_ZD_REF_FRAC) +#define B2_GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , B2_GV_ZD_REF_FRAC) /* second order model natural frequency and damping */ -#ifndef BOOZ2_GUIDANCE_V_REF_OMEGA -#define BOOZ2_GUIDANCE_V_REF_OMEGA RadOfDeg(100.) +#ifndef GUIDANCE_V_REF_OMEGA +#define GUIDANCE_V_REF_OMEGA RadOfDeg(100.) #endif -#ifndef BOOZ2_GUIDANCE_V_REF_ZETA -#define BOOZ2_GUIDANCE_V_REF_ZETA 0.85 +#ifndef GUIDANCE_V_REF_ZETA +#define GUIDANCE_V_REF_ZETA 0.85 #endif #define B2_GV_ZETA_OMEGA_FRAC 10 -#define B2_GV_ZETA_OMEGA BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_ZETA*BOOZ2_GUIDANCE_V_REF_OMEGA), B2_GV_ZETA_OMEGA_FRAC) +#define B2_GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), B2_GV_ZETA_OMEGA_FRAC) #define B2_GV_OMEGA_2_FRAC 7 -#define B2_GV_OMEGA_2 BFP_OF_REAL((BOOZ2_GUIDANCE_V_REF_OMEGA*BOOZ2_GUIDANCE_V_REF_OMEGA), B2_GV_OMEGA_2_FRAC) +#define B2_GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), B2_GV_OMEGA_2_FRAC) /* first order time constant */ #define B2_GV_REF_THAU_F 0.25 #define B2_GV_REF_INV_THAU_FRAC 16 #define B2_GV_REF_INV_THAU BFP_OF_REAL((1./0.25), B2_GV_REF_INV_THAU_FRAC) -#ifdef B2_GUIDANCE_V_C +#ifdef GUIDANCE_V_C static inline void b2_gv_set_ref(int32_t alt, int32_t speed, int32_t accel); static inline void b2_gv_update_ref_from_z_sp(int32_t z_sp); static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_sp); @@ -160,6 +160,6 @@ static inline void b2_gv_update_ref_from_zd_sp(int32_t zd_sp) { } } -#endif /* B2_GUIDANCE_V_C */ +#endif /* GUIDANCE_V_C */ -#endif /* BOOZ2_GUIDANCE_V_REF_H */ +#endif /* GUIDANCE_V_REF_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index a58e04d03c..430b9a70d9 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -50,7 +50,7 @@ #include "autopilot.h" #include "booz_stabilization.h" -#include "booz_guidance.h" +#include "guidance.h" #include "ahrs.h" #include "ins.h" @@ -115,8 +115,8 @@ STATIC_INLINE void main_init( void ) { booz_fms_init(); autopilot_init(); booz2_nav_init(); - booz2_guidance_h_init(); - booz2_guidance_v_init(); + guidance_h_init(); + guidance_v_init(); booz_stabilization_init(); ahrs_aligner_init(); diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index 332113fac9..38daacc465 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -41,9 +41,9 @@ void vi_init(void) { vi.timeouted = TRUE; vi.last_msg = VI_TIMEOUT; - vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; INT_EULERS_ZERO(vi.input.h_sp.attitude); - vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + vi.input.v_mode = GUIDANCE_V_MODE_CLIMB; vi.input.v_sp.climb = 0; vi_impl_init(); @@ -55,9 +55,9 @@ void vi_periodic(void) { vi.last_msg++; else { vi.timeouted = TRUE; - vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; + vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; INT_EULERS_ZERO(vi.input.h_sp.attitude); - vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_CLIMB; + vi.input.v_mode = GUIDANCE_V_MODE_CLIMB; vi.input.v_sp.climb = 0; } #endif diff --git a/sw/airborne/modules/vehicle_interface/vi.h b/sw/airborne/modules/vehicle_interface/vi.h index 8042a65ab5..fe7ac30259 100644 --- a/sw/airborne/modules/vehicle_interface/vi.h +++ b/sw/airborne/modules/vehicle_interface/vi.h @@ -32,7 +32,7 @@ #include "math/pprz_algebra_int.h" #include "autopilot.h" #include "booz/booz_stabilization.h" -#include "booz/booz_guidance.h" +#include "booz/guidance.h" #include "booz/booz2_navigation.h" struct Vi_imu_info { diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.h b/sw/airborne/modules/vehicle_interface/vi_datalink.h index f971d27e20..47cc5dc46e 100644 --- a/sw/airborne/modules/vehicle_interface/vi_datalink.h +++ b/sw/airborne/modules/vehicle_interface/vi_datalink.h @@ -56,10 +56,10 @@ extern void vi_update_wp(uint8_t wp_id); vi.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \ vi.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ switch (vi.input.h_mode) { \ - case BOOZ2_GUIDANCE_H_MODE_KILL: \ - case BOOZ2_GUIDANCE_H_MODE_RATE : \ + case GUIDANCE_H_MODE_KILL: \ + case GUIDANCE_H_MODE_RATE : \ break; \ - case BOOZ2_GUIDANCE_H_MODE_ATTITUDE : \ + case GUIDANCE_H_MODE_ATTITUDE : \ { \ vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ vi.input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ @@ -68,13 +68,13 @@ extern void vi_update_wp(uint8_t wp_id); VI_LIMIT_ATTITUDE(vi.input.h_sp.attitude); \ } \ break; \ - case BOOZ2_GUIDANCE_H_MODE_HOVER : \ + case GUIDANCE_H_MODE_HOVER : \ { \ vi.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ vi.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ } \ break; \ - case BOOZ2_GUIDANCE_H_MODE_NAV : \ + case GUIDANCE_H_MODE_NAV : \ { \ vi.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ vi.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ @@ -85,17 +85,17 @@ extern void vi_update_wp(uint8_t wp_id); break; \ } \ switch (vi.input.v_mode) { \ - case BOOZ2_GUIDANCE_V_MODE_KILL: \ - case BOOZ2_GUIDANCE_V_MODE_RC_DIRECT: \ - case BOOZ2_GUIDANCE_V_MODE_RC_CLIMB: \ + case GUIDANCE_V_MODE_KILL: \ + case GUIDANCE_V_MODE_RC_DIRECT: \ + case GUIDANCE_V_MODE_RC_CLIMB: \ break; \ - case BOOZ2_GUIDANCE_V_MODE_CLIMB : \ + case GUIDANCE_V_MODE_CLIMB : \ vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ break; \ - case BOOZ2_GUIDANCE_V_MODE_HOVER : \ + case GUIDANCE_V_MODE_HOVER : \ vi.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ break; \ - case BOOZ2_GUIDANCE_V_MODE_NAV : \ + case GUIDANCE_V_MODE_NAV : \ vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ break; \ default: \ @@ -105,8 +105,8 @@ extern void vi_update_wp(uint8_t wp_id); #define VI_NAV_STICK_PARSE_DL(_dl_buffer) { \ vi.last_msg = 0; \ - vi.input.h_mode = BOOZ2_GUIDANCE_H_MODE_NAV; \ - vi.input.v_mode = BOOZ2_GUIDANCE_V_MODE_NAV; \ + vi.input.h_mode = GUIDANCE_H_MODE_NAV; \ + vi.input.v_mode = GUIDANCE_V_MODE_NAV; \ vi.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \ vi.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \ vi.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \ diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.c b/sw/airborne/modules/vehicle_interface/vi_test_signal.c index ff80e87af6..ae2eb36335 100644 --- a/sw/airborne/modules/vehicle_interface/vi_test_signal.c +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.c @@ -37,8 +37,8 @@ void booz_fms_impl_init(void) { fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD; fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; fms_test_signal.counter = 0; - fms.input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; - fms.input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; + fms.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; + fms.input.v_mode = GUIDANCE_V_MODE_HOVER; } void booz_fms_impl_periodic(void) { @@ -68,7 +68,7 @@ void booz_fms_impl_periodic(void) { break; #if 0 case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { - if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER) + if (guidance_v_mode < GUIDANCE_V_MODE_HOVER) booz_fms_test_signal_start_z = ins_ltp_pos.z; else { booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ?