Ardupilot mission handling and vehicle type checking

src/core/ardupilot_custom_mode.h

@@ -0,0 +1,52 @@
+#pragma once
+
+namespace ardupilot {
+
+// Enumeration representing the available modes for the Arudpilot rover autopilot.
+enum class RoverMode {
+    Manual = 0,
+    Acro = 1,
+    Steering = 3,
+    Hold = 4,
+    Loiter = 5,
+    Follow = 6,
+    Simple = 7,
+    Auto = 10,
+    RTL = 11,
+    Smart_RTL = 12,
+    Guided = 15,
+    Initializing = 16,
+    Unknown = 100
+};
+
+// Enumeration representing the available modes for the Arudpilot copter autopilot.
+enum class CopterMode {
+    Stabilize = 0,
+    Acro = 1,
+    Alt_Hold = 2,
+    Auto = 3,
+    Guided = 4,
+    Loiter = 5,
+    RTL = 6,
+    Circle = 7,
+    Land = 9,
+    Drift = 11,
+    Sport = 13,
+    Flip = 14,
+    Auto_Tune = 15,
+    POS_HOLD = 16,
+    Break = 17,
+    Throw = 18,
+    Avoid_ADBS = 19,
+    Guided_No_GPS = 20,
+    Smart_RTL = 21,
+    Flow_Hold = 22,
+    Follow = 23,
+    Zigzag = 24,
+    System_ID = 25,
+    Auto_Rotate = 26,
+    Auto_RTL = 27,
+    Turtle = 28,
+    Unknown = 100
+};
+} // namespace ardupilot

src/core/system_impl.cpp

@@ -5,6 +5,7 @@
 #include "system_impl.h"
 #include "plugin_impl_base.h"
 #include "px4_custom_mode.h"
+#include "ardupilot_custom_mode.h"
 #include <cstdlib>
 #include <functional>
 #include <algorithm>
@@ -205,20 +206,26 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
     mavlink_heartbeat_t heartbeat;
     mavlink_msg_heartbeat_decode(&message, &heartbeat);
 
-    if (message.compid == MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) {
-        _armed = ((heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false);
-        _hitl_enabled = ((heartbeat.base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? true : false);
-
-        if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-            _flight_mode = to_flight_mode_from_custom_mode(heartbeat.custom_mode);
-        }
-    }
-
     if (heartbeat.autopilot == MAV_AUTOPILOT_PX4) {
         _autopilot = Autopilot::Px4;
     } else if (heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) {
         _autopilot = Autopilot::ArduPilot;
     }
+    // This check only works if the MAV_TYPE::MAV_TYPE_ENUM_END is actually the
+    // last enumerator.
+    if (MAV_TYPE::MAV_TYPE_ENUM_END > heartbeat.type) {
+        _vehicle_type = static_cast<MAV_TYPE>(heartbeat.type);
+    } else {
+        LogErr() << "type received in HEARTBEAT was not recognized";
+    }
+
+    if (message.compid == MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) {
+        _armed = ((heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false);
+        _hitl_enabled = ((heartbeat.base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? true : false);
+    }
+    if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+        _flight_mode = to_flight_mode_from_custom_mode(heartbeat.custom_mode);
+    }
 
     set_connected();
 }
@@ -605,6 +612,11 @@ uint8_t SystemImpl::get_own_component_id() const
     return _parent.get_own_component_id();
 }
 
