diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index b49e1f8eba82..3b906be4dbce 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -53,9 +53,7 @@ uint8 component_id # subsystem / component id, contains MAVLink's component ID bool is_rotary_wing # True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter bool is_vtol # True if the system is VTOL capable -bool vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode bool in_transition_mode # True if VTOL is doing a transition -bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW bool rc_signal_lost # true if RC reception lost uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 1efece610fa4..b07ddd5d3340 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -5,8 +5,8 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 uint8 VEHICLE_VTOL_STATE_MC = 3 uint8 VEHICLE_VTOL_STATE_FW = 4 -bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode -bool vtol_in_trans_mode -bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW +uint8 vtol_state + bool vtol_transition_failsafe # vtol in transition failsafe mode + bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 58a5fd7c5187..db3034948b0a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -436,23 +436,15 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "transition")) { - struct vehicle_command_s cmd = { - .timestamp = 0, - .param5 = NAN, - .param6 = NAN, - /* transition to the other mode */ - .param1 = (float)((status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), - .param2 = NAN, - .param3 = NAN, - .param4 = NAN, - .param7 = NAN, - .command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, - .target_system = status.system_id, - .target_component = status.component_id - }; + vehicle_command_s vcmd = {}; + vcmd.timestamp = hrt_absolute_time(); + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION; + vcmd.param1 = (float)((status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + vcmd.target_system = status.system_id; + vcmd.target_component = status.component_id; - orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(h); + orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + orb_unadvertise(h); return 0; } @@ -1349,9 +1341,6 @@ Commander::run() /* Subscribe to vtol vehicle status topic */ int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); - //struct vtol_vehicle_status_s vtol_status; - memset(&vtol_status, 0, sizeof(vtol_status)); - vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing /* subscribe to estimator status topic */ int estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); @@ -1488,15 +1477,6 @@ Commander::run() status.system_type = (uint8_t)system_type; } - /* disable manual override for all systems that rely on electronic stabilization */ - if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) { - status.is_rotary_wing = true; - - } else { - status.is_rotary_wing = false; - } - - /* set vehicle_status.is_vtol flag */ status.is_vtol = is_vtol(&status); /* check and update system / component ID */ @@ -1586,7 +1566,34 @@ Commander::run() orb_check(sp_man_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + if (orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man) == PX4_OK) { + + // VTOL transition switch + if ((sp_man.transition_switch != _last_sp_man.transition_switch) && + (sp_man.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE) && + control_mode.flag_control_manual_enabled) { + + uint8_t vtol_new_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_UNDEFINED; + + if (sp_man.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + vtol_new_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; + } else if (sp_man.transition_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + vtol_new_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + } + + if (vtol_new_state != vtol_status.vtol_state) { + vehicle_command_s vcmd = {}; + vcmd.timestamp = hrt_absolute_time(); + vcmd.param1 = vtol_new_state; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION; + vcmd.target_system = status.system_id; + vcmd.target_component = status.component_id; + + orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + orb_unadvertise(h); + } + } + } } orb_check(offboard_control_mode_sub, &updated); @@ -1699,31 +1706,27 @@ Commander::run() orb_check(vtol_vehicle_status_sub, &updated); if (updated) { - /* vtol status changed */ - orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); - status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; - - /* Make sure that this is only adjusted if vehicle really is of type vtol */ - if (is_vtol(&status)) { + const uint8_t prev_vtol_state = vtol_status.vtol_state; + if (orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status) == PX4_OK) { - // Check if there has been any change while updating the flags - if (status.is_rotary_wing != vtol_status.vtol_in_rw_mode) { - status.is_rotary_wing = vtol_status.vtol_in_rw_mode; - status_changed = true; - } + status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe; - if (status.in_transition_mode != vtol_status.vtol_in_trans_mode) { - status.in_transition_mode = vtol_status.vtol_in_trans_mode; - status_changed = true; - } + status.is_rotary_wing = true; + status.in_transition_mode = false; - if (status.in_transition_to_fw != vtol_status.in_transition_to_fw) { - status.in_transition_to_fw = vtol_status.in_transition_to_fw; - status_changed = true; + switch (vtol_status.vtol_state) { + case vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW: + case vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC: + status.in_transition_mode = true; + break; + case vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW: + status.is_rotary_wing = false; + break; + default: + break; } - if (status_flags.vtol_transition_failure != vtol_status.vtol_transition_failsafe) { - status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe; + if (vtol_status.vtol_state != prev_vtol_state) { status_changed = true; } @@ -2962,6 +2965,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) && (_last_sp_man.posctl_switch == sp_man.posctl_switch) && (_last_sp_man.loiter_switch == sp_man.loiter_switch) && + (_last_sp_man.transition_switch == sp_man.transition_switch) && (_last_sp_man.mode_slot == sp_man.mode_slot) && (_last_sp_man.stab_switch == sp_man.stab_switch) && (_last_sp_man.man_switch == sp_man.man_switch)))) { @@ -3605,9 +3609,9 @@ bool stabilization_required() { return (status.is_rotary_wing || // is a rotary wing, or - status.vtol_fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or - (vtol_status.vtol_in_trans_mode && // is currently a VTOL transitioning AND - !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode + vtol_status.fw_permanent_stab || // is a VTOL in fixed wing mode and stabilisation is on, or + (status.in_transition_mode && // is currently a VTOL transitioning AND + !status.is_rotary_wing)); // is a fixed wing, ie: transitioning back to rotary wing mode } void diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 789695593484..e9555d41acdf 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -33,6 +33,8 @@ #include "FixedwingAttitudeControl.hpp" +using namespace time_literals; + /** * Fixedwing attitude control app start / stop handling function * @@ -372,19 +374,21 @@ FixedwingAttitudeControl::vehicle_status_poll() /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_rates_sp_id) { - if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_fw); - _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) { + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_fw); + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); - _parameter_handles.vtol_type = param_find("VT_TYPE"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); - parameters_update(); + parameters_update(); - } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); - _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } } } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ac6f0559176d..4958fbe7ccad 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -33,6 +33,8 @@ #include "FixedwingPositionControl.hpp" +using namespace time_literals; + extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); FixedwingPositionControl::FixedwingPositionControl() : @@ -329,16 +331,18 @@ FixedwingPositionControl::vehicle_status_poll() /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (_attitude_setpoint_id == nullptr) { - if (_vehicle_status.is_vtol) { - _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); - _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); - _parameter_handles.vtol_type = param_find("VT_TYPE"); + _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); - parameters_update(); + parameters_update(); - } else { - _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } } } } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 92b8f5fc9caf..d663181d8312 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -626,7 +626,7 @@ void Logger::add_default_topics() add_topic("vehicle_status_flags"); add_topic("vehicle_vision_attitude"); add_topic("vehicle_vision_position"); - add_topic("vtol_vehicle_status", 200); + add_topic("vtol_vehicle_status"); add_topic("wind_estimate", 200); #ifdef CONFIG_ARCH_BOARD_SITL diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c8f6c6f5bde6..c1aca216af41 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3613,6 +3613,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream private: MavlinkOrbSubscription *_status_sub; + MavlinkOrbSubscription *_vtol_status_sub; MavlinkOrbSubscription *_landed_sub; MavlinkOrbSubscription *_pos_sp_triplet_sub; MavlinkOrbSubscription *_control_mode_sub; @@ -3625,6 +3626,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream protected: explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _vtol_status_sub(_mavlink->add_orb_subscription(ORB_ID(vtol_vehicle_status))), _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _control_mode_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_control_mode))), @@ -3642,20 +3644,10 @@ class MavlinkStreamExtendedSysState : public MavlinkStream if (_status_sub->update(&status)) { updated = true; + vtol_vehicle_status_s vtol_status; - if (status.is_vtol) { - if (!status.in_transition_mode && status.is_rotary_wing) { - _msg.vtol_state = MAV_VTOL_STATE_MC; - - } else if (!status.in_transition_mode) { - _msg.vtol_state = MAV_VTOL_STATE_FW; - - } else if (status.in_transition_mode && status.in_transition_to_fw) { - _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; - - } else if (status.in_transition_mode) { - _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; - } + if (status.is_vtol && _vtol_status_sub->update(&vtol_status)) { + _msg.vtol_state = vtol_status.vtol_state; } } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 7dc71e0d5b23..6606f2ea2e93 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -61,7 +61,7 @@ #define AXIS_COUNT 3 using namespace matrix; - +using namespace time_literals; int MulticopterAttitudeControl::print_usage(const char *reason) { @@ -279,13 +279,14 @@ MulticopterAttitudeControl::vehicle_status_poll() /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (_rates_sp_id == nullptr) { - if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_mc); - - } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); + if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) { + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c03c6507cd08..2bf55b9d0285 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -80,6 +80,8 @@ #include "PositionControl.hpp" #include "Utility/ControlMath.hpp" +using namespace time_literals; + #define SIGMA_SINGLE_OP 0.000001f #define SIGMA_NORM 0.001f /** @@ -656,11 +658,13 @@ MulticopterPositionControl::poll_subscriptions() /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_attitude_setpoint_id) { - if (_vehicle_status.is_vtol) { - _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); + if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) { + if (_vehicle_status.is_vtol) { + _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); - } else { - _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } else { + _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); + } } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 20706e2cef74..907c9facfa81 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1036,7 +1036,7 @@ bool Navigator::force_vtol() { return _vstatus.is_vtol && - (!_vstatus.is_rotary_wing || _vstatus.in_transition_to_fw) + (!_vstatus.is_rotary_wing || (_vstatus.is_rotary_wing && _vstatus.in_transition_mode)) && _param_force_vtol.get(); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 61bc67def5ca..1687c64bf8d7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1567,8 +1567,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VTOL VEHICLE STATUS --- */ if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) { log_msg.msg_type = LOG_VTOL_MSG; - log_msg.body.log_VTOL.rw_mode = buf.vtol_status.vtol_in_rw_mode; - log_msg.body.log_VTOL.trans_mode = buf.vtol_status.vtol_in_trans_mode; + log_msg.body.log_VTOL.rw_mode = (buf.vtol_status.vtol_state != VEHICLE_VTOL_STATE_FW); + log_msg.body.log_VTOL.trans_mode = (buf.vtol_status.vtol_state == VEHICLE_VTOL_STATE_TRANSITION_TO_FW) || (buf.vtol_status.vtol_state == VEHICLE_VTOL_STATE_TRANSITION_TO_MC); log_msg.body.log_VTOL.failsafe_mode = buf.vtol_status.vtol_transition_failsafe; LOGBUFFER_WRITE_AND_COUNT(VTOL); } diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c2b721fe0562..a6ff60dfc106 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -47,20 +47,8 @@ #include Standard::Standard(VtolAttitudeControl *attc) : - VtolType(attc), - _pusher_throttle(0.0f), - _reverse_output(0.0f), - _airspeed_trans_blend_margin(0.0f) + VtolType(attc) { - _vtol_schedule.flight_mode = MC_MODE; - _vtol_schedule.transition_start = 0; - _pusher_active = false; - - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - _mc_yaw_weight = 1.0f; - _mc_throttle_weight = 1.0f; - _params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT"); _params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP"); _params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX"); @@ -70,10 +58,6 @@ Standard::Standard(VtolAttitudeControl *attc) : _params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL"); } -Standard::~Standard() -{ -} - void Standard::parameters_update() { @@ -117,45 +101,43 @@ void Standard::update_vtol_state() * For the back transition the pusher motor is immediately stopped and rotors reactivated. */ + VtolType::update_vtol_state(); + float mc_weight = _mc_roll_weight; - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off - if (_vtol_schedule.flight_mode == MC_MODE) { + if (_flight_mode == MC_MODE) { // in mc mode - _vtol_schedule.flight_mode = MC_MODE; mc_weight = 1.0f; _pusher_throttle = 0.0f; _reverse_output = 0.0f; - } else if (_vtol_schedule.flight_mode == FW_MODE) { + } else if (_flight_mode == FW_MODE) { // transition to mc mode - if (_vtol_vehicle_status->vtol_transition_failsafe == true) { + if (_vtol_vehicle_status.vtol_transition_failsafe) { // Failsafe event, engage mc motors immediately - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; _pusher_throttle = 0.0f; _reverse_output = 0.0f; - } else { // Regular backtransition - _vtol_schedule.flight_mode = TRANSITION_TO_MC; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_TO_MC; + _transition_start_time = hrt_absolute_time(); _reverse_output = _params_standard.reverse_output; - } - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + } else if (_flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; mc_weight = 1.0f; _pusher_throttle = 0.0f; _reverse_output = 0.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + } else if (_flight_mode == TRANSITION_TO_MC) { // transition to MC mode if transition time has passed or forward velocity drops below MPC cruise speed const matrix::Dcmf R_to_body(matrix::Quatf(_v_att->q).inversed()); @@ -163,40 +145,39 @@ void Standard::update_vtol_state() float x_vel = vel(0); - if (time_since_trans_start > _params->back_trans_duration || + if (get_time_since_trans_start() > _params->back_trans_duration || (_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) { - _vtol_schedule.flight_mode = MC_MODE; - } + _flight_mode = MC_MODE; + } } } else { // the transition to fw mode switch is on - if (_vtol_schedule.flight_mode == MC_MODE || _vtol_schedule.flight_mode == TRANSITION_TO_MC) { + if (_flight_mode == MC_MODE || _flight_mode == TRANSITION_TO_MC) { // start transition to fw mode /* NOTE: The failsafe transition to fixed-wing was removed because it can result in an * unsafe flying state. */ - _vtol_schedule.flight_mode = TRANSITION_TO_FW; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_TO_FW; + _transition_start_time = hrt_absolute_time(); - } else if (_vtol_schedule.flight_mode == FW_MODE) { + } else if (_flight_mode == FW_MODE) { // in fw mode - _vtol_schedule.flight_mode = FW_MODE; + _flight_mode = FW_MODE; mc_weight = 0.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + } else if (_flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode if (((_params->airspeed_disabled || _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) && - time_since_trans_start > _params->front_trans_time_min) || + get_time_since_trans_start() > _params->front_trans_time_min) || can_transition_on_ground()) { - _vtol_schedule.flight_mode = FW_MODE; + _flight_mode = FW_MODE; // don't set pusher throttle here as it's being ramped up elsewhere _trans_finished_ts = hrt_absolute_time(); } - } } @@ -206,7 +187,7 @@ void Standard::update_vtol_state() _mc_throttle_weight = mc_weight; // map specific control phases to simple control modes - switch (_vtol_schedule.flight_mode) { + switch (_flight_mode) { case MC_MODE: _vtol_mode = mode::ROTARY_WING; break; @@ -228,33 +209,32 @@ void Standard::update_vtol_state() void Standard::update_transition_state() { float mc_weight = 1.0f; - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; VtolType::update_transition_state(); // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { + if (_flight_mode == TRANSITION_TO_FW) { if (_params_standard.pusher_ramp_dt <= 0.0f) { // just set the final target throttle value _pusher_throttle = _params->front_trans_throttle; } else if (_pusher_throttle <= _params->front_trans_throttle) { // ramp up throttle to the target throttle value - _pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt; + _pusher_throttle = _params->front_trans_throttle * get_time_since_trans_start() / _params_standard.pusher_ramp_dt; } // do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed if (_airspeed_trans_blend_margin > 0.0f && _airspeed->indicated_airspeed_m_s >= _params->airspeed_blend && - time_since_trans_start > _params->front_trans_time_min) { + get_time_since_trans_start() > _params->front_trans_time_min) { mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) / _airspeed_trans_blend_margin; // time based blending when no airspeed sensor is set } else if (_params->airspeed_disabled) { - mc_weight = 1.0f - time_since_trans_start / _params->front_trans_time_min; + mc_weight = 1.0f - get_time_since_trans_start() / _params->front_trans_time_min; mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f); } @@ -265,15 +245,9 @@ void Standard::update_transition_state() q_sp.copyTo(_v_att_sp->q_d); _v_att_sp->q_d_valid = true; - // check front transition timeout - if (_params->front_trans_timeout > FLT_EPSILON) { - if (time_since_trans_start > _params->front_trans_timeout) { - // transition timeout occured, abort transition - _attc->abort_front_transition("Transition timeout"); - } - } - } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { + + } else if (_flight_mode == TRANSITION_TO_MC) { // maintain FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset; @@ -283,16 +257,16 @@ void Standard::update_transition_state() _pusher_throttle = 0.0f; - if (time_since_trans_start >= _params_standard.reverse_delay) { + if (get_time_since_trans_start() >= _params_standard.reverse_delay) { // Handle throttle reversal for active breaking - float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt); + float thrscale = (get_time_since_trans_start() - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt); thrscale = math::constrain(thrscale, 0.0f, 1.0f); _pusher_throttle = thrscale * _params->back_trans_throttle; } // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_ramp > FLT_EPSILON) { - mc_weight = time_since_trans_start / _params_standard.back_trans_ramp; + mc_weight = get_time_since_trans_start() / _params_standard.back_trans_ramp; } @@ -323,7 +297,7 @@ void Standard::update_mc_state() // Do not engage pusher assist during a failsafe event // There could be a problem with the fixed wing drive - if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) { + if (_attc->get_vtol_vehicle_status().vtol_transition_failsafe) { return; } @@ -386,11 +360,6 @@ void Standard::update_mc_state() } -void Standard::update_fw_state() -{ - VtolType::update_fw_state(); -} - /** * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine * what proportion of control should be applied to each of the control groups (mc and fw). @@ -398,7 +367,7 @@ void Standard::update_fw_state() void Standard::fill_actuator_outputs() { // multirotor controls - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; // roll _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = @@ -415,9 +384,9 @@ void Standard::fill_actuator_outputs() // fixed wing controls - _actuators_out_1->timestamp = _actuators_fw_in->timestamp; + _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - if (_vtol_schedule.flight_mode != MC_MODE) { + if (_flight_mode != MC_MODE) { // roll _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; @@ -455,7 +424,7 @@ void Standard::fill_actuator_outputs() } // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_flight_mode == FW_MODE) { // take the throttle value commanded by the fw controller _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 30ffdfc1efaa..bd2ad42b5d7e 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -55,11 +55,10 @@ class Standard : public VtolType public: Standard(VtolAttitudeControl *_att_controller); - ~Standard(); + ~Standard() = default; virtual void update_vtol_state(); virtual void update_transition_state(); - virtual void update_fw_state(); virtual void update_mc_state(); virtual void fill_actuator_outputs(); virtual void waiting_on_tecs(); @@ -93,14 +92,13 @@ class Standard : public VtolType FW_MODE }; - struct { - vtol_mode flight_mode; // indicates in which mode the vehicle is in - hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) - } _vtol_schedule; - float _pusher_throttle; - float _reverse_output; - float _airspeed_trans_blend_margin; + vtol_mode _flight_mode = MC_MODE; /**< vtol flight mode, defined by enum vtol_mode */ + + + float _pusher_throttle = 0.0f; + float _reverse_output = 0.0f; + float _airspeed_trans_blend_margin = 0.0f; virtual void parameters_update(); }; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 73fee6f092dd..d141176acae9 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -48,24 +48,7 @@ #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC Tailsitter::Tailsitter(VtolAttitudeControl *attc) : - VtolType(attc), - _thrust_transition_start(0.0f), - _yaw_transition(0.0f), - _pitch_transition_start(0.0f) -{ - _vtol_schedule.flight_mode = MC_MODE; - _vtol_schedule.transition_start = 0; - - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - _mc_yaw_weight = 1.0f; - - _flag_was_in_trans_mode = false; - - _params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); -} - -Tailsitter::~Tailsitter() + VtolType(attc) { } @@ -73,46 +56,43 @@ Tailsitter::~Tailsitter() void Tailsitter::parameters_update() { - float v; - /* vtol front transition phase 2 duration */ - param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); - _params_tailsitter.front_trans_dur_p2 = v; } void Tailsitter::update_vtol_state() { - /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting in MC control mode, picking up * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ + VtolType::update_vtol_state(); + matrix::Eulerf euler = matrix::Quatf(_v_att->q); float pitch = euler.theta(); if (!_attc->is_fixed_wing_requested()) { - switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + switch (_flight_mode) { // user switchig to MC mode case MC_MODE: break; case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_BACK; + _transition_start_time = hrt_absolute_time(); break; case TRANSITION_FRONT_P1: // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; break; case TRANSITION_BACK: // check if we have reached pitch angle to switch to MC mode if (pitch >= PITCH_TRANSITION_BACK) { - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; } break; @@ -120,11 +100,11 @@ void Tailsitter::update_vtol_state() } else { // user switchig to FW mode - switch (_vtol_schedule.flight_mode) { + switch (_flight_mode) { case MC_MODE: // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_FRONT_P1; + _transition_start_time = hrt_absolute_time(); break; case FW_MODE: @@ -137,7 +117,7 @@ void Tailsitter::update_vtol_state() // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode if ((airspeed_condition_satisfied && pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) { - _vtol_schedule.flight_mode = FW_MODE; + _flight_mode = FW_MODE; } break; @@ -145,40 +125,36 @@ void Tailsitter::update_vtol_state() case TRANSITION_BACK: // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; + _flight_mode = FW_MODE; break; } } // map tailsitter specific control phases to simple control modes - switch (_vtol_schedule.flight_mode) { + switch (_flight_mode) { case MC_MODE: _vtol_mode = ROTARY_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; case FW_MODE: _vtol_mode = FIXED_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; _flag_was_in_trans_mode = false; break; case TRANSITION_FRONT_P1: _vtol_mode = TRANSITION_TO_FW; - _vtol_vehicle_status->vtol_in_trans_mode = true; break; case TRANSITION_BACK: _vtol_mode = TRANSITION_TO_MC; - _vtol_vehicle_status->vtol_in_trans_mode = true; break; } } void Tailsitter::update_transition_state() { - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; + VtolType::update_transition_state(); if (!_flag_was_in_trans_mode) { // save desired heading for transition and last thrust value @@ -190,11 +166,11 @@ void Tailsitter::update_transition_state() _flag_was_in_trans_mode = true; } - if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + if (_flight_mode == TRANSITION_FRONT_P1) { // create time dependant pitch angle set point + 0.2 rad overlap over the switch value _v_att_sp->pitch_body = _pitch_transition_start - fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * - time_since_trans_start / _params->front_trans_duration; + get_time_since_trans_start() / _params->front_trans_duration; _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, PITCH_TRANSITION_FRONT_P1 - 0.2f, _pitch_transition_start); @@ -209,7 +185,7 @@ void Tailsitter::update_transition_state() _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + } else if (_flight_mode == TRANSITION_BACK) { if (!flag_idle_mc) { flag_idle_mc = set_idle_mc(); @@ -217,14 +193,14 @@ void Tailsitter::update_transition_state() // create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * - time_since_trans_start / _params->back_trans_duration; + get_time_since_trans_start() / _params->back_trans_duration; _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body, -2.0f, PITCH_TRANSITION_BACK + 0.2f); // keep yaw disabled _mc_yaw_weight = 0.0f; // smoothly move control weight to MC - _mc_roll_weight = _mc_pitch_weight = time_since_trans_start / _params->back_trans_duration; + _mc_roll_weight = _mc_pitch_weight = get_time_since_trans_start() / _params->back_trans_duration; } @@ -240,8 +216,6 @@ void Tailsitter::update_transition_state() _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); // compute desired attitude and thrust setpoint for the transition - - _v_att_sp->timestamp = hrt_absolute_time(); _v_att_sp->roll_body = 0.0f; _v_att_sp->yaw_body = _yaw_transition; @@ -256,24 +230,16 @@ void Tailsitter::waiting_on_tecs() _v_att_sp->thrust = _thrust_transition; } -void Tailsitter::update_mc_state() -{ - VtolType::update_mc_state(); -} - -void Tailsitter::update_fw_state() -{ - VtolType::update_fw_state(); -} - /** * Write data to actuator output topic. */ void Tailsitter::fill_actuator_outputs() { + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + switch (_vtol_mode) { case ROTARY_WING: - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; @@ -281,8 +247,6 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - _actuators_out_1->timestamp = _actuators_mc_in->timestamp; - if (_params->elevons_mc_lock) { _actuators_out_1->control[0] = 0; _actuators_out_1->control[1] = 0; @@ -299,7 +263,7 @@ void Tailsitter::fill_actuator_outputs() case FIXED_WING: // in fixed wing mode we use engines only for providing thrust, no moments are generated - _actuators_out_0->timestamp = _actuators_fw_in->timestamp; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; @@ -319,8 +283,7 @@ void Tailsitter::fill_actuator_outputs() case TRANSITION_TO_FW: case TRANSITION_TO_MC: // in transition engines are mixed by weight (BACK TRANSITION ONLY) - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; - _actuators_out_1->timestamp = _actuators_mc_in->timestamp; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 1b2b47d0ce82..0b2ecdb58868 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -43,7 +43,7 @@ #define TAILSITTER_H #include "vtol_type.h" -#include /** is it necsacery? **/ +#include #include #include @@ -52,25 +52,15 @@ class Tailsitter : public VtolType public: Tailsitter(VtolAttitudeControl *_att_controller); - ~Tailsitter(); + ~Tailsitter() = default; virtual void update_vtol_state(); virtual void update_transition_state(); - virtual void update_mc_state(); - virtual void update_fw_state(); virtual void fill_actuator_outputs(); virtual void waiting_on_tecs(); private: - struct { - float front_trans_dur_p2; - } _params_tailsitter; - - struct { - param_t front_trans_dur_p2; - } _params_handles_tailsitter; - enum vtol_mode { MC_MODE = 0, /**< vtol is in multicopter mode */ TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */ @@ -78,14 +68,11 @@ class Tailsitter : public VtolType FW_MODE /**< vtol is in fixed wing mode */ }; - struct { - vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ - hrt_abstime transition_start; /**< absoulte time at which front transition started */ - } _vtol_schedule; + vtol_mode _flight_mode = MC_MODE; /**< vtol flight mode, defined by enum vtol_mode */ - float _thrust_transition_start; // throttle value when we start the front transition - float _yaw_transition; // yaw angle in which transition will take place - float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + float _thrust_transition_start = 0.0f; // throttle value when we start the front transition + float _yaw_transition = 0.0f; // yaw angle in which transition will take place + float _pitch_transition_start = 0.0f; // pitch angle at the start of transition (tailsitter) /** * Update parameters. diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index 34b17b0025bd..f0b9aadc2a21 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -37,19 +37,4 @@ * * @author Roman Bapst * @author David Vorsin - */ - -/** - * Duration of front transition phase 2 - * - * Time in seconds it should take for the rotors to rotate forward completely from the point - * when the plane has picked up enough airspeed and is ready to go into fixed wind mode. - * - * @unit s - * @min 0.1 - * @max 5.0 - * @increment 0.01 - * @decimal 3 - * @group VTOL Attitude Control - -PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ + */ \ No newline at end of file diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 17dbb0c359cc..465140b080ae 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -45,18 +45,8 @@ #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : - VtolType(attc), - _tilt_control(0.0f) + VtolType(attc) { - _vtol_schedule.flight_mode = MC_MODE; - _vtol_schedule.transition_start = 0; - - _mc_roll_weight = 1.0f; - _mc_pitch_weight = 1.0f; - _mc_yaw_weight = 1.0f; - - _flag_was_in_trans_mode = false; - _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); @@ -65,11 +55,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC"); } -Tiltrotor::~Tiltrotor() -{ - -} - void Tiltrotor::parameters_update() { @@ -106,31 +91,33 @@ void Tiltrotor::update_vtol_state() * forward completely. For the backtransition the motors simply rotate back. */ + VtolType::update_vtol_state(); + if (!_attc->is_fixed_wing_requested()) { // plane is in multicopter mode - switch (_vtol_schedule.flight_mode) { + switch (_flight_mode) { case MC_MODE: break; case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_BACK; + _transition_start_time = hrt_absolute_time(); break; case TRANSITION_FRONT_P1: // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; break; case TRANSITION_FRONT_P2: // failsafe into multicopter mode - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; break; case TRANSITION_BACK: if (_tilt_control <= _params_tiltrotor.tilt_mc) { - _vtol_schedule.flight_mode = MC_MODE; + _flight_mode = MC_MODE; } break; @@ -138,11 +125,11 @@ void Tiltrotor::update_vtol_state() } else { - switch (_vtol_schedule.flight_mode) { + switch (_flight_mode) { case MC_MODE: // initialise a front transition - _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_FRONT_P1; + _transition_start_time = hrt_absolute_time(); break; case FW_MODE: @@ -152,21 +139,19 @@ void Tiltrotor::update_vtol_state() // allow switch if we are not armed for the sake of bench testing bool transition_to_p2 = can_transition_on_ground(); - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; - // check if we have reached airspeed to switch to fw mode transition_to_p2 |= !_params->airspeed_disabled && _airspeed->indicated_airspeed_m_s >= _params->transition_airspeed && - time_since_trans_start > _params->front_trans_time_min; + get_time_since_trans_start() > _params->front_trans_time_min; // check if airspeed is invalid and transition by time transition_to_p2 |= _params->airspeed_disabled && _tilt_control > _params_tiltrotor.tilt_transition && - time_since_trans_start > _params->front_trans_time_openloop; + get_time_since_trans_start() > _params->front_trans_time_openloop; if (transition_to_p2) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - _vtol_schedule.transition_start = hrt_absolute_time(); + _flight_mode = TRANSITION_FRONT_P2; + _transition_start_time = hrt_absolute_time(); } break; @@ -176,7 +161,7 @@ void Tiltrotor::update_vtol_state() // if the rotors have been tilted completely we switch to fw mode if (_tilt_control >= _params_tiltrotor.tilt_fw) { - _vtol_schedule.flight_mode = FW_MODE; + _flight_mode = FW_MODE; _tilt_control = _params_tiltrotor.tilt_fw; } @@ -184,13 +169,13 @@ void Tiltrotor::update_vtol_state() case TRANSITION_BACK: // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_MODE; + _flight_mode = FW_MODE; break; } } // map tiltrotor specific control phases to simple control modes - switch (_vtol_schedule.flight_mode) { + switch (_flight_mode) { case MC_MODE: _vtol_mode = ROTARY_WING; break; @@ -230,14 +215,12 @@ void Tiltrotor::update_transition_state() { VtolType::update_transition_state(); - float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; - if (!_flag_was_in_trans_mode) { // save desired heading for transition and last thrust value _flag_was_in_trans_mode = true; } - if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + if (_flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_motor_state != ENABLED) { _motor_state = set_motor_state(_motor_state, ENABLED); @@ -246,7 +229,7 @@ void Tiltrotor::update_transition_state() // tilt rotors forward up to certain angle if (_tilt_control <= _params_tiltrotor.tilt_transition) { _tilt_control = _params_tiltrotor.tilt_mc + - fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * time_since_trans_start / + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * get_time_since_trans_start() / _params->front_trans_duration; } @@ -267,25 +250,25 @@ void Tiltrotor::update_transition_state() // without airspeed do timed weight changes if (_params->airspeed_disabled - && time_since_trans_start > _params->front_trans_time_min) { - _mc_roll_weight = 1.0f - (time_since_trans_start - _params->front_trans_time_min) / + && get_time_since_trans_start() > _params->front_trans_time_min) { + _mc_roll_weight = 1.0f - (get_time_since_trans_start() - _params->front_trans_time_min) / (_params->front_trans_time_openloop - _params->front_trans_time_min); _mc_yaw_weight = _mc_roll_weight; } _thrust_transition = _mc_virtual_att_sp->thrust; - } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + } else if (_flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start / + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * get_time_since_trans_start() / _params_tiltrotor.front_trans_dur_p2; _mc_roll_weight = 0.0f; _mc_yaw_weight = 0.0f; // ramp down rear motors (setting MAX_PWM down scales the given output into the new range) - int rear_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) * + int rear_value = (1.0f - get_time_since_trans_start() / _params_tiltrotor.front_trans_dur_p2) * (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN; @@ -294,7 +277,7 @@ void Tiltrotor::update_transition_state() _thrust_transition = _mc_virtual_att_sp->thrust; - } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + } else if (_flight_mode == TRANSITION_BACK) { if (_motor_state != ENABLED) { _motor_state = set_motor_state(_motor_state, ENABLED); } @@ -308,7 +291,8 @@ void Tiltrotor::update_transition_state() // tilt rotors back if (_tilt_control > _params_tiltrotor.tilt_mc) { _tilt_control = _params_tiltrotor.tilt_fw - - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * time_since_trans_start / _params->back_trans_duration; + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * get_time_since_trans_start() / + _params->back_trans_duration; } // set zero throttle for backtransition otherwise unwanted moments will be created @@ -336,7 +320,8 @@ void Tiltrotor::waiting_on_tecs() */ void Tiltrotor::fill_actuator_outputs() { - _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = @@ -344,7 +329,7 @@ void Tiltrotor::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_flight_mode == FW_MODE) { _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; @@ -359,7 +344,9 @@ void Tiltrotor::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; } - _actuators_out_1->timestamp = _actuators_fw_in->timestamp; + + _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 53a8f2765987..61f2db79325b 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -50,7 +50,7 @@ class Tiltrotor : public VtolType public: Tiltrotor(VtolAttitudeControl *_att_controller); - ~Tiltrotor(); + ~Tiltrotor() = default; virtual void update_vtol_state(); virtual void update_transition_state(); @@ -87,19 +87,10 @@ class Tiltrotor : public VtolType FW_MODE /**< vtol is in fixed wing mode */ }; - /** - * Specific to tiltrotor with vertical aligned rear engine/s. - * These engines need to be shut down in fw mode. During the back-transition - * they need to idle otherwise they need too much time to spin up for mc mode. - */ + vtol_mode _flight_mode = MC_MODE; /**< vtol flight mode, defined by enum vtol_mode */ - struct { - vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ - hrt_abstime transition_start; /**< absoulte time at which front transition started */ - } _vtol_schedule; - - float _tilt_control; /**< actuator value for the tilt servo */ + float _tilt_control{0.0f}; /**< actuator value for the tilt servo */ /** * Update parameters. diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index f6e2c230a485..a728a8f5b684 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -49,6 +49,8 @@ #include "vtol_att_control_main.h" #include +using namespace time_literals; + namespace VTOL_att_control { VtolAttitudeControl *g_control; @@ -59,11 +61,13 @@ VtolAttitudeControl *g_control; */ VtolAttitudeControl::VtolAttitudeControl() { - _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ + _vtol_vehicle_status.vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; _params.idle_pwm_mc = PWM_DEFAULT_MIN; _params.vtol_motor_count = 0; + _params.vtol_type = -1; + // parameter handles _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); @@ -87,13 +91,11 @@ VtolAttitudeControl::VtolAttitudeControl() _params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE"); _params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID"); - _params_handles.wv_takeoff = param_find("VT_WV_TKO_EN"); _params_handles.wv_land = param_find("VT_WV_LND_EN"); _params_handles.wv_loiter = param_find("VT_WV_LTR_EN"); - /* fetch initial parameter values */ - parameters_update(); + parameters_update(); // initialize parameter cache if (_params.vtol_type == vtol_type::TAILSITTER) { _vtol_type = new Tailsitter(this); @@ -105,40 +107,57 @@ VtolAttitudeControl::VtolAttitudeControl() _vtol_type = new Standard(this); } else { - _task_should_exit = true; + request_stop(); } -} -/** -* Destructor -*/ -VtolAttitudeControl::~VtolAttitudeControl() -{ - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; + if (_vtol_type == nullptr || !_vtol_type->init()) { + request_stop(); - /* wait for a second for the task to quit at our request */ - unsigned i = 0; + } - do { - /* wait 20ms */ - usleep(20000); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - /* if we have given up, kill it */ - if (++i > 50) { - px4_task_delete(_control_task); - break; - } - } while (_control_task != -1); - } + // MC virtual subscriptions + _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); + _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); + _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); - // free memory used by instances of base class VtolType - if (_vtol_type != nullptr) { - delete _vtol_type; - } + // FW virtual subscriptions + _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); + _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); + _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); - VTOL_att_control::g_control = nullptr; +} + +VtolAttitudeControl::~VtolAttitudeControl() +{ + orb_unsubscribe(_actuator_inputs_fw); + orb_unsubscribe(_actuator_inputs_mc); + orb_unsubscribe(_airspeed_sub); + orb_unsubscribe(_fw_virtual_att_sp_sub); + orb_unsubscribe(_fw_virtual_v_rates_sp_sub); + orb_unsubscribe(_land_detected_sub); + orb_unsubscribe(_local_pos_sp_sub); + orb_unsubscribe(_local_pos_sub); + orb_unsubscribe(_mc_virtual_att_sp_sub); + orb_unsubscribe(_mc_virtual_v_rates_sp_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_pos_sp_triplet_sub); + orb_unsubscribe(_tecs_status_sub); + orb_unsubscribe(_v_att_sp_sub); + orb_unsubscribe(_v_att_sub); + orb_unsubscribe(_v_control_mode_sub); + orb_unsubscribe(_vehicle_cmd_sub); } /** @@ -156,21 +175,6 @@ void VtolAttitudeControl::vehicle_control_mode_poll() } } -/** -* Check for changes in manual inputs. -*/ -void VtolAttitudeControl::vehicle_manual_poll() -{ - bool updated; - - /* get pilots inputs */ - orb_check(_manual_control_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); - } -} - /** * Check for inputs from mc attitude controller. */ @@ -359,6 +363,12 @@ VtolAttitudeControl::handle_command() if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { _transition_command = int(_vehicle_cmd.param1 + 0.5f); + if (_vtol_vehicle_status.vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW) { + if (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC) { + abort_front_transition("manually"); + } + } + // Report that we have received the command no matter what we actually do with it. // This might not be optimal but is better than no response at all. @@ -393,25 +403,16 @@ VtolAttitudeControl::handle_command() bool VtolAttitudeControl::is_fixed_wing_requested() { - bool to_fw = false; - - if (_manual_control_sp.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE && - _v_control_mode.flag_control_manual_enabled) { - to_fw = (_manual_control_sp.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON); - - } else { - // listen to transition commands if not in manual or mode switch is not mapped - to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - } + // listen to transition commands if not in manual or mode switch is not mapped + bool to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); // handle abort request - if (_abort_front_transition) { - if (to_fw) { - to_fw = false; + if (_vtol_vehicle_status.vtol_transition_failsafe) { + to_fw = false; + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - } else { - // the state changed to mc mode, reset the abort request - _abort_front_transition = false; + if (_vtol_vehicle_status.vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC) { + // the state changed to MC mode, reset the abort request _vtol_vehicle_status.vtol_transition_failsafe = false; } } @@ -425,9 +426,8 @@ VtolAttitudeControl::is_fixed_wing_requested() void VtolAttitudeControl::abort_front_transition(const char *reason) { - if (!_abort_front_transition) { + if (!_vtol_vehicle_status.vtol_transition_failsafe) { mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason); - _abort_front_transition = true; _vtol_vehicle_status.vtol_transition_failsafe = true; } } @@ -572,47 +572,14 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() } } -void -VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) +void VtolAttitudeControl::run() { - VTOL_att_control::g_control->task_main(); -} - -void VtolAttitudeControl::task_main() -{ - fflush(stdout); - - /* do subscriptions */ - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint)); - _fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint)); - _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); - _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - _vehicle_cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - _tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); - _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); - _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); - - parameters_update(); // initialize parameter cache - - _task_should_exit = !_vtol_type->init(); - /* wakeup source*/ px4_pollfd_struct_t fds[1] = {}; fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; - while (!_task_should_exit) { + while (!should_exit()) { /* only update parameters if they changed */ bool params_updated = false; orb_check(_params_sub, ¶ms_updated); @@ -642,7 +609,6 @@ void VtolAttitudeControl::task_main() /* wait for up to 100ms for data */ int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); - /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } @@ -656,7 +622,6 @@ void VtolAttitudeControl::task_main() } vehicle_control_mode_poll(); - vehicle_manual_poll(); vehicle_attitude_poll(); vehicle_local_pos_poll(); vehicle_local_pos_sp_poll(); @@ -671,181 +636,140 @@ void VtolAttitudeControl::task_main() // update the vtol state machine which decides which mode we are in _vtol_type->update_vtol_state(); - // reset transition command if not auto control - if (_v_control_mode.flag_control_manual_enabled) { - if (_vtol_type->get_mode() == ROTARY_WING) { - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - - } else if (_vtol_type->get_mode() == FIXED_WING) { - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - - } else if (_vtol_type->get_mode() == TRANSITION_TO_MC) { - /* We want to make sure that a mode change (manual>auto) during the back transition - * doesn't result in an unsafe state. This prevents the instant fall back to - * fixed-wing on the switch from manual to auto */ - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - } - } - // check in which mode we are in and call mode specific functions if (_vtol_type->get_mode() == ROTARY_WING) { - mc_virtual_att_sp_poll(); - - // vehicle is in rotary wing mode - _vtol_vehicle_status.vtol_in_rw_mode = true; - _vtol_vehicle_status.vtol_in_trans_mode = false; - _vtol_vehicle_status.in_transition_to_fw = false; - - // got data from mc attitude controller _vtol_type->update_mc_state(); fill_mc_att_rates_sp(); } else if (_vtol_type->get_mode() == FIXED_WING) { - fw_virtual_att_sp_poll(); - - // vehicle is in fw mode - _vtol_vehicle_status.vtol_in_rw_mode = false; - _vtol_vehicle_status.vtol_in_trans_mode = false; - _vtol_vehicle_status.in_transition_to_fw = false; - _vtol_type->update_fw_state(); fill_fw_att_rates_sp(); } else if (_vtol_type->get_mode() == TRANSITION_TO_MC || _vtol_type->get_mode() == TRANSITION_TO_FW) { - mc_virtual_att_sp_poll(); fw_virtual_att_sp_poll(); - - // vehicle is doing a transition - _vtol_vehicle_status.vtol_in_trans_mode = true; - _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition - _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW); - _vtol_type->update_transition_state(); fill_mc_att_rates_sp(); } _vtol_type->fill_actuator_outputs(); - /* Only publish if the proper mode(s) are enabled */ - if (_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) { + // attitude setpoint publish + _v_att_sp.timestamp = hrt_absolute_time(); - if (_v_att_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + if (_v_att_sp_pub != nullptr) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); - } else { - /* advertise and publish */ - _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } + } else { + _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); + } - if (_actuators_0_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + // actuators 0 publish + _actuators_out_0.timestamp = hrt_absolute_time(); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } + if (_actuators_0_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - if (_actuators_1_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); } - /*Advertise/Publish vtol vehicle status*/ - _vtol_vehicle_status.timestamp = hrt_absolute_time(); + // actuators 1 publish + _actuators_out_1.timestamp = hrt_absolute_time(); - if (_vtol_vehicle_status_pub != nullptr) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); + if (_actuators_1_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); } else { - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + + // vtol vehicle status publish + bool publish_status = false; + + if (hrt_elapsed_time(&_vtol_vehicle_status.timestamp) > 1_s) { + publish_status = true; + } + + if (_vtol_vehicle_status.vtol_state != _vtol_type->get_mode()) { + publish_status = true; + } + + if (publish_status) { + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status.vtol_state = _vtol_type->get_mode(); + + if (_vtol_vehicle_status_pub != nullptr) { + orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); + + } else { + _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); + } } } +} - PX4_WARN("exit"); - _control_task = -1; +VtolAttitudeControl *VtolAttitudeControl::instantiate(int argc, char *argv[]) +{ + return new VtolAttitudeControl(); } -int -VtolAttitudeControl::start() +int VtolAttitudeControl::task_spawn(int argc, char *argv[]) { - /* start the task */ - _control_task = px4_task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_ATTITUDE_CONTROL + 1, - 1230, - (px4_main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - PX4_WARN("task start failed"); + _task_id = px4_task_spawn_cmd("vtol_att_controol", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 1300, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; return -errno; } - return OK; + return 0; } +int VtolAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} -int vtol_att_control_main(int argc, char *argv[]) +int VtolAttitudeControl::print_usage(const char *reason) { - if (argc < 2) { - PX4_WARN("usage: vtol_att_control {start|stop|status}"); - return 1; + if (reason) { + PX4_WARN("%s\n", reason); } - if (!strcmp(argv[1], "start")) { - - if (VTOL_att_control::g_control != nullptr) { - PX4_WARN("already running"); - return 0; - } - - VTOL_att_control::g_control = new VtolAttitudeControl; + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +vtol_att_control is the VTOL attitude controller. - if (VTOL_att_control::g_control == nullptr) { - PX4_WARN("alloc failed"); - return 1; - } +)DESCR_STR"); - if (OK != VTOL_att_control::g_control->start()) { - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - PX4_WARN("start failed"); - return 1; - } + PRINT_MODULE_USAGE_COMMAND("start"); - return 0; - } + PRINT_MODULE_USAGE_NAME("vtol_att_control", "controller"); - if (!strcmp(argv[1], "stop")) { - if (VTOL_att_control::g_control == nullptr) { - PX4_WARN("not running"); - return 0; - } + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - return 0; - } + return 0; +} - if (!strcmp(argv[1], "status")) { - if (VTOL_att_control::g_control) { - PX4_WARN("running"); +int VtolAttitudeControl::print_status() +{ + PX4_INFO("Running"); - } else { - PX4_WARN("not running"); - } + print_message(get_vtol_vehicle_status()); - return 0; - } + return 0; +} - PX4_WARN("unrecognized command"); - return 1; +int vtol_att_control_main(int argc, char *argv[]) +{ + return VtolAttitudeControl::main(argc, argv); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 61409be88cdb..f55794039f34 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -49,22 +49,20 @@ #ifndef VTOL_ATT_CONTROL_MAIN_H #define VTOL_ATT_CONTROL_MAIN_H -#include -#include -#include -#include - -#include #include #include #include #include +#include +#include +#include +#include +#include #include - #include #include -#include #include +#include #include #include #include @@ -74,7 +72,6 @@ #include #include #include -#include #include #include @@ -86,14 +83,31 @@ extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); -class VtolAttitudeControl +class VtolAttitudeControl : public ModuleBase { public: VtolAttitudeControl(); - ~VtolAttitudeControl(); + ~VtolAttitudeControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static VtolAttitudeControl *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; - int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); void abort_front_transition(const char *reason); @@ -112,17 +126,14 @@ class VtolAttitudeControl struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} - struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} - struct Params *get_params() {return &_params;} + const vtol_vehicle_status_s &get_vtol_vehicle_status() { return _vtol_vehicle_status; } + struct Params *get_params() {return &_params;} private: -//******************flags & handlers****************************************************** - bool _task_should_exit{false}; - int _control_task{-1}; //task handle for VTOL attitude controller - /* handlers for subscriptions */ + // handlers for subscriptions int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs int _airspeed_sub{-1}; // airspeed subscription @@ -131,7 +142,6 @@ class VtolAttitudeControl int _land_detected_sub{-1}; int _local_pos_sp_sub{-1}; // setpoint subscription int _local_pos_sub{-1}; // sensor subscription - int _manual_control_sp_sub{-1}; //manual control setpoint subscription int _mc_virtual_att_sp_sub{-1}; int _mc_virtual_v_rates_sp_sub{-1}; //vehicle rates setpoint subscription int _params_sub{-1}; //parameter updates subscription @@ -142,7 +152,7 @@ class VtolAttitudeControl int _v_control_mode_sub{-1}; //vehicle control mode subscription int _vehicle_cmd_sub{-1}; - //handlers for publishers + // handlers for publishers orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle orb_advert_t _v_att_sp_pub{nullptr}; @@ -151,7 +161,7 @@ class VtolAttitudeControl orb_advert_t _vtol_vehicle_status_pub{nullptr}; orb_advert_t _actuators_1_pub{nullptr}; -//*******************data containers*********************************************************** + //*******************data containers*********************************************************** vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint @@ -163,7 +173,6 @@ class VtolAttitudeControl actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons) airspeed_s _airspeed{}; // airspeed - manual_control_setpoint_s _manual_control_sp{}; //manual control setpoint position_setpoint_triplet_s _pos_sp_triplet{}; tecs_status_s _tecs_status{}; vehicle_attitude_s _v_att{}; //vehicle attitude @@ -203,25 +212,15 @@ class VtolAttitudeControl param_t fw_motors_off; } _params_handles{}; - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; - bool _abort_front_transition{false}; + uint8_t _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; VtolType *_vtol_type{nullptr}; // base class for different vtol types -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - void land_detected_poll(); void tecs_status_poll(); void vehicle_attitude_poll(); //Check for attitude updates. void vehicle_cmd_poll(); void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output void fw_virtual_att_sp_poll(); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 4c8893f35dca..ecdb2116501a 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -48,14 +48,14 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _attc(att_controller), - _vtol_mode(ROTARY_WING) + _vtol_mode(ROTARY_WING), + _vtol_vehicle_status(_attc->get_vtol_vehicle_status()) { _v_att = _attc->get_att(); _v_att_sp = _attc->get_att_sp(); _mc_virtual_att_sp = _attc->get_mc_virtual_att_sp(); _fw_virtual_att_sp = _attc->get_fw_virtual_att_sp(); _v_control_mode = _attc->get_control_mode(); - _vtol_vehicle_status = _attc->get_vtol_vehicle_status(); _actuators_out_0 = _attc->get_actuators_out0(); _actuators_out_1 = _attc->get_actuators_out1(); _actuators_mc_in = _attc->get_actuators_mc_in(); @@ -76,11 +76,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : } } -VtolType::~VtolType() -{ - -} - bool VtolType::init() { const char *dev = PWM_OUTPUT0_DEVICE_PATH; @@ -109,10 +104,22 @@ bool VtolType::init() return false; } + parameters_update(); + return true; } +void VtolType::update_vtol_state() +{ + if (_vtol_mode == TRANSITION_TO_FW || _vtol_mode == TRANSITION_TO_MC) { + _time_since_trans_start = (float)(hrt_absolute_time() - _transition_start_time) * 1e-6f; + + } else { + _time_since_trans_start = 0.0f; + } +} + void VtolType::update_mc_state() { if (!flag_idle_mc) { @@ -188,6 +195,10 @@ void VtolType::update_fw_state() void VtolType::update_transition_state() { check_quadchute_condition(); + + if (_vtol_mode == TRANSITION_TO_FW) { + check_front_trans_timeout(); + } } bool VtolType::can_transition_on_ground() @@ -254,6 +265,17 @@ void VtolType::check_quadchute_condition() } } +void VtolType::check_front_trans_timeout() +{ + // check front transition timeout + if (_params->front_trans_timeout > FLT_EPSILON) { + if (_time_since_trans_start > _params->front_trans_timeout) { + // transition timeout occured, abort transition + _attc->abort_front_transition("Transition timeout"); + } + } +} + bool VtolType::set_idle_mc() { diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 04acccf46a25..abc30344cb84 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -46,6 +46,7 @@ #include #include #include +#include struct Params { int32_t idle_pwm_mc; // pwm value for idle in mc mode @@ -75,10 +76,10 @@ struct Params { // Has to match 1:1 msg/vtol_vehicle_status.msg enum mode { - TRANSITION_TO_FW = 1, - TRANSITION_TO_MC = 2, - ROTARY_WING = 3, - FIXED_WING = 4 + TRANSITION_TO_FW = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW, + TRANSITION_TO_MC = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC, + ROTARY_WING = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC, + FIXED_WING = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW, }; enum vtol_type { @@ -113,11 +114,11 @@ class VtolType public: VtolType(VtolAttitudeControl *att_controller); + ~VtolType() = default; + VtolType(const VtolType &) = delete; VtolType &operator=(const VtolType &) = delete; - virtual ~VtolType(); - /** * Initialise. */ @@ -164,11 +165,17 @@ class VtolType */ bool can_transition_on_ground(); + mode get_mode() { return _vtol_mode; } + virtual void parameters_update() = 0; - mode get_mode() {return _vtol_mode;} - virtual void parameters_update() = 0; + /** + * @brief Gets the time in seconds since the start of a transition. + * + * @return The time in second since a transition started. Returns 0 is not in a transition. + */ + float get_time_since_trans_start() {return _time_since_trans_start;} protected: VtolAttitudeControl *_attc; @@ -179,7 +186,6 @@ class VtolType struct vehicle_attitude_setpoint_s *_mc_virtual_att_sp; // virtual mc attitude setpoint struct vehicle_attitude_setpoint_s *_fw_virtual_att_sp; // virtual fw attitude setpoint struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s *_vtol_vehicle_status; struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control @@ -190,6 +196,8 @@ class VtolType struct tecs_status_s *_tecs_status; struct vehicle_land_detected_s *_land_detected; + const vtol_vehicle_status_s &_vtol_vehicle_status; + struct Params *_params; bool flag_idle_mc = false; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" @@ -206,6 +214,8 @@ class VtolType float _ra_hrate = 0.0f; // rolling average on height rate for quadchute condition float _ra_hrate_sp = 0.0f; // rolling average on height rate setpoint for quadchute condition + float _time_since_trans_start = 0.0f; + bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition hrt_abstime _trans_finished_ts = 0; @@ -215,7 +225,7 @@ class VtolType motor_state _motor_state = motor_state::DISABLED; - + hrt_abstime _transition_start_time = 0.0f; /**< absoulte time at which front transition started */ /** * @brief Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures @@ -274,6 +284,11 @@ class VtolType */ bool is_motor_off_channel(const int channel); + /** + * @brief Abort front transition if timeout value is exceeded. + */ + void check_front_trans_timeout(); + }; #endif