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 1 commit
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
51 changes: 47 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,46 @@ bool CustomFirmwarePlugin::hasGimbal(Vehicle* /*vehicle*/, bool& rollSupported,

return true;
}

void CustomFirmwarePlugin::updateAvailableFlightModes(FlightModeList modeList)
{
_availableFlightModeList.clear();
for(auto mode: modeList){

// Update Multi Rotor
switch (mode.custom_mode) {
case PX4CustomMode::POSCTL_ORBIT:
mode.multiRotor = false;
break;
default:
mode.multiRotor = true;
break;
}

// Update Fixed Wing
switch (mode.custom_mode){
case PX4CustomMode::OFFBOARD:
case PX4CustomMode::SIMPLE:
case PX4CustomMode::POSCTL_ORBIT:
case PX4CustomMode::AUTO_FOLLOW_TARGET:
case PX4CustomMode::AUTO_PRECLAND:
mode.fixedWing = false;
break;
default:
mode.fixedWing = true;
}

// Update CanBeSet
switch (mode.custom_mode){
case PX4CustomMode::AUTO_LOITER:
case PX4CustomMode::AUTO_RTL:
case PX4CustomMode::AUTO_MISSION:
mode.canBeSet = true;
break;
default:
mode.canBeSet = false;
}

_updateModeMappings(mode);
}
}
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
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 , SM, Custom Mode CanBeSet adv FW MR
{ _guidedFlightMode , 0, APMCustomMode::GUIDED, true , true , false , true },
{ _rtlFlightMode , 0, APMCustomMode::RTL, true , true , false , true },
{ _smartRtlFlightMode , 0, APMCustomMode::SMART_RTL, true , true , false , true },
{ _autoFlightMode , 0, APMCustomMode::AUTO, true , true , false , 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
36 changes: 18 additions & 18 deletions src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,21 +19,14 @@

Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLog)

class APMCustomMode
struct APMCustomMode
{
public:
APMCustomMode(uint32_t mode, bool settable);
uint32_t modeAsInt() const { return _mode; }
bool canBeSet() const { return _settable; }
QString modeString() const;

protected:
void setEnumToStringMapping(const QMap<uint32_t,QString>& enumToString);

private:
uint32_t _mode;
bool _settable;
QMap<uint32_t,QString> _enumToString;
enum Mode : uint32_t {
AUTO = 3,
GUIDED = 4,
RTL = 6,
SMART_RTL = 21
};
};

/// This is the base class for all stack specific APM firmware plugins
Expand All @@ -59,10 +52,11 @@ class APMFirmwarePlugin : public FirmwarePlugin
bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) override;
bool MAV_CMD_DO_SET_MODE_is_supported() const override { return true; }
bool isGuidedMode (const Vehicle* vehicle) const override;
QString gotoFlightMode (void) const override { return QStringLiteral("Guided"); }
QString rtlFlightMode (void) const override { return QString("RTL"); }
QString smartRTLFlightMode (void) const override { return QString("Smart RTL"); }
QString missionFlightMode (void) const override { return QString("Auto"); }
QString gotoFlightMode (void) const override;
QString rtlFlightMode (void) const override;
QString smartRTLFlightMode (void) const override;
QString missionFlightMode (void) const override;
virtual QString guidedFlightMode (void) const;
void pauseVehicle (Vehicle* vehicle) override;
void guidedModeRTL (Vehicle* vehicle, bool smartRTL) override;
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeChange, bool pauseVehicle) override;
Expand Down Expand Up @@ -110,6 +104,12 @@ class APMFirmwarePlugin : public FirmwarePlugin

bool _coaxialMotors;


QString _guidedFlightMode ;
QString _rtlFlightMode ;
QString _smartRtlFlightMode;
QString _autoFlightMode ;

private slots:
void _artooSocketError(QAbstractSocket::SocketError socketError);

Expand Down
Loading
Loading