diff --git a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml index d85cf8c309..60bc43d668 100644 --- a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml +++ b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml @@ -133,9 +133,7 @@ - - - + diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml index ce3adc8c25..1cf9ae7201 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml @@ -138,9 +138,7 @@ - - - + diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml index 98ad2de41f..38044f59e2 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml @@ -140,9 +140,7 @@ - - - + diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml index 067a355951..c084012f24 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml @@ -131,9 +131,7 @@ - - - + @@ -216,7 +214,24 @@ +
+ + + + + + + + + + + + + + + +
@@ -237,8 +252,6 @@ - -
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml index f953d78602..19b1a6a441 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml @@ -99,11 +99,7 @@ - - - - - + @@ -186,7 +182,24 @@ +
+ + + + + + + + + + + + + + + +
@@ -264,26 +277,6 @@ - - - - - - - - - - - - - - - - - - - -
diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml new file mode 100644 index 0000000000..57d4538da2 --- /dev/null +++ b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml @@ -0,0 +1,373 @@ + + + + RotatingWingV3G with optitrack and INS ext pose + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + Enter the PIC + Enter the PAC + Enter the GCS op + Goal of the flight + Location, airspace and weather + Check the RC battery + Check tail connection + Check wings taped and secured + Inspect airframe condition + Check attitude and heading + Airspeed sensor calibration + Put UAV on take-off location + Check flight plan + Switch to correct flight block + Switch on drone tag + Switch on camera + Announce flight to other airspace users + + +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + +
+ diff --git a/conf/autopilot/rotorcraft_oneloop_switch.xml b/conf/autopilot/rotorcraft_oneloop_switch.xml index c0d2baf611..784ec2c589 100644 --- a/conf/autopilot/rotorcraft_oneloop_switch.xml +++ b/conf/autopilot/rotorcraft_oneloop_switch.xml @@ -15,7 +15,7 @@ - + diff --git a/conf/flight_plans/tudelft/oneloop_valkenburg.xml b/conf/flight_plans/tudelft/oneloop_valkenburg.xml index ab8b4d57b5..7879814cd1 100644 --- a/conf/flight_plans/tudelft/oneloop_valkenburg.xml +++ b/conf/flight_plans/tudelft/oneloop_valkenburg.xml @@ -60,6 +60,11 @@ + + + + + - + - + - + - - + - - - - + + + - - + - + - - - + + - - - + + - - - + + - - - + + - + - - + + + + + + + - + - - + + - - - + + + - - - + + + - + - + - - - + + diff --git a/conf/modules/eff_scheduling_rotwing.xml b/conf/modules/eff_scheduling_rotwing.xml index 0d206200d0..331b41da3b 100644 --- a/conf/modules/eff_scheduling_rotwing.xml +++ b/conf/modules/eff_scheduling_rotwing.xml @@ -55,7 +55,7 @@ - + @@ -70,6 +70,14 @@ + + + + + + + + \ No newline at end of file diff --git a/conf/modules/rotwing_state_V2.xml b/conf/modules/rotwing_state_V2.xml deleted file mode 100644 index 9e84dfd830..0000000000 --- a/conf/modules/rotwing_state_V2.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state. -
- -
- - - - - - - - - - - - - - - - -
- -
- - - - - - - diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index 9cb80b584d..bbeabaff91 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -411,7 +411,7 @@ void guidance_indi_enter(void) init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0); } -void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed) { +void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed) { gih_params.min_airspeed = min_airspeed; gih_params.max_airspeed = max_airspeed; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h index fb7a9f0b3d..408e49fc33 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h @@ -70,7 +70,7 @@ enum GuidanceIndiHybrid_VMode { extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp); extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode); -extern void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed); +extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed); struct guidance_indi_hybrid_params { float pos_gain; diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c index 5197cf1c63..d9ca4a9a42 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rotwing.c @@ -29,7 +29,8 @@ #include "state.h" #include "firmwares/rotorcraft/stabilization/stabilization_indi.h" - +#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" +#include "modules/rotwing_drone/rotwing_state.h" #include "autopilot.h" #include "modules/actuators/actuators.h" @@ -173,6 +174,8 @@ inline float bound_or_zero(float value, float low_lim, float up_lim) { float eff_sched_pusher_time = 0.002; float roll_eff_slider = 12.f; +static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; + struct rotwing_eff_sched_var_t eff_sched_var; inline void eff_scheduling_rotwing_update_wing_angle(void); @@ -189,6 +192,7 @@ inline void eff_scheduling_rotwing_schedule_liftd(void); inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED); void stabilization_indi_set_wls_settings(void); +inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle); /** ABI binding wing position data. @@ -496,3 +500,91 @@ void stabilization_indi_set_wls_settings(void) } } } + +void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) +{ + // adjust weights + float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ... + Bound(fixed_wing_percentile, 0, 1); +#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 + + float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; + + // Increase importance of forward acceleration in forward flight + wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile * + AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) + + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + + float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - + actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * rotwing_state_hover_motors_running(); + Bound(du_min_thrust_z, -50., 0.); + float du_max_thrust_z = -(actuator_state_filt_vect[0] * g1g2[3][0] + actuator_state_filt_vect[1] * g1g2[3][1] + + actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]); + Bound(du_max_thrust_z, 0., 50.); + + float roll_limit_rad = guidance_indi_max_bank; + float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); + float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); + + float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); + float quad_pitch_limit_rad = RadOfDeg(5.0); + + float airspeed = stateGetAirspeed_f(); + + float scheduled_pitch_angle = 0.f; + float pitch_angle_range = 3.; + float meas_skew_angle = rotwing_state.meas_skew_angle_deg; + Bound(meas_skew_angle, 0, 90); // Bound to prevent errors + if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) { + scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH; + wls_guid_p.Wu[1] = Wu_gih_original[1]; + max_pitch_limit_rad = quad_pitch_limit_rad; + } else { + float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f; + Bound(pitch_progression, 0.f, 1.f); + scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression); + wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99); + max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression; + } + if (!rotwing_state_hover_motors_running()) { + scheduled_pitch_angle = 8.; + max_pitch_limit_rad = fwd_pitch_limit_rad; + } + Bound(scheduled_pitch_angle, -5., 8.); + guidance_indi_pitch_pref_deg = scheduled_pitch_angle; + + float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); + + // Set lower limits + wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll + wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch + + // Set upper limits limits + wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll + wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch + + if(rotwing_state_hover_motors_running()) { + wls_guid_p.u_min[2] = du_min_thrust_z; + wls_guid_p.u_max[2] = du_max_thrust_z; + } else { + wls_guid_p.u_min[2] = 0.; + wls_guid_p.u_max[2] = 0.; + } + + if(rotwing_state_pusher_motor_running()) { + wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); + wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition + } else { + wls_guid_p.u_min[3] = 0.; + wls_guid_p.u_max[3] = 0.; + } + + // Set prefered states + wls_guid_p.u_pref[0] = 0; // prefered delta roll angle + wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle + wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency + wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration +} \ No newline at end of file diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.c b/sw/airborne/modules/rotwing_drone/rotwing_state.c index 500956c40a..63d509b030 100644 --- a/sw/airborne/modules/rotwing_drone/rotwing_state.c +++ b/sw/airborne/modules/rotwing_drone/rotwing_state.c @@ -24,13 +24,12 @@ */ #include "modules/rotwing_drone/rotwing_state.h" -#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" -#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" #include "firmwares/rotorcraft/autopilot_firmware.h" - +#include "modules/core/commands.h" #include "modules/actuators/actuators.h" #include "modules/core/abi.h" + /* Minimum measured RPM to consider the hover motors running (RPM) */ #ifndef ROTWING_QUAD_MIN_RPM #define ROTWING_QUAD_MIN_RPM 400 @@ -61,11 +60,6 @@ #define ROTWING_FW_SKEW_ANGLE 85.0 #endif -/* Magnitude skew angle jump away from quad */ -#ifndef ROTWING_SKEW_ANGLE_STEP -#define ROTWING_SKEW_ANGLE_STEP 55.0 -#endif - /* TODO: Give a name.... */ #ifndef ROTWING_SKEW_BACK_MARGIN #define ROTWING_SKEW_BACK_MARGIN 5.0 @@ -81,11 +75,6 @@ #define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE 30.0 #endif -/* Preferred pitch angle for the quad mode (deg) */ -#ifndef ROTWING_QUAD_PREF_PITCH -#define ROTWING_QUAD_PREF_PITCH -5.0 -#endif - /* Amount of time the airspeed needs to be below the FW_MIN_AIRSPEED */ #ifndef ROTWING_FW_STALL_TIMEOUT #define ROTWING_FW_STALL_TIMEOUT 0.5 @@ -107,13 +96,7 @@ #endif abi_event rotwing_state_feedback_ev; static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act); - -static bool rotwing_state_hover_motors_idling(void); -static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; struct rotwing_state_t rotwing_state; - -inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle); - #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev) @@ -164,7 +147,6 @@ void rotwing_state_init(void) rotwing_state.fail_hover_motor = false; rotwing_state.fail_pusher_motor = false; rotwing_state.ref_model_skew_angle_deg = 0; - // Bind ABI messages AbiBindMsgACT_FEEDBACK(ROTWING_STATE_ACT_FEEDBACK_ID, &rotwing_state_feedback_ev, rotwing_state_feedback_cb); @@ -177,7 +159,7 @@ void rotwing_state_init(void) * @brief Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDLE_TIMEOUT time * @return true if hover motors are idling, false otherwise */ -static bool rotwing_state_hover_motors_idling(void) { +bool rotwing_state_hover_motors_idling(void) { static float last_idle_time = 0; // Check if hover motors are idling and reset timer if(stabilization.cmd[COMMAND_THRUST] > ROTWING_QUAD_IDLE_MIN_THRUST) { @@ -306,9 +288,7 @@ void rotwing_state_periodic(void) rotwing_state.max_airspeed = 0; // Max airspeed FW }*/ - guidance_indi_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed); - - + guidance_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed); /* Set navigation/guidance settings */ nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew? @@ -333,13 +313,18 @@ void rotwing_state_periodic(void) #else rotwing_state.skew_cmd = servo_pprz_cmd; #endif - +#ifdef COMMAND_ROT_MECH + commands[COMMAND_ROT_MECH] = rotwing_state.skew_cmd; +#endif /* Add simulation feedback for the skewing and RPM */ #if USE_NPS // Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array +#ifdef COMMAND_ROT_MECH + commands[COMMAND_ROT_MECH] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command +#else actuators_pprz[INDI_NUM_ACT] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command - +#endif // SEND ABI Message to ctr_eff_sched, ourself and other modules that want Actuator position feedback struct act_feedback_t feedback; feedback.idx = SERVO_ROTATION_MECH_IDX; @@ -448,94 +433,6 @@ bool rotwing_state_skew_angle_valid(void) { return (!skew_timeout && skew_angle_match); } -void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) -{ - // adjust weights - float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ... - Bound(fixed_wing_percentile, 0, 1); -#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 - - float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES; - - // Increase importance of forward acceleration in forward flight - wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile * - AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) - - struct FloatEulers eulers_zxy; - float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); - - float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + - (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * rotwing_state_hover_motors_running(); - Bound(du_min_thrust_z, -50., 0.); - float du_max_thrust_z = -(actuator_state_filt_vect[0] * g1g2[3][0] + actuator_state_filt_vect[1] * g1g2[3][1] + - actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]); - Bound(du_max_thrust_z, 0., 50.); - - float roll_limit_rad = guidance_indi_max_bank; - float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); - float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); - - float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); - float quad_pitch_limit_rad = RadOfDeg(5.0); - - float airspeed = stateGetAirspeed_f(); - - float scheduled_pitch_angle = 0.f; - float pitch_angle_range = 3.; - float meas_skew_angle = rotwing_state.meas_skew_angle_deg; - Bound(meas_skew_angle, 0, 90); // Bound to prevent errors - if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) { - scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH; - wls_guid_p.Wu[1] = Wu_gih_original[1]; - max_pitch_limit_rad = quad_pitch_limit_rad; - } else { - float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f; - Bound(pitch_progression, 0.f, 1.f); - scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression); - wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99); - max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression; - } - if (!rotwing_state_hover_motors_running()) { - scheduled_pitch_angle = 8.; - max_pitch_limit_rad = fwd_pitch_limit_rad; - } - Bound(scheduled_pitch_angle, -5., 8.); - guidance_indi_pitch_pref_deg = scheduled_pitch_angle; - - float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); - - // Set lower limits - wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll - wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch - - // Set upper limits limits - wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll - wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch - - if(rotwing_state_hover_motors_running()) { - wls_guid_p.u_min[2] = du_min_thrust_z; - wls_guid_p.u_max[2] = du_max_thrust_z; - } else { - wls_guid_p.u_min[2] = 0.; - wls_guid_p.u_max[2] = 0.; - } - - if(rotwing_state_pusher_motor_running()) { - wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); - wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition - } else { - wls_guid_p.u_min[3] = 0.; - wls_guid_p.u_max[3] = 0.; - } - - // Set prefered states - wls_guid_p.u_pref[0] = 0; // prefered delta roll angle - wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle - wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency - wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration -} - void rotwing_state_set(enum rotwing_states_t state) { rotwing_state.nav_state = state; } diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state.h b/sw/airborne/modules/rotwing_drone/rotwing_state.h index cfb03d887c..9d1db31259 100644 --- a/sw/airborne/modules/rotwing_drone/rotwing_state.h +++ b/sw/airborne/modules/rotwing_drone/rotwing_state.h @@ -28,6 +28,16 @@ #include "std.h" +/* Magnitude skew angle jump away from quad */ +#ifndef ROTWING_SKEW_ANGLE_STEP +#define ROTWING_SKEW_ANGLE_STEP 55.0 +#endif + +/* Preferred pitch angle for the quad mode (deg) */ +#ifndef ROTWING_QUAD_PREF_PITCH +#define ROTWING_QUAD_PREF_PITCH -5.0 +#endif + enum rotwing_states_t { ROTWING_STATE_FORCE_HOVER, ROTWING_STATE_REQUEST_HOVER, @@ -84,6 +94,7 @@ void rotwing_state_periodic(void); bool rotwing_state_hover_motors_running(void); bool rotwing_state_pusher_motor_running(void); bool rotwing_state_skew_angle_valid(void); +bool rotwing_state_hover_motors_idling(void); void rotwing_state_set(enum rotwing_states_t state); bool rotwing_state_choose_circle_direction(uint8_t wp_id); diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state_V2.c b/sw/airborne/modules/rotwing_drone/rotwing_state_V2.c deleted file mode 100644 index 6a9d517736..0000000000 --- a/sw/airborne/modules/rotwing_drone/rotwing_state_V2.c +++ /dev/null @@ -1,797 +0,0 @@ -/* - * Copyright (C) 2023 Tomaso De Ponti - * - * This file is part of paparazzi - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** @file "modules/rotwing/rotwing_state_V2.c" - * @author Tomaso De Ponti - * This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state. - */ - -#include "modules/rotwing_drone/rotwing_state_V2.h" -#include "firmwares/rotorcraft/autopilot_firmware.h" -#include "modules/core/commands.h" -#include "modules/actuators/actuators.h" -#include "modules/core/abi.h" - -// Quad state identification -#ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD -#define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0 -#endif - -#ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER -#define ROTWING_MIN_SKEW_ANGLE_COUNTER 10 // Minimum number of loops the skew angle is below ROTWING_MIN_SKEW_ANGLE_COUNTER -#endif - -// Skewing state identification -#ifndef ROTWING_SKEWING_COUNTER -#define ROTWING_SKEWING_COUNTER 10 // Minimum number of loops the skew angle is in between QUAD and FW -#endif - -// maximum quad airspeed to force quad state -#ifndef ROTWING_MAX_QUAD_AIRSPEED -#define ROTWING_MAX_QUAD_AIRSPEED 20.0 -#endif - -// Half skew state identification -#ifndef ROTWING_HALF_SKEW_ANGLE_DEG -#define ROTWING_HALF_SKEW_ANGLE_DEG 55.0 -#endif - -#ifndef ROTWING_HALF_SKEW_ANGLE_RANG -#define ROTWING_HALF_SKEW_ANGLE_HALF_RANGE 10.0 -#endif - -#ifndef ROTWING_HALF_SKEW_COUNTER -#define ROTWING_HALF_SKEW_COUNTER 10 // Minimum number of loops the skew angle is at HALF_SKEW_ANGLE_DEG +/- ROTWING_HALF_SKEW_ANGLE_HALF_RANGE to trigger ROTWING_HALF_SKEW_ANGLE state -#endif - -// FW state identification -#ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG -#define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state -#endif - -#ifndef ROTWING_MIN_FW_COUNTER -#define ROTWING_MIN_FW_COUNTER 10 // Minimum number of loops the skew angle is above the MIN_FW_SKEW_ANGLE -#endif - -// FW idle state identification -#ifndef ROTWING_MIN_THRUST_IDLE -#define ROTWING_MIN_THRUST_IDLE 100 -#endif - -#ifndef ROTWING_MIN_THRUST_IDLE_COUNTER -#define ROTWING_MIN_THRUST_IDLE_COUNTER 10 -#endif - -// FW hov mot off state identification -#ifndef ROTWING_HOV_MOT_RUN_RPM_TH -#define ROTWING_HOV_MOT_RUN_RPM_TH 800 -#endif -#ifndef ROTWING_HOV_MOT_OFF_RPM_TH -#define ROTWING_HOV_MOT_OFF_RPM_TH 50 -#endif - -#ifndef ROTWING_HOV_MOT_OFF_COUNTER -#define ROTWING_HOV_MOT_OFF_COUNTER 10 -#endif - -#ifndef ROTWING_STATE_USE_ROTATION_REF_MODEL -#define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE -#endif - -// Hover preferred pitch (deg) -#ifndef ROTWING_STATE_HOVER_PREF_PITCH -#define ROTWING_STATE_HOVER_PREF_PITCH 0.0 -#endif - -// Transition preferred pitch (deg) -#ifndef ROTWING_STATE_TRANSITION_PREF_PITCH -#define ROTWING_STATE_TRANSITION_PREF_PITCH 3.0 -#endif - -// Forward preferred pitch (deg) -#ifndef ROTWING_STATE_FW_PREF_PITCH -#define ROTWING_STATE_FW_PREF_PITCH 8.0 -#endif - -// Make sure the rotmech dynamics are provided if the virtual rotmech is used -#ifndef USE_ROTMECH_VIRTUAL -#define USE_ROTMECH_VIRTUAL FALSE -#endif - -#if USE_ROTMECH_VIRTUAL -#ifndef ROTMECH_DYN -#error "Rotmech dynamics are not provided. Please provide them in your airframe file." -#endif -#endif - -// stream ADC data if ADC rotation sensor -#ifndef ADC_WING_ROTATION -#define ADC_WING_ROTATION FALSE -#endif -#if ADC_WING_ROTATION -#include "wing_rotation_adc_sensor.h" -#endif -/** ABI binding feedback data. - */ -#ifndef ROTWING_STATE_ACT_FEEDBACK_ID -#define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST -#endif -abi_event rotwing_state_feedback_ev; -static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act); -#define ROTWING_STATE_NUM_HOVER_RPM 4 -int32_t rotwing_state_hover_rpm[ROTWING_STATE_NUM_HOVER_RPM] = {0, 0, 0, 0}; - -struct RotwingState rotwing_state; -struct RotWingStateSettings rotwing_state_settings; -struct RotWingStateSkewing rotwing_state_skewing; - -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; -#ifdef NAV_HYBRID_MAX_AIRSPEED -float rotwing_state_max_fw_speed = NAV_HYBRID_MAX_AIRSPEED; -#else -float rotwing_state_max_fw_speed = 20; -#endif - -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); - -inline void rotwing_state_set_hover_settings(void); -inline void rotwing_state_set_skewing_settings(void); -inline void rotwing_state_set_fw_settings(void); -inline void rotwing_state_set_fw_hov_mot_idle_settings(void); -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); - -#if PERIODIC_TELEMETRY -#include "modules/datalink/telemetry.h" -static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev) -{ - uint16_t dummy = 0; - float nav_airspeed = stateGetAirspeed_f();; - float min_airspeed = 0; - float max_airspeed = rotwing_state_max_fw_speed; - - pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID, - &rotwing_state.current_state, - &rotwing_state.desired_state, - &dummy, - &rotwing_state_skewing.wing_angle_deg, - &rotwing_state_skewing.wing_angle_deg_sp, - &nav_airspeed, - &min_airspeed, - &max_airspeed); -} -#endif // PERIODIC_TELEMETRY - -void rotwing_state_force_skew_off(void) -{ - rotwing_state_skewing.force_rotation_angle = false; - demo_skew = false; -} - -void init_rotwing_state(void) -{ - // Bind ABI messages - AbiBindMsgACT_FEEDBACK(ROTWING_STATE_ACT_FEEDBACK_ID, &rotwing_state_feedback_ev, rotwing_state_feedback_cb); - - // 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_settings.preferred_pitch_value = 0; - - 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; - demo_skew = false; - -#if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state); -#endif -} - -void periodic_rotwing_state(void) -{ - // Check and set the current state - rotwing_check_set_current_state(); - - // 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) { - if (stabilization.att_submode == STABILIZATION_ATT_SUBMODE_FORWARD) { - rotwing_state_set_fw_settings(); - } else { - rotwing_state_set_hover_settings(); - } - } - } - // } else { - // rotwing_switch_state(); - // } - - // Run the wing skewer - rotwing_state_skewer(); - - //TODO: incorparate motor active / disbaling depending on called flight state - // Switch on motors if flight mode is attitude - if (guidance_h.mode == GUIDANCE_H_MODE_NONE) { - bool_disable_hover_motors = false; - } - struct FloatEulers eulers_zxy; - float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); - - // Evaluate motors_on boolean - if (!hover_motors_active) { - if (stateGetAirspeed_f() < 15.) { - hover_motors_active = true; - bool_disable_hover_motors = false; - } else if (eulers_zxy.theta > RadOfDeg(15.0)) { - hover_motors_active = true; - bool_disable_hover_motors = false; - } - } else { - bool_disable_hover_motors = false; - } - float pitch_progression = (rotwing_state_skewing.wing_angle_deg - ROTWING_HALF_SKEW_ANGLE_DEG) / (90.0-ROTWING_HALF_SKEW_ANGLE_DEG); - // Calculate scheduled pitch angle - switch (rotwing_state_settings.preferred_pitch_setting){ - case ROTWING_STATE_PITCH_QUAD_SETTING: - rotwing_state_settings.preferred_pitch_value = RadOfDeg(ROTWING_STATE_HOVER_PREF_PITCH); - break; - case ROTWING_STATE_PITCH_TRANSITION_SETTING: - rotwing_state_settings.preferred_pitch_value = RadOfDeg(ROTWING_STATE_TRANSITION_PREF_PITCH * pitch_progression); - break; - case ROTWING_STATE_PITCH_FW_SETTING: - rotwing_state_settings.preferred_pitch_value = RadOfDeg(ROTWING_STATE_FW_PREF_PITCH); - break; - } -} - -// Function to request a state -void request_rotwing_state(uint8_t state) -{ - if (state <= ROTWING_STATE_FW_HOV_MOT_OFF) { - rotwing_state.desired_state = state; - } -} - -// Function to prefer configuration -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; - } -} - -void rotwing_check_set_current_state(void) -{ - float current_thrust = (commands[COMMAND_MOTOR_FRONT] + commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_BACK] + commands[COMMAND_MOTOR_LEFT])/4.0; - // if !in_flight, set state to hover - if (!autopilot.in_flight) { - rotwing_state_hover_counter = 0; - rotwing_state_skewing_counter = 0; - rotwing_state_fw_counter = 0; - rotwing_state_fw_idle_counter = 0; - rotwing_state_fw_m_off_counter = 0; - rotwing_state.current_state = ROTWING_STATE_HOVER; - return; - } - - // States can be checked according to wing angle sensor, setpoints ..... - uint8_t prev_state = rotwing_state.current_state; - switch (prev_state) { - case ROTWING_STATE_HOVER: - // Check if state needs to be set to skewing - if (rotwing_state_skewing.wing_angle_deg > ROTWING_MIN_SKEW_ANGLE_DEG_QUAD) { - rotwing_state_skewing_counter++; - } else { - rotwing_state_skewing_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_skewing_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_SKEWING; - rotwing_state_skewing_counter = 0; - } - break; - - case ROTWING_STATE_SKEWING: - // Check if state needs to be set to hover - if (rotwing_state_skewing.wing_angle_deg < ROTWING_MIN_SKEW_ANGLE_DEG_QUAD) { - rotwing_state_hover_counter++; - } else { - rotwing_state_hover_counter = 0; - } - - // Check if state needs to be set to fixed wing - if (rotwing_state_skewing.wing_angle_deg > ROTWING_MIN_FW_SKEW_ANGLE_DEG) { - rotwing_state_fw_counter++; - } else { - rotwing_state_fw_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_hover_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_HOVER; - rotwing_state_hover_counter = 0; - } - - if (rotwing_state_fw_counter > ROTWING_MIN_FW_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_FW; - rotwing_state_fw_counter = 0; - } - break; - - case ROTWING_STATE_FW: - // Check if state needs to be set to skewing - if (rotwing_state_skewing.wing_angle_deg < ROTWING_MIN_FW_SKEW_ANGLE_DEG) { - rotwing_state_skewing_counter++; - } else { - rotwing_state_skewing_counter = 0; - } - - // Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold) - if (current_thrust < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state > ROTWING_STATE_FW) { - rotwing_state_fw_idle_counter++; - } else { - rotwing_state_fw_idle_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_skewing_counter > ROTWING_MIN_FW_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_SKEWING; - rotwing_state_skewing_counter = 0; - rotwing_state_fw_idle_counter = 0; - } else if (rotwing_state_fw_idle_counter > ROTWING_MIN_THRUST_IDLE_COUNTER - && rotwing_state_skewing_counter == 0) { - rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE; - rotwing_state_skewing_counter = 0; - rotwing_state_fw_idle_counter = 0; - } - break; - - case ROTWING_STATE_FW_HOV_MOT_IDLE: - // Check if state needs to be set to fixed wing with hover motors activated - if (current_thrust > ROTWING_MIN_THRUST_IDLE - || rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) { - rotwing_state_fw_counter++; - } else { - rotwing_state_fw_counter = 0; - } - // Check if state needs to be set to fixed wing with hover motors off (if zero rpm on hover motors) - if (rotwing_state_hover_rpm[0] == 0 - && rotwing_state_hover_rpm[1] == 0 - && rotwing_state_hover_rpm[2] == 0 - && rotwing_state_hover_rpm[3] == 0) { -#if !USE_NPS - rotwing_state_fw_m_off_counter++; -#endif - } else { - rotwing_state_fw_m_off_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_fw_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_FW; - rotwing_state_fw_counter = 0; - rotwing_state_fw_m_off_counter = 0; - } else if (rotwing_state_fw_m_off_counter > ROTWING_HOV_MOT_OFF_COUNTER - && rotwing_state_fw_counter == 0) { - rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_OFF; - rotwing_state_fw_counter = 0; - rotwing_state_fw_m_off_counter = 0; - } - break; - - case ROTWING_STATE_FW_HOV_MOT_OFF: - // Check if state needs to be set to fixed wing with hover motors idle (if rpm on hover motors) - if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_OFF_RPM_TH - && rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_OFF_RPM_TH - && rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_OFF_RPM_TH - && rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_OFF_RPM_TH) { - rotwing_state_fw_idle_counter++; - } else { - rotwing_state_fw_idle_counter = 0; - } - - // Switch state if necessary - if (rotwing_state_fw_idle_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) { - rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE; - rotwing_state_fw_idle_counter = 0; - } - break; - - default: - break; - } -} - -// Function that handles settings for switching state(s) -void rotwing_switch_state(void) -{ - switch (rotwing_state.current_state) { - case ROTWING_STATE_HOVER: - if (rotwing_state.desired_state > ROTWING_STATE_HOVER) { - rotwing_state_set_skewing_settings(); - } else { - rotwing_state_set_hover_settings(); - } - break; - - case ROTWING_STATE_SKEWING: - if (rotwing_state.desired_state < ROTWING_STATE_SKEWING && stateGetAirspeed_f() < ROTWING_MAX_QUAD_AIRSPEED) { - rotwing_state_set_hover_settings(); - } else { - rotwing_state_set_skewing_settings(); - } - break; - - case ROTWING_STATE_FW: - if (rotwing_state.desired_state > ROTWING_STATE_FW) { - rotwing_state_set_fw_hov_mot_idle_settings(); - } else if (rotwing_state.desired_state < ROTWING_STATE_FW) { - rotwing_state_set_skewing_settings(); - } else { - rotwing_state_set_fw_settings(); - } - break; - - case ROTWING_STATE_FW_HOV_MOT_IDLE: - if (rotwing_state.desired_state > ROTWING_STATE_FW_HOV_MOT_IDLE) { - rotwing_state_set_fw_hov_mot_off_settings(); - } else if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) { - rotwing_state_set_fw_settings(); - } else { - rotwing_state_set_fw_hov_mot_idle_settings(); - } - break; - - case ROTWING_STATE_FW_HOV_MOT_OFF: - if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_OFF) { - rotwing_state_set_fw_hov_mot_idle_settings(); - } else { - rotwing_state_set_fw_hov_mot_off_settings(); - } - break; - } -} - -void rotwing_state_set_hover_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_QUAD_SETTING; - rotwing_state_settings.hover_motors_active = true; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = false; - rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_QUAD_SETTING; - 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) -{ - // 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; - rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_TRANSITION_SETTING; - 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_fw_speed;// Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_fw_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; - rotwing_state_settings.hover_motors_active = true; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = true; - rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_FW_SETTING; - 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 = rotwing_state_max_fw_speed; // Big as we use airspeed guidance - //rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_fw_hov_mot_idle_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; - rotwing_state_settings.hover_motors_active = false; - rotwing_state_settings.hover_motors_disable = false; - rotwing_state_settings.force_forward = true; - rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_FW_SETTING; - 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 = rotwing_state_max_fw_speed; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_fw_hov_mot_off_settings(void) -{ - rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; - rotwing_state_settings.hover_motors_active = false; - rotwing_state_settings.hover_motors_disable = true; - rotwing_state_settings.force_forward = true; - rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_FW_SETTING; - 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 = rotwing_state_max_fw_speed; // Big as we use airspeed guidance - rotwing_state_set_state_settings(); -} - -void rotwing_state_set_state_settings(void) -{ - if (!rotwing_state_skewing.force_rotation_angle) { - switch (rotwing_state_settings.wing_scheduler) { - case ROTWING_STATE_WING_QUAD_SETTING: - rotwing_state_skewing.airspeed_scheduling = false; - rotwing_state_skewing.wing_angle_deg_sp = 0; - break; - case ROTWING_STATE_WING_SCHEDULING_SETTING: - rotwing_state_skewing.airspeed_scheduling = true; - break; - case ROTWING_STATE_WING_FW_SETTING: - rotwing_state_skewing.airspeed_scheduling = true; - break; - } - } else { - rotwing_state_skewing.airspeed_scheduling = false; - } - - hover_motors_active = rotwing_state_settings.hover_motors_active; - - bool_disable_hover_motors = rotwing_state_settings.hover_motors_disable; - - 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 - - // TO DO: Climb and descend speeds now handled by guidance airspeed -} - -void rotwing_state_skewer(void) -{ - if (rotwing_state_skewing.airspeed_scheduling) { - float wing_angle_scheduled_sp_deg = 0; - float airspeed = stateGetAirspeed_f(); - if (airspeed < 8) { - wing_angle_scheduled_sp_deg = 0; - } else if (airspeed < 10 && (rotwing_state.desired_state > ROTWING_STATE_HOVER)) { - wing_angle_scheduled_sp_deg = ROTWING_HALF_SKEW_ANGLE_DEG; - } else if (airspeed > 10) { - wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * (90.-ROTWING_HALF_SKEW_ANGLE_DEG) + ROTWING_HALF_SKEW_ANGLE_DEG; - } else { - wing_angle_scheduled_sp_deg = 0; - } - - 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; - } - } -} - -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 = rotwing_state_max_fw_speed - 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) -{ - - // 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_USE_ROTATION_REF_MODEL - // Rotate with second order filter - static float rotwing_state_skew_p_cmd = -MAX_PPRZ; - static float rotwing_state_skew_d_cmd = 0; - float speed_sp = 0.001 * (servo_pprz_cmd - rotwing_state_skew_p_cmd); - rotwing_state_skew_d_cmd += 0.003 * (speed_sp - rotwing_state_skew_d_cmd); - 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 - // Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array - commands[COMMAND_ROT_MECH] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command - -#if USE_ROTMECH_VIRTUAL - // Calculate discrete first order dynamics of rot mech - float dyn_dis = 1.0-exp(-ROTMECH_DYN / PERIODIC_FREQUENCY); - // Simulate wing angle from command - float prev_rotmech_state = rotwing_state_skewing.wing_angle_deg; - rotwing_state_skewing.wing_angle_deg = prev_rotmech_state + dyn_dis * (rotwing_state_skewing.wing_angle_deg_sp - prev_rotmech_state); -#else - // Simulate wing angle from command - rotwing_state_skewing.wing_angle_deg = (float) rotwing_state_skewing.servo_pprz_cmd / MAX_PPRZ * 45. + 45.; -#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 = 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); -#else - commands[COMMAND_ROT_MECH] = rotwing_state_skewing.servo_pprz_cmd; -#endif // USE_NPS -} - -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_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); - - // Bound wing rotation angle - Bound(rotwing_state_skewing.wing_angle_deg, 0, 90.); - } - } - - // Sanity check that index is valid - int idx = feedback_msg[i].idx; - if (feedback_msg[i].set.rpm) { - if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) { - rotwing_state_hover_rpm[0] = feedback_msg->rpm; - } else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) { - rotwing_state_hover_rpm[1] = feedback_msg->rpm; - } else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) { - rotwing_state_hover_rpm[2] = feedback_msg->rpm; - } else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) { - rotwing_state_hover_rpm[3] = feedback_msg->rpm; - } - } - } -} - diff --git a/sw/airborne/modules/rotwing_drone/rotwing_state_V2.h b/sw/airborne/modules/rotwing_drone/rotwing_state_V2.h deleted file mode 100644 index e3a0ac93cc..0000000000 --- a/sw/airborne/modules/rotwing_drone/rotwing_state_V2.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright (C) 2023 Tomaso De Ponti - * - * This file is part of paparazzi - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, see - * . - */ - -/** @file "modules/rotwing/rotwing_state_V2.h" - * @author Tomaso De Ponti - * This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state. - */ - -#ifndef ROTWING_STATE_H -#define ROTWING_STATE_H - -#include "std.h" - -/** Rotwing States - */ -#define ROTWING_STATE_HOVER 0 // Wing is skewed in 0 degrees (quad) -#define ROTWING_STATE_SKEWING 1 // WIng is skewing -#define ROTWING_STATE_FW 2 // Wing is skewed at 90 degrees (fixed wing), hover motors have full authority -#define ROTWING_STATE_FW_HOV_MOT_IDLE 3 // Wing is skewed at 90 degrees (fixed wing), hover motors are forced to idle -#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) -#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 -#define ROTWING_STATE_WING_SCHEDULING_SETTING 1 // Wing skew handled by airspeed scheduler -#define ROTWING_STATE_WING_FW_SETTING 2 // Wing skew at 90 degrees - -#define ROTWING_STATE_PITCH_QUAD_SETTING 0 // Pitch at prefered hover -#define ROTWING_STATE_PITCH_TRANSITION_SETTING 1 // Pitch scheduled -#define ROTWING_STATE_PITCH_FW_SETTING 2 // Pitch at prefered forward - -struct RotWingStateSettings { - uint8_t wing_scheduler; - bool hover_motors_active; - bool hover_motors_disable; - bool force_forward; - uint8_t preferred_pitch_setting; - float preferred_pitch_value; - 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 -}; - -extern struct RotwingState rotwing_state; -extern struct RotWingStateSettings rotwing_state_settings; -extern struct RotWingStateSkewing rotwing_state_skewing; - -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); -extern void request_rotwing_state(uint8_t state); -extern void rotwing_request_configuration(uint8_t configuration); -extern void rotwing_state_skew_actuator_periodic(void); -extern void rotwing_state_force_skew_off(void); - -#endif // ROTWING_STATE_H