Rotating wing state machine (#3169)

* Rotating wing state machine
* state now drives the wing rotation
* module does not run stand-alone
* Update conf/modules/rotwing_state.xml

Co-authored-by: Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>

conf/modules/rotwing_state.xml

@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+<module name="rotwing_state" dir="rot_wing_drone">
+  <doc>
+    <description>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.</description>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="RotWingState">
+        <dl_setting var="rotwing_state_skewing.wing_angle_deg_sp" min="0" step="1" max="90" shortname="skew angle"/>
+        <dl_setting var="rotwing_state_skewing.force_rotation_angle" min="0" step="1" max="1" values="FALSE|TRUE" shortname="force_skew"/>
+        <dl_setting var="hover_motors_active" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_active"/>
+        <dl_setting var="bool_disable_hover_motors" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_disable"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+  <header>
+    <file name="rotwing_state.h"/>
+  </header>
+    <init fun="init_rotwing_state()"/>
+    <periodic fun="periodic_rotwing_state()" freq="50"/>
+  <makefile>
+    <file name="rotwing_state.c"/>
+  </makefile>
+</module>

conf/modules/wing_rotation_controller_servo.xml

@@ -6,7 +6,6 @@
   <settings>
     <dl_settings NAME="wing_rotation">
       <dl_settings NAME="wing_rot">
-        <dl_setting MAX="90" MIN="0" STEP="1" VAR="wing_rotation_controller.wing_angle_deg_sp" shortname="wing_sp_deg"/>
         <dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation_controller.wing_rotation_first_order_dynamics" shortname="first_dyn"/>
         <dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation_controller.wing_rotation_second_order_dynamics" shortname="second_dyn"/>
       </dl_settings>
@@ -16,7 +15,6 @@
     <file name="wing_rotation_controller_servo.h"/>
   </header>
     <init fun="wing_rotation_init()"/>
-    <periodic fun="wing_rotation_periodic()" freq="1.0"/>
     <periodic fun="wing_rotation_event()"/>
   <makefile>
     <configure name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" default="ADC_5" case="lower|upper"/>

sw/airborne/modules/rot_wing_drone/rotwing_state.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/** @file "modules/rotwing/rotwing_state.h"
+ * @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
+ * 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.
+ */
+
+#ifndef ROTWING_STATE_H
+#define ROTWING_STATE_H
+
+#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
+
+struct RotwingState {
+  uint8_t current_state;
+  uint8_t desired_state;
+};
+
+#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;
+};
+
+struct RotWingStateSkewing {
+  float wing_angle_deg_sp;   // Wing angle setpoint in deg
+  float wing_angle_deg;      // Wing angle from sensor in deg
+  bool airspeed_scheduling;  // Airspeed scheduling on or off
+  bool force_rotation_angle; // Setting to force wing_angle_deg_sp
+};
+
+extern struct RotwingState rotwing_state;
+extern struct RotWingStateSettings rotwing_state_settings;
+extern struct RotWingStateSkewing rotwing_state_skewing;
+
+extern bool rotwing_state_force_quad;
+
+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);
+
+#endif  // ROTWING_STATE_H

sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c

@@ -24,10 +24,14 @@
  */
 
 #include "modules/rot_wing_drone/wing_rotation_controller_servo.h"
+#include "modules/rot_wing_drone/rotwing_state.h"
 #include "modules/radio_control/radio_control.h"
 #include "firmwares/rotorcraft/guidance/guidance_h.h"
 #include "generated/airframe.h"
 #include "modules/core/abi.h"
+#include "state.h"
+
+#include <stdlib.h>
 #include "mcu_periph/adc.h"
 
 #if USE_NPS
@@ -58,6 +62,7 @@
 // Parameters
 struct wing_rotation_controller_t wing_rotation_controller = {0};
 
+bool in_transition = false;
 
 #if !USE_NPS
 static struct adc_buf buf_wing_rot_pos;
@@ -77,38 +82,18 @@ void wing_rotation_init(void)
 #endif
 
   // Init Data
