From ef42fd374c999be27e5926ed9d426d51a7d7f8bf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Sep 2022 14:06:23 +1200 Subject: [PATCH 1/3] Backport of ArduPlane flight modes --- src/mavsdk/core/ardupilot_custom_mode.h | 55 ++++++++++---- src/mavsdk/core/system_impl.cpp | 97 ++++++++++++++++++++----- src/mavsdk/core/system_impl.h | 4 + 3 files changed, 125 insertions(+), 31 deletions(-) diff --git a/src/mavsdk/core/ardupilot_custom_mode.h b/src/mavsdk/core/ardupilot_custom_mode.h index 95cc2ed93f..54c65a8f9d 100644 --- a/src/mavsdk/core/ardupilot_custom_mode.h +++ b/src/mavsdk/core/ardupilot_custom_mode.h @@ -12,8 +12,8 @@ enum class RoverMode { Follow = 6, Simple = 7, Auto = 10, - RTL = 11, - Smart_RTL = 12, + Rtl = 11, + SmartRtl = 12, Guided = 15, Initializing = 16, Unknown = 100 @@ -23,30 +23,59 @@ enum class RoverMode { enum class CopterMode { Stabilize = 0, Acro = 1, - Alt_Hold = 2, + AltHold = 2, Auto = 3, Guided = 4, Loiter = 5, - RTL = 6, + Rtl = 6, Circle = 7, Land = 9, Drift = 11, Sport = 13, Flip = 14, - Auto_Tune = 15, - POS_HOLD = 16, + AutoTune = 15, + PosHold = 16, Break = 17, Throw = 18, - Avoid_ADBS = 19, - Guided_No_GPS = 20, - Smart_RTL = 21, - Flow_Hold = 22, + AvoidAdbs = 19, + GuidedNoGps = 20, + SmartRtl = 21, + FlowHold = 22, Follow = 23, Zigzag = 24, - System_ID = 25, - Auto_Rotate = 26, - Auto_RTL = 27, + SystemId = 25, + AutoRotate = 26, + AutoRtl = 27, Turtle = 28, Unknown = 100 }; + +enum class PlaneMode { + Manual = 0, + Circle = 1, + Stabilize = 2, + Training = 3, + Acro = 4, + Fbwa = 5, + Fbwb = 6, + Cruise = 7, + Autotune = 8, + Auto = 10, + Rtl = 11, + Loiter = 12, + Takeoff = 13, + AvoidAdsb = 14, + Guided = 15, + Initializing = 16, + QStabilize = 17, + QHover = 18, + QLoiter = 19, + QLand = 20, + QRtl = 21, + QAutotune = 22, + QAcro = 23, + Thermal = 24, + Unknown = 100 +}; + } // namespace ardupilot \ No newline at end of file diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index bf66405724..1f91964eaa 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -902,7 +902,7 @@ SystemImpl::make_command_ardupilot_mode(FlightMode flight_mode, uint8_t componen case MAV_TYPE::MAV_TYPE_SURFACE_BOAT: 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."; + LogErr() << "Cannot translate flight mode to ArduPilot Rover mode."; MavlinkCommandSender::CommandLong empty_command{}; return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command); } else { @@ -910,10 +910,36 @@ SystemImpl::make_command_ardupilot_mode(FlightMode flight_mode, uint8_t componen static_cast(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."; + case MAV_TYPE::MAV_TYPE_FIXED_WING: + case MAV_TYPE::MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: + case MAV_TYPE::MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: + case MAV_TYPE::MAV_TYPE_VTOL_TILTROTOR: + case MAV_TYPE::MAV_TYPE_VTOL_FIXEDROTOR: + case MAV_TYPE::MAV_TYPE_VTOL_TAILSITTER: + case MAV_TYPE::MAV_TYPE_VTOL_RESERVED4: + case MAV_TYPE::MAV_TYPE_VTOL_RESERVED5: { + const auto new_mode = flight_mode_to_ardupilot_plane_mode(flight_mode); + if (new_mode == ardupilot::PlaneMode::Unknown) { + LogErr() << "Cannot translate flight mode to ArduPilot Plane mode."; + MavlinkCommandSender::CommandLong empty_command{}; + return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command); + } else { + command.params.maybe_param2 = static_cast(new_mode); + } + break; + } + + case MAV_TYPE::MAV_TYPE_QUADROTOR: + case MAV_TYPE::MAV_TYPE_COAXIAL: + case MAV_TYPE::MAV_TYPE_HELICOPTER: + case MAV_TYPE::MAV_TYPE_HEXAROTOR: + case MAV_TYPE::MAV_TYPE_OCTOROTOR: + case MAV_TYPE::MAV_TYPE_TRICOPTER: + case MAV_TYPE::MAV_TYPE_DODECAROTOR: + case MAV_TYPE::MAV_TYPE_DECAROTOR: { + const auto new_mode = flight_mode_to_ardupilot_copter_mode(flight_mode); + if (new_mode == ardupilot::CopterMode::Unknown) { + LogErr() << "Cannot translate flight mode to ArduPilot Copter mode."; MavlinkCommandSender::CommandLong empty_command{}; return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command); } else { @@ -921,6 +947,13 @@ SystemImpl::make_command_ardupilot_mode(FlightMode flight_mode, uint8_t componen static_cast(flight_mode_to_ardupilot_copter_mode(flight_mode)); } break; + } + + default: + LogErr() << "Cannot translate flight mode to ArduPilot mode, for MAV_TYPE: " + << _vehicle_type; + MavlinkCommandSender::CommandLong empty_command{}; + return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command); } command.target_component_id = component_id; @@ -936,7 +969,7 @@ ardupilot::RoverMode SystemImpl::flight_mode_to_ardupilot_rover_mode(FlightMode case FlightMode::Hold: return ardupilot::RoverMode::Hold; case FlightMode::ReturnToLaunch: - return ardupilot::RoverMode::RTL; + return ardupilot::RoverMode::Rtl; case FlightMode::Manual: return ardupilot::RoverMode::Manual; case FlightMode::FollowMe: @@ -962,25 +995,53 @@ ardupilot::CopterMode SystemImpl::flight_mode_to_ardupilot_copter_mode(FlightMod case FlightMode::Acro: return ardupilot::CopterMode::Acro; case FlightMode::Hold: - return ardupilot::CopterMode::Alt_Hold; + return ardupilot::CopterMode::AltHold; case FlightMode::ReturnToLaunch: - return ardupilot::CopterMode::RTL; + 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: + return ardupilot::CopterMode::Follow; case FlightMode::Offboard: + return ardupilot::CopterMode::Guided; case FlightMode::Altctl: + return ardupilot::CopterMode::AltHold; case FlightMode::Posctl: - case FlightMode::Rattitude: + return ardupilot::CopterMode::PosHold; case FlightMode::Stabilized: + return ardupilot::CopterMode::Stabilize; + case FlightMode::Unknown: + case FlightMode::Ready: + case FlightMode::Takeoff: + case FlightMode::Rattitude: default: return ardupilot::CopterMode::Unknown; } } +ardupilot::PlaneMode SystemImpl::flight_mode_to_ardupilot_plane_mode(FlightMode flight_mode) +{ + switch (flight_mode) { + case FlightMode::Mission: + return ardupilot::PlaneMode::Auto; + case FlightMode::Acro: + return ardupilot::PlaneMode::Acro; + case FlightMode::Hold: + return ardupilot::PlaneMode::Loiter; + case FlightMode::ReturnToLaunch: + return ardupilot::PlaneMode::Rtl; + case FlightMode::Manual: + return ardupilot::PlaneMode::Manual; + case FlightMode::Fbwa: + return ardupilot::PlaneMode::Fbwa; + case FlightMode::Stabilized: + return ardupilot::PlaneMode::Stabilize; + case FlightMode::Unknown: + return ardupilot::PlaneMode::Unknown; + default: + return ardupilot::PlaneMode::Unknown; + } +} std::pair SystemImpl::make_command_px4_mode(FlightMode flight_mode, uint8_t component_id) @@ -1080,7 +1141,7 @@ SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_rover_mode(uint return FlightMode::Acro; case ardupilot::RoverMode::Hold: return FlightMode::Hold; - case ardupilot::RoverMode::RTL: + case ardupilot::RoverMode::Rtl: return FlightMode::ReturnToLaunch; case ardupilot::RoverMode::Manual: return FlightMode::Manual; @@ -1097,12 +1158,12 @@ SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_copter_mode(uin 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: + case ardupilot::CopterMode::AltHold: + case ardupilot::CopterMode::PosHold: + case ardupilot::CopterMode::FlowHold: return FlightMode::Hold; - case ardupilot::CopterMode::RTL: - case ardupilot::CopterMode::Auto_RTL: + case ardupilot::CopterMode::Rtl: + case ardupilot::CopterMode::AutoRtl: return FlightMode::ReturnToLaunch; case ardupilot::CopterMode::Land: return FlightMode::Land; diff --git a/src/mavsdk/core/system_impl.h b/src/mavsdk/core/system_impl.h index d7235e074c..9cb611e421 100644 --- a/src/mavsdk/core/system_impl.h +++ b/src/mavsdk/core/system_impl.h @@ -52,6 +52,9 @@ class SystemImpl : public Sender { Acro, Rattitude, Stabilized, + Fbwa, + Autotune, + Guided }; explicit SystemImpl(MavsdkImpl& parent); @@ -413,6 +416,7 @@ class SystemImpl : public Sender { static ardupilot::RoverMode flight_mode_to_ardupilot_rover_mode(FlightMode flight_mode); static ardupilot::CopterMode flight_mode_to_ardupilot_copter_mode(FlightMode flight_mode); + static ardupilot::PlaneMode flight_mode_to_ardupilot_plane_mode(FlightMode flight_mode); MavlinkCommandSender::CommandLong make_command_msg_rate(uint16_t message_id, double rate_hz, uint8_t component_id); From b988c608464ecdefb8728d4b91a01ab17d2e48c4 Mon Sep 17 00:00:00 2001 From: Seb Horsewell Date: Wed, 8 Jun 2022 15:13:14 +0100 Subject: [PATCH 2/3] Fix vehicle type --- src/mavsdk/core/system_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index 1f91964eaa..4c64389899 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -221,7 +221,7 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message) } else { const auto new_vehicle_type = static_cast(heartbeat.type); if (heartbeat.autopilot != MAV_AUTOPILOT_INVALID && _vehicle_type != new_vehicle_type && - _vehicle_type != MAV_TYPE_GENERIC) { + _new_vehicle_type != MAV_TYPE_GENERIC) { LogWarn() << "Vehicle type changed (new type: " << static_cast(heartbeat.type) << ", old type: " << static_cast(_vehicle_type) << ")"; _vehicle_type = new_vehicle_type; From 213a40287448537fe4b7a5586deabe20e6bdf842 Mon Sep 17 00:00:00 2001 From: Seb Horsewell Date: Wed, 8 Jun 2022 15:18:46 +0100 Subject: [PATCH 3/3] Remove underscore from new_vehicle_type --- src/mavsdk/core/system_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index 4c64389899..c91d015403 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -221,7 +221,7 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message) } else { const auto new_vehicle_type = static_cast(heartbeat.type); if (heartbeat.autopilot != MAV_AUTOPILOT_INVALID && _vehicle_type != new_vehicle_type && - _new_vehicle_type != MAV_TYPE_GENERIC) { + new_vehicle_type != MAV_TYPE_GENERIC) { LogWarn() << "Vehicle type changed (new type: " << static_cast(heartbeat.type) << ", old type: " << static_cast(_vehicle_type) << ")"; _vehicle_type = new_vehicle_type;