Skip to content

Commit

Permalink
RotWing Demo (#3382)
Browse files Browse the repository at this point in the history
  • Loading branch information
tmldeponti authored Oct 1, 2024
1 parent 0bb4700 commit 5fce154
Show file tree
Hide file tree
Showing 5 changed files with 32 additions and 3 deletions.
1 change: 1 addition & 0 deletions conf/modules/eff_scheduling_rotwing_V2.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
4 changes: 4 additions & 0 deletions conf/modules/rotwing_state_V2.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
24 changes: 22 additions & 2 deletions sw/airborne/modules/rotwing_drone/rotwing_state_V2.c
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand All @@ -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);
Expand Down Expand Up @@ -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;
}
}
}

Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/modules/rotwing_drone/rotwing_state_V2.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down

0 comments on commit 5fce154

Please sign in to comment.