-  wing_rotation_controller.wing_angle_virtual_deg_sp = 45;
+  wing_rotation_controller.wing_angle_virtual_deg_sp = 0;
   wing_rotation_controller.wing_rotation_first_order_dynamics = WING_ROTATION_CONTROLLER_FIRST_DYN;
   wing_rotation_controller.wing_rotation_second_order_dynamics = WING_ROTATION_CONTROLLER_SECOND_DYN;
 }
 
-void wing_rotation_periodic(void)
-{
-  // After 5 loops, set current setpoint and enable wing_rotation
-  // freq = 1.0 Hz
-  if (!wing_rotation_controller.initialized) {
-    wing_rotation_controller.init_loop_count += 1;
-    if (wing_rotation_controller.init_loop_count > 4) {
-      wing_rotation_controller.initialized = true;
-      wing_rotation_controller.wing_angle_deg_sp = 45.;
-    }
-  }
-}
-
 void wing_rotation_event(void)
 {
   // Update Wing position sensor
   wing_rotation_adc_to_deg();
 
-  // Run control if initialized
+  // Control the wing rotation position.
-  if (wing_rotation_controller.initialized) {
+  wing_rotation_compute_pprz_cmd();
-
-    // Setpoint checks
-    Bound(wing_rotation_controller.wing_angle_deg_sp, 0., 90.);
-
-    // Control the wing rotation position.
-    wing_rotation_compute_pprz_cmd();
-  }
 }
 
 void wing_rotation_adc_to_deg(void)
@@ -116,8 +101,8 @@ void wing_rotation_adc_to_deg(void)
 #if !USE_NPS
   // Read ADC
   wing_rotation_controller.adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample;
-  wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294;
 
+  wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294;
 #else // !USE_NPS
   // Copy setpoint as actual angle in simulation
   wing_rotation_controller.wing_angle_deg = wing_rotation_controller.wing_angle_virtual_deg_sp;
@@ -131,11 +116,11 @@ void wing_rotation_adc_to_deg(void)
 
   // Send ABI message
   AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
-
 }
 
 void wing_rotation_compute_pprz_cmd(void)
 {
+  wing_rotation_controller.wing_angle_deg_sp = rotwing_state_skewing.wing_angle_deg_sp;
   // Smooth accerelation and rate limited setpoint
   float angle_error = wing_rotation_controller.wing_angle_deg_sp - wing_rotation_controller.wing_angle_virtual_deg_sp;
   float speed_sp = wing_rotation_controller.wing_rotation_first_order_dynamics * angle_error;
@@ -148,8 +133,8 @@ void wing_rotation_compute_pprz_cmd(void)
   servo_pprz_cmd = (int32_t)(wing_rotation_controller.wing_angle_virtual_deg_sp / 90. * (float)MAX_PPRZ);
   Bound(servo_pprz_cmd, 0, MAX_PPRZ);
   wing_rotation_controller.servo_pprz_cmd = servo_pprz_cmd;
-  
-  actuators_pprz[SERVO_ROTATION_MECH] = servo_pprz_cmd;
-}
-
 
+#if USE_NPS
+  actuators_pprz[INDI_NUM_ACT] = servo_pprz_cmd;
+#endif
+}

sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h

@@ -29,7 +29,6 @@
 #include "std.h"
 
 extern void wing_rotation_init(void);
-extern void wing_rotation_periodic(void);
 extern void wing_rotation_event(void);
 
 // Paramaters
@@ -44,9 +43,6 @@ struct wing_rotation_controller_t {
   float wing_angle_virtual_deg_sp;            ///< Rate limiter state variable 2
   float wing_rotation_first_order_dynamics;   ///< Rate limiter for wing rotation
   float wing_rotation_second_order_dynamics;  ///< Acceleration limiter for wing rotation
-
-  bool initialized;                           ///< Wing rotation controller initialized                
-  uint8_t init_loop_count;                    ///< Wing rotation controller initialization loop count
 };
 
 // Setters