+MAV_TYPE SystemImpl::get_vehicle_type() const
+{
+    return _vehicle_type;
+}
+
 uint8_t SystemImpl::get_own_mav_type() const
 {
     return _parent.get_mav_type();
@@ -917,6 +929,110 @@ void SystemImpl::subscribe_param_float(
 
 std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
 SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_id)
+{
+    if (_autopilot == Autopilot::ArduPilot) {
+        return make_command_ardupilot_mode(flight_mode, component_id);
+    } else {
+        return make_command_px4_mode(flight_mode, component_id);
+    }
+}
+
+std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
+SystemImpl::make_command_ardupilot_mode(FlightMode flight_mode, uint8_t component_id)
+{
+    const uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
+    const uint8_t flag_hitl_enabled = _hitl_enabled ? MAV_MODE_FLAG_HIL_ENABLED : 0;
+    const uint8_t mode_type =
+        MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed | flag_hitl_enabled;
+
+    MavlinkCommandSender::CommandLong command{*this};
+
+    command.command = MAV_CMD_DO_SET_MODE;
+    command.params.param1 = float(mode_type);
+
+    switch (_vehicle_type) {
+        case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
+            if (flight_mode_to_ardupilot_rover_mode(flight_mode) == ardupilot::RoverMode::Unknown) {
+                LogErr() << "Cannot translate flight mode to ardupilot rover mode.";
+                MavlinkCommandSender::CommandLong empty_command{*this};
+                return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command);
+            } else {
+                command.params.param2 = float(flight_mode_to_ardupilot_rover_mode(flight_mode));
+            }
+            break;
+        default:
+            if (flight_mode_to_ardupilot_copter_mode(flight_mode) ==
+                ardupilot::CopterMode::Unknown) {
+                LogErr() << "Cannot translate flight mode to ardupilot copter mode.";
+                MavlinkCommandSender::CommandLong empty_command{*this};
+                return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command);
+            } else {
+                command.params.param2 = float(flight_mode_to_ardupilot_copter_mode(flight_mode));
+            }
+            break;
+    }
+    command.target_component_id = component_id;
+
+    return std::make_pair<>(MavlinkCommandSender::Result::Success, command);
+}
+ardupilot::RoverMode SystemImpl::flight_mode_to_ardupilot_rover_mode(FlightMode flight_mode)
+{
+    switch (flight_mode) {
+        case FlightMode::Mission:
+            return ardupilot::RoverMode::Auto;
+        case FlightMode::Acro:
+            return ardupilot::RoverMode::Acro;
+        case FlightMode::Hold:
+            return ardupilot::RoverMode::Hold;
+        case FlightMode::ReturnToLaunch:
+            return ardupilot::RoverMode::RTL;
+        case FlightMode::Manual:
+            return ardupilot::RoverMode::Manual;
+        case FlightMode::FollowMe:
+            return ardupilot::RoverMode::Follow;
+        case FlightMode::Unknown:
+        case FlightMode::Ready:
+        case FlightMode::Takeoff:
+        case FlightMode::Land:
+        case FlightMode::Offboard:
+        case FlightMode::Altctl:
+        case FlightMode::Posctl:
+        case FlightMode::Rattitude:
+        case FlightMode::Stabilized:
+        default:
+            return ardupilot::RoverMode::Unknown;
+    }
+}
+ardupilot::CopterMode SystemImpl::flight_mode_to_ardupilot_copter_mode(FlightMode flight_mode)
+{
+    switch (flight_mode) {
+        case FlightMode::Mission:
+            return ardupilot::CopterMode::Auto;
+        case FlightMode::Acro:
+            return ardupilot::CopterMode::Acro;
+        case FlightMode::Hold:
+            return ardupilot::CopterMode::Alt_Hold;
+        case FlightMode::ReturnToLaunch:
+            return ardupilot::CopterMode::RTL;
+        case FlightMode::Land:
+            return ardupilot::CopterMode::Land;
+        case FlightMode::Manual:
+        case FlightMode::FollowMe:
+        case FlightMode::Unknown:
+        case FlightMode::Ready:
+        case FlightMode::Takeoff:
+        case FlightMode::Offboard:
+        case FlightMode::Altctl:
+        case FlightMode::Posctl:
+        case FlightMode::Rattitude:
+        case FlightMode::Stabilized:
+        default:
+            return ardupilot::CopterMode::Unknown;
+    }
+}
+
+std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
+SystemImpl::make_command_px4_mode(FlightMode flight_mode, uint8_t component_id)
 {
     const uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
     const uint8_t flag_hitl_enabled = _hitl_enabled ? MAV_MODE_FLAG_HIL_ENABLED : 0;
@@ -989,8 +1105,61 @@ SystemImpl::FlightMode SystemImpl::get_flight_mode() const
 {
     return _flight_mode;
 }
-
 SystemImpl::FlightMode SystemImpl::to_flight_mode_from_custom_mode(uint32_t custom_mode)
+{
+    if (_autopilot == Autopilot::ArduPilot) {
+        switch (_vehicle_type) {
+            case MAV_TYPE::MAV_TYPE_GROUND_ROVER:
+                return to_flight_mode_from_ardupilot_rover_mode(custom_mode);
+            default:
+                return to_flight_mode_from_ardupilot_copter_mode(custom_mode);
+        }
+    } else {
+        return to_flight_mode_from_px4_mode(custom_mode);
+    }
+}
+
+SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode)
+{
+    switch (static_cast<ardupilot::RoverMode>(custom_mode)) {
+        case ardupilot::RoverMode::Auto:
+            return FlightMode::Mission;
+        case ardupilot::RoverMode::Acro:
+            return FlightMode::Acro;
+        case ardupilot::RoverMode::Hold:
+            return FlightMode::Hold;
+        case ardupilot::RoverMode::RTL:
+            return FlightMode::ReturnToLaunch;
+        case ardupilot::RoverMode::Manual:
+            return FlightMode::Manual;
+        case ardupilot::RoverMode::Follow:
+            return FlightMode::FollowMe;
+        default:
+            return FlightMode::Unknown;
+    }
+}
+SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode)
+{
+    switch (static_cast<ardupilot::CopterMode>(custom_mode)) {
+        case ardupilot::CopterMode::Auto:
+            return FlightMode::Mission;
+        case ardupilot::CopterMode::Acro:
+            return FlightMode::Acro;
+        case ardupilot::CopterMode::Alt_Hold:
+        case ardupilot::CopterMode::POS_HOLD:
+        case ardupilot::CopterMode::Flow_Hold:
+            return FlightMode::Hold;
+        case ardupilot::CopterMode::RTL:
+        case ardupilot::CopterMode::Auto_RTL:
+            return FlightMode::ReturnToLaunch;
+        case ardupilot::CopterMode::Land:
+            return FlightMode::Land;
+        default:
+            return FlightMode::Unknown;
+    }
+}
+
+SystemImpl::FlightMode SystemImpl::to_flight_mode_from_px4_mode(uint32_t custom_mode)
 {
     px4::px4_custom_mode px4_custom_mode;
     px4_custom_mode.data = custom_mode;

src/core/system_impl.h

@@ -8,6 +8,7 @@
 #include "mavlink_message_handler.h"
 #include "mavlink_mission_transfer.h"
 #include "mavlink_statustext_handler.h"
+#include "ardupilot_custom_mode.h"
 #include "ping.h"
 #include "timeout_handler.h"
 #include "safe_queue.h"
@@ -87,7 +88,10 @@ class SystemImpl : public Sender {
 
     Autopilot autopilot() const override { return _autopilot; };
 
-    static FlightMode to_flight_mode_from_custom_mode(uint32_t custom_mode);
+    FlightMode to_flight_mode_from_custom_mode(uint32_t custom_mode);
+    static FlightMode to_flight_mode_from_px4_mode(uint32_t custom_mode);
+    static FlightMode to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode);
+    static FlightMode to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode);
 
     using CommandResultCallback = MavlinkCommandSender::CommandResultCallback;
 
