[refactor] using less verbose convenience method instead of vehicle_status fields directly

[fix] Adding boat condition for NAV_CMD_TAKEOFF

msg/versioned/VehicleStatus.msg

@@ -92,6 +92,8 @@ uint8 vehicle_type
 uint8 VEHICLE_TYPE_ROTARY_WING = 0
 uint8 VEHICLE_TYPE_FIXED_WING = 1
 uint8 VEHICLE_TYPE_ROVER = 2
+uint8 VEHICLE_TYPE_BOAT = 5
+
 
 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1

src/modules/commander/Commander.cpp

@@ -583,6 +583,16 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 				return TRANSITION_DENIED;
 			}
 
+			if ((is_ground_vehicle(_vehicle_status))
+			    && !_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) {
@@ -1723,7 +1733,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_ground = is_ground_vehicle(_vehicle_status);
 
 	/* disable manual override for all systems that rely on electronic stabilization */
 	if (is_rotary) {
@@ -1732,8 +1741,11 @@ void Commander::updateParameters()
 	} else if (is_fixed) {
 		_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 
-	} else if (is_ground) {
+	} else if (is_rover_type(_vehicle_status)) {
 		_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
+
+	} 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);
@@ -2901,13 +2913,15 @@ void Commander::manualControlCheck()
 
 		_is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f);
 		_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
+		_is_throttle_near_center = (fabsf(manual_control_setpoint.throttle) < 0.05f);
 
 		if (isArmed()) {
 			// Abort autonomous mode and switch to position mode if sticks are moved significantly
 			// 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_ROTARY_WING
+				|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT)
 			   ) {
 				bool override_enabled = false;
 

src/modules/commander/Commander.hpp

@@ -275,6 +275,7 @@ private:
 
 	bool _is_throttle_above_center{false};
 	bool _is_throttle_low{false};
+	bool _is_throttle_near_center{false};
 
 	bool _arm_tune_played{false};
 	bool _have_taken_off_since_arming{false};

src/modules/commander/commander_helper.cpp

@@ -122,6 +122,16 @@ bool is_ground_vehicle(const vehicle_status_s &current_status)
 	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }
 
+bool is_rover_type(const vehicle_status_s &current_status)
+{
+	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
+}
+
+bool is_boat_type(const vehicle_status_s &current_status)
+{
+	return current_status.system_type == VEHICLE_TYPE_BOAT;
+}
+
 // End time for currently blinking LED message, 0 if no blink message
 static hrt_abstime blink_msg_end = 0;
 static int fd_leds{-1};

src/modules/commander/commander_helper.h

@@ -56,6 +56,8 @@ bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
 bool is_ground_vehicle(const vehicle_status_s &current_status);
+bool is_rover_type(const vehicle_status_s &current_status);
+bool is_boat_type(const vehicle_status_s &current_status);
 
 int buzzer_init(void);
 void buzzer_deinit(void);

src/modules/navigator/mission_block.cpp

@@ -206,8 +206,9 @@ MissionBlock::is_mission_item_reached_or_completed()
 				_waypoint_position_reached = true;
 			}
 
-		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
-			   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
+		} 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)) {
 			// 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;

src/modules/navigator/navigator_main.cpp

@@ -1137,7 +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) {
+	} 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 {