RotWing Demo (#3382)

conf/modules/eff_scheduling_rotwing_V2.xml

@@ -61,6 +61,7 @@
       <define name="COMMAND_RUDDER"              value="6"/>
       <define name="COMMAND_AILERONS"            value="7"/>
       <define name="COMMAND_ROT_MECH"            value="11"/>
+      <define name="SERVO_ROTATION_MECH_IDX"     value="11"/>
       <define name="RW_aX"                       value="0"/>
       <define name="RW_aY"                       value="1"/>
       <define name="RW_aZ"                       value="2"/>

conf/modules/rotwing_state_V2.xml

@@ -11,6 +11,10 @@
       <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="demo_skew" min="0" step="1" max="1" values="FALSE|TRUE" shortname="demo_skew"/>
+        <dl_setting var="max_skew_demo" min="0" step="1" max="80" shortname="demo_max_skew"/>
+        <dl_setting var="min_skew_demo" min="0" step="1" max="80" shortname="demo_min_skew"/>
+        <dl_setting var="freq_skew_demo" min="0.1" step="0.1" max="10" shortname="demo_freq_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"/>

sw/airborne/modules/rotwing_drone/rotwing_state_V2.c

@@ -157,6 +157,13 @@ float rotwing_state_max_fw_speed = 20;
 
 bool hover_motors_active = true;
 bool bool_disable_hover_motors = false;
+//DEMO Sine skew
+bool demo_skew = false;
+float max_skew_demo = 50;
+float min_skew_demo = 0;
+float freq_skew_demo = 0.8;
+int time_step_skew_demo = 0;
+
 
 inline void rotwing_check_set_current_state(void);
 inline void rotwing_switch_state(void);
@@ -190,6 +197,7 @@ static void send_rotating_wing_state(struct transport_tx *trans, struct link_dev
 void rotwing_state_force_skew_off(void)
 {
   rotwing_state_skewing.force_rotation_angle = false;
+  demo_skew = false;
 }
 
 void init_rotwing_state(void)
@@ -209,6 +217,7 @@ void init_rotwing_state(void)
   rotwing_state_skewing.servo_pprz_cmd        = -MAX_PPRZ;
   rotwing_state_skewing.airspeed_scheduling   = false;
   rotwing_state_skewing.force_rotation_angle  = false;
+  demo_skew = false;
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state);
@@ -628,6 +637,17 @@ void rotwing_state_skewer(void)
 
     Bound(wing_angle_scheduled_sp_deg, 0., 90.)
     rotwing_state_skewing.wing_angle_deg_sp = wing_angle_scheduled_sp_deg;
+  } else {
+    if(demo_skew) {
+      float amplitude_skew_demo = (max_skew_demo - min_skew_demo) / 2;
+      float offset_skew_demo = (max_skew_demo + min_skew_demo) / 2;
+      float time_skew_demo = (float) time_step_skew_demo / PERIODIC_FREQUENCY;
+      float angle_skew_demo = amplitude_skew_demo * (-cosf(2 * M_PI * freq_skew_demo * time_skew_demo)) + offset_skew_demo;
+      rotwing_state_skewing.wing_angle_deg_sp = angle_skew_demo;
+      time_step_skew_demo++;
+    }else{
+      time_step_skew_demo = 0;
+    }
   }
 }
 
@@ -727,7 +747,7 @@ void rotwing_state_skew_actuator_periodic(void)
 #endif // USE_ROTMECH_VIRTUAL
   // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback
   struct act_feedback_t feedback;
-  feedback.idx =  COMMAND_ROT_MECH;
+  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
@@ -744,7 +764,7 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id,
 
     for (int i = 0; i < num_act_message; i++) {
       // Check for wing rotation feedback
-      if ((feedback_msg[i].set.position) && (feedback_msg[i].idx == COMMAND_ROT_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;
         rotwing_state_skewing.wing_angle_deg = DegOfRad(wing_angle_rad);

sw/airborne/modules/rotwing_drone/rotwing_state_V2.h

@@ -87,6 +87,10 @@ extern float rotwing_state_max_hover_speed;
 
 extern bool hover_motors_active;
 extern bool bool_disable_hover_motors;
+extern bool demo_skew;
+extern float max_skew_demo;
+extern float min_skew_demo;
+extern float freq_skew_demo;
 
 extern void init_rotwing_state(void);
 extern void periodic_rotwing_state(void);

sw/airborne/modules/rotwing_drone/wing_rotation_adc_sensor.c

@@ -71,7 +71,7 @@ void wing_rotation_adc_to_deg(void)
 
   // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback
   struct act_feedback_t feedback = {0};
-  feedback.idx =  COMMAND_ROT_MECH;
+  feedback.idx =  SERVO_ROTATION_MECH_IDX;
   feedback.position = 0.5 * M_PI - RadOfDeg(wing_angle_deg);
   feedback.set.position = true;