@@ -131,6 +135,7 @@ class SystemImpl : public Sender {
     uint8_t get_own_system_id() const override;
     uint8_t get_own_component_id() const override;
     uint8_t get_own_mav_type() const;
+    MAV_TYPE get_vehicle_type() const;
 
     bool is_armed() const { return _armed; }
 
@@ -165,7 +170,6 @@ class SystemImpl : public Sender {
 
     MavlinkCommandSender::Result
     set_flight_mode(FlightMode mode, uint8_t component_id = MAV_COMP_ID_AUTOPILOT1);
-
     void set_flight_mode_async(
         FlightMode mode,
         CommandResultCallback callback,
@@ -303,10 +307,19 @@ class SystemImpl : public Sender {
 
     void system_thread();
 
-    // We use std::pair instead of a std::optional.
     std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
     make_command_flight_mode(FlightMode mode, uint8_t component_id);
 
+    // We use std::pair instead of a std::optional.
+    std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
+    make_command_px4_mode(FlightMode mode, uint8_t component_id);
+
+    std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
+    make_command_ardupilot_mode(FlightMode flight_mode, uint8_t component_id);
+
+    static ardupilot::RoverMode flight_mode_to_ardupilot_rover_mode(FlightMode flight_mode);
+    static ardupilot::CopterMode flight_mode_to_ardupilot_copter_mode(FlightMode flight_mode);
+
     MavlinkCommandSender::CommandLong
     make_command_msg_rate(uint16_t message_id, double rate_hz, uint8_t component_id);
 
@@ -385,7 +398,10 @@ class SystemImpl : public Sender {
     std::function<bool(mavlink_message_t&)> _incoming_messages_intercept_callback{nullptr};
     std::function<bool(mavlink_message_t&)> _outgoing_messages_intercept_callback{nullptr};
 
+    MAV_TYPE _vehicle_type{MAV_TYPE::MAV_TYPE_GENERIC};
+
     std::atomic<FlightMode> _flight_mode{FlightMode::Unknown};
+
     std::mutex _autopilot_version_mutex{};
     System::AutopilotVersion _autopilot_version{
         MAV_PROTOCOL_CAPABILITY_COMMAND_INT, 0, 0, 0, 0, 0, 0, 0};

src/plugins/mission_raw/mission_raw_impl.cpp

@@ -8,6 +8,9 @@
 
 namespace mavsdk {
 
+// This is an empty item that can be sent to ArduPilot to mimic clearing of mission.
+constexpr MissionRaw::MissionItem empty_item{0, 3, 16, 1};
+
 using namespace std::placeholders; // for `_1`
 
 MissionRawImpl::MissionRawImpl(System& system) : PluginImplBase(system)
@@ -377,15 +380,21 @@ void MissionRawImpl::clear_mission_async(const MissionRaw::ResultCallback& callb
 {
     reset_mission_progress();
 
-    _parent->mission_transfer().clear_items_async(
-        MAV_MISSION_TYPE_MISSION, [this, callback](MAVLinkMissionTransfer::Result result) {
-            auto converted_result = convert_result(result);
-            _parent->call_user_callback([callback, converted_result]() {
-                if (callback) {
-                    callback(converted_result);
-                }
+    // For ArduPilot to clear a mission we need to upload an empty mission.
+    if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) {
+        std::vector<MissionRaw::MissionItem> mission_items{empty_item};
+        upload_mission_async(mission_items, callback);
+    } else {
+        _parent->mission_transfer().clear_items_async(
+            MAV_MISSION_TYPE_MISSION, [this, callback](MAVLinkMissionTransfer::Result result) {
+                auto converted_result = convert_result(result);
+                _parent->call_user_callback([callback, converted_result]() {
+                    if (callback) {
+                        callback(converted_result);
+                    }
+                });
             });
-        });
+    }
 }
 
 MissionRaw::Result MissionRawImpl::set_current_mission_item(int index)
@@ -519,5 +528,4 @@ MissionRaw::Result MissionRawImpl::convert_result(MAVLinkMissionTransfer::Result
             return MissionRaw::Result::Unknown;
     }
 }
-
 } // namespace mavsdk