diff --git a/conf/airframes/BR/conf.xml b/conf/airframes/BR/conf.xml
index b9b3969bd6..17fdf901fa 100644
--- a/conf/airframes/BR/conf.xml
+++ b/conf/airframes/BR/conf.xml
@@ -132,16 +132,4 @@
settings_modules="modules/ahrs_float_mlkf.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/switch_servo.xml"
gui_color="blue"
/>
-
diff --git a/conf/airframes/ENAC/cyfoam.xml b/conf/airframes/ENAC/cyfoam.xml
index 00f36e9586..3bb1e2bca8 100644
--- a/conf/airframes/ENAC/cyfoam.xml
+++ b/conf/airframes/ENAC/cyfoam.xml
@@ -97,8 +97,6 @@
-
-
diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
index 1e3266624b..3cfeca5c9a 100644
--- a/conf/airframes/examples/cube_orange.xml
+++ b/conf/airframes/examples/cube_orange.xml
@@ -116,7 +116,6 @@
-
diff --git a/conf/airframes/tudelft/bebop2_indi_convergence.xml b/conf/airframes/tudelft/bebop2_indi_convergence.xml
index c36b88d75a..1629e3cf67 100644
--- a/conf/airframes/tudelft/bebop2_indi_convergence.xml
+++ b/conf/airframes/tudelft/bebop2_indi_convergence.xml
@@ -203,7 +203,6 @@
diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml
index 40193f3048..13f02a8146 100644
--- a/conf/airframes/tudelft/cyfoam.xml
+++ b/conf/airframes/tudelft/cyfoam.xml
@@ -66,6 +66,7 @@
@@ -302,8 +303,9 @@
-
+
+
@@ -311,6 +313,7 @@
+
@@ -319,11 +322,11 @@
-
-
-
+
+
+
diff --git a/conf/airframes/tudelft/disco_rotorcraft_indi.xml b/conf/airframes/tudelft/disco_rotorcraft_indi.xml
index 0c192e3d83..9cc90d0ac5 100644
--- a/conf/airframes/tudelft/disco_rotorcraft_indi.xml
+++ b/conf/airframes/tudelft/disco_rotorcraft_indi.xml
@@ -65,7 +65,6 @@
-->
-
diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml
index fcf79517e9..7dbfb5580d 100644
--- a/conf/airframes/tudelft/nederdrone4.xml
+++ b/conf/airframes/tudelft/nederdrone4.xml
@@ -118,7 +118,6 @@
-
diff --git a/conf/airframes/tudelft/nederdrone4_tem.xml b/conf/airframes/tudelft/nederdrone4_tem.xml
index b6327196ff..1b643f7912 100644
--- a/conf/airframes/tudelft/nederdrone4_tem.xml
+++ b/conf/airframes/tudelft/nederdrone4_tem.xml
@@ -107,7 +107,6 @@
-
diff --git a/conf/airframes/tudelft/nederdrone5.xml b/conf/airframes/tudelft/nederdrone5.xml
index e4bc0444a0..f5e26b2dd2 100644
--- a/conf/airframes/tudelft/nederdrone5.xml
+++ b/conf/airframes/tudelft/nederdrone5.xml
@@ -114,7 +114,6 @@
-
diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml
index d16849bfa2..af50bfd205 100644
--- a/conf/airframes/tudelft/nederdrone6.xml
+++ b/conf/airframes/tudelft/nederdrone6.xml
@@ -137,7 +137,6 @@
-
-
-
-
+
diff --git a/conf/flight_plans/ENAC/fish_outdoor.xml b/conf/flight_plans/ENAC/fish_outdoor.xml
index 8bc26942b3..5525b5e6c7 100644
--- a/conf/flight_plans/ENAC/fish_outdoor.xml
+++ b/conf/flight_plans/ENAC/fish_outdoor.xml
@@ -76,7 +76,7 @@
-
+
@@ -96,7 +96,7 @@
-
+
diff --git a/conf/flight_plans/ENAC/fish_voliere.xml b/conf/flight_plans/ENAC/fish_voliere.xml
index be5b859ecc..35daf50b69 100644
--- a/conf/flight_plans/ENAC/fish_voliere.xml
+++ b/conf/flight_plans/ENAC/fish_voliere.xml
@@ -60,7 +60,7 @@
-
+
@@ -80,7 +80,7 @@
-
+
diff --git a/conf/flight_plans/HOOPERFLY/hooperfly_gsa_one.xml b/conf/flight_plans/HOOPERFLY/hooperfly_gsa_one.xml
index 291f008256..b729f7afc2 100644
--- a/conf/flight_plans/HOOPERFLY/hooperfly_gsa_one.xml
+++ b/conf/flight_plans/HOOPERFLY/hooperfly_gsa_one.xml
@@ -38,7 +38,7 @@
-
+
@@ -122,7 +122,7 @@
-
+
diff --git a/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_multiflight.xml b/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_multiflight.xml
index 7a5f3493bd..8fdd1916a4 100644
--- a/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_multiflight.xml
+++ b/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_multiflight.xml
@@ -38,7 +38,7 @@
-
+
@@ -177,7 +177,7 @@
-
+
diff --git a/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_nocturnal.xml b/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_nocturnal.xml
index bfe36c138a..850fa6c06e 100644
--- a/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_nocturnal.xml
+++ b/conf/flight_plans/HOOPERFLY/hooperfly_rotorcraft_nocturnal.xml
@@ -38,7 +38,7 @@
-
+
@@ -165,7 +165,7 @@
-
+
@@ -177,7 +177,7 @@
-
+
diff --git a/conf/flight_plans/OPENUAS/openuas_rotorcraft_simple.xml b/conf/flight_plans/OPENUAS/openuas_rotorcraft_simple.xml
index 0fe01a8858..d39a14c0ab 100644
--- a/conf/flight_plans/OPENUAS/openuas_rotorcraft_simple.xml
+++ b/conf/flight_plans/OPENUAS/openuas_rotorcraft_simple.xml
@@ -81,7 +81,7 @@ Your safe aircraft operation is *your* responsibility
If the aircraft is later moved to a takeoff spot and the heading is changed it will try to rotate to the original heading as it is taking off which can make some motors spin too fast and cause a flip or other unwanted manovering.
Altough Switching the mode to auto2/NAV from any other mode will reset nav_heading to the current heading this is not enough therefore just befor takeo we set the nav heading to the current heading-->
-
+
diff --git a/conf/flight_plans/competitions/IMAV2019_drop.xml b/conf/flight_plans/competitions/IMAV2019_drop.xml
index 40072d02dc..e4bbefd24d 100644
--- a/conf/flight_plans/competitions/IMAV2019_drop.xml
+++ b/conf/flight_plans/competitions/IMAV2019_drop.xml
@@ -116,7 +116,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
@@ -127,7 +127,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
@@ -157,7 +157,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
@@ -189,7 +189,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
@@ -221,7 +221,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
@@ -253,7 +253,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
@@ -287,7 +287,7 @@ static inline bool delay_test_gf(bool test, int delay) {
-
+
diff --git a/conf/flight_plans/competitions/IMAV2022_drop.xml b/conf/flight_plans/competitions/IMAV2022_drop.xml
index c869060ab7..2c3ba89d4d 100644
--- a/conf/flight_plans/competitions/IMAV2022_drop.xml
+++ b/conf/flight_plans/competitions/IMAV2022_drop.xml
@@ -214,7 +214,7 @@
-
+
@@ -458,7 +458,7 @@
-
+
diff --git a/conf/flight_plans/competitions/IMAV2022_falcon.xml b/conf/flight_plans/competitions/IMAV2022_falcon.xml
index 850644e0f7..632d0e4627 100644
--- a/conf/flight_plans/competitions/IMAV2022_falcon.xml
+++ b/conf/flight_plans/competitions/IMAV2022_falcon.xml
@@ -123,7 +123,7 @@
-
+
@@ -181,7 +181,7 @@
-
+
@@ -190,7 +190,7 @@
-
+
diff --git a/conf/flight_plans/competitions/IMAV2022_search.xml b/conf/flight_plans/competitions/IMAV2022_search.xml
index 389ee204b8..2cab330154 100644
--- a/conf/flight_plans/competitions/IMAV2022_search.xml
+++ b/conf/flight_plans/competitions/IMAV2022_search.xml
@@ -146,7 +146,7 @@
-
+
@@ -207,7 +207,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml
index 8155416550..0649bc9d6c 100644
--- a/conf/flight_plans/rotorcraft_basic.xml
+++ b/conf/flight_plans/rotorcraft_basic.xml
@@ -36,7 +36,7 @@
-
+
@@ -72,7 +72,7 @@
-
+
@@ -84,7 +84,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_basic_geofence.xml b/conf/flight_plans/rotorcraft_basic_geofence.xml
index 4235c2b986..ece3600f56 100644
--- a/conf/flight_plans/rotorcraft_basic_geofence.xml
+++ b/conf/flight_plans/rotorcraft_basic_geofence.xml
@@ -85,7 +85,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_basic_superbitrf.xml b/conf/flight_plans/rotorcraft_basic_superbitrf.xml
index bc25136a04..5de3066e3d 100644
--- a/conf/flight_plans/rotorcraft_basic_superbitrf.xml
+++ b/conf/flight_plans/rotorcraft_basic_superbitrf.xml
@@ -92,7 +92,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml b/conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml
index 2ad80a7777..26b7901db8 100644
--- a/conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml
+++ b/conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml
@@ -82,7 +82,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
-
+
@@ -109,7 +109,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
-
+
@@ -118,7 +118,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
-
+
diff --git a/conf/flight_plans/rotorcraft_cam.xml b/conf/flight_plans/rotorcraft_cam.xml
index 98c9c6180e..233cd40951 100644
--- a/conf/flight_plans/rotorcraft_cam.xml
+++ b/conf/flight_plans/rotorcraft_cam.xml
@@ -35,7 +35,7 @@
-
+
@@ -65,9 +65,9 @@
-
+
-
+
@@ -81,7 +81,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_guido_optitrack.xml b/conf/flight_plans/rotorcraft_guido_optitrack.xml
index 233a42d854..7ee822195d 100644
--- a/conf/flight_plans/rotorcraft_guido_optitrack.xml
+++ b/conf/flight_plans/rotorcraft_guido_optitrack.xml
@@ -44,7 +44,7 @@
-
+
@@ -88,7 +88,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_joystick_enac.xml b/conf/flight_plans/rotorcraft_joystick_enac.xml
index b8b7b06973..0d1941299d 100644
--- a/conf/flight_plans/rotorcraft_joystick_enac.xml
+++ b/conf/flight_plans/rotorcraft_joystick_enac.xml
@@ -7,10 +7,10 @@
#include "autopilot.h"
static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), int16_t roll, int16_t pitch, int16_t yaw, int16_t throttle) {
if (And(autopilot_get_mode() == AP_MODE_NAV, nav_block == joystick_block)) {
- nav_roll = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI * roll / MAX_PPRZ);
- nav_pitch = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA * pitch / MAX_PPRZ);
- nav_heading = ANGLE_BFP_OF_REAL(3.1416f * yaw / MAX_PPRZ);
- nav_throttle = throttle;
+ nav.roll = (STABILIZATION_ATTITUDE_SP_MAX_PHI * (float)roll / MAX_PPRZ);
+ nav.pitch = (STABILIZATION_ATTITUDE_SP_MAX_THETA * (float)pitch / MAX_PPRZ);
+ nav.heading = (3.1416f * (float)yaw / MAX_PPRZ);
+ nav.throttle = throttle;
}
}
#endif
@@ -67,7 +67,7 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
-
+
@@ -106,7 +106,7 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
-
+
diff --git a/conf/flight_plans/rotorcraft_optitrack.xml b/conf/flight_plans/rotorcraft_optitrack.xml
index 29a2b47723..a5083d41d6 100644
--- a/conf/flight_plans/rotorcraft_optitrack.xml
+++ b/conf/flight_plans/rotorcraft_optitrack.xml
@@ -43,7 +43,7 @@
-
+
@@ -87,7 +87,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml b/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml
index f75dbdca24..37a2c3f65c 100644
--- a/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml
+++ b/conf/flight_plans/rotorcraft_optitrack_stereoavoid.xml
@@ -44,13 +44,13 @@
-
+
-
+
@@ -74,7 +74,7 @@
-
+
@@ -107,7 +107,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_survey.xml b/conf/flight_plans/rotorcraft_survey.xml
index 04946c2f79..1e76107c33 100644
--- a/conf/flight_plans/rotorcraft_survey.xml
+++ b/conf/flight_plans/rotorcraft_survey.xml
@@ -45,7 +45,7 @@
-
+
@@ -88,7 +88,7 @@
-
+
@@ -100,7 +100,7 @@
-
+
@@ -109,7 +109,7 @@
-
+
diff --git a/conf/flight_plans/rotorcraft_vision.xml b/conf/flight_plans/rotorcraft_vision.xml
index 670dba11c5..da6acf2603 100644
--- a/conf/flight_plans/rotorcraft_vision.xml
+++ b/conf/flight_plans/rotorcraft_vision.xml
@@ -27,7 +27,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
index b70b95e5ca..09ebfbfd68 100644
--- a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
+++ b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo.xml
@@ -96,7 +96,7 @@
-
+
@@ -124,7 +124,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
index 48758f834a..349d084fb1 100644
--- a/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
+++ b/conf/flight_plans/tudelft/course_orangeavoid_cyberzoo_guided.xml
@@ -104,7 +104,7 @@ inline void setGuided(void){
-
+
@@ -135,7 +135,7 @@ inline void setGuided(void){
-
+
diff --git a/conf/flight_plans/tudelft/cyclone_valkenburg.xml b/conf/flight_plans/tudelft/cyclone_valkenburg.xml
index e7a56fd568..46a852e371 100644
--- a/conf/flight_plans/tudelft/cyclone_valkenburg.xml
+++ b/conf/flight_plans/tudelft/cyclone_valkenburg.xml
@@ -34,7 +34,7 @@
-
+
@@ -67,7 +67,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/delfly_nimble_cyberzoo.xml b/conf/flight_plans/tudelft/delfly_nimble_cyberzoo.xml
index ea7fae38f9..3bcddd9f77 100644
--- a/conf/flight_plans/tudelft/delfly_nimble_cyberzoo.xml
+++ b/conf/flight_plans/tudelft/delfly_nimble_cyberzoo.xml
@@ -43,7 +43,7 @@
-
+
@@ -102,7 +102,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/delft_basic.xml b/conf/flight_plans/tudelft/delft_basic.xml
index 834a59a291..46ab657cc0 100644
--- a/conf/flight_plans/tudelft/delft_basic.xml
+++ b/conf/flight_plans/tudelft/delft_basic.xml
@@ -69,7 +69,7 @@
-
+
@@ -102,7 +102,7 @@
-
+
@@ -114,7 +114,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/delft_bebop.xml b/conf/flight_plans/tudelft/delft_bebop.xml
index 08b61869e3..997edad550 100644
--- a/conf/flight_plans/tudelft/delft_bebop.xml
+++ b/conf/flight_plans/tudelft/delft_bebop.xml
@@ -70,7 +70,7 @@
-
+
@@ -103,7 +103,7 @@
-
+
@@ -115,7 +115,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/delft_bebop_convergence.xml b/conf/flight_plans/tudelft/delft_bebop_convergence.xml
index 879f5e229a..6f42f66e91 100644
--- a/conf/flight_plans/tudelft/delft_bebop_convergence.xml
+++ b/conf/flight_plans/tudelft/delft_bebop_convergence.xml
@@ -85,7 +85,7 @@
-
+
@@ -117,7 +117,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/disco_convergence.xml b/conf/flight_plans/tudelft/disco_convergence.xml
index 6f4aa729df..30d39d808a 100644
--- a/conf/flight_plans/tudelft/disco_convergence.xml
+++ b/conf/flight_plans/tudelft/disco_convergence.xml
@@ -67,7 +67,7 @@
-
+
@@ -88,7 +88,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/fan_demo.xml b/conf/flight_plans/tudelft/fan_demo.xml
index 75b5e0be2a..2c435e4b7b 100644
--- a/conf/flight_plans/tudelft/fan_demo.xml
+++ b/conf/flight_plans/tudelft/fan_demo.xml
@@ -67,7 +67,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/include_rotorcraft_survey_mission.xml b/conf/flight_plans/tudelft/include_rotorcraft_survey_mission.xml
index 3f54209ac6..eb1aef38b2 100644
--- a/conf/flight_plans/tudelft/include_rotorcraft_survey_mission.xml
+++ b/conf/flight_plans/tudelft/include_rotorcraft_survey_mission.xml
@@ -23,10 +23,14 @@
#endif
#define WpAlt(X) (30)
+
+
+
+
-
-
+
+
@@ -54,22 +58,22 @@
-
+
-
+
-
+
-
+
@@ -82,7 +86,7 @@
-
+
@@ -99,7 +103,7 @@
-
+
@@ -107,7 +111,7 @@
-
+
@@ -132,43 +136,43 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
@@ -233,7 +237,7 @@
-
+
@@ -247,7 +251,7 @@
-
+
diff --git a/conf/flight_plans/tudelft/nederdrone_valkenburg.xml b/conf/flight_plans/tudelft/nederdrone_valkenburg.xml
index 4901d4d078..7968968ceb 100644
--- a/conf/flight_plans/tudelft/nederdrone_valkenburg.xml
+++ b/conf/flight_plans/tudelft/nederdrone_valkenburg.xml
@@ -78,7 +78,7 @@
-
+
-
+
+
diff --git a/conf/modules/guidance_pid_rotorcraft.xml b/conf/modules/guidance_pid_rotorcraft.xml
new file mode 100644
index 0000000000..32ffd42026
--- /dev/null
+++ b/conf/modules/guidance_pid_rotorcraft.xml
@@ -0,0 +1,51 @@
+
+
+
+
+
+ Basic guidance code for rotorcraft with PID control
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ @stabilization
+ guidance,attitude_command
+
+
+
+
+
+
+
diff --git a/conf/modules/guidance_rotorcraft.xml b/conf/modules/guidance_rotorcraft.xml
index 625efdd3ac..7a1a79711e 100644
--- a/conf/modules/guidance_rotorcraft.xml
+++ b/conf/modules/guidance_rotorcraft.xml
@@ -9,42 +9,19 @@
- vertical guidance with reference and adaptive control
- flip mode
-
-
-
-
-
-
-
-
+
+
-
+
-
-
-
-
-
-
@@ -52,7 +29,7 @@
- nav_basic_rotorcraft,@stabilization
+ nav_basic_rotorcraft,guidance_pid_rotorcraft,@stabilization
guidance,attitude_command
diff --git a/conf/modules/nav_basic_rotorcraft.xml b/conf/modules/nav_basic_rotorcraft.xml
index b68b1ec978..add7cd5d8a 100644
--- a/conf/modules/nav_basic_rotorcraft.xml
+++ b/conf/modules/nav_basic_rotorcraft.xml
@@ -6,31 +6,16 @@
Standard navigation patterns and flight plan handling for rotorcraft
-
-
-
-
-
-
-
-
-
-
-
+ nav_rotorcraft
navigation
-
-
-
+
-
-
-
-
+
diff --git a/conf/modules/nav_hybrid.xml b/conf/modules/nav_hybrid.xml
new file mode 100644
index 0000000000..fb15df615d
--- /dev/null
+++ b/conf/modules/nav_hybrid.xml
@@ -0,0 +1,33 @@
+
+
+
+
+
+ Navigation patterns and flight plan handling for hybrid airframes
+
+
+
+
+
+
+
+
+
+
+ nav_basic_rotorcraft,guidance_indi_hybrid
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/nav_rotorcraft.xml b/conf/modules/nav_rotorcraft.xml
new file mode 100644
index 0000000000..e8962ef6b0
--- /dev/null
+++ b/conf/modules/nav_rotorcraft.xml
@@ -0,0 +1,41 @@
+
+
+
+
+
+ Standard navigation API for rotorcraft
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ navigation
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/control/rotorcraft_guidance.xml b/conf/settings/control/rotorcraft_guidance.xml
index f15c7ebf11..13fb880e81 100644
--- a/conf/settings/control/rotorcraft_guidance.xml
+++ b/conf/settings/control/rotorcraft_guidance.xml
@@ -30,10 +30,10 @@
-
-
-
-
+
+
+
+
diff --git a/conf/userconf/OPENUAS/openuas_all_ac.xml b/conf/userconf/OPENUAS/openuas_all_ac.xml
index 1562e9471a..6edbeb093d 100644
--- a/conf/userconf/OPENUAS/openuas_all_ac.xml
+++ b/conf/userconf/OPENUAS/openuas_all_ac.xml
@@ -220,28 +220,6 @@
settings_modules="modules/gps.xml modules/guidance_energy.xml modules/nav_basic_fw.xml modules/nav_catapult.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_attitude_fw.xml"
gui_color="#ffffed9b40f2"
/>
-
-
-
diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c
index e2831275bf..56d40665e1 100644
--- a/sw/airborne/firmwares/fixedwing/nav.c
+++ b/sw/airborne/firmwares/fixedwing/nav.c
@@ -46,9 +46,6 @@ enum oval_status oval_status;
float last_x, last_y;
-/** Index of last waypoint. Used only in "go" stage in "route" horiz mode */
-uint8_t last_wp __attribute__((unused));
-
float rc_pitch;
float carrot_x, carrot_y;
diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h
index 870d6391d2..7d9779ec57 100644
--- a/sw/airborne/firmwares/fixedwing/nav.h
+++ b/sw/airborne/firmwares/fixedwing/nav.h
@@ -81,8 +81,6 @@ extern bool nav_in_segment;
extern float nav_circle_x, nav_circle_y, nav_circle_radius; /* m */
extern float nav_segment_x_1, nav_segment_y_1, nav_segment_x_2, nav_segment_y_2; /* m */
-extern uint8_t last_wp __attribute__((unused));
-
extern int nav_mode;
#define NAV_MODE_ROLL 1
#define NAV_MODE_COURSE 2
@@ -147,13 +145,6 @@ extern void nav_glide(uint8_t start_wp, uint8_t wp);
#define NavCircleWaypoint(wp, radius) \
nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)
-/** Normalize a degree angle between 0 and 359 */
-#define NormCourse(x) { \
- uint8_t dont_loop_forever = 0; \
- while (x < 0 && ++dont_loop_forever) x += 360; \
- while (x >= 360 && ++dont_loop_forever) x -= 360; \
- }
-
#define NavCircleCountNoRewind() (nav_circle_radians_no_rewind / (2*M_PI))
#define NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI))
#define NavCircleQdr() ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; })
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
index af8dc89050..33a1bc6728 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
@@ -102,7 +102,7 @@ static void send_status(struct transport_tx *trans, struct link_device *dev)
&imu_nb_err, &_motor_nb_err,
&radio_control.status, &radio_control.frame_rate,
&fix, &autopilot.mode, &in_flight, &motors_on,
- &autopilot.arming_status, &guidance_h.mode, &guidance_v_mode,
+ &autopilot.arming_status, &guidance_h.mode, &guidance_v.mode,
&time_sec, &electrical.vsupply);
}
@@ -116,9 +116,10 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev)
static void send_fp(struct transport_tx *trans, struct link_device *dev)
{
- int32_t carrot_up = -guidance_v_z_sp;
+ int32_t carrot_up = -guidance_v.z_sp;
int32_t carrot_heading = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
int32_t thrust = (int32_t)autopilot.throttle;
+ struct EnuCoor_i *pos = stateGetPositionEnu_i();
#if GUIDANCE_INDI_HYBRID
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
@@ -128,9 +129,9 @@ static void send_fp(struct transport_tx *trans, struct link_device *dev)
struct Int32Eulers att = *stateGetNedToBodyEulers_i();
#endif
pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID,
- &(stateGetPositionEnu_i()->x),
- &(stateGetPositionEnu_i()->y),
- &(stateGetPositionEnu_i()->z),
+ &pos->x,
+ &pos->y,
+ &pos->z,
&(stateGetSpeedEnu_i()->x),
&(stateGetSpeedEnu_i()->y),
&(stateGetSpeedEnu_i()->z),
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h
index 5dc4520ecb..777158c943 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h
@@ -106,8 +106,8 @@ extern void autopilot_guided_parse_GUIDED(uint8_t *buf);
/** Macro for the flight plan insctructions
*/
#define NavGuided(_flags, _x, _y, _z, _yaw) { \
- horizontal_mode = HORIZONTAL_MODE_GUIDED; \
- vertical_mode = VERTICAL_MODE_GUIDED; \
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_GUIDED; \
+ nav.vertical_mode = NAV_VERTICAL_MODE_GUIDED; \
autopilot_guided_update(_flags, _x, _y, _z, _yaw); \
}
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_static.c b/sw/airborne/firmwares/rotorcraft/autopilot_static.c
index 72e1ff27f3..45b92b361a 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_static.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_static.c
@@ -114,8 +114,8 @@ void autopilot_static_periodic(void)
RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home());
if (autopilot_in_flight() && autopilot.mode == AP_MODE_NAV) {
- if (too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
- if (dist2_to_home > failsafe_mode_dist2) {
+ if (nav.too_far_from_home || datalink_lost() || higher_than_max_altitude()) {
+ if (nav.dist2_to_home > nav.failsafe_mode_dist2) {
autopilot_static_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME);
} else {
autopilot_static_set_mode(AP_MODE_HOME);
@@ -262,7 +262,7 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB);
- guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED);
+ guidance_v_set_vz(FAILSAFE_DESCENT_SPEED);
break;
#endif
case AP_MODE_KILL:
@@ -353,7 +353,7 @@ void autopilot_static_on_rc_frame(void)
#if UNLOCKED_HOME_MODE
/* Allowed to leave home mode when UNLOCKED_HOME_MODE */
- || !too_far_from_home
+ || !nav.too_far_from_home
#endif
) {
autopilot_static_set_mode(new_autopilot_mode);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 91a2b02084..1aa0f8e5d6 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -34,38 +34,17 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/radio_control/radio_control.h"
-#if GUIDANCE_INDI_HYBRID
-#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
-#else
-#include "firmwares/rotorcraft/guidance/guidance_indi.h"
-#endif
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-/* for guidance_v_thrust_coeff */
+/* for guidance_v.thrust_coeff */
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "state.h"
-#ifndef GUIDANCE_H_AGAIN
-#define GUIDANCE_H_AGAIN 0
-#endif
-
-#ifndef GUIDANCE_H_VGAIN
-#define GUIDANCE_H_VGAIN 0
-#endif
-
-/* error if some gains are negative */
-#if (GUIDANCE_H_PGAIN < 0) || \
- (GUIDANCE_H_DGAIN < 0) || \
- (GUIDANCE_H_IGAIN < 0) || \
- (GUIDANCE_H_AGAIN < 0) || \
- (GUIDANCE_H_VGAIN < 0)
-#error "ALL control gains have to be positive!!!"
-#endif
-
+// FIXME check again here ?
#ifndef GUIDANCE_H_MAX_BANK
#define GUIDANCE_H_MAX_BANK RadOfDeg(20)
#endif
@@ -73,41 +52,16 @@
PRINT_CONFIG_VAR(GUIDANCE_H_USE_REF)
PRINT_CONFIG_VAR(GUIDANCE_H_USE_SPEED_REF)
-#ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST
-#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE
-#endif
-
-#ifndef GUIDANCE_INDI
-#define GUIDANCE_INDI FALSE
-#endif
-
-// Navigation can set heading freely
-// This is false if sideslip is a problem
-#ifndef GUIDANCE_HEADING_IS_FREE
-#define GUIDANCE_HEADING_IS_FREE TRUE
-#endif
-
struct HorizontalGuidance guidance_h;
int32_t transition_percentage;
-/*
- * internal variables
- */
-struct Int32Vect2 guidance_h_pos_err;
-struct Int32Vect2 guidance_h_speed_err;
-struct Int32Vect2 guidance_h_trim_att_integrator;
-
/** horizontal guidance command.
* In north/east with #INT32_ANGLE_FRAC
- * @todo convert to real force command
*/
-struct Int32Vect2 guidance_h_cmd_earth;
+struct StabilizationSetpoint guidance_h_cmd;
static void guidance_h_update_reference(void);
-#if !GUIDANCE_INDI
-static void guidance_h_traj_run(bool in_flight);
-#endif
static inline void transition_run(bool to_forward);
static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight);
@@ -123,28 +77,6 @@ static void send_gh(struct transport_tx *trans, struct link_device *dev)
&(pos->x), &(pos->y));
}
-static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
-{
- struct NedCoor_i *pos = stateGetPositionNed_i();
- struct NedCoor_i *speed = stateGetSpeedNed_i();
- struct NedCoor_i *accel = stateGetAccelNed_i();
- pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
- &guidance_h.sp.pos.x,
- &guidance_h.sp.pos.y,
- &(pos->x), &(pos->y),
- &(speed->x), &(speed->y),
- &(accel->x), &(accel->y),
- &guidance_h_pos_err.x,
- &guidance_h_pos_err.y,
- &guidance_h_speed_err.x,
- &guidance_h_speed_err.y,
- &guidance_h_trim_att_integrator.x,
- &guidance_h_trim_att_integrator.y,
- &guidance_h_cmd_earth.x,
- &guidance_h_cmd_earth.y,
- &guidance_h.sp.heading);
-}
-
static void send_href(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID,
@@ -178,18 +110,11 @@ void guidance_h_init(void)
guidance_h.mode = GUIDANCE_H_MODE_KILL;
guidance_h.use_ref = GUIDANCE_H_USE_REF;
- guidance_h.approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST;
INT_VECT2_ZERO(guidance_h.sp.pos);
- INT_VECT2_ZERO(guidance_h_trim_att_integrator);
FLOAT_EULERS_ZERO(guidance_h.rc_sp);
guidance_h.sp.heading = 0.0;
guidance_h.sp.heading_rate = 0.0;
- guidance_h.gains.p = GUIDANCE_H_PGAIN;
- guidance_h.gains.i = GUIDANCE_H_IGAIN;
- guidance_h.gains.d = GUIDANCE_H_DGAIN;
- guidance_h.gains.a = GUIDANCE_H_AGAIN;
- guidance_h.gains.v = GUIDANCE_H_VGAIN;
transition_percentage = 0;
transition_theta_offset = 0;
@@ -201,7 +126,6 @@ void guidance_h_init(void)
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_INT, send_gh);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HOVER_LOOP, send_hover_loop);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_H_REF_INT, send_href);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_TUNE_HOVER, send_tune_hover);
#endif
@@ -221,8 +145,6 @@ static inline void reset_guidance_reference_from_current_position(void)
struct FloatVect2 ref_accel;
FLOAT_VECT2_ZERO(ref_accel);
gh_set_ref(guidance_h.ref.pos, ref_speed, ref_accel);
-
- INT_VECT2_ZERO(guidance_h_trim_att_integrator);
}
void guidance_h_mode_changed(uint8_t new_mode)
@@ -231,10 +153,6 @@ void guidance_h_mode_changed(uint8_t new_mode)
return;
}
-#if HYBRID_NAVIGATION
- guidance_hybrid_norm_ref_airspeed = 0;
-#endif
-
switch (new_mode) {
case GUIDANCE_H_MODE_RC_DIRECT:
stabilization_none_enter();
@@ -262,9 +180,6 @@ void guidance_h_mode_changed(uint8_t new_mode)
case GUIDANCE_H_MODE_GUIDED:
case GUIDANCE_H_MODE_HOVER:
-#if GUIDANCE_INDI
- guidance_indi_enter();
-#endif
guidance_h_hover_enter();
#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
/* reset attitude stabilization if previous mode was not using it */
@@ -282,9 +197,6 @@ void guidance_h_mode_changed(uint8_t new_mode)
#endif
case GUIDANCE_H_MODE_NAV:
-#if GUIDANCE_INDI
- guidance_indi_enter();
-#endif
guidance_h_nav_enter();
#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
/* reset attitude stabilization if previous mode was not using it */
@@ -471,88 +383,6 @@ static void guidance_h_update_reference(void)
}
}
-#define MAX_POS_ERR POS_BFP_OF_REAL(16.)
-#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
-
-#ifndef GUIDANCE_H_THRUST_CMD_FILTER
-#define GUIDANCE_H_THRUST_CMD_FILTER 10
-#endif
-
-/* with a pgain of 100 and a scale of 2,
- * you get an angle of 5.6 degrees for 1m pos error */
-#define GH_GAIN_SCALE 2
-
-#if !GUIDANCE_INDI
-static void guidance_h_traj_run(bool in_flight)
-{
- /* maximum bank angle: default 20 deg, max 40 deg*/
- static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
- BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
- static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
-
- /* compute position error */
- VECT2_DIFF(guidance_h_pos_err, guidance_h.ref.pos, *stateGetPositionNed_i());
- /* saturate it */
- VECT2_STRIM(guidance_h_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
-
- /* compute speed error */
- VECT2_DIFF(guidance_h_speed_err, guidance_h.ref.speed, *stateGetSpeedNed_i());
- /* saturate it */
- VECT2_STRIM(guidance_h_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
-
- /* run PID */
- int32_t pd_x =
- ((guidance_h.gains.p * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
- ((guidance_h.gains.d * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
- int32_t pd_y =
- ((guidance_h.gains.p * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
- ((guidance_h.gains.d * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
- guidance_h_cmd_earth.x = pd_x +
- ((guidance_h.gains.v * guidance_h.ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
- ((guidance_h.gains.a * guidance_h.ref.accel.x) >> (INT32_ACCEL_FRAC -
- GH_GAIN_SCALE)); /* acceleration feedforward gain */
- guidance_h_cmd_earth.y = pd_y +
- ((guidance_h.gains.v * guidance_h.ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
- ((guidance_h.gains.a * guidance_h.ref.accel.y) >> (INT32_ACCEL_FRAC -
- GH_GAIN_SCALE)); /* acceleration feedforward gain */
-
- /* trim max bank angle from PD */
- VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
-
- /* Update pos & speed error integral, zero it if not in_flight.
- * Integrate twice as fast when not only POS but also SPEED are wrong,
- * but do not integrate POS errors when the SPEED is already catching up.
- */
- if (in_flight) {
- /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
- guidance_h_trim_att_integrator.x += (guidance_h.gains.i * pd_x);
- guidance_h_trim_att_integrator.y += (guidance_h.gains.i * pd_y);
- /* saturate it */
- VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)),
- (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
- /* add it to the command */
- guidance_h_cmd_earth.x += (guidance_h_trim_att_integrator.x >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2));
- guidance_h_cmd_earth.y += (guidance_h_trim_att_integrator.y >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2));
- } else {
- INT_VECT2_ZERO(guidance_h_trim_att_integrator);
- }
-
- /* compute a better approximation of force commands by taking thrust into account */
- if (guidance_h.approx_force_by_thrust && in_flight) {
- static int32_t thrust_cmd_filt;
- int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
- thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
- (GUIDANCE_H_THRUST_CMD_FILTER + 1);
- guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2),
- thrust_cmd_filt));
- guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2),
- thrust_cmd_filt));
- }
-
- VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank);
-}
-#endif
-
void guidance_h_hover_enter(void)
{
/* reset speed setting */
@@ -569,18 +399,21 @@ void guidance_h_hover_enter(void)
/* set guidance to current heading and position */
guidance_h.rc_sp.psi = stateGetNedToBodyEulers_f()->psi;
guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
+
+ /* call specific implementation */
+ guidance_h_run_enter();
}
void guidance_h_nav_enter(void)
{
/* horizontal position setpoint from navigation/flightplan */
- guidance_h_set_pos(
- POS_FLOAT_OF_BFP(navigation_carrot.y),
- POS_FLOAT_OF_BFP(navigation_carrot.x));
+ guidance_h_set_pos(nav.carrot.y, nav.carrot.x);
reset_guidance_reference_from_current_position();
/* set nav_heading to current heading */
- nav_heading = stateGetNedToBodyEulers_i()->psi;
- guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi);
+ nav.heading = stateGetNedToBodyEulers_f()->psi;
+ guidance_h_set_heading(nav.heading);
+ /* call specific implementation */
+ guidance_h_run_enter();
}
void guidance_h_from_nav(bool in_flight)
@@ -589,54 +422,60 @@ void guidance_h_from_nav(bool in_flight)
guidance_h_nav_enter();
}
- if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
- stabilization_cmd[COMMAND_ROLL] = nav_cmd_roll;
- stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
- stabilization_cmd[COMMAND_YAW] = nav_cmd_yaw;
- } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
- struct Int32Eulers sp_cmd_i;
- sp_cmd_i.phi = nav_roll;
- sp_cmd_i.theta = nav_pitch;
- sp_cmd_i.psi = nav_heading;
- stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
- stabilization_attitude_run(in_flight);
-
-#if HYBRID_NAVIGATION
- //make sure the heading is right before leaving horizontal_mode attitude
- guidance_hybrid_reset_heading(&sp_cmd_i);
-#endif
- } else if (horizontal_mode == HORIZONTAL_MODE_GUIDED) {
+ if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_MANUAL) {
+ stabilization_cmd[COMMAND_ROLL] = nav.cmd_roll;
+ stabilization_cmd[COMMAND_PITCH] = nav.cmd_pitch;
+ stabilization_cmd[COMMAND_YAW] = nav.cmd_yaw;
+ } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
+ if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
+ // directly apply quat setpoint
+ QUAT_BFP_OF_REAL(stab_att_sp_quat, nav.quat);
+ stabilization_attitude_run(in_flight);
+ }
+ else {
+ // it should be nav.setpoint_mode == NAV_SETPOINT_MODE_ATTITUDE
+ // TODO error handling ?
+ struct Int32Eulers sp_cmd_i;
+ sp_cmd_i.phi = ANGLE_BFP_OF_REAL(nav.roll);
+ sp_cmd_i.theta = ANGLE_BFP_OF_REAL(nav.pitch);
+ sp_cmd_i.psi = ANGLE_BFP_OF_REAL(nav.heading);
+ stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i);
+ stabilization_attitude_run(in_flight);
+ }
+ } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_GUIDED) {
guidance_h_guided_run(in_flight);
} else {
+ // update carrot for display, even if sp is changed in speed mode
+ guidance_h_set_pos(nav.carrot.y, nav.carrot.x);
+ switch (nav.setpoint_mode) {
+ case NAV_SETPOINT_MODE_POS:
+ // set guidance in NED
+ guidance_h_update_reference();
+ guidance_h_set_heading(nav.heading);
+ guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);
+ break;
-#if HYBRID_NAVIGATION
- INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target);
- guidance_hybrid_run();
-#else
- // set guidance in NED
- guidance_h_set_pos(
- POS_FLOAT_OF_BFP(navigation_carrot.y),
- POS_FLOAT_OF_BFP(navigation_carrot.x));
- guidance_h_update_reference();
+ case NAV_SETPOINT_MODE_SPEED:
+ guidance_h_set_vel(nav.speed.y, nav.speed.x); // nav speed is in ENU frame, convert to NED
+ guidance_h_update_reference();
+ guidance_h_set_heading(nav.heading);
+ guidance_h_cmd = guidance_h_run_speed(in_flight, &guidance_h);
+ break;
-#if GUIDANCE_HEADING_IS_FREE
- /* set psi command */
- guidance_h_set_heading(ANGLE_FLOAT_OF_BFP(nav_heading));
-#endif
+ case NAV_SETPOINT_MODE_ACCEL:
+ // TODO set_accel ref
+ guidance_h_set_heading(nav.heading);
+ guidance_h_cmd = guidance_h_run_accel(in_flight, &guidance_h);
+ break;
-#if GUIDANCE_INDI
- guidance_indi_run(&guidance_h.sp.heading);
-#else
- /* compute x,y earth commands */
- guidance_h_traj_run(in_flight);
+ default:
+ // nothing to do for other cases at the moment
+ break;
+ }
/* set final attitude setpoint */
- int32_t heading_sp_i = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
- stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
- heading_sp_i);
-#endif
-
-#endif
+ stabilization_attitude_set_stab_sp(&guidance_h_cmd);
stabilization_attitude_run(in_flight);
+
}
}
@@ -688,13 +527,6 @@ static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool in_flight
}
}
-void guidance_h_set_igain(uint32_t igain)
-{
- guidance_h.gains.i = igain;
- INT_VECT2_ZERO(guidance_h_trim_att_integrator);
-}
-
-
void guidance_h_guided_run(bool in_flight)
{
/* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
@@ -704,15 +536,9 @@ void guidance_h_guided_run(bool in_flight)
guidance_h_update_reference();
-#if GUIDANCE_INDI
- guidance_indi_run(&guidance_h.sp.heading);
-#else
- /* compute x,y earth commands */
- guidance_h_traj_run(in_flight);
+ guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);
/* set final attitude setpoint */
- int32_t heading_sp_i = ANGLE_BFP_OF_REAL(guidance_h.sp.heading);
- stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, heading_sp_i);
-#endif
+ stabilization_attitude_set_stab_sp(&guidance_h_cmd);
stabilization_attitude_run(in_flight);
}
@@ -751,7 +577,3 @@ void guidance_h_set_heading_rate(float rate)
guidance_h.sp.heading_rate = rate;
}
-const struct Int32Vect2 *guidance_h_get_pos_err(void)
-{
- return &guidance_h_pos_err;
-}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
index 2567fa6e09..64c85f7b70 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
@@ -14,9 +14,8 @@
* GNU General Public License for more details.
*
* 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.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
*/
/** @file firmwares/rotorcraft/guidance/guidance_h.h
@@ -35,6 +34,7 @@ extern "C" {
#include "math/pprz_algebra_float.h"
#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
+#include "firmwares/rotorcraft/stabilization.h"
#include "generated/airframe.h"
#include "std.h"
@@ -84,26 +84,15 @@ struct HorizontalGuidanceReference {
struct Int32Vect2 accel; ///< with #INT32_ACCEL_FRAC
};
-struct HorizontalGuidanceGains {
- int32_t p;
- int32_t d;
- int32_t i;
- int32_t v;
- int32_t a;
-};
-
struct HorizontalGuidance {
uint8_t mode;
/* configuration options */
bool use_ref;
- bool approx_force_by_thrust;
- /* gains */
- struct HorizontalGuidanceGains gains;
- struct HorizontalGuidanceSetpoint sp; ///< setpoints
+ struct HorizontalGuidanceSetpoint sp; ///< setpoints
struct HorizontalGuidanceReference ref; ///< reference calculated from setpoints
- struct FloatEulers rc_sp;
+ struct FloatEulers rc_sp; ///< remote control setpoint
};
extern struct HorizontalGuidance guidance_h;
@@ -114,6 +103,10 @@ extern void guidance_h_init(void);
extern void guidance_h_mode_changed(uint8_t new_mode);
extern void guidance_h_read_rc(bool in_flight);
extern void guidance_h_run(bool in_flight);
+extern void guidance_h_run_enter(void);
+extern struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
+extern struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
+extern struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
extern void guidance_h_hover_enter(void);
extern void guidance_h_nav_enter(void);
@@ -122,8 +115,6 @@ extern void guidance_h_nav_enter(void);
*/
extern void guidance_h_from_nav(bool in_flight);
-extern void guidance_h_set_igain(uint32_t igain);
-
/** Run GUIDED mode control
*/
extern void guidance_h_guided_run(bool in_flight);
@@ -156,12 +147,6 @@ extern void guidance_h_set_vel(float vx, float vy);
*/
extern void guidance_h_set_heading_rate(float rate);
-/** Gets the position error
- * @param none.
- * @return Pointer to a structure containing x and y position errors
- */
-extern const struct Int32Vect2 *guidance_h_get_pos_err(void);
-
/* Make sure that ref can only be temporarily disabled for testing,
* but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
*/
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
index 34d1f22bee..de75bc7bf8 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
@@ -37,24 +37,48 @@
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-/* for guidance_v_thrust_coeff */
+/* for guidance_v.thrust_coeff */
#include "firmwares/rotorcraft/guidance/guidance_v.h"
+#include "firmwares/rotorcraft/guidance/guidance_pid.h" // FIXME is it really what we want ?
+
// max airspeed for quadshot guidance
+#ifndef MAX_AIRSPEED
#define MAX_AIRSPEED 15
+#endif
+
// high res frac for integration of angles
#define INT32_ANGLE_HIGH_RES_FRAC 18
// Variables used for settings
int32_t guidance_hybrid_norm_ref_airspeed;
-float alt_pitch_gain = 0.3;
+float guidance_hybrid_norm_ref_airspeed_f;
int32_t max_airspeed = MAX_AIRSPEED;
int32_t wind_gain;
int32_t horizontal_speed_gain;
float max_turn_bank;
float turn_bank_gain;
+#define AIRSPEED_HOVER 4
+#define AIRSPEED_FORWARD 12
+#define CRUISE_THROTTLE 4000
+#define FWD_SPEED_P_GAIN 30
+#define FWD_ALT_THRUST_GAIN 0.35
+#define FWD_PID_DIV 2
+#define FWD_NOMINAL_PITCH 78.0
+#define FWD_PITCH_GAIN 2.1
+#define HOVER_P_GAIN 12
+
+int32_t cruise_throttle = CRUISE_THROTTLE;
+int32_t fwd_speed_p_gain = FWD_SPEED_P_GAIN;
+float fwd_alt_thrust_gain = FWD_ALT_THRUST_GAIN;
+float fwd_pid_div = FWD_PID_DIV;
+float fwd_nominal_pitch = FWD_NOMINAL_PITCH;
+float fwd_pitch_gain = FWD_PITCH_GAIN;
+int32_t hover_p_gain = HOVER_P_GAIN;
+
// Private variables
+static struct Int32Vect2 guidance_hybrid_groundspeed_sp;
static struct Int32Eulers guidance_hybrid_ypr_sp;
static struct Int32Vect2 guidance_hybrid_airspeed_sp;
static struct Int32Vect2 guidance_h_pos_err;
@@ -102,14 +126,16 @@ void guidance_hybrid_init(void)
INT_EULERS_ZERO(guidance_hybrid_ypr_sp);
INT_VECT2_ZERO(guidance_hybrid_airspeed_sp);
INT_VECT2_ZERO(guidance_hybrid_airspeed_ref);
+ INT_VECT2_ZERO(guidance_hybrid_groundspeed_sp);
high_res_psi = 0;
guidance_hovering = true;
- horizontal_speed_gain = 8;
+ horizontal_speed_gain = 6;
guidance_hybrid_norm_ref_airspeed = 0;
- max_turn_bank = 40.0;
- turn_bank_gain = 0.8;
- wind_gain = 64;
+ guidance_hybrid_norm_ref_airspeed_f = 0;
+ max_turn_bank = 23.0;
+ turn_bank_gain = 0.5;
+ wind_gain = 35;
force_forward_flight = 0;
INT_VECT2_ZERO(wind_estimate);
INT_VECT2_ZERO(guidance_hybrid_ref_airspeed);
@@ -126,14 +152,56 @@ void guidance_hybrid_init(void)
while ((_a) < -(INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) += (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
}
-void guidance_hybrid_run(void)
+struct StabilizationSetpoint guidance_hybrid_run(void)
{
guidance_hybrid_determine_wind_estimate();
- guidance_hybrid_position_to_airspeed();
+ guidance_hybrid_groundspeed_to_airspeed();
guidance_hybrid_airspeed_to_attitude(&guidance_hybrid_ypr_sp);
- guidance_hybrid_set_cmd_i(&guidance_hybrid_ypr_sp);
+ return guidance_hybrid_set_cmd_i(&guidance_hybrid_ypr_sp);
}
+struct StabilizationSetpoint guidance_hybrid_h_run_pos(bool in_flight UNUSED, struct HorizontalGuidance *gh)
+{
+ // compute position error
+ VECT2_DIFF(guidance_h_pos_err, gh->sp.pos, *stateGetPositionNed_i());
+ // Compute ground speed setpoint
+ VECT2_SDIV(guidance_hybrid_groundspeed_sp, guidance_h_pos_err, horizontal_speed_gain);
+ return guidance_hybrid_run();
+}
+
+struct StabilizationSetpoint guidance_hybrid_h_run_speed(bool in_flight UNUSED, struct HorizontalGuidance *gh)
+{
+ // directly set ground speed setpoint
+ VECT2_COPY(guidance_hybrid_groundspeed_sp, gh->sp.speed);
+ return guidance_hybrid_run();
+}
+
+struct StabilizationSetpoint guidance_hybrid_h_run_accel(bool in_flight UNUSED, struct HorizontalGuidance *gh UNUSED)
+{
+ struct StabilizationSetpoint sp = { 0 };
+ // TODO
+ return sp;
+}
+
+int32_t guidance_hybrid_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
+{
+ int32_t delta_t = guidance_pid_v_run_pos(in_flight, gv);
+ return guidance_hybrid_vertical(delta_t);
+}
+
+int32_t guidance_hybrid_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
+{
+ int32_t delta_t = guidance_pid_v_run_speed(in_flight, gv);
+ return guidance_hybrid_vertical(delta_t);
+}
+
+int32_t guidance_hybrid_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
+{
+ int32_t delta_t = guidance_pid_v_run_accel(in_flight, gv);
+ return guidance_hybrid_vertical(delta_t);
+}
+
+
void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
{
guidance_hybrid_ypr_sp.psi = sp_cmd->psi;
@@ -170,10 +238,11 @@ void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
//reference goes with a steady pace towards the setpoint airspeed
//hold ref norm below 4 m/s until heading is aligned
- if (!((norm_sp_airspeed > (4 << 8)) && (guidance_hybrid_norm_ref_airspeed < (4 << 8))
- && (guidance_hybrid_norm_ref_airspeed > ((4 << 8) - 10)) && (fabs(heading_diff) > (5.0 / 180.0 * 3.14)))) {
+ if (!((norm_sp_airspeed > (AIRSPEED_HOVER << 8)) && (guidance_hybrid_norm_ref_airspeed < (AIRSPEED_HOVER << 8))
+ && (guidance_hybrid_norm_ref_airspeed > ((AIRSPEED_HOVER << 8) - 10)) && (fabs(heading_diff) > (5.0 / 180.0 * 3.14)))) {
guidance_hybrid_norm_ref_airspeed = guidance_hybrid_norm_ref_airspeed + ((int32_t)(norm_sp_airspeed >
guidance_hybrid_norm_ref_airspeed) * 2 - 1) * 3 / 2;
+ guidance_hybrid_norm_ref_airspeed_f = FLOAT_OF_BFP(guidance_hybrid_norm_ref_airspeed, 8);
}
norm_sp_airspeed_disp = norm_sp_airspeed;
@@ -186,7 +255,7 @@ void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
guidance_hybrid_ref_airspeed.x = (guidance_hybrid_norm_ref_airspeed * c_psi) >> INT32_TRIG_FRAC;
guidance_hybrid_ref_airspeed.y = (guidance_hybrid_norm_ref_airspeed * s_psi) >> INT32_TRIG_FRAC;
- if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
+ if (guidance_hybrid_norm_ref_airspeed_f < AIRSPEED_HOVER) {
/// if required speed is lower than 4 m/s act like a rotorcraft
// translate speed_sp into bank angle and heading
@@ -203,35 +272,34 @@ void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
// 2) calculate roll/pitch commands
struct Int32Vect2 hover_sp;
//if the setpoint is beyond 4m/s but the ref is not, the norm of the hover sp will stay at 4m/s
- if (norm_sp_airspeed > (4 << 8)) {
- hover_sp.x = (guidance_hybrid_airspeed_sp.x << 8) / norm_sp_airspeed * 4;
- hover_sp.y = (guidance_hybrid_airspeed_sp.y << 8) / norm_sp_airspeed * 4;
+ if (norm_sp_airspeed > (AIRSPEED_HOVER << 8)) {
+ hover_sp.x = (guidance_hybrid_airspeed_sp.x << 8) / norm_sp_airspeed * AIRSPEED_HOVER;
+ hover_sp.y = (guidance_hybrid_airspeed_sp.y << 8) / norm_sp_airspeed * AIRSPEED_HOVER;
} else {
hover_sp.x = guidance_hybrid_airspeed_sp.x;
hover_sp.y = guidance_hybrid_airspeed_sp.y;
}
// gain of 10 means that for 4 m/s an angle of 40 degrees is needed
- ypr_sp->theta = (((- (c_psi * hover_sp.x + s_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
- ypr_sp->phi = ((((- s_psi * hover_sp.x + c_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
+ ypr_sp->theta = (((- (c_psi * hover_sp.x + s_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * hover_p_gain * INT32_ANGLE_PI / 180) >> 8;
+ ypr_sp->phi = ((((- s_psi * hover_sp.x + c_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * hover_p_gain * INT32_ANGLE_PI / 180) >> 8;
} else {
/// if required speed is higher than 4 m/s act like a fixedwing
// translate speed_sp into theta + thrust
// coordinated turns to change heading
// calculate required pitch angle from airspeed_sp magnitude
- if (guidance_hybrid_norm_ref_airspeed > (15 << 8)) {
- ypr_sp->theta = -ANGLE_BFP_OF_REAL(RadOfDeg(78.0));
- } else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) {
- ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (8 << 8)) * 2 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
- RadOfDeg(68.0));
+ if (guidance_hybrid_norm_ref_airspeed_f > AIRSPEED_FORWARD) {
+ ypr_sp->theta = -ANGLE_BFP_OF_REAL(RadOfDeg(fwd_nominal_pitch));
} else {
- ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (4 << 8)) * 7 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
- RadOfDeg(40.0));
+ float airspeed_transition = (guidance_hybrid_norm_ref_airspeed_f - AIRSPEED_HOVER) / (AIRSPEED_FORWARD - AIRSPEED_HOVER);
+ float hover_max_deg = hover_p_gain * AIRSPEED_HOVER;
+ float diff_deg = (fwd_nominal_pitch - hover_max_deg) * airspeed_transition;
+ ypr_sp->theta = -ANGLE_BFP_OF_REAL(RadOfDeg(diff_deg + hover_max_deg));
}
// if the sp_airspeed is within hovering range, don't start a coordinated turn
- if (norm_sp_airspeed < (4 << 8)) {
+ if (norm_sp_airspeed < (AIRSPEED_HOVER << 8)) {
omega = 0;
ypr_sp->phi = 0;
} else { // coordinated turn
@@ -240,8 +308,7 @@ void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
if (ypr_sp->phi < ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI); }
//feedforward estimate angular rotation omega = g*tan(phi)/v
- omega = ANGLE_BFP_OF_REAL(9.81 / POS_FLOAT_OF_BFP(guidance_hybrid_norm_ref_airspeed) * tanf(ANGLE_FLOAT_OF_BFP(
- ypr_sp->phi)));
+ omega = ANGLE_BFP_OF_REAL(9.81 / guidance_hybrid_norm_ref_airspeed_f * tanf(ANGLE_FLOAT_OF_BFP(ypr_sp->phi)));
if (omega > ANGLE_BFP_OF_REAL(0.7)) { omega = ANGLE_BFP_OF_REAL(0.7); }
if (omega < ANGLE_BFP_OF_REAL(-0.7)) { omega = ANGLE_BFP_OF_REAL(-0.7); }
@@ -261,15 +328,8 @@ void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
ypr_sp->theta = ypr_sp->theta + v_control_pitch;
}
-void guidance_hybrid_position_to_airspeed(void)
+void guidance_hybrid_groundspeed_to_airspeed(void)
{
- /* compute position error */
- VECT2_DIFF(guidance_h_pos_err, guidance_h.sp.pos, *stateGetPositionNed_i());
-
- // Compute ground speed setpoint
- struct Int32Vect2 guidance_hybrid_groundspeed_sp;
- VECT2_SDIV(guidance_hybrid_groundspeed_sp, guidance_h_pos_err, horizontal_speed_gain);
-
int32_t norm_groundspeed_sp;
norm_groundspeed_sp = int32_vect2_norm(&guidance_hybrid_groundspeed_sp);
@@ -284,8 +344,8 @@ void guidance_hybrid_position_to_airspeed(void)
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
- guidance_hybrid_groundspeed_sp.x = (15 * c_psi) >> (INT32_TRIG_FRAC - 8);
- guidance_hybrid_groundspeed_sp.y = (15 * s_psi) >> (INT32_TRIG_FRAC - 8);
+ guidance_hybrid_groundspeed_sp.x = (max_airspeed * c_psi) >> (INT32_TRIG_FRAC - 8);
+ guidance_hybrid_groundspeed_sp.y = (max_airspeed * s_psi) >> (INT32_TRIG_FRAC - 8);
}
}
@@ -347,7 +407,7 @@ void guidance_hybrid_determine_wind_estimate(void)
wind_estimate.y = ((wind_estimate_high_res.y) >> 8);
}
-void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
+struct StabilizationSetpoint guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
{
/// @todo calc sp_quat in fixed-point
@@ -370,41 +430,108 @@ void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
int32_quat_of_axis_angle(&q_yaw_sp, &zaxis, sp_cmd->psi);
// first apply the roll/pitch setpoint and then the yaw
- int32_quat_comp(&stab_att_sp_quat, &q_yaw_sp, &q_rp_i);
+ struct Int32Quat quat_sp;
+ int32_quat_comp(&quat_sp, &q_yaw_sp, &q_rp_i);
- int32_eulers_of_quat(&stab_att_sp_euler, &stab_att_sp_quat);
+ return stab_sp_from_quat_i(&quat_sp);
}
-void guidance_hybrid_vertical(void)
+/**
+ * Take a thrust command as input and returns the modified value
+ * according to the current flight regime
+ */
+int32_t guidance_hybrid_vertical(int32_t delta_t)
{
- if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
- //if airspeed ref < 4 only thrust
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+ int32_t hybrid_delta_t = 0;
+
+ float fwd_speed_err = guidance_hybrid_norm_ref_airspeed_f - AIRSPEED_FORWARD;
+ float fwd_thrust = cruise_throttle
+ + (fwd_speed_err * fwd_speed_p_gain)
+ + (delta_t - (MAX_PPRZ * guidance_v.nominal_throttle)) * fwd_alt_thrust_gain;
+ int32_t hover_thrust = delta_t;
+
+ float alt_control_pitch = (delta_t - MAX_PPRZ * guidance_v.nominal_throttle) * fwd_pitch_gain;
+ int32_t fwd_pitch = ANGLE_BFP_OF_REAL(alt_control_pitch / MAX_PPRZ);
+
+ /* Hover regime */
+ if (guidance_hybrid_norm_ref_airspeed_f < AIRSPEED_HOVER) {
+ hybrid_delta_t = hover_thrust;
+
+ // Do not control pitch and only PID for hover
v_control_pitch = 0;
- guidance_v_kp = GUIDANCE_V_HOVER_KP;
- guidance_v_kd = GUIDANCE_V_HOVER_KD;
- guidance_v_ki = GUIDANCE_V_HOVER_KI;
- } else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) { //if airspeed ref > 8 only pitch,
- //at 15 m/s the thrust has to be 33%
- stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ / 5 + (((guidance_hybrid_norm_ref_airspeed - (8 << 8)) / 7 *
- (MAX_PPRZ / 3 - MAX_PPRZ / 5)) >> 8) + (guidance_v_delta_t - MAX_PPRZ / 2) / 10;
- //stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ/5;
- // stabilization_cmd[COMMAND_THRUST] = ((guidance_hybrid_norm_ref_airspeed - (8<<8)) / 7 * (MAX_PPRZ/3 - MAX_PPRZ/5))>>8 + 9600/5;
- //Control altitude with pitch, now only proportional control
- float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
- v_control_pitch = ANGLE_BFP_OF_REAL(alt_control_pitch / (MAX_PPRZ * guidance_v_nominal_throttle));
- guidance_v_kp = GUIDANCE_V_HOVER_KP / 2;
- guidance_v_kd = GUIDANCE_V_HOVER_KD / 2;
- guidance_v_ki = GUIDANCE_V_HOVER_KI / 2;
- } else { //if airspeed ref > 4 && < 8 both
- int32_t airspeed_transition = (guidance_hybrid_norm_ref_airspeed - (4 << 8)) / 4; //divide by 4 to scale it to 0-1 (<<8)
- stabilization_cmd[COMMAND_THRUST] = ((MAX_PPRZ / 5 + (guidance_v_delta_t - MAX_PPRZ / 2) / 10) * airspeed_transition +
- guidance_v_delta_t * ((1 << 8) - airspeed_transition)) >> 8;
- float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
- v_control_pitch = INT_MULT_RSHIFT((int32_t) ANGLE_BFP_OF_REAL(alt_control_pitch / (MAX_PPRZ *
- guidance_v_nominal_throttle)), airspeed_transition, 8);
- guidance_v_kp = GUIDANCE_V_HOVER_KP;
- guidance_v_kd = GUIDANCE_V_HOVER_KD;
- guidance_v_ki = GUIDANCE_V_HOVER_KI;
+ guidance_pid.kp = GUIDANCE_V_HOVER_KP;
+ guidance_pid.kd = GUIDANCE_V_HOVER_KD;
+ guidance_pid.ki = GUIDANCE_V_HOVER_KI;
}
+ /* Forward regime */
+ else if (guidance_hybrid_norm_ref_airspeed_f > AIRSPEED_FORWARD) {
+ hybrid_delta_t = fwd_thrust;
+
+ //Control altitude with pitch, now only proportional control
+ v_control_pitch = fwd_pitch;
+ guidance_pid.kp = GUIDANCE_V_HOVER_KP / fwd_pid_div;
+ guidance_pid.kd = GUIDANCE_V_HOVER_KD / fwd_pid_div;
+ guidance_pid.ki = GUIDANCE_V_HOVER_KI / fwd_pid_div;
+ }
+ /* Transition regime */
+ else {
+ float airspeed_transition = (guidance_hybrid_norm_ref_airspeed_f - AIRSPEED_HOVER) / (AIRSPEED_FORWARD - AIRSPEED_HOVER); // scaled to 0-1
+ hybrid_delta_t = (fwd_thrust * airspeed_transition
+ + hover_thrust * (1 - airspeed_transition));
+
+ // Control by both thrust and pitch
+ v_control_pitch = fwd_pitch * airspeed_transition;
+ guidance_pid.kp = GUIDANCE_V_HOVER_KP;
+ guidance_pid.kd = GUIDANCE_V_HOVER_KD;
+ guidance_pid.ki = GUIDANCE_V_HOVER_KI;
+ }
+
+ return hybrid_delta_t;
}
+
+
+#if GUIDANCE_HYBRID_USE_AS_DEFAULT
+// guidance hybrid is implementing the default functions of guidance
+
+void guidance_h_run_enter(void)
+{
+ // nothing to do
+}
+
+void guidance_v_run_enter(void)
+{
+ // nothing to do
+}
+
+struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_hybrid_h_run_pos(in_flight, gh);
+}
+
+struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_hybrid_h_run_speed(in_flight, gh);
+}
+
+struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_hybrid_h_run_accel(in_flight, gh);
+}
+
+int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_hybrid_v_run_pos(in_flight, gv);
+}
+
+int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_hybrid_v_run_speed(in_flight, gv);
+}
+
+int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_hybrid_v_run_accel(in_flight, gv);
+}
+
+#endif
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
index eec1077f6b..bb253b0a2e 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
@@ -31,28 +31,44 @@
#define GUIDANCE_HYBRID_H
#include "math/pprz_algebra_int.h"
+#include "firmwares/rotorcraft/guidance.h"
+#include "firmwares/rotorcraft/stabilization.h"
extern int32_t guidance_hybrid_norm_ref_airspeed;
-extern float alt_pitch_gain;
extern int32_t max_airspeed;
extern int32_t wind_gain;
extern int32_t horizontal_speed_gain;
extern float max_turn_bank;
extern float turn_bank_gain;
-/** Runs the Hybrid Guidance main functions.
- */
-extern void guidance_hybrid_run(void);
+extern int32_t cruise_throttle;
+extern int32_t fwd_speed_p_gain;
+extern float fwd_alt_thrust_gain;
+extern float fwd_pid_div;
+extern float fwd_nominal_pitch;
+extern float fwd_pitch_gain;
+extern int32_t hover_p_gain;
/** Hybrid Guidance Initialization function.
* @param
*/
extern void guidance_hybrid_init(void);
+/** Runs the Hybrid Guidance main functions.
+ */
+extern struct StabilizationSetpoint guidance_hybrid_run(void);
+
+extern struct StabilizationSetpoint guidance_hybrid_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
+extern struct StabilizationSetpoint guidance_hybrid_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
+extern struct StabilizationSetpoint guidance_hybrid_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
+extern int32_t guidance_hybrid_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
+extern int32_t guidance_hybrid_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
+extern int32_t guidance_hybrid_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
+
/** Creates the attitude set-points from an orientation vector.
* @param sp_cmd The orientation vector
*/
-extern void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd);
+extern struct StabilizationSetpoint guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd);
/** Convert a required airspeed to a certain attitude for the Hybrid.
* @param ypr_sp Attitude set-point
@@ -61,7 +77,7 @@ extern void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp);
/** Description.
*/
-extern void guidance_hybrid_position_to_airspeed(void);
+extern void guidance_hybrid_groundspeed_to_airspeed(void);
/** Description.
*/
@@ -73,8 +89,11 @@ extern void guidance_hybrid_determine_wind_estimate(void);
extern void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd);
/** Description.
+ * Change thurst command from standard hover loop to hybrid thrust command
+ * @param delta_t original thurst command
+ * @return modified thrust command
*/
-extern void guidance_hybrid_vertical(void);
+extern int32_t guidance_hybrid_vertical(int32_t delta_t);
extern bool force_forward_flight;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
index 9ec6310120..04cf15491f 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -69,12 +69,13 @@ float guidance_indi_speed_gain = 1.8;
#endif
abi_event accel_sp_ev;
static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp);
-struct FloatVect3 indi_accel_sp = {0.0, 0.0, 0.0};
+struct FloatVect3 indi_accel_sp = {0.0f, 0.0f, 0.0f};
bool indi_accel_sp_set_2d = false;
bool indi_accel_sp_set_3d = false;
-struct FloatVect3 speed_sp = {0.0, 0.0, 0.0};
-struct FloatVect3 sp_accel = {0.0, 0.0, 0.0};
+// FIXME make a proper structure for these variables
+struct FloatVect3 speed_sp = {0.0f, 0.0f, 0.0f};
+struct FloatVect3 sp_accel = {0.0f, 0.0f, 0.0f};
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
static void guidance_indi_filter_thrust(void);
@@ -172,29 +173,26 @@ void guidance_indi_enter(void)
}
/**
+ * @param accel_sp accel setpoint in NED frame [m/s^2]
* @param heading_sp the desired heading [rad]
+ * @return stabilization setpoint structure
*
* main indi guidance function
*/
-void guidance_indi_run(float *heading_sp)
+struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
{
struct FloatEulers eulers_yxz;
struct FloatQuat * statequat = stateGetNedToBodyQuat_f();
float_eulers_of_quat_yxz(&eulers_yxz, statequat);
+ // set global accel sp variable FIXME clean this
+ sp_accel = *accel_sp;
+
//filter accel to get rid of noise and filter attitude to synchronize with accel
guidance_indi_propagate_filters(&eulers_yxz);
- //Linear controller to find the acceleration setpoint from position and velocity
- float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
- float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
- float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
-
- // Use feed forward part from reference model
- speed_sp.x = pos_x_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.x);
- speed_sp.y = pos_y_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.y);
- speed_sp.z = pos_z_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_v_zd_ref);
-
+ // FIXME the ABI message overwrite the accel setpoint
+ // it update should be replaced by a call to the run function
// If the acceleration setpoint is set over ABI message
if (indi_accel_sp_set_2d) {
sp_accel.x = indi_accel_sp.x;
@@ -215,11 +213,8 @@ void guidance_indi_run(float *heading_sp)
if (dt > 0.5) {
indi_accel_sp_set_3d = false;
}
- } else {
- sp_accel.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(guidance_h.ref.accel.x);
- sp_accel.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(guidance_h.ref.accel.y);
- sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(guidance_v_zdd_ref);
}
+ // else, don't change sp_accel
#if GUIDANCE_INDI_RC_DEBUG
#warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
@@ -262,7 +257,7 @@ void guidance_indi_run(float *heading_sp)
guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;
- guidance_euler_cmd.psi = *heading_sp;
+ guidance_euler_cmd.psi = heading_sp;
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
guidance_indi_filter_thrust();
@@ -288,7 +283,61 @@ void guidance_indi_run(float *heading_sp)
//set the quat setpoint with the calculated roll and pitch
struct FloatQuat q_sp;
float_quat_of_eulers_yxz(&q_sp, &guidance_euler_cmd);
- QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
+
+ return stab_sp_from_quat_f(&q_sp);
+}
+
+struct StabilizationSetpoint guidance_indi_run_pos(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv)
+{
+ struct FloatVect3 pos_err;
+ struct FloatVect3 accel_sp;
+
+ //Linear controller to find the acceleration setpoint from position and velocity
+ pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
+ pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
+ pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
+
+ // Use feed forward part from reference model
+ speed_sp.x = pos_err.x * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
+ speed_sp.y = pos_err.y * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+ speed_sp.z = pos_err.z * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(gv->zd_ref);
+
+ accel_sp.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
+ accel_sp.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
+ accel_sp.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
+
+ return guidance_indi_run(&accel_sp, gh->sp.heading);
+}
+
+struct StabilizationSetpoint guidance_indi_run_speed(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv)
+{
+ struct FloatVect3 accel_sp;
+
+ // Use feed forward part from reference model
+ speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
+ speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+ speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
+
+ accel_sp.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
+ accel_sp.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
+ accel_sp.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
+
+ return guidance_indi_run(&accel_sp, gh->sp.heading);
+}
+
+struct StabilizationSetpoint guidance_indi_run_accel(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv)
+{
+ struct FloatVect3 accel_sp;
+
+ speed_sp.x = 0.f;
+ speed_sp.y = 0.f;
+ speed_sp.z = 0.f;
+
+ accel_sp.x = (speed_sp.x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
+ accel_sp.y = (speed_sp.y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
+ accel_sp.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
+
+ return guidance_indi_run(&accel_sp, gh->sp.heading);
}
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
@@ -401,3 +450,47 @@ static void accel_sp_cb(uint8_t sender_id __attribute__((unused)), uint8_t flag,
}
}
+#if GUIDANCE_INDI_USE_AS_DEFAULT
+// guidance indi control function is implementing the default functions of guidance
+
+void guidance_h_run_enter(void)
+{
+ guidance_indi_enter();
+}
+
+void guidance_v_run_enter(void)
+{
+ // nothing to do
+}
+
+struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_indi_run_pos(in_flight, gh, &guidance_v);
+}
+
+struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_indi_run_speed(in_flight, gh, &guidance_v);
+}
+
+struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_indi_run_accel(in_flight, gh, &guidance_v);
+}
+
+int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ return 0; // nothing to do
+}
+
+int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ return 0; // nothing to do
+}
+
+int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ return 0; // nothing to do
+}
+
+#endif
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
index 68de45053c..17f21540e4 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
@@ -31,11 +31,16 @@
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
+#include "firmwares/rotorcraft/guidance.h"
+#include "firmwares/rotorcraft/stabilization.h"
-extern void guidance_indi_enter(void);
-extern void guidance_indi_run(float *heading_sp);
-extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, float heading);
extern void guidance_indi_init(void);
+extern void guidance_indi_enter(void);
+
+extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
+extern struct StabilizationSetpoint guidance_indi_run_pos(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv);
+extern struct StabilizationSetpoint guidance_indi_run_speed(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv);
+extern struct StabilizationSetpoint guidance_indi_run_accel(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv);
extern float guidance_indi_specific_force_gain;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index a16240ac34..170f0c0dc8 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -29,18 +29,12 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
-#include "modules/ins/ins_int.h"
#include "modules/radio_control/radio_control.h"
#include "state.h"
-#include "modules/imu/imu.h"
-#include "firmwares/rotorcraft/guidance/guidance_h.h"
-#include "firmwares/rotorcraft/guidance/guidance_v.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
-#include "firmwares/rotorcraft/stabilization.h"
#include "stdio.h"
#include "filters/low_pass_filter.h"
#include "modules/core/abi.h"
@@ -103,17 +97,6 @@ float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;
#define GUIDANCE_INDI_ZERO_AIRSPEED FALSE
#endif
-// Max ground speed that will be commanded
-#ifndef GUIDANCE_INDI_NAV_SPEED_MARGIN
-#define GUIDANCE_INDI_NAV_SPEED_MARGIN 10.0
-#endif
-#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
-float nav_max_speed = NAV_MAX_SPEED;
-
-#ifndef MAX_DECELERATION
-#define MAX_DECELERATION 1.
-#endif
-
/*Airspeed threshold where making a turn is "worth it"*/
#ifndef TURN_AIRSPEED_TH
#define TURN_AIRSPEED_TH 10.0
@@ -122,6 +105,8 @@ float nav_max_speed = NAV_MAX_SPEED;
/*Boolean to force the heading to a static value (only use for specific experiments)*/
bool take_heading_control = false;
+bool force_forward = false;
+
struct FloatVect3 sp_accel = {0.0,0.0,0.0};
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
@@ -145,12 +130,6 @@ static void guidance_indi_filter_thrust(void);
#endif
#endif
-#ifdef GUIDANCE_INDI_LINE_GAIN
-float guidance_indi_line_gain = GUIDANCE_INDI_LINE_GAIN;
-#else
-float guidance_indi_line_gain = 1.0;
-#endif
-
float inv_eff[4];
// Max bank angle in radians
@@ -174,6 +153,7 @@ struct FloatVect3 euler_cmd;
float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
+float guidance_indi_hybrid_heading_sp = 0.f;
struct FloatEulers guidance_euler_cmd;
float thrust_in;
@@ -190,9 +170,6 @@ float time_of_vel_sp = 0.0;
void guidance_indi_propagate_filters(void);
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
static float guidance_indi_get_liftd(float pitch, float theta);
-struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain);
-struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain);
-struct FloatVect3 nav_get_speed_setpoint(float pos_gain);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -244,6 +221,7 @@ void guidance_indi_init(void)
void guidance_indi_enter(void) {
thrust_in = stabilization_cmd[COMMAND_THRUST];
thrust_act = thrust_in;
+ guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;
float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
float sample_time = 1.0 / PERIODIC_FREQUENCY;
@@ -258,162 +236,46 @@ void guidance_indi_enter(void) {
#include "firmwares/rotorcraft/navigation.h"
/**
+ * @param accel_sp accel setpoint in NED frame [m/s^2]
* @param heading_sp the desired heading [rad]
+ * @return stabilization setpoint structure
*
* main indi guidance function
*/
-void guidance_indi_run(float *heading_sp) {
+struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp)
+{
+ // set global accel sp variable FIXME clean this
+ sp_accel = *accel_sp;
- /*Obtain eulers with zxy rotation order*/
+ /* Obtain eulers with zxy rotation order */
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
- /*Calculate the transition percentage so that the ctrl_effecitveness scheduling works*/
- transition_percentage = BFP_OF_REAL((eulers_zxy.theta/RadOfDeg(-75.0))*100,INT32_PERCENTAGE_FRAC);
- Bound(transition_percentage,0,BFP_OF_REAL(100.0,INT32_PERCENTAGE_FRAC));
+ /* Calculate the transition percentage so that the ctrl_effecitveness scheduling works */
+ transition_percentage = BFP_OF_REAL((eulers_zxy.theta/RadOfDeg(-75.0f))*100,INT32_PERCENTAGE_FRAC);
+ Bound(transition_percentage,0,BFP_OF_REAL(100.0f,INT32_PERCENTAGE_FRAC));
const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
transition_theta_offset = INT_MULT_RSHIFT((transition_percentage <<
(INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100, max_offset, INT32_ANGLE_FRAC);
- //filter accel to get rid of noise and filter attitude to synchronize with accel
+ // filter accel to get rid of noise and filter attitude to synchronize with accel
guidance_indi_propagate_filters();
- //Linear controller to find the acceleration setpoint from position and velocity
- float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
- float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
- float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
-
- // First check for velocity setpoint from module
- float dt = get_sys_time_float() - time_of_vel_sp;
- // If the input command is not updated after a timeout, switch back to flight plan control
- if (dt < 0.5) {
- gi_speed_sp.x = indi_vel_sp.x;
- gi_speed_sp.y = indi_vel_sp.y;
- gi_speed_sp.z = indi_vel_sp.z;
- } else if(autopilot.mode == AP_MODE_NAV) {
- gi_speed_sp = nav_get_speed_setpoint(gih_params.pos_gain);
- } else {
- gi_speed_sp.x = pos_x_err * gih_params.pos_gain;
- gi_speed_sp.y = pos_y_err * gih_params.pos_gain;
- gi_speed_sp.z = pos_z_err * gih_params.pos_gainz;
- }
-
- //for rc control horizontal, rotate from body axes to NED
- float psi = eulers_zxy.psi;
- /*NAV mode*/
- float speed_sp_b_x = cosf(psi) * gi_speed_sp.x + sinf(psi) * gi_speed_sp.y;
- float speed_sp_b_y =-sinf(psi) * gi_speed_sp.x + cosf(psi) * gi_speed_sp.y;
-
- // Get airspeed or zero it
- float airspeed;
- if (GUIDANCE_INDI_ZERO_AIRSPEED) {
- airspeed = 0.0;
- } else {
- airspeed = stateGetAirspeed_f();
- }
-
- struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
- struct FloatVect2 airspeed_v = {cos(psi)*airspeed, sin(psi)*airspeed};
- struct FloatVect2 windspeed;
- VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
-
- VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp
- float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
-
- // Make turn instead of straight line
- if((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0) )) {
-
- // Give the wind cancellation priority.
- if (norm_des_as > guidance_indi_max_airspeed) {
- float groundspeed_factor = 0.0;
-
- // if the wind is faster than we can fly, just fly in the wind direction
- if(FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) {
- float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y;
- float bv = -2 * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y);
- float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
-
- float dv = bv * bv - 4.0 * av * cv;
-
- // dv can only be positive, but just in case
- if(dv < 0.0) {
- dv = fabs(dv);
- }
- float d_sqrt = sqrtf(dv);
-
- groundspeed_factor = (-bv + d_sqrt) / (2.0 * av);
- }
-
- desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x;
- desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y;
-
- speed_sp_b_x = guidance_indi_max_airspeed;
- }
-
- // desired airspeed can not be larger than max airspeed
- speed_sp_b_x = Min(norm_des_as,guidance_indi_max_airspeed);
-
- if(force_forward) {
- speed_sp_b_x = guidance_indi_max_airspeed;
- }
-
- // Calculate accel sp in body axes, because we need to regulate airspeed
- struct FloatVect2 sp_accel_b;
- // In turn acceleration proportional to heading diff
- sp_accel_b.y = atan2(desired_airspeed.y, desired_airspeed.x) - psi;
- FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
- sp_accel_b.y *= gih_params.heading_bank_gain;
-
- // Control the airspeed
- sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
-
- sp_accel.x = cosf(psi) * sp_accel_b.x - sinf(psi) * sp_accel_b.y;
- sp_accel.y = sinf(psi) * sp_accel_b.x + cosf(psi) * sp_accel_b.y;
-
- sp_accel.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
- } else { // Go somewhere in the shortest way
-
- if(airspeed > 10.0) {
- // Groundspeed vector in body frame
- float groundspeed_x = cosf(psi) * stateGetSpeedNed_f()->x + sinf(psi) * stateGetSpeedNed_f()->y;
- float speed_increment = speed_sp_b_x - groundspeed_x;
-
- // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
- if((speed_increment + airspeed) > guidance_indi_max_airspeed) {
- speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
- }
- }
-
- gi_speed_sp.x = cosf(psi) * speed_sp_b_x - sinf(psi) * speed_sp_b_y;
- gi_speed_sp.y = sinf(psi) * speed_sp_b_x + cosf(psi) * speed_sp_b_y;
-
- sp_accel.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
- sp_accel.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
- sp_accel.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
- }
-
- // Bound the acceleration setpoint
- float accelbound = 3.0 + airspeed/guidance_indi_max_airspeed*5.0;
- vect_bound_in_2d(&sp_accel, accelbound);
- /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
- /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
- BoundAbs(sp_accel.z, 3.0);
-
#if GUIDANCE_INDI_RC_DEBUG
#warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
- //for rc control horizontal, rotate from body axes to NED
+ // for rc control horizontal, rotate from body axes to NED
float psi = eulers_zxy.psi;
float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
- //for rc vertical control
+ // for rc vertical control
sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
#endif
- //Calculate matrix of partial derivatives
+ // Calculate matrix of partial derivatives
guidance_indi_calcg_wing(&Ga);
- //Invert this matrix
+ // Invert this matrix
MAT33_INV(Ga_inv, Ga);
struct FloatVect3 accel_filt;
@@ -426,13 +288,13 @@ void guidance_indi_run(float *heading_sp) {
a_diff.y = sp_accel.y - accel_filt.y;
a_diff.z = sp_accel.z - accel_filt.z;
- //Bound the acceleration error so that the linearization still holds
+ // Bound the acceleration error so that the linearization still holds
Bound(a_diff.x, -6.0, 6.0);
Bound(a_diff.y, -6.0, 6.0);
Bound(a_diff.z, -9.0, 9.0);
- //If the thrust to specific force ratio has been defined, include vertical control
- //else ignore the vertical acceleration error
+ // If the thrust to specific force ratio has been defined, include vertical control
+ // else ignore the vertical acceleration error
#ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
#ifndef STABILIZATION_ATTITUDE_INDI_FULL
a_diff.z = 0.0;
@@ -442,15 +304,20 @@ void guidance_indi_run(float *heading_sp) {
//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
+ //printf("abi thrust %f\n", euler_cmd.z);
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
- //Coordinated turn
- //feedforward estimate angular rotation omega = g*tan(phi)/v
+ // Coordinated turn
+ // feedforward estimate angular rotation omega = g*tan(phi)/v
float omega;
- const float max_phi = RadOfDeg(60.0);
- float airspeed_turn = airspeed;
- //We are dividing by the airspeed, so a lower bound is important
- Bound(airspeed_turn,10.0,30.0);
+ const float max_phi = RadOfDeg(60.0f);
+#if GUIDANCE_INDI_ZERO_AIRSPEED
+ float airspeed_turn = 0.f;
+#else
+ float airspeed_turn = stateGetAirspeed_f();
+#endif
+ // We are dividing by the airspeed, so a lower bound is important
+ Bound(airspeed_turn, 10.0f, 30.0f);
guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y;
@@ -462,14 +329,14 @@ void guidance_indi_run(float *heading_sp) {
// Use the current roll angle to determine the corresponding heading rate of change.
float coordinated_turn_roll = eulers_zxy.phi;
- if( (guidance_euler_cmd.theta > 0.0) && ( fabs(guidance_euler_cmd.phi) < guidance_euler_cmd.theta)) {
- coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0) - (guidance_euler_cmd.phi < 0.0))*guidance_euler_cmd.theta;
+ if( (guidance_euler_cmd.theta > 0.0f) && ( fabs(guidance_euler_cmd.phi) < guidance_euler_cmd.theta)) {
+ coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0f) - (guidance_euler_cmd.phi < 0.0f)) * guidance_euler_cmd.theta;
}
- if (fabs(coordinated_turn_roll) < max_phi) {
- omega = 9.81 / airspeed_turn * tanf(coordinated_turn_roll);
+ if (fabsf(coordinated_turn_roll) < max_phi) {
+ omega = 9.81f / airspeed_turn * tanf(coordinated_turn_roll);
} else { //max 60 degrees roll
- omega = 9.81 / airspeed_turn * 1.72305 * ((coordinated_turn_roll > 0.0) - (coordinated_turn_roll < 0.0));
+ omega = 9.81f / airspeed_turn * 1.72305f * ((coordinated_turn_roll > 0.0f) - (coordinated_turn_roll < 0.0f));
}
#ifdef FWD_SIDESLIP_GAIN
@@ -477,47 +344,46 @@ void guidance_indi_run(float *heading_sp) {
omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN;
#endif
-// For a hybrid it is important to reduce the sideslip, which is done by changing the heading.
-// For experiments, it is possible to fix the heading to a different value.
-#ifndef KNIFE_EDGE_TEST
- if(take_heading_control) {
- *heading_sp = ANGLE_FLOAT_OF_BFP(nav_heading);
- } else {
- *heading_sp += omega / PERIODIC_FREQUENCY;
- FLOAT_ANGLE_NORMALIZE(*heading_sp);
- // limit heading setpoint to be within bounds of current heading
- #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
- float delta_limit = STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
- float heading = stabilization_attitude_get_heading_f();
-
- float delta_psi = *heading_sp - heading;
- FLOAT_ANGLE_NORMALIZE(delta_psi);
- if (delta_psi > delta_limit) {
- *heading_sp = heading + delta_limit;
- } else if (delta_psi < -delta_limit) {
- *heading_sp = heading - delta_limit;
- }
- FLOAT_ANGLE_NORMALIZE(*heading_sp);
- #endif
+ // For a hybrid it is important to reduce the sideslip, which is done by changing the heading.
+ // For experiments, it is possible to fix the heading to a different value.
+ if (take_heading_control) {
+ // heading is fixed by nav
+ guidance_euler_cmd.psi = heading_sp;
}
+ else {
+ // heading is free and controlled by guidance
+ guidance_indi_hybrid_heading_sp += omega / PERIODIC_FREQUENCY;
+ FLOAT_ANGLE_NORMALIZE(guidance_indi_hybrid_heading_sp);
+ // limit heading setpoint to be within bounds of current heading
+#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
+ float delta_limit = STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
+ float heading = stabilization_attitude_get_heading_f();
+ float delta_psi = guidance_indi_hybrid_heading_sp - heading;
+ FLOAT_ANGLE_NORMALIZE(delta_psi);
+ if (delta_psi > delta_limit) {
+ guidance_indi_hybrid_heading_sp = heading + delta_limit;
+ } else if (delta_psi < -delta_limit) {
+ guidance_indi_hybrid_heading_sp = heading - delta_limit;
+ }
+ FLOAT_ANGLE_NORMALIZE(guidance_indi_hybrid_heading_sp);
#endif
-
- guidance_euler_cmd.psi = *heading_sp;
+ guidance_euler_cmd.psi = guidance_indi_hybrid_heading_sp;
+ }
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
guidance_indi_filter_thrust();
- //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
- thrust_in = thrust_filt.o[0] + euler_cmd.z*guidance_indi_specific_force_gain;
+ // Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
+ thrust_in = thrust_filt.o[0] + euler_cmd.z * guidance_indi_specific_force_gain;
Bound(thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
#if GUIDANCE_INDI_RC_DEBUG
- if(radio_control.values[RADIO_THROTTLE]<300) {
+ if (radio_control.values[RADIO_THROTTLE] < 300) {
thrust_in = 0;
}
#endif
- //Overwrite the thrust command from guidance_v
+ // Overwrite the thrust command from guidance_v
stabilization_cmd[COMMAND_THRUST] = thrust_in;
#endif
@@ -525,7 +391,198 @@ void guidance_indi_run(float *heading_sp) {
struct FloatQuat sp_quat;
float_quat_of_eulers_zxy(&sp_quat, &guidance_euler_cmd);
float_quat_normalize(&sp_quat);
- QUAT_BFP_OF_REAL(stab_att_sp_quat,sp_quat);
+
+ return stab_sp_from_quat_f(&sp_quat);
+}
+
+// compute accel setpoint from speed setpoint (use global variables ! FIXME)
+static struct FloatVect3 compute_accel_from_speed_sp(void)
+{
+ struct FloatVect3 accel_sp = { 0.f, 0.f, 0.f };
+
+ float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+
+ //for rc control horizontal, rotate from body axes to NED
+ float psi = eulers_zxy.psi;
+ float cpsi = cosf(psi);
+ float spsi = sinf(psi);
+ float speed_sp_b_x = cpsi * gi_speed_sp.x + spsi * gi_speed_sp.y;
+ float speed_sp_b_y = -spsi * gi_speed_sp.x + cpsi * gi_speed_sp.y;
+
+ // Get airspeed or zero it
+#if GUIDANCE_INDI_ZERO_AIRSPEED
+ float airspeed = 0.f;
+#else
+ float airspeed = stateGetAirspeed_f();
+#endif
+ struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
+ struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed };
+ struct FloatVect2 windspeed;
+ VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
+
+ VECT2_DIFF(desired_airspeed, gi_speed_sp, windspeed); // Use 2d part of gi_speed_sp
+ float norm_des_as = FLOAT_VECT2_NORM(desired_airspeed);
+
+ // Make turn instead of straight line
+ if ((airspeed > TURN_AIRSPEED_TH) && (norm_des_as > (TURN_AIRSPEED_TH+2.0f))) {
+
+ // Give the wind cancellation priority.
+ if (norm_des_as > guidance_indi_max_airspeed) {
+ float groundspeed_factor = 0.0f;
+
+ // if the wind is faster than we can fly, just fly in the wind direction
+ if (FLOAT_VECT2_NORM(windspeed) < guidance_indi_max_airspeed) {
+ float av = gi_speed_sp.x * gi_speed_sp.x + gi_speed_sp.y * gi_speed_sp.y;
+ float bv = -2.f * (windspeed.x * gi_speed_sp.x + windspeed.y * gi_speed_sp.y);
+ float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - guidance_indi_max_airspeed * guidance_indi_max_airspeed;
+
+ float dv = bv * bv - 4.0f * av * cv;
+
+ // dv can only be positive, but just in case
+ if (dv < 0.0f) {
+ dv = fabsf(dv);
+ }
+ float d_sqrt = sqrtf(dv);
+
+ groundspeed_factor = (-bv + d_sqrt) / (2.0f * av);
+ }
+
+ desired_airspeed.x = groundspeed_factor * gi_speed_sp.x - windspeed.x;
+ desired_airspeed.y = groundspeed_factor * gi_speed_sp.y - windspeed.y;
+
+ speed_sp_b_x = guidance_indi_max_airspeed;
+ }
+
+ // desired airspeed can not be larger than max airspeed
+ speed_sp_b_x = Min(norm_des_as, guidance_indi_max_airspeed);
+
+ if (force_forward) {
+ speed_sp_b_x = guidance_indi_max_airspeed;
+ }
+
+ // Calculate accel sp in body axes, because we need to regulate airspeed
+ struct FloatVect2 sp_accel_b;
+ // In turn acceleration proportional to heading diff
+ sp_accel_b.y = atan2f(desired_airspeed.y, desired_airspeed.x) - psi;
+ FLOAT_ANGLE_NORMALIZE(sp_accel_b.y);
+ sp_accel_b.y *= gih_params.heading_bank_gain;
+
+ // Control the airspeed
+ sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain;
+
+ accel_sp.x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y;
+ accel_sp.y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y;
+ accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
+ }
+ else { // Go somewhere in the shortest way
+
+ if (airspeed > 10.f) {
+ // Groundspeed vector in body frame
+ float groundspeed_x = cpsi * stateGetSpeedNed_f()->x + spsi * stateGetSpeedNed_f()->y;
+ float speed_increment = speed_sp_b_x - groundspeed_x;
+
+ // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
+ if ((speed_increment + airspeed) > guidance_indi_max_airspeed) {
+ speed_sp_b_x = guidance_indi_max_airspeed + groundspeed_x - airspeed;
+ }
+ }
+
+ gi_speed_sp.x = cpsi * speed_sp_b_x - spsi * speed_sp_b_y;
+ gi_speed_sp.y = spsi * speed_sp_b_x + cpsi * speed_sp_b_y;
+
+ accel_sp.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
+ accel_sp.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
+ accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
+ }
+
+ // Bound the acceleration setpoint
+ float accelbound = 3.0f + airspeed / guidance_indi_max_airspeed * 5.0f; // FIXME remove hard coded values
+ float_vect3_bound_in_2d(&accel_sp, accelbound);
+ /*BoundAbs(sp_accel.x, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
+ /*BoundAbs(sp_accel.y, 3.0 + airspeed/guidance_indi_max_airspeed*6.0);*/
+ BoundAbs(accel_sp.z, 3.0);
+
+ //printf("accel_sp %f %f %f\n", accel_sp.x, accel_sp.y, accel_sp.z);
+ return accel_sp;
+}
+
+struct StabilizationSetpoint guidance_indi_run_pos(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv)
+{
+ struct FloatVect3 pos_err;
+ struct FloatVect3 accel_sp;
+
+ //Linear controller to find the acceleration setpoint from position and velocity
+ pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x;
+ pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y;
+ pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
+
+ // First check for velocity setpoint from module // FIXME should be called like this
+ float dt = get_sys_time_float() - time_of_vel_sp;
+ // If the input command is not updated after a timeout, switch back to flight plan control
+ if (dt < 0.5) {
+ gi_speed_sp.x = indi_vel_sp.x;
+ gi_speed_sp.y = indi_vel_sp.y;
+ gi_speed_sp.z = indi_vel_sp.z;
+ } else {
+ gi_speed_sp.x = pos_err.x * gih_params.pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
+ gi_speed_sp.y = pos_err.y * gih_params.pos_gain + SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+ gi_speed_sp.z = pos_err.z * gih_params.pos_gainz + SPEED_FLOAT_OF_BFP(gv->zd_ref);
+ }
+
+ // Bound vertical speed setpoint
+ if (stateGetAirspeed_f() > 13.f) {
+ Bound(gi_speed_sp.z, -4.0f, 4.0f); // FIXME no harcoded values
+ } else {
+ Bound(gi_speed_sp.z, nav.descend_vspeed, nav.climb_vspeed); // FIXME don't use nav settings
+ }
+
+ accel_sp = compute_accel_from_speed_sp(); // compute accel sp
+
+ return guidance_indi_run(&accel_sp, gh->sp.heading);
+}
+
+struct StabilizationSetpoint guidance_indi_run_speed(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv)
+{
+ struct FloatVect3 accel_sp;
+
+ // First check for velocity setpoint from module // FIXME should be called like this
+ float dt = get_sys_time_float() - time_of_vel_sp;
+ // If the input command is not updated after a timeout, switch back to flight plan control
+ if (dt < 0.5) {
+ gi_speed_sp.x = indi_vel_sp.x;
+ gi_speed_sp.y = indi_vel_sp.y;
+ gi_speed_sp.z = indi_vel_sp.z;
+ } else {
+ gi_speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
+ gi_speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
+ gi_speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
+ }
+
+ // Bound vertical speed setpoint
+ if (stateGetAirspeed_f() > 13.f) {
+ Bound(gi_speed_sp.z, -4.0f, 4.0f); // FIXME no harcoded values
+ } else {
+ Bound(gi_speed_sp.z, nav.descend_vspeed, nav.climb_vspeed); // FIXME don't use nav settings
+ }
+
+ accel_sp = compute_accel_from_speed_sp(); // compute accel sp
+
+ return guidance_indi_run(&accel_sp, gh->sp.heading);
+}
+
+struct StabilizationSetpoint guidance_indi_run_accel(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv)
+{
+ struct FloatVect3 accel_sp;
+
+ gi_speed_sp.x = 0.f;
+ gi_speed_sp.y = 0.f;
+ gi_speed_sp.z = 0.f;
+
+ accel_sp.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
+ accel_sp.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
+ accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref);
+
+ return guidance_indi_run(&accel_sp, gh->sp.heading);
}
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
@@ -640,169 +697,6 @@ float guidance_indi_get_liftd(float airspeed, float theta) {
return liftd;
}
-/**
- * @brief function that returns a speed setpoint based on flight plan.
- *
- * The routines are meant for a hybrid UAV and assume measurement of airspeed.
- * Makes the vehicle track a vector field with a sink at a waypoint.
- * Use force_forward to maintain airspeed and fly 'through' waypoints.
- *
- * @return desired speed setpoint FloatVect3
- */
-struct FloatVect3 nav_get_speed_setpoint(float pos_gain) {
- struct FloatVect3 speed_sp;
- if(horizontal_mode == HORIZONTAL_MODE_ROUTE) {
- speed_sp = nav_get_speed_sp_from_line(line_vect, to_end_vect, navigation_target, pos_gain);
- } else {
- speed_sp = nav_get_speed_sp_from_go(navigation_target, pos_gain);
- }
- return speed_sp;
-}
-
-/**
- * @brief follow a line.
- *
- * @param line_v_enu 2d vector from beginning (0) line to end in enu
- * @param to_end_v_enu 2d vector from current position to end in enu
- * @param target end waypoint in enu
- *
- * @return desired speed setpoint FloatVect3
- */
-struct FloatVect3 nav_get_speed_sp_from_line(struct FloatVect2 line_v_enu, struct FloatVect2 to_end_v_enu, struct EnuCoor_i target, float pos_gain) {
-
- // enu -> ned
- struct FloatVect2 line_v = {line_v_enu.y, line_v_enu.x};
- struct FloatVect2 to_end_v = {to_end_v_enu.y, to_end_v_enu.x};
-
- struct NedCoor_f ned_target;
- // Target in NED instead of ENU
- VECT3_ASSIGN(ned_target, POS_FLOAT_OF_BFP(target.y), POS_FLOAT_OF_BFP(target.x), -POS_FLOAT_OF_BFP(target.z));
-
- // Calculate magnitude of the desired speed vector based on distance to waypoint
- float dist_to_target = float_vect2_norm(&to_end_v);
- float desired_speed;
- if(force_forward) {
- desired_speed = nav_max_speed;
- } else {
- desired_speed = dist_to_target * pos_gain;
- Bound(desired_speed, 0.0, nav_max_speed);
- }
-
- // Calculate length of line segment
- float length_line = float_vect2_norm(&line_v);
- if(length_line < 0.01) {
- length_line = 0.01;
- }
-
- //Normal vector to the line, with length of the line
- struct FloatVect2 normalv;
- VECT2_ASSIGN(normalv, -line_v.y, line_v.x);
- // Length of normal vector is the same as of the line segment
- float length_normalv = length_line;
- if(length_normalv < 0.01) {
- length_normalv = 0.01;
- }
-
- // Distance along the normal vector
- float dist_to_line = (to_end_v.x*normalv.x + to_end_v.y*normalv.y)/length_normalv;
-
- // Normal vector scaled to be the distance to the line
- struct FloatVect2 v_to_line, v_along_line;
- v_to_line.x = dist_to_line*normalv.x/length_normalv*guidance_indi_line_gain;
- v_to_line.y = dist_to_line*normalv.y/length_normalv*guidance_indi_line_gain;
-
- // Depending on the normal vector, the distance could be negative
- float dist_to_line_abs = fabs(dist_to_line);
-
- // The distance that needs to be traveled along the line
- /*float dist_along_line = (line_v.x*to_end_v.x + line_v.y*to_end_v.y)/length_line;*/
- v_along_line.x = line_v.x/length_line*50.0;
- v_along_line.y = line_v.y/length_line*50.0;
-
- // Calculate the desired direction to converge to the line
- struct FloatVect2 direction;
- VECT2_SMUL(direction, v_along_line, (1.0/(1+dist_to_line_abs*0.05)));
- VECT2_ADD(direction, v_to_line);
- float length_direction = float_vect2_norm(&direction);
- if(length_direction < 0.01) {
- length_direction = 0.01;
- }
-
- // Scale to have the desired speed
- struct FloatVect2 final_vector;
- VECT2_SMUL(final_vector, direction, desired_speed/length_direction);
-
- struct FloatVect3 speed_sp_return = {final_vector.x, final_vector.y, gih_params.pos_gainz*(ned_target.z - stateGetPositionNed_f()->z)};
- if((guidance_v_mode == GUIDANCE_V_MODE_NAV) && (vertical_mode == VERTICAL_MODE_CLIMB)) {
- speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
- }
-
- // Bound vertical speed setpoint
- if(stateGetAirspeed_f() > 13.0) {
- Bound(speed_sp_return.z, -4.0, 5.0);
- } else {
- Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
- }
-
- return speed_sp_return;
-}
-
-/**
- * @brief Go to a waypoint in the shortest way
- *
- * @param target the target waypoint
- *
- * @return desired speed FloatVect3
- */
-struct FloatVect3 nav_get_speed_sp_from_go(struct EnuCoor_i target, float pos_gain) {
- // The speed sp that will be returned
- struct FloatVect3 speed_sp_return;
- struct NedCoor_f ned_target;
- // Target in NED instead of ENU
- VECT3_ASSIGN(ned_target, POS_FLOAT_OF_BFP(target.y), POS_FLOAT_OF_BFP(target.x), POS_FLOAT_OF_BFP(-nav_flight_altitude));
-
- // Calculate position error
- struct FloatVect3 pos_error;
- struct NedCoor_f *pos = stateGetPositionNed_f();
- VECT3_DIFF(pos_error, ned_target, *pos);
-
- VECT3_SMUL(speed_sp_return, pos_error, pos_gain);
- speed_sp_return.z = gih_params.pos_gainz*pos_error.z;
-
- if((guidance_v_mode == GUIDANCE_V_MODE_NAV) && (vertical_mode == VERTICAL_MODE_CLIMB)) {
- speed_sp_return.z = SPEED_FLOAT_OF_BFP(guidance_v_zd_sp);
- }
-
- if(force_forward) {
- vect_scale(&speed_sp_return, nav_max_speed);
- } else {
- // Calculate distance to waypoint
- float dist_to_wp = FLOAT_VECT2_NORM(pos_error);
-
- // Calculate max speed to decelerate from
-
- // dist_to_wp can only be positive, but just in case
- float max_speed_decel2 = 2*dist_to_wp*MAX_DECELERATION;
- if(max_speed_decel2 < 0.0) {
- max_speed_decel2 = fabs(max_speed_decel2);
- }
- float max_speed_decel = sqrtf(max_speed_decel2);
-
- // Bound the setpoint velocity vector
- float max_h_speed = Min(nav_max_speed, max_speed_decel);
- vect_bound_in_2d(&speed_sp_return, max_h_speed);
- }
-
- // Bound vertical speed setpoint
- if(stateGetAirspeed_f() > 13.0) {
- Bound(speed_sp_return.z, -4.0, 5.0);
- } else {
- Bound(speed_sp_return.z, -nav_climb_vspeed, -nav_descend_vspeed);
- }
-
- return speed_sp_return;
-}
-
/**
* ABI callback that obtains the velocity setpoint from a module
*/
@@ -813,3 +707,50 @@ static void vel_sp_cb(uint8_t sender_id __attribute__((unused)), struct FloatVec
indi_vel_sp.z = vel_sp->z;
time_of_vel_sp = get_sys_time_float();
}
+
+
+#if GUIDANCE_INDI_HYBRID_USE_AS_DEFAULT
+// guidance indi control function is implementing the default functions of guidance
+
+void guidance_h_run_enter(void)
+{
+ guidance_indi_enter();
+}
+
+void guidance_v_run_enter(void)
+{
+ // nothing to do
+}
+
+struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_indi_run_pos(in_flight, gh, &guidance_v);
+}
+
+struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_indi_run_speed(in_flight, gh, &guidance_v);
+}
+
+struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_indi_run_accel(in_flight, gh, &guidance_v);
+}
+
+int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ return (int32_t)thrust_in; // nothing to do
+}
+
+int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ return (int32_t)thrust_in; // nothing to do
+}
+
+int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ return (int32_t)thrust_in; // nothing to do
+}
+
+#endif
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
index a25e64021b..6d66b436f0 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
@@ -34,11 +34,18 @@
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "filters/high_pass_filter.h"
+#include "firmwares/rotorcraft/guidance.h"
+#include "firmwares/rotorcraft/stabilization.h"
+
+// TODO change names for _indi_hybrid_
-extern void guidance_indi_enter(void);
-extern void guidance_indi_run(float *heading_sp);
-extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading);
extern void guidance_indi_init(void);
+extern void guidance_indi_enter(void);
+
+extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
+extern struct StabilizationSetpoint guidance_indi_run_pos(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv);
+extern struct StabilizationSetpoint guidance_indi_run_speed(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv);
+extern struct StabilizationSetpoint guidance_indi_run_accel(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv);
struct guidance_indi_hybrid_params {
float pos_gain;
@@ -57,8 +64,9 @@ extern struct FloatVect3 gi_speed_sp;
extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed;
-extern float nav_max_speed;
extern bool take_heading_control;
extern float guidance_indi_max_bank;
+extern bool force_forward; ///< forward flight for hybrid nav
+
#endif /* GUIDANCE_INDI_HYBRID_H */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c
new file mode 100644
index 0000000000..7214e0648a
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c
@@ -0,0 +1,466 @@
+/*
+ * Copyright (C) 2023 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/** @file firmwares/rotorcraft/guidance/guidance_pid.c
+ * Guidance controller with PID for rotorcrafts.
+ *
+ */
+
+#include "firmwares/rotorcraft/guidance/guidance_pid.h"
+#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
+#include "firmwares/rotorcraft/stabilization.h"
+#include "generated/airframe.h"
+#include "state.h"
+
+
+// Keep GUIDANCE_H_XXX format for backward compatibility
+
+#ifndef GUIDANCE_H_AGAIN
+#define GUIDANCE_H_AGAIN 0
+#endif
+
+#ifndef GUIDANCE_H_VGAIN
+#define GUIDANCE_H_VGAIN 0
+#endif
+
+/* error if some gains are negative */
+#if (GUIDANCE_H_PGAIN < 0) || \
+ (GUIDANCE_H_DGAIN < 0) || \
+ (GUIDANCE_H_IGAIN < 0) || \
+ (GUIDANCE_H_AGAIN < 0) || \
+ (GUIDANCE_H_VGAIN < 0)
+#error "ALL PID_H control gains have to be positive!!!"
+#endif
+
+#ifndef GUIDANCE_H_MAX_BANK
+#define GUIDANCE_H_MAX_BANK RadOfDeg(20)
+#endif
+
+#ifndef GUIDANCE_H_THRUST_CMD_FILTER
+#define GUIDANCE_H_THRUST_CMD_FILTER 10
+#endif
+
+#ifndef GUIDANCE_H_APPROX_FORCE_BY_THRUST
+#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE
+#endif
+
+// TODO configurable
+#define MAX_POS_ERR POS_BFP_OF_REAL(16.)
+#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)
+
+// Keep GUIDANCE_V_XXX format for backward compatibility
+
+/* error if some gains are negative */
+#if (GUIDANCE_V_HOVER_KP < 0) || \
+ (GUIDANCE_V_HOVER_KD < 0) || \
+ (GUIDANCE_V_HOVER_KI < 0)
+#error "ALL PID_V control gains must be positive!!!"
+#endif
+
+/* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined,
+ * disable the adaptive throttle estimation by default.
+ * Otherwise enable adaptive estimation by default.
+ */
+#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
+# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
+# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE
+# endif
+#else
+# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
+# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE
+# endif
+#endif
+PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED)
+
+#ifndef GUIDANCE_V_MIN_ERR_Z
+#define GUIDANCE_V_MIN_ERR_Z POS_BFP_OF_REAL(-10.)
+#endif
+
+#ifndef GUIDANCE_V_MAX_ERR_Z
+#define GUIDANCE_V_MAX_ERR_Z POS_BFP_OF_REAL(10.)
+#endif
+
+#ifndef GUIDANCE_V_MIN_ERR_ZD
+#define GUIDANCE_V_MIN_ERR_ZD SPEED_BFP_OF_REAL(-10.)
+#endif
+
+#ifndef GUIDANCE_V_MAX_ERR_ZD
+#define GUIDANCE_V_MAX_ERR_ZD SPEED_BFP_OF_REAL(10.)
+#endif
+
+#ifndef GUIDANCE_V_MAX_SUM_ERR
+#define GUIDANCE_V_MAX_SUM_ERR 2000000
+#endif
+
+#ifndef GUIDANCE_V_MAX_CMD
+#define GUIDANCE_V_MAX_CMD 0.9*MAX_PPRZ
+#endif
+
+
+
+/** The PID controller is used by default
+ */
+#ifndef GUIDANCE_PID_USE_AS_DEFAULT
+#define GUIDANCE_PID_USE_AS_DEFAULT TRUE
+#endif
+
+/*
+ * external variables
+ */
+
+struct GuidancePID guidance_pid;
+
+/*
+ * internal variables
+ */
+
+struct Int32Vect2 guidance_pid_pos_err;
+struct Int32Vect2 guidance_pid_speed_err;
+struct Int32Vect2 guidance_pid_trim_att_integrator;
+
+int32_t guidance_pid_z_sum_err; ///< accumulator for I-gain
+int32_t guidance_pid_v_ff_cmd; ///< feed-forward command
+int32_t guidance_pid_v_fb_cmd; ///< feed-back command
+
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void send_hover_loop(struct transport_tx *trans, struct link_device *dev)
+{
+ struct NedCoor_i *pos = stateGetPositionNed_i();
+ struct NedCoor_i *speed = stateGetSpeedNed_i();
+ struct NedCoor_i *accel = stateGetAccelNed_i();
+ pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID,
+ &guidance_h.sp.pos.x,
+ &guidance_h.sp.pos.y,
+ &(pos->x), &(pos->y),
+ &(speed->x), &(speed->y),
+ &(accel->x), &(accel->y),
+ &guidance_pid_pos_err.x,
+ &guidance_pid_pos_err.y,
+ &guidance_pid_speed_err.x,
+ &guidance_pid_speed_err.y,
+ &guidance_pid_trim_att_integrator.x,
+ &guidance_pid_trim_att_integrator.y,
+ &guidance_pid.cmd_earth.x,
+ &guidance_pid.cmd_earth.y,
+ &guidance_h.sp.heading);
+}
+
+static void send_vert_loop(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_VERT_LOOP(trans, dev, AC_ID,
+ &guidance_v.z_sp, &guidance_v.zd_sp,
+ &(stateGetPositionNed_i()->z),
+ &(stateGetSpeedNed_i()->z),
+ &(stateGetAccelNed_i()->z),
+ &guidance_v.z_ref, &guidance_v.zd_ref,
+ &guidance_v.zdd_ref,
+ &gv_adapt_X,
+ &gv_adapt_P,
+ &gv_adapt_Xmeas,
+ &guidance_pid_z_sum_err,
+ &guidance_pid_v_ff_cmd,
+ &guidance_pid_v_fb_cmd,
+ &guidance_pid.cmd_thrust);
+}
+
+#endif
+
+void guidance_pid_init(void)
+{
+ INT_VECT2_ZERO(guidance_pid.cmd_earth);
+ INT_VECT2_ZERO(guidance_pid_pos_err);
+ INT_VECT2_ZERO(guidance_pid_speed_err);
+ INT_VECT2_ZERO(guidance_pid_trim_att_integrator);
+ guidance_pid.cmd_thrust = 0;
+ guidance_pid_z_sum_err = 0;
+ guidance_pid_v_ff_cmd = 0;
+ guidance_pid_v_fb_cmd = 0;
+ guidance_pid.kp = GUIDANCE_H_PGAIN;
+ guidance_pid.ki = GUIDANCE_H_IGAIN;
+ guidance_pid.kd = GUIDANCE_H_DGAIN;
+ guidance_pid.ka = GUIDANCE_H_AGAIN;
+ guidance_pid.kv = GUIDANCE_H_VGAIN;
+ guidance_pid.v_kp = GUIDANCE_V_HOVER_KP;
+ guidance_pid.v_kd = GUIDANCE_V_HOVER_KD;
+ guidance_pid.v_ki = GUIDANCE_V_HOVER_KI;
+ guidance_pid.approx_force_by_thrust = GUIDANCE_H_APPROX_FORCE_BY_THRUST;
+ guidance_pid.adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HOVER_LOOP, send_hover_loop);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VERT_LOOP, send_vert_loop);
+#endif
+}
+
+/* with a pgain of 100 and a scale of 2,
+ * you get an angle of 5.6 degrees for 1m pos error */
+#define GH_GAIN_SCALE 2
+
+/**
+ * run horizontal control loop for position and speed control
+ */
+static struct StabilizationSetpoint guidance_pid_h_run(bool in_flight, struct HorizontalGuidance *gh)
+{
+ /* maximum bank angle: default 20 deg, max 40 deg*/
+ static const int32_t traj_max_bank = Min(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC),
+ BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC));
+ static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC);
+
+ /* run PID */
+ int32_t pd_x =
+ ((guidance_pid.kp * guidance_pid_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+ ((guidance_pid.kd * (guidance_pid_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
+ int32_t pd_y =
+ ((guidance_pid.kp * guidance_pid_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+ ((guidance_pid.kd * (guidance_pid_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
+ guidance_pid.cmd_earth.x = pd_x +
+ ((guidance_pid.kv * gh->ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
+ ((guidance_pid.ka * gh->ref.accel.x) >> (INT32_ACCEL_FRAC - GH_GAIN_SCALE)); /* acceleration feedforward gain */
+ guidance_pid.cmd_earth.y = pd_y +
+ ((guidance_pid.kv * gh->ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
+ ((guidance_pid.ka * gh->ref.accel.y) >> (INT32_ACCEL_FRAC - GH_GAIN_SCALE)); /* acceleration feedforward gain */
+
+ /* trim max bank angle from PD */
+ VECT2_STRIM(guidance_pid.cmd_earth, -traj_max_bank, traj_max_bank);
+
+ /* Update pos & speed error integral, zero it if not in_flight.
+ * Integrate twice as fast when not only POS but also SPEED are wrong,
+ * but do not integrate POS errors when the SPEED is already catching up.
+ */
+ if (in_flight) {
+ /* ANGLE_FRAC (12) * GAIN (8) * LOOP_FREQ (9) -> INTEGRATOR HIGH RES ANGLE_FRAX (28) */
+ guidance_pid_trim_att_integrator.x += (guidance_pid.ki * pd_x);
+ guidance_pid_trim_att_integrator.y += (guidance_pid.ki * pd_y);
+ /* saturate it */
+ VECT2_STRIM(guidance_pid_trim_att_integrator, -(traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)),
+ (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2)));
+ /* add it to the command */
+ guidance_pid.cmd_earth.x += (guidance_pid_trim_att_integrator.x >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2));
+ guidance_pid.cmd_earth.y += (guidance_pid_trim_att_integrator.y >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE * 2));
+ } else {
+ INT_VECT2_ZERO(guidance_pid_trim_att_integrator);
+ }
+
+ /* compute a better approximation of force commands by taking thrust into account */
+ if (guidance_pid.approx_force_by_thrust && in_flight) {
+ static int32_t thrust_cmd_filt;
+ // FIXME strong coupling with guidance_v here !!!
+ int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
+ thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
+ (GUIDANCE_H_THRUST_CMD_FILTER + 1);
+ guidance_pid.cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_pid.cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2),
+ thrust_cmd_filt));
+ guidance_pid.cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_pid.cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2),
+ thrust_cmd_filt));
+ }
+
+ VECT2_STRIM(guidance_pid.cmd_earth, -total_max_bank, total_max_bank);
+
+ return stab_sp_from_ltp_i(&guidance_pid.cmd_earth, ANGLE_BFP_OF_REAL(guidance_h.sp.heading));
+}
+
+struct StabilizationSetpoint guidance_pid_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
+{
+ /* compute position error */
+ VECT2_DIFF(guidance_pid_pos_err, gh->ref.pos, *stateGetPositionNed_i());
+ /* saturate it */
+ VECT2_STRIM(guidance_pid_pos_err, -MAX_POS_ERR, MAX_POS_ERR);
+
+ /* compute speed error */
+ VECT2_DIFF(guidance_pid_speed_err, gh->ref.speed, *stateGetSpeedNed_i());
+ /* saturate it */
+ VECT2_STRIM(guidance_pid_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+
+ /* run PID */
+ return guidance_pid_h_run(in_flight, gh);
+}
+
+struct StabilizationSetpoint guidance_pid_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
+{
+ /* cancel position error */
+ INT_VECT2_ZERO(guidance_pid_pos_err);
+
+ /* compute speed error */
+ VECT2_DIFF(guidance_pid_speed_err, gh->ref.speed, *stateGetSpeedNed_i());
+ /* saturate it */
+ VECT2_STRIM(guidance_pid_speed_err, -MAX_SPEED_ERR, MAX_SPEED_ERR);
+
+ /* run PID */
+ return guidance_pid_h_run(in_flight, gh);
+}
+
+struct StabilizationSetpoint guidance_pid_h_run_accel(bool in_flight UNUSED, struct HorizontalGuidance *gh UNUSED)
+{
+ struct StabilizationSetpoint sp = { 0 };
+ // TODO
+ return sp;
+}
+
+/**
+ * run vertical control loop for position and speed control
+ */
+
+#define FF_CMD_FRAC 18
+
+static int32_t guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
+{
+ /* compute the error to our reference */
+ int32_t err_z = gv->z_ref - stateGetPositionNed_i()->z;
+ Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
+ int32_t err_zd = gv->zd_ref - stateGetSpeedNed_i()->z;
+ Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
+
+ if (in_flight) {
+ guidance_pid_z_sum_err += err_z;
+ Bound(guidance_pid_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
+ } else {
+ guidance_pid_z_sum_err = 0;
+ }
+
+ /* our nominal command : (g + zdd)*m */
+ int32_t inv_m;
+ if (guidance_pid.adapt_throttle_enabled) {
+ inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
+ } else {
+ /* use the fixed nominal throttle */
+ inv_m = BFP_OF_REAL(9.81 / (gv->nominal_throttle * MAX_PPRZ), FF_CMD_FRAC);
+ }
+
+ const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
+ (gv->zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC));
+
+ guidance_pid_v_ff_cmd = g_m_zdd / inv_m;
+ /* feed forward command */
+ guidance_pid_v_ff_cmd = (guidance_pid_v_ff_cmd << INT32_TRIG_FRAC) / gv->thrust_coeff;
+
+ /* bound the nominal command to GUIDANCE_V_MAX_CMD */
+ Bound(guidance_pid_v_ff_cmd, 0, GUIDANCE_V_MAX_CMD);
+
+
+ /* our error feed back command */
+ /* z-axis pointing down -> positive error means we need less thrust */
+ guidance_pid_v_fb_cmd =
+ ((-guidance_pid.v_kp * err_z) >> 7) +
+ ((-guidance_pid.v_kd * err_zd) >> 16) +
+ ((-guidance_pid.v_ki * guidance_pid_z_sum_err) >> 16);
+
+ guidance_pid.cmd_thrust = guidance_pid_v_ff_cmd + guidance_pid_v_fb_cmd;
+
+ /* bound the result */
+ Bound(guidance_pid.cmd_thrust, 0, MAX_PPRZ);
+
+ return guidance_pid.cmd_thrust;
+}
+
+int32_t guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_pid_v_run(in_flight, gv);
+}
+
+int32_t guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_pid_v_run(in_flight, gv);
+}
+
+int32_t guidance_pid_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
+{
+ // TODO
+ return 0;
+}
+
+void guidance_pid_h_enter(void)
+{
+}
+
+void guidance_pid_v_enter(void)
+{
+ guidance_pid_z_sum_err = 0;
+}
+
+/**
+ * settings handler
+ */
+
+void guidance_pid_set_h_igain(uint32_t igain)
+{
+ guidance_pid.ki = igain;
+ INT_VECT2_ZERO(guidance_pid_trim_att_integrator);
+}
+
+void guidance_pid_set_v_igain(uint32_t igain)
+{
+ guidance_pid.v_ki = igain;
+ guidance_pid_z_sum_err = 0;
+}
+
+
+const struct Int32Vect2 *guidance_pid_get_h_pos_err(void)
+{
+ return &guidance_pid_pos_err;
+}
+
+#if GUIDANCE_PID_USE_AS_DEFAULT
+// guidance pid control function is implementing the default functions of guidance
+
+void guidance_h_run_enter(void)
+{
+ guidance_pid_h_enter();
+}
+
+void guidance_v_run_enter(void)
+{
+ guidance_pid_v_enter();
+}
+
+struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_pid_h_run_pos(in_flight, gh);
+}
+
+struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_pid_h_run_speed(in_flight, gh);
+}
+
+struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh)
+{
+ return guidance_pid_h_run_accel(in_flight, gh);
+}
+
+int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_pid_v_run_pos(in_flight, gv);
+}
+
+int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_pid_v_run_speed(in_flight, gv);
+}
+
+int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
+{
+ return guidance_pid_v_run_accel(in_flight, gv);
+}
+
+#endif
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h
new file mode 100644
index 0000000000..5b4bfd1967
--- /dev/null
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2023 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/** @file firmwares/rotorcraft/guidance/guidance_pid.h
+ * Guidance controller with PID for rotorcrafts.
+ *
+ */
+
+#ifndef GUIDANCE_PID_H
+#define GUIDANCE_PID_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "std.h"
+#include "math/pprz_algebra_int.h"
+#include "firmwares/rotorcraft/guidance/guidance_h.h"
+#include "firmwares/rotorcraft/guidance/guidance_v.h"
+
+struct GuidancePID {
+ // horizontal gains
+ int32_t kp;
+ int32_t kd;
+ int32_t ki;
+ int32_t kv;
+ int32_t ka;
+ // vertical gains
+ int32_t v_kp;
+ int32_t v_kd;
+ int32_t v_ki;
+ // outputs
+ struct Int32Vect2 cmd_earth; // Horizontal guidance command (north/east with #INT32_ANGLE_FRAC)
+ int32_t cmd_thrust; // Vertical guidance command (in pprz_t)
+ // options
+ bool approx_force_by_thrust; // Correction of force commands from thrust
+ bool adapt_throttle_enabled; // Use adaptive throttle command estimation
+};
+
+/** Guidance PID structyre
+ */
+extern struct GuidancePID guidance_pid;
+
+extern void guidance_pid_init(void);
+extern void guidance_pid_h_enter(void);
+extern void guidance_pid_v_enter(void);
+extern struct StabilizationSetpoint guidance_pid_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
+extern struct StabilizationSetpoint guidance_pid_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
+extern struct StabilizationSetpoint guidance_pid_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
+extern int32_t guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
+extern int32_t guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
+extern int32_t guidance_pid_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
+
+extern void guidance_pid_set_h_igain(uint32_t igain);
+extern void guidance_pid_set_v_igain(uint32_t igain);
+
+/** Gets the position error
+ * @param none.
+ * @return Pointer to a structure containing x and y position errors
+ */
+extern const struct Int32Vect2 *guidance_pid_get_h_pos_err(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* GUIDANCE_PID_H */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index eb5cf88cff..5f0d132b95 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -38,30 +38,10 @@
#include "math/pprz_algebra_int.h"
-/* error if some gains are negative */
-#if (GUIDANCE_V_HOVER_KP < 0) || \
- (GUIDANCE_V_HOVER_KD < 0) || \
- (GUIDANCE_V_HOVER_KI < 0)
-#error "ALL control gains must be positive!!!"
-#endif
-
-
-/* If only GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined,
- * disable the adaptive throttle estimation by default.
- * Otherwise enable adaptive estimation by default.
- */
-#ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
-# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
-# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED FALSE
-# endif
-#else
-# define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
-# ifndef GUIDANCE_V_ADAPT_THROTTLE_ENABLED
-# define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE
-# endif
+#ifndef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
+#define GUIDANCE_V_NOMINAL_HOVER_THROTTLE 0.4
#endif
PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE)
-PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED)
#ifndef GUIDANCE_V_CLIMB_RC_DEADBAND
@@ -76,122 +56,38 @@ PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED)
#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD
#endif
-#ifndef GUIDANCE_V_MIN_ERR_Z
-#define GUIDANCE_V_MIN_ERR_Z POS_BFP_OF_REAL(-10.)
-#endif
+struct VerticalGuidance guidance_v;
-#ifndef GUIDANCE_V_MAX_ERR_Z
-#define GUIDANCE_V_MAX_ERR_Z POS_BFP_OF_REAL(10.)
-#endif
-
-#ifndef GUIDANCE_V_MIN_ERR_ZD
-#define GUIDANCE_V_MIN_ERR_ZD SPEED_BFP_OF_REAL(-10.)
-#endif
-
-#ifndef GUIDANCE_V_MAX_ERR_ZD
-#define GUIDANCE_V_MAX_ERR_ZD SPEED_BFP_OF_REAL(10.)
-#endif
-
-#ifndef GUIDANCE_V_MAX_SUM_ERR
-#define GUIDANCE_V_MAX_SUM_ERR 2000000
-#endif
-
-#ifndef GUIDANCE_V_MAX_CMD
-#define GUIDANCE_V_MAX_CMD 0.9*MAX_PPRZ
-#endif
-
-uint8_t guidance_v_mode;
-int32_t guidance_v_ff_cmd;
-int32_t guidance_v_fb_cmd;
-int32_t guidance_v_delta_t;
-
-float guidance_v_nominal_throttle;
-bool guidance_v_adapt_throttle_enabled;
static bool desired_zd_updated;
#define GUIDANCE_V_GUIDED_MODE_ZHOLD 0
#define GUIDANCE_V_GUIDED_MODE_CLIMB 1
#define GUIDANCE_V_GUIDED_MODE_THROTTLE 2
-int guidance_v_guided_mode;
+static int guidance_v_guided_mode;
-/** Direct throttle from radio control.
- * range 0:#MAX_PPRZ
- */
-int32_t guidance_v_rc_delta_t;
-
-/** Vertical speed setpoint from radio control.
- * fixed point representation: Q12.19
- * accuracy 0.0000019, range +/-4096
- */
-int32_t guidance_v_rc_zd_sp;
-
-int32_t guidance_v_z_sp;
-int32_t guidance_v_zd_sp;
-int32_t guidance_v_th_sp;
-int32_t guidance_v_z_ref;
-int32_t guidance_v_zd_ref;
-int32_t guidance_v_zdd_ref;
-
-int32_t guidance_v_kp;
-int32_t guidance_v_kd;
-int32_t guidance_v_ki;
-
-int32_t guidance_v_z_sum_err;
-
-int32_t guidance_v_thrust_coeff;
-
-
-static int32_t get_vertical_thrust_coeff(void);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
-static void send_vert_loop(struct transport_tx *trans, struct link_device *dev)
-{
- pprz_msg_send_VERT_LOOP(trans, dev, AC_ID,
- &guidance_v_z_sp, &guidance_v_zd_sp,
- &(stateGetPositionNed_i()->z),
- &(stateGetSpeedNed_i()->z),
- &(stateGetAccelNed_i()->z),
- &guidance_v_z_ref, &guidance_v_zd_ref,
- &guidance_v_zdd_ref,
- &gv_adapt_X,
- &gv_adapt_P,
- &gv_adapt_Xmeas,
- &guidance_v_z_sum_err,
- &guidance_v_ff_cmd,
- &guidance_v_fb_cmd,
- &guidance_v_delta_t);
-}
-
static void send_tune_vert(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_TUNE_VERT(trans, dev, AC_ID,
- &guidance_v_z_sp,
+ &guidance_v.z_sp,
&(stateGetPositionNed_i()->z),
- &guidance_v_z_ref,
- &guidance_v_zd_ref);
+ &guidance_v.z_ref,
+ &guidance_v.zd_ref);
}
#endif
void guidance_v_init(void)
{
- guidance_v_mode = GUIDANCE_V_MODE_KILL;
-
- guidance_v_kp = GUIDANCE_V_HOVER_KP;
- guidance_v_kd = GUIDANCE_V_HOVER_KD;
- guidance_v_ki = GUIDANCE_V_HOVER_KI;
-
- guidance_v_z_sum_err = 0;
-
- guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
- guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
- desired_zd_updated = false;
+ guidance_v.mode = GUIDANCE_V_MODE_KILL;
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
- guidance_v_thrust_coeff = BFP_OF_REAL(1.f, INT32_TRIG_FRAC);
+ guidance_v.thrust_coeff = BFP_OF_REAL(1.f, INT32_TRIG_FRAC);
+ desired_zd_updated = false;
gv_adapt_init();
@@ -200,7 +96,6 @@ void guidance_v_init(void)
#endif
#if PERIODIC_TELEMETRY
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VERT_LOOP, send_vert_loop);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_TUNE_VERT, send_tune_vert);
#endif
}
@@ -210,28 +105,28 @@ void guidance_v_read_rc(void)
{
/* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
- guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE];
+ guidance_v.rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE];
/* used in RC_CLIMB */
- guidance_v_rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)radio_control.values[RADIO_THROTTLE];
- DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
+ guidance_v.rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)radio_control.values[RADIO_THROTTLE];
+ DeadBand(guidance_v.rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
(MAX_PPRZ / 2 - GUIDANCE_V_CLIMB_RC_DEADBAND));
static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) /
(MAX_PPRZ / 2 - GUIDANCE_V_CLIMB_RC_DEADBAND));
- if (guidance_v_rc_zd_sp > 0) {
- guidance_v_rc_zd_sp *= descent_scale;
+ if (guidance_v.rc_zd_sp > 0) {
+ guidance_v.rc_zd_sp *= descent_scale;
} else {
- guidance_v_rc_zd_sp *= climb_scale;
+ guidance_v.rc_zd_sp *= climb_scale;
}
}
void guidance_v_mode_changed(uint8_t new_mode)
{
- if (new_mode == guidance_v_mode) {
+ if (new_mode == guidance_v.mode) {
return;
}
@@ -243,10 +138,10 @@ void guidance_v_mode_changed(uint8_t new_mode)
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
- guidance_v_zd_sp = 0;
+ guidance_v.zd_sp = 0;
/* Falls through. */
case GUIDANCE_V_MODE_NAV:
- guidance_v_z_sum_err = 0;
+ guidance_v_run_enter();
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
@@ -264,7 +159,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
}
- guidance_v_mode = new_mode;
+ guidance_v.mode = new_mode;
}
@@ -275,9 +170,38 @@ void guidance_v_notify_in_flight(bool in_flight)
}
}
-void guidance_v_thrust_adapt(bool in_flight)
+/// get the cosine of the angle between thrust vector and gravity vector
+static int32_t get_vertical_thrust_coeff(void)
{
- guidance_v_thrust_coeff = get_vertical_thrust_coeff();
+ // cos(30°) = 0.8660254
+ static const int32_t max_bank_coef = BFP_OF_REAL(0.8660254f, INT32_TRIG_FRAC);
+
+ struct Int32RMat *att = stateGetNedToBodyRMat_i();
+ /* thrust vector:
+ * int32_rmat_vmult(&thrust_vect, &att, &zaxis)
+ * same as last colum of rmat with INT32_TRIG_FRAC
+ * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
+ *
+ * Angle between two vectors v1 and v2:
+ * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
+ * since here both are already of unit length:
+ * angle = acos(dot(v1, v2))
+ * since we we want the cosine of the angle we simply need
+ * thrust_coeff = dot(v1, v2)
+ * also can be simplified considering: v1 is zaxis with (0,0,1)
+ * dot(v1, v2) = v1.z * v2.z = v2.z
+ */
+ int32_t coef = att->m[8];
+ if (coef < max_bank_coef) {
+ coef = max_bank_coef;
+ }
+ return coef;
+}
+
+
+static void guidance_v_thrust_adapt(bool in_flight)
+{
+ guidance_v.thrust_coeff = get_vertical_thrust_coeff();
if (in_flight) {
/* Only run adaptive throttle estimation if we are in flight and
@@ -287,8 +211,8 @@ void guidance_v_thrust_adapt(bool in_flight)
* FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
*/
if (desired_zd_updated) {
- int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC;
- gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref);
+ int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
+ gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v.zd_ref);
}
} else {
/* reset estimate while not in_flight */
@@ -303,30 +227,30 @@ void guidance_v_run(bool in_flight)
/* reset flag indicating if desired zd was updated */
desired_zd_updated = false;
- switch (guidance_v_mode) {
+ switch (guidance_v.mode) {
case GUIDANCE_V_MODE_RC_DIRECT:
- guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
- stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t;
+ guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
+ 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, stateGetPositionNed_i()->z);
- run_hover_loop(in_flight);
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+ guidance_v.zd_sp = guidance_v.rc_zd_sp;
+ gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+ guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
+ stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
break;
case GUIDANCE_V_MODE_CLIMB:
- gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
- run_hover_loop(in_flight);
+ gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+ guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
- 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
#endif
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+ stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
break;
case GUIDANCE_V_MODE_HOVER:
@@ -359,149 +283,66 @@ void guidance_v_run(bool in_flight)
void guidance_v_z_enter(void)
{
/* set current altitude as setpoint */
- guidance_v_z_sp = stateGetPositionNed_i()->z;
+ guidance_v.z_sp = stateGetPositionNed_i()->z;
/* reset guidance reference */
- guidance_v_z_sum_err = 0;
+ guidance_v_run_enter();
GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
/* reset speed setting */
- guidance_v_zd_sp = 0;
+ guidance_v.zd_sp = 0;
}
void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
{
gv_set_ref(pos, speed, accel);
- guidance_v_z_ref = pos;
- guidance_v_zd_ref = speed;
- guidance_v_zdd_ref = accel;
+ guidance_v.z_ref = pos;
+ guidance_v.zd_ref = speed;
+ guidance_v.zdd_ref = accel;
}
-/// get the cosine of the angle between thrust vector and gravity vector
-static int32_t get_vertical_thrust_coeff(void)
+static void guidance_v_update_ref(void)
{
- // cos(30°) = 0.8660254
- static const int32_t max_bank_coef = BFP_OF_REAL(0.8660254f, INT32_TRIG_FRAC);
-
- struct Int32RMat *att = stateGetNedToBodyRMat_i();
- /* thrust vector:
- * int32_rmat_vmult(&thrust_vect, &att, &zaxis)
- * same as last colum of rmat with INT32_TRIG_FRAC
- * struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
- *
- * Angle between two vectors v1 and v2:
- * angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
- * since here both are already of unit length:
- * angle = acos(dot(v1, v2))
- * since we we want the cosine of the angle we simply need
- * thrust_coeff = dot(v1, v2)
- * also can be simplified considering: v1 is zaxis with (0,0,1)
- * dot(v1, v2) = v1.z * v2.z = v2.z
- */
- int32_t coef = att->m[8];
- if (coef < max_bank_coef) {
- coef = max_bank_coef;
- }
- return coef;
-}
-
-
-#define FF_CMD_FRAC 18
-
-void run_hover_loop(bool in_flight)
-{
-
/* convert our reference to generic representation */
int64_t tmp = gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC);
- guidance_v_z_ref = (int32_t)tmp;
- guidance_v_zd_ref = gv_zd_ref << (INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
- guidance_v_zdd_ref = gv_zdd_ref << (INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
+ guidance_v.z_ref = (int32_t)tmp;
+ guidance_v.zd_ref = gv_zd_ref << (INT32_SPEED_FRAC - GV_ZD_REF_FRAC);
+ guidance_v.zdd_ref = gv_zdd_ref << (INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC);
/* set flag to indicate that desired zd was updated */
desired_zd_updated = true;
- /* compute the error to our reference */
- int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z;
- Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z);
- int32_t err_zd = guidance_v_zd_ref - stateGetSpeedNed_i()->z;
- Bound(err_zd, GUIDANCE_V_MIN_ERR_ZD, GUIDANCE_V_MAX_ERR_ZD);
-
- if (in_flight) {
- guidance_v_z_sum_err += err_z;
- Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR);
- } else {
- guidance_v_z_sum_err = 0;
- }
-
- /* our nominal command : (g + zdd)*m */
- int32_t inv_m;
- if (guidance_v_adapt_throttle_enabled) {
- inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC);
- } else {
- /* use the fixed nominal throttle */
- inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC);
- }
-
- const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) -
- (guidance_v_zdd_ref << (FF_CMD_FRAC - INT32_ACCEL_FRAC));
-
- guidance_v_ff_cmd = g_m_zdd / inv_m;
- /* feed forward command */
- guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / guidance_v_thrust_coeff;
-
-#if HYBRID_NAVIGATION
- //FIXME: NOT USING FEEDFORWARD COMMAND BECAUSE OF QUADSHOT NAVIGATION
- guidance_v_ff_cmd = guidance_v_nominal_throttle * MAX_PPRZ;
-#endif
-
- /* bound the nominal command to GUIDANCE_V_MAX_CMD */
- Bound(guidance_v_ff_cmd, 0, GUIDANCE_V_MAX_CMD);
-
-
- /* our error feed back command */
- /* z-axis pointing down -> positive error means we need less thrust */
- guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 7) +
- ((-guidance_v_kd * err_zd) >> 16) +
- ((-guidance_v_ki * guidance_v_z_sum_err) >> 16);
-
- guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd;
-
- /* bound the result */
- Bound(guidance_v_delta_t, 0, MAX_PPRZ);
-
}
void guidance_v_from_nav(bool in_flight)
{
- if (vertical_mode == VERTICAL_MODE_ALT) {
- guidance_v_z_sp = -nav_flight_altitude;
- guidance_v_zd_sp = 0;
- gv_update_ref_from_z_sp(guidance_v_z_sp);
- run_hover_loop(in_flight);
- } else if (vertical_mode == VERTICAL_MODE_CLIMB) {
- guidance_v_z_sp = stateGetPositionNed_i()->z;
- guidance_v_zd_sp = -nav_climb;
- gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
- run_hover_loop(in_flight);
- } else if (vertical_mode == VERTICAL_MODE_MANUAL) {
- guidance_v_z_sp = stateGetPositionNed_i()->z;
- guidance_v_zd_sp = stateGetSpeedNed_i()->z;
- GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
- guidance_v_z_sum_err = 0;
- guidance_v_delta_t = nav_throttle;
- } else if (vertical_mode == VERTICAL_MODE_GUIDED) {
+ if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
+ guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude);
+ guidance_v.zd_sp = 0;
+ gv_update_ref_from_z_sp(guidance_v.z_sp);
+ guidance_v_update_ref();
+ guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
+ } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
+ guidance_v.z_sp = stateGetPositionNed_i()->z;
+ guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb);
+ gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+ guidance_v_update_ref();
+ guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
+ } else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
+ guidance_v.z_sp = stateGetPositionNed_i()->z;
+ guidance_v.zd_sp = stateGetSpeedNed_i()->z;
+ GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0);
+ guidance_v_run_enter();
+ guidance_v.delta_t = nav.throttle;
+ } else if (nav.vertical_mode == NAV_VERTICAL_MODE_GUIDED) {
guidance_v_guided_run(in_flight);
}
-#if HYBRID_NAVIGATION
- guidance_hybrid_vertical();
-#else
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
- 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
#endif
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
-#endif
+ stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
}
void guidance_v_guided_enter(void)
@@ -510,7 +351,7 @@ void guidance_v_guided_enter(void)
guidance_v_set_z(stateGetPositionNed_f()->z);
/* reset guidance reference */
- guidance_v_z_sum_err = 0;
+ guidance_v_run_enter();
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
}
@@ -520,19 +361,21 @@ void guidance_v_guided_run(bool in_flight)
{
case GUIDANCE_V_GUIDED_MODE_ZHOLD:
// Altitude Hold
- guidance_v_zd_sp = 0;
- gv_update_ref_from_z_sp(guidance_v_z_sp);
- run_hover_loop(in_flight);
+ guidance_v.zd_sp = 0;
+ gv_update_ref_from_z_sp(guidance_v.z_sp);
+ guidance_v_update_ref();
+ guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
break;
case GUIDANCE_V_GUIDED_MODE_CLIMB:
// Climb
- gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
- run_hover_loop(in_flight);
+ gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+ guidance_v_update_ref();
+ guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
break;
case GUIDANCE_V_GUIDED_MODE_THROTTLE:
// Throttle
- guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only
- guidance_v_delta_t = guidance_v_th_sp;
+ guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
+ guidance_v.delta_t = guidance_v.th_sp;
break;
default:
break;
@@ -540,10 +383,10 @@ void guidance_v_guided_run(bool in_flight)
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
- 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
#endif
- stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
+ stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
}
void guidance_v_set_z(float z)
@@ -551,9 +394,9 @@ void guidance_v_set_z(float z)
/* disable vertical velocity setpoints */
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
/* set altitude setpoint */
- guidance_v_z_sp = POS_BFP_OF_REAL(z);
+ guidance_v.z_sp = POS_BFP_OF_REAL(z);
/* reset speed setting */
- guidance_v_zd_sp = 0;
+ guidance_v.zd_sp = 0;
}
void guidance_v_set_vz(float vz)
@@ -561,7 +404,7 @@ void guidance_v_set_vz(float vz)
/* enable vertical velocity setpoints */
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB;
/* set speed setting */
- guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz);
+ guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz);
}
void guidance_v_set_th(float th)
@@ -571,8 +414,8 @@ void guidance_v_set_th(float th)
/* reset guidance reference */
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
- guidance_v_th_sp = (int32_t)(MAX_PPRZ * th);
- Bound(guidance_v_th_sp, 0, MAX_PPRZ);
+ guidance_v.th_sp = (int32_t)(MAX_PPRZ * th);
+ Bound(guidance_v.th_sp, 0, MAX_PPRZ);
}
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
index 18f4bb15cc..e6a843ef07 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
@@ -14,9 +14,8 @@
* GNU General Public License for more details.
*
* 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.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
*/
/** @file firmwares/rotorcraft/guidance/guidance_v.h
@@ -42,95 +41,100 @@
#define GUIDANCE_V_MODE_FLIP 7
#define GUIDANCE_V_MODE_GUIDED 8
-extern uint8_t guidance_v_mode;
+struct VerticalGuidance {
+ uint8_t mode;
-/** altitude setpoint in meters (input).
- * fixed point representation: Q23.8
- * accuracy 0.0039, range 8388km
- */
-extern int32_t guidance_v_z_sp;
+ /** altitude setpoint in meters (input).
+ * fixed point representation: Q23.8
+ * accuracy 0.0039, range 8388km
+ */
+ int32_t z_sp;
-/** vertical speed setpoint in meter/s (input).
- * fixed point representation: Q12.19
- * accuracy 0.0000019, range +/-4096
- */
-extern int32_t guidance_v_zd_sp;
+ /** vertical speed setpoint in meter/s (input).
+ * fixed point representation: Q12.19
+ * accuracy 0.0000019, range +/-4096
+ */
+ int32_t zd_sp;
-/** altitude reference in meters.
- * fixed point representation: Q23.8
- * accuracy 0.0039, range 8388km
- */
-extern int32_t guidance_v_z_ref;
+ /** altitude reference in meters.
+ * fixed point representation: Q23.8
+ * accuracy 0.0039, range 8388km
+ */
+ int32_t z_ref;
-/** vertical speed reference in meter/s.
- * fixed point representation: Q12.19
- * accuracy 0.0000038, range 4096
- */
-extern int32_t guidance_v_zd_ref;
+ /** vertical speed reference in meter/s.
+ * fixed point representation: Q12.19
+ * accuracy 0.0000038, range 4096
+ */
+ int32_t zd_ref;
-/** vertical acceleration reference in meter/s^2.
- * fixed point representation: Q21.10
- * accuracy 0.0009766, range 2097152
- */
-extern int32_t guidance_v_zdd_ref;
+ /** vertical acceleration reference in meter/s^2.
+ * fixed point representation: Q21.10
+ * accuracy 0.0009766, range 2097152
+ */
+ int32_t zdd_ref;
-extern int32_t guidance_v_z_sum_err; ///< accumulator for I-gain
-extern int32_t guidance_v_ff_cmd; ///< feed-forward command
-extern int32_t guidance_v_fb_cmd; ///< feed-back command
+ /** Direct throttle from radio control.
+ * range 0:#MAX_PPRZ
+ */
+ int32_t rc_delta_t;
-/** Direct throttle from radio control.
- * range 0:#MAX_PPRZ
- */
-extern int32_t guidance_v_rc_delta_t;
+ /** Vertical speed setpoint from radio control.
+ * fixed point representation: Q12.19
+ * accuracy 0.0000019, range +/-4096
+ */
+ int32_t rc_zd_sp;
-/** thrust command.
- * summation of feed-forward and feed-back commands,
- * valid range 0 : #MAX_PPRZ
- */
-extern int32_t guidance_v_delta_t;
+ /** thrust setpoint.
+ * valid range 0 : #MAX_PPRZ
+ */
+ int32_t th_sp;
-/** nominal throttle for hover.
- * This is only used if #GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined!
- * Unit: factor of #MAX_PPRZ with range 0.1 : 0.9
- */
-extern float guidance_v_nominal_throttle;
+ /** thrust command.
+ * summation of feed-forward and feed-back commands,
+ * valid range 0 : #MAX_PPRZ
+ */
+ int32_t delta_t;
-/** Use adaptive throttle command estimation.
- */
-extern bool guidance_v_adapt_throttle_enabled;
+ /** nominal throttle for hover.
+ * This is only used if #GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined!
+ * Unit: factor of #MAX_PPRZ with range 0.1 : 0.9
+ */
+ float nominal_throttle;
-extern int32_t guidance_v_thrust_coeff;
+ int32_t thrust_coeff;
+};
-extern int32_t guidance_v_kp; ///< vertical control P-gain
-extern int32_t guidance_v_kd; ///< vertical control D-gain
-extern int32_t guidance_v_ki; ///< vertical control I-gain
+extern struct VerticalGuidance guidance_v;
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_thrust_adapt(bool in_flight);
extern void guidance_v_notify_in_flight(bool in_flight);
extern void guidance_v_run(bool in_flight);
extern void guidance_v_z_enter(void);
+extern void guidance_v_run_enter(void);
+extern int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
+extern int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
+extern int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
+
/** Set guidance ref parameters
- */
+*/
extern void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel);
// macro for backward compatibility
#define GuidanceVSetRef guidance_v_set_ref
-extern void run_hover_loop(bool in_flight);
-
/** Set guidance setpoint from NAV and run hover loop
- */
+*/
extern void guidance_v_from_nav(bool in_flight);
/** Enter GUIDED mode control
- */
+*/
extern void guidance_v_guided_enter(void);
/** Run GUIDED mode control
- */
+*/
extern void guidance_v_guided_run(bool in_flight);
/** Set z position setpoint.
@@ -148,9 +152,4 @@ extern void guidance_v_set_vz(float vz);
*/
extern void guidance_v_set_th(float th);
-#define guidance_v_SetKi(_val) { \
- guidance_v_ki = _val; \
- guidance_v_z_sum_err = 0; \
- }
-
#endif /* GUIDANCE_V_H */
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 52986e58b9..98257bc18b 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin
+ * Copyright (C) 2022 Gautier Hattenberger
*
* This file is part of paparazzi.
*
@@ -14,9 +15,8 @@
* GNU General Public License for more details.
*
* 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.
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
*/
/**
@@ -31,13 +31,11 @@
#include "firmwares/rotorcraft/navigation.h"
#include "pprz_debug.h"
-#include "modules/gps/gps.h" // needed by auto_nav from the flight plan
-#include "modules/ins/ins.h"
#include "state.h"
-
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
+#include "modules/ins/ins.h"
/* for default GUIDANCE_H_USE_REF */
#include "firmwares/rotorcraft/guidance/guidance_h.h"
@@ -48,181 +46,61 @@
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
-
PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
-/** default nav_circle_radius in meters */
-#ifndef DEFAULT_CIRCLE_RADIUS
-#define DEFAULT_CIRCLE_RADIUS 5.
-#endif
-
-#ifndef NAV_CLIMB_VSPEED
-#define NAV_CLIMB_VSPEED 0.5
-#endif
-
-#ifndef NAV_DESCEND_VSPEED
-#define NAV_DESCEND_VSPEED -0.8
-#endif
-
-/** minimum horizontal distance to waypoint to mark as arrived */
-#ifndef ARRIVED_AT_WAYPOINT
-#define ARRIVED_AT_WAYPOINT 3.0
-#endif
-
-/** Maximum distance from HOME waypoint before going into failsafe mode */
-#ifndef FAILSAFE_MODE_DISTANCE
-#define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
-#endif
-
-#ifndef NAV_CARROT_DIST
-#define NAV_CARROT_DIST 12
-#endif
-
-#define CLOSE_TO_WAYPOINT (15 << INT32_POS_FRAC)
-#define CARROT_DIST ((int32_t) POS_BFP_OF_REAL(NAV_CARROT_DIST))
-
-bool force_forward = false;
-
-struct FloatVect2 line_vect, to_end_vect;
+struct RotorcraftNavigation nav;
const float max_dist_from_home = MAX_DIST_FROM_HOME;
const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
-float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
-float dist2_to_home;
-bool too_far_from_home;
-float dist2_to_wp;
-
-struct EnuCoor_i navigation_target;
-struct EnuCoor_i navigation_carrot;
-
-struct EnuCoor_i nav_last_point;
-
-uint8_t last_wp UNUSED;
-
-bool exception_flag[10] = {0}; //exception flags that can be used in the flight plan
-
-uint8_t horizontal_mode;
-
-int32_t nav_leg_progress;
-uint32_t nav_leg_length;
-
-bool nav_survey_active;
-
-int32_t nav_roll, nav_pitch;
-int32_t nav_heading;
-int32_t nav_cmd_roll, nav_cmd_pitch, nav_cmd_yaw;
-float nav_radius;
-float nav_climb_vspeed, nav_descend_vspeed;
-
-uint8_t vertical_mode;
-uint32_t nav_throttle;
-int32_t nav_climb, nav_altitude, nav_flight_altitude;
float flight_altitude;
-/* nav_circle variables */
-struct EnuCoor_i nav_circle_center;
-int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
-
-/* nav_route variables */
-struct EnuCoor_i nav_segment_start, nav_segment_end;
-
static inline void nav_set_altitude(void);
-#if PERIODIC_TELEMETRY
-#include "modules/datalink/telemetry.h"
-
-void set_exception_flag(uint8_t flag_num)
-{
- exception_flag[flag_num] = 1;
-}
-
-static void send_segment(struct transport_tx *trans, struct link_device *dev)
-{
- float sx = POS_FLOAT_OF_BFP(nav_segment_start.x);
- float sy = POS_FLOAT_OF_BFP(nav_segment_start.y);
- float ex = POS_FLOAT_OF_BFP(nav_segment_end.x);
- float ey = POS_FLOAT_OF_BFP(nav_segment_end.y);
- pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey);
-}
-
-static void send_circle(struct transport_tx *trans, struct link_device *dev)
-{
- float cx = POS_FLOAT_OF_BFP(nav_circle_center.x);
- float cy = POS_FLOAT_OF_BFP(nav_circle_center.y);
- float r = POS_FLOAT_OF_BFP(nav_circle_radius);
- pprz_msg_send_CIRCLE(trans, dev, AC_ID, &cx, &cy, &r);
-}
-
-static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
-{
- float dist_home = sqrtf(dist2_to_home);
- float dist_wp = sqrtf(dist2_to_wp);
- pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
- &block_time, &stage_time,
- &dist_home, &dist_wp,
- &nav_block, &nav_stage,
- &horizontal_mode);
- if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
- send_segment(trans, dev);
- } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {
- send_circle(trans, dev);
- }
-}
-
-static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
-{
- static uint8_t i;
- i++;
- if (i >= nb_waypoint) { i = 0; }
- pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
- &i,
- &(waypoints[i].enu_i.x),
- &(waypoints[i].enu_i.y),
- &(waypoints[i].enu_i.z));
-}
-#endif
-
void nav_init(void)
{
waypoints_init();
nav_block = 0;
nav_stage = 0;
- nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
- nav_flight_altitude = nav_altitude;
+
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
+ nav.vertical_mode = NAV_VERTICAL_MODE_ALT;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
+
+ VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
+ VECT3_COPY(nav.carrot, waypoints[WP_HOME].enu_f);
+ FLOAT_VECT3_ZERO(nav.speed);
+ FLOAT_VECT3_ZERO(nav.accel);
+ float_quat_identity(&nav.quat);
+ FLOAT_RATES_ZERO(nav.rates);
+
+ nav.throttle = 0;
+ nav.cmd_roll = 0;
+ nav.cmd_pitch = 0;
+ nav.cmd_yaw = 0;
+ nav.roll = 0.f;
+ nav.pitch = 0.f;
+ nav.heading = 0.f;
+ nav.radius = DEFAULT_CIRCLE_RADIUS;
+ nav.climb = 0.f;
+ nav.fp_altitude = SECURITY_HEIGHT;
+ nav.nav_altitude = SECURITY_HEIGHT;
flight_altitude = SECURITY_ALT;
- VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
- VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
- vertical_mode = VERTICAL_MODE_ALT;
+ nav.too_far_from_home = false;
+ nav.failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
+ nav.dist2_to_home = 0.f;
+ nav.climb_vspeed = NAV_CLIMB_VSPEED;
+ nav.descend_vspeed = NAV_DESCEND_VSPEED;
- nav_roll = 0;
- nav_pitch = 0;
- nav_heading = 0;
- nav_cmd_roll = 0;
- nav_cmd_pitch = 0;
- nav_cmd_yaw = 0;
- nav_radius = DEFAULT_CIRCLE_RADIUS;
- nav_climb_vspeed = NAV_CLIMB_VSPEED;
- nav_descend_vspeed = NAV_DESCEND_VSPEED;
- nav_throttle = 0;
- nav_climb = 0;
- nav_leg_progress = 0;
- nav_leg_length = 1;
-
- too_far_from_home = false;
- dist2_to_home = 0;
- dist2_to_wp = 0;
-
- FLOAT_VECT2_ZERO(line_vect);
- FLOAT_VECT2_ZERO(to_end_vect);
-
-#if PERIODIC_TELEMETRY
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WP_MOVED, send_wp_moved);
-#endif
+ nav.nav_stage_init = NULL;
+ nav.nav_goto = NULL;
+ nav.nav_route = NULL;
+ nav.nav_approaching = NULL;
+ nav.nav_circle = NULL;
+ nav.nav_oval_init = NULL;
+ nav.nav_oval = NULL;
// generated init function
auto_nav_init();
@@ -252,25 +130,29 @@ void nav_parse_MOVE_WP(uint8_t *buf)
}
}
+#ifndef CLOSE_TO_WAYPOINT
+#define CLOSE_TO_WAYPOINT 15.f
+#endif
+
static inline void UNUSED nav_advance_carrot(void)
{
- struct EnuCoor_i *pos = stateGetPositionEnu_i();
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
/* compute a vector to the waypoint */
- struct Int32Vect2 path_to_waypoint;
- VECT2_DIFF(path_to_waypoint, navigation_target, *pos);
+ struct FloatVect2 path_to_waypoint;
+ VECT2_DIFF(path_to_waypoint, nav.target, *pos);
/* saturate it */
- VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15));
+ VECT2_STRIM(path_to_waypoint, -150.f, 150.f);
- int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
+ float dist_to_waypoint = float_vect2_norm(&path_to_waypoint);
if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
- VECT2_COPY(navigation_carrot, navigation_target);
+ VECT2_COPY(nav.carrot, nav.target);
} else {
struct Int32Vect2 path_to_carrot;
- VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
+ VECT2_SMUL(path_to_carrot, path_to_waypoint, NAV_CARROT_DIST);
VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
- VECT2_SUM(navigation_carrot, path_to_carrot, *pos);
+ VECT2_SUM(nav.carrot, path_to_carrot, *pos);
}
}
@@ -279,76 +161,34 @@ void nav_run(void)
#if GUIDANCE_H_USE_REF
// if GUIDANCE_H_USE_REF, CARROT_DIST is not used
- VECT2_COPY(navigation_carrot, navigation_target);
+ VECT2_COPY(nav.carrot, nav.target);
#else
nav_advance_carrot();
#endif
+ // update altitude setpoint if needed
nav_set_altitude();
}
-bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
-{
- float dist_to_point;
- struct Int32Vect2 diff;
- struct EnuCoor_i *pos = stateGetPositionEnu_i();
-
- /* if an approaching_time is given, estimate diff after approching_time secs */
- if (approaching_time > 0) {
- struct Int32Vect2 estimated_pos;
- struct Int32Vect2 estimated_progress;
- struct EnuCoor_i *speed = stateGetSpeedEnu_i();
- VECT2_SMUL(estimated_progress, *speed, approaching_time);
- INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
- VECT2_SUM(estimated_pos, *pos, estimated_progress);
- VECT2_DIFF(diff, *wp, estimated_pos);
- }
- /* else use current position */
- else {
- VECT2_DIFF(diff, *wp, *pos);
- }
- /* compute distance of estimated/current pos to target wp
- * POS_FRAC resolution
- * convert to float to compute the norm without overflow in 32bit
- */
- struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
- dist_to_point = float_vect2_norm(&diff_f);
-
- /* return TRUE if we have arrived */
- if (dist_to_point < ARRIVED_AT_WAYPOINT) {
- return true;
- }
-
- /* if coming from a valid waypoint */
- if (from != NULL) {
- /* return TRUE if normal line at the end of the segment is crossed */
- struct Int32Vect2 from_diff;
- VECT2_DIFF(from_diff, *wp, *from);
- struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
- return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
- }
-
- return false;
-}
-
-bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
+bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time)
{
uint16_t time_at_wp;
float dist_to_point;
static uint16_t wp_entry_time = 0;
static bool wp_reached = false;
static struct EnuCoor_i wp_last = { 0, 0, 0 };
- struct Int32Vect2 diff;
+ struct EnuCoor_i wp_i;
+ struct FloatVect2 diff;
- if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
+ ENU_BFP_OF_REAL(wp_i, *wp);
+ if ((wp_last.x != wp_i.x) || (wp_last.y != wp_i.y)) {
wp_reached = false;
- wp_last = *wp;
+ wp_last = wp_i;
}
- VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
- struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
- dist_to_point = float_vect2_norm(&diff_f);
+ VECT2_DIFF(diff, *wp, *stateGetPositionEnu_f());
+ dist_to_point = float_vect2_norm(&diff);
if (dist_to_point < ARRIVED_AT_WAYPOINT) {
if (!wp_reached) {
wp_reached = true;
@@ -370,10 +210,16 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
static inline void nav_set_altitude(void)
{
- static int32_t last_nav_alt = 0;
- if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
- nav_flight_altitude = nav_altitude;
- last_nav_alt = nav_altitude;
+ static float last_alt = 0.f;
+ // if the fp_altitude setpoint change is large enough, set this alt as the new reference
+ // otherwise, don't change nav_altitude (whose value can be changed by the operator
+ // through the flight_altitude setting)
+ // nav_altitude is the value that is used by guidance as a setpoint when flying in
+ // altitude mode
+ if (fabsf(nav.fp_altitude - last_alt) > 0.2f) {
+ nav.nav_altitude = nav.fp_altitude;
+ flight_altitude = nav.nav_altitude + state.ned_origin_f.hmsl;
+ last_alt = nav.fp_altitude;
}
}
@@ -394,9 +240,10 @@ void nav_reset_alt(void)
void nav_init_stage(void)
{
- VECT3_COPY(nav_last_point, *stateGetPositionEnu_i());
stage_time = 0;
- nav_circle_radians = 0;
+ if (nav.nav_stage_init) {
+ nav.nav_stage_init();
+ }
}
#include
@@ -404,10 +251,6 @@ void nav_periodic_task(void)
{
RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
- nav_survey_active = false;
-
- dist2_to_wp = 0;
-
/* from flight_plan.h */
auto_nav();
@@ -430,14 +273,12 @@ bool nav_is_in_flight(void)
/** Home mode navigation */
void nav_home(void)
{
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
- VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
+ VECT3_COPY(nav.target, waypoints[WP_HOME].enu_f);
- vertical_mode = VERTICAL_MODE_ALT;
- nav_altitude = waypoints[WP_HOME].enu_i.z;
- nav_flight_altitude = nav_altitude;
-
- dist2_to_wp = dist2_to_home;
+ nav.vertical_mode = NAV_VERTICAL_MODE_ALT;
+ nav.nav_altitude = waypoint_get_alt(WP_HOME);
/* run carrot loop */
nav_run();
@@ -455,26 +296,27 @@ void nav_home(void)
*/
void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
{
- horizontal_mode = HORIZONTAL_MODE_MANUAL;
- nav_cmd_roll = roll;
- nav_cmd_pitch = pitch;
- nav_cmd_yaw = yaw;
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_MANUAL;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_MANUAL;
+ nav.cmd_roll = roll;
+ nav.cmd_pitch = pitch;
+ nav.cmd_yaw = yaw;
}
/** Returns squared horizontal distance to given point */
-float get_dist2_to_point(struct EnuCoor_i *p)
+float get_dist2_to_point(struct EnuCoor_f *p)
{
struct EnuCoor_f *pos = stateGetPositionEnu_f();
struct FloatVect2 pos_diff;
- pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
- pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
+ pos_diff.x = p->x - pos->x;
+ pos_diff.y = p->y - pos->y;
return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
}
/** Returns squared horizontal distance to given waypoint */
float get_dist2_to_waypoint(uint8_t wp_id)
{
- return get_dist2_to_point(&waypoints[wp_id].enu_i);
+ return get_dist2_to_point(&waypoints[wp_id].enu_f);
}
/** Computes squared distance to the HOME waypoint potentially sets
@@ -482,19 +324,19 @@ float get_dist2_to_waypoint(uint8_t wp_id)
*/
void compute_dist2_to_home(void)
{
- dist2_to_home = get_dist2_to_waypoint(WP_HOME);
- too_far_from_home = dist2_to_home > max_dist2_from_home;
+ nav.dist2_to_home = get_dist2_to_waypoint(WP_HOME);
+ nav.too_far_from_home = nav.dist2_to_home > max_dist2_from_home;
#ifdef InGeofenceSector
struct EnuCoor_f *pos = stateGetPositionEnu_f();
- too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
+ nav.too_far_from_home = nav.too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
#endif
}
/** Set nav_heading in radians. */
void nav_set_heading_rad(float rad)
{
- nav_heading = ANGLE_BFP_OF_REAL(rad);
- INT32_COURSE_NORMALIZE(nav_heading);
+ nav.heading = rad;
+ NormCourseRad(nav.heading);
}
/** Set nav_heading in degrees. */
@@ -510,9 +352,9 @@ void nav_set_heading_towards(float x, float y)
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
// don't change heading if closer than 0.5m to target
- if (VECT2_NORM2(pos_diff) > 0.25) {
+ if (VECT2_NORM2(pos_diff) > 0.25f) {
float heading_f = atan2f(pos_diff.x, pos_diff.y);
- nav_heading = ANGLE_BFP_OF_REAL(heading_f);
+ nav.heading = heading_f;
}
}
@@ -525,14 +367,13 @@ void nav_set_heading_towards_waypoint(uint8_t wp)
/** Set heading in the direction of the target*/
void nav_set_heading_towards_target(void)
{
- nav_set_heading_towards(POS_FLOAT_OF_BFP(navigation_target.x),
- POS_FLOAT_OF_BFP(navigation_target.y));
+ nav_set_heading_towards(nav.target.x, nav.target.y);
}
/** Set heading to the current yaw angle */
void nav_set_heading_current(void)
{
- nav_heading = stateGetNedToBodyEulers_i()->psi;
+ nav.heading = stateGetNedToBodyEulers_f()->psi;
}
void nav_set_failsafe(void)
@@ -540,211 +381,28 @@ void nav_set_failsafe(void)
autopilot_set_mode(AP_MODE_FAILSAFE);
}
-
-/***********************************************************
- * built in navigation routines
- **********************************************************/
-
-void nav_circle(struct EnuCoor_i *wp_center, int32_t radius)
-{
- if (radius == 0) {
- VECT2_COPY(navigation_target, *wp_center);
- dist2_to_wp = get_dist2_to_point(wp_center);
- } else {
- struct Int32Vect2 pos_diff;
- VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
- // go back to half metric precision or values are too large
- //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
- // store last qdr
- int32_t last_qdr = nav_circle_qdr;
- // compute qdr
- nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x);
- // increment circle radians
- if (nav_circle_radians != 0) {
- int32_t angle_diff = nav_circle_qdr - last_qdr;
- INT32_ANGLE_NORMALIZE(angle_diff);
- nav_circle_radians += angle_diff;
- } else {
- // Smallest angle to increment at next step
- nav_circle_radians = 1;
- }
-
- // direction of rotation
- int8_t sign_radius = radius > 0 ? 1 : -1;
- // absolute radius
- int32_t abs_radius = abs(radius);
- // carrot_angle
- int32_t carrot_angle = ((CARROT_DIST << INT32_ANGLE_FRAC) / abs_radius);
- Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
- carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
- int32_t s_carrot, c_carrot;
- PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
- PPRZ_ITRIG_COS(c_carrot, carrot_angle);
- // compute setpoint
- VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
- INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
- VECT2_SUM(navigation_target, *wp_center, pos_diff);
- }
- nav_circle_center = *wp_center;
- nav_circle_radius = radius;
- horizontal_mode = HORIZONTAL_MODE_CIRCLE;
-}
-
-
-void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
-{
- struct Int32Vect2 wp_diff, pos_diff, wp_diff_prec;
- VECT2_DIFF(wp_diff, *wp_end, *wp_start);
- VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_start);
- // go back to metric precision or values are too large
- VECT2_COPY(wp_diff_prec, wp_diff);
- INT32_VECT2_RSHIFT(wp_diff, wp_diff, INT32_POS_FRAC);
- INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_POS_FRAC);
- uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 1);
- nav_leg_length = int32_sqrt(leg_length2);
- nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / (int32_t)nav_leg_length;
- int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
- nav_leg_progress += progress;
- int32_t prog_2 = nav_leg_length;
- Bound(nav_leg_progress, 0, prog_2);
- struct Int32Vect2 progress_pos;
- VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length);
- VECT2_SUM(navigation_target, *wp_start, progress_pos);
-
- nav_segment_start = *wp_start;
- nav_segment_end = *wp_end;
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
-
- dist2_to_wp = get_dist2_to_point(wp_end);
-}
-
-
-/************** Oval Navigation **********************************************/
-
-#ifndef LINE_START_FUNCTION
-#define LINE_START_FUNCTION {}
-#endif
-#ifndef LINE_STOP_FUNCTION
-#define LINE_STOP_FUNCTION {}
-#endif
-
-enum oval_status { OR12, OC2, OR21, OC1 };
-enum oval_status oval_status;
-uint8_t nav_oval_count;
-
-void nav_oval_init(void)
-{
- oval_status = OC2;
- nav_oval_count = 0;
-}
-
-/**
- * Navigation along a figure O. One side leg is defined by waypoints [p1] and [p2].
- * The navigation goes through 4 states: OC1 (half circle next to [p1]),
- * OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).
- * Initial state is the route along the desired segment (OC2).
+/** Register functions
*/
-void nav_oval(uint8_t p1, uint8_t p2, float radius)
+void nav_register_stage_init(navigation_stage_init nav_stage_init)
{
- radius = - radius; /* Historical error ? */
- int32_t alt = waypoints[p1].enu_i.z;
- waypoints[p2].enu_i.z = alt;
-
- float p2_p1_x = waypoints[p1].enu_f.x - waypoints[p2].enu_f.x;
- float p2_p1_y = waypoints[p1].enu_f.y - waypoints[p2].enu_f.y;
- float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);
-
- /* Unit vector from p1 to p2 */
- int32_t u_x = POS_BFP_OF_REAL(p2_p1_x / d);
- int32_t u_y = POS_BFP_OF_REAL(p2_p1_y / d);
-
- /* The half circle centers and the other leg */
- struct EnuCoor_i p1_center = { waypoints[p1].enu_i.x + radius * -u_y,
- waypoints[p1].enu_i.y + radius * u_x,
- alt
- };
- struct EnuCoor_i p1_out = { waypoints[p1].enu_i.x + 2 * radius * -u_y,
- waypoints[p1].enu_i.y + 2 * radius * u_x,
- alt
- };
-
- struct EnuCoor_i p2_in = { waypoints[p2].enu_i.x + 2 * radius * -u_y,
- waypoints[p2].enu_i.y + 2 * radius * u_x,
- alt
- };
- struct EnuCoor_i p2_center = { waypoints[p2].enu_i.x + radius * -u_y,
- waypoints[p2].enu_i.y + radius * u_x,
- alt
- };
-
- int32_t qdr_out_2 = INT32_ANGLE_PI - int32_atan2_2(u_y, u_x);
- int32_t qdr_out_1 = qdr_out_2 + INT32_ANGLE_PI;
- if (radius < 0) {
- qdr_out_2 += INT32_ANGLE_PI;
- qdr_out_1 += INT32_ANGLE_PI;
- }
- int32_t qdr_anticipation = ANGLE_BFP_OF_REAL(radius > 0 ? -15 : 15);
-
- switch (oval_status) {
- case OC1 :
- nav_circle(&p1_center, POS_BFP_OF_REAL(-radius));
- if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_1) - qdr_anticipation)) {
- oval_status = OR12;
- InitStage();
- LINE_START_FUNCTION;
- }
- return;
-
- case OR12:
- nav_route(&p1_out, &p2_in);
- if (nav_approaching_from(&p2_in, &p1_out, CARROT)) {
- oval_status = OC2;
- nav_oval_count++;
- InitStage();
- LINE_STOP_FUNCTION;
- }
- return;
-
- case OC2 :
- nav_circle(&p2_center, POS_BFP_OF_REAL(-radius));
- if (NavQdrCloseTo(INT32_DEG_OF_RAD(qdr_out_2) - qdr_anticipation)) {
- oval_status = OR21;
- InitStage();
- LINE_START_FUNCTION;
- }
- return;
-
- case OR21:
- nav_route(&waypoints[p2].enu_i, &waypoints[p1].enu_i);
- if (nav_approaching_from(&waypoints[p1].enu_i, &waypoints[p2].enu_i, CARROT)) {
- oval_status = OC1;
- InitStage();
- LINE_STOP_FUNCTION;
- }
- return;
-
- default: /* Should not occur !!! Doing nothing */
- return;
- }
+ nav.nav_stage_init = nav_stage_init;
}
-/*
-#ifdef TRAFFIC_INFO
-#include "modules/multi/traffic_info.h"
-void nav_follow(uint8_t ac_id, uint32_t distance, uint32_t height)
+void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
{
- struct EnuCoor_i* target = acInfoGetPositionEnu_i(ac_id);
-
-
- float alpha = M_PI / 2 - acInfoGetCourse(ac_id);
- float ca = cosf(alpha), sa = sinf(alpha);
- target->x += - distance * ca;
- target->y += - distance * sa;
- target->z = (Max(target->z + height, SECURITY_HEIGHT)); // todo add ground height to check
-
- ENU_OF_TO_NED(navigation_target, *target);
+ nav.nav_goto = nav_goto;
+ nav.nav_route = nav_route;
+ nav.nav_approaching = nav_approaching;
}
-#else*/
-void nav_follow(uint8_t __attribute__((unused)) _ac_id, uint32_t __attribute__((unused)) distance,
- uint32_t __attribute__((unused)) height) {}
-//#endif
+
+void nav_register_circle(navigation_circle nav_circle)
+{
+ nav.nav_circle = nav_circle;
+}
+
+void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval)
+{
+ nav.nav_oval_init = _nav_oval_init;
+ nav.nav_oval = nav_oval;
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
index 094a0211ef..dd22a3de9c 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.h
+++ b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -1,23 +1,23 @@
/*
-* Copyright (C) 2008-2011 The Paparazzi Team
-*
-* This file is part of paparazzi.
-*
-* paparazzi is free software; you can redistribute it and/or modify
-* it under the terms of the GNU General Public License as published by
-* the Free Software Foundation; either version 2, or (at your option)
-* any later version.
-*
-* paparazzi is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* 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.
-*/
+ * Copyright (C) 2008-2011 The Paparazzi Team
+ * Copyright (C) 2022 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
/**
* @file firmwares/rotorcraft/navigation.h
@@ -30,10 +30,40 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
-
+#include "math/pprz_geodetic_float.h"
+#include "state.h"
#include "modules/nav/waypoints.h"
#include "modules/nav/common_flight_plan.h"
#include "autopilot.h"
+#include "generated/airframe.h"
+
+/** default nav_circle_radius in meters */
+#ifndef DEFAULT_CIRCLE_RADIUS
+#define DEFAULT_CIRCLE_RADIUS 5.f
+#endif
+
+#ifndef NAV_CLIMB_VSPEED
+#define NAV_CLIMB_VSPEED 0.5f
+#endif
+
+#ifndef NAV_DESCEND_VSPEED
+#define NAV_DESCEND_VSPEED -0.8f
+#endif
+
+/** minimum horizontal distance to waypoint to mark as arrived */
+#ifndef ARRIVED_AT_WAYPOINT
+#define ARRIVED_AT_WAYPOINT 3.0f
+#endif
+
+/** Maximum distance from HOME waypoint before going into failsafe mode */
+#ifndef FAILSAFE_MODE_DISTANCE
+#define FAILSAFE_MODE_DISTANCE (1.2*MAX_DIST_FROM_HOME)
+#endif
+
+/** Carrot distance during navigation */
+#ifndef NAV_CARROT_DIST
+#define NAV_CARROT_DIST 12
+#endif
/** default approaching_time for a wp */
#ifndef CARROT
@@ -49,47 +79,98 @@
#endif
#endif
-extern struct EnuCoor_i navigation_target;
-extern struct EnuCoor_i navigation_carrot;
+/** Nav modes
+ * these modes correspond to the flight plan instructions used
+ * to set the high level navigation */
+#define NAV_HORIZONTAL_MODE_WAYPOINT 0
+#define NAV_HORIZONTAL_MODE_ROUTE 1
+#define NAV_HORIZONTAL_MODE_CIRCLE 2
+#define NAV_HORIZONTAL_MODE_ATTITUDE 3
+#define NAV_HORIZONTAL_MODE_MANUAL 4
+#define NAV_HORIZONTAL_MODE_GUIDED 5
-extern uint8_t last_wp __attribute__((unused));
+#define NAV_VERTICAL_MODE_MANUAL 0
+#define NAV_VERTICAL_MODE_CLIMB 1
+#define NAV_VERTICAL_MODE_ALT 2
+#define NAV_VERTICAL_MODE_GUIDED 3
-extern uint8_t horizontal_mode;
+/** Nav setpoint modes
+ * these modes correspond to submodes defined by navigation routines
+ * to tell which setpoint should be considered */
+#define NAV_SETPOINT_MODE_POS 0
+#define NAV_SETPOINT_MODE_SPEED 1
+#define NAV_SETPOINT_MODE_ACCEL 2
+#define NAV_SETPOINT_MODE_ATTITUDE 3 // attitude defined by roll, pitch and heading
+#define NAV_SETPOINT_MODE_QUAT 4 // attitude defined by unit quaternion
+#define NAV_SETPOINT_MODE_RATE 5
+#define NAV_SETPOINT_MODE_MANUAL 6
-extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
-#define HORIZONTAL_MODE_WAYPOINT 0
-#define HORIZONTAL_MODE_ROUTE 1
-#define HORIZONTAL_MODE_CIRCLE 2
-#define HORIZONTAL_MODE_ATTITUDE 3
-#define HORIZONTAL_MODE_MANUAL 4
-#define HORIZONTAL_MODE_GUIDED 5
-extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC
-extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
-extern int32_t nav_cmd_roll, nav_cmd_pitch, nav_cmd_yaw;
-extern float nav_radius;
-extern float nav_climb_vspeed, nav_descend_vspeed;
+typedef void (*navigation_stage_init)(void);
+typedef void (*navigation_goto)(struct EnuCoor_f *wp);
+typedef void (*navigation_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end);
+typedef bool (*navigation_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time);
+typedef void (*navigation_circle)(struct EnuCoor_f *wp_center, float radius);
+typedef void (*navigation_oval_init)(void);
+typedef void (*navigation_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius);
-extern int32_t nav_leg_progress;
-extern uint32_t nav_leg_length;
-extern bool nav_survey_active;
+/** General Navigation structure
+ */
+struct RotorcraftNavigation {
+ // mode
+ uint8_t horizontal_mode; // nav horizontal mode
+ uint8_t vertical_mode; // nav vertical mode
+ uint8_t setpoint_mode; // nav setpoint mode
-extern uint8_t vertical_mode;
-extern uint32_t nav_throttle; ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
-extern int32_t nav_climb, nav_altitude, nav_flight_altitude;
-extern float flight_altitude;
-#define VERTICAL_MODE_MANUAL 0
-#define VERTICAL_MODE_CLIMB 1
-#define VERTICAL_MODE_ALT 2
-#define VERTICAL_MODE_GUIDED 3
+ // commands
+ struct EnuCoor_f target; ///< final target position (in meters)
+ struct EnuCoor_f carrot; ///< carrot position (also used for GCS display)
+ struct EnuCoor_f speed; ///< speed setpoint (in m/s)
+ struct EnuCoor_f accel; ///< accel setpoint (in m/s)
+ uint32_t throttle; ///< throttle command (in pprz_t)
+ int32_t cmd_roll; ///< roll command (in pprz_t)
+ int32_t cmd_pitch; ///< pitch command (in pprz_t)
+ int32_t cmd_yaw; ///< yaw command (in pprz_t)
+ float roll; ///< roll angle (in radians)
+ float pitch; ///< pitch angle (in radians)
+ float heading; ///< heading setpoint (in radians)
+ struct FloatQuat quat; ///< quaternion setpoint
+ struct FloatRates rates; ///< rates setpoint (in rad/s)
+ float radius; ///< radius setpoint (in meters)
+ float climb; ///< climb setpoint (in m/s)
+ float fp_altitude; ///< altitude setpoint from flight plan (in meters)
+ float nav_altitude; ///< current altitude setpoint (in meters): might differ from fp_altitude depending on altitude shift from operator
-extern float dist2_to_home; ///< squared distance to home waypoint
-extern bool too_far_from_home;
-extern float failsafe_mode_dist2; ///< maximum squared distance to home wp before going to failsafe mode
+ // misc
+ float dist2_to_home; ///< squared distance to home waypoint
+ bool too_far_from_home; ///< too_far flag
+ float failsafe_mode_dist2; ///< maximum squared distance to home wp before going to failsafe mode
+ float climb_vspeed; ///< climb speed setting, mostly used in flight plans
+ float descend_vspeed; ///< descend speed setting, mostly used in flight plans
-extern float dist2_to_wp; ///< squared distance to next waypoint
+ // pointers to basic nav functions
+ navigation_stage_init nav_stage_init;
+ navigation_goto nav_goto;
+ navigation_route nav_route;
+ navigation_approaching nav_approaching;
+ navigation_circle nav_circle;
+ navigation_oval_init nav_oval_init;
+ navigation_oval nav_oval;
+};
-extern bool exception_flag[10];
+extern struct RotorcraftNavigation nav;
+
+/** Registering functions
+ */
+extern void nav_register_stage_init(navigation_stage_init nav_stage_init);
+extern void nav_register_goto_wp(navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching);
+extern void nav_register_circle(navigation_circle nav_circle);
+extern void nav_register_oval(navigation_oval_init _nav_oval_init, navigation_oval nav_oval);
+// TODO: eight, survey
+
+
+// flight altitude setting
+extern float flight_altitude; // hmsl flight altitude in meters
/*****************************************************************
@@ -113,23 +194,13 @@ extern bool exception_flag[10];
#define GetAltRef() (state.ned_origin_f.hmsl)
-/** Normalize a degree angle between 0 and 359 */
-#define NormCourse(x) { \
- while (x < 0) x += 360; \
- while (x >= 360) x -= 360; \
- }
-
extern void nav_init(void);
extern void nav_run(void);
extern void nav_parse_BLOCK(uint8_t *buf);
extern void nav_parse_MOVE_WP(uint8_t *buf);
-extern void set_exception_flag(uint8_t flag_num);
-
-extern bool force_forward;
-
extern float get_dist2_to_waypoint(uint8_t wp_id);
-extern float get_dist2_to_point(struct EnuCoor_i *p);
+extern float get_dist2_to_point(struct EnuCoor_f *p);
extern void compute_dist2_to_home(void);
extern void nav_home(void);
extern void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw);
@@ -140,12 +211,14 @@ extern void nav_periodic_task(void);
extern bool nav_is_in_flight(void);
+/** heading utility functions */
extern void nav_set_heading_rad(float rad);
extern void nav_set_heading_deg(float deg);
extern void nav_set_heading_towards(float x, float y);
extern void nav_set_heading_towards_waypoint(uint8_t wp);
extern void nav_set_heading_towards_target(void);
extern void nav_set_heading_current(void);
+
extern void nav_set_failsafe(void);
/* ground detection */
@@ -167,7 +240,6 @@ static inline void NavResurrect(void)
#define NavSetManual nav_set_manual
#define NavSetFailsafe nav_set_failsafe
-
#define NavSetGroundReferenceHere nav_reset_reference
#define NavSetAltitudeReferenceHere nav_reset_alt
@@ -175,74 +247,50 @@ static inline void NavResurrect(void)
#define NavCopyWaypoint waypoint_copy
#define NavCopyWaypointPositionOnly waypoint_position_copy
-
-/** Proximity tests on approaching a wp */
-bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time);
-#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time)
-#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)
-
/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
-bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
-#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time)
-
-
-/* should we really keep this one ??
- * maybe better to use the `goto` flight plan primitive and
- * add a `pre_call` or `call_once` to set the heading?
- */
-static inline void NavGotoWaypointHeading(uint8_t wp)
-{
- vertical_mode = VERTICAL_MODE_ALT;
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
- VECT3_COPY(navigation_target, waypoints[wp].enu_i);
- dist2_to_wp = get_dist2_to_waypoint(wp);
- nav_set_heading_towards_waypoint(wp);
-}
-
+bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time);
+#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_f, time)
/***********************************************************
* macros used by flight plan to set different modes
**********************************************************/
+#define NavAttitude(_roll) { \
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_ATTITUDE; \
+ nav.setpoint_mode = NAV_SETPOINT_MODE_ATTITUDE; \
+ nav.roll = _roll; \
+ }
+
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
#define NavVerticalAutoThrottleMode(_pitch) { \
- nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
+ nav.pitch = _pitch; \
}
-/** Set the climb control to auto-pitch with the specified throttle
- pre-command */
-#define NavVerticalAutoPitchMode(_throttle) {}
-
/** Set the vertical mode to altitude control with the specified altitude
setpoint and climb pre-command. */
#define NavVerticalAltitudeMode(_alt, _pre_climb) { \
- vertical_mode = VERTICAL_MODE_ALT; \
- nav_altitude = POS_BFP_OF_REAL(_alt); \
+ nav.vertical_mode = NAV_VERTICAL_MODE_ALT; \
+ nav.fp_altitude = _alt; \
}
/** Set the vertical mode to climb control with the specified climb setpoint */
-#define NavVerticalClimbMode(_climb) { \
- vertical_mode = VERTICAL_MODE_CLIMB; \
- nav_climb = SPEED_BFP_OF_REAL(_climb); \
+#define NavVerticalClimbMode(_climb) { \
+ nav.vertical_mode = NAV_VERTICAL_MODE_CLIMB; \
+ nav.climb = _climb; \
+ nav.speed.z = _climb; \
}
/** Set the vertical mode to fixed throttle with the specified setpoint */
-#define NavVerticalThrottleMode(_throttle) { \
- vertical_mode = VERTICAL_MODE_MANUAL; \
- nav_throttle = _throttle; \
+#define NavVerticalThrottleMode(_throttle) { \
+ nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL; \
+ nav.throttle = _throttle; \
}
/** Set the heading of the rotorcraft, nothing else */
#define NavHeading nav_set_heading_rad
-#define NavAttitude(_roll) { \
- horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
- nav_roll = ANGLE_BFP_OF_REAL(_roll); \
- }
-
-
/***********************************************************
* built in navigation routines
@@ -251,66 +299,78 @@ static inline void NavGotoWaypointHeading(uint8_t wp)
/*********** Navigation to waypoint *************************************/
static inline void NavGotoWaypoint(uint8_t wp)
{
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
- VECT3_COPY(navigation_target, waypoints[wp].enu_i);
- dist2_to_wp = get_dist2_to_waypoint(wp);
+ if (nav.nav_goto) {
+ nav.nav_goto(&waypoints[wp].enu_f);
+ }
+}
+
+/*********** Navigation along a line *************************************/
+static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
+{
+ if (nav.nav_route) {
+ nav.nav_route(&waypoints[wp_start].enu_f, &waypoints[wp_end].enu_f);
+ }
+}
+
+static inline bool NavApproaching(uint8_t wp, float approaching_time)
+{
+ if (nav.nav_approaching) {
+ return nav.nav_approaching(&waypoints[wp].enu_f, NULL, approaching_time);
+ }
+ else {
+ return true;
+ }
+}
+
+static inline bool NavApproachingFrom(uint8_t to, uint8_t from, float approaching_time)
+{
+ if (nav.nav_approaching) {
+ return nav.nav_approaching(&waypoints[to].enu_f, &waypoints[from].enu_f, approaching_time);
+ }
+ else {
+ return true;
+ }
}
/*********** Navigation on a circle **************************************/
-extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius);
static inline void NavCircleWaypoint(uint8_t wp_center, float radius)
{
- horizontal_mode = HORIZONTAL_MODE_CIRCLE;
- nav_circle(&waypoints[wp_center].enu_i, POS_BFP_OF_REAL(radius));
+ if (nav.nav_circle) {
+ nav.nav_circle(&waypoints[wp_center].enu_f, radius);
+ }
}
-#define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)
-#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
-
-#define CloseDegAngles(_c1, _c2) ({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
-#define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
-/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
-#define NavQdrCloseTo(x) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())
-#define NavCourseCloseTo(x) {}
/*********** Navigation along an oval *************************************/
-extern void nav_oval_init(void);
-extern void nav_oval(uint8_t, uint8_t, float);
-extern uint8_t nav_oval_count;
-#define Oval(a, b, c) nav_oval((b), (a), (c))
+static inline void nav_oval_init(void)
+{
+ if (nav.nav_oval_init) {
+ nav.nav_oval_init();
+ }
+}
-/*********** Navigation along a line *************************************/
-extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end);
-extern struct FloatVect2 line_vect, to_end_vect;
-#ifdef GUIDANCE_INDI_HYBRID
-static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
+static inline void Oval(uint8_t wp1, uint8_t wp2, float radius)
{
- VECT2_DIFF(line_vect, waypoints[wp_end].enu_f, waypoints[wp_start].enu_f);
- VECT2_DIFF(to_end_vect, waypoints[wp_end].enu_f, *stateGetPositionEnu_f());
- VECT3_COPY(navigation_target, waypoints[wp_end].enu_i);
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
+ if (nav.nav_oval) {
+ nav.nav_oval(&waypoints[wp1].enu_f, &waypoints[wp2].enu_f, radius);
+ }
}
-#else
-static inline void NavSegment(uint8_t wp_start, uint8_t wp_end)
-{
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(&waypoints[wp_start].enu_i, &waypoints[wp_end].enu_i);
-}
-#endif
+
/** Nav glide routine */
static inline void NavGlide(uint8_t start_wp, uint8_t wp)
{
- int32_t start_alt = waypoints[start_wp].enu_i.z;
- int32_t diff_alt = waypoints[wp].enu_i.z - start_alt;
- int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / (int32_t)nav_leg_length);
- NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt), 0);
+ struct FloatVect2 wp_diff, pos_diff;
+ VECT2_DIFF(wp_diff, waypoints[wp].enu_f, waypoints[start_wp].enu_f);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), waypoints[start_wp].enu_f);
+ float start_alt = waypoint_get_alt(start_wp);
+ float diff_alt = waypoint_get_alt(wp) - start_alt;
+ float length2 = Max(float_vect2_norm2(&wp_diff), 0.1f);
+ float progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / length2;
+ float alt = start_alt + diff_alt * progress;
+ NavVerticalAltitudeMode(alt, 0);
}
-/* follow another aircraft */
-#define NavFollow nav_follow
-extern void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height);
-
/***********************************************************
@@ -318,9 +378,19 @@ extern void nav_follow(uint8_t _ac_id, uint32_t distance, uint32_t height);
**********************************************************/
#define nav_IncreaseShift(x) {}
#define nav_SetNavRadius(x) {}
-#define navigation_SetFlightAltitude(x) { \
- flight_altitude = x; \
- nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \
+#define navigation_SetFlightAltitude(x) { \
+ flight_altitude = x; \
+ nav.nav_altitude = flight_altitude - state.ned_origin_f.hmsl; \
}
+/** Unused compat macros
+ */
+
+#define NavVerticalAutoPitchMode(_throttle) {}
+
+/* follow another aircraft not implemented with this function
+ * see dedicated nav modules
+ */
+#define NavFollow(_i, _d, _h) {}
+
#endif /* NAVIGATION_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.c b/sw/airborne/firmwares/rotorcraft/stabilization.c
index 56ac99cf03..41b38d7e20 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization.c
@@ -24,6 +24,8 @@
*/
#include "firmwares/rotorcraft/stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
+#include "state.h"
#if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW)
#include "filters/low_pass_filter.h"
@@ -72,6 +74,35 @@ void stabilization_init(void)
}
+// compute sp_euler phi/theta for debugging/telemetry FIXME really needed ?
+/* Rotate horizontal commands to body frame by psi */
+static struct Int32Eulers stab_sp_rotate_i(struct Int32Vect2 *vect, int32_t heading)
+{
+ struct Int32Eulers sp;
+ int32_t psi = stateGetNedToBodyEulers_i()->psi;
+ int32_t s_psi, c_psi;
+ PPRZ_ITRIG_SIN(s_psi, psi);
+ PPRZ_ITRIG_COS(c_psi, psi);
+ sp.phi = (-s_psi * vect->x + c_psi * vect->y) >> INT32_TRIG_FRAC;
+ sp.theta = -(c_psi * vect->x + s_psi * vect->y) >> INT32_TRIG_FRAC;
+ sp.psi = heading;
+ return sp;
+}
+
+/* Rotate horizontal commands to body frame by psi */
+static struct FloatEulers stab_sp_rotate_f(struct FloatVect2 *vect, float heading)
+{
+ struct FloatEulers sp;
+ float psi = stateGetNedToBodyEulers_f()->psi;
+ float s_psi = sinf(psi);
+ float c_psi = cosf(psi);
+ sp.phi = -s_psi * vect->x + c_psi * vect->y;
+ sp.theta = -c_psi * vect->x + s_psi * vect->y;
+ sp.psi = heading;
+ return sp;
+}
+
+
void stabilization_filter_commands(void)
{
/* Filter the commands & bound the result */
@@ -88,3 +119,282 @@ void stabilization_filter_commands(void)
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
#endif
}
+
+struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
+{
+ if (sp->type == STAB_SP_QUAT) {
+ if (sp->format == STAB_SP_INT) {
+ return sp->sp.quat_i;
+ } else {
+ struct Int32Quat quat;
+ QUAT_BFP_OF_REAL(quat, sp->sp.quat_f);
+ return quat;
+ }
+ } else if (sp->type == STAB_SP_EULERS) {
+ if (sp->format == STAB_SP_INT) {
+ struct Int32Quat quat;
+ int32_quat_of_eulers(&quat, &sp->sp.eulers_i);
+ return quat;
+ } else {
+ struct Int32Quat quat;
+ struct Int32Eulers eulers;
+ EULERS_BFP_OF_REAL(eulers, sp->sp.eulers_f);
+ int32_quat_of_eulers(&quat, &eulers);
+ return quat;
+ }
+ } else if (sp->type == STAB_SP_LTP) {
+ if (sp->format == STAB_SP_INT) {
+ struct Int32Quat quat;
+ quat_from_earth_cmd_i(&quat, &sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
+ return quat;
+ } else {
+ struct FloatQuat quat_f;
+ struct Int32Quat quat_i;
+ quat_from_earth_cmd_f(&quat_f, &sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
+ QUAT_BFP_OF_REAL(quat_i, quat_f);
+ return quat_i;
+ }
+ } else {
+ // error, rates setpoint
+ struct Int32Quat quat;
+ int32_quat_identity(&quat);
+ return quat;
+ }
+}
+
+struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
+{
+ if (sp->type == STAB_SP_QUAT) {
+ if (sp->format == STAB_SP_FLOAT) {
+ return sp->sp.quat_f;
+ } else {
+ struct FloatQuat quat;
+ QUAT_FLOAT_OF_BFP(quat, sp->sp.quat_i);
+ return quat;
+ }
+ } else if (sp->type == STAB_SP_EULERS) {
+ if (sp->format == STAB_SP_FLOAT) {
+ struct FloatQuat quat;
+ float_quat_of_eulers(&quat, &sp->sp.eulers_f);
+ return quat;
+ } else {
+ struct FloatQuat quat;
+ struct FloatEulers eulers;
+ EULERS_FLOAT_OF_BFP(eulers, sp->sp.eulers_i);
+ float_quat_of_eulers(&quat, &eulers);
+ return quat;
+ }
+ } else if (sp->type == STAB_SP_LTP) {
+ if (sp->format == STAB_SP_FLOAT) {
+ struct FloatQuat quat;
+ quat_from_earth_cmd_f(&quat, &sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
+ return quat;
+ } else {
+ struct FloatQuat quat_f;
+ struct Int32Quat quat_i;
+ quat_from_earth_cmd_i(&quat_i, &sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
+ QUAT_FLOAT_OF_BFP(quat_f, quat_i);
+ return quat_f;
+ }
+ } else {
+ // error, rates setpoint
+ struct FloatQuat quat;
+ float_quat_identity(&quat);
+ return quat;
+ }
+}
+
+struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
+{
+ if (sp->type == STAB_SP_EULERS) {
+ if (sp->format == STAB_SP_INT) {
+ return sp->sp.eulers_i;
+ } else {
+ struct Int32Eulers eulers;
+ EULERS_BFP_OF_REAL(eulers, sp->sp.eulers_f);
+ return eulers;
+ }
+ } else if (sp->type == STAB_SP_QUAT) {
+ if (sp->format == STAB_SP_INT) {
+ struct Int32Eulers eulers;
+ int32_eulers_of_quat(&eulers, &sp->sp.quat_i);
+ return eulers;
+ } else {
+ struct Int32Eulers eulers;
+ struct Int32Quat quat;
+ QUAT_BFP_OF_REAL(quat, sp->sp.quat_f);
+ int32_eulers_of_quat(&eulers, &quat);
+ return eulers;
+ }
+ } else if (sp->type == STAB_SP_LTP) {
+ if (sp->format == STAB_SP_INT) {
+ struct Int32Eulers eulers = stab_sp_rotate_i(&sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
+ return eulers;
+ } else {
+ struct FloatEulers eulers_f = stab_sp_rotate_f(&sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
+ struct Int32Eulers eulers_i;
+ EULERS_BFP_OF_REAL(eulers_i, eulers_f);
+ return eulers_i;
+ }
+ } else {
+ // error, rates setpoint
+ struct Int32Eulers eulers = {0};
+ return eulers;
+ }
+}
+
+struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
+{
+ if (sp->type == STAB_SP_EULERS) {
+ if (sp->format == STAB_SP_FLOAT) {
+ return sp->sp.eulers_f;
+ } else {
+ struct FloatEulers eulers;
+ EULERS_FLOAT_OF_BFP(eulers, sp->sp.eulers_i);
+ return eulers;
+ }
+ } else if (sp->type == STAB_SP_QUAT) {
+ if (sp->format == STAB_SP_FLOAT) {
+ struct FloatEulers eulers;
+ float_eulers_of_quat(&eulers, &sp->sp.quat_f);
+ return eulers;
+ } else {
+ struct FloatEulers eulers;
+ struct FloatQuat quat;
+ QUAT_FLOAT_OF_BFP(quat, sp->sp.quat_i);
+ float_eulers_of_quat(&eulers, &quat);
+ return eulers;
+ }
+ } else if (sp->type == STAB_SP_LTP) {
+ if (sp->format == STAB_SP_FLOAT) {
+ struct FloatEulers eulers = stab_sp_rotate_f(&sp->sp.ltp_f.vect, sp->sp.ltp_f.heading);
+ return eulers;
+ } else {
+ struct Int32Eulers eulers_i = stab_sp_rotate_i(&sp->sp.ltp_i.vect, sp->sp.ltp_i.heading);
+ struct FloatEulers eulers_f;
+ EULERS_FLOAT_OF_BFP(eulers_f, eulers_i);
+ return eulers_f;
+ }
+ } else {
+ // error, rates setpoint
+ struct FloatEulers eulers = {0};
+ return eulers;
+ }
+}
+
+struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
+{
+ if (sp->type == STAB_SP_RATES) {
+ if (sp->format == STAB_SP_INT) {
+ return sp->sp.rates_i;
+ } else {
+ struct Int32Rates rates;
+ RATES_BFP_OF_REAL(rates, sp->sp.rates_f);
+ return rates;
+ }
+ } else {
+ // error, attitude setpoint
+ struct Int32Rates rates = {0};
+ return rates;
+ }
+}
+
+struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
+{
+ if (sp->type == STAB_SP_RATES) {
+ if (sp->format == STAB_SP_FLOAT) {
+ return sp->sp.rates_f;
+ } else {
+ struct FloatRates rates;
+ RATES_FLOAT_OF_BFP(rates, sp->sp.rates_i);
+ return rates;
+ }
+ } else {
+ // error, attitude setpoint
+ struct FloatRates rates = {0};
+ return rates;
+ }
+}
+
+struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_QUAT,
+ .format = STAB_SP_INT,
+ .sp.quat_i = *quat
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_QUAT,
+ .format = STAB_SP_FLOAT,
+ .sp.quat_f = *quat
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_EULERS,
+ .format = STAB_SP_INT,
+ .sp.eulers_i = *eulers
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_EULERS,
+ .format = STAB_SP_FLOAT,
+ .sp.eulers_f = *eulers
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_LTP,
+ .format = STAB_SP_INT,
+ .sp.ltp_i.vect = *vect,
+ .sp.ltp_i.heading = heading
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_LTP,
+ .format = STAB_SP_FLOAT,
+ .sp.ltp_f.vect = *vect,
+ .sp.ltp_f.heading = heading
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_RATES,
+ .format = STAB_SP_INT,
+ .sp.rates_i = *rates
+ };
+ return sp;
+}
+
+struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
+{
+ struct StabilizationSetpoint sp = {
+ .type = STAB_SP_RATES,
+ .format = STAB_SP_FLOAT,
+ .sp.rates_f = *rates
+ };
+ return sp;
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.h b/sw/airborne/firmwares/rotorcraft/stabilization.h
index 063c8dfad2..5718118a2b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization.h
@@ -29,10 +29,57 @@
#include "std.h"
#include "generated/airframe.h"
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
extern void stabilization_init(void);
extern void stabilization_filter_commands(void);
+/** Stabilization setpoint.
+ * Struture to store the desired attitude with different
+ * frames and representations
+ */
+struct StabilizationSetpoint {
+ enum {
+ STAB_SP_QUAT, ///< LTP to Body orientation in unit quaternion
+ STAB_SP_EULERS, ///< LTP to Body orientation in euler angles
+ STAB_SP_LTP, ///< banking and heading in LTP (NED) frame
+ STAB_SP_RATES ///< body rates
+ } type;
+ enum {
+ STAB_SP_INT,
+ STAB_SP_FLOAT
+ } format;
+ union {
+ struct Int32Quat quat_i;
+ struct FloatQuat quat_f;
+ struct Int32Eulers eulers_i;
+ struct FloatEulers eulers_f;
+ struct { struct Int32Vect2 vect; int32_t heading; } ltp_i;
+ struct { struct FloatVect2 vect; float heading; } ltp_f;
+ struct Int32Rates rates_i;
+ struct FloatRates rates_f;
+ } sp;
+};
+
+// helper convert functions
+extern struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp);
+extern struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp);
+extern struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp);
+extern struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp);
+extern struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp);
+extern struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp);
+
+// helper make functions
+extern struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat);
+extern struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat);
+extern struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers);
+extern struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers);
+extern struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading);
+extern struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading);
+extern struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates);
+extern struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates);
+
/** Stabilization commands.
* Contains the resulting stabilization commands,
* regardless of whether rate or attitude is currently used.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
index f5bf533b92..74a2970cd3 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -41,6 +41,7 @@ extern void stabilization_attitude_enter(void);
extern void stabilization_attitude_set_failsafe_setpoint(void);
extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
+extern void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_attitude_run(bool in_flight);
#ifdef __cplusplus
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 775418d308..074bd9afd4 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
@@ -171,6 +171,11 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading);
}
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_f(sp);
+}
+
#define MAX_SUM_ERR 200
void stabilization_attitude_run(bool in_flight)
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 2667e065a2..110ff04a19 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -204,6 +204,11 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
stab_att_sp_euler.psi = heading;
}
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+}
+
#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))
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
index 67fc018921..9c2277e20e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_heli_indi.c
@@ -493,8 +493,8 @@ void stabilization_attitude_run(bool in_flight)
}
/* RADIO throttle stick value, for 4dof mode */
- //int32_t accel_z_sp = (-1)*3*((guidance_v_rc_delta_t - MAX_PPRZ/2) << INT32_ACCEL_FRAC) / (MAX_PPRZ/2);
- //accel_z_sp = ((accel_z_sp << INT32_TRIG_FRAC) / guidance_v_thrust_coeff);
+ //int32_t accel_z_sp = (-1)*3*((guidance_v.rc_delta_t - MAX_PPRZ/2) << INT32_ACCEL_FRAC) / (MAX_PPRZ/2);
+ //accel_z_sp = ((accel_z_sp << INT32_TRIG_FRAC) / guidance_v.thrust_coeff);
/* Transform yaw into a delta yaw while keeping filtered yawrate (kinda hacky) */
int32_t filtered_measurement_vector[INDI_DOF];
@@ -603,6 +603,12 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+ stab_att_sp_quat = stab_sp_to_quat_i(sp);
+}
+
void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)
{
struct FloatQuat q_sp;
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
index 1fa401d10f..92bc4d070c 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
@@ -99,3 +99,9 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.psi = heading;
}
+
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+}
+
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 f2a13e7d7b..c91c1f4c25 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
@@ -245,6 +245,12 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f);
}
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_f(sp);
+ stab_att_sp_quat = stab_sp_to_quat_f(sp);
+}
+
#ifndef GAIN_PRESCALER_FF
#define GAIN_PRESCALER_FF 1
#endif
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
index 4470630ce1..d56b6445f8 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_indi.c
@@ -60,6 +60,11 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
stabilization_indi_set_earth_cmd_i(cmd, heading);
}
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stabilization_indi_set_stab_sp(sp);
+}
+
void stabilization_attitude_run(bool in_flight)
{
stabilization_indi_attitude_run(stab_att_sp_quat, in_flight);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
index 2e78c4224b..718695af2e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -208,6 +208,12 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
+void stabilization_attitude_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+ stab_att_sp_quat = stab_sp_to_quat_i(sp);
+}
+
#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))
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c
index 5722231f54..0d0eb00471 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c
@@ -102,10 +102,10 @@ void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float
* dot(b,n) = 0
*/
float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
-
+
struct FloatVect3 b;
VECT3_ASSIGN(b, psi_vect.x, psi_vect.y, -dot/thrust_vect.z);
-
+
dot = VECT3_DOT_PRODUCT(b_x, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, b_x, b);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index f5d6c4be4a..9f50bc48f3 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -365,6 +365,17 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
+/**
+ * @brief Set attitude setpoint from stabilization setpoint struct
+ *
+ * @param sp Stabilization setpoint structure
+ */
+void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+ stab_att_sp_quat = stab_sp_to_quat_i(sp);
+}
+
/**
* @param att_err attitude error
* @param rate_control boolean that states if we are in rate control or attitude control
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
index d6bd6faa40..58ed1d2ead 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
@@ -23,6 +23,7 @@
#ifndef STABILIZATION_INDI
#define STABILIZATION_INDI
+#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
@@ -50,6 +51,7 @@ extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void);
extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
+extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
index 6384e81b7e..9e7ed8398f 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
@@ -279,6 +279,17 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
}
+/**
+ * @brief Set attitude setpoint from stabilization setpoint struct
+ *
+ * @param sp Stabilization setpoint structure
+ */
+void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp)
+{
+ stab_att_sp_euler = stab_sp_to_eulers_i(sp);
+ stab_att_sp_quat = stab_sp_to_quat_i(sp);
+}
+
/**
* @brief Update butterworth filter for p, q and r of a FloatRates struct
*
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
index 3d5af3292d..b00c2fe0ce 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
@@ -34,6 +34,7 @@
#ifndef STABILIZATION_INDI_SIMPLE_H
#define STABILIZATION_INDI_SIMPLE_H
+#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "filters/low_pass_filter.h"
@@ -85,6 +86,7 @@ extern void stabilization_indi_enter(void);
extern void stabilization_indi_set_failsafe_setpoint(void);
extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
+extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
diff --git a/sw/airborne/firmwares/rover/navigation.c b/sw/airborne/firmwares/rover/navigation.c
index ef36015c2b..125029b220 100644
--- a/sw/airborne/firmwares/rover/navigation.c
+++ b/sw/airborne/firmwares/rover/navigation.c
@@ -49,8 +49,6 @@ PRINT_CONFIG_VAR(NAVIGATION_FREQUENCY)
struct RoverNavigation nav;
-uint8_t last_wp __attribute__((unused));
-
const float max_dist_from_home = MAX_DIST_FROM_HOME;
const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME;
float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h
index 5d1e0f1840..3fd4602709 100644
--- a/sw/airborne/firmwares/rover/navigation.h
+++ b/sw/airborne/firmwares/rover/navigation.h
@@ -158,8 +158,6 @@ extern void nav_register_oval(nav_rover_oval_init nav_oval_init, nav_rover_oval
while (x >= 2*M_PI) x -= 2*M_PI; \
}
-extern uint8_t last_wp __attribute__((unused));
-
extern void nav_init(void);
extern void nav_run(void);
extern void nav_parse_BLOCK(uint8_t *buf);
diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c
index 8523a41459..d170d530dc 100644
--- a/sw/airborne/math/pprz_algebra_float.c
+++ b/sw/airborne/math/pprz_algebra_float.c
@@ -1012,17 +1012,17 @@ float float_mat_norm_li(float **a, int m, int n)
}
/* Scale a 3D vector to within a 2D bound */
-void vect_bound_in_2d(struct FloatVect3 *vect3, float bound) {
+void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound) {
float norm = FLOAT_VECT2_NORM(*vect3);
- if(norm>bound) {
- float scale = bound/norm;
+ if (norm > bound) {
+ float scale = bound / norm;
vect3->x *= scale;
vect3->y *= scale;
}
}
/* Scale a 3D vector to within a 3D bound */
-void vect_bound_in_3d(struct FloatVect3 *vect3, float bound) {
+void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound) {
float norm = FLOAT_VECT3_NORM(*vect3);
if(norm>bound) {
float scale = bound/norm;
@@ -1033,11 +1033,32 @@ void vect_bound_in_3d(struct FloatVect3 *vect3, float bound) {
}
/* Scale a 3D vector to a certain length in 2D */
-void vect_scale(struct FloatVect3 *vect3, float norm_des) {
+void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des) {
float norm = FLOAT_VECT2_NORM(*vect3);
- if(norm>0.1) {
- float scale = norm_des/norm;
+ if (norm > 0.01f) {
+ float scale = norm_des / norm;
vect3->x *= scale;
vect3->y *= scale;
}
}
+
+/* Scale a 2D vector to within a 2D bound */
+void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound) {
+ float norm = FLOAT_VECT2_NORM(*vect2);
+ if (norm > bound) {
+ float scale = bound / norm;
+ vect2->x *= scale;
+ vect2->y *= scale;
+ }
+}
+
+/* Scale a 2D vector to a certain length in 2D */
+void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des) {
+ float norm = FLOAT_VECT2_NORM(*vect2);
+ if (norm > 0.01f) {
+ float scale = norm_des / norm;
+ vect2->x *= scale;
+ vect2->y *= scale;
+ }
+}
+
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index fe63e50000..af6bf3a444 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -885,9 +885,11 @@ extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
extern bool float_mat_inv_4d(float invOut[16], float mat_in[16]);
-extern void vect_bound_in_2d(struct FloatVect3 *vect3, float bound);
-extern void vect_bound_in_3d(struct FloatVect3 *vect3, float bound);
-extern void vect_scale(struct FloatVect3 *vect3, float norm_des);
+extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
+extern void float_vect3_bound_in_3d(struct FloatVect3 *vect3, float bound);
+extern void float_vect3_scale_in_2d(struct FloatVect3 *vect3, float norm_des);
+extern void float_vect2_bound_in_2d(struct FloatVect2 *vect2, float bound);
+extern void float_vect2_scale_in_2d(struct FloatVect2 *vect2, float norm_des);
#ifdef __cplusplus
} /* extern "C" */
diff --git a/sw/airborne/modules/ctrl/approach_moving_target.c b/sw/airborne/modules/ctrl/approach_moving_target.c
index ac5786de3b..6459ceab2f 100644
--- a/sw/airborne/modules/ctrl/approach_moving_target.c
+++ b/sw/airborne/modules/ctrl/approach_moving_target.c
@@ -160,7 +160,7 @@ void follow_diagonal_approach(void) {
ref_relvel.z + target_vel.z + ec_vel.z,
};
- vect_bound_in_3d(&des_vel, 10.0);
+ float_vect3_bound_in_3d(&des_vel, 10.0);
// Bound vertical speed setpoint
if(stateGetAirspeed_f() > 13.0) {
diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.c b/sw/airborne/modules/ctrl/optical_flow_landing.c
index 3e1d7f54a4..4121f2e524 100644
--- a/sw/airborne/modules/ctrl/optical_flow_landing.c
+++ b/sw/airborne/modules/ctrl/optical_flow_landing.c
@@ -210,7 +210,7 @@ void vertical_ctrl_module_init(void)
of_landing_ctrl.previous_err = 0.;
of_landing_ctrl.sum_err = 0.0f;
of_landing_ctrl.d_err = 0.0f;
- of_landing_ctrl.nominal_thrust = (float)guidance_v_nominal_throttle / MAX_PPRZ; // copy this value from guidance
+ of_landing_ctrl.nominal_thrust = (float)guidance_v.nominal_throttle / MAX_PPRZ; // copy this value from guidance
of_landing_ctrl.VISION_METHOD = OFL_VISION_METHOD;
of_landing_ctrl.CONTROL_METHOD = OFL_CONTROL_METHOD;
of_landing_ctrl.COV_METHOD = OFL_COV_METHOD;
diff --git a/sw/airborne/modules/gps/gps_sim_hitl.c b/sw/airborne/modules/gps/gps_sim_hitl.c
index b7f5b390ea..e2648536cc 100644
--- a/sw/airborne/modules/gps/gps_sim_hitl.c
+++ b/sw/airborne/modules/gps/gps_sim_hitl.c
@@ -58,20 +58,20 @@ void gps_sim_hitl_event(void)
INT_VECT2_ZERO(guidance_h.ref.speed);
INT_VECT2_ZERO(guidance_h.ref.accel);
gv_set_ref(0, 0, 0);
- guidance_v_z_ref = 0;
- guidance_v_zd_ref = 0;
- guidance_v_zdd_ref = 0;
+ guidance_v.z_ref = 0;
+ guidance_v.zd_ref = 0;
+ guidance_v.zdd_ref = 0;
}
struct NedCoor_i ned_c;
ned_c.x = guidance_h.ref.pos.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ned_c.y = guidance_h.ref.pos.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
- ned_c.z = guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
+ ned_c.z = guidance_v.z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM;
ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c);
gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z;
gps.hmsl = state.ned_origin_i.hmsl - ned_c.z;
ned_c.x = guidance_h.ref.speed.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ned_c.y = guidance_h.ref.speed.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
- ned_c.z = guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
+ ned_c.z = guidance_v.zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM;
ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c);
gps.fix = GPS_FIX_3D;
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
diff --git a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h
index 26c1e91a43..79283711a3 100644
--- a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h
+++ b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h
@@ -31,10 +31,10 @@
#include "firmwares/fixedwing/nav.h"
#include "modules/nav/common_nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#define gvf_setNavMode(_navMode) (horizontal_mode = _navMode)
-#define GVF_MODE_ROUTE HORIZONTAL_MODE_ROUTE
-#define GVF_MODE_WAYPOINT HORIZONTAL_MODE_WAYPOINT
-#define GVF_MODE_CIRCLE HORIZONTAL_MODE_CIRCLE
+#define gvf_setNavMode(_navMode) (nav.horizontal_mode = _navMode)
+#define GVF_MODE_ROUTE NAV_HORIZONTAL_MODE_ROUTE
+#define GVF_MODE_WAYPOINT NAV_HORIZONTAL_MODE_WAYPOINT
+#define GVF_MODE_CIRCLE NAV_HORIZONTAL_MODE_CIRCLE
#elif defined(ROVER_FIRMWARE)
#include "state.h"
diff --git a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
index be830ee46d..8d646956ec 100644
--- a/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
+++ b/sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
@@ -355,7 +355,7 @@ bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, f
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
{
- horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
// Safety first! If the asked altitude is low
if (zl > zh) {
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 7861e1e864..595cb81481 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -725,7 +725,7 @@ static void ins_ekf2_publish_attitude(uint32_t stamp)
#endif
guidance_h.sp.heading += psi;
guidance_h.rc_sp.psi += psi;
- nav_heading += ANGLE_BFP_OF_REAL(psi);
+ nav.heading += psi;
guidance_h_read_rc(autopilot_in_flight());
stabilization_attitude_enter();
ekf2.quat_reset_counter = quat_reset_counter;
diff --git a/sw/airborne/modules/mission/mission_common.c b/sw/airborne/modules/mission/mission_common.c
index 3c9b0de32d..574561a6b8 100644
--- a/sw/airborne/modules/mission/mission_common.c
+++ b/sw/airborne/modules/mission/mission_common.c
@@ -45,14 +45,10 @@ void mission_init(void)
mission.current_idx = 0;
mission.element_time = 0.;
- // FIXME
- // we have no guarantee that nav modules init are called after mission_init
- // this would erase the already registered elements
- // for now, rely on the static initialization
- //for (int i = 0; i < MISSION_REGISTER_NB; i++) {
- // mission.registered[i].cb = NULL;
- // memset(mission.registered[i].type, '\0', MISSION_TYPE_SIZE);
- //}
+ for (int i = 0; i < MISSION_REGISTER_NB; i++) {
+ mission.registered[i].cb = NULL;
+ memset(mission.registered[i].type, '\0', MISSION_TYPE_SIZE);
+ }
}
@@ -68,9 +64,6 @@ bool mission_insert(enum MissionInsertMode insert, struct _mission_element *elem
}
#endif
- // convert element if needed, return FALSE if failed
- if (!mission_element_convert(element)) { return false; }
-
switch (insert) {
case Append:
tmp = (mission.insert_idx + 1) % MISSION_ELEMENT_NB;
@@ -154,10 +147,6 @@ struct _mission_element *mission_get_from_index(uint8_t index)
return NULL; // not found
}
-// Weak implementation of mission_element_convert (leave element unchanged)
-bool __attribute__((weak)) mission_element_convert(struct _mission_element *el __attribute__((unused))) { return true; }
-
-
// Get element
struct _mission_element *mission_get(void)
{
@@ -200,9 +189,9 @@ int mission_parse_GOTO_WP(uint8_t *buf)
struct _mission_element me;
me.type = MissionWP;
- me.element.mission_wp.wp.wp_f.x = DL_MISSION_GOTO_WP_wp_east(buf);
- me.element.mission_wp.wp.wp_f.y = DL_MISSION_GOTO_WP_wp_north(buf);
- me.element.mission_wp.wp.wp_f.z = DL_MISSION_GOTO_WP_wp_alt(buf);
+ me.element.mission_wp.wp.x = DL_MISSION_GOTO_WP_wp_east(buf);
+ me.element.mission_wp.wp.y = DL_MISSION_GOTO_WP_wp_north(buf);
+ me.element.mission_wp.wp.z = DL_MISSION_GOTO_WP_wp_alt(buf);
me.duration = DL_MISSION_GOTO_WP_duration(buf);
me.index = DL_MISSION_GOTO_WP_index(buf);
@@ -223,7 +212,7 @@ int mission_parse_GOTO_WP_LLA(uint8_t *buf)
struct _mission_element me;
me.type = MissionWP;
// if there is no valid local coordinate, do not insert mission element
- if (!mission_point_of_lla(&me.element.mission_wp.wp.wp_f, &lla)) { return false; }
+ if (!mission_point_of_lla(&me.element.mission_wp.wp, &lla)) { return false; }
me.duration = DL_MISSION_GOTO_WP_LLA_duration(buf);
me.index = DL_MISSION_GOTO_WP_LLA_index(buf);
@@ -238,9 +227,9 @@ int mission_parse_CIRCLE(uint8_t *buf)
struct _mission_element me;
me.type = MissionCircle;
- me.element.mission_circle.center.center_f.x = DL_MISSION_CIRCLE_center_east(buf);
- me.element.mission_circle.center.center_f.y = DL_MISSION_CIRCLE_center_north(buf);
- me.element.mission_circle.center.center_f.z = DL_MISSION_CIRCLE_center_alt(buf);
+ me.element.mission_circle.center.x = DL_MISSION_CIRCLE_center_east(buf);
+ me.element.mission_circle.center.y = DL_MISSION_CIRCLE_center_north(buf);
+ me.element.mission_circle.center.z = DL_MISSION_CIRCLE_center_alt(buf);
me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(buf);
me.duration = DL_MISSION_CIRCLE_duration(buf);
me.index = DL_MISSION_CIRCLE_index(buf);
@@ -262,7 +251,7 @@ int mission_parse_CIRCLE_LLA(uint8_t *buf)
struct _mission_element me;
me.type = MissionCircle;
// if there is no valid local coordinate, do not insert mission element
- if (!mission_point_of_lla(&me.element.mission_circle.center.center_f, &lla)) { return false; }
+ if (!mission_point_of_lla(&me.element.mission_circle.center, &lla)) { return false; }
me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(buf);
me.duration = DL_MISSION_CIRCLE_LLA_duration(buf);
me.index = DL_MISSION_CIRCLE_LLA_index(buf);
@@ -278,12 +267,12 @@ int mission_parse_SEGMENT(uint8_t *buf)
struct _mission_element me;
me.type = MissionSegment;
- me.element.mission_segment.from.from_f.x = DL_MISSION_SEGMENT_segment_east_1(buf);
- me.element.mission_segment.from.from_f.y = DL_MISSION_SEGMENT_segment_north_1(buf);
- me.element.mission_segment.from.from_f.z = DL_MISSION_SEGMENT_segment_alt(buf);
- me.element.mission_segment.to.to_f.x = DL_MISSION_SEGMENT_segment_east_2(buf);
- me.element.mission_segment.to.to_f.y = DL_MISSION_SEGMENT_segment_north_2(buf);
- me.element.mission_segment.to.to_f.z = DL_MISSION_SEGMENT_segment_alt(buf);
+ me.element.mission_segment.from.x = DL_MISSION_SEGMENT_segment_east_1(buf);
+ me.element.mission_segment.from.y = DL_MISSION_SEGMENT_segment_north_1(buf);
+ me.element.mission_segment.from.z = DL_MISSION_SEGMENT_segment_alt(buf);
+ me.element.mission_segment.to.x = DL_MISSION_SEGMENT_segment_east_2(buf);
+ me.element.mission_segment.to.y = DL_MISSION_SEGMENT_segment_north_2(buf);
+ me.element.mission_segment.to.z = DL_MISSION_SEGMENT_segment_alt(buf);
me.duration = DL_MISSION_SEGMENT_duration(buf);
me.index = DL_MISSION_SEGMENT_index(buf);
@@ -307,8 +296,8 @@ int mission_parse_SEGMENT_LLA(uint8_t *buf)
struct _mission_element me;
me.type = MissionSegment;
// if there is no valid local coordinate, do not insert mission element
- if (!mission_point_of_lla(&me.element.mission_segment.from.from_f, &from_lla)) { return false; }
- if (!mission_point_of_lla(&me.element.mission_segment.to.to_f, &to_lla)) { return false; }
+ if (!mission_point_of_lla(&me.element.mission_segment.from, &from_lla)) { return false; }
+ if (!mission_point_of_lla(&me.element.mission_segment.to, &to_lla)) { return false; }
me.duration = DL_MISSION_SEGMENT_LLA_duration(buf);
me.index = DL_MISSION_SEGMENT_LLA_index(buf);
@@ -323,21 +312,21 @@ int mission_parse_PATH(uint8_t *buf)
struct _mission_element me;
me.type = MissionPath;
- me.element.mission_path.path.path_f[0].x = DL_MISSION_PATH_point_east_1(buf);
- me.element.mission_path.path.path_f[0].y = DL_MISSION_PATH_point_north_1(buf);
- me.element.mission_path.path.path_f[0].z = DL_MISSION_PATH_path_alt(buf);
- me.element.mission_path.path.path_f[1].x = DL_MISSION_PATH_point_east_2(buf);
- me.element.mission_path.path.path_f[1].y = DL_MISSION_PATH_point_north_2(buf);
- me.element.mission_path.path.path_f[1].z = DL_MISSION_PATH_path_alt(buf);
- me.element.mission_path.path.path_f[2].x = DL_MISSION_PATH_point_east_3(buf);
- me.element.mission_path.path.path_f[2].y = DL_MISSION_PATH_point_north_3(buf);
- me.element.mission_path.path.path_f[2].z = DL_MISSION_PATH_path_alt(buf);
- me.element.mission_path.path.path_f[3].x = DL_MISSION_PATH_point_east_4(buf);
- me.element.mission_path.path.path_f[3].y = DL_MISSION_PATH_point_north_4(buf);
- me.element.mission_path.path.path_f[3].z = DL_MISSION_PATH_path_alt(buf);
- me.element.mission_path.path.path_f[4].x = DL_MISSION_PATH_point_east_5(buf);
- me.element.mission_path.path.path_f[4].y = DL_MISSION_PATH_point_north_5(buf);
- me.element.mission_path.path.path_f[4].z = DL_MISSION_PATH_path_alt(buf);
+ me.element.mission_path.path[0].x = DL_MISSION_PATH_point_east_1(buf);
+ me.element.mission_path.path[0].y = DL_MISSION_PATH_point_north_1(buf);
+ me.element.mission_path.path[0].z = DL_MISSION_PATH_path_alt(buf);
+ me.element.mission_path.path[1].x = DL_MISSION_PATH_point_east_2(buf);
+ me.element.mission_path.path[1].y = DL_MISSION_PATH_point_north_2(buf);
+ me.element.mission_path.path[1].z = DL_MISSION_PATH_path_alt(buf);
+ me.element.mission_path.path[2].x = DL_MISSION_PATH_point_east_3(buf);
+ me.element.mission_path.path[2].y = DL_MISSION_PATH_point_north_3(buf);
+ me.element.mission_path.path[2].z = DL_MISSION_PATH_path_alt(buf);
+ me.element.mission_path.path[3].x = DL_MISSION_PATH_point_east_4(buf);
+ me.element.mission_path.path[3].y = DL_MISSION_PATH_point_north_4(buf);
+ me.element.mission_path.path[3].z = DL_MISSION_PATH_path_alt(buf);
+ me.element.mission_path.path[4].x = DL_MISSION_PATH_point_east_5(buf);
+ me.element.mission_path.path[4].y = DL_MISSION_PATH_point_north_5(buf);
+ me.element.mission_path.path[4].z = DL_MISSION_PATH_path_alt(buf);
me.element.mission_path.nb = DL_MISSION_PATH_nb(buf);
if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
me.element.mission_path.path_idx = 0;
@@ -377,7 +366,7 @@ int mission_parse_PATH_LLA(uint8_t *buf)
if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
for (i = 0; i < me.element.mission_path.nb; i++) {
// if there is no valid local coordinate, do not insert mission element
- if (!mission_point_of_lla(&me.element.mission_path.path.path_f[i], &lla[i])) { return false; }
+ if (!mission_point_of_lla(&me.element.mission_path.path[i], &lla[i])) { return false; }
}
me.element.mission_path.path_idx = 0;
me.duration = DL_MISSION_PATH_LLA_duration(buf);
diff --git a/sw/airborne/modules/mission/mission_common.h b/sw/airborne/modules/mission/mission_common.h
index 44eede1722..f8f33ee77f 100644
--- a/sw/airborne/modules/mission/mission_common.h
+++ b/sw/airborne/modules/mission/mission_common.h
@@ -57,40 +57,22 @@ enum MissionRunFlag {
};
struct _mission_wp {
- union {
- struct EnuCoor_f wp_f;
- struct EnuCoor_i wp_i;
- } wp;
+ struct EnuCoor_f wp;
};
struct _mission_circle {
- union {
- struct EnuCoor_f center_f;
- struct EnuCoor_i center_i;
- } center;
-
+ struct EnuCoor_f center;
float radius;
};
struct _mission_segment {
- union {
- struct EnuCoor_f from_f;
- struct EnuCoor_i from_i;
- } from;
-
- union {
- struct EnuCoor_f to_f;
- struct EnuCoor_i to_i;
- } to;
+ struct EnuCoor_f from;
+ struct EnuCoor_f to;
};
#define MISSION_PATH_NB 5
struct _mission_path {
- union {
- struct EnuCoor_f path_f[MISSION_PATH_NB];
- struct EnuCoor_i path_i[MISSION_PATH_NB];
- } path;
-
+ struct EnuCoor_f path[MISSION_PATH_NB];
uint8_t path_idx;
uint8_t nb;
};
@@ -173,12 +155,6 @@ extern bool mission_insert(enum MissionInsertMode insert, struct _mission_elemen
*/
extern bool mission_register(mission_custom_cb cb, char *type);
-/** Convert mission element's points format if needed
- * @param el pointer to the mission element
- * @return return TRUE if conversion is succesful, FALSE otherwise
- */
-extern bool mission_element_convert(struct _mission_element *el);
-
/** Get current mission element
* @return return a pointer to the next mission element or NULL if no more elements
*/
diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c
index 630e8463c2..06004f5c02 100644
--- a/sw/airborne/modules/mission/mission_fw_nav.c
+++ b/sw/airborne/modules/mission/mission_fw_nav.c
@@ -64,23 +64,23 @@ bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
}
// navigation time step
-static const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
+static const float dt_navigation = 1.0f / ((float)NAVIGATION_FREQUENCY);
// dirty hack to comply with nav_approaching_xy function
-struct EnuCoor_f last_wp_f = { 0., 0., 0. };
+struct EnuCoor_f last_wp_f = { 0.f, 0.f, 0.f };
/** Navigation function to a single waypoint
*/
static inline bool mission_nav_wp(struct _mission_wp *wp)
{
- if (nav_approaching_xy(wp->wp.wp_f.x, wp->wp.wp_f.y, last_wp_f.x, last_wp_f.y, CARROT)) {
- last_wp_f = wp->wp.wp_f; // store last wp
+ if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp_f.x, last_wp_f.y, CARROT)) {
+ last_wp_f = wp->wp; // store last wp
return false; // end of mission element
}
// set navigation command
- fly_to_xy(wp->wp.wp_f.x, wp->wp.wp_f.y);
- NavVerticalAutoThrottleMode(0.);
- NavVerticalAltitudeMode(wp->wp.wp_f.z, 0.);
+ fly_to_xy(wp->wp.x, wp->wp.y);
+ NavVerticalAutoThrottleMode(0.f);
+ NavVerticalAltitudeMode(wp->wp.z, 0.f);
return true;
}
@@ -88,9 +88,9 @@ static inline bool mission_nav_wp(struct _mission_wp *wp)
*/
static inline bool mission_nav_circle(struct _mission_circle *circle)
{
- nav_circle_XY(circle->center.center_f.x, circle->center.center_f.y, circle->radius);
- NavVerticalAutoThrottleMode(0.);
- NavVerticalAltitudeMode(circle->center.center_f.z, 0.);
+ nav_circle_XY(circle->center.x, circle->center.y, circle->radius);
+ NavVerticalAutoThrottleMode(0.f);
+ NavVerticalAltitudeMode(circle->center.z, 0.f);
return true;
}
@@ -98,14 +98,13 @@ static inline bool mission_nav_circle(struct _mission_circle *circle)
*/
static inline bool mission_nav_segment(struct _mission_segment *segment)
{
- if (nav_approaching_xy(segment->to.to_f.x, segment->to.to_f.y, segment->from.from_f.x, segment->from.from_f.y,
- CARROT)) {
- last_wp_f = segment->to.to_f;
+ if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) {
+ last_wp_f = segment->to;
return false; // end of mission element
}
- nav_route_xy(segment->from.from_f.x, segment->from.from_f.y, segment->to.to_f.x, segment->to.to_f.y);
- NavVerticalAutoThrottleMode(0.);
- NavVerticalAltitudeMode(segment->to.to_f.z, 0.); // both altitude should be the same anyway
+ nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y);
+ NavVerticalAutoThrottleMode(0.f);
+ NavVerticalAltitudeMode(segment->to.z, 0.f); // both altitude should be the same anyway
return true;
}
@@ -119,20 +118,20 @@ static inline bool mission_nav_path(struct _mission_path *path)
if (path->nb == 1) {
// handle as a single waypoint
struct _mission_wp wp;
- wp.wp.wp_f = path->path.path_f[0];
+ wp.wp = path->path[0];
return mission_nav_wp(&wp);
}
if (path->path_idx == path->nb - 1) {
- last_wp_f = path->path.path_f[path->path_idx]; // store last wp
+ last_wp_f = path->path[path->path_idx]; // store last wp
return false; // end of path
}
// normal case
- struct EnuCoor_f from_f = path->path.path_f[path->path_idx];
- struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1];
- nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y);
- NavVerticalAutoThrottleMode(0.);
- NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway
- if (nav_approaching_xy(to_f.x, to_f.y, from_f.x, from_f.y, CARROT)) {
+ struct EnuCoor_f from = path->path[path->path_idx];
+ struct EnuCoor_f to = path->path[path->path_idx + 1];
+ nav_route_xy(from.x, from.y, to.x, to.y);
+ NavVerticalAutoThrottleMode(0.f);
+ NavVerticalAltitudeMode(to.z, 0.f); // both altitude should be the same anyway
+ if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)) {
path->path_idx++; // go to next segment
}
return true;
@@ -162,8 +161,8 @@ static float mission_wait_time = 0.f;
static struct _mission_circle mission_wait_circle;
static bool mission_wait_pattern(void) {
if (!mission_wait_started) {
- mission_wait_circle.center.center_f = *stateGetPositionEnu_f();
- mission_wait_circle.center.center_f.z += GetAltRef();
+ mission_wait_circle.center = *stateGetPositionEnu_f();
+ mission_wait_circle.center.z += GetAltRef();
mission_wait_circle.radius = DEFAULT_CIRCLE_RADIUS;
mission_wait_time = 0.f;
mission_wait_started = true;
@@ -213,12 +212,13 @@ int mission_run()
mission.element_time += dt_navigation;
// test if element is finished or element time is elapsed
- if (!el_running || (el->duration > 0. && mission.element_time >= el->duration)) {
+ if (!el_running || (el->duration > 0.f && mission.element_time >= el->duration)) {
// reset timer
- mission.element_time = 0.;
+ mission.element_time = 0.f;
// go to next element
mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
}
return true;
}
+
diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
index 54e2747a2a..20fda48709 100644
--- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c
+++ b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
@@ -75,63 +75,31 @@ bool mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
return true;
}
-//Function that converts target wp from float point versions to int
-bool mission_element_convert(struct _mission_element *el)
-{
- struct _mission_element tmp_element = *el;
- uint8_t i = 0;
-
- switch (tmp_element.type) {
- case MissionWP:
- ENU_BFP_OF_REAL(el->element.mission_wp.wp.wp_i, tmp_element.element.mission_wp.wp.wp_f);
- break;
- case MissionCircle:
- ENU_BFP_OF_REAL(el->element.mission_circle.center.center_i, tmp_element.element.mission_circle.center.center_f);
- break;
- case MissionSegment:
- ENU_BFP_OF_REAL(el->element.mission_segment.from.from_i, tmp_element.element.mission_segment.from.from_f);
- ENU_BFP_OF_REAL(el->element.mission_segment.to.to_i, tmp_element.element.mission_segment.to.to_f);
- break;
- case MissionPath:
- for (i = 0; i < 5; i++) {
- ENU_BFP_OF_REAL(el->element.mission_path.path.path_i[i], tmp_element.element.mission_path.path.path_f[i]);
- }
- break;
- default:
- // invalid element type
- return false;
- break;
- }
-
- return true;
-}
-
// navigation time step
-static const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
+static const float dt_navigation = 1.0f / ((float)NAVIGATION_FREQUENCY);
// last_mission_wp, last target wp from mission elements, not used actively and kept for future implementations
-struct EnuCoor_i last_mission_wp = { 0., 0., 0. };
+struct EnuCoor_f last_mission_wp = { 0.f, 0.f, 0.f };
/** Navigation function to a single waypoint
*/
static inline bool mission_nav_wp(struct _mission_element *el)
{
- struct EnuCoor_i *target_wp = &(el->element.mission_wp.wp.wp_i);
+ struct EnuCoor_f *target_wp = &(el->element.mission_wp.wp);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
- if (nav_approaching_from(target_wp, NULL, CARROT)) {
+ if (nav.nav_approaching(target_wp, NULL, CARROT)) {
last_mission_wp = *target_wp;
- if (el->duration > 0.) {
+ if (el->duration > 0.f) {
if (nav_check_wp_time(target_wp, el->duration)) { return false; }
} else { return false; }
}
//Go to Mission Waypoint
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
- VECT3_COPY(navigation_target, *target_wp);
- NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
- NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(target_wp->z), 0.);
+ nav.nav_goto(target_wp);
+ NavVerticalAutoThrottleMode(RadOfDeg(0.f));
+ NavVerticalAltitudeMode(target_wp->z, 0.f);
return true;
}
@@ -140,16 +108,15 @@ static inline bool mission_nav_wp(struct _mission_element *el)
*/
static inline bool mission_nav_circle(struct _mission_element *el)
{
- struct EnuCoor_i *center_wp = &(el->element.mission_circle.center.center_i);
- int32_t radius = el->element.mission_circle.radius;
+ struct EnuCoor_f *center_wp = &(el->element.mission_circle.center);
+ float radius = el->element.mission_circle.radius;
//Draw the desired circle for a 'duration' time
- horizontal_mode = HORIZONTAL_MODE_CIRCLE;
- nav_circle(center_wp, POS_BFP_OF_REAL(radius));
- NavVerticalAutoThrottleMode(RadOfDeg(0.0));
- NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(center_wp->z), 0.);
+ nav.nav_circle(center_wp, radius);
+ NavVerticalAutoThrottleMode(RadOfDeg(0.f));
+ NavVerticalAltitudeMode(center_wp->z, 0.f);
- if (el->duration > 0. && mission.element_time >= el->duration) {
+ if (el->duration > 0.f && mission.element_time >= el->duration) {
return false;
}
@@ -160,23 +127,22 @@ static inline bool mission_nav_circle(struct _mission_element *el)
*/
static inline bool mission_nav_segment(struct _mission_element *el)
{
- struct EnuCoor_i *from_wp = &(el->element.mission_segment.from.from_i);
- struct EnuCoor_i *to_wp = &(el->element.mission_segment.to.to_i);
+ struct EnuCoor_f *from_wp = &(el->element.mission_segment.from);
+ struct EnuCoor_f *to_wp = &(el->element.mission_segment.to);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
- if (nav_approaching_from(to_wp, from_wp, CARROT)) {
+ if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
- if (el->duration > 0.) {
+ if (el->duration > 0.f) {
if (nav_check_wp_time(to_wp, el->duration)) { return false; }
} else { return false; }
}
//Route Between from-to
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(from_wp, to_wp);
- NavVerticalAutoThrottleMode(RadOfDeg(0.0));
- NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(to_wp->z), 0.);
+ nav.nav_route(from_wp, to_wp);
+ NavVerticalAutoThrottleMode(RadOfDeg(0.f));
+ NavVerticalAltitudeMode(to_wp->z, 0.f);
return true;
}
@@ -191,30 +157,29 @@ static inline bool mission_nav_path(struct _mission_element *el)
}
if (el->element.mission_path.path_idx == 0) { //first wp of path
- el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0];
+ el->element.mission_wp.wp = el->element.mission_path.path[0];
if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; }
}
else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path
- struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]);
- struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]);
+ struct EnuCoor_f *from_wp = &(el->element.mission_path.path[(el->element.mission_path.path_idx) - 1]);
+ struct EnuCoor_f *to_wp = &(el->element.mission_path.path[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
- if (nav_approaching_from(to_wp, from_wp, CARROT)) {
+ if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
last_mission_wp = *to_wp;
- if (el->duration > 0.) {
+ if (el->duration > 0.f) {
if (nav_check_wp_time(to_wp, el->duration)) {
el->element.mission_path.path_idx++;
}
} else { el->element.mission_path.path_idx++; }
}
//Route Between from-to
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(from_wp, to_wp);
- NavVerticalAutoThrottleMode(RadOfDeg(0.0));
- NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.);
+ nav.nav_route(from_wp, to_wp);
+ NavVerticalAutoThrottleMode(RadOfDeg(0.f));
+ NavVerticalAltitudeMode(from_wp->z, 0.f);
} else { return false; } //end of path
return true;
@@ -244,7 +209,7 @@ static float mission_wait_time = 0.f;
static struct _mission_element mission_wait_wp;
static bool mission_wait_pattern(void) {
if (!mission_wait_started) {
- mission_wait_wp.element.mission_wp.wp.wp_i = *stateGetPositionEnu_i();
+ mission_wait_wp.element.mission_wp.wp = *stateGetPositionEnu_f();
mission_wait_time = 0.f;
mission_wait_started = true;
}
diff --git a/sw/airborne/modules/nav/common_flight_plan.c b/sw/airborne/modules/nav/common_flight_plan.c
index 00710d144e..b37b29ce01 100644
--- a/sw/airborne/modules/nav/common_flight_plan.c
+++ b/sw/airborne/modules/nav/common_flight_plan.c
@@ -28,7 +28,6 @@
#include "generated/flight_plan.h"
-
/** In s */
uint16_t stage_time, block_time;
@@ -36,6 +35,7 @@ uint8_t nav_stage, nav_block;
/** To save the current block/stage to enable return */
uint8_t last_block, last_stage;
+uint8_t last_wp UNUSED;
void nav_init_block(void)
@@ -57,3 +57,4 @@ void nav_goto_block(uint8_t b)
nav_block = b;
nav_init_block();
}
+
diff --git a/sw/airborne/modules/nav/common_flight_plan.h b/sw/airborne/modules/nav/common_flight_plan.h
index 665b7aefd9..dab080adc6 100644
--- a/sw/airborne/modules/nav/common_flight_plan.h
+++ b/sw/airborne/modules/nav/common_flight_plan.h
@@ -34,6 +34,7 @@ extern uint16_t stage_time, block_time;
extern uint8_t nav_stage, nav_block;
extern uint8_t last_block, last_stage;
+extern uint8_t last_wp __attribute__((unused));
/** needs to be implemented by fixedwing and rotorcraft seperately */
void nav_init_stage(void);
diff --git a/sw/airborne/modules/nav/nav_base.h b/sw/airborne/modules/nav/nav_base.h
new file mode 100644
index 0000000000..c1868bd07c
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_base.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2022 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_base.h"
+ * @author 2018 Gautier Hattenberger
+ * Basic navigation structure and definition
+ */
+
+#ifndef NAV_BASE_H
+#define NAV_BASE_H
+
+#include "std.h"
+#include "math/pprz_geodetic_float.h"
+
+/** Waypoint and route pattern
+ */
+struct NavGoto_t {
+ struct EnuCoor_f from; ///< start WP position
+ struct EnuCoor_f to; ///< end WP position
+ float dist2_to_wp; ///< squared distance to next waypoint
+ float leg_progress; ///< progress over leg
+ float leg_length; ///< leg length
+};
+
+/** Circle pattern
+ */
+struct NavCircle_t {
+ struct EnuCoor_f center; ///< center WP position
+ float radius; ///< radius in meters
+ float qdr; ///< qdr in radians
+ float radians; ///< incremental angular distance
+};
+
+/** Oval pattern
+ */
+enum oval_status { OR12, OC2, OR21, OC1 };
+struct NavOval_t {
+ enum oval_status status; ///< oval status
+ uint8_t count; ///< number of laps
+};
+
+/** Basic Nav struct
+ */
+struct NavBase_t {
+ struct NavGoto_t goto_wp;
+ struct NavCircle_t circle;
+ struct NavOval_t oval;
+};
+
+/** helper functions
+ */
+static inline float nav_circle_get_count(struct NavCircle_t *circle)
+{
+ return fabsf(circle->radians) / (2.f*M_PI);
+}
+
+static inline float nav_circle_qdr(struct NavCircle_t *circle)
+{
+ float qdr = DegOfRad(M_PI_2 - circle->qdr);
+ NormCourse(qdr);
+ return qdr;
+}
+
+
+#endif
+
diff --git a/sw/airborne/modules/nav/nav_heli_spinup.c b/sw/airborne/modules/nav/nav_heli_spinup.c
index 3488b8c81e..1c1071677b 100644
--- a/sw/airborne/modules/nav/nav_heli_spinup.c
+++ b/sw/airborne/modules/nav/nav_heli_spinup.c
@@ -41,12 +41,12 @@ void nav_heli_spinup_setup(uint16_t duration, float throttle)
nav_heli_spinup.duration = (duration > 0) ? duration : 1;
nav_heli_spinup.throttle = throttle * MAX_PPRZ;
- nav_throttle = 0;
- nav_cmd_roll = 0;
- nav_cmd_pitch = 0;
- nav_cmd_yaw = 0;
- horizontal_mode = HORIZONTAL_MODE_ATTITUDE;
- vertical_mode = VERTICAL_MODE_MANUAL;
+ nav.throttle = 0;
+ nav.cmd_roll = 0;
+ nav.cmd_pitch = 0;
+ nav.cmd_yaw = 0;
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_ATTITUDE;
+ nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL;
}
/**
@@ -59,11 +59,11 @@ bool nav_heli_spinup_run(void)
return false;
}
- nav_cmd_roll = 0;
- nav_cmd_pitch = 0;
- nav_cmd_yaw = 0;
- horizontal_mode = HORIZONTAL_MODE_MANUAL;
- vertical_mode = VERTICAL_MODE_MANUAL;
- nav_throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;
+ nav.cmd_roll = 0;
+ nav.cmd_pitch = 0;
+ nav.cmd_yaw = 0;
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_MANUAL;
+ nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL;
+ nav.throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;
return true;
}
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.c b/sw/airborne/modules/nav/nav_rotorcraft_base.c
new file mode 100644
index 0000000000..aed98796ef
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_rotorcraft_base.c
@@ -0,0 +1,314 @@
+/*
+ * Copyright (C) 2022 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_rotorcraft_base.c"
+ * @author 2018 Gautier Hattenberger
+ * Basic navigation functions for Rotorcraft
+ */
+
+#include "modules/nav/nav_rotorcraft_base.h"
+#include "firmwares/rotorcraft/navigation.h"
+
+struct NavBase_t nav_rotorcraft_base;
+
+/** Implement basic nav function
+ */
+static void nav_stage_init(void)
+{
+ nav_rotorcraft_base.circle.radians = 0.f;
+ nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
+ nav_rotorcraft_base.oval.count = 0;
+}
+
+static void nav_goto(struct EnuCoor_f *wp)
+{
+ nav_rotorcraft_base.goto_wp.to = *wp;
+ nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
+ VECT2_COPY(nav.target, *wp);
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
+}
+
+static void nav_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
+{
+ struct FloatVect2 wp_diff, pos_diff;
+ VECT2_DIFF(wp_diff, *wp_end, *wp_start);
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_start);
+ // leg length
+ float leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y), 0.1f);
+ nav_rotorcraft_base.goto_wp.leg_length = sqrtf(leg_length2);
+ // leg progress
+ nav_rotorcraft_base.goto_wp.leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_rotorcraft_base.goto_wp.leg_length;
+ nav_rotorcraft_base.goto_wp.leg_progress += Max(NAV_CARROT_DIST, 0.f);
+ // next pos on leg
+ struct FloatVect2 progress_pos;
+ VECT2_SMUL(progress_pos, wp_diff, nav_rotorcraft_base.goto_wp.leg_progress / nav_rotorcraft_base.goto_wp.leg_length);
+ VECT2_SUM(nav.target, *wp_start, progress_pos);
+
+ nav_rotorcraft_base.goto_wp.from = *wp_start;
+ nav_rotorcraft_base.goto_wp.to = *wp_end;
+ nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_ROUTE;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
+}
+
+static bool nav_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
+{
+ float dist_to_point;
+ struct FloatVect2 diff;
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+
+ /* if an approaching_time is given, estimate diff after approching_time secs */
+ if (approaching_time > 0.f) {
+ struct FloatVect2 estimated_pos;
+ struct FloatVect2 estimated_progress;
+ struct EnuCoor_f *speed = stateGetSpeedEnu_f();
+ VECT2_SMUL(estimated_progress, *speed, approaching_time);
+ VECT2_SUM(estimated_pos, *pos, estimated_progress);
+ VECT2_DIFF(diff, *wp, estimated_pos);
+ }
+ /* else use current position */
+ else {
+ VECT2_DIFF(diff, *wp, *pos);
+ }
+ /* compute distance of estimated/current pos to target wp
+ */
+ dist_to_point = float_vect2_norm(&diff);
+
+ /* return TRUE if we have arrived */
+ if (dist_to_point < ARRIVED_AT_WAYPOINT) {
+ return true;
+ }
+
+ /* if coming from a valid waypoint */
+ if (from != NULL) {
+ /* return TRUE if normal line at the end of the segment is crossed */
+ struct FloatVect2 from_diff;
+ VECT2_DIFF(from_diff, *wp, *from);
+ return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
+ }
+
+ return false;
+}
+
+static void nav_circle(struct EnuCoor_f *wp_center, float radius)
+{
+ struct FloatVect2 pos_diff;
+
+ if (fabsf(radius) < 0.001f) {
+ VECT2_COPY(nav.target, *wp_center);
+ } else {
+ VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center);
+ // store last qdr
+ float last_qdr = nav_rotorcraft_base.circle.qdr;
+ // compute qdr
+ nav_rotorcraft_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x);
+ // increment circle radians
+ float trigo_diff = nav_rotorcraft_base.circle.qdr - last_qdr;
+ NormRadAngle(trigo_diff);
+ nav_rotorcraft_base.circle.radians += trigo_diff;
+ }
+
+ // direction of rotation
+ float sign_radius = radius > 0.f ? 1.f : -1.f;
+ // absolute radius
+ float abs_radius = fabsf(radius);
+ if (abs_radius > 0.1f) {
+ // carrot_angle
+ float carrot_angle = NAV_CARROT_DIST / abs_radius;
+ carrot_angle = Min(carrot_angle, M_PI / 4);
+ carrot_angle = Max(carrot_angle, M_PI / 16);
+ carrot_angle = nav_rotorcraft_base.circle.qdr - sign_radius * carrot_angle;
+ // compute setpoint
+ VECT2_ASSIGN(pos_diff, abs_radius * cosf(carrot_angle), abs_radius * sinf(carrot_angle));
+ VECT2_SUM(nav.target, *wp_center, pos_diff);
+ }
+ else {
+ // radius is too small, direct to center
+ VECT2_COPY(nav.target, *wp_center);
+ }
+
+ nav_rotorcraft_base.circle.center = *wp_center;
+ nav_rotorcraft_base.circle.radius = radius;
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
+}
+
+/**
+ * Navigation along a figure O. One side leg is defined by waypoints [p1] and [p2].
+ * The navigation goes through 4 states: OC1 (half circle next to [p1]),
+ * OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).
+ * Initial state is the route along the desired segment (OC2).
+ */
+#ifndef LINE_START_FUNCTION
+#define LINE_START_FUNCTION {}
+#endif
+#ifndef LINE_STOP_FUNCTION
+#define LINE_STOP_FUNCTION {}
+#endif
+
+static void _nav_oval_init(void)
+{
+ nav_rotorcraft_base.oval.status = OC2;
+ nav_rotorcraft_base.oval.count = 0;
+}
+
+static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
+{
+ float alt = wp1->z;
+ wp2->z = alt;
+
+ /* Unit vector from p1 to p2 */
+ struct FloatVect2 dir;
+ VECT2_DIFF(dir, *wp1, *wp2);
+ float_vect2_normalize(&dir);
+
+ /* The half circle centers and the other leg */
+ struct EnuCoor_f p1_center = {
+ wp1->x + radius * dir.y,
+ wp1->y - radius * dir.x,
+ alt
+ };
+ struct EnuCoor_f p1_out = {
+ wp1->x + 2.f * radius * dir.y,
+ wp1->y - 2.f * radius * dir.x,
+ alt
+ };
+ struct EnuCoor_f p2_in = {
+ wp2->x + 2.f * radius * dir.y,
+ wp2->y - 2.f * radius * dir.x,
+ alt
+ };
+ struct EnuCoor_f p2_center = {
+ wp2->x + radius * dir.y,
+ wp2->y - radius * dir.x,
+ alt
+ };
+
+ float qdr_out_2 = M_PI - atan2f(dir.y, dir.x);
+ float qdr_out_1 = qdr_out_2 + M_PI;
+ if (radius > 0.f) {
+ qdr_out_2 += M_PI;
+ qdr_out_1 += M_PI;
+ }
+ float qdr_anticipation = (radius > 0.f ? 15.f : -15.f);
+
+ switch (nav_rotorcraft_base.oval.status) {
+ case OC1 :
+ nav_circle(&p1_center, radius);
+ if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
+ nav_rotorcraft_base.oval.status = OR12;
+ InitStage();
+ LINE_START_FUNCTION;
+ }
+ return;
+
+ case OR12:
+ nav_route(&p1_out, &p2_in);
+ if (nav_approaching(&p2_in, &p1_out, CARROT)) {
+ nav_rotorcraft_base.oval.status = OC2;
+ nav_rotorcraft_base.oval.count++;
+ InitStage();
+ LINE_STOP_FUNCTION;
+ }
+ return;
+
+ case OC2 :
+ nav_circle(&p2_center, radius);
+ if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
+ nav_rotorcraft_base.oval.status = OR21;
+ InitStage();
+ LINE_START_FUNCTION;
+ }
+ return;
+
+ case OR21:
+ nav_route(wp2, wp1);
+ if (nav_approaching(wp1, wp2, CARROT)) {
+ nav_rotorcraft_base.oval.status = OC1;
+ InitStage();
+ LINE_STOP_FUNCTION;
+ }
+ return;
+
+ default: /* Should not occur !!! Doing nothing */
+ return;
+ }
+}
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void send_segment(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_SEGMENT(trans, dev, AC_ID,
+ &nav_rotorcraft_base.goto_wp.from.x,
+ &nav_rotorcraft_base.goto_wp.from.y,
+ &nav_rotorcraft_base.goto_wp.to.x,
+ &nav_rotorcraft_base.goto_wp.to.y);
+}
+
+static void send_circle(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_CIRCLE(trans, dev, AC_ID,
+ &nav_rotorcraft_base.circle.center.x,
+ &nav_rotorcraft_base.circle.center.y,
+ &nav_rotorcraft_base.circle.radius);
+}
+
+static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
+{
+ float dist_home = sqrtf(nav.dist2_to_home);
+ float dist_wp = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp);
+ pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID,
+ &block_time, &stage_time,
+ &dist_home, &dist_wp,
+ &nav_block, &nav_stage,
+ &nav.horizontal_mode);
+ if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ROUTE) {
+ send_segment(trans, dev);
+ } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_CIRCLE) {
+ send_circle(trans, dev);
+ }
+}
+#endif
+
+/** Init and register nav functions
+ */
+void nav_rotorcraft_init(void)
+{
+ nav_rotorcraft_base.circle.radius = DEFAULT_CIRCLE_RADIUS;
+ nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
+ nav_rotorcraft_base.goto_wp.leg_length = 1.f;
+
+ // register nav functions
+ nav_register_stage_init(nav_stage_init);
+ nav_register_goto_wp(nav_goto, nav_route, nav_approaching);
+ nav_register_circle(nav_circle);
+ nav_register_oval(_nav_oval_init, nav_oval);
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_NAV_STATUS, send_nav_status);
+#endif
+
+}
+
+
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.h b/sw/airborne/modules/nav/nav_rotorcraft_base.h
new file mode 100644
index 0000000000..85ed7cfbe6
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_rotorcraft_base.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (C) 2022 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_rotorcraft_base.h"
+ * @author 2018 Gautier Hattenberger
+ * Basic navigation functions for Rotorcraft
+ */
+
+#ifndef NAV_ROTORCRAFT_BASE_H
+#define NAV_ROTORCRAFT_BASE_H
+
+#include "modules/nav/nav_base.h"
+
+/** Basic Nav struct
+ */
+extern struct NavBase_t nav_rotorcraft_base;
+
+extern void nav_rotorcraft_init(void);
+
+
+/** Macros for circle nav
+ */
+#define NavCircleCount() nav_circle_get_count(&nav_rotorcraft_base.circle)
+#define NavCircleQdr() nav_circle_qdr(&nav_rotorcraft_base.circle)
+
+/** True if x (in degrees) is close to the current QDR (less than 10 degrees)
+ */
+#define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
+#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f()))
+
+
+#endif
+
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
new file mode 100644
index 0000000000..68fbe6a2d4
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2023 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_rotorcraft_hybrid.c"
+ * Specific navigation functions for hybrid aircraft
+ */
+
+#include "modules/nav/nav_rotorcraft_hybrid.h"
+#include "firmwares/rotorcraft/navigation.h"
+#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" // strong dependency for now
+
+// Max ground speed that will be commanded
+#ifndef GUIDANCE_INDI_NAV_SPEED_MARGIN
+#define GUIDANCE_INDI_NAV_SPEED_MARGIN 10.0f
+#endif
+#define NAV_MAX_SPEED (GUIDANCE_INDI_MAX_AIRSPEED + GUIDANCE_INDI_NAV_SPEED_MARGIN)
+float nav_max_speed = NAV_MAX_SPEED;
+
+#ifndef MAX_DECELERATION
+#define MAX_DECELERATION 1.f
+#endif
+
+#ifdef GUIDANCE_INDI_LINE_GAIN
+static float guidance_indi_line_gain = GUIDANCE_INDI_LINE_GAIN;
+#else
+static float guidance_indi_line_gain = 1.0f;
+#endif
+
+#ifndef GUIDANCE_INDI_NAV_LINE_DIST
+#define GUIDANCE_INDI_NAV_LINE_DIST 50.f
+#endif
+
+/** Implement basic nav function for the hybrid case
+ */
+
+static void nav_hybrid_goto(struct EnuCoor_f *wp)
+{
+ nav_rotorcraft_base.goto_wp.to = *wp;
+ nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
+ VECT2_COPY(nav.target, *wp);
+
+ // Calculate position error
+ struct FloatVect2 pos_error;
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+ VECT2_DIFF(pos_error, nav.target, *pos);
+
+ struct FloatVect2 speed_sp;
+ VECT2_SMUL(speed_sp, pos_error, gih_params.pos_gain);
+
+ if (force_forward) {
+ float_vect2_scale_in_2d(&speed_sp, nav_max_speed);
+ } else {
+ // Calculate distance to waypoint
+ float dist_to_wp = float_vect2_norm(&pos_error);
+ // Calculate max speed to decelerate from
+ float max_speed_decel2 = fabsf(2.f * dist_to_wp * MAX_DECELERATION); // dist_to_wp can only be positive, but just in case
+ float max_speed_decel = sqrtf(max_speed_decel2);
+ // Bound the setpoint velocity vector
+ float max_h_speed = Min(nav_max_speed, max_speed_decel);
+ float_vect2_bound_in_2d(&speed_sp, max_h_speed);
+ }
+
+ VECT2_COPY(nav.speed, speed_sp);
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
+}
+
+static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
+{
+ struct FloatVect2 wp_diff, pos_diff;
+ VECT2_DIFF(wp_diff, *wp_end, *wp_start);
+ VECT2_DIFF(pos_diff, *wp_end, *stateGetPositionEnu_f());
+
+ // Calculate magnitude of the desired speed vector based on distance to waypoint
+ float dist_to_target = float_vect2_norm(&pos_diff);
+ float desired_speed;
+ if (force_forward) {
+ desired_speed = nav_max_speed;
+ } else {
+ desired_speed = dist_to_target * gih_params.pos_gain;
+ Bound(desired_speed, 0.0f, nav_max_speed);
+ }
+
+ // Calculate length of line segment
+ float length_line = Max(float_vect2_norm(&wp_diff), 0.01f);
+ //Normal vector to the line, with length of the line
+ struct FloatVect2 normalv;
+ VECT2_ASSIGN(normalv, -wp_diff.y, wp_diff.x);
+ // Length of normal vector is the same as of the line segment
+ float length_normalv = length_line; // >= 0.01
+ // Distance along the normal vector
+ float dist_to_line = (pos_diff.x * normalv.x + pos_diff.y * normalv.y) / length_normalv;
+ // Normal vector scaled to be the distance to the line
+ struct FloatVect2 v_to_line, v_along_line;
+ v_to_line.x = dist_to_line * normalv.x / length_normalv * guidance_indi_line_gain;
+ v_to_line.y = dist_to_line * normalv.y / length_normalv * guidance_indi_line_gain;
+ // The distance that needs to be traveled along the line
+ v_along_line.x = wp_diff.x / length_line * GUIDANCE_INDI_NAV_LINE_DIST;
+ v_along_line.y = wp_diff.y / length_line * GUIDANCE_INDI_NAV_LINE_DIST;
+ // Calculate the desired direction to converge to the line
+ struct FloatVect2 direction;
+ VECT2_SMUL(direction, v_along_line, (1.f / (1.f + fabsf(dist_to_line) * 0.05f)));
+ VECT2_ADD(direction, v_to_line);
+ float length_direction = Max(float_vect2_norm(&direction), 0.01f);
+ // Scale to have the desired speed
+ VECT2_SMUL(nav.speed, direction, desired_speed / length_direction);
+ // final target position, should be on the line, for display
+ VECT2_SUM(nav.target, *stateGetPositionEnu_f(), direction);
+
+ nav_rotorcraft_base.goto_wp.from = *wp_start;
+ nav_rotorcraft_base.goto_wp.to = *wp_end;
+ nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp_end);
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_ROUTE;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
+}
+
+static bool nav_hybrid_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, float approaching_time)
+{
+ float dist_to_point;
+ struct FloatVect2 diff;
+ struct EnuCoor_f *pos = stateGetPositionEnu_f();
+
+ /* if an approaching_time is given, estimate diff after approching_time secs */
+ if (approaching_time > 0.f) {
+ struct FloatVect2 estimated_pos;
+ struct FloatVect2 estimated_progress;
+ struct EnuCoor_f *speed = stateGetSpeedEnu_f();
+ VECT2_SMUL(estimated_progress, *speed, approaching_time);
+ VECT2_SUM(estimated_pos, *pos, estimated_progress);
+ VECT2_DIFF(diff, *wp, estimated_pos);
+ }
+ /* else use current position */
+ else {
+ VECT2_DIFF(diff, *wp, *pos);
+ }
+ /* compute distance of estimated/current pos to target wp
+ */
+ dist_to_point = float_vect2_norm(&diff);
+
+ /* return TRUE if we have arrived */
+ if (dist_to_point < ARRIVED_AT_WAYPOINT) {
+ return true;
+ }
+
+ /* if coming from a valid waypoint */
+ if (from != NULL) {
+ /* return TRUE if normal line at the end of the segment is crossed */
+ struct FloatVect2 from_diff;
+ VECT2_DIFF(from_diff, *wp, *from);
+ return (diff.x * from_diff.x + diff.y * from_diff.y < 0.f);
+ }
+
+ return false;
+}
+
+#if 0 // TODO reuse existing patterns for now
+
+static void nav_circle(struct EnuCoor_f *wp_center, float radius)
+{
+
+ // implement nav hybrid circle
+
+ nav_rotorcraft_base.circle.center = *wp_center;
+ nav_rotorcraft_base.circle.radius = radius;
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE;
+ nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
+}
+
+#endif
+
+/** Init and register nav functions
+ *
+ * For hybrid vehicle nav
+ * Init should be called after the normal rotorcraft nav_init
+ * as we are reusing some of the functions and overwritting others
+ */
+void nav_rotorcraft_hybrid_init(void)
+{
+ nav_rotorcraft_base.circle.radius = DEFAULT_CIRCLE_RADIUS;
+ nav_rotorcraft_base.goto_wp.leg_progress = 0.f;
+ nav_rotorcraft_base.goto_wp.leg_length = 1.f;
+
+ // register nav functions
+ nav_register_goto_wp(nav_hybrid_goto, nav_hybrid_route, nav_hybrid_approaching);
+ //nav_register_circle(nav_circle);
+}
+
+
diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
new file mode 100644
index 0000000000..98491f3544
--- /dev/null
+++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2023 Gautier Hattenberger
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ */
+
+/**
+ * @file "modules/nav/nav_rotorcraft_hybrid.h"
+ * Specific navigation functions for hybrid aircraft
+ *
+ * FIXME for now, build on top of nav_rotorcraft_base
+ */
+
+#ifndef NAV_ROTORCRAFT_HYBRID_H
+#define NAV_ROTORCRAFT_HYBRID_H
+
+#include "modules/nav/nav_base.h"
+#include "modules/nav/nav_rotorcraft_base.h"
+
+// settings
+extern float nav_max_speed;
+
+extern void nav_rotorcraft_hybrid_init(void);
+
+#endif
+
diff --git a/sw/airborne/modules/nav/nav_rover_base.c b/sw/airborne/modules/nav/nav_rover_base.c
index 4dba9fcde3..9f6cda64da 100644
--- a/sw/airborne/modules/nav/nav_rover_base.c
+++ b/sw/airborne/modules/nav/nav_rover_base.c
@@ -276,7 +276,7 @@ static void send_nav_status(struct transport_tx *trans, struct link_device *dev)
&dist_home, &dist_wp,
&nav_block, &nav_stage,
&nav.mode);
-#if ROVER_BASE_SEND_TRAJECTORY
+#if ROVER_BASE_SEND_TRAJECTORY
if (nav.mode == NAV_MODE_ROUTE) {
send_segment(trans, dev);
} else if (nav.mode == NAV_MODE_CIRCLE) {
diff --git a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c
index ff6ae34137..2b0be744eb 100644
--- a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c
+++ b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c
@@ -109,7 +109,7 @@ static struct EnuCoor_f SurveyToWP;
static struct EnuCoor_f SurveyFromWP;
static struct EnuCoor_f SurveyEntry;
-static struct EnuCoor_i survey_from_i, survey_to_i;
+static struct EnuCoor_f survey_from, survey_to;
static uint8_t SurveyEntryWP;
static uint8_t SurveySize;
@@ -306,6 +306,10 @@ void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orient
//=========================================================================================================================
bool nav_survey_poly_run(void)
{
+ // check if nav route is available
+ if (nav.nav_goto == NULL || nav.nav_route == NULL || nav.nav_approaching == NULL) {
+ return false;
+ }
#ifdef NAV_SURVEY_POLY_DYNAMIC
dSweep = (nav_survey_shift > 0 ? Poly_Distance : -Poly_Distance);
@@ -334,11 +338,10 @@ bool nav_survey_poly_run(void)
RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0);
- ENU_BFP_OF_REAL(survey_from_i, C);
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- VECT3_COPY(navigation_target, survey_from_i);
+ VECT3_COPY(survey_from, C);
+ nav.nav_goto(&survey_from);
- if (((nav_approaching_from(&survey_from_i, NULL, 0))
+ if (((nav.nav_approaching(&survey_from, NULL, 0))
&& (fabsf(stateGetPositionEnu_f()->z - waypoints[SurveyEntryWP].enu_f.z)) < 1.)) {
CSurveyStatus = Sweep;
nav_init_stage();
@@ -358,13 +361,11 @@ bool nav_survey_poly_run(void)
RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
//follow the line
- ENU_BFP_OF_REAL(survey_to_i, ToP);
- ENU_BFP_OF_REAL(survey_from_i, FromP);
+ VECT3_COPY(survey_to, ToP);
+ VECT3_COPY(survey_from, FromP);
+ nav.nav_route(&survey_from, &survey_to);
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(&survey_from_i, &survey_to_i);
-
- if (nav_approaching_from(&survey_to_i, NULL, 0)) {
+ if (nav.nav_approaching(&survey_to, NULL, 0)) {
LastPoint = SurveyToWP;
#ifdef DIGITAL_CAM
@@ -467,13 +468,11 @@ bool nav_survey_poly_run(void)
RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
//follow the line
- ENU_BFP_OF_REAL(survey_to_i, ToP);
- ENU_BFP_OF_REAL(survey_from_i, FromP);
+ VECT3_COPY(survey_to, ToP);
+ VECT3_COPY(survey_from, FromP);
+ nav.nav_route(&survey_from, &survey_to);
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(&survey_from_i, &survey_to_i);
-
- if (nav_approaching_from(&survey_to_i, NULL, 0)) {
+ if (nav.nav_approaching(&survey_to, NULL, 0)) {
CSurveyStatus = Sweep;
nav_init_stage();
LINE_START_FUNCTION;
diff --git a/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c b/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c
index ffab5d9bf9..37b1f08c42 100644
--- a/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c
+++ b/sw/airborne/modules/nav/nav_survey_rectangle_rotorcraft.c
@@ -67,10 +67,10 @@ bool nav_in_circle = false;
bool interleave = USE_INTERLEAVE;
static struct EnuCoor_f survey_from, survey_to;
-static struct EnuCoor_i survey_from_i, survey_to_i;
static bool survey_uturn __attribute__((unused)) = false;
static survey_orientation_t survey_orientation = NS;
+static bool nav_survey_send = false;
float nav_survey_shift;
float nav_survey_west, nav_survey_east, nav_survey_north, nav_survey_south;
@@ -91,9 +91,10 @@ float nav_survey_west, nav_survey_east, nav_survey_north, nav_survey_south;
static void send_survey(struct transport_tx *trans, struct link_device *dev)
{
- if (nav_survey_active) {
+ if (nav_survey_send) {
pprz_msg_send_SURVEY(trans, dev, AC_ID,
&nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south);
+ nav_survey_send= false; // stop sending when run function is not called anymore
}
}
@@ -150,9 +151,8 @@ void nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid,
nav_survey_rectangle_active = false;
//go to start position
- ENU_BFP_OF_REAL(survey_from_i, survey_from);
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- VECT3_COPY(navigation_target, survey_from_i);
+ nav.horizontal_mode = NAV_HORIZONTAL_MODE_ROUTE;
+ VECT3_COPY(nav.target, survey_from);
LINE_STOP_FUNCTION;
NavVerticalAltitudeMode(waypoints[wp1].enu_f.z, 0.);
if (survey_orientation == NS) {
@@ -164,16 +164,21 @@ void nav_survey_rectangle_rotorcraft_setup(uint8_t wp1, uint8_t wp2, float grid,
bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
{
+ // check if nav route is available
+ if (nav.nav_route == NULL || nav.nav_approaching == NULL) {
+ return false;
+ }
+
#ifdef NAV_SURVEY_RECTANGLE_DYNAMIC
nav_survey_shift = (nav_survey_shift > 0 ? sweep : -sweep);
- #endif
+ #endif
static bool is_last_half = false;
static float survey_radius;
- nav_survey_active = true;
+ nav_survey_send = true;
/* entry scan */ // wait for start position and altitude be reached
- if (!nav_survey_rectangle_active && ((!nav_approaching_from(&survey_from_i, NULL, 0))
+ if (!nav_survey_rectangle_active && ((!nav.nav_approaching(&survey_from, NULL, 0))
|| (fabsf(stateGetPositionEnu_f()->z - waypoints[wp1].enu_f.z)) > 1.)) {
} else {
if (!nav_survey_rectangle_active) {
@@ -202,20 +207,15 @@ bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
}
if (!survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
- /* if you like to use position croos instead of approaching uncoment this line
+ /* if you like to use position cross instead of approaching uncoment this line
if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
(stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
(stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
(stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
*/
/* Continue ... */
- ENU_BFP_OF_REAL(survey_to_i, survey_to);
-
- if (!nav_approaching_from(&survey_to_i, NULL, 0)) {
- ENU_BFP_OF_REAL(survey_from_i, survey_from);
-
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(&survey_from_i, &survey_to_i);
+ if (!nav.nav_approaching(&survey_to, NULL, 0)) {
+ nav.nav_route(&survey_from, &survey_to);
} else {
if (survey_orientation == NS) {
@@ -225,11 +225,11 @@ bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
|| (x0 + nav_survey_shift > nav_survey_east)) { // not room for full sweep
if (((x0 + (nav_survey_shift / 2)) < nav_survey_west)
|| ((x0 + (nav_survey_shift / 2)) > nav_survey_east)) { //not room for half sweep
- if (is_last_half) {// was last sweep half?
+ if (is_last_half) { // was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
- }else {
+ } else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = false;
@@ -237,7 +237,7 @@ bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
- }else{
+ } else {
survey_radius = nav_survey_shift ;
}
}
@@ -274,11 +274,11 @@ bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
|| my_y0 + nav_survey_shift > nav_survey_north) { // not room for full sweep
if (((my_y0 + (nav_survey_shift / 2)) < nav_survey_south)
|| ((my_y0 + (nav_survey_shift / 2)) > nav_survey_north)) { //not room for half sweep
- if (is_last_half) {// was last sweep half?
+ if (is_last_half) { // was last sweep half?
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift;
- }else {
+ } else {
survey_radius = nav_survey_shift /2.;
}
is_last_half = false;
@@ -286,7 +286,7 @@ bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
nav_survey_shift = -nav_survey_shift;
if (interleave) {
survey_radius = nav_survey_shift /2.;
- }else{
+ } else {
survey_radius = nav_survey_shift ;
}
}
@@ -336,46 +336,29 @@ bool nav_survey_rectangle_rotorcraft_run(uint8_t wp1, uint8_t wp2)
}
} else { /* START turn */
- static struct EnuCoor_f temp_f;
+ static struct EnuCoor_f turn_from;
if (survey_orientation == WE) {
- temp_f.x = waypoints[0].enu_f.x;
- temp_f.y = waypoints[0].enu_f.y - survey_radius;
+ turn_from.x = waypoints[0].enu_f.x;
+ turn_from.y = waypoints[0].enu_f.y - survey_radius;
} else {
- temp_f.y = waypoints[0].enu_f.y;
- temp_f.x = waypoints[0].enu_f.x - survey_radius;
+ turn_from.y = waypoints[0].enu_f.y;
+ turn_from.x = waypoints[0].enu_f.x - survey_radius;
}
//detect when segment has done
/* if you like to use position croos instead of approaching uncoment this line
- if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y < waypoints[0].enu_f.y)) )||
- (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (temp_f.y > waypoints[0].enu_f.y)) )||
- (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x > waypoints[0].enu_f.x)) )||
- (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (temp_f.x < waypoints[0].enu_f.x)) ) ) {
+ if ( (stateGetPositionEnu_f()->y > waypoints[0].enu_f.y && ((survey_orientation == WE) && (turn_from.y < waypoints[0].enu_f.y)) )||
+ (stateGetPositionEnu_f()->y < waypoints[0].enu_f.y && ((survey_orientation == WE) && (turn_from.y > waypoints[0].enu_f.y)) )||
+ (stateGetPositionEnu_f()->x < waypoints[0].enu_f.x && ((survey_orientation == NS) && (turn_from.x > waypoints[0].enu_f.x)) )||
+ (stateGetPositionEnu_f()->x > waypoints[0].enu_f.x && ((survey_orientation == NS) && (turn_from.x < waypoints[0].enu_f.x)) ) ) {
*/
- if (survey_orientation == WE) {
- ENU_BFP_OF_REAL(survey_from_i, temp_f);
- ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
- } else {
- ENU_BFP_OF_REAL(survey_from_i, temp_f);
- ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
- }
- if (nav_approaching_from(&survey_to_i, NULL, 0)) {
+ if (nav.nav_approaching(&waypoints[0].enu_f, NULL, 0)) {
survey_uturn = false;
nav_in_circle = false;
LINE_START_FUNCTION;
} else {
-
- if (survey_orientation == WE) {
- ENU_BFP_OF_REAL(survey_from_i, temp_f);
- ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
- } else {
- ENU_BFP_OF_REAL(survey_from_i, temp_f);
- ENU_BFP_OF_REAL(survey_to_i, waypoints[0].enu_f);
- }
-
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
- nav_route(&survey_from_i, &survey_to_i);
+ nav.nav_route(&turn_from, &waypoints[0].enu_f);
}
} /* END turn */
diff --git a/sw/airborne/modules/nav/waypoints.c b/sw/airborne/modules/nav/waypoints.c
index ad28dd0432..c92b15cf76 100644
--- a/sw/airborne/modules/nav/waypoints.c
+++ b/sw/airborne/modules/nav/waypoints.c
@@ -31,6 +31,22 @@
const uint8_t nb_waypoint = NB_WAYPOINT;
struct Waypoint waypoints[NB_WAYPOINT];
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
+{
+ static uint8_t i;
+ i++;
+ if (i >= nb_waypoint) { i = 0; }
+ pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
+ &i,
+ &(waypoints[i].enu_i.x),
+ &(waypoints[i].enu_i.y),
+ &(waypoints[i].enu_i.z));
+}
+#endif
+
/** initialize global and local waypoints */
void waypoints_init(void)
{
@@ -50,6 +66,10 @@ void waypoints_init(void)
waypoint_set_enu(i, &wp_tmp_float[i]);
}
}
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WP_MOVED, send_wp_moved);
+#endif
}
bool waypoint_is_global(uint8_t wp_id)
diff --git a/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c
index 1e5022e4a5..a6534712b9 100644
--- a/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c
+++ b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.c
@@ -114,7 +114,7 @@ void run_avoid_navigation_onvision(void)
avoid_navigation_data.stereo_bin[4] = counter;
}
-void increase_nav_heading(int32_t *heading, int32_t increment)
+void increase_nav_heading(float *heading, float increment)
{
*heading = *heading + increment;
}
diff --git a/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h
index c1734b5d73..e8848534dc 100644
--- a/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h
+++ b/sw/airborne/modules/stereocam/nav_line_avoid/avoid_navigation.h
@@ -51,7 +51,7 @@ extern bool obstacle_detected;
void init_avoid_navigation(void);
void run_avoid_navigation_onvision(void);
-extern void increase_nav_heading(int32_t *heading, int32_t increment);
+extern void increase_nav_heading(float *heading, float increment);
#endif /* AVOID_NAVIGATION_H */
diff --git a/sw/include/std.h b/sw/include/std.h
index 4f974568ac..3b4f802cfc 100644
--- a/sw/include/std.h
+++ b/sw/include/std.h
@@ -85,10 +85,35 @@ typedef uint8_t unit_t;
#define SetBit(a, n) a |= (1 << n)
#define ClearBit(a, n) a &= ~(1 << n)
+/** Normalize a rad angle between -PI and PI */
#define NormRadAngle(x) { \
while (x > M_PI) x -= 2 * M_PI; \
while (x < -M_PI) x += 2 * M_PI; \
}
+/** Normalize a degree angle between 0 and 359 */
+#define NormCourse(x) { \
+ while (x < 0) x += 360; \
+ while (x >= 360) x -= 360; \
+ }
+/** Normalize a rad angle between 0 and 2*PI */
+#define NormCourseRad(x) { \
+ while (x < 0) x += 2*M_PI; \
+ while (x >= 2*M_PI) x -= 2*M_PI; \
+ }
+
+/** Normalize a degree angle between 0 and 359 */
+// FIXME should we use a protected version ? of NormXxx ?
+/*
+#define NormCourse(x) { \
+ uint8_t dont_loop_forever = 0; \
+ while (x < 0 && ++dont_loop_forever) x += 360; \
+ while (x >= 360 && ++dont_loop_forever) x -= 360; \
+ }
+*/
+
+#define CloseDegAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
+#define CloseRadAngles(_c1, _c2) ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
+
#define DegOfRad(x) ((x) * (180. / M_PI))
#define DeciDegOfRad(x) ((x) * (1800./ M_PI))
#define RadOfDeg(x) ((x) * (M_PI/180.))