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

Solo param usage fixes (#3663) 

* Deal with missing parameters on Solo

* Fix trigger set
QGC4.4
Don Gagne 9 years ago committed by GitHub
parent
f5ae0a2eb1
commit
ba486be4a9
5 changed files with 96 additions and 60 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. 47
      src/AutoPilotPlugins/APM/APMSensorsComponent.cc
  2. 2
      src/FactSystem/ParameterLoader.cc
  3. 1
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  4. 10
      src/Vehicle/Vehicle.cc
  5. 96
      src/Vehicle/Vehicle.h

47
src/AutoPilotPlugins/APM/APMSensorsComponent.cc
Unescape View File

@ -62,6 +62,12 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const @@ -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 @@ -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; i<cAccel; i++) {
if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) {
for (size_t j=0; j<cOffset; j++) {
if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
QStringList rgUse;
QStringList rgOffsets;
QList<QStringList> 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; i<rgAccels.count(); i++) {
if (rgUse.count() == 0 || _autopilot->getParameterFact(FactSystem::defaultComponentId, rgUse[i])->rawValue().toInt() != 0) {
for (int j=0; j<rgAccels[0].count(); j++) {
if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgAccels[i][j])->rawValue().toFloat() == 0.0f) {
return true;
}
}

2
src/FactSystem/ParameterLoader.cc
Unescape View File

@ -1196,7 +1196,7 @@ QString ParameterLoader::_remapParamNameToVersion(const QString& paramName) @@ -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)) {

1
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
Unescape View File

@ -373,6 +373,7 @@ bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m @@ -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);

10
src/Vehicle/Vehicle.cc
Unescape View File

@ -67,6 +67,8 @@ Vehicle::Vehicle(LinkInterface* link, @@ -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) @@ -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";

96
src/Vehicle/Vehicle.h
Unescape View File

@ -231,51 +231,52 @@ public: @@ -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: @@ -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: @@ -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: @@ -656,6 +661,7 @@ private:
FirmwarePlugin* _firmwarePlugin;
AutoPilotPlugin* _autopilotPlugin;
MAVLinkProtocol* _mavlink;
bool _soloFirmware;
QList<LinkInterface*> _links;

Write Preview
Loading…
Cancel
Save