diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml index b1930d4f4b..ac0238c28c 100644 --- a/conf/airframes/tudelft/rot_wing_25kg.xml +++ b/conf/airframes/tudelft/rot_wing_25kg.xml @@ -31,6 +31,10 @@ + + + + @@ -139,7 +143,7 @@ - + @@ -187,7 +191,7 @@ - + @@ -202,7 +206,7 @@ - + @@ -212,7 +216,7 @@
- + @@ -278,8 +282,8 @@ - - +
@@ -373,8 +377,8 @@ - - + + diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml index b75f1e47c8..4ce71b4180 100644 --- a/conf/airframes/tudelft/rot_wing_v3b.xml +++ b/conf/airframes/tudelft/rot_wing_v3b.xml @@ -119,7 +119,7 @@ - + @@ -189,7 +189,7 @@ - + @@ -240,7 +240,7 @@ - + @@ -254,7 +254,7 @@
- + @@ -299,16 +299,17 @@ - - - - - - - - - - + + + + + + + + + + +
@@ -384,9 +385,9 @@ - - - + + + @@ -416,14 +417,6 @@
-
- - - - - -
-
@@ -450,7 +443,7 @@
- + diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml new file mode 100644 index 0000000000..62b836a45c --- /dev/null +++ b/conf/airframes/tudelft/rot_wing_v3d.xml @@ -0,0 +1,516 @@ + + + + RotatingWingV3D + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + +
+ +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + + + +
+ +
+ + + + +
+ +
+ + + + + + +
+ +
+ + + + + + +
+ diff --git a/conf/modules/eff_scheduling_rot_wing.xml b/conf/modules/eff_scheduling_rot_wing.xml index 5e8bbcdfa9..34cc2d94c4 100644 --- a/conf/modules/eff_scheduling_rot_wing.xml +++ b/conf/modules/eff_scheduling_rot_wing.xml @@ -27,7 +27,7 @@ - + diff --git a/conf/modules/rotwing_state.xml b/conf/modules/rotwing_state.xml index a233d39aad..72dad119fd 100644 --- a/conf/modules/rotwing_state.xml +++ b/conf/modules/rotwing_state.xml @@ -2,6 +2,9 @@ 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. +
+ +
@@ -19,6 +22,7 @@ + diff --git a/conf/modules/sys_id_doublet.xml b/conf/modules/sys_id_doublet.xml index 5376563279..eb3f1db01c 100644 --- a/conf/modules/sys_id_doublet.xml +++ b/conf/modules/sys_id_doublet.xml @@ -37,7 +37,7 @@ - + diff --git a/conf/modules/wing_rotation_adc_sensor.xml b/conf/modules/wing_rotation_adc_sensor.xml new file mode 100644 index 0000000000..4760d7c9c0 --- /dev/null +++ b/conf/modules/wing_rotation_adc_sensor.xml @@ -0,0 +1,18 @@ + + + + Module to read wing skew angle from ADC + + +
+ +
+ + + + + + + + + diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml index d7def2a1d6..218e3e3497 100644 --- a/conf/telemetry/highspeed_rotorcraft.xml +++ b/conf/telemetry/highspeed_rotorcraft.xml @@ -254,7 +254,9 @@ - + + + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 50da0328b0..5eacc1e6a9 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -139,7 +139,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/tudelft/cyclone_valkenburg.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml" + settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml" gui_color="#ffff7f7f0000" /> + diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c index c4561855bc..cf9b5756ce 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c @@ -33,8 +33,8 @@ #include "modules/actuators/actuators.h" #include "modules/core/abi.h" -#ifndef SERVO_ROTATION_MECH -#error ctrl_eff_sched_rot_wing requires a servo named ROTATION_MECH +#ifndef SERVO_ROTATION_MECH_IDX +#error ctrl_eff_sched_rot_wing requires a servo named ROTATION_MECH_IDX #endif #ifndef ROT_WING_EFF_SCHED_IXX_BODY @@ -172,7 +172,7 @@ static abi_event wing_position_ev; static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act) { for (int i=0; i rot_wing_a.trans_airspeed) - { + if (airspeed > rot_wing_a.trans_airspeed) { rot_wing_a.transitioned = true; } else { rot_wing_a.transitioned = false; @@ -101,7 +100,8 @@ void periodic_rot_wing_automation(void) } // Update a waypoint such that you can see on the GCS where the drone wants to go -void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned) { +void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 *target_ned) +{ // Update the waypoint struct EnuCoor_f target_enu; @@ -109,13 +109,13 @@ void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * targ waypoint_set_enu(wp_id, &target_enu); // Send waypoint update roughly every half second - RunOnceEvery(100/2, { + RunOnceEvery(100 / 2, { // Send to the GCS that the waypoint has been moved DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, - &waypoints[wp_id].enu_i.x, - &waypoints[wp_id].enu_i.y, - &waypoints[wp_id].enu_i.z); - } ); + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); + }); } void update_wind_vector(void) diff --git a/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h b/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h index 5c18d4ff64..2ca119d0b4 100644 --- a/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h +++ b/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h @@ -44,7 +44,7 @@ struct rot_wing_automation { // Variables bool transitioned; // Boolean that indicates if drone flies faster than transion airspeed struct FloatVect2 windvect; // Wind vector - struct FloatVect2 windvect_f; // Filtered wind vector + struct FloatVect2 windvect_f; // Filtered wind vector }; extern struct rot_wing_automation rot_wing_a; diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/sw/airborne/modules/rot_wing_drone/rotwing_state.c index b52086a31d..99ea1ad4f6 100644 --- a/sw/airborne/modules/rot_wing_drone/rotwing_state.c +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state.c @@ -85,19 +85,8 @@ #define ROTWING_HOV_MOT_OFF_COUNTER 10 #endif -#ifndef ROTWING_STATE_PD_WING_ROTATION -#define ROTWING_STATE_PD_WING_ROTATION FALSE -#endif - -#if ROTWING_STATE_PD_WING_ROTATION -#ifndef ROTWING_STATE_WING_ROTATION_P_GAIN -#define ROTWING_STATE_WING_ROTATION_P_GAIN 0.001 -#endif -#ifndef ROTWING_STATE_WING_ROTATION_D_GAIN -#define ROTWING_STATE_WING_ROTATION_D_GAIN 0.003 -#endif -float rotwing_state_skew_p_cmd = -MAX_PPRZ; -float rotwing_state_skew_d_cmd = 0; +#ifndef ROTWING_STATE_USE_ROTATION_REF_MODEL +#define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE #endif @@ -122,7 +111,7 @@ float rotwing_state_skew_d_cmd = 0; #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); +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}; @@ -161,18 +150,18 @@ inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_an static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev) { uint16_t adc_dummy = 0; - pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID, - &rotwing_state.current_state, - &rotwing_state.desired_state, - &rotwing_state_skewing.wing_angle_deg, - &rotwing_state_skewing.wing_angle_deg_sp, - &adc_dummy, - &rotwing_state_skewing.servo_pprz_cmd); + pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID, + &rotwing_state.current_state, + &rotwing_state.desired_state, + &rotwing_state_skewing.wing_angle_deg, + &rotwing_state_skewing.wing_angle_deg_sp, + &adc_dummy, + &rotwing_state_skewing.servo_pprz_cmd); } #endif // PERIODIC_TELEMETRY void init_rotwing_state(void) -{ +{ // Bind ABI messages AbiBindMsgACT_FEEDBACK(ROTWING_STATE_ACT_FEEDBACK_ID, &rotwing_state_feedback_ev, rotwing_state_feedback_cb); @@ -182,13 +171,13 @@ void init_rotwing_state(void) 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.servo_pprz_cmd = -MAX_PPRZ; + rotwing_state_skewing.airspeed_scheduling = false; rotwing_state_skewing.force_rotation_angle = false; - #if PERIODIC_TELEMETRY +#if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state); - #endif +#endif } void periodic_rotwing_state(void) @@ -220,8 +209,7 @@ void periodic_rotwing_state(void) // Function to request a state void request_rotwing_state(uint8_t state) { - if (state <= ROTWING_STATE_FW_HOV_MOT_OFF) - { + if (state <= ROTWING_STATE_FW_HOV_MOT_OFF) { rotwing_state.desired_state = state; } } @@ -229,18 +217,18 @@ void request_rotwing_state(uint8_t state) // Function to prefer configuration void rotwing_request_configuration(uint8_t configuration) { - switch (configuration) { - case ROTWING_CONFIGURATION_HOVER: - request_rotwing_state(ROTWING_STATE_HOVER); - break; - case ROTWING_CONFIGURATION_HYBRID: - request_rotwing_state(ROTWING_STATE_SKEWING); + switch (configuration) { + case ROTWING_CONFIGURATION_HOVER: + request_rotwing_state(ROTWING_STATE_HOVER); break; - case ROTWING_CONFIGURATION_EFFICIENT: - request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF); + case ROTWING_CONFIGURATION_HYBRID: + request_rotwing_state(ROTWING_STATE_SKEWING); break; - } -} + case ROTWING_CONFIGURATION_EFFICIENT: + request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF); + break; + } +} void rotwing_check_set_current_state(void) { @@ -258,132 +246,130 @@ void rotwing_check_set_current_state(void) // 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; - } + 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; + // 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; - } + 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; - } + // 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; - } + // 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; + 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; - } + 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 (stabilization_cmd[COMMAND_THRUST] < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state > ROTWING_STATE_FW) { - rotwing_state_fw_idle_counter++; - } else { - rotwing_state_fw_idle_counter = 0; - } + // Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold) + if (stabilization_cmd[COMMAND_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; + // 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 (stabilization_cmd[COMMAND_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; - } + case ROTWING_STATE_FW_HOV_MOT_IDLE: + // Check if state needs to be set to fixed wing with hover motors activated + if (stabilization_cmd[COMMAND_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; - } + // 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; + // 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; - } + 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; + // 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; + default: + break; } } @@ -433,7 +419,7 @@ void rotwing_switch_state(void) } else { rotwing_state_set_fw_hov_mot_off_settings(); } - break; + break; } } @@ -454,8 +440,7 @@ void rotwing_state_set_hover_settings(void) 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) - { + 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; @@ -516,8 +501,7 @@ void rotwing_state_set_fw_hov_mot_off_settings(void) void rotwing_state_set_state_settings(void) { - if (!rotwing_state_skewing.force_rotation_angle) - { + 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; @@ -552,21 +536,21 @@ void rotwing_state_set_state_settings(void) 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 = 55; - } else if (airspeed > 10) { - wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.; - } 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; + 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 = 55; + } else if (airspeed > 10) { + wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.; + } 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; + } } void rotwing_state_skew_actuator_periodic(void) @@ -580,46 +564,43 @@ void rotwing_state_skew_actuator_periodic(void) // Calulcate rotation_cmd Bound(servo_pprz_cmd, -MAX_PPRZ, MAX_PPRZ); - #if ROTWING_STATE_PD_WING_ROTATION +#if ROTWING_STATE_USE_ROTATION_REF_MODEL // Rotate with second order filter - // Smooth accerelation and rate limited setpoint - - float error_cmd = servo_pprz_cmd - rotwing_state_skew_p_cmd; - float speed_sp = ROTWING_STATE_WING_ROTATION_P_GAIN * error_cmd; - float speed_error = speed_sp - rotwing_state_skew_d_cmd; - rotwing_state_skew_d_cmd += ROTWING_STATE_WING_ROTATION_D_GAIN * speed_error; + 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 +#else // Directly controlling the wing rotation rotwing_state_skewing.servo_pprz_cmd = servo_pprz_cmd; - #endif +#endif - #if USE_NPS - actuators_pprz[INDI_NUM_ACT] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command - rotwing_state_skewing.wing_angle_deg = (float) rotwing_state_skewing.servo_pprz_cmd / MAX_PPRZ * 45. + 45.; +#if USE_NPS + actuators_pprz[INDI_NUM_ACT] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command + rotwing_state_skewing.wing_angle_deg = (float) rotwing_state_skewing.servo_pprz_cmd / MAX_PPRZ * 45. + 45.; - // 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 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); - #endif + // Send ABI message + AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); +#endif } -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) +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; irpm; - } else if (idx == SERVO_MOTOR_RIGHT_IDX) - { + } else if (idx == SERVO_MOTOR_RIGHT_IDX) { rotwing_state_hover_rpm[1] = feedback_msg->rpm; - } else if (idx == SERVO_MOTOR_BACK_IDX) - { + } else if (idx == SERVO_MOTOR_BACK_IDX) { rotwing_state_hover_rpm[2] = feedback_msg->rpm; - } else if (idx == SERVO_MOTOR_LEFT_IDX) - { + } else if (idx == SERVO_MOTOR_LEFT_IDX) { rotwing_state_hover_rpm[3] = feedback_msg->rpm; } } @@ -667,13 +643,15 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl Wu_gih[3] = pusher_priority_factor * 1.0; // adjust weights - float thrust_command = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + actuator_state_filt_vect[3]) / 4; + float thrust_command = (actuator_state_filt_vect[0] + actuator_state_filt_vect[1] + actuator_state_filt_vect[2] + + actuator_state_filt_vect[3]) / 4; Bound(thrust_command, 0, MAX_PPRZ); float fixed_wing_percentage = !hover_motors_active; // TODO: when hover props go below 40%, ... Bound(fixed_wing_percentage, 0, 1); - #define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 +#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16 - Wv_gih[0] = horizontal_accel_weight * (1.0f + fixed_wing_percentage * AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) + Wv_gih[0] = horizontal_accel_weight * (1.0f + fixed_wing_percentage * + AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight) Wv_gih[1] = horizontal_accel_weight; Wv_gih[2] = vertical_accel_weight; @@ -693,9 +671,12 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl bool_disable_hover_motors = false; } - 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]) * hover_motors_active; + 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]) * hover_motors_active; 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]); + 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 = 2.0; // big roll limit hacked in to overcome wls problems at roll limit @@ -722,7 +703,7 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl du_min_gih[0] = -roll_limit_rad - roll_angle; //roll du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch du_min_gih[2] = du_min_thrust_z; - du_min_gih[3] = (-actuator_state_filt_vect[8]*g1g2[4][8]); + du_min_gih[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]); // Set upper limits limits du_max_gih[0] = roll_limit_rad - roll_angle; //roll diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c new file mode 100644 index 0000000000..86c809c28c --- /dev/null +++ b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2022 Dennis van Wijngaarden + * + * 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/rot_wing_drone/wing_rotation_adc_sensor.c" + * @author Dennis van Wijngaarden + * Module to read skew angle from adc sensor + */ + +#include "modules/rot_wing_drone/wing_rotation_adc_sensor.h" +#include "generated/airframe.h" +#include "modules/core/abi.h" + +#include +#include "mcu_periph/adc.h" + +/*** ADC channel connected to the wing rotation potentiometer */ +#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION +#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION ADC_5 +#endif + +#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES +#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES 16 +#endif + +static struct adc_buf buf_wing_rot_pos; + +// Initialization +void wing_rotation_adc_init(void) +{ + // ADC init + adc_buf_channel(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION, &buf_wing_rot_pos, + ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES); +} + +void wing_rotation_adc_to_deg(void) +{ + float adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample; + float wing_angle_deg = 0.00247111 * adc_wing_rotation - 25.635294; + + + // 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(wing_angle_deg); + feedback.set.position = true; + + // Send ABI message + AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); +} diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h new file mode 100644 index 0000000000..736474d748 --- /dev/null +++ b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2022 Dennis van Wijngaarden + * + * 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/rot_wing_drone/wing_rotation_controller_adc_sensor.h" + * @author Dennis van Wijngaarden + * Module to read skew angle from adc sensor + */ + +#ifndef WING_ROTATION_ADC_SENSOR_H +#define WING_ROTATION_ADC_SENSOR_H + +extern void wing_rotation_adc_init(void); +extern void wing_rotation_adc_to_deg(void); + +#endif // WING_ROTATION_ADC_SENSOR_H diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c b/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c deleted file mode 100644 index 31580bb7c6..0000000000 --- a/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.c +++ /dev/null @@ -1,140 +0,0 @@ -/* - * Copyright (C) 2022 Dennis van Wijngaarden - * - * 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/rot_wing_drone/wing_rotation_controller_v3b.c" - * @author Dennis van Wijngaarden - * Module to control wing rotation servo command based on prefered angle setpoint - */ - -#include "modules/rot_wing_drone/wing_rotation_controller_servo.h" -#include "modules/rot_wing_drone/rotwing_state.h" -#include "modules/radio_control/radio_control.h" -#include "firmwares/rotorcraft/guidance/guidance_h.h" -#include "generated/airframe.h" -#include "modules/core/abi.h" -#include "state.h" - -#include -#include "mcu_periph/adc.h" - -#if USE_NPS -#include "modules/actuators/actuators.h" -#endif - - -#if !USE_NPS - -#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION -#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION ADC_5 -#endif - -#ifndef ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES -#define ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES 16 -#endif - -#endif // !USE_NPS - -#ifndef WING_ROTATION_CONTROLLER_FIRST_DYN -#define WING_ROTATION_CONTROLLER_FIRST_DYN 0.001 -#endif - -#ifndef WING_ROTATION_CONTROLLER_SECOND_DYN -#define WING_ROTATION_CONTROLLER_SECOND_DYN 0.003 -#endif - -// Parameters -struct wing_rotation_controller_t wing_rotation_controller = {0}; - -bool in_transition = false; - -#if !USE_NPS -static struct adc_buf buf_wing_rot_pos; -#endif - - -// Inline functions -inline void wing_rotation_adc_to_deg(void); -inline void wing_rotation_compute_pprz_cmd(void); - -// Initialization -void wing_rotation_init(void) -{ - // ADC init -#if !USE_NPS - adc_buf_channel(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION, &buf_wing_rot_pos, ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_NB_SAMPLES); -#endif - - // Init Data - wing_rotation_controller.wing_angle_virtual_deg_sp = 0; - wing_rotation_controller.wing_rotation_first_order_dynamics = WING_ROTATION_CONTROLLER_FIRST_DYN; - wing_rotation_controller.wing_rotation_second_order_dynamics = WING_ROTATION_CONTROLLER_SECOND_DYN; -} - -void wing_rotation_event(void) -{ - // Update Wing position sensor - wing_rotation_adc_to_deg(); - - // Control the wing rotation position. - wing_rotation_compute_pprz_cmd(); -} - -void wing_rotation_adc_to_deg(void) -{ -#if !USE_NPS - // Read ADC - wing_rotation_controller.adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample; - - wing_rotation_controller.wing_angle_deg = 0.00247111 * (float)wing_rotation_controller.adc_wing_rotation - 25.635294; -#else // !USE_NPS - // Copy setpoint as actual angle in simulation - wing_rotation_controller.wing_angle_deg = wing_rotation_controller.wing_angle_virtual_deg_sp; -#endif - - // 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; - feedback.position = 0.5 * M_PI - RadOfDeg(wing_rotation_controller.wing_angle_deg); - feedback.set.position = true; - - // Send ABI message - AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); -} - -void wing_rotation_compute_pprz_cmd(void) -{ - wing_rotation_controller.wing_angle_deg_sp = rotwing_state_skewing.wing_angle_deg_sp; - // Smooth accerelation and rate limited setpoint - float angle_error = wing_rotation_controller.wing_angle_deg_sp - wing_rotation_controller.wing_angle_virtual_deg_sp; - float speed_sp = wing_rotation_controller.wing_rotation_first_order_dynamics * angle_error; - float speed_error = speed_sp - wing_rotation_controller.wing_rotation_speed; - wing_rotation_controller.wing_rotation_speed += wing_rotation_controller.wing_rotation_second_order_dynamics * speed_error; - wing_rotation_controller.wing_angle_virtual_deg_sp += wing_rotation_controller.wing_rotation_speed; - - // Send to actuators - int32_t servo_pprz_cmd; - servo_pprz_cmd = (int32_t)(wing_rotation_controller.wing_angle_virtual_deg_sp / 90. * (float)MAX_PPRZ); - Bound(servo_pprz_cmd, 0, MAX_PPRZ); - wing_rotation_controller.servo_pprz_cmd = servo_pprz_cmd; - -#if USE_NPS - actuators_pprz[INDI_NUM_ACT] = servo_pprz_cmd; -#endif -} \ No newline at end of file diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h b/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h deleted file mode 100644 index 0cae2a338d..0000000000 --- a/sw/airborne/modules/rot_wing_drone/wing_rotation_controller_servo.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright (C) 2022 Dennis van Wijngaarden - * - * 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/rot_wing_drone/wing_rotation_controller_servo.h" - * @author Dennis van Wijngaarden - * Module to control wing rotation servo command based on prefered angle setpoint - */ - -#ifndef WING_ROTATION_CONTROLLER_SERVO_H -#define WING_ROTATION_CONTROLLER_SERVO_H - -#include "std.h" - -extern void wing_rotation_init(void); -extern void wing_rotation_event(void); - -// Paramaters -struct wing_rotation_controller_t { - float wing_angle_deg; ///< Wing angle measurement in degrees - float wing_angle_deg_sp; ///< Wing angle setpoint in degrees - - int32_t servo_pprz_cmd; ///< Servo command in pprz - uint16_t adc_wing_rotation; ///< ADC value of wing - - float wing_rotation_speed; ///< Rate limiter state variable 1 - float wing_angle_virtual_deg_sp; ///< Rate limiter state variable 2 - float wing_rotation_first_order_dynamics; ///< Rate limiter for wing rotation - float wing_rotation_second_order_dynamics; ///< Acceleration limiter for wing rotation -}; - -// Setters -#define SetWingAngleDegSp(_wing_angle_deg_sp) (wing_rotation_controller.wing_angle_deg_sp = _wing_angle_deg_sp) - -extern struct wing_rotation_controller_t wing_rotation_controller; - -#endif // WING_ROTATION_CONTROLLER_SERVO_H