Rotwing State Update: no hardcoded defines in flightplan (#3201)

conf/flight_plans/tudelft/rotating_wing_EHVB.xml

@@ -10,7 +10,6 @@
     <waypoint name="end_trans" x="100." y="100."/>
     <waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/>
     <waypoint name="begin_trans" lat="52.168412" lon="4.4149282"/>
-    <!--<waypoint name="p1" lat="52.1674262" lon="4.4141448"/>-->
     <waypoint name="p1" lat="52.1672408" lon="4.4144528"/>
     <waypoint name="p2" lat="52.1682897" lon="4.4138441"/>
     <waypoint name="p3" lat="52.1687878" lon="4.4155648"/>
@@ -43,7 +42,9 @@
     </sector>
   </sectors>
   <modules>
-    <!--<module name="follow_me"/>-->
+    <!-- <module name="follow_me">
+      <define name="FOLLOW_ME_MOVING_WPS" value="WP_p1"/>
+    </module> -->
   </modules>
   <exceptions>
     <exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
@@ -51,28 +52,28 @@
   </exceptions>
   <blocks>
    <block name="Wait GPS">
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <call_once fun="NavKillThrottle()"/>
       <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
     </block>
     <block name="Geo init">
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <while cond="LessThan(NavBlockTime(), 10)"/>
       <call_once fun="NavSetAltitudeReferenceHere()"/>
     </block>
     <block name="Holding point">
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <call_once fun="NavKillThrottle()"/>
       <set var="stabilization_cmd[COMMAND_THRUST_X]" value="0"/>
       <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
     <block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
         <call_once fun="NavResurrect()"/>
         <attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
         <exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
         <call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
 
@@ -81,55 +82,48 @@
     </block>
     <block name="Climb">
       <exception cond="GetPosAlt() @GT 20.0" deroute="Standby"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <call_once fun="nav_set_heading_current()"/>
       <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
     <block name="Approach APP">
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
     </block>
-    <!--<block name="follow_module">
+    <!-- <block name="follow_module">
-      <set value="false" var="force_forward"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-      <stay pre_call="follow_me_set_wp(WP_FOLLOW, 0)" wp="FOLLOW"/>
+      <stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
-    </block>-->
+    </block> -->
-<block name="stay_begin-trans">
+  <block name="stay_begin-trans">
-      <set value="7" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
       <stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
     <block name="line_trans_fwd">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
       <call_once fun="NavSetWaypointHere(WP_begin_trans)"/>
       <go wp="end_trans"/>
       <deroute block="end_transition"/>
     </block>
     <block name="end_transition">
-      <set value="7" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
       <stay wp="end_trans"/>
     </block>
     <block name="transition_CW">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
       <stay wp="end_trans"/>
       <exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CW_fwd"/>
     </block>
     <block name="transition_CCW">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
       <stay wp="end_trans"/>
       <exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CCW_fwd"/>
     </block>
     <block name="route">
-      <call_once fun="request_rotwing_state(ROTWING_STATE_FW)"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
-      <set value="50" var="nav_max_speed"/>
       <go from="p1" hmode="route" wp="p2"/>
       <go from="p2" hmode="route" wp="p3"/>
       <go from="p3" hmode="route" wp="p4"/>
@@ -137,8 +131,7 @@
       <deroute block="route"/>
     </block>
     <block name="small_route" strip_button="Route" strip_icon="path.png">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_FW)"/>
       <go wp="p2"/>
       <go wp="p3"/>
       <go wp="p4"/>
@@ -146,57 +139,47 @@
       <deroute block="small_route"/>
     </block>
     <block name="Circle_CW_fwd" strip_button="CircleRight" strip_icon="circle-right.png">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF)"/>
       <circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
     <block name="Circle_CCW_fwd" strip_button="CircleLeft" strip_icon="circle-left.png">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF)"/>
       <circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
     <block name="big_Circle_CW_fwd">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_IDLE)"/>
       <circle radius="150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
     <block name="big_Circle_CCW_fwd">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_IDLE)"/>
       <circle radius="-150" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
     <block name="Transition_quad">
