APMSensorsComponent.cc 4.16 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

Don Gagne's avatar
Don Gagne committed
10 11 12 13

#include "APMSensorsComponent.h"
#include "APMAutoPilotPlugin.h"
#include "APMSensorsComponentController.h"
14
#include "APMAirframeComponent.h"
15
#include "ParameterManager.h"
Don Gagne's avatar
Don Gagne committed
16 17 18 19

// These two list must be kept in sync

APMSensorsComponent::APMSensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) :
20
    VehicleComponent(vehicle, autopilot, parent),
Don Gagne's avatar
Don Gagne committed
21 22 23 24 25 26 27 28 29 30 31 32
    _name(tr("Sensors"))
{

}

QString APMSensorsComponent::name(void) const
{
    return _name;
}

QString APMSensorsComponent::description(void) const
{
33
    return tr("Sensors Setup is used to calibrate the sensors within your vehicle.");
Don Gagne's avatar
Don Gagne committed
34 35 36 37 38 39 40 41 42 43 44 45 46 47
}

QString APMSensorsComponent::iconResource(void) const
{
    return "/qmlimages/SensorsComponentIcon.png";
}

bool APMSensorsComponent::requiresSetup(void) const
{
    return true;
}

bool APMSensorsComponent::setupComplete(void) const
{
48
    return !compassSetupNeeded() && !accelSetupNeeded();
Don Gagne's avatar
Don Gagne committed
49 50 51 52 53 54
}

QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const
{
    QStringList triggers;
    
55 56
    // Compass triggers
    triggers << "COMPASS_DEV_ID" << "COMPASS_DEV_ID2" << "COMPASS_DEV_ID3"
57
             << "COMPASS_USE" << "COMPASS_USE2" << "COMPASS_USE3"
58 59 60 61
             << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X"
             << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" << "COMPASS_OFS2_X"
             << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X";

Don Gagne's avatar
Don Gagne committed
62
    // Accelerometer triggers
Don Gagne's avatar
Don Gagne committed
63
    triggers << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z";
Don Gagne's avatar
Don Gagne committed
64 65 66 67 68 69 70 71 72 73 74 75 76 77

    return triggers;
}

QUrl APMSensorsComponent::setupSource(void) const
{
    return QUrl::fromUserInput("qrc:/qml/APMSensorsComponent.qml");
}

QUrl APMSensorsComponent::summaryQmlSource(void) const
{
    return QUrl::fromUserInput("qrc:/qml/APMSensorsComponentSummary.qml");
}

78 79 80 81
bool APMSensorsComponent::compassSetupNeeded(void) const
{
    const size_t cCompass = 3;
    const size_t cOffset = 3;
82 83
    QStringList rgDevicesIds;
    QStringList rgCompassUse;
84 85
    QStringList rgOffsets[cCompass];

86 87
    rgDevicesIds << QStringLiteral("COMPASS_DEV_ID") << QStringLiteral("COMPASS_DEV_ID2") << QStringLiteral("COMPASS_DEV_ID3");
    rgCompassUse << QStringLiteral("COMPASS_USE") << QStringLiteral("COMPASS_USE2") << QStringLiteral("COMPASS_USE3");
Don Gagne's avatar
Don Gagne committed
88 89 90
    rgOffsets[0] << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_Y") << QStringLiteral("COMPASS_OFS_Z");
    rgOffsets[1] << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_Y") << QStringLiteral("COMPASS_OFS2_Z");
    rgOffsets[2] << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_Y") << QStringLiteral("COMPASS_OFS3_Z");
91 92

    for (size_t i=0; i<cCompass; i++) {
93 94
        if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgDevicesIds[i])->rawValue().toInt() != 0 &&
            _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgCompassUse[i])->rawValue().toInt() != 0) {
95
            for (size_t j=0; j<cOffset; j++) {
96
                if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
97 98 99 100 101 102 103 104 105 106 107
                    return true;
                }
            }
        }
    }

    return false;
}

bool APMSensorsComponent::accelSetupNeeded(void) const
{
108
    QStringList rgOffsets;
109

110 111
    // The best we can do is test the first accel which will always be there. We don't have enough information to know
    // whether any of the other accels are available.
112 113
    rgOffsets << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z");

114 115 116 117
    int zeroCount = 0;
    for (int i=0; i<rgOffsets.count(); i++) {
        if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgOffsets[i])->rawValue().toFloat() == 0.0f) {
            zeroCount++;
118 119 120
        }
    }

121
    return zeroCount == rgOffsets.count();
122
}