Commit b70e1880 authored by Don Gagne's avatar Don Gagne

Correctly place firmware plugin instance data with Vehicle

parent 22f6b1a4
......@@ -139,9 +139,15 @@ QString APMCustomMode::modeString() const
return mode;
}
APMFirmwarePluginInstanceData::APMFirmwarePluginInstanceData(QObject* parent)
: QObject(parent)
, textSeverityAdjustmentNeeded(false)
{
}
APMFirmwarePlugin::APMFirmwarePlugin(void)
: _coaxialMotors(false)
, _textSeverityAdjustmentNeeded(false)
{
qmlRegisterType<APMFlightModesComponentController> ("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController");
qmlRegisterType<APMAirframeComponentController> ("QGroundControl.Controllers", 1, 0, "APMAirframeComponentController");
......@@ -321,6 +327,7 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface*
bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message)
{
QString messageText;
APMFirmwarePluginInstanceData* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
mavlink_statustext_t statusText;
mavlink_msg_statustext_decode(message, &statusText);
......@@ -334,7 +341,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_SUB_REXP)) {
// found version string
APMFirmwareVersion firmwareVersion(messageText);
_textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion);
instanceData->textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion);
vehicle->setFirmwareVersion(firmwareVersion.majorNumber(), firmwareVersion.minorNumber(), firmwareVersion.patchNumber());
......@@ -378,7 +385,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
}
// adjust mesasge if needed
if (_textSeverityAdjustmentNeeded) {
if (instanceData->textSeverityAdjustmentNeeded) {
_adjustSeverity(message);
}
......@@ -416,10 +423,10 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
if (messageText.startsWith("PreArm")) {
// ArduPilot PreArm messages can come across very frequently especially on Solo, which seems to send them once a second.
// Filter them out if they come too quickly.
if (_noisyPrearmMap.contains(messageText) && _noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
if (instanceData->noisyPrearmMap.contains(messageText) && instanceData->noisyPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
return false;
}
_noisyPrearmMap[messageText] = QTime::currentTime();
instanceData->noisyPrearmMap[messageText] = QTime::currentTime();
vehicle->setPrearmError(messageText);
}
......@@ -578,6 +585,8 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes
void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData);
if (vehicle->isOfflineEditingVehicle()) {
switch (vehicle->vehicleType()) {
case MAV_TYPE_QUADROTOR:
......
......@@ -120,13 +120,24 @@ private:
void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message);
void _soloVideoHandshake(Vehicle* vehicle);
bool _textSeverityAdjustmentNeeded;
// Any instance data here must be global to all vehicles
// Vehicle specific data should go into APMFirmwarePluginInstanceData
QList<APMCustomMode> _supportedModes;
QMap<QString, QTime> _noisyPrearmMap;
static const char* _artooIP;
static const int _artooVideoHandshakePort;
};
class APMFirmwarePluginInstanceData : public QObject
{
Q_OBJECT
public:
APMFirmwarePluginInstanceData(QObject* parent = NULL);
bool textSeverityAdjustmentNeeded;
QMap<QString, QTime> noisyPrearmMap;
};
#endif
......@@ -26,6 +26,13 @@
#include "px4_custom_mode.h"
PX4FirmwarePluginInstanceData::PX4FirmwarePluginInstanceData(QObject* parent)
: QObject(parent)
, versionNotified(false)
{
}
PX4FirmwarePlugin::PX4FirmwarePlugin(void)
: _manualFlightMode(tr("Manual"))
, _acroFlightMode(tr("Acro"))
......@@ -43,7 +50,6 @@ PX4FirmwarePlugin::PX4FirmwarePlugin(void)
, _rtgsFlightMode(tr("Return to Groundstation"))
, _followMeFlightMode(tr("Follow Me"))
, _simpleFlightMode(tr("Simple"))
, _versionNotified(false)
{
qmlRegisterType<PX4AdvancedFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4AdvancedFlightModesController");
qmlRegisterType<PX4SimpleFlightModesController> ("QGroundControl.Controllers", 1, 0, "PX4SimpleFlightModesController");
......@@ -229,9 +235,7 @@ bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities c
void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle)
{
Q_UNUSED(vehicle);
// PX4 Flight Stack doesn't need to do any extra work
vehicle->setFirmwarePluginInstanceData(new PX4FirmwarePluginInstanceData);
}
bool PX4FirmwarePlugin::sendHomePositionToVehicle(void)
......@@ -500,7 +504,8 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag
{
Q_UNUSED(vehicle);
if (!_versionNotified) {
PX4FirmwarePluginInstanceData* instanceData = qobject_cast<PX4FirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
if (!instanceData->versionNotified) {
bool notifyUser = false;
int supportedMajorVersion = 1;
int supportedMinorVersion = 4;
......@@ -530,7 +535,7 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag
}
if (notifyUser) {
_versionNotified = true;
instanceData->versionNotified = true;
qgcApp()->showMessage(QString("QGroundControl supports PX4 Pro firmware Version %1.%2.%3 and above. You are using a version prior to that which will lead to unpredictable results. Please upgrade your firmware.").arg(supportedMajorVersion).arg(supportedMinorVersion).arg(supportedPatchVersion));
}
}
......
......@@ -108,7 +108,18 @@ private slots:
private:
void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);
bool _versionNotified; ///< true: user notified over version issue
// Any instance data here must be global to all vehicles
// Vehicle specific data should go into PX4FirmwarePluginInstanceData
};
class PX4FirmwarePluginInstanceData : public QObject
{
Q_OBJECT
public:
PX4FirmwarePluginInstanceData(QObject* parent = NULL);
bool versionNotified; ///< true: user notified over version issue
};
#endif
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment