From e907fb0aa21bb6170b087b0af6349c6611710403 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Jan 2025 11:34:51 +0100 Subject: [PATCH] Suggestions for boat and rover handling --- src/modules/commander/Commander.cpp | 32 ++++++++++++++---------- src/modules/navigator/mission_block.cpp | 4 +-- src/modules/navigator/navigator_main.cpp | 6 ++--- 3 files changed, 23 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index aa05e601e1..e303077a90 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -572,12 +572,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ return TRANSITION_DENIED; } - if ((!_failsafe_flags.manual_control_signal_lost) && - ((!_vehicle_control_mode.flag_control_climb_rate_enabled && - !_is_throttle_low) || - ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER || - _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) && - !_is_throttle_near_center))) { + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && + !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low + && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER + && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_BOAT) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, @@ -586,6 +584,17 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ return TRANSITION_DENIED; } + if ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER + || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) + && !_failsafe_flags.manual_control_signal_lost && !_is_throttle_near_center) { + + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle not centered\t"); + events::send(events::ID("commander_arm_denied_throttle_not_centered"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: throttle not centered"); + tune_negative(true); + return TRANSITION_DENIED; + } + } else if (calling_reason == arm_disarm_reason_t::stick_gesture || calling_reason == arm_disarm_reason_t::rc_switch || calling_reason == arm_disarm_reason_t::rc_button) { @@ -1728,8 +1737,6 @@ void Commander::updateParameters() && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status) && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - const bool is_rover = is_rover_type(_vehicle_status); - const bool is_boat = is_boat_type(_vehicle_status); /* disable manual override for all systems that rely on electronic stabilization */ if (is_rotary) { @@ -1738,14 +1745,13 @@ void Commander::updateParameters() } else if (is_fixed) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - } else if (is_rover) { + } else if (is_rover_type(_vehicle_status)) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; - } else if (is_boat) { + } else if (is_boat_type(_vehicle_status)) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_BOAT; } - _vehicle_status.is_vtol = is_vtol(_vehicle_status); _vehicle_status.is_vtol_tailsitter = is_vtol_tailsitter(_vehicle_status); @@ -2943,8 +2949,8 @@ void Commander::manualControlCheck() // but only if actually in air. if (manual_control_setpoint.sticks_moving && !_vehicle_control_mode.flag_control_manual_enabled - && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || - _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) + && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) ) { bool override_enabled = false; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index db881b731a..6c53a1ef09 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -207,8 +207,8 @@ MissionBlock::is_mission_item_reached_or_completed() } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && - (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER || - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT)) { + (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER + || _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT)) { // Accept takeoff waypoint to be reached if the distance in 2D plane is within acceptance radius if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d998545f4c..995ee2cb10 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1137,10 +1137,8 @@ float Navigator::get_altitude_acceptance_radius() return _param_nav_fw_alt_rad.get(); } - } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { - return INFINITY; - - } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) { + } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER + || get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) { return INFINITY; } else {