[rotwing_state] Added free configuration to be requested for rotwing …

…drone
paparazzi · Apr 29, 2024 · c540c46 · c540c46
1 parent e9b81bb
commit c540c46
Show file tree

Hide file tree

Showing 2 changed files with 78 additions and 0 deletions.
diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/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;
@@ -211,6 +213,11 @@ void periodic_rotwing_state(void)
   } else if (guidance_h.mode == GUIDANCE_H_MODE_FORWARD) {
     bool_disable_hover_motors = false;
   }
+
+  // Run the free state requester if free configuration requestes
+  if(rotwing_state.requested_config == ROTWING_CONFIGURATION_FREE) {
+    rotwing_state_free_processor();
+  }
 }
 
 // Function to request a state
@@ -227,12 +234,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;
   }
 }
@@ -560,6 +573,69 @@ 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 speed_to_target = (pos_error_norm.x * groundspeed->x + pos_error_norm.y * groundspeed->y);
+  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);
+
+  if (speed_to_target > 0) {
+    // 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) {
+      request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF);
+    }
+  } else { // If speed to target negative (flying away from target)
+    if (airspeed > 15) {
+      request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF);
+    } else {
+      request_rotwing_state(ROTWING_STATE_SKEWING);
+    }
+  }
+}
+
 void rotwing_state_skew_actuator_periodic(void)
 {
 

diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.h b/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