[RotWing] FREE_STATE mode and pusher rate limiter (#3268)

* [rotwing_state] Added free configuration to be requested for rotwing drone

* [rot_wing_eff_sched] Added rate limiter on pusher prop

* [rot_wing_state] fix nav_max_goto_speed setting from state machine

* [rotwing_state] move rotwing free state processor to NAV if statement

* [rot_wing_state] Updated free state in rotwing_state

* [fp] Added free state standby waypoint to rotwing EHVB fp

* [rotwing_state] Removed non used speed_to_target variable

sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c

@@ -144,6 +144,8 @@ struct rot_wing_eff_sched_param_t eff_sched_p = {
   .k_lift_tail              = ROT_WING_EFF_SCHED_K_LIFT_TAIL
 };
 
+float eff_sched_pusher_time = 0.002; 
+
 struct rot_wing_eff_sched_var_t eff_sched_var;
 
 inline void eff_scheduling_rot_wing_update_wing_angle(void);
@@ -411,5 +413,15 @@ void stabilization_indi_set_wls_settings(void)
         u_pref_stab_indi[i] = actuator_state_filt_vect[i]; // Set change in prefered state to 0 for elevator
         u_min_stab_indi[i] = 0; // cmd 0 is lowest position for elevator
       }
+      if (i==8) {
+        // 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);
+        u_min_stab_indi[i] = actuators_pprz[i] - max_increment;
+        u_max_stab_indi[i] = actuators_pprz[i] + max_increment;
+
+        Bound(u_min_stab_indi[i], 0, MAX_PPRZ);
+        Bound(u_max_stab_indi[i], 0, MAX_PPRZ);
+      }
   }
 }

sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h

@@ -80,6 +80,8 @@ struct rot_wing_eff_sched_var_t {
 extern float rotation_angle_setpoint_deg;
 extern int16_t rotation_cmd;
 
+extern float eff_sched_pusher_time;
+
 extern void eff_scheduling_rot_wing_init(void);
 extern void eff_scheduling_rot_wing_periodic(void);
 

sw/airborne/modules/rot_wing_drone/rotwing_state.c

@@ -149,6 +149,7 @@ 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);
 
 inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
 
@@ -175,6 +176,7 @@ void init_rotwing_state(void)
   // 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;
@@ -194,6 +196,10 @@ void periodic_rotwing_state(void)
 
   // 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();
+    }
     rotwing_switch_state();
   } else if (guidance_h.mode == GUIDANCE_H_MODE_NONE) {
     if (stabilization.mode == STABILIZATION_MODE_ATTITUDE) {
@@ -213,6 +219,8 @@ void periodic_rotwing_state(void)
   if (guidance_h.mode == GUIDANCE_H_MODE_NONE) {
     bool_disable_hover_motors = false;
   }
+
+  
 }
 
 // Function to request a state
@@ -229,12 +237,18 @@ 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;
   }
 }
@@ -534,6 +548,7 @@ void rotwing_state_set_state_settings(void)
   force_forward = rotwing_state_settings.force_forward;
 
   nav_max_speed = rotwing_state_settings.nav_max_speed;
+  nav_goto_max_speed = rotwing_state_settings.nav_max_speed;
 
   // TO DO: pitch angle now hard coded scheduled by wing angle
 
@@ -562,6 +577,61 @@ void rotwing_state_skewer(void)
   }
 }
 
+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();
+
+  // Get windspeed vector
+  struct FloatEulers eulers_zxy;
+  float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
+
+  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);
+
+  // 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);
+  
+  // 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;
+
+    // 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)
 {
 

sw/airborne/modules/rot_wing_drone/rotwing_state.h

@@ -42,10 +42,12 @@
 #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;
 };
 
 #define ROTWING_STATE_WING_QUAD_SETTING         0 // Wing skew at 0 degrees