Commit ba486be4 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Solo param usage fixes (#3663)

* Deal with missing parameters on Solo

* Fix trigger set
parent f5ae0a2e
......@@ -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; 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;
}
}
......
......@@ -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)) {
......
......@@ -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);
......
......@@ -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";
......
This diff is collapsed.
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