Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[backport v1.4] ArduPlane flight modes #1901

Merged
merged 3 commits into from
Sep 19, 2022
Merged

[backport v1.4] ArduPlane flight modes #1901

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ef42fd3
Backport of ArduPlane flight modes
julianoes Sep 19, 2022
b988c60
Fix vehicle type
Jun 8, 2022
213a402
Remove underscore from new_vehicle_type
Jun 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
55 changes: 42 additions & 13 deletions src/mavsdk/core/ardupilot_custom_mode.h
View file
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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