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

All third-party flight mode overrides

QGC4.4
Don Gagne 8 years ago
parent
8d5151b896
commit
9e29a3b943
2 changed files with 70 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 82
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  2. 12
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

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

@ -26,15 +26,6 @@ @@ -26,15 +26,6 @@
#include "px4_custom_mode.h"
struct Modes2Name {
uint8_t main_mode;
uint8_t sub_mode;
const char* name; ///< Name for flight mode
bool canBeSet; ///< true: Vehicle can be set to this flight mode
bool fixedWing; /// fixed wing compatible
bool multiRotor; /// multi rotor compatible
};
const char* PX4FirmwarePlugin::_manualFlightMode = "Manual";
const char* PX4FirmwarePlugin::_altCtlFlightMode = "Altitude";
const char* PX4FirmwarePlugin::_posCtlFlightMode = "Position";
@ -52,7 +43,24 @@ const char* PX4FirmwarePlugin::_rtgsFlightMode = "Return to Groundstation @@ -52,7 +43,24 @@ const char* PX4FirmwarePlugin::_rtgsFlightMode = "Return to Groundstation
const char* PX4FirmwarePlugin::_readyFlightMode = "Ready";
const char* PX4FirmwarePlugin::_simpleFlightMode = "Simple";
/// Tranlates from PX4 custom modes to flight mode names
PX4FirmwarePlugin::PX4FirmwarePlugin(void)
: _versionNotified(false)
{
qmlRegisterType<PX4AdvancedFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController");
qmlRegisterType<PX4SimpleFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController");
qmlRegisterType<AirframeComponentController> ("QGroundControl.Controllers", 1, 0, "AirframeComponentController");
qmlRegisterType<SensorsComponentController> ("QGroundControl.Controllers", 1, 0, "SensorsComponentController");
qmlRegisterType<PowerComponentController> ("QGroundControl.Controllers", 1, 0, "PowerComponentController");
qmlRegisterType<RadioComponentController> ("QGroundControl.Controllers", 1, 0, "RadioComponentController");
struct Modes2Name {
uint8_t main_mode;
uint8_t sub_mode;
const char* name; ///< Name for flight mode
bool canBeSet; ///< true: Vehicle can be set to this flight mode
bool fixedWing; /// fixed wing compatible
bool multiRotor; /// multi rotor compatible
};
static const struct Modes2Name rgModes2Name[] = {
//main_mode sub_mode name canBeSet FW MC
@ -75,15 +83,21 @@ static const struct Modes2Name rgModes2Name[] = { @@ -75,15 +83,21 @@ static const struct Modes2Name rgModes2Name[] = {
{ PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, PX4FirmwarePlugin::_takeoffFlightMode, false, true, true },
};
PX4FirmwarePlugin::PX4FirmwarePlugin(void)
: _versionNotified(false)
{
qmlRegisterType<PX4AdvancedFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController");
qmlRegisterType<PX4SimpleFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController");
qmlRegisterType<AirframeComponentController> ("QGroundControl.Controllers", 1, 0, "AirframeComponentController");
qmlRegisterType<SensorsComponentController> ("QGroundControl.Controllers", 1, 0, "SensorsComponentController");
qmlRegisterType<PowerComponentController> ("QGroundControl.Controllers", 1, 0, "PowerComponentController");
qmlRegisterType<RadioComponentController> ("QGroundControl.Controllers", 1, 0, "RadioComponentController");
// Convert static information to dynamic list. This allows for plugin override class to manipulate list.
for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
const struct Modes2Name* pModes2Name = &rgModes2Name[i];
FlightModeInfo_t info;
info.main_mode = pModes2Name->main_mode;
info.sub_mode = pModes2Name->sub_mode;
info.name = pModes2Name->name;
info.canBeSet = pModes2Name->canBeSet;
info.fixedWing = pModes2Name->fixedWing;
info.multiRotor = pModes2Name->multiRotor;
_flightModeInfoList.append(info);
}
}
AutoPilotPlugin* PX4FirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
@ -102,18 +116,16 @@ QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle) @@ -102,18 +116,16 @@ QStringList PX4FirmwarePlugin::flightModes(Vehicle* vehicle)
{
QStringList flightModes;
for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
const struct Modes2Name* pModes2Name = &rgModes2Name[i];
if (pModes2Name->canBeSet) {
bool fw = (vehicle->fixedWing() && pModes2Name->fixedWing);
bool mc = (vehicle->multiRotor() && pModes2Name->multiRotor);
foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
if (info.canBeSet) {
bool fw = (vehicle->fixedWing() && info.fixedWing);
bool mc = (vehicle->multiRotor() && info.multiRotor);
// show all modes for generic, vtol, etc
bool other = !vehicle->fixedWing() && !vehicle->multiRotor();
if (fw || mc || other) {
flightModes += pModes2Name->name;
flightModes += info.name;
}
}
}
@ -130,11 +142,9 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c @@ -130,11 +142,9 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
px4_mode.data = custom_mode;
bool found = false;
for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
const struct Modes2Name* pModes2Name = &rgModes2Name[i];
if (pModes2Name->main_mode == px4_mode.main_mode && pModes2Name->sub_mode == px4_mode.sub_mode) {
flightMode = pModes2Name->name;
foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
if (info.main_mode == px4_mode.main_mode && info.sub_mode == px4_mode.sub_mode) {
flightMode = info.name;
found = true;
break;
}
@ -156,15 +166,13 @@ bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_m @@ -156,15 +166,13 @@ bool PX4FirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_m
*custom_mode = 0;
bool found = false;
for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
const struct Modes2Name* pModes2Name = &rgModes2Name[i];
if (flightMode.compare(pModes2Name->name, Qt::CaseInsensitive) == 0) {
foreach (const FlightModeInfo_t& info, _flightModeInfoList) {
if (flightMode.compare(info.name, Qt::CaseInsensitive) == 0) {
union px4_custom_mode px4_mode;
px4_mode.data = 0;
px4_mode.main_mode = pModes2Name->main_mode;
px4_mode.sub_mode = pModes2Name->sub_mode;
px4_mode.main_mode = info.main_mode;
px4_mode.sub_mode = info.sub_mode;
*base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
*custom_mode = px4_mode.data;

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

@ -83,6 +83,18 @@ public: @@ -83,6 +83,18 @@ public:
static const char* _followMeFlightMode;
static const char* _simpleFlightMode;
protected:
typedef struct {
uint8_t main_mode;
uint8_t sub_mode;
QString name; ///< Name for flight mode
bool canBeSet; ///< true: Vehicle can be set to this flight mode
bool fixedWing; /// fixed wing compatible
bool multiRotor; /// multi rotor compatible
} FlightModeInfo_t;
QList<FlightModeInfo_t> _flightModeInfoList;
private:
void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);

Write Preview
Loading…
Cancel
Save