Skip to content

Commit

Permalink
Suggestions for boat and rover handling
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR @TomasTwardzik
MaEtUgR authored and TomasTwardzik committed Feb 10, 2025
1 parent 586dc2b commit 7ac6300
Show file tree
Hide file tree
Showing 3 changed files with 23 additions and 19 deletions.
32 changes: 19 additions & 13 deletions src/modules/commander/Commander.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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},
Expand All @@ -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) {
Expand Down Expand Up @@ -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) {
Expand All @@ -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);

Expand Down Expand Up @@ -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;

Expand Down
4 changes: 2 additions & 2 deletions src/modules/navigator/mission_block.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
6 changes: 2 additions & 4 deletions src/modules/navigator/navigator_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down

0 comments on commit 7ac6300

Please sign in to comment.