From 46cfbbe9de90c59fdadceefb985e5eaeeeea02f7 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 10 Oct 2015 09:46:06 -0700 Subject: [PATCH] Add support for MAV_TYPE specific FirmwarePlugin --- QGCApplication.pro | 2 + src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 1 - src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 12 +-- .../APM/ArduCopterFirmwarePlugin.cc | 81 +++++++++++++++++++ .../APM/ArduCopterFirmwarePlugin.h | 53 ++++++++++++ src/FirmwarePlugin/FirmwarePluginManager.cc | 41 +++++++++- src/FirmwarePlugin/FirmwarePluginManager.h | 3 +- src/QGCApplication.cc | 6 +- src/Vehicle/MultiVehicleManager.cc | 2 +- src/Vehicle/Vehicle.cc | 5 +- src/Vehicle/Vehicle.h | 4 +- src/uas/UAS.cc | 2 +- 12 files changed, 194 insertions(+), 18 deletions(-) create mode 100644 src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc create mode 100644 src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h diff --git a/QGCApplication.pro b/QGCApplication.pro index 7d91f27b9..46557fc9d 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -577,6 +577,7 @@ HEADERS+= \ src/FirmwarePlugin/FirmwarePluginManager.h \ src/FirmwarePlugin/FirmwarePlugin.h \ src/FirmwarePlugin/APM/APMFirmwarePlugin.h \ + src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h \ src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h \ src/Vehicle/MultiVehicleManager.h \ @@ -614,6 +615,7 @@ SOURCES += \ src/AutoPilotPlugins/PX4/SensorsComponent.cc \ src/AutoPilotPlugins/PX4/SensorsComponentController.cc \ src/FirmwarePlugin/APM/APMFirmwarePlugin.cc \ + src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc \ src/FirmwarePlugin/FirmwarePluginManager.cc \ src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc \ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc \ diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 5ee2a5594..7b359e2a9 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -30,7 +30,6 @@ #include -IMPLEMENT_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin) QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog") static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)"); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 67630e326..7d0c348d5 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -28,8 +28,9 @@ #define APMFirmwarePlugin_H #include "FirmwarePlugin.h" +#include "QGCLoggingCategory.h" -Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLogsLog) +Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLog) class APMFirmwareVersion { @@ -54,11 +55,10 @@ private: int _patch; }; +/// This is the base class for all stack specific APM firmware plugins class APMFirmwarePlugin : public FirmwarePlugin { Q_OBJECT - - DECLARE_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin) public: // Overrides from FirmwarePlugin @@ -71,9 +71,11 @@ public: virtual void adjustMavlinkMessage(mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); -private: - /// All access to singleton is through AutoPilotPluginManager::instance +protected: + /// All access to singleton is through stack specific implementation APMFirmwarePlugin(QObject* parent = NULL); + +private: void _adjustSeverity(mavlink_message_t* message) const; static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion); diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc new file mode 100644 index 000000000..7d51a871a --- /dev/null +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -0,0 +1,81 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2015 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#include "ArduCopterFirmwarePlugin.h" +#include "Generic/GenericFirmwarePlugin.h" +#include "QGCMAVLink.h" + +#include + +IMPLEMENT_QGC_SINGLETON(ArduCopterFirmwarePlugin, ArduCopterFirmwarePlugin) + +ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(QObject* parent) : + APMFirmwarePlugin(parent) +{ + +} + +bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities) +{ + Q_UNUSED(capabilities); + + // FIXME: No capabilitis yet supported + + return false; +} + +QList ArduCopterFirmwarePlugin::componentsForVehicle(AutoPilotPlugin* vehicle) +{ + Q_UNUSED(vehicle); + + return QList(); +} + +QStringList ArduCopterFirmwarePlugin::flightModes(void) +{ + // FIXME: NYI + + qWarning() << "ArduCopterFirmwarePlugin::flightModes not supported"; + + return QStringList(); +} + +QString ArduCopterFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) +{ + // FIXME: Nothing more than generic support yet + return GenericFirmwarePlugin::instance()->flightMode(base_mode, custom_mode); +} + +bool ArduCopterFirmwarePlugin::setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) +{ + Q_UNUSED(flightMode); + Q_UNUSED(base_mode); + Q_UNUSED(custom_mode); + + qWarning() << "ArduCopterFirmwarePlugin::setFlightMode called on base class, not supported"; + + return false; +} diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h new file mode 100644 index 000000000..db930bc76 --- /dev/null +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -0,0 +1,53 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#ifndef ArduCopterFirmwarePlugin_H +#define ArduCopterFirmwarePlugin_H + +#include "APMFirmwarePlugin.h" + +class ArduCopterFirmwarePlugin : public APMFirmwarePlugin +{ + Q_OBJECT + + DECLARE_QGC_SINGLETON(ArduCopterFirmwarePlugin, ArduCopterFirmwarePlugin) + +public: + // Overrides from FirmwarePlugin + virtual bool isCapable(FirmwareCapabilities capabilities); + virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); + virtual QStringList flightModes(void); + virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode); + virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); + +protected: + /// All access to singleton is through instance() + ArduCopterFirmwarePlugin(QObject* parent = NULL); + +private: +}; + +#endif diff --git a/src/FirmwarePlugin/FirmwarePluginManager.cc b/src/FirmwarePlugin/FirmwarePluginManager.cc index b64edde08..1c00ecbde 100644 --- a/src/FirmwarePlugin/FirmwarePluginManager.cc +++ b/src/FirmwarePlugin/FirmwarePluginManager.cc @@ -26,7 +26,7 @@ #include "FirmwarePluginManager.h" #include "Generic/GenericFirmwarePlugin.h" -#include "APM/APMFirmwarePlugin.h" +#include "APM/ArduCopterFirmwarePlugin.h" #include "PX4/PX4FirmwarePlugin.h" IMPLEMENT_QGC_SINGLETON(FirmwarePluginManager, FirmwarePluginManager) @@ -42,11 +42,46 @@ FirmwarePluginManager::~FirmwarePluginManager() } -FirmwarePlugin* FirmwarePluginManager::firmwarePluginForAutopilot(MAV_AUTOPILOT autopilotType) +FirmwarePlugin* FirmwarePluginManager::firmwarePluginForAutopilot(MAV_AUTOPILOT autopilotType, MAV_TYPE vehicleType) { switch (autopilotType) { case MAV_AUTOPILOT_ARDUPILOTMEGA: - return APMFirmwarePlugin::instance(); + switch (vehicleType) { + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_TRICOPTER: + return ArduCopterFirmwarePlugin::instance(); + break; + // FIXME: The remainder of these need to be correctly assigned and new plugin classes created as needed. + // Once done, the unused cases can be removed and just the fall back default: left + case MAV_TYPE_FIXED_WING: + case MAV_TYPE_GENERIC: + case MAV_TYPE_COAXIAL: + case MAV_TYPE_HELICOPTER: + case MAV_TYPE_ANTENNA_TRACKER: + case MAV_TYPE_GCS: + case MAV_TYPE_AIRSHIP: + case MAV_TYPE_FREE_BALLOON: + case MAV_TYPE_ROCKET: + case MAV_TYPE_GROUND_ROVER: + case MAV_TYPE_SURFACE_BOAT: + case MAV_TYPE_SUBMARINE: + case MAV_TYPE_FLAPPING_WING: + case MAV_TYPE_KITE: + case MAV_TYPE_ONBOARD_CONTROLLER: + case MAV_TYPE_VTOL_DUOROTOR: + case MAV_TYPE_VTOL_QUADROTOR: + case MAV_TYPE_VTOL_TILTROTOR: + case MAV_TYPE_VTOL_RESERVED2: + case MAV_TYPE_VTOL_RESERVED3: + case MAV_TYPE_VTOL_RESERVED4: + case MAV_TYPE_VTOL_RESERVED5: + case MAV_TYPE_GIMBAL: + default: + return GenericFirmwarePlugin::instance(); + break; + } case MAV_AUTOPILOT_PX4: return PX4FirmwarePlugin::instance(); default: diff --git a/src/FirmwarePlugin/FirmwarePluginManager.h b/src/FirmwarePlugin/FirmwarePluginManager.h index baff92a4b..1d4153545 100644 --- a/src/FirmwarePlugin/FirmwarePluginManager.h +++ b/src/FirmwarePlugin/FirmwarePluginManager.h @@ -44,8 +44,9 @@ class FirmwarePluginManager : public QGCSingleton public: /// Returns appropriate plugin for autopilot type. /// @param autopilotType Type of autopilot to return plugin for. + /// @param vehicleType Vehicle type of autopilot to return plugin for. /// @return Singleton FirmwarePlugin instance for the specified MAV_AUTOPILOT. - FirmwarePlugin* firmwarePluginForAutopilot(MAV_AUTOPILOT autopilotType); + FirmwarePlugin* firmwarePluginForAutopilot(MAV_AUTOPILOT autopilotType, MAV_TYPE vehicleType); private: /// All access to singleton is through FirmwarePluginManager::instance diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 7c32d613a..d98e021f4 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -74,7 +74,7 @@ #include "FirmwarePluginManager.h" #include "MultiVehicleManager.h" #include "Generic/GenericFirmwarePlugin.h" -#include "APM/APMFirmwarePlugin.h" +#include "APM/ArduCopterFirmwarePlugin.h" #include "PX4/PX4FirmwarePlugin.h" #include "Vehicle.h" #include "MavlinkQmlSingleton.h" @@ -597,7 +597,7 @@ void QGCApplication::_createSingletons(void) // No dependencies firmwarePlugin = PX4FirmwarePlugin::_createSingleton(); - firmwarePlugin = APMFirmwarePlugin::_createSingleton(); + firmwarePlugin = ArduCopterFirmwarePlugin::_createSingleton(); // No dependencies FirmwarePluginManager* firmwarePluginManager = FirmwarePluginManager::_createSingleton(); @@ -673,7 +673,7 @@ void QGCApplication::_destroySingletons(void) FirmwarePluginManager::_deleteSingleton(); GenericFirmwarePlugin::_deleteSingleton(); PX4FirmwarePlugin::_deleteSingleton(); - APMFirmwarePlugin::_deleteSingleton(); + ArduCopterFirmwarePlugin::_deleteSingleton(); HomePositionManager::_deleteSingleton(); FlightMapSettings::_deleteSingleton(); } diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 136a6b512..4ff9a53ab 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -64,7 +64,7 @@ bool MultiVehicleManager::notifyHeartbeatInfo(LinkInterface* link, int vehicleId return false; } - Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)heartbeat.autopilot); + Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)heartbeat.autopilot, (MAV_TYPE)heartbeat.type); if (!vehicle) { qWarning() << "New Vehicle allocation failed"; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 707d0bfba..78d4377e0 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -42,10 +42,11 @@ const char* Vehicle::_settingsGroup = "Vehicle%1"; // %1 re const char* Vehicle::_joystickModeSettingsKey = "JoystickMode"; const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled"; -Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) +Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType) : _id(vehicleId) , _active(false) , _firmwareType(firmwareType) + , _vehicleType(vehicleType) , _firmwarePlugin(NULL) , _autopilotPlugin(NULL) , _joystickMode(JoystickModeRC) @@ -106,7 +107,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude); connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude); - _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(firmwareType); + _firmwarePlugin = FirmwarePluginManager::instance()->firmwarePluginForAutopilot(_firmwareType, _vehicleType); _autopilotPlugin = AutoPilotPluginManager::instance()->newAutopilotPluginForVehicle(this); connect(_autopilotPlugin, &AutoPilotPlugin::missingParametersChanged, this, &Vehicle::missingParametersChanged); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 44cd75d19..1e29cfcb3 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -51,7 +51,7 @@ class Vehicle : public QObject Q_OBJECT public: - Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType); + Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType); ~Vehicle(); Q_PROPERTY(int id READ id CONSTANT) @@ -147,6 +147,7 @@ public: // Property accesors int id(void) { return _id; } MAV_AUTOPILOT firmwareType(void) { return _firmwareType; } + MAV_TYPE vehicleType(void) { return _vehicleType; } /// Sends this specified message to all links accociated with this vehicle void sendMessage(mavlink_message_t message); @@ -344,6 +345,7 @@ private: bool _active; MAV_AUTOPILOT _firmwareType; + MAV_TYPE _vehicleType; FirmwarePlugin* _firmwarePlugin; AutoPilotPlugin* _autopilotPlugin; MAVLinkProtocol* _mavlink; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 861b7b4e7..c4185c0c3 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -388,7 +388,7 @@ void UAS::receiveMessage(mavlink_message_t message) bool statechanged = false; bool modechanged = false; - QString audiomodeText = FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)autopilot)->flightMode(state.base_mode, state.custom_mode); + QString audiomodeText = FirmwarePluginManager::instance()->firmwarePluginForAutopilot((MAV_AUTOPILOT)state.autopilot, (MAV_TYPE)state.type)->flightMode(state.base_mode, state.custom_mode); if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) { -- 2.22.0