diff --git a/conf/airframes/tudelft/rotwing5.xml b/conf/airframes/tudelft/rotwing5.xml index 3799450579..1fec38d2d1 100644 --- a/conf/airframes/tudelft/rotwing5.xml +++ b/conf/airframes/tudelft/rotwing5.xml @@ -1,13 +1,13 @@ - - RotatingWing25Kg + + RotatingWing 5 @@ -37,14 +37,14 @@ - + - - - + + + @@ -64,6 +64,18 @@ + + + + + + + + + + + + @@ -79,7 +91,7 @@ - + @@ -102,7 +114,7 @@ - + @@ -163,7 +175,7 @@ - + @@ -183,7 +195,7 @@ - + @@ -198,7 +210,7 @@ - + @@ -214,7 +226,7 @@ - + @@ -226,9 +238,9 @@ - + - + @@ -240,13 +252,13 @@ - + - + - + @@ -266,71 +278,8 @@ -
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - -
-
- + @@ -339,13 +288,13 @@ - - + + - + @@ -360,7 +309,7 @@ - + @@ -379,201 +328,9 @@ - + - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - +
@@ -590,8 +347,20 @@ Put UAV on take-off location Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Arm parachute Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing6.xml b/conf/airframes/tudelft/rotwing6.xml index 3c22a3e626..63d4dda8cf 100644 --- a/conf/airframes/tudelft/rotwing6.xml +++ b/conf/airframes/tudelft/rotwing6.xml @@ -1,13 +1,13 @@ - - RotatingWing25Kg + + RotatingWing 6 @@ -40,6 +40,13 @@ + + + + + + + @@ -57,6 +64,18 @@ + + + + + + + + + + + + @@ -72,7 +91,7 @@ - + @@ -95,7 +114,7 @@ - + @@ -155,7 +174,7 @@ - + @@ -175,7 +194,7 @@ - + @@ -190,7 +209,7 @@ - + @@ -206,7 +225,7 @@ - + @@ -218,9 +237,9 @@ - + - + @@ -232,13 +251,13 @@ - + - + - + @@ -258,71 +277,8 @@ -
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - -
-
- + @@ -331,13 +287,13 @@ - - + + - + @@ -352,7 +308,7 @@ - + @@ -371,201 +327,9 @@ - + - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - +
@@ -582,8 +346,20 @@ Put UAV on take-off location Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Arm parachute Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing_25kg_common.xml b/conf/airframes/tudelft/rotwing_25kg_common.xml new file mode 100644 index 0000000000..37d3fe5877 --- /dev/null +++ b/conf/airframes/tudelft/rotwing_25kg_common.xml @@ -0,0 +1,258 @@ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + +
+ +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing_7kg_common.xml b/conf/airframes/tudelft/rotwing_7kg_common.xml new file mode 100644 index 0000000000..598d9c533e --- /dev/null +++ b/conf/airframes/tudelft/rotwing_7kg_common.xml @@ -0,0 +1,316 @@ + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + + +
+ +
+ + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ diff --git a/conf/airframes/tudelft/rotwing_v3b.xml b/conf/airframes/tudelft/rotwing_v3b.xml index f1a8fce855..e67f2fc5be 100644 --- a/conf/airframes/tudelft/rotwing_v3b.xml +++ b/conf/airframes/tudelft/rotwing_v3b.xml @@ -30,16 +30,12 @@ + - - - - - @@ -69,6 +65,7 @@ + @@ -99,12 +96,17 @@ + + + + + @@ -141,64 +143,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -257,9 +201,9 @@ - - - + + + @@ -270,7 +214,7 @@ - + @@ -295,36 +239,7 @@ -
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
+
@@ -333,7 +248,7 @@ - + @@ -342,7 +257,7 @@ - + @@ -441,33 +356,10 @@ - + - - - - - - - - - - - - - -
- -
- - - - - - -
@@ -481,168 +373,26 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- - - - -
+ + Enter the PIC + Enter the PAC + Enter the GCS op + Goal of the flight + Location, airspace and weather + Check the RC battery + Check tail connection + Check wings taped and secured + Inspect airframe condition + Check attitude and heading + Airspeed sensor calibration + Put UAV on take-off location + Automated actuator check + Check flight plan + Switch to correct flight block + Switch on drone tag + Switch on camera + Announce flight to other airspace users +
@@ -653,29 +403,5 @@
-
- - - - - - -
- - - Enter the PIC - Enter the PAC - Enter the GCS op - Goal of the flight - Location, airspace and weather - Check the RC battery - Check tail connection - Check wings taped and secured - Check attitude and heading - Put UAV on take-off location - Check flight plan - Switch to correct flight block - Switch on camera - Announce flight to other airspace users - + diff --git a/conf/airframes/tudelft/rotwing_v3d.xml b/conf/airframes/tudelft/rotwing_v3d.xml index d14796ae4e..18d7d82e05 100644 --- a/conf/airframes/tudelft/rotwing_v3d.xml +++ b/conf/airframes/tudelft/rotwing_v3d.xml @@ -30,6 +30,7 @@ + @@ -56,6 +57,7 @@ + @@ -70,7 +72,7 @@ - + @@ -82,8 +84,12 @@ + + + + @@ -92,6 +98,7 @@ + @@ -128,73 +135,15 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + @@ -206,10 +155,10 @@ - - - - + + + + @@ -244,9 +193,9 @@ - - - + + + @@ -257,7 +206,7 @@ - + @@ -268,7 +217,7 @@ - +
@@ -283,36 +232,7 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
+
@@ -429,37 +349,10 @@ - + - - - - - - - - - - - - - - - - - -
- -
- - - - - - -
@@ -473,193 +366,6 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - -
- Enter the PIC Enter the PAC @@ -676,7 +382,19 @@ Automated actuator check Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing_v3e.xml b/conf/airframes/tudelft/rotwing_v3e.xml index 9c31d6a42d..e87c0e75a1 100644 --- a/conf/airframes/tudelft/rotwing_v3e.xml +++ b/conf/airframes/tudelft/rotwing_v3e.xml @@ -30,6 +30,7 @@ + @@ -56,6 +57,7 @@ + @@ -82,8 +84,12 @@ + + + + @@ -92,6 +98,7 @@ + @@ -128,64 +135,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -194,7 +143,7 @@ - + @@ -244,9 +193,9 @@ - - - + + + @@ -257,7 +206,7 @@ - + @@ -268,7 +217,7 @@ - +
@@ -283,36 +232,7 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
+
@@ -429,37 +349,10 @@ - + - - - - - - - - - - - - - - - - - -
- -
- - - - - - -
@@ -473,193 +366,6 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - -
- Enter the PIC Enter the PAC @@ -676,7 +382,19 @@ Automated actuator check Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing_v3f.xml b/conf/airframes/tudelft/rotwing_v3f.xml index 31b8bfd117..dd3c8255bb 100644 --- a/conf/airframes/tudelft/rotwing_v3f.xml +++ b/conf/airframes/tudelft/rotwing_v3f.xml @@ -30,6 +30,7 @@ + @@ -56,6 +57,7 @@ + @@ -70,7 +72,7 @@ - + @@ -82,8 +84,12 @@ + + + + @@ -92,6 +98,7 @@ + @@ -128,64 +135,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -194,14 +143,14 @@ - + - + @@ -244,9 +193,9 @@ - - - + + + @@ -257,18 +206,18 @@ - + - - + + - +
@@ -283,36 +232,7 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
+
@@ -382,7 +302,7 @@ - + @@ -429,37 +349,10 @@ - + - - - - - - - - - - - - - - - - - -
- -
- - - - - - -
@@ -473,193 +366,6 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - -
- Enter the PIC Enter the PAC @@ -676,7 +382,19 @@ Automated actuator check Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing_v3g.xml b/conf/airframes/tudelft/rotwing_v3g.xml index ac76f9f699..0b124d6b49 100644 --- a/conf/airframes/tudelft/rotwing_v3g.xml +++ b/conf/airframes/tudelft/rotwing_v3g.xml @@ -57,6 +57,7 @@ + @@ -71,7 +72,7 @@ - + @@ -97,6 +98,7 @@ + @@ -133,64 +135,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -199,14 +143,14 @@ - - + + - + @@ -249,9 +193,9 @@ - - - + + + @@ -262,7 +206,7 @@ - + @@ -273,7 +217,7 @@ - +
@@ -288,36 +232,7 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
+
@@ -387,7 +302,7 @@ - + @@ -434,41 +349,28 @@ - + - - - - - - - - - - - - - - - - -
- -
- - - - - - -
+ + + + + + + + + + + + + + - @@ -477,193 +379,6 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - -
- Enter the PIC Enter the PAC @@ -680,7 +395,19 @@ Automated actuator check Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/airframes/tudelft/rotwing_v3h.xml b/conf/airframes/tudelft/rotwing_v3h.xml index 47e9eb564d..0816565335 100644 --- a/conf/airframes/tudelft/rotwing_v3h.xml +++ b/conf/airframes/tudelft/rotwing_v3h.xml @@ -57,6 +57,7 @@ + @@ -71,7 +72,7 @@ - + @@ -97,6 +98,7 @@ + @@ -133,64 +135,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -199,14 +143,14 @@ - - + + - + @@ -249,9 +193,9 @@ - - - + + + @@ -262,7 +206,7 @@ - + @@ -273,7 +217,7 @@ - +
@@ -288,36 +232,7 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
+
@@ -434,36 +349,10 @@ - + - - - - - - - - - - - - - - - - -
- -
- - - - - - -
@@ -477,191 +366,6 @@
-
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - -
- -
- - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - -
- -
- -
- -
- - - - -
- -
- - - - - - -
- -
- - - - - - -
- Enter the PIC Enter the PAC @@ -678,7 +382,19 @@ Automated actuator check Check flight plan Switch to correct flight block + Switch on drone tag Switch on camera Announce flight to other airspace users + +
+ + + + + + +
+ + diff --git a/conf/flight_plans/tudelft/rotating_wing_EHR8.xml b/conf/flight_plans/tudelft/rotating_wing_EHR8.xml index ff81f7d2ef..25cfe36055 100644 --- a/conf/flight_plans/tudelft/rotating_wing_EHR8.xml +++ b/conf/flight_plans/tudelft/rotating_wing_EHR8.xml @@ -53,34 +53,35 @@ - + - + - + - + + - + @@ -92,31 +93,31 @@ - + - + - + - + - + @@ -124,7 +125,7 @@ - + @@ -132,47 +133,47 @@ - + - + - + - + - + - + - + - + - + - + @@ -180,7 +181,7 @@ - + diff --git a/conf/flight_plans/tudelft/rotating_wing_EHVB.xml b/conf/flight_plans/tudelft/rotating_wing_EHVB.xml index d8bf210d59..c1c2551c4a 100644 --- a/conf/flight_plans/tudelft/rotating_wing_EHVB.xml +++ b/conf/flight_plans/tudelft/rotating_wing_EHVB.xml @@ -1,5 +1,5 @@ - +
@@ -53,34 +53,35 @@ - + - + - + - + + - + - + @@ -94,52 +95,44 @@ - + - - - - - + - + - + - + - + - - + - + - - - + + + + + - - - - - - + - + - + - + - + - + - + @@ -215,7 +198,7 @@ - + diff --git a/conf/flight_plans/tudelft/rotating_wing_terschelling.xml b/conf/flight_plans/tudelft/rotating_wing_terschelling.xml deleted file mode 100644 index 3ec0767f32..0000000000 --- a/conf/flight_plans/tudelft/rotating_wing_terschelling.xml +++ /dev/null @@ -1,166 +0,0 @@ - - -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/flight_plans/tudelft/rotating_wing_tharde.xml b/conf/flight_plans/tudelft/rotating_wing_tharde.xml index 69b620d915..e71fe39720 100644 --- a/conf/flight_plans/tudelft/rotating_wing_tharde.xml +++ b/conf/flight_plans/tudelft/rotating_wing_tharde.xml @@ -40,28 +40,28 @@ - + - + - + - + - + @@ -70,24 +70,24 @@ - + - + - + - + @@ -95,7 +95,7 @@ - + @@ -103,47 +103,47 @@ - + - + - + - + - + - + - + - + - + - + @@ -151,7 +151,7 @@ - + diff --git a/conf/flight_plans/tudelft/rotating_wing_troia.xml b/conf/flight_plans/tudelft/rotating_wing_troia.xml new file mode 100644 index 0000000000..9adef32f28 --- /dev/null +++ b/conf/flight_plans/tudelft/rotating_wing_troia.xml @@ -0,0 +1,309 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/eff_scheduling_rotwing.xml b/conf/modules/eff_scheduling_rotwing.xml index fc002d8105..0d206200d0 100644 --- a/conf/modules/eff_scheduling_rotwing.xml +++ b/conf/modules/eff_scheduling_rotwing.xml @@ -22,7 +22,12 @@ - + + + + + + @@ -67,4 +72,4 @@ - + \ No newline at end of file diff --git a/conf/modules/guidance_indi_hybrid.xml b/conf/modules/guidance_indi_hybrid.xml index ffc50824b8..47394e3a14 100644 --- a/conf/modules/guidance_indi_hybrid.xml +++ b/conf/modules/guidance_indi_hybrid.xml @@ -7,10 +7,14 @@ - guidance_indi_hybid_tailsitter - guidance_indi_hybid_quadplane -
+
+ + + +
@@ -24,11 +28,17 @@ - + + + + + + + diff --git a/conf/modules/rotwing_state.xml b/conf/modules/rotwing_state.xml index 2eb1fcc33c..9af4f76bb8 100644 --- a/conf/modules/rotwing_state.xml +++ b/conf/modules/rotwing_state.xml @@ -3,26 +3,28 @@ This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state.
- +
- - - - - - + + + + + + + + +
- - - + + diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml index 3437509e1e..e6e0209038 100644 --- a/conf/telemetry/highspeed_rotorcraft.xml +++ b/conf/telemetry/highspeed_rotorcraft.xml @@ -24,7 +24,7 @@ - + @@ -42,7 +42,7 @@ - + @@ -64,6 +64,8 @@ + + @@ -276,10 +278,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -327,7 +384,7 @@ - + @@ -347,6 +404,7 @@ + diff --git a/conf/userconf/tudelft/RW_control_panel.xml b/conf/userconf/tudelft/RW_control_panel.xml index 23e8fab250..5d8c7db428 100644 --- a/conf/userconf/tudelft/RW_control_panel.xml +++ b/conf/userconf/tudelft/RW_control_panel.xml @@ -1506,5 +1506,15 @@ + + + + + + + + + +
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 45c7d655f3..afb58b072a 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -610,7 +610,7 @@ airframe="airframes/tudelft/rotwing_v3f.xml" radio="radios/crossfire_sbus.xml" telemetry="telemetry/highspeed_rotorcraft.xml" - flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml" + flight_plan="flight_plans/tudelft/rotating_wing_troia.xml" settings="settings/rotorcraft_basic.xml" settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" gui_color="red" @@ -665,9 +665,9 @@ airframe="airframes/tudelft/rotwing5.xml" radio="radios/crossfire_sbus.xml" telemetry="telemetry/highspeed_rotorcraft.xml" - flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml" + flight_plan="flight_plans/tudelft/rotating_wing_troia.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" + settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" gui_color="red" /> - - + + @@ -229,8 +229,8 @@ - - + + @@ -294,6 +294,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index adcdc277d4..bf4d9d354e 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -71,6 +71,42 @@ #define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) #endif +#ifndef GUIDANCE_INDI_MAX_AIRSPEED +#error "You must have an airspeed sensor to use this guidance" +#endif + +#ifndef GUIDANCE_INDI_MIN_AIRSPEED +#define GUIDANCE_INDI_MIN_AIRSPEED -10.f +#endif + +/** + * Climb speed when navigation is making turns instead of direct lines + */ +#ifndef GUIDANCE_INDI_FWD_CLIMB_SPEED +#define GUIDANCE_INDI_FWD_CLIMB_SPEED 4.0 +#endif + +/** + * Descend speed when navigation is making turns instead of direct lines + */ +#ifndef GUIDANCE_INDI_FWD_DESCEND_SPEED +#define GUIDANCE_INDI_FWD_DESCEND_SPEED -4.0 +#endif + +/** + * Climb speed when navigation is doing direct lines + */ +#ifndef GUIDANCE_INDI_QUAD_CLIMB_SPEED +#define GUIDANCE_INDI_QUAD_CLIMB_SPEED 2.0 +#endif + +/** + * Descend speed when navigation is doing direct lines + */ +#ifndef GUIDANCE_INDI_QUAD_DESCEND_SPEED +#define GUIDANCE_INDI_QUAD_DESCEND_SPEED -2.0 +#endif + struct guidance_indi_hybrid_params gih_params = { .pos_gain = GUIDANCE_INDI_POS_GAIN, .pos_gainz = GUIDANCE_INDI_POS_GAINZ, @@ -82,13 +118,15 @@ struct guidance_indi_hybrid_params gih_params = { .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared .liftd_p80 = GUIDANCE_INDI_LIFTD_P80, .liftd_p50 = GUIDANCE_INDI_LIFTD_P50, + .min_airspeed = GUIDANCE_INDI_MIN_AIRSPEED, + .max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED, + .stall_protect_gain = 1.5, // m/s^2 downward acceleration per m/s airspeed loss + .climb_vspeed_fwd = GUIDANCE_INDI_FWD_CLIMB_SPEED, + .descend_vspeed_fwd = GUIDANCE_INDI_FWD_DESCEND_SPEED, + .climb_vspeed_quad = GUIDANCE_INDI_QUAD_CLIMB_SPEED, + .descend_vspeed_quad = GUIDANCE_INDI_QUAD_DESCEND_SPEED, }; -#ifndef GUIDANCE_INDI_MAX_AIRSPEED -#error "You must have an airspeed sensor to use this guidance" -#endif -float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED; - // Quadplanes can hover at various pref pitch float guidance_indi_pitch_pref_deg = 0; @@ -113,7 +151,7 @@ float guidance_indi_pitch_pref_deg = 0; /*Airspeed threshold where making a turn is "worth it"*/ #ifndef TURN_AIRSPEED_TH -#define TURN_AIRSPEED_TH 10.0 +#define TURN_AIRSPEED_TH 13 .0 #endif /*Boolean to force the heading to a static value (only use for specific experiments)*/ @@ -158,20 +196,21 @@ static void guidance_indi_filter_thrust(void); #define GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF 0.5 #endif -#ifndef GUIDANCE_INDI_CLIMB_SPEED_FWD -#define GUIDANCE_INDI_CLIMB_SPEED_FWD 4.0 -#endif - -#ifndef GUIDANCE_INDI_DESCEND_SPEED_FWD -#define GUIDANCE_INDI_DESCEND_SPEED_FWD -4.0 -#endif - #ifndef GUIDANCE_INDI_MAX_LAT_ACCEL #define GUIDANCE_INDI_MAX_LAT_ACCEL 9.81 #endif -float climb_vspeed_fwd = GUIDANCE_INDI_CLIMB_SPEED_FWD; -float descend_vspeed_fwd = GUIDANCE_INDI_DESCEND_SPEED_FWD; +#ifndef GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED +#define GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED 10.0 +#endif + +#ifndef GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED +#define GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED 30.0 +#endif + +#ifndef GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN +#define GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN 0.0 +#endif float inv_eff[4]; @@ -179,6 +218,16 @@ float inv_eff[4]; float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK; float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH; +#if defined(ROTWING_STATE_FW_MAX_AIRSPEED) && defined(ROTWING_STATE_QUAD_MAX_AIRSPEED) + float gih_coordinated_turn_min_airspeed = ROTWING_STATE_QUAD_MAX_AIRSPEED; + float gih_coordinated_turn_max_airspeed = ROTWING_STATE_FW_MAX_AIRSPEED + GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN; +#else + float gih_coordinated_turn_min_airspeed = GUIDANCE_INDI_COORDINATED_TURN_MIN_AIRSPEED; + float gih_coordinated_turn_max_airspeed = GUIDANCE_INDI_COORDINATED_TURN_MAX_AIRSPEED + GUIDANCE_INDI_COORDINATED_TURN_AIRSPEED_MARGIN; +#endif + +bool coordinated_turn_use_accel = false; + /** state eulers in zxy order */ struct FloatEulers eulers_zxy; @@ -192,6 +241,7 @@ Butterworth2LowPass accely_filt; Butterworth2LowPass guidance_indi_airspeed_filt; struct FloatVect2 desired_airspeed; +float gi_unbounded_airspeed_sp = 0.f; float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U]; struct FloatVect3 euler_cmd; @@ -247,6 +297,7 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0}; float time_of_vel_sp = 0.0; void guidance_indi_propagate_filters(void); +void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed); #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" @@ -344,7 +395,7 @@ void guidance_indi_enter(void) thrust_in = stabilization.cmd[COMMAND_THRUST]; thrust_act = thrust_in; - guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi; + guidance_indi_hybrid_heading_sp = eulers_zxy.psi; float tau = 1.0 / (2.0 * M_PI * filter_cutoff); float sample_time = 1.0 / PERIODIC_FREQUENCY; @@ -352,9 +403,6 @@ void guidance_indi_enter(void) init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0); } - /*Obtain eulers with zxy rotation order*/ - float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); - init_butterworth_2_low_pass(&roll_filt, tau, sample_time, eulers_zxy.phi); init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta); init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in); @@ -364,6 +412,11 @@ void guidance_indi_enter(void) init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0); } +void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed) { + gih_params.min_airspeed = min_airspeed; + gih_params.max_airspeed = max_airspeed; +} + /** * @param accel_sp accel setpoint in NED frame [m/s^2] * @param heading_sp the desired heading [rad] @@ -459,7 +512,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa 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); + Bound(airspeed_turn, gih_coordinated_turn_min_airspeed, gih_coordinated_turn_max_airspeed); guidance_euler_cmd.phi = roll_filt.o[0] + euler_cmd.x; guidance_euler_cmd.theta = pitch_filt.o[0] + euler_cmd.y; @@ -586,6 +639,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) float airspeed = 0.f; #else float airspeed = stateGetAirspeed_f(); + Bound(airspeed, 0.0f, 100.0f); if (guidance_indi_airspeed_filtering) { airspeed = guidance_indi_airspeed_filt.o[0]; } @@ -598,18 +652,27 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) 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 + gi_unbounded_airspeed_sp = norm_des_as; + + // Check if some minimum airspeed is desired (e.g. to prevent stall) + if (norm_des_as < gih_params.min_airspeed) { + norm_des_as = gih_params.min_airspeed; + } + + float gi_airspeed_sp = norm_des_as; + + // Make turn instead of straight line, control airspeed 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) { + if (norm_des_as > gih_params.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) { + if (FLOAT_VECT2_NORM(windspeed) < gih_params.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 cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - gih_params.max_airspeed * gih_params.max_airspeed; float dv = bv * bv - 4.0f * av * cv; @@ -625,14 +688,11 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) 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; + gi_airspeed_sp = gih_params.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; + gi_airspeed_sp = gih_params.max_airspeed; } // Calculate accel sp in body axes, because we need to regulate airspeed @@ -645,7 +705,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) BoundAbs(sp_accel_b.y, GUIDANCE_INDI_MAX_LAT_ACCEL); // Control the airspeed - sp_accel_b.x = (speed_sp_b_x - airspeed) * gih_params.speed_gain; + sp_accel_b.x = (gi_airspeed_sp - 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; @@ -659,8 +719,8 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) 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; + if ((speed_increment + airspeed) > gih_params.max_airspeed) { + speed_sp_b_x = gih_params.max_airspeed + groundspeed_x - airspeed; } } @@ -673,22 +733,25 @@ static struct FloatVect3 compute_accel_from_speed_sp(void) } // Bound the acceleration setpoint - float accelbound = 3.0f + airspeed / guidance_indi_max_airspeed * 5.0f; // FIXME remove hard coded values + float accelbound = 3.0f + airspeed / gih_params.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); + if (!rotwing_state_pusher_motor_running() && !rotwing_state_hover_motors_running()) { + accel_sp.z = gih_params.stall_protect_gain * (gi_airspeed_sp - airspeed); + BoundAbs(accel_sp.z, 5.0); + } + return accel_sp; } static float bound_vz_sp(float vz_sp) { // Bound vertical speed setpoint - if (stateGetAirspeed_f() > 13.f) { - Bound(vz_sp, -climb_vspeed_fwd, -descend_vspeed_fwd); + if (stateGetAirspeed_f() > TURN_AIRSPEED_TH) { + Bound(vz_sp, -gih_params.climb_vspeed_fwd, -gih_params.descend_vspeed_fwd); } else { - Bound(vz_sp, -nav.climb_vspeed, -nav.descend_vspeed); // FIXME don't use nav settings + Bound(vz_sp, -gih_params.climb_vspeed_quad, -gih_params.descend_vspeed_quad); } return vz_sp; } @@ -804,6 +867,7 @@ void guidance_indi_propagate_filters(void) update_butterworth_2_low_pass(&accely_filt, accely); float airspeed = stateGetAirspeed_f(); + Bound(airspeed, 0.0f, 100.0f); update_butterworth_2_low_pass(&guidance_indi_airspeed_filt, airspeed); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h index 7385d1fff3..0540aa8ce6 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h @@ -70,6 +70,7 @@ enum GuidanceIndiHybrid_VMode { extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp); extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode); +extern void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed); struct guidance_indi_hybrid_params { float pos_gain; @@ -80,30 +81,32 @@ struct guidance_indi_hybrid_params { float liftd_asq; float liftd_p80; float liftd_p50; + float min_airspeed; + float max_airspeed; + float stall_protect_gain; + float climb_vspeed_fwd; + float descend_vspeed_fwd; + float climb_vspeed_quad; + float descend_vspeed_quad; }; extern struct FloatVect3 sp_accel; extern struct FloatVect3 gi_speed_sp; +extern struct guidance_indi_hybrid_params gih_params; extern float guidance_indi_pitch_pref_deg; -#if GUIDANCE_INDI_HYBRID_USE_WLS -extern float Wu_gih[GUIDANCE_INDI_HYBRID_U]; -extern float Wv_gih[GUIDANCE_INDI_HYBRID_V]; -extern float du_min_gih[GUIDANCE_INDI_HYBRID_U]; -extern float du_max_gih[GUIDANCE_INDI_HYBRID_U]; -extern float du_pref_gih[GUIDANCE_INDI_HYBRID_U]; -#endif +extern float gi_unbounded_airspeed_sp; extern float guidance_indi_thrust_z_eff; extern struct guidance_indi_hybrid_params gih_params; extern float guidance_indi_specific_force_gain; -extern float guidance_indi_max_airspeed; extern bool take_heading_control; extern float guidance_indi_max_bank; extern float guidance_indi_min_pitch; extern bool force_forward; ///< forward flight for hybrid nav extern bool guidance_indi_airspeed_filtering; +extern bool coordinated_turn_use_accel; #endif /* GUIDANCE_INDI_HYBRID_H */ diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index b15380fc92..a3bb4e6e8b 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -267,9 +267,12 @@ void nav_periodic_task(void) bool nav_detect_ground(void) { - if (!autopilot.ground_detected) { return false; } - autopilot.ground_detected = false; - return true; + if (autopilot.ground_detected) { + autopilot.ground_detected = false; + return true; + } else { + return false; + } } bool nav_is_in_flight(void) diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c index 7717dd3142..0aa5d75243 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c @@ -130,6 +130,7 @@ struct rotwing_eff_sched_param_t eff_sched_p = { .Ixx_wing = ROTWING_EFF_SCHED_IXX_WING, .Iyy_wing = ROTWING_EFF_SCHED_IYY_WING, .m = ROTWING_EFF_SCHED_M, + .DMdpprz_hover_pitch = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH, .DMdpprz_hover_roll = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL, .hover_roll_pitch_coef = ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF, .hover_roll_roll_coef = ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF, @@ -146,9 +147,28 @@ struct rotwing_eff_sched_param_t eff_sched_p = { .k_lift_tail = ROTWING_EFF_SCHED_K_LIFT_TAIL }; -float eff_sched_pusher_time = 0.002; +int32_t rw_flap_offset = 0; -float roll_eff_scaling = 1.f; +// for negative values, still should be low_lim < up_lim +inline float bound_or_zero(float value, float low_lim, float up_lim) { + float output = value; + if (low_lim > 0.f) { + if (value < low_lim) { + output = 0.f; + } else if (value > up_lim) { + output = up_lim; + } + } else { + if (value > up_lim) { + output = 0.f; + } else if (value < low_lim) { + output = low_lim; + } + } + return output; +} + +float eff_sched_pusher_time = 0.002; struct rotwing_eff_sched_var_t eff_sched_var; @@ -204,8 +224,9 @@ void eff_scheduling_rotwing_init(void) eff_sched_var.sinr3 = 0; // Set moment derivative variables - eff_sched_var.pitch_motor_dMdpprz = ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH; - eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + (MAX_PPRZ/2.) * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust + float hover_thrust = 6000; + eff_sched_var.pitch_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.; // Dmdpprz hover pitch for hover thrust + eff_sched_var.roll_motor_dMdpprz = (eff_sched_p.DMdpprz_hover_roll[0] + 2*hover_thrust * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; // Dmdpprz hover roll for hover thrust eff_sched_var.cmd_elevator = 0; eff_sched_var.cmd_pusher = 0; @@ -282,44 +303,45 @@ void eff_scheduling_rotwing_update_airspeed(void) void eff_scheduling_rotwing_update_hover_motor_effectiveness(void) { - // Pitch motor effectiveness + float cmd_quat[4]; + float dM_dpprz[4]; + // Quadratic thrust (and therefore moment) model of the hover propellers + for (uint8_t i = 0; i < 4; i++) { + cmd_quat[i] = actuator_state_filt_vect[i]; + Bound(cmd_quat[i], 2500, MAX_PPRZ); - float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy; - - float cmd_right = actuator_state_filt_vect[1]; - float cmd_left = actuator_state_filt_vect[3]; - Bound(cmd_right, 3500, MAX_PPRZ); - Bound(cmd_left, 3500, MAX_PPRZ); + if(i==0 || i==2) { // pitch motors + dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_pitch[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_pitch[1]) / 10000.; + // Bound dM_dpprz to half and 2 times the hover effectiveness + Bound(dM_dpprz[i], eff_sched_var.pitch_motor_dMdpprz * 0.5, eff_sched_var.pitch_motor_dMdpprz * 2.0); + } else { // roll motors + dM_dpprz[i] = (eff_sched_p.DMdpprz_hover_roll[0] + 2*cmd_quat[i] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; + Bound(dM_dpprz[i], eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0); + } + } // Roll motor effectiveness - float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_right * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; - float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_left * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; - - dM_dpprz_right = dM_dpprz_right * roll_eff_scaling; - dM_dpprz_left = dM_dpprz_left * roll_eff_scaling; - - // Bound dM_dpprz to half and 2 times the hover effectiveness - Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0); - Bound(dM_dpprz_left, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0); + float dM_dpprz_right = dM_dpprz[1]; + float dM_dpprz_left = dM_dpprz[3];; float roll_motor_p_eff_right = -(dM_dpprz_right * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx; - Bound(roll_motor_p_eff_right, -1, -0.00001); + roll_motor_p_eff_right = bound_or_zero(roll_motor_p_eff_right, -1.f, -0.00001f); float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx; if(autopilot.in_flight) { float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx; roll_motor_p_eff_left += roll_motor_airspeed_compensation; } - Bound(roll_motor_p_eff_left, 0.00001, 1); + roll_motor_p_eff_left = bound_or_zero(roll_motor_p_eff_left, 0.00001f, 1.f); float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy; Bound(roll_motor_q_eff, 0, 1); // Update front pitch motor q effectiveness - g1g2[1][0] = pitch_motor_q_eff; // pitch effectiveness front motor + g1g2[1][0] = dM_dpprz[0] / eff_sched_var.Iyy; // pitch effectiveness front motor // Update back motor q effectiveness - g1g2[1][2] = -pitch_motor_q_eff; // pitch effectiveness back motor + g1g2[1][2] = - dM_dpprz[2] / eff_sched_var.Iyy; // pitch effectiveness back motor // Update right motor p and q effectiveness g1g2[0][1] = roll_motor_p_eff_right; // roll effectiveness right motor (no airspeed compensation) @@ -376,6 +398,11 @@ void eff_scheduling_rotwing_update_aileron_effectiveness(void) float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx; Bound(eff_x_aileron, 0, 0.005) g1g2[0][6] = eff_x_aileron; + + float dMydpprz = 4.0*(eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr2 * eff_sched_var.cosr) / 1000000.; + float eff_y_aileron = dMydpprz / eff_sched_var.Iyy; + eff_y_aileron = bound_or_zero(eff_y_aileron, 0.00003f, 0.005f); + g1g2[1][6] = eff_y_aileron; } void eff_scheduling_rotwing_update_flaperon_effectiveness(void) @@ -427,11 +454,18 @@ void stabilization_indi_set_wls_settings(void) wls_stab_p.u_min[i] = -MAX_PPRZ * act_is_servo[i]; wls_stab_p.u_max[i] = MAX_PPRZ; wls_stab_p.u_pref[i] = act_pref[i]; - if (i == 5) { - wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in preferred state to 0 for elevator + if (i == 5) { // elevator + wls_stab_p.u_pref[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator wls_stab_p.u_min[i] = 0; // cmd 0 is lowest position for elevator } - if (i==8) { + if (i == 7) { // flaperons + // If an offset is used, limit the max differential command to prevent unilateral saturation. + int32_t flap_saturation_limit = MAX_PPRZ - abs(rw_flap_offset); + BoundAbs(flap_saturation_limit, MAX_PPRZ); + wls_stab_p.u_min[i] = -flap_saturation_limit; + wls_stab_p.u_max[i] = flap_saturation_limit; + } + if (i==8) { // pusher // dt (min to max) MAX_PPRZ / (dt * f) dt_min == 0.002 Bound(eff_sched_pusher_time, 0.002, 5.); float max_increment = MAX_PPRZ / (eff_sched_pusher_time * 500); diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.h b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.h index 5b08e15002..da59fcfcbf 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.h +++ b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.h @@ -36,6 +36,7 @@ struct rotwing_eff_sched_param_t { float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²] float m; // mass [kg] float DMdpprz_hover_roll[2]; // Moment coeficients for roll motors (Scaled by 10000) + float DMdpprz_hover_pitch[2]; // Moment coeficients for pitch motors (Scaled by 10000) float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors float hover_roll_roll_coef[2]; // Model coefficients to correct roll effectiveness for roll motors float k_elevator[3]; @@ -50,6 +51,7 @@ struct rotwing_eff_sched_param_t { float k_lift_fuselage; float k_lift_tail; }; +extern struct rotwing_eff_sched_param_t eff_sched_p; struct rotwing_eff_sched_var_t { float Ixx; // Total MMOI around roll axis [kgm²] @@ -77,12 +79,13 @@ struct rotwing_eff_sched_var_t { float airspeed2; }; -extern float roll_eff_scaling; +extern int32_t rw_flap_offset; extern float rotation_angle_setpoint_deg; extern int16_t rotation_cmd; extern float eff_sched_pusher_time; +extern float roll_eff_slider; extern void eff_scheduling_rotwing_init(void); extern void eff_scheduling_rotwing_periodic(void); diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c index b4a7e03ee9..a31c5c14de 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c @@ -87,12 +87,18 @@ float nav_hybrid_line_gain = 1.0f; #define NAV_HYBRID_NAV_CIRCLE_DIST 40.f #endif -#ifdef NAV_HYBRID_POS_GAIN +#ifdef GUIDANCE_INDI_POS_GAIN +float nav_hybrid_pos_gain = GUIDANCE_INDI_POS_GAIN; +#elif defined(NAV_HYBRID_POS_GAIN) float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN; #else float nav_hybrid_pos_gain = 1.0f; #endif +#if defined(NAV_HYBRID_POS_GAIN) && defined(GUIDANCE_INDI_POS_GAIN) +#warning "NAV_HYBRID_POS_GAIN and GUIDANCE_INDI_POS_GAIN serve similar purpose and are both defined, using GUIDANCE_INDI_POS_GAIN" +#endif + #ifndef GUIDANCE_INDI_HYBRID bool force_forward = 0.0f; #endif diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.c b/sw/airborne/modules/rotwing_drone/rotwing_state.c index ed3032d796..500956c40a 100644 --- a/sw/airborne/modules/rotwing_drone/rotwing_state.c +++ b/sw/airborne/modules/rotwing_drone/rotwing_state.c @@ -26,134 +26,91 @@ #include "modules/rotwing_drone/rotwing_state.h" #include "firmwares/rotorcraft/stabilization/stabilization_indi.h" #include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" -#include "modules/nav/nav_rotorcraft_hybrid.h" #include "firmwares/rotorcraft/autopilot_firmware.h" #include "modules/actuators/actuators.h" #include "modules/core/abi.h" -// Quad state identification -#ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD -#define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0 -#endif -#ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER -#define ROTWING_MIN_SKEW_ANGLE_COUNTER 10 // Minimum number of loops the skew angle is below ROTWING_MIN_SKEW_ANGLE_COUNTER +/* Minimum measured RPM to consider the hover motors running (RPM) */ +#ifndef ROTWING_QUAD_MIN_RPM +#define ROTWING_QUAD_MIN_RPM 400 #endif -// Skewing state identification -#ifndef ROTWING_SKEWING_COUNTER -#define ROTWING_SKEWING_COUNTER 10 // Minimum number of loops the skew angle is in between QUAD and FW +/* Minimum measured RPM to consider the pusher motor running (RPM) */ +#ifndef ROTWING_PUSH_MIN_RPM +#define ROTWING_PUSH_MIN_RPM 300 #endif -// maximum quad airspeed to force quad state -#ifndef ROTWING_MAX_QUAD_AIRSPEED -#define ROTWING_MAX_QUAD_AIRSPEED 20.0 +/* Timeout for the RPM feedback (seconds) */ +#ifndef ROTWING_RPM_TIMEOUT +#define ROTWING_RPM_TIMEOUT 1.0 #endif -// Half skew state identification -#ifndef ROTWING_HALF_SKEW_ANGLE_DEG -#define ROTWING_HALF_SKEW_ANGLE_DEG 55.0 -#endif -#ifndef ROTWING_HALF_SKEW_ANGLE_RANG -#define ROTWING_HALF_SKEW_ANGLE_HALF_RANGE 10.0 -#endif -#ifndef ROTWING_HALF_SKEW_COUNTER -#define ROTWING_HALF_SKEW_COUNTER 10 // Minimum number of loops the skew angle is at HALF_SKEW_ANGLE_DEG +/- ROTWING_HALF_SKEW_ANGLE_HALF_RANGE to trigger ROTWING_HALF_SKEW_ANGLE state +/* Minimum thrust to consider the hover motors idling (PPRZ) */ +#ifndef ROTWING_QUAD_IDLE_MIN_THRUST +#define ROTWING_QUAD_IDLE_MIN_THRUST 100 #endif -// FW state identification -#ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG -#define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state -#endif -#ifndef ROTWING_MIN_FW_COUNTER -#define ROTWING_MIN_FW_COUNTER 10 // Minimum number of loops the skew angle is above the MIN_FW_SKEW_ANGLE +/* Minimum time for the hover motors to be considered idling (seconds) */ +#ifndef ROTWING_QUAD_IDLE_TIMEOUT +#define ROTWING_QUAD_IDLE_TIMEOUT 0.4 #endif -// FW idle state identification -#ifndef ROTWING_MIN_THRUST_IDLE -#define ROTWING_MIN_THRUST_IDLE 100 -#endif -#ifndef ROTWING_MIN_THRUST_IDLE_COUNTER -#define ROTWING_MIN_THRUST_IDLE_COUNTER 10 +/* Minimum measured skew angle to consider the rotating wing in fixedwing (deg) */ +#ifndef ROTWING_FW_SKEW_ANGLE +#define ROTWING_FW_SKEW_ANGLE 85.0 #endif -// FW hov mot off state identification -#ifndef ROTWING_HOV_MOT_RUN_RPM_TH -#define ROTWING_HOV_MOT_RUN_RPM_TH 800 -#endif -#ifndef ROTWING_HOV_MOT_OFF_RPM_TH -#define ROTWING_HOV_MOT_OFF_RPM_TH 50 -#endif -#ifndef ROTWING_HOV_MOT_OFF_COUNTER -#define ROTWING_HOV_MOT_OFF_COUNTER 10 +/* Magnitude skew angle jump away from quad */ +#ifndef ROTWING_SKEW_ANGLE_STEP +#define ROTWING_SKEW_ANGLE_STEP 55.0 #endif -#ifndef ROTWING_STATE_USE_ROTATION_REF_MODEL -#define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE +/* TODO: Give a name.... */ +#ifndef ROTWING_SKEW_BACK_MARGIN +#define ROTWING_SKEW_BACK_MARGIN 5.0 #endif - -// Hover preferred pitch (deg) -#ifndef ROTWING_STATE_HOVER_PREF_PITCH -#define ROTWING_STATE_HOVER_PREF_PITCH 0.0 +/* Maximum angle difference between the measured skew and modelled skew for skew failure detection */ +#ifndef ROTWING_SKEW_REF_MODEL_MAX_DIFF +#define ROTWING_SKEW_REF_MODEL_MAX_DIFF 5.f #endif -// Transition preffered pitch (deg) -#ifndef ROTWING_STATE_TRANSITION_PREF_PITCH -#define ROTWING_STATE_TRANSITION_PREF_PITCH 3.0 +/* Skew angle at which the mininum airspeed starts its linear portion */ +#ifndef ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE +#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE 30.0 #endif -// Forward preffered pitch (deg) -#ifndef ROTWING_STATE_FW_PREF_PITCH -#define ROTWING_STATE_FW_PREF_PITCH 8.0 +/* Preferred pitch angle for the quad mode (deg) */ +#ifndef ROTWING_QUAD_PREF_PITCH +#define ROTWING_QUAD_PREF_PITCH -5.0 #endif -// stream ADC data if ADC rotation sensor -#ifndef ADC_WING_ROTATION -#define ADC_WING_ROTATION FALSE +/* Amount of time the airspeed needs to be below the FW_MIN_AIRSPEED */ +#ifndef ROTWING_FW_STALL_TIMEOUT +#define ROTWING_FW_STALL_TIMEOUT 0.5 #endif -#if ADC_WING_ROTATION -#include "wing_rotation_adc_sensor.h" + +/* Fix for not having double can busses */ +#ifndef SERVO_BMOTOR_PUSH_IDX +#define SERVO_BMOTOR_PUSH_IDX SERVO_MOTOR_PUSH_IDX #endif -/** ABI binding feedback data. - */ + +/* Fix for not having double can busses */ +#ifndef SERVO_BROTATION_MECH_IDX +#define SERVO_BROTATION_MECH_IDX SERVO_ROTATION_MECH_IDX +#endif + +/** ABI binding feedback data */ #ifndef ROTWING_STATE_ACT_FEEDBACK_ID #define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST #endif abi_event rotwing_state_feedback_ev; static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act); -#define ROTWING_STATE_NUM_HOVER_RPM 4 -int32_t rotwing_state_hover_rpm[ROTWING_STATE_NUM_HOVER_RPM] = {0, 0, 0, 0}; -struct RotwingState rotwing_state; -struct RotWingStateSettings rotwing_state_settings; -struct RotWingStateSkewing rotwing_state_skewing; - -uint8_t rotwing_state_hover_counter = 0; -uint8_t rotwing_state_skewing_counter = 0; -uint8_t rotwing_state_fw_counter = 0; -uint8_t rotwing_state_fw_idle_counter = 0; -uint8_t rotwing_state_fw_m_off_counter = 0; - -float rotwing_state_max_hover_speed = 7; - -bool hover_motors_active = true; -bool bool_disable_hover_motors = false; - -float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; - -inline void rotwing_check_set_current_state(void); -inline void rotwing_switch_state(void); - -inline void rotwing_state_set_hover_settings(void); -inline void rotwing_state_set_skewing_settings(void); -inline void rotwing_state_set_fw_settings(void); -inline void rotwing_state_set_fw_hov_mot_idle_settings(void); -inline void rotwing_state_set_fw_hov_mot_off_settings(void); - -inline void rotwing_state_set_state_settings(void); -inline void rotwing_state_skewer(void); -inline void rotwing_state_free_processor(void); +static bool rotwing_state_hover_motors_idling(void); +static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; +struct rotwing_state_t rotwing_state; inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle); @@ -161,605 +118,355 @@ inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_an #include "modules/datalink/telemetry.h" static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev) { - uint16_t adc_dummy = 0; + // Set the status + union rotwing_bitmask_t status; + status.value = 0; + status.skew_angle_valid = rotwing_state_skew_angle_valid(); + status.hover_motors_enabled = rotwing_state.hover_motors_enabled; + status.hover_motors_idle = rotwing_state_hover_motors_idling(); + status.hover_motors_running = rotwing_state_hover_motors_running(); + status.pusher_motor_running = rotwing_state_pusher_motor_running(); + status.skew_forced = rotwing_state.force_skew; + + // Send the message + uint8_t state = rotwing_state.state; + uint8_t nav_state = rotwing_state.nav_state; pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID, - &rotwing_state.current_state, - &rotwing_state.desired_state, - &rotwing_state_skewing.wing_angle_deg, - &rotwing_state_skewing.wing_angle_deg_sp, - &adc_dummy, - &rotwing_state_skewing.servo_pprz_cmd); + &state, + &nav_state, + &status.value, + &rotwing_state.meas_skew_angle_deg, + &rotwing_state.sp_skew_angle_deg, + &gi_unbounded_airspeed_sp, + &rotwing_state.min_airspeed, + &rotwing_state.max_airspeed); } #endif // PERIODIC_TELEMETRY -void init_rotwing_state(void) +void rotwing_state_init(void) { + // Initialize rotwing state + rotwing_state.state = ROTWING_STATE_FORCE_HOVER; // For takeoff + rotwing_state.nav_state = ROTWING_STATE_FORCE_HOVER; + rotwing_state.hover_motors_enabled = true; + rotwing_state.fw_min_airspeed = ROTWING_FW_MIN_AIRSPEED; + rotwing_state.cruise_airspeed = ROTWING_FW_CRUISE_AIRSPEED; + rotwing_state.sp_skew_angle_deg = 0; + rotwing_state.meas_skew_angle_deg = 0; + rotwing_state.meas_skew_angle_time = 0; + rotwing_state.skew_cmd = 0; + rotwing_state.force_skew = false; + for (int i = 0; i < 5; i++) { + rotwing_state.meas_rpm[i] = 0; + rotwing_state.meas_rpm_time[i] = 0; + } + rotwing_state.fail_skew_angle = false; + rotwing_state.fail_hover_motor = false; + rotwing_state.fail_pusher_motor = false; + rotwing_state.ref_model_skew_angle_deg = 0; + // Bind ABI messages AbiBindMsgACT_FEEDBACK(ROTWING_STATE_ACT_FEEDBACK_ID, &rotwing_state_feedback_ev, rotwing_state_feedback_cb); - // Start the drone in a desired hover state - rotwing_state.current_state = ROTWING_STATE_HOVER; - rotwing_state.desired_state = ROTWING_STATE_HOVER; - rotwing_state.requested_config = ROTWING_CONFIGURATION_HOVER; - - rotwing_state_skewing.wing_angle_deg_sp = 0; - rotwing_state_skewing.wing_angle_deg = 0; - rotwing_state_skewing.servo_pprz_cmd = -MAX_PPRZ; - rotwing_state_skewing.airspeed_scheduling = false; - rotwing_state_skewing.force_rotation_angle = false; - #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state); #endif } -void periodic_rotwing_state(void) -{ - // Check and set the current state - rotwing_check_set_current_state(); +/** + * @brief Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDLE_TIMEOUT time + * @return true if hover motors are idling, false otherwise + */ +static bool rotwing_state_hover_motors_idling(void) { + static float last_idle_time = 0; + // Check if hover motors are idling and reset timer + if(stabilization.cmd[COMMAND_THRUST] > ROTWING_QUAD_IDLE_MIN_THRUST) { + last_idle_time = get_sys_time_float(); + } - // Check and update desired state - if (guidance_h.mode == GUIDANCE_H_MODE_NAV) { - // Run the free state requester if free configuration requestes - if(rotwing_state.requested_config == ROTWING_CONFIGURATION_FREE) { - rotwing_state_free_processor(); + // Check if we exceeded the idle timeout + if(get_sys_time_float() - last_idle_time > ROTWING_QUAD_IDLE_TIMEOUT) { + return true; + } + return false; +} + +void rotwing_state_periodic(void) +{ + /* Get some genericly used variables */ + static float last_stall_time = 0; + float current_time = get_sys_time_float(); + float meas_airspeed = stateGetAirspeed_f(); + float meas_skew_angle = rotwing_state.meas_skew_angle_deg; + Bound(meas_skew_angle, 0, 90); // Bound to prevent errors + + if(meas_airspeed > rotwing_state.fw_min_airspeed) { + last_stall_time = current_time; + } + + /* Override modes if flying with RC */ + rotwing_state.state = rotwing_state.nav_state; + if(guidance_h.mode == GUIDANCE_H_MODE_NONE) { + // Kill mode + if(stabilization.mode == STABILIZATION_MODE_NONE) { + rotwing_state.state = ROTWING_STATE_FORCE_HOVER; } - rotwing_switch_state(); - } else if (guidance_h.mode == GUIDANCE_H_MODE_NONE) { - if (stabilization.mode == STABILIZATION_MODE_ATTITUDE) { + // ATT and ATT_FWD + else if(stabilization.mode == STABILIZATION_MODE_ATTITUDE) { if (stabilization.att_submode == STABILIZATION_ATT_SUBMODE_FORWARD) { - rotwing_state_set_fw_settings(); + rotwing_state.state = ROTWING_STATE_FORCE_FW; } else { - rotwing_state_set_hover_settings(); + rotwing_state.state = ROTWING_STATE_FORCE_HOVER; } } } - // Run the wing skewer - rotwing_state_skewer(); - - //TODO: incorparate motor active / disbaling depending on called flight state - // Switch on motors if flight mode is attitude - if (guidance_h.mode == GUIDANCE_H_MODE_NONE) { - bool_disable_hover_motors = false; + /* Handle the quad motors on/off state */ + if(rotwing_state.state == ROTWING_STATE_FORCE_HOVER || rotwing_state.state == ROTWING_STATE_REQUEST_HOVER) { + rotwing_state.hover_motors_enabled = true; + } + else if((current_time - last_stall_time) < ROTWING_FW_STALL_TIMEOUT && rotwing_state_hover_motors_idling() && rotwing_state_pusher_motor_running() && meas_skew_angle >= ROTWING_FW_SKEW_ANGLE + && (gi_unbounded_airspeed_sp >= rotwing_state.fw_min_airspeed || rotwing_state.state != ROTWING_STATE_FREE)) { + rotwing_state.hover_motors_enabled = false; + } + else { + rotwing_state.hover_motors_enabled = true; } -} + /* Calculate min/max airspeed bounds based on skew angle */ + float skew_min_airspeed = ROTWING_FW_QUAD_MIN_AIRSPEED * (meas_skew_angle - ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE) / (90.f - ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE); + float skew_max_airspeed = ROTWING_QUAD_MAX_AIRSPEED + (ROTWING_FW_MAX_AIRSPEED - ROTWING_QUAD_MAX_AIRSPEED) * meas_skew_angle / ROTWING_FW_SKEW_ANGLE; + Bound(skew_min_airspeed, 0, rotwing_state.fw_min_airspeed); + Bound(skew_max_airspeed, ROTWING_QUAD_MAX_AIRSPEED, ROTWING_FW_MAX_AIRSPEED); -// Function to request a state -void request_rotwing_state(uint8_t state) -{ - if (state <= ROTWING_STATE_FW_HOV_MOT_OFF) { - rotwing_state.desired_state = state; - } -} - -// Function to prefer configuration -void rotwing_request_configuration(uint8_t configuration) -{ - switch (configuration) { - case ROTWING_CONFIGURATION_HOVER: - request_rotwing_state(ROTWING_STATE_HOVER); - rotwing_state.requested_config = ROTWING_CONFIGURATION_HOVER; - break; - case ROTWING_CONFIGURATION_HYBRID: - request_rotwing_state(ROTWING_STATE_SKEWING); - rotwing_state.requested_config = ROTWING_CONFIGURATION_HYBRID; - break; - case ROTWING_CONFIGURATION_EFFICIENT: - request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF); - rotwing_state.requested_config = ROTWING_CONFIGURATION_EFFICIENT; - break; - case ROTWING_CONFIGURATION_FREE: - rotwing_state.requested_config = ROTWING_CONFIGURATION_FREE; - break; - } -} - -void rotwing_check_set_current_state(void) -{ - // if !in_flight, set state to hover - if (!autopilot.in_flight) { - rotwing_state_hover_counter = 0; - rotwing_state_skewing_counter = 0; - rotwing_state_fw_counter = 0; - rotwing_state_fw_idle_counter = 0; - rotwing_state_fw_m_off_counter = 0; - rotwing_state.current_state = ROTWING_STATE_HOVER; - return; + if(!rotwing_state_hover_motors_running() || !rotwing_state.hover_motors_enabled) { + skew_min_airspeed = rotwing_state.fw_min_airspeed; + skew_max_airspeed = ROTWING_FW_MAX_AIRSPEED; } - // States can be checked according to wing angle sensor, setpoints ..... - uint8_t prev_state = rotwing_state.current_state; - switch (prev_state) { - case ROTWING_STATE_HOVER: - // Check if state needs to be set to skewing - if (rotwing_state_skewing.wing_angle_deg > ROTWING_MIN_SKEW_ANGLE_DEG_QUAD) { - rotwing_state_skewing_counter++; - } else { - rotwing_state_skewing_counter = 0; - } - // Switch state if necessary - if (rotwing_state_skewing_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_SKEWING; - rotwing_state_skewing_counter = 0; - } - break; - - case ROTWING_STATE_SKEWING: - // Check if state needs to be set to hover - if (rotwing_state_skewing.wing_angle_deg < ROTWING_MIN_SKEW_ANGLE_DEG_QUAD) { - rotwing_state_hover_counter++; - } else { - rotwing_state_hover_counter = 0; - } - - // Check if state needs to be set to fixed wing - if (rotwing_state_skewing.wing_angle_deg > ROTWING_MIN_FW_SKEW_ANGLE_DEG) { - rotwing_state_fw_counter++; - } else { - rotwing_state_fw_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_hover_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_HOVER; - rotwing_state_hover_counter = 0; - } - - if (rotwing_state_fw_counter > ROTWING_MIN_FW_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_FW; - rotwing_state_fw_counter = 0; - } - break; - - case ROTWING_STATE_FW: - // Check if state needs to be set to skewing - if (rotwing_state_skewing.wing_angle_deg < ROTWING_MIN_FW_SKEW_ANGLE_DEG) { - rotwing_state_skewing_counter++; - } else { - rotwing_state_skewing_counter = 0; - } - - // Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold) - if (stabilization.cmd[COMMAND_THRUST] < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state > ROTWING_STATE_FW) { - rotwing_state_fw_idle_counter++; - } else { - rotwing_state_fw_idle_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_skewing_counter > ROTWING_MIN_FW_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_SKEWING; - rotwing_state_skewing_counter = 0; - rotwing_state_fw_idle_counter = 0; - } else if (rotwing_state_fw_idle_counter > ROTWING_MIN_THRUST_IDLE_COUNTER - && rotwing_state_skewing_counter == 0) { - rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE; - rotwing_state_skewing_counter = 0; - rotwing_state_fw_idle_counter = 0; - } - break; - - case ROTWING_STATE_FW_HOV_MOT_IDLE: - // Check if state needs to be set to fixed wing with hover motors activated - if (stabilization.cmd[COMMAND_THRUST] > ROTWING_MIN_THRUST_IDLE - || rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) { - rotwing_state_fw_counter++; - } else { - rotwing_state_fw_counter = 0; - } - - // Check if state needs to be set to fixed wing with hover motors off (if zero rpm on hover motors) - if (rotwing_state_hover_rpm[0] == 0 - && rotwing_state_hover_rpm[1] == 0 - && rotwing_state_hover_rpm[2] == 0 - && rotwing_state_hover_rpm[3] == 0) { -#if !USE_NPS - rotwing_state_fw_m_off_counter++; -#endif - } else { - rotwing_state_fw_m_off_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_fw_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_FW; - rotwing_state_fw_counter = 0; - rotwing_state_fw_m_off_counter = 0; - } else if (rotwing_state_fw_m_off_counter > ROTWING_HOV_MOT_OFF_COUNTER - && rotwing_state_fw_counter == 0) { - rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_OFF; - rotwing_state_fw_counter = 0; - rotwing_state_fw_m_off_counter = 0; - } - break; - - case ROTWING_STATE_FW_HOV_MOT_OFF: - // Check if state needs to be set to fixed wing with hover motors idle (if rpm on hover motors) - if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_OFF_RPM_TH - && rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_OFF_RPM_TH - && rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_OFF_RPM_TH - && rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_OFF_RPM_TH) { - rotwing_state_fw_idle_counter++; - } else { - rotwing_state_fw_idle_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_fw_idle_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE; - rotwing_state_fw_idle_counter = 0; - } - break; - - default: - break; + /* Handle the skew angle setpoint */ + if (rotwing_state.force_skew) { + // Do nothing } -} - -// Function that handles settings for switching state(s) -void rotwing_switch_state(void) -{ - switch (rotwing_state.current_state) { - case ROTWING_STATE_HOVER: - if (rotwing_state.desired_state > ROTWING_STATE_HOVER) { - rotwing_state_set_skewing_settings(); - } else { - rotwing_state_set_hover_settings(); - } - break; - - case ROTWING_STATE_SKEWING: - if (rotwing_state.desired_state < ROTWING_STATE_SKEWING && stateGetAirspeed_f() < ROTWING_MAX_QUAD_AIRSPEED) { - rotwing_state_set_hover_settings(); - } else { - rotwing_state_set_skewing_settings(); - } - break; - - case ROTWING_STATE_FW: - if (rotwing_state.desired_state > ROTWING_STATE_FW) { - rotwing_state_set_fw_hov_mot_idle_settings(); - } else if (rotwing_state.desired_state < ROTWING_STATE_FW) { - rotwing_state_set_skewing_settings(); - } else { - rotwing_state_set_fw_settings(); - } - break; - - case ROTWING_STATE_FW_HOV_MOT_IDLE: - if (rotwing_state.desired_state > ROTWING_STATE_FW_HOV_MOT_IDLE) { - rotwing_state_set_fw_hov_mot_off_settings(); - } else if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) { - rotwing_state_set_fw_settings(); - } else { - rotwing_state_set_fw_hov_mot_idle_settings(); - } - break; - - case ROTWING_STATE_FW_HOV_MOT_OFF: - if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_OFF) { - rotwing_state_set_fw_hov_mot_idle_settings(); - } else { - rotwing_state_set_fw_hov_mot_off_settings(); - } - break; + else if(rotwing_state.state == ROTWING_STATE_FORCE_HOVER) { + rotwing_state.sp_skew_angle_deg = 0.f; } -} - -void rotwing_state_set_hover_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_QUAD_SETTING; - rotwing_state_settings.hover_motors_active = true; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = false; - rotwing_state_settings.preferred_pitch = ROTWING_STATE_PITCH_QUAD_SETTING; - rotwing_state_settings.stall_protection = false; - rotwing_state_settings.max_v_climb = 2.0; - rotwing_state_settings.max_v_descend = 1.0; - rotwing_state_settings.nav_max_speed = rotwing_state_max_hover_speed; // Using setting - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_skewing_settings(void) -{ - // Wing may be skewed to quad when desired state is hover and skewing settings set by state machine - if (rotwing_state.desired_state == ROTWING_STATE_HOVER) { - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_QUAD_SETTING; - } else { - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_SCHEDULING_SETTING; + else if(!rotwing_state_hover_motors_running() || !rotwing_state.hover_motors_enabled || rotwing_state.state == ROTWING_STATE_FORCE_FW) { + rotwing_state.sp_skew_angle_deg = 90.f; } - rotwing_state_settings.hover_motors_active = true; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = false; - rotwing_state_settings.preferred_pitch = ROTWING_STATE_PITCH_TRANSITION_SETTING; - rotwing_state_settings.stall_protection = false; - rotwing_state_settings.max_v_climb = 2.0; - rotwing_state_settings.max_v_descend = 1.0; - rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_fw_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; - rotwing_state_settings.hover_motors_active = true; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = true; - rotwing_state_settings.preferred_pitch = ROTWING_STATE_PITCH_FW_SETTING; - rotwing_state_settings.stall_protection = false; - rotwing_state_settings.max_v_climb = 4.0; - rotwing_state_settings.max_v_descend = 4.0; - rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_fw_hov_mot_idle_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; - rotwing_state_settings.hover_motors_active = false; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = true; - rotwing_state_settings.preferred_pitch = ROTWING_STATE_PITCH_FW_SETTING; - rotwing_state_settings.stall_protection = true; - rotwing_state_settings.max_v_climb = 4.0; - rotwing_state_settings.max_v_descend = 4.0; - rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_fw_hov_mot_off_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; - rotwing_state_settings.hover_motors_active = false; - rotwing_state_settings.hover_motors_disable = true; - rotwing_state_settings.force_forward = true; - rotwing_state_settings.preferred_pitch = ROTWING_STATE_PITCH_FW_SETTING; - rotwing_state_settings.stall_protection = true; - rotwing_state_settings.max_v_climb = 4.0; - rotwing_state_settings.max_v_descend = 4.0; - rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_state_settings(void) -{ - - if (!rotwing_state_skewing.force_rotation_angle) { - switch (rotwing_state_settings.wing_scheduler) { - case ROTWING_STATE_WING_QUAD_SETTING: - rotwing_state_skewing.airspeed_scheduling = false; - rotwing_state_skewing.wing_angle_deg_sp = 0; - break; - case ROTWING_STATE_WING_SCHEDULING_SETTING: - rotwing_state_skewing.airspeed_scheduling = true; - break; - case ROTWING_STATE_WING_FW_SETTING: - rotwing_state_skewing.airspeed_scheduling = true; - break; - } - } else { - rotwing_state_skewing.airspeed_scheduling = false; + else if(!rotwing_state_pusher_motor_running()) { + rotwing_state.sp_skew_angle_deg = 0.f; } - - hover_motors_active = rotwing_state_settings.hover_motors_active; - - bool_disable_hover_motors = rotwing_state_settings.hover_motors_disable; - - force_forward = rotwing_state_settings.force_forward; - - nav_max_speed = rotwing_state_settings.nav_max_speed; - - // TO DO: pitch angle now hard coded scheduled by wing angle - - // Stall protection handled by hover_motors_active boolean - - // TO DO: Climb and descend speeds now handled by guidance airspeed -} - -void rotwing_state_skewer(void) -{ - if (rotwing_state_skewing.airspeed_scheduling) { - float wing_angle_scheduled_sp_deg = 0; - float airspeed = stateGetAirspeed_f(); - if (airspeed < 8) { - wing_angle_scheduled_sp_deg = 0; - } else if (airspeed < 10 && (rotwing_state.desired_state > ROTWING_STATE_HOVER)) { - wing_angle_scheduled_sp_deg = 55; - } else if (airspeed > 10) { - wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.; + else if(rotwing_state.state == ROTWING_STATE_REQUEST_HOVER && meas_skew_angle <= (ROTWING_SKEW_ANGLE_STEP + ROTWING_SKEW_BACK_MARGIN)) { + rotwing_state.sp_skew_angle_deg = 0.f; + } + else { + // SKEWING function based on Vair + if (meas_airspeed < ROTWING_SKEW_DOWN_AIRSPEED) { + rotwing_state.sp_skew_angle_deg = 0.f; + } else if (meas_airspeed < ROTWING_SKEW_UP_AIRSPEED) { + // Hysteresis do nothing + } else if (meas_airspeed < ROTWING_QUAD_MAX_AIRSPEED) { + rotwing_state.sp_skew_angle_deg = ROTWING_SKEW_ANGLE_STEP; } else { - wing_angle_scheduled_sp_deg = 0; + rotwing_state.sp_skew_angle_deg = ((meas_airspeed - ROTWING_QUAD_MAX_AIRSPEED)) / (rotwing_state.fw_min_airspeed - ROTWING_QUAD_MAX_AIRSPEED) * (90.f - ROTWING_SKEW_ANGLE_STEP) + ROTWING_SKEW_ANGLE_STEP; } - - Bound(wing_angle_scheduled_sp_deg, 0., 90.) - rotwing_state_skewing.wing_angle_deg_sp = wing_angle_scheduled_sp_deg; } -} + Bound(rotwing_state.sp_skew_angle_deg, 0.f, 90.f); -void rotwing_state_free_processor(void) -{ - // Get current speed vector - struct EnuCoor_f *groundspeed = stateGetSpeedEnu_f(); - float current_groundspeed = FLOAT_VECT2_NORM(*groundspeed); - // Get current airspeed - float airspeed = stateGetAirspeed_f(); + /* Handle the airspeed bounding */ + Bound(rotwing_state.cruise_airspeed, rotwing_state.fw_min_airspeed, ROTWING_FW_MAX_AIRSPEED); + if((!rotwing_state_hover_motors_running() && rotwing_state.state != ROTWING_STATE_FORCE_HOVER) || rotwing_state.state == ROTWING_STATE_FORCE_FW) { + rotwing_state.min_airspeed = rotwing_state.cruise_airspeed; + rotwing_state.max_airspeed = rotwing_state.cruise_airspeed; + } + else if(!rotwing_state_pusher_motor_running()) { + rotwing_state.min_airspeed = 0; + rotwing_state.max_airspeed = ROTWING_QUAD_NOPUSH_AIRSPEED; + } + else if(rotwing_state.state == ROTWING_STATE_FORCE_HOVER || rotwing_state.state == ROTWING_STATE_REQUEST_HOVER) { + rotwing_state.min_airspeed = skew_min_airspeed; + rotwing_state.max_airspeed = fmax(ROTWING_QUAD_MAX_AIRSPEED, skew_min_airspeed); + } + else if(rotwing_state.state == ROTWING_STATE_REQUEST_FW) { + rotwing_state.min_airspeed = fmin(rotwing_state.cruise_airspeed, skew_max_airspeed); + rotwing_state.max_airspeed = fmin(rotwing_state.cruise_airspeed, skew_max_airspeed); + } + else{ + rotwing_state.min_airspeed = skew_min_airspeed; + rotwing_state.max_airspeed = skew_max_airspeed; + } - // Get windspeed vector - struct FloatEulers eulers_zxy; - float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + // Override failing skewing while fwd + /*if(meas_skew_angle > 70 && rotwing_state_.skewing_failing) { + rotwing_state.min_airspeed = 0; // Vstall + margin + rotwing_state.max_airspeed = 0; // Max airspeed FW + }*/ - float psi = eulers_zxy.psi; - float cpsi = cosf(psi); - float spsi = sinf(psi); - struct FloatVect2 airspeed_v = { spsi * airspeed, cpsi * airspeed }; - struct FloatVect2 windspeed_v; - VECT2_DIFF(windspeed_v, *groundspeed, airspeed_v); + guidance_indi_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed); - // Get goto target information - struct FloatVect2 pos_error; - struct EnuCoor_f *pos = stateGetPositionEnu_f(); - VECT2_DIFF(pos_error, nav_rotorcraft_base.goto_wp.to, *pos); - /* - Calculations - */ - // speed over pos_error projection - struct FloatVect2 pos_error_norm; - VECT2_COPY(pos_error_norm, pos_error); - float_vect2_normalize(&pos_error_norm); - float dist_to_target = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp); - float max_speed_decel2 = fabsf(2.f * dist_to_target * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case - float max_speed_decel = sqrtf(max_speed_decel2); + /* Set navigation/guidance settings */ + nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew? - // Check if speed setpoint above set airspeed - struct FloatVect2 desired_airspeed_v; - struct FloatVect2 groundspeed_sp; - groundspeed_sp.x = pos_error.x * nav_hybrid_pos_gain; - groundspeed_sp.y = pos_error.y * nav_hybrid_pos_gain; - VECT2_COPY(desired_airspeed_v, groundspeed_sp); - VECT2_ADD(desired_airspeed_v, windspeed_v); - float desired_airspeed = FLOAT_VECT2_NORM(desired_airspeed_v); - float airspeed_error = guidance_indi_max_airspeed - airspeed; + /* Calculate the skew command */ + float servo_pprz_cmd = MAX_PPRZ * (rotwing_state.sp_skew_angle_deg - 45.f) / 45.f; + BoundAbs(servo_pprz_cmd, MAX_PPRZ); - // Request hybrid if we have to decelerate and approaching target - if (max_speed_decel < current_groundspeed) { - request_rotwing_state(ROTWING_STATE_SKEWING); - } else if ((desired_airspeed > 15) && ((current_groundspeed + airspeed_error) < max_speed_decel)) { - request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF); - } -} - -void rotwing_state_skew_actuator_periodic(void) -{ - - // calc rotation percentage of setpoint (0 deg = -1, 45 deg = 0, 90 deg = 1) - float wing_rotation_percentage = (rotwing_state_skewing.wing_angle_deg_sp - 45.) / 45.; - Bound(wing_rotation_percentage, -1., 1.); - - float servo_pprz_cmd = MAX_PPRZ * wing_rotation_percentage; - // Calulcate rotation_cmd - Bound(servo_pprz_cmd, -MAX_PPRZ, MAX_PPRZ); - -#if ROTWING_STATE_USE_ROTATION_REF_MODEL // Rotate with second order filter static float rotwing_state_skew_p_cmd = -MAX_PPRZ; static float rotwing_state_skew_d_cmd = 0; - float speed_sp = 0.001 * (servo_pprz_cmd - rotwing_state_skew_p_cmd); - rotwing_state_skew_d_cmd += 0.003 * (speed_sp - rotwing_state_skew_d_cmd); + + float speed_sp = ROTWING_SKEW_REF_MODEL_P_GAIN * (servo_pprz_cmd - rotwing_state_skew_p_cmd); + BoundAbs(speed_sp, ROTWING_SKEW_REF_MODEL_MAX_SPEED); + rotwing_state_skew_d_cmd += ROTWING_SKEW_REF_MODEL_D_GAIN * (speed_sp - rotwing_state_skew_d_cmd); rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd; - Bound(rotwing_state_skew_p_cmd, -MAX_PPRZ, MAX_PPRZ); - rotwing_state_skewing.servo_pprz_cmd = rotwing_state_skew_p_cmd; + BoundAbs(rotwing_state_skew_p_cmd, MAX_PPRZ); + rotwing_state.ref_model_skew_angle_deg = 45.0 / MAX_PPRZ * rotwing_state_skew_p_cmd + 45.0; + +#if (ROTWING_SKEW_REF_MODEL || USE_NPS) + rotwing_state.skew_cmd = rotwing_state_skew_p_cmd; #else - // Directly controlling the wing rotation - rotwing_state_skewing.servo_pprz_cmd = servo_pprz_cmd; + rotwing_state.skew_cmd = servo_pprz_cmd; #endif + + /* Add simulation feedback for the skewing and RPM */ #if USE_NPS // Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array - actuators_pprz[INDI_NUM_ACT] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command + actuators_pprz[INDI_NUM_ACT] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command - // Simulate wing angle from command - rotwing_state_skewing.wing_angle_deg = (float) rotwing_state_skewing.servo_pprz_cmd / MAX_PPRZ * 45. + 45.; - - // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback + // SEND ABI Message to ctr_eff_sched, ourself and other modules that want Actuator position feedback struct act_feedback_t feedback; feedback.idx = SERVO_ROTATION_MECH_IDX; - feedback.position = 0.5 * M_PI - RadOfDeg(rotwing_state_skewing.wing_angle_deg); + feedback.position = 0.5f * M_PI - RadOfDeg((float) rotwing_state.skew_cmd / MAX_PPRZ * 45.f + 45.f); feedback.set.position = true; - // Send ABI message - AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); + // Send ABI message (or simulate failure) + if(!rotwing_state.fail_skew_angle) { + AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_BOARD_ID, &feedback, 1); + } + + // Simulate to always have RPM if on and active feedback + rotwing_state.meas_rpm[0] = (actuators[SERVO_MOTOR_FRONT_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0; + rotwing_state.meas_rpm[1] = (actuators[SERVO_MOTOR_RIGHT_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0; + rotwing_state.meas_rpm[2] = (actuators[SERVO_MOTOR_BACK_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0; + rotwing_state.meas_rpm[3] = (actuators[SERVO_MOTOR_LEFT_IDX].pprz_val >= 0)? (ROTWING_QUAD_MIN_RPM + 100) : 0; + rotwing_state.meas_rpm[4] = (actuators[SERVO_MOTOR_PUSH_IDX].pprz_val >= 0)? (ROTWING_PUSH_MIN_RPM + 100) : 0; + for(uint8_t i = 0; i < 5; i++) { + rotwing_state.meas_rpm_time[i] = current_time; + } + +#ifdef SITL + if(rotwing_state.fail_hover_motor) { + rotwing_state.meas_rpm[0] = 0; + rotwing_state.meas_rpm[1] = 0; + rotwing_state.meas_rpm[2] = 0; + rotwing_state.meas_rpm[3] = 0; + } + + if(rotwing_state.fail_pusher_motor) { + rotwing_state.meas_rpm[4] = 0; + } +#endif + #endif } static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id, struct act_feedback_t UNUSED *feedback_msg, uint8_t UNUSED num_act_message) { + float current_time = get_sys_time_float(); for (int i = 0; i < num_act_message; i++) { + int idx = feedback_msg[i].idx; - for (int i = 0; i < num_act_message; i++) { - // Check for wing rotation feedback - if ((feedback_msg[i].set.position) && (feedback_msg[i].idx == SERVO_ROTATION_MECH_IDX)) { - // Get wing rotation angle from sensor - float wing_angle_rad = 0.5 * M_PI - feedback_msg[i].position; - rotwing_state_skewing.wing_angle_deg = DegOfRad(wing_angle_rad); - - // Bound wing rotation angle - Bound(rotwing_state_skewing.wing_angle_deg, 0, 90.); - } + // Check for wing rotation feedback + if ((feedback_msg[i].set.position) && (idx == SERVO_ROTATION_MECH_IDX || idx == SERVO_BROTATION_MECH_IDX)) { + // Get wing rotation angle from sensor + float skew_angle_rad = 0.5 * M_PI - feedback_msg[i].position; + rotwing_state.meas_skew_angle_deg = DegOfRad(skew_angle_rad); + rotwing_state.meas_skew_angle_time = current_time; } - // Sanity check that index is valid - int idx = feedback_msg[i].idx; + // Get the RPM feedbacks of the motors if (feedback_msg[i].set.rpm) { if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) { - rotwing_state_hover_rpm[0] = feedback_msg->rpm; + rotwing_state.meas_rpm[0] = feedback_msg->rpm; + rotwing_state.meas_rpm_time[0] = current_time; } else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) { - rotwing_state_hover_rpm[1] = feedback_msg->rpm; + rotwing_state.meas_rpm[1] = feedback_msg->rpm; + rotwing_state.meas_rpm_time[1] = current_time; } else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) { - rotwing_state_hover_rpm[2] = feedback_msg->rpm; + rotwing_state.meas_rpm[2] = feedback_msg->rpm; + rotwing_state.meas_rpm_time[2] = current_time; } else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) { - rotwing_state_hover_rpm[3] = feedback_msg->rpm; + rotwing_state.meas_rpm[3] = feedback_msg->rpm; + rotwing_state.meas_rpm_time[3] = current_time; + } else if ((idx == SERVO_MOTOR_PUSH_IDX) || (idx == SERVO_BMOTOR_PUSH_IDX)) { + rotwing_state.meas_rpm[4] = feedback_msg->rpm; + rotwing_state.meas_rpm_time[4] = current_time; } } } } bool rotwing_state_hover_motors_running(void) { + float current_time = get_sys_time_float(); // Check if hover motors are running - -#if USE_NPS - return true; -#endif - - if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_RUN_RPM_TH - && rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_RUN_RPM_TH - && rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_RUN_RPM_TH - && rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_RUN_RPM_TH) { + if (rotwing_state.meas_rpm[0] > ROTWING_QUAD_MIN_RPM + && rotwing_state.meas_rpm[1] > ROTWING_QUAD_MIN_RPM + && rotwing_state.meas_rpm[2] > ROTWING_QUAD_MIN_RPM + && rotwing_state.meas_rpm[3] > ROTWING_QUAD_MIN_RPM + && (current_time - rotwing_state.meas_rpm_time[0]) < ROTWING_RPM_TIMEOUT + && (current_time - rotwing_state.meas_rpm_time[1]) < ROTWING_RPM_TIMEOUT + && (current_time - rotwing_state.meas_rpm_time[2]) < ROTWING_RPM_TIMEOUT + && (current_time - rotwing_state.meas_rpm_time[3]) < ROTWING_RPM_TIMEOUT) { return true; } else { return false; } } +bool rotwing_state_pusher_motor_running(void) { + // Check if pusher motor is running + if (rotwing_state.meas_rpm[4] > ROTWING_PUSH_MIN_RPM && (get_sys_time_float() - rotwing_state.meas_rpm_time[4]) < ROTWING_RPM_TIMEOUT) { + return true; + } else { + return false; + } +} + +bool rotwing_state_skew_angle_valid(void) { + // Check if the measured skew angle is timed out and the reference model is matching + bool skew_timeout = (get_sys_time_float() - rotwing_state.meas_skew_angle_time) > ROTWING_RPM_TIMEOUT; + bool skew_angle_match = fabs(rotwing_state.meas_skew_angle_deg - rotwing_state.ref_model_skew_angle_deg) < ROTWING_SKEW_REF_MODEL_MAX_DIFF; + + return (!skew_timeout && skew_angle_match); +} + void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) { // adjust weights - float fixed_wing_percentage = !hover_motors_active; // TODO: when hover props go below 40%, ... - Bound(fixed_wing_percentage, 0, 1); + float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ... + Bound(fixed_wing_percentile, 0, 1); #define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; // Increase importance of forward acceleration in forward flight - wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentage * + wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile * AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) struct FloatEulers eulers_zxy; float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); - // Evaluate motors_on boolean - if (!hover_motors_active) { - if (stateGetAirspeed_f() < 15.) { - hover_motors_active = true; - bool_disable_hover_motors = false; - } else if (eulers_zxy.theta > RadOfDeg(15.0)) { - hover_motors_active = true; - bool_disable_hover_motors = false; - } - } else { - bool_disable_hover_motors = false; - } - float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + - (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * hover_motors_active; + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * rotwing_state_hover_motors_running(); Bound(du_min_thrust_z, -50., 0.); float du_max_thrust_z = -(actuator_state_filt_vect[0] * g1g2[3][0] + actuator_state_filt_vect[1] * g1g2[3][1] + actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]); @@ -772,19 +479,24 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); float quad_pitch_limit_rad = RadOfDeg(5.0); - float scheduled_pitch_angle = 0; + float airspeed = stateGetAirspeed_f(); + + float scheduled_pitch_angle = 0.f; float pitch_angle_range = 3.; - if (rotwing_state_skewing.wing_angle_deg < 55) { - scheduled_pitch_angle = 0; + float meas_skew_angle = rotwing_state.meas_skew_angle_deg; + Bound(meas_skew_angle, 0, 90); // Bound to prevent errors + if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) { + scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH; wls_guid_p.Wu[1] = Wu_gih_original[1]; max_pitch_limit_rad = quad_pitch_limit_rad; } else { - float pitch_progression = (rotwing_state_skewing.wing_angle_deg - 55) / 35.; - scheduled_pitch_angle = pitch_angle_range * pitch_progression; - wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.9); + float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f; + Bound(pitch_progression, 0.f, 1.f); + scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression); + wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99); max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression; } - if (!hover_motors_active) { + if (!rotwing_state_hover_motors_running()) { scheduled_pitch_angle = 8.; max_pitch_limit_rad = fwd_pitch_limit_rad; } @@ -796,14 +508,26 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl // Set lower limits wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch - wls_guid_p.u_min[2] = du_min_thrust_z; - wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); // Set upper limits limits wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch - wls_guid_p.u_max[2] = du_max_thrust_z; - wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition + + if(rotwing_state_hover_motors_running()) { + wls_guid_p.u_min[2] = du_min_thrust_z; + wls_guid_p.u_max[2] = du_max_thrust_z; + } else { + wls_guid_p.u_min[2] = 0.; + wls_guid_p.u_max[2] = 0.; + } + + if(rotwing_state_pusher_motor_running()) { + wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); + wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition + } else { + wls_guid_p.u_min[3] = 0.; + wls_guid_p.u_max[3] = 0.; + } // Set prefered states wls_guid_p.u_pref[0] = 0; // prefered delta roll angle @@ -811,3 +535,86 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration } + +void rotwing_state_set(enum rotwing_states_t state) { + rotwing_state.nav_state = state; +} + + +/* TODO move these rotwing navigation helper functions to an appropriate module/file */ +bool rotwing_state_choose_circle_direction(uint8_t wp_id) { + // Get circle waypoint coordinates in NED + struct FloatVect3 circ_ned = {.x = waypoints[wp_id].enu_f.y, + .y = waypoints[wp_id].enu_f.x, + .z = -waypoints[wp_id].enu_f.z}; + + // Get drone position coordinates in NED + struct FloatVect3 pos_ned = {.x = stateGetPositionNed_f()->x, + .y = stateGetPositionNed_f()->y, + .z = stateGetPositionNed_f()->z}; + + // Get vector pointing from drone to waypoint + struct FloatVect3 vect_pos_circ; + VECT3_DIFF(vect_pos_circ, circ_ned, pos_ned); + + static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0}; + static struct FloatVect3 z_axis = {.x = 0, .y = 0, .z = 1}; + struct FloatVect3 att_NED; + struct FloatVect3 cross_att_circ; + + float_rmat_transp_vmult(&att_NED, stateGetNedToBodyRMat_f(), &x_axis); + + VECT3_CROSS_PRODUCT(cross_att_circ, vect_pos_circ, att_NED); + float y = VECT3_DOT_PRODUCT(cross_att_circ, z_axis); + float x = VECT3_DOT_PRODUCT(vect_pos_circ, att_NED); + + float body_to_wp_angle_rad = atan2f(y, x); + + if (body_to_wp_angle_rad >= 0.f) { + return true; // CCW circle + } else { + return false; // CW circle + } +} + +void rotwing_state_set_transition_wp(uint8_t wp_id) { + + // Get drone position coordinates in NED + struct EnuCoor_f target_enu = {.x = stateGetPositionNed_f()->y, + .y = stateGetPositionNed_f()->x, + .z = -stateGetPositionNed_f()->z}; + + static struct FloatVect3 x_axis = {.x = 1, .y = 0, .z = 0}; + struct FloatVect3 x_att_NED; + + float_rmat_transp_vmult(&x_att_NED, stateGetNedToBodyRMat_f(), &x_axis); + + // set the new transition WP coordinates 100m ahead of the drone + target_enu.x = 100.f * x_att_NED.y + target_enu.x; + target_enu.y = 100.f * x_att_NED.x + target_enu.y; + + waypoint_set_enu(wp_id, &target_enu); + + // Send waypoint update every half second + RunOnceEvery(100 / 2, { + // Send to the GCS that the waypoint has been moved + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); + }); + +} + +void rotwing_state_update_WP_height(uint8_t wp_id, float height) { + struct EnuCoor_f target_enu = {.x = waypoints[wp_id].enu_f.x, + .y = waypoints[wp_id].enu_f.y, + .z = height}; + + waypoint_set_enu(wp_id, &target_enu); + + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); +} diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.h b/sw/airborne/modules/rotwing_drone/rotwing_state.h index 981f7977b1..cfb03d887c 100644 --- a/sw/airborne/modules/rotwing_drone/rotwing_state.h +++ b/sw/airborne/modules/rotwing_drone/rotwing_state.h @@ -28,70 +28,66 @@ #include "std.h" -/** Rotwing States - */ -#define ROTWING_STATE_HOVER 0 // Wing is skewed in 0 degrees (quad) -#define ROTWING_STATE_SKEWING 1 // WIng is skewing -#define ROTWING_STATE_FW 2 // Wing is skewed at 90 degrees (fixed wing), hover motors have full authority -#define ROTWING_STATE_FW_HOV_MOT_IDLE 3 // Wing is skewed at 90 degrees (fixed wing), hover motors are forced to idle -#define ROTWING_STATE_FW_HOV_MOT_OFF 4 // Wing is skewed at 90 degrees (fixed wubg), hover motors are switched off -#define ROTWING_STATE_FREE 5 // This is a desired state for which the controller has to decide the desired state itself - -/** Rotwing Configurations - */ -#define ROTWING_CONFIGURATION_HOVER 0 // UAV is in hover -#define ROTWING_CONFIGURATION_HYBRID 1 // UAV can fly forward and hover if airspeed low, hover motors on -#define ROTWING_CONFIGURATION_EFFICIENT 2 // UAV flies efficiently forward (FW) -#define ROTWING_CONFIGURATION_FREE 3 // UAV switched between efficient and hybrid dependent on deceleration - -struct RotwingState { - uint8_t current_state; - uint8_t desired_state; - uint8_t requested_config; +enum rotwing_states_t { + ROTWING_STATE_FORCE_HOVER, + ROTWING_STATE_REQUEST_HOVER, + ROTWING_STATE_FORCE_FW, + ROTWING_STATE_REQUEST_FW, + ROTWING_STATE_FREE, }; -#define ROTWING_STATE_WING_QUAD_SETTING 0 // Wing skew at 0 degrees -#define ROTWING_STATE_WING_SCHEDULING_SETTING 1 // Wing skew handled by airspeed scheduler -#define ROTWING_STATE_WING_FW_SETTING 2 // Wing skew at 90 degrees - -#define ROTWING_STATE_PITCH_QUAD_SETTING 0 // Pitch at prefered hover -#define ROTWING_STATE_PITCH_TRANSITION_SETTING 1 // Pitch scheduled -#define ROTWING_STATE_PITCH_FW_SETTING 2 // Pitch at prefered forward - -struct RotWingStateSettings { - uint8_t wing_scheduler; - bool hover_motors_active; - bool hover_motors_disable; - bool force_forward; - uint8_t preferred_pitch; - bool stall_protection; - float max_v_climb; - float max_v_descend; - float nav_max_speed; +union rotwing_bitmask_t { + uint16_t value; + struct { + bool skew_angle_valid : 1; // Skew angle didn't timeout + bool hover_motors_enabled : 1; // Hover motors command is enabled + bool hover_motors_idle : 1; // Hover motors are idling (throttle < IDLE_THROTTLE) + bool hover_motors_running : 1; // Hover motors are running (RPM >= MIN_RPM) + bool pusher_motor_running : 1; // Pusher motor is running (RPM >= MIN_RPM) + bool skew_forced : 1; // Skew angle is forced + }; }; -struct RotWingStateSkewing { - float wing_angle_deg_sp; // Wing angle setpoint in deg - float wing_angle_deg; // Wing angle from sensor in deg - int32_t servo_pprz_cmd; // Wing rotation servo pprz cmd - bool airspeed_scheduling; // Airspeed scheduling on or off - bool force_rotation_angle; // Setting to force wing_angle_deg_sp +struct rotwing_state_t { + /* Control */ + enum rotwing_states_t state; // Current state + enum rotwing_states_t nav_state; // Desired navigation state (requested only by NAV and can be overruled by RC) + bool hover_motors_enabled; // Hover motors enabled (> idle throttle) + + /* Skew */ + float sp_skew_angle_deg; // Setpoint skew angle in degrees + float ref_model_skew_angle_deg; // Reference model skew angle in degrees + float meas_skew_angle_deg; // Measured skew angle in degrees + float meas_skew_angle_time; // Time of the last skew angle measurement + bool force_skew; // Force skew angle to a certain value by the GCS + int16_t skew_cmd; // Skewing command in pprz values + + /* Airspeeds */ + float fw_min_airspeed; // Minimum airspeed (stall+margin) + float cruise_airspeed; // Airspeed for cruising + float min_airspeed; // Minimum airspeed for bounding + float max_airspeed; // Maximum airspeed for bounding + + /* RPM measurements*/ + int32_t meas_rpm[5]; // Measured RPM of the hover and pusher motors + float meas_rpm_time[5]; // Time of the last RPM measurement + + /* Sim failures */ + bool fail_skew_angle; // Skew angle sensor failure + bool fail_hover_motor; // Hover motor failure + bool fail_pusher_motor; // Pusher motor failure }; +extern struct rotwing_state_t rotwing_state; -extern struct RotwingState rotwing_state; -extern struct RotWingStateSettings rotwing_state_settings; -extern struct RotWingStateSkewing rotwing_state_skewing; +void rotwing_state_init(void); +void rotwing_state_periodic(void); +bool rotwing_state_hover_motors_running(void); +bool rotwing_state_pusher_motor_running(void); +bool rotwing_state_skew_angle_valid(void); -extern float rotwing_state_max_hover_speed; - -extern bool hover_motors_active; -extern bool bool_disable_hover_motors; - -extern void init_rotwing_state(void); -extern void periodic_rotwing_state(void); -extern void request_rotwing_state(uint8_t state); -extern void rotwing_request_configuration(uint8_t configuration); -extern void rotwing_state_skew_actuator_periodic(void); -extern bool rotwing_state_hover_motors_running(void); +void rotwing_state_set(enum rotwing_states_t state); +bool rotwing_state_choose_circle_direction(uint8_t wp_id); +void rotwing_state_set_transition_wp(uint8_t wp_id); +void rotwing_state_update_WP_height(uint8_t wp_id, float height); #endif // ROTWING_STATE_H diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 27ddd6516e..3f994e7f9b 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 27ddd6516e384d204d68314503df469b80c72b6e +Subproject commit 3f994e7f9b9b898f2c500406f57df0349f30ce3f diff --git a/sw/ground_segment/python/rotwing_visualizer/rotwing_vis_o3d_app.py b/sw/ground_segment/python/rotwing_visualizer/rotwing_vis_o3d_app.py index a39987eac6..1fac92d8a4 100755 --- a/sw/ground_segment/python/rotwing_visualizer/rotwing_vis_o3d_app.py +++ b/sw/ground_segment/python/rotwing_visualizer/rotwing_vis_o3d_app.py @@ -70,14 +70,14 @@ class AHRSRefQuatMessage(object): class RotWingControllerMessage(object): def __init__(self, msg): - self.wing_angle_deg = msg['wing_angle_deg'] - self.wing_angle_deg_sp = msg['wing_angle_deg_sp'] + self.meas_skew_angle_deg = msg['meas_skew_angle'] + self.sp_skew_angle_deg = msg['sp_skew_angle'] def get_wing_angle(self): - return np.deg2rad(float(self.wing_angle_deg)) + return np.deg2rad(float(self.meas_skew_angle_deg)) def get_wing_angle_sp(self): - return np.deg2rad(float(self.wing_angle_deg_sp)) + return np.deg2rad(float(self.sp_skew_angle_deg)) class ActuatorsMessage(object): def __init__(self, msg):