-      <set value="50" var="nav_max_speed"/>
+      <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
-      <call_once fun="request_rotwing_state(ROTWING_STATE_SKEWING)"/>
       <go wp="STDBY"/>
       <deroute block="Standby"/>
     </block>
     <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <call_once fun="NavSetWaypointHere(WP_TD)"/>
     </block>
     <block name="land">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <go wp="TD"/>
     </block>
     <block name="descend">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <exception cond="GetPosAlt() @LT 12.0" deroute="flare"/>
         <stay climb="-1.0" vmode="climb" wp="TD"/>
     </block>
     <block name="flare">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <stay climb="-0.5" vmode="climb" wp="TD"/>
         <!--<exception cond="!(GetPosAlt() @LT 2.0)" deroute="flare_low"/>-->
         <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
     </block>
     <block name="flare_low">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <exception cond="NavDetectGround()" deroute="Holding point"/>
         <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
         <exception cond="ground_detect()" deroute="Holding point"/>
@@ -204,8 +187,7 @@
         <stay climb="-0.5" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
-        <set value="7" var="nav_max_speed"/>
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
-        <call_once fun="request_rotwing_state(ROTWING_STATE_HOVER)"/>
         <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
   </blocks>

conf/modules/rotwing_state.xml

@@ -8,6 +8,7 @@
       <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="rotwing_state_max_hover_speed" min="5" step="0.5" max="25" shortname="hover_speed"/>
         <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>

sw/airborne/modules/rot_wing_drone/rotwing_state.c

@@ -26,6 +26,7 @@
 #include "modules/rot_wing_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"
@@ -84,6 +85,21 @@
 #define ROTWING_HOV_MOT_OFF_COUNTER 10
 #endif
 
+#ifndef ROTWING_STATE_PD_WING_ROTATION
+#define ROTWING_STATE_PD_WING_ROTATION FALSE
+#endif
+
+#if ROTWING_STATE_PD_WING_ROTATION
+#ifndef ROTWING_STATE_WING_ROTATION_P_GAIN
+#define ROTWING_STATE_WING_ROTATION_P_GAIN 0.001
+#endif
+#ifndef ROTWING_STATE_WING_ROTATION_D_GAIN
+#define ROTWING_STATE_WING_ROTATION_D_GAIN 0.003
+#endif
+float rotwing_state_skew_p_cmd = -MAX_PPRZ;
+float rotwing_state_skew_d_cmd = 0;
+#endif
+
 
 // Hover preferred pitch (deg)
 #ifndef ROTWING_STATE_HOVER_PREF_PITCH
@@ -114,14 +130,14 @@ struct RotwingState rotwing_state;
 struct RotWingStateSettings rotwing_state_settings;
 struct RotWingStateSkewing  rotwing_state_skewing;
 
-bool rotwing_state_force_quad = false;
-
 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;
 
@@ -145,14 +161,13 @@ inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_an
 static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
 {
   uint16_t adc_dummy = 0;
-  int32_t cmd_dummy = 0;
   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,
-                                        &cmd_dummy);
+                                        &rotwing_state_skewing.servo_pprz_cmd);
 }
 #endif // PERIODIC_TELEMETRY
 
@@ -167,6 +182,7 @@ void init_rotwing_state(void)
 
   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;
 
@@ -210,6 +226,22 @@ void request_rotwing_state(uint8_t state)
   }
 }
 
+// Function to prefer configuration
+void rotwing_request_configuration(uint8_t configuration)
+{
+    switch (configuration) {
+      case ROTWING_CONFIGURATION_HOVER:
+        request_rotwing_state(ROTWING_STATE_HOVER);
+        break;
+      case ROTWING_CONFIGURATION_HYBRID:
+        request_rotwing_state(ROTWING_STATE_SKEWING);
+      break;
+      case ROTWING_CONFIGURATION_EFFICIENT:
+        request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF);
+      break;
+    }
+} 
+
 void rotwing_check_set_current_state(void)
 {
   // if !in_flight, set state to hover
@@ -415,12 +447,19 @@ void rotwing_state_set_hover_settings(void)
   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)
 {
-  rotwing_state_settings.wing_scheduler       = ROTWING_STATE_WING_SCHEDULING_SETTING;
+  // 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;
+  }
   rotwing_state_settings.hover_motors_active  = true;
   rotwing_state_settings.hover_motors_disable = false;
   rotwing_state_settings.force_forward        = false;
