zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #4769 from DonLakeFlyer/ReworkFlightModeOverride 

PX4FirmwarePlugin: Rework flight mode override mechanism
QGC4.4
Don Gagne 8 years ago committed by GitHub
parent
01dc49bd39 48ffa75927
commit
0d0fdbf745
2 changed files with 84 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 96
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  2. 43
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

96
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
Unescape View File

@ -26,25 +26,24 @@
#include "px4_custom_mode.h" #include "px4_custom_mode.h"
const char* PX4FirmwarePlugin::_manualFlightMode = "Manual";
const char* PX4FirmwarePlugin::_altCtlFlightMode = "Altitude";
const char* PX4FirmwarePlugin::_posCtlFlightMode = "Position";
const char* PX4FirmwarePlugin::_missionFlightMode = "Mission";
const char* PX4FirmwarePlugin::_holdFlightMode = "Hold";
const char* PX4FirmwarePlugin::_takeoffFlightMode = "Takeoff";
const char* PX4FirmwarePlugin::_landingFlightMode = "Land";
const char* PX4FirmwarePlugin::_rtlFlightMode = "Return";
const char* PX4FirmwarePlugin::_acroFlightMode = "Acro";
const char* PX4FirmwarePlugin::_offboardFlightMode = "Offboard";
const char* PX4FirmwarePlugin::_stabilizedFlightMode = "Stabilized";
const char* PX4FirmwarePlugin::_rattitudeFlightMode = "Rattitude";
const char* PX4FirmwarePlugin::_followMeFlightMode = "Follow Me";
const char* PX4FirmwarePlugin::_rtgsFlightMode = "Return to Groundstation";
const char* PX4FirmwarePlugin::_readyFlightMode = "Ready";
const char* PX4FirmwarePlugin::_simpleFlightMode = "Simple";
PX4FirmwarePlugin::PX4FirmwarePlugin(void) PX4FirmwarePlugin::PX4FirmwarePlugin(void)
: _versionNotified(false) : _manualFlightMode(tr("Manual"))
, _acroFlightMode(tr("Acro"))
, _stabilizedFlightMode(tr("Stabilized"))
, _rattitudeFlightMode(tr("Rattitude"))
, _altCtlFlightMode(tr("Altitude"))
, _posCtlFlightMode(tr("Position"))
, _offboardFlightMode(tr("Offboard"))
, _readyFlightMode(tr("Ready"))
, _takeoffFlightMode(tr("Takeoff"))
, _holdFlightMode(tr("Hold"))
, _missionFlightMode(tr("Mission"))
, _rtlFlightMode(tr("Return"))
, _landingFlightMode(tr("Land"))
, _rtgsFlightMode(tr("Return to Groundstation"))
, _followMeFlightMode(tr("Follow Me"))
, _simpleFlightMode(tr("Simple"))
, _versionNotified(false)
{ {
qmlRegisterType<PX4AdvancedFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController"); qmlRegisterType<PX4AdvancedFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController");
qmlRegisterType<PX4SimpleFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController"); qmlRegisterType<PX4SimpleFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController");
@ -56,31 +55,50 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void)
struct Modes2Name { struct Modes2Name {
uint8_t main_mode; uint8_t main_mode;
uint8_t sub_mode; uint8_t sub_mode;
const char* name; ///< Name for flight mode
bool canBeSet; ///< true: Vehicle can be set to this flight mode bool canBeSet; ///< true: Vehicle can be set to this flight mode
bool fixedWing; /// fixed wing compatible bool fixedWing; /// fixed wing compatible
bool multiRotor; /// multi rotor compatible bool multiRotor; /// multi rotor compatible
}; };
static const struct Modes2Name rgModes2Name[] = { static const struct Modes2Name rgModes2Name[] = {
//main_mode sub_mode name canBeSet FW MC //main_mode sub_mode canBeSet FW MC
{ PX4_CUSTOM_MAIN_MODE_MANUAL, 0, PX4FirmwarePlugin::_manualFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_MANUAL, 0, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, PX4FirmwarePlugin::_stabilizedFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_STABILIZED, 0, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_ACRO, 0, PX4FirmwarePlugin::_acroFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_ACRO, 0, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, PX4FirmwarePlugin::_rattitudeFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_RATTITUDE, 0, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, PX4FirmwarePlugin::_altCtlFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_ALTCTL, 0, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_POSCTL, 0, PX4FirmwarePlugin::_posCtlFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_POSCTL, 0, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_SIMPLE, 0, PX4FirmwarePlugin::_simpleFlightMode, true, false, true }, { PX4_CUSTOM_MAIN_MODE_SIMPLE, 0, true, false, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4FirmwarePlugin::_holdFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4FirmwarePlugin::_missionFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4FirmwarePlugin::_rtlFlightMode, true, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTL, true, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4FirmwarePlugin::_followMeFlightMode, true, false, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, true, false, true },
{ PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, PX4FirmwarePlugin::_offboardFlightMode, true, false, true }, { PX4_CUSTOM_MAIN_MODE_OFFBOARD, 0, true, false, true },
// modes that can't be directly set by the user // modes that can't be directly set by the user
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4FirmwarePlugin::_landingFlightMode, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_LAND, false, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, PX4FirmwarePlugin::_readyFlightMode, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_READY, false, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, PX4FirmwarePlugin::_rtgsFlightMode, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_RTGS, false, true, true },
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::_takeoffFlightMode, false, true, true }, { PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, false, true, true },
};
// Must be in same order as above structure
const QString* rgModeNames[] = {
&_manualFlightMode,
&_stabilizedFlightMode,
&_acroFlightMode,
&_rattitudeFlightMode,
&_altCtlFlightMode,
&_posCtlFlightMode,
&_simpleFlightMode,
&_holdFlightMode,
&_missionFlightMode,
&_rtlFlightMode,
&_followMeFlightMode,
&_offboardFlightMode,
&_landingFlightMode,
&_readyFlightMode,
&_rtgsFlightMode,
&_takeoffFlightMode,
}; };
// Convert static information to dynamic list. This allows for plugin override class to manipulate list. // Convert static information to dynamic list. This allows for plugin override class to manipulate list.
@ -91,7 +109,7 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void)
info.main_mode = pModes2Name->main_mode; info.main_mode = pModes2Name->main_mode;
info.sub_mode = pModes2Name->sub_mode; info.sub_mode = pModes2Name->sub_mode;
info.name = pModes2Name->name; info.name = rgModeNames[i];
info.canBeSet = pModes2Name->canBeSet; info.canBeSet = pModes2Name->canBeSet;
info.fixedWing = pModes2Name->fixedWing; info.fixedWing = pModes2Name->fixedWing;
info.multiRotor = pModes2Name->multiRotor; info.multiRotor = pModes2Name->multiRotor;
@ -125,7 +143,7 @@ QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle)
bool other = !vehicle->fixedWing() && !vehicle->multiRotor(); bool other = !vehicle->fixedWing() && !vehicle->multiRotor();
if (fw || mc || other) { if (fw || mc || other) {
flightModes += info.name; flightModes += *info.name;
} }
} }
} }
@ -144,7 +162,7 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
bool found = false; bool found = false;
foreach (const FlightModeInfo_t& info, _flightModeInfoList) { foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
if (info.main_mode == px4_mode.main_mode && info.sub_mode == px4_mode.sub_mode) { if (info.main_mode == px4_mode.main_mode && info.sub_mode == px4_mode.sub_mode) {
flightMode = info.name; flightMode = *info.name;
found = true; found = true;
break; break;
} }

43
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
Unescape View File

@ -63,31 +63,11 @@ public:
QString rtlFlightMode (void) override; QString rtlFlightMode (void) override;
QString takeControlFlightMode (void) override; QString takeControlFlightMode (void) override;
// NOTE: For internal use only. Do not use in mainline QGC code.
// Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the
// names may change.
static const char* _manualFlightMode;
static const char* _acroFlightMode;
static const char* _stabilizedFlightMode;
static const char* _rattitudeFlightMode;
static const char* _altCtlFlightMode;
static const char* _posCtlFlightMode;
static const char* _offboardFlightMode;
static const char* _readyFlightMode;
static const char* _takeoffFlightMode;
static const char* _holdFlightMode;
static const char* _missionFlightMode;
static const char* _rtlFlightMode;
static const char* _landingFlightMode;
static const char* _rtgsFlightMode;
static const char* _followMeFlightMode;
static const char* _simpleFlightMode;
protected: protected:
typedef struct { typedef struct {
uint8_t main_mode; uint8_t main_mode;
uint8_t sub_mode; uint8_t sub_mode;
QString name; ///< Name for flight mode const QString* name; ///< Name for flight mode
bool canBeSet; ///< true: Vehicle can be set to this flight mode bool canBeSet; ///< true: Vehicle can be set to this flight mode
bool fixedWing; /// fixed wing compatible bool fixedWing; /// fixed wing compatible
bool multiRotor; /// multi rotor compatible bool multiRotor; /// multi rotor compatible
@ -95,6 +75,27 @@ protected:
QList<FlightModeInfo_t> _flightModeInfoList; QList<FlightModeInfo_t> _flightModeInfoList;
// Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the
// names may change.
// If plugin superclass wants to change a mode name, then set a new name for the flight mode in the superclass constructor
QString _manualFlightMode;
QString _acroFlightMode;
QString _stabilizedFlightMode;
QString _rattitudeFlightMode;
QString _altCtlFlightMode;
QString _posCtlFlightMode;
QString _offboardFlightMode;
QString _readyFlightMode;
QString _takeoffFlightMode;
QString _holdFlightMode;
QString _missionFlightMode;
QString _rtlFlightMode;
QString _landingFlightMode;
QString _rtgsFlightMode;
QString _followMeFlightMode;
QString _simpleFlightMode;
private: private:
void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message); void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);

Write Preview
Loading…
Cancel
Save