Skip to content

Commit

Permalink
Merge pull request #1901 from mavlink/pr-backport-arduplane-flight-modes
Browse files Browse the repository at this point in the history 
[BACKPORT v1.4] ArduPlane flight modes
  • Loading branch information
@julianoes
julianoes committed Sep 19, 2022
2 parents 894280e + 213a402 commit bd80ac8
Show file tree
Hide file tree
Showing 3 changed files with 126 additions and 32 deletions.
55 changes: 42 additions & 13 deletions src/mavsdk/core/ardupilot_custom_mode.h
View file
Expand Up @@ -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
Expand All @@ -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
99 changes: 80 additions & 19 deletions src/mavsdk/core/system_impl.cpp
View file
Expand Up @@ -221,7 +221,7 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message)
} else {
const auto new_vehicle_type = static_cast<MAV_TYPE>(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<unsigned>(heartbeat.type)
<< ", old type: " << static_cast<unsigned>(_vehicle_type) << ")";
_vehicle_type = new_vehicle_type;
Expand Down Expand Up @@ -902,25 +902,58 @@ 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 {
command.params.maybe_param2 =
static_cast<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.";
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<float>(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 {
command.params.maybe_param2 =
static_cast<float>(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;

Expand All @@ -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:
Expand All @@ -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<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
SystemImpl::make_command_px4_mode(FlightMode flight_mode, uint8_t component_id)
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down
4 changes: 4 additions & 0 deletions src/mavsdk/core/system_impl.h
View file
Expand Up @@ -52,6 +52,9 @@ class SystemImpl : public Sender {
Acro,
Rattitude,
Stabilized,
Fbwa,
Autotune,
Guided
};

explicit SystemImpl(MavsdkImpl& parent);
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit bd80ac8

Please sign in to comment.