@@ -428,6 +467,7 @@ void rotwing_state_set_skewing_settings(void)
   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();
 }
 
@@ -441,6 +481,7 @@ void rotwing_state_set_fw_settings(void)
   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();
 }
 
@@ -454,6 +495,7 @@ void rotwing_state_set_fw_hov_mot_idle_settings(void)
   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();
 }
 
@@ -467,6 +509,7 @@ void rotwing_state_set_fw_hov_mot_off_settings(void)
   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();
 }
 
@@ -497,6 +540,8 @@ void rotwing_state_set_state_settings(void)
 
   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
@@ -524,13 +569,56 @@ void rotwing_state_skewer(void)
     }
 }
 
+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_PD_WING_ROTATION
+  // Rotate with second order filter
+  // Smooth accerelation and rate limited setpoint
+  
+  float error_cmd = servo_pprz_cmd - rotwing_state_skew_p_cmd;
+  float speed_sp  = ROTWING_STATE_WING_ROTATION_P_GAIN * error_cmd;
+  float speed_error = speed_sp - rotwing_state_skew_d_cmd;
+  rotwing_state_skew_d_cmd += ROTWING_STATE_WING_ROTATION_D_GAIN * speed_error;
+  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;
+
+  #else
+  // Directly controlling the wing rotation
+  rotwing_state_skewing.servo_pprz_cmd = servo_pprz_cmd;
+  #endif
+
+  #if USE_NPS
+    actuators_pprz[INDI_NUM_ACT] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation 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
+    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.set.position = true;
+
+    // Send ABI message
+    AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1);
+  #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)
 {
   for (int i=0; i<num_act_message; i++) {
 
     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))
+      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;
@@ -542,9 +630,22 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
     }
 
     // Sanity check that index is valid
-    int idx = feedback_msg[i].idx - 1;
+    int idx = feedback_msg[i].idx;
-    if ((feedback_msg[i].set.rpm) && (idx >= 0) && (idx < ROTWING_STATE_NUM_HOVER_RPM)){
+    if (feedback_msg[i].set.rpm)
-      rotwing_state_hover_rpm[idx] = feedback_msg->rpm;
+    {
+      if (idx == SERVO_MOTOR_FRONT_IDX)
+      {
+        rotwing_state_hover_rpm[0] = feedback_msg->rpm;
+      } else if (idx == SERVO_MOTOR_RIGHT_IDX)
+      {
+        rotwing_state_hover_rpm[1] = feedback_msg->rpm;
+      } else if (idx == SERVO_MOTOR_BACK_IDX)
+      {
+        rotwing_state_hover_rpm[2] = feedback_msg->rpm;
+      } else if (idx == SERVO_MOTOR_LEFT_IDX)
+      {
+        rotwing_state_hover_rpm[3] = feedback_msg->rpm;
+      }
     }
   }
 }
@@ -634,4 +735,4 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl
   du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
   du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency
   du_pref_gih[3] = body_v[0]; // solve the body acceleration
-}
+}

sw/airborne/modules/rot_wing_drone/rotwing_state.h

@@ -37,6 +37,12 @@
 #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)
+
 struct RotwingState {
   uint8_t current_state;
   uint8_t desired_state;
@@ -59,11 +65,13 @@ struct RotWingStateSettings {
   bool stall_protection;
   float max_v_climb;
   float max_v_descend;
+  float nav_max_speed;
 };
 
 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
 };
@@ -72,7 +80,7 @@ extern struct RotwingState rotwing_state;
 extern struct RotWingStateSettings rotwing_state_settings;
 extern struct RotWingStateSkewing rotwing_state_skewing;
 
-extern bool rotwing_state_force_quad;
+extern float rotwing_state_max_hover_speed;
 
 extern bool hover_motors_active;
 extern bool bool_disable_hover_motors;
@@ -80,5 +88,7 @@ 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);
 
 #endif  // ROTWING_STATE_H