From ba486be4a9b5f48ceb41e1a1d6707ea4cd96dce0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 25 Jun 2016 21:13:30 -0700 Subject: [PATCH] Solo param usage fixes (#3663) * Deal with missing parameters on Solo * Fix trigger set --- .../APM/APMSensorsComponent.cc | 47 ++++++--- src/FactSystem/ParameterLoader.cc | 2 +- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 1 + src/Vehicle/Vehicle.cc | 10 ++ src/Vehicle/Vehicle.h | 96 ++++++++++--------- 5 files changed, 96 insertions(+), 60 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc index d57c2f3df..74bdc6a72 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.cc @@ -62,6 +62,12 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const // Accelerometer triggers triggers << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z"; + if (_autopilot->parameterExists(FactSystem::defaultComponentId, QStringLiteral("INS_USE"))) { + triggers << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3"); + triggers << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z"); + triggers << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z"); + } + return triggers; } @@ -117,20 +123,33 @@ bool APMSensorsComponent::compassSetupNeeded(void) const bool APMSensorsComponent::accelSetupNeeded(void) const { - const size_t cAccel = 3; - const size_t cOffset = 3; - QStringList rgUse; - QStringList rgOffsets[cAccel]; - - rgUse << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3"); - rgOffsets[0] << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); - rgOffsets[1] << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z"); - rgOffsets[2] << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z"); - - for (size_t i=0; igetParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) { - for (size_t j=0; jgetParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) { + QStringList rgUse; + QStringList rgOffsets; + QList rgAccels; + + // We always at a minimum test the first accel + rgOffsets << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z"); + rgAccels << rgOffsets; + rgOffsets.clear(); + + // This parameter is not available in all firmware version. Specifically missing from older Solo firmware. + if (_autopilot->parameterExists(FactSystem::defaultComponentId, QStringLiteral("INS_USE"))) { + rgUse << QStringLiteral("INS_USE") << QStringLiteral("INS_USE2") << QStringLiteral("INS_USE3"); + + // We have usage information for the remaining accels, so we can test them sa well + rgOffsets << QStringLiteral("INS_ACC2OFFS_X") << QStringLiteral("INS_ACC2OFFS_Y") << QStringLiteral("INS_ACC2OFFS_Z"); + rgAccels << rgOffsets; + rgOffsets.clear(); + + rgOffsets << QStringLiteral("INS_ACC3OFFS_X") << QStringLiteral("INS_ACC3OFFS_Y") << QStringLiteral("INS_ACC3OFFS_Z"); + rgAccels << rgOffsets; + rgOffsets.clear(); + } + + for (int i=0; igetParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) { + for (int j=0; jgetParameterFact(FactSystem::defaultComponentId, rgAccels[i][j])->rawValue().toFloat() == 0.0f) { return true; } } diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index 19113fcbf..e2c6d342f 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -1196,7 +1196,7 @@ QString ParameterLoader::_remapParamNameToVersion(const QString& paramName) const FirmwarePlugin::remapParamNameMinorVersionRemapMap_t& remapMinorVersion = majorVersionRemap[majorVersion]; - // We must map from the highest known minor version to one above the vehicle's minor version + // We must map backwards from the highest known minor version to one above the vehicle's minor version for (int currentMinorVersion=_vehicle->firmwarePlugin()->remapParamNameHigestMinorVersionNumber(majorVersion); currentMinorVersion>minorVersion; currentMinorVersion--) { if (remapMinorVersion.contains(currentMinorVersion)) { diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 74adc1751..66e4bc19f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -373,6 +373,7 @@ bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m if (messageText.contains(APM_SOLO_REXP)) { qDebug() << "Found Solo"; + vehicle->setSoloFirmware(true); // Fix up severity _setInfoSeverity(message); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index cbc818b8d..dfb150e2f 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -67,6 +67,8 @@ Vehicle::Vehicle(LinkInterface* link, , _vehicleType(vehicleType) , _firmwarePlugin(NULL) , _autopilotPlugin(NULL) + , _mavlink(NULL) + , _soloFirmware(false) , _joystickMode(JoystickModeRC) , _joystickEnabled(false) , _uas(NULL) @@ -1697,6 +1699,14 @@ int Vehicle::defaultComponentId(void) return _parameterLoader->defaultComponenentId(); } +void Vehicle::setSoloFirmware(bool soloFirmware) +{ + if (soloFirmware != _soloFirmware) { + _soloFirmware = soloFirmware; + emit soloFirmwareChanged(soloFirmware); + } +} + const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2c7c4ec31..52e89fb1b 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -231,51 +231,52 @@ public: ~Vehicle(); - Q_PROPERTY(int id READ id CONSTANT) - Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) - Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) - Q_PROPERTY(bool coordinateValid READ coordinateValid NOTIFY coordinateValidChanged) - Q_PROPERTY(bool homePositionAvailable READ homePositionAvailable NOTIFY homePositionAvailableChanged) - Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged) - Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged) - Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT) - Q_PROPERTY(QStringList flightModes READ flightModes CONSTANT) - Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) - Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) - Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) - Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) - Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) - Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) - Q_PROPERTY(QString currentState READ currentState NOTIFY currentStateChanged) - Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) - Q_PROPERTY(bool messageTypeNormal READ messageTypeNormal NOTIFY messageTypeChanged) - Q_PROPERTY(bool messageTypeWarning READ messageTypeWarning NOTIFY messageTypeChanged) - Q_PROPERTY(bool messageTypeError READ messageTypeError NOTIFY messageTypeChanged) - Q_PROPERTY(int newMessageCount READ newMessageCount NOTIFY newMessageCountChanged) - Q_PROPERTY(int messageCount READ messageCount NOTIFY messageCountChanged) - Q_PROPERTY(QString formatedMessages READ formatedMessages NOTIFY formatedMessagesChanged) - Q_PROPERTY(QString formatedMessage READ formatedMessage NOTIFY formatedMessageChanged) - Q_PROPERTY(QString latestError READ latestError NOTIFY latestErrorChanged) - Q_PROPERTY(int joystickMode READ joystickMode WRITE setJoystickMode NOTIFY joystickModeChanged) - Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT) - Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) - Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) - Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) - Q_PROPERTY(int rcRSSI READ rcRSSI NOTIFY rcRSSIChanged) - Q_PROPERTY(bool px4Firmware READ px4Firmware CONSTANT) - Q_PROPERTY(bool apmFirmware READ apmFirmware CONSTANT) - Q_PROPERTY(bool genericFirmware READ genericFirmware CONSTANT) - Q_PROPERTY(bool connectionLost READ connectionLost NOTIFY connectionLostChanged) - Q_PROPERTY(bool connectionLostEnabled READ connectionLostEnabled WRITE setConnectionLostEnabled NOTIFY connectionLostEnabledChanged) - Q_PROPERTY(uint messagesReceived READ messagesReceived NOTIFY messagesReceivedChanged) - Q_PROPERTY(uint messagesSent READ messagesSent NOTIFY messagesSentChanged) - Q_PROPERTY(uint messagesLost READ messagesLost NOTIFY messagesLostChanged) - Q_PROPERTY(bool fixedWing READ fixedWing CONSTANT) - Q_PROPERTY(bool multiRotor READ multiRotor CONSTANT) - Q_PROPERTY(bool vtol READ vtol CONSTANT) - Q_PROPERTY(bool rover READ rover CONSTANT) - Q_PROPERTY(bool autoDisconnect MEMBER _autoDisconnect NOTIFY autoDisconnectChanged) - Q_PROPERTY(QString prearmError READ prearmError WRITE setPrearmError NOTIFY prearmErrorChanged) + Q_PROPERTY(int id READ id CONSTANT) + Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) + Q_PROPERTY(bool coordinateValid READ coordinateValid NOTIFY coordinateValidChanged) + Q_PROPERTY(bool homePositionAvailable READ homePositionAvailable NOTIFY homePositionAvailableChanged) + Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged) + Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged) + Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT) + Q_PROPERTY(QStringList flightModes READ flightModes CONSTANT) + Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) + Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) + Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) + Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) + Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) + Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) + Q_PROPERTY(QString currentState READ currentState NOTIFY currentStateChanged) + Q_PROPERTY(bool messageTypeNone READ messageTypeNone NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeNormal READ messageTypeNormal NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeWarning READ messageTypeWarning NOTIFY messageTypeChanged) + Q_PROPERTY(bool messageTypeError READ messageTypeError NOTIFY messageTypeChanged) + Q_PROPERTY(int newMessageCount READ newMessageCount NOTIFY newMessageCountChanged) + Q_PROPERTY(int messageCount READ messageCount NOTIFY messageCountChanged) + Q_PROPERTY(QString formatedMessages READ formatedMessages NOTIFY formatedMessagesChanged) + Q_PROPERTY(QString formatedMessage READ formatedMessage NOTIFY formatedMessageChanged) + Q_PROPERTY(QString latestError READ latestError NOTIFY latestErrorChanged) + Q_PROPERTY(int joystickMode READ joystickMode WRITE setJoystickMode NOTIFY joystickModeChanged) + Q_PROPERTY(QStringList joystickModes READ joystickModes CONSTANT) + Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) + Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) + Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) + Q_PROPERTY(int rcRSSI READ rcRSSI NOTIFY rcRSSIChanged) + Q_PROPERTY(bool px4Firmware READ px4Firmware CONSTANT) + Q_PROPERTY(bool apmFirmware READ apmFirmware CONSTANT) + Q_PROPERTY(bool soloFirmware READ soloFirmware WRITE setSoloFirmware NOTIFY soloFirmwareChanged) + Q_PROPERTY(bool genericFirmware READ genericFirmware CONSTANT) + Q_PROPERTY(bool connectionLost READ connectionLost NOTIFY connectionLostChanged) + Q_PROPERTY(bool connectionLostEnabled READ connectionLostEnabled WRITE setConnectionLostEnabled NOTIFY connectionLostEnabledChanged) + Q_PROPERTY(uint messagesReceived READ messagesReceived NOTIFY messagesReceivedChanged) + Q_PROPERTY(uint messagesSent READ messagesSent NOTIFY messagesSentChanged) + Q_PROPERTY(uint messagesLost READ messagesLost NOTIFY messagesLostChanged) + Q_PROPERTY(bool fixedWing READ fixedWing CONSTANT) + Q_PROPERTY(bool multiRotor READ multiRotor CONSTANT) + Q_PROPERTY(bool vtol READ vtol CONSTANT) + Q_PROPERTY(bool rover READ rover CONSTANT) + Q_PROPERTY(bool autoDisconnect MEMBER _autoDisconnect NOTIFY autoDisconnectChanged) + Q_PROPERTY(QString prearmError READ prearmError WRITE setPrearmError NOTIFY prearmErrorChanged) /// true: Vehicle is flying, false: Vehicle is on ground Q_PROPERTY(bool flying READ flying WRITE setFlying NOTIFY flyingChanged) @@ -529,6 +530,9 @@ public: void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType = FIRMWARE_VERSION_TYPE_OFFICIAL); static const int versionNotSetValue = -1; + bool soloFirmware(void) const { return _soloFirmware; } + void setSoloFirmware(bool soloFirmware); + int defaultComponentId(void); public slots: @@ -556,6 +560,7 @@ signals: void guidedModeChanged(bool guidedMode); void prearmErrorChanged(const QString& prearmError); void commandLongAck(uint8_t compID, uint16_t command, uint8_t result); + void soloFirmwareChanged(bool soloFirmware); void messagesReceivedChanged (); void messagesSentChanged (); @@ -656,6 +661,7 @@ private: FirmwarePlugin* _firmwarePlugin; AutoPilotPlugin* _autopilotPlugin; MAVLinkProtocol* _mavlink; + bool _soloFirmware; QList _links; -- 2.22.0