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

Standard methods for parsing available flight mode for PX4 and APM #12254

Merged
merged 2 commits into from
Jan 3, 2025
Merged

Standard methods for parsing available flight mode for PX4 and APM #12254

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
79b1d7d
Common Available Flight Mode parser for PX4 and Arducopter
bytesByHarsh Dec 23, 2024
41e4226
Firmware: Flight Mode Review Changes
bytesByHarsh Dec 28, 2024
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
8 changes: 8 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -553,4 +553,12 @@ elseif(MACOS)
install(SCRIPT "${CMAKE_SOURCE_DIR}/cmake/CreateMacDMG.cmake")
endif()

## Enforce Handling of All Enum Cases

if(WIN32)
add_compile_options(/w44265 /we44265)
else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wswitch -Werror=switch")
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@HTRamsey This won't be needed with the switch to -Wall right?

endif()

include(printSummary)
94 changes: 90 additions & 4 deletions custom-example/src/FirmwarePlugin/CustomFirmwarePlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,16 @@
#include "CustomFirmwarePlugin.h"
#include "CustomAutoPilotPlugin.h"
#include "Vehicle.h"
#include "px4_custom_mode.h"

//-----------------------------------------------------------------------------
CustomFirmwarePlugin::CustomFirmwarePlugin()
{
for (int i = 0; i < _flightModeInfoList.count(); i++) {
FlightModeInfo_t& info = _flightModeInfoList[i];
for (auto &mode: _availableFlightModeList){
//-- Narrow the flight mode options to only these
if (*info.name != _holdFlightMode && *info.name != _rtlFlightMode && *info.name != _missionFlightMode) {
if(mode.mode_name != _holdFlightMode && mode.mode_name != _rtlFlightMode && mode.mode_name != _missionFlightMode){
// No other flight modes can be set
info.canBeSet = false;
mode.canBeSet = false;
}
}
}
Expand Down Expand Up @@ -56,3 +56,89 @@ bool CustomFirmwarePlugin::hasGimbal(Vehicle* /*vehicle*/, bool& rollSupported,

return true;
}

void CustomFirmwarePlugin::updateAvailableFlightModes(FlightModeList modeList)
{

for(auto &mode: modeList){
PX4CustomMode::Mode cMode = static_cast<PX4CustomMode::Mode>(mode.custom_mode);
// Update Multi Rotor
switch (cMode) {
case PX4CustomMode::MANUAL :
case PX4CustomMode::STABILIZED :
case PX4CustomMode::ACRO :
case PX4CustomMode::RATTITUDE :
case PX4CustomMode::ALTCTL :
case PX4CustomMode::OFFBOARD :
case PX4CustomMode::SIMPLE :
case PX4CustomMode::POSCTL_POSCTL :
case PX4CustomMode::AUTO_LOITER :
case PX4CustomMode::AUTO_MISSION :
case PX4CustomMode::AUTO_RTL :
case PX4CustomMode::AUTO_FOLLOW_TARGET:
case PX4CustomMode::AUTO_LAND :
case PX4CustomMode::AUTO_PRECLAND :
case PX4CustomMode::AUTO_READY :
case PX4CustomMode::AUTO_RTGS :
case PX4CustomMode::AUTO_TAKEOFF :
mode.multiRotor = true;
break;
case PX4CustomMode::POSCTL_ORBIT :
mode.multiRotor = false;
break;
}

// Update Fixed Wing
switch (cMode){
case PX4CustomMode::OFFBOARD :
case PX4CustomMode::SIMPLE :
case PX4CustomMode::POSCTL_ORBIT :
case PX4CustomMode::AUTO_FOLLOW_TARGET:
case PX4CustomMode::AUTO_PRECLAND :
mode.fixedWing = false;
break;
case PX4CustomMode::MANUAL :
case PX4CustomMode::STABILIZED :
case PX4CustomMode::ACRO :
case PX4CustomMode::RATTITUDE :
case PX4CustomMode::ALTCTL :
case PX4CustomMode::POSCTL_POSCTL :
case PX4CustomMode::AUTO_LOITER :
case PX4CustomMode::AUTO_MISSION :
case PX4CustomMode::AUTO_RTL :
case PX4CustomMode::AUTO_LAND :
case PX4CustomMode::AUTO_READY :
case PX4CustomMode::AUTO_RTGS :
case PX4CustomMode::AUTO_TAKEOFF :
mode.fixedWing = true;
break;
}

// Update CanBeSet
switch (cMode){
case PX4CustomMode::AUTO_LOITER:
case PX4CustomMode::AUTO_RTL:
case PX4CustomMode::AUTO_MISSION:
mode.canBeSet = true;
break;
case PX4CustomMode::OFFBOARD :
case PX4CustomMode::SIMPLE :
case PX4CustomMode::POSCTL_ORBIT :
case PX4CustomMode::AUTO_FOLLOW_TARGET:
case PX4CustomMode::AUTO_PRECLAND :
case PX4CustomMode::MANUAL :
case PX4CustomMode::STABILIZED :
case PX4CustomMode::ACRO :
case PX4CustomMode::RATTITUDE :
case PX4CustomMode::ALTCTL :
case PX4CustomMode::POSCTL_POSCTL :
case PX4CustomMode::AUTO_LAND :
case PX4CustomMode::AUTO_READY :
case PX4CustomMode::AUTO_RTGS :
case PX4CustomMode::AUTO_TAKEOFF :
mode.canBeSet = false;
break;
}
}
_updateModeMappings(modeList);
}
1 change: 1 addition & 0 deletions custom-example/src/FirmwarePlugin/CustomFirmwarePlugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ class CustomFirmwarePlugin : public PX4FirmwarePlugin
AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) final;
const QVariantList& toolIndicators (const Vehicle* vehicle) final;
bool hasGimbal (Vehicle* vehicle, bool& rollSupported, bool& pitchSupported, bool& yawSupported) final;
void updateAvailableFlightModes (FlightModeList modeList) override;

private:
QVariantList _toolIndicatorList;
Expand Down
6 changes: 3 additions & 3 deletions src/AutoPilotPlugins/APM/APMSubMotorComponent.qml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ SetupPage {

property int neutralValue: 50;
property int _lastIndex: 0;
property bool canRunManualTest: controller.vehicle.flightMode !== 'Motor Detection' && controller.vehicle.armed && motorPage.visible && setupView.visible
property bool canRunManualTest: controller.vehicle.flightMode !== controller.vehicle.motorDetectionFlightMode && controller.vehicle.armed && motorPage.visible && setupView.visible
property var shouldRunManualTest: false // Does the operator intend to run the motor test?

APMSubMotorComponentController {
Expand Down Expand Up @@ -237,10 +237,10 @@ SetupPage {
QGCButton {
id: startAutoDetection
text: "Auto-Detect Directions"
enabled: controller.vehicle.flightMode !== 'Motor Detection'
enabled: controller.vehicle.flightMode !== controller.vehicle.motorDetectionFlightMode

onClicked: function() {
controller.vehicle.flightMode = "Motor Detection"
controller.vehicle.flightMode = controller.vehicle.motorDetectionFlightMode
controller.vehicle.armed = true
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/AutoPilotPlugins/APM/APMSubMotorComponentController.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ void APMSubMotorComponentController::handleNewMessages(int uasid, int componenti
Q_UNUSED(uasid);
Q_UNUSED(componentid);
Q_UNUSED(severity);
if (_vehicle->flightMode() == "Motor Detection"
if (_vehicle->flightMode() == _vehicle->motorDetectionFlightMode()
&& (text.toLower().contains("thruster") || text.toLower().contains("motor"))) {
_motorDetectionMessages += text + QStringLiteral("\n");
emit motorDetectionMessagesChanged();
Expand Down
5 changes: 1 addition & 4 deletions src/Comms/MockLink/MockLink.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,10 +99,7 @@ MockLink::MockLink(SharedLinkConfigurationPtr& config)

QObject::connect(this, &MockLink::writeBytesQueuedSignal, this, &MockLink::_writeBytesQueued, Qt::QueuedConnection);

union px4_custom_mode px4_cm;
px4_cm.data = 0;
px4_cm.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
_mavCustomMode = px4_cm.data;
_mavCustomMode = PX4CustomMode::MANUAL;

_mockLinkFTP = new MockLinkFTP(_vehicleSystemId, _vehicleComponentId, this);

Expand Down
98 changes: 58 additions & 40 deletions src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,32 +41,29 @@

QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog")

/*
* @brief APMCustomMode encapsulates the custom modes for APM
*/
APMCustomMode::APMCustomMode(uint32_t mode, bool settable) :
_mode(mode),
_settable(settable)
{
}

void APMCustomMode::setEnumToStringMapping(const QMap<uint32_t, QString>& enumToString)
{
_enumToString = enumToString;
}

QString APMCustomMode::modeString() const
{
QString mode = _enumToString.value(modeAsInt());
if (mode.isEmpty()) {
mode = "mode" + QString::number(modeAsInt());
}
return mode;
}

APMFirmwarePlugin::APMFirmwarePlugin(void)
: _coaxialMotors(false)
{
, _guidedFlightMode (tr("Guided"))
, _rtlFlightMode (tr("RTL"))
, _smartRtlFlightMode (tr("Smart RTL"))
, _autoFlightMode (tr("Auto"))
{
_setModeEnumToModeStringMapping({
{ APMCustomMode::GUIDED, _guidedFlightMode },
{ APMCustomMode::RTL, _rtlFlightMode },
{ APMCustomMode::SMART_RTL, _smartRtlFlightMode },
{ APMCustomMode::AUTO, _autoFlightMode },
});

updateAvailableFlightModes({
// Mode Name , Custom Mode CanBeSet adv
{ _guidedFlightMode , APMCustomMode::GUIDED, true , true },
{ _rtlFlightMode , APMCustomMode::RTL, true , true },
{ _smartRtlFlightMode , APMCustomMode::SMART_RTL, true , true },
{ _autoFlightMode , APMCustomMode::AUTO, true , true },
});

qmlRegisterType<APMFlightModesComponentController> ("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController");
qmlRegisterType<APMAirframeComponentController> ("QGroundControl.Controllers", 1, 0, "APMAirframeComponentController");
qmlRegisterType<APMSensorsComponentController> ("QGroundControl.Controllers", 1, 0, "APMSensorsComponentController");
Expand Down Expand Up @@ -106,9 +103,9 @@ QStringList APMFirmwarePlugin::flightModes(Vehicle* vehicle)
{
Q_UNUSED(vehicle)
QStringList flightModesList;
foreach (const APMCustomMode& customMode, _supportedModes) {
if (customMode.canBeSet()) {
flightModesList << customMode.modeString();
for (auto &mode : _availableFlightModeList) {
if (mode.canBeSet){
flightModesList += mode.mode_name;
}
}
return flightModesList;
Expand All @@ -119,11 +116,7 @@ QString APMFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
QString flightMode = "Unknown";

if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
foreach (const APMCustomMode& customMode, _supportedModes) {
if (customMode.modeAsInt() == custom_mode) {
flightMode = customMode.modeString();
}
}
return _modeEnumToString.value(custom_mode, flightMode);
}
return flightMode;
}
Expand All @@ -135,10 +128,10 @@ bool APMFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_m

bool found = false;

foreach(const APMCustomMode& mode, _supportedModes) {
if (flightMode.compare(mode.modeString(), Qt::CaseInsensitive) == 0) {
for (auto &mode: _availableFlightModeList){
if(flightMode.compare(mode.mode_name, Qt::CaseInsensitive) == 0){
*base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
*custom_mode = mode.modeAsInt();
*custom_mode = mode.custom_mode;
found = true;
break;
}
Expand Down Expand Up @@ -670,7 +663,32 @@ const QVariantList& APMFirmwarePlugin::toolIndicators(const Vehicle* vehicle)

bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
return vehicle->flightMode() == "Guided";
return vehicle->flightMode() == guidedFlightMode();
}

QString APMFirmwarePlugin::gotoFlightMode() const
{
return guidedFlightMode();
}

QString APMFirmwarePlugin::rtlFlightMode() const
{
return _modeEnumToString.value(_convertToCustomFlightModeEnum(APMCustomMode::RTL), _rtlFlightMode);
}

QString APMFirmwarePlugin::smartRTLFlightMode() const
{
return _modeEnumToString.value(_convertToCustomFlightModeEnum(APMCustomMode::SMART_RTL), _smartRtlFlightMode);
}

QString APMFirmwarePlugin::missionFlightMode() const
{
return _modeEnumToString.value(_convertToCustomFlightModeEnum(APMCustomMode::AUTO), _autoFlightMode);
}

QString APMFirmwarePlugin::guidedFlightMode() const
{
return _modeEnumToString.value(_convertToCustomFlightModeEnum(APMCustomMode::GUIDED), _guidedFlightMode);
}

void APMFirmwarePlugin::_soloVideoHandshake(void)
Expand Down Expand Up @@ -751,7 +769,7 @@ QString APMFirmwarePlugin::_internalParameterMetaDataFile(const Vehicle* vehicle
void APMFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
_setFlightModeAndValidate(vehicle, "Guided");
_setFlightModeAndValidate(vehicle, guidedFlightMode());
} else {
pauseVehicle(vehicle);
}
Expand Down Expand Up @@ -993,7 +1011,7 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
takeoffAltRel = altitudeRel;
}

if (!_setFlightModeAndValidate(vehicle, "Guided")) {
if (!_setFlightModeAndValidate(vehicle, guidedFlightMode())) {
qgcApp()->showAppMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
return false;
}
Expand All @@ -1016,7 +1034,7 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
{
if (vehicle->flying()) {
// Vehicle already in the air, we just need to switch to auto
if (!_setFlightModeAndValidate(vehicle, "Auto")) {
if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
}
return;
Expand All @@ -1027,12 +1045,12 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
// In Ardupilot for vtols and airplanes we need to set the mode to auto and then arm, otherwise if arming in guided
// If the vehicle has tilt rotors, it will arm them in forward flight position, being dangerous.
if (vehicle->fixedWing()) {
if (!_setFlightModeAndValidate(vehicle, "Auto")) {
if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to change to Auto mode."));
return;
}
} else {
if (!_setFlightModeAndValidate(vehicle, "Guided")) {
if (!_setFlightModeAndValidate(vehicle, guidedFlightMode())) {
qgcApp()->showAppMessage(tr("Unable to start mission: Vehicle failed to change to Guided mode."));
return;
}
Expand Down
Loading
Loading