Skip to content
Snippets Groups Projects
Vehicle.cc 94.9 KiB
Newer Older
  • Learn to ignore specific revisions
  •         { MAV_TYPE_ADSB,            tr("Onboard ADSB peripheral")},
        };
        return typeNames[_vehicleType];
    }
    
    
    /// Returns the string to speak to identify the vehicle
    QString Vehicle::_vehicleIdSpeech(void)
    {
        if (qgcApp()->toolbox()->multiVehicleManager()->vehicles()->count() > 1) {
            return QString("vehicle %1").arg(id());
        } else {
    
            return QString();
    
    Don Gagne's avatar
    Don Gagne committed
    void Vehicle::_handleFlightModeChanged(const QString& flightMode)
    
        _say(QString("%1 %2 flight mode").arg(_vehicleIdSpeech()).arg(flightMode));
    
    Don Gagne's avatar
    Don Gagne committed
        emit guidedModeChanged(_firmwarePlugin->isGuidedMode(this));
    
    }
    
    void Vehicle::_announceArmedChanged(bool armed)
    {
        _say(QString("%1 %2").arg(_vehicleIdSpeech()).arg(armed ? QStringLiteral("armed") : QStringLiteral("disarmed")));
    
    void Vehicle::_setFlying(bool flying)
    
    Don Gagne's avatar
    Don Gagne committed
    {
    
        if (_flying != flying) {
    
    Don Gagne's avatar
    Don Gagne committed
            _flying = flying;
            emit flyingChanged(flying);
        }
    }
    
    
    void Vehicle::_setLanding(bool landing)
    {
        if (armed() && _landing != landing) {
            _landing = landing;
            emit landingChanged(landing);
        }
    }
    
    
    Don Gagne's avatar
    Don Gagne committed
    bool Vehicle::guidedModeSupported(void) const
    {
    
        return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability);
    
    Don Gagne's avatar
    Don Gagne committed
    }
    
    bool Vehicle::pauseVehicleSupported(void) const
    {
    
        return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability);
    }
    
    bool Vehicle::orbitModeSupported() const
    {
        return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
    
    Don Gagne's avatar
    Don Gagne committed
    }
    
    void Vehicle::guidedModeRTL(void)
    {
        if (!guidedModeSupported()) {
    
            qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
    
    Don Gagne's avatar
    Don Gagne committed
            return;
        }
        _firmwarePlugin->guidedModeRTL(this);
    }
    
    void Vehicle::guidedModeLand(void)
    {
        if (!guidedModeSupported()) {
    
            qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
    
    Don Gagne's avatar
    Don Gagne committed
            return;
        }
        _firmwarePlugin->guidedModeLand(this);
    }
    
    
    void Vehicle::guidedModeTakeoff(void)
    
    Don Gagne's avatar
    Don Gagne committed
    {
        if (!guidedModeSupported()) {
    
            qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
    
    Don Gagne's avatar
    Don Gagne committed
            return;
        }
        setGuidedMode(true);
    
        _firmwarePlugin->guidedModeTakeoff(this);
    }
    
    void Vehicle::startMission(void)
    {
        _firmwarePlugin->startMission(this);
    
    Don Gagne's avatar
    Don Gagne committed
    }
    
    void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
    {
        if (!guidedModeSupported()) {
    
            qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
    
    Don Gagne's avatar
    Don Gagne committed
            return;
        }
    
    DonLakeFlyer's avatar
    DonLakeFlyer committed
        if (!coordinate().isValid()) {
            return;
        }
        double maxDistance = 1000.0;
        if (coordinate().distanceTo(gotoCoord) > maxDistance) {
            qgcApp()->showMessage(QString("New location is too far. Must be less than %1 %2").arg(qRound(FactMetaData::metersToAppSettingsDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsDistanceUnitsString()));
            return;
        }
    
    Don Gagne's avatar
    Don Gagne committed
        _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
    }
    
    
    void Vehicle::guidedModeChangeAltitude(double altitudeChange)
    
    Don Gagne's avatar
    Don Gagne committed
    {
        if (!guidedModeSupported()) {
    
            qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
    
    Don Gagne's avatar
    Don Gagne committed
            return;
        }
    
        _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
    
    void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
    {
    
        if (!orbitModeSupported()) {
            qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
    
            return;
        }
        _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
    }
    
    
    Don Gagne's avatar
    Don Gagne committed
    void Vehicle::pauseVehicle(void)
    {
        if (!pauseVehicleSupported()) {
            qgcApp()->showMessage(QStringLiteral("Pause not supported by vehicle."));
            return;
        }
        _firmwarePlugin->pauseVehicle(this);
    }
    
    
    void Vehicle::abortLanding(double climbOutAltitude)
    
    {
        sendMavCommand(defaultComponentId(),
                       MAV_CMD_DO_GO_AROUND,
                       true,        // show error if fails
    
    Don Gagne's avatar
    Don Gagne committed
    bool Vehicle::guidedMode(void) const
    {
        return _firmwarePlugin->isGuidedMode(this);
    }
    
    void Vehicle::setGuidedMode(bool guidedMode)
    {
        return _firmwarePlugin->setGuidedMode(this, guidedMode);
    }
    
    void Vehicle::emergencyStop(void)
    {
    
        sendMavCommand(_defaultComponentId,
    
                       MAV_CMD_COMPONENT_ARM_DISARM,
                       true,        // show error if fails
                       0.0f,
                       21196.0f);  // Magic number for emergency stop
    
    void Vehicle::setCurrentMissionSequence(int seq)
    {
        if (!_firmwarePlugin->sendHomePositionToVehicle()) {
            seq--;
        }
        mavlink_message_t msg;
    
        mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(),
                                                  _mavlink->getComponentId(),
                                                  priorityLink()->mavlinkChannel(),
                                                  &msg,
                                                  id(),
                                                  _compID,
                                                  seq);
        sendMessageOnLink(priorityLink(), msg);
    
    void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
    
        MavCommandQueueEntry_t entry;
    
        entry.component = component;
        entry.command = command;
        entry.showError = showError;
        entry.rgParam[0] = param1;
        entry.rgParam[1] = param2;
        entry.rgParam[2] = param3;
        entry.rgParam[3] = param4;
        entry.rgParam[4] = param5;
        entry.rgParam[5] = param6;
        entry.rgParam[6] = param7;
    
        _mavCommandQueue.append(entry);
    
        if (_mavCommandQueue.count() == 1) {
            _mavCommandRetryCount = 0;
            _sendMavCommandAgain();
        }
    }
    
    void Vehicle::_sendMavCommandAgain(void)
    {
    
        if(!_mavCommandQueue.size()) {
            qWarning() << "Command resend with no commands in queue";
            _mavCommandAckTimer.stop();
            return;
        }
    
    
        MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
    
        if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
    
            if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
                // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
    
    DonLakeFlyer's avatar
    DonLakeFlyer committed
                _setCapabilities(0);
    
    DonLakeFlyer's avatar
    DonLakeFlyer committed
                _startPlanRequest();
    
            emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
            if (queuedCommand.showError) {
                qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command)));
            }
            _mavCommandQueue.removeFirst();
            _sendNextQueuedMavCommand();
            return;
        }
    
    
        if (_mavCommandRetryCount > 1) {
    
            // We always let AUTOPILOT_CAPABILITIES go through multiple times even if we don't get acks. This is because
            // we really need to get capabilities and version info back over a lossy link.
            if (queuedCommand.command != MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
                if (px4Firmware()) {
                    // Older PX4 firmwares are inconsistent with repect to sending back an Ack from a COMMAND_LONG, hence we can't support retry logic for it.
                    if (_firmwareMajorVersion != versionNotSetValue) {
                        // If no version set assume lastest master dev build, so acks are suppored
                        if (_firmwareMajorVersion <= 1 && _firmwareMinorVersion <= 5 && _firmwarePatchVersion <= 3) {
                            // Acks not supported in this version
                            return;
                        }
                    }
                } else {
                    if (queuedCommand.command == MAV_CMD_START_RX_PAIR) {
                        // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares
                        // we aren't really sure whether they are correct or not.
                        return;
                    }
                }
    
    DonLakeFlyer's avatar
    DonLakeFlyer committed
            qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
    
        _mavCommandAckTimer.start();
    
    
    Don Gagne's avatar
    Don Gagne committed
        mavlink_message_t       msg;
        mavlink_command_long_t  cmd;
    
    
    Don Gagne's avatar
    Don Gagne committed
        memset(&cmd, 0, sizeof(cmd));
    
        cmd.command = queuedCommand.command;
    
    Don Gagne's avatar
    Don Gagne committed
        cmd.confirmation = 0;
    
        cmd.param1 = queuedCommand.rgParam[0];
        cmd.param2 = queuedCommand.rgParam[1];
        cmd.param3 = queuedCommand.rgParam[2];
        cmd.param4 = queuedCommand.rgParam[3];
        cmd.param5 = queuedCommand.rgParam[4];
        cmd.param6 = queuedCommand.rgParam[5];
        cmd.param7 = queuedCommand.rgParam[6];
        cmd.target_system = _id;
        cmd.target_component = queuedCommand.component;
    
        mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                             _mavlink->getComponentId(),
                                             priorityLink()->mavlinkChannel(),
                                             &msg,
                                             &cmd);
    
        sendMessageOnLink(priorityLink(), msg);
    
    void Vehicle::_sendNextQueuedMavCommand(void)
    {
        if (_mavCommandQueue.count()) {
            _mavCommandRetryCount = 0;
            _sendMavCommandAgain();
        }
    }
    
    
    
    Don Gagne's avatar
    Don Gagne committed
    void Vehicle::setPrearmError(const QString& prearmError)
    {
        _prearmError = prearmError;
        emit prearmErrorChanged(_prearmError);
        if (!_prearmError.isEmpty()) {
            _prearmErrorTimer.start();
        }
    }
    
    void Vehicle::_prearmErrorTimeout(void)
    {
        setPrearmError(QString());
    }
    
    void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType)
    
    Don Gagne's avatar
    Don Gagne committed
    {
        _firmwareMajorVersion = majorVersion;
        _firmwareMinorVersion = minorVersion;
        _firmwarePatchVersion = patchVersion;
    
        _firmwareVersionType = versionType;
    
        emit firmwareVersionChanged();
    }
    
    void Vehicle::setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion)
    {
        _firmwareCustomMajorVersion = majorVersion;
        _firmwareCustomMinorVersion = minorVersion;
        _firmwareCustomPatchVersion = patchVersion;
        emit firmwareCustomVersionChanged();
    
    }
    
    QString Vehicle::firmwareVersionTypeString(void) const
    {
        switch (_firmwareVersionType) {
        case FIRMWARE_VERSION_TYPE_DEV:
            return QStringLiteral("dev");
        case FIRMWARE_VERSION_TYPE_ALPHA:
            return QStringLiteral("alpha");
        case FIRMWARE_VERSION_TYPE_BETA:
            return QStringLiteral("beta");
        case FIRMWARE_VERSION_TYPE_RC:
            return QStringLiteral("rc");
        case FIRMWARE_VERSION_TYPE_OFFICIAL:
        default:
            return QStringLiteral("");
        }
    
    void Vehicle::rebootVehicle()
    {
    
        sendMavCommand(_defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
    
    void Vehicle::setSoloFirmware(bool soloFirmware)
    {
        if (soloFirmware != _soloFirmware) {
            _soloFirmware = soloFirmware;
            emit soloFirmwareChanged(soloFirmware);
        }
    }
    
    
    Don Gagne's avatar
    Don Gagne committed
    #if 0
        // Temporarily removed, waiting for new command implementation
    
    Don Gagne's avatar
    Don Gagne committed
    void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
    {
    
        doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
    
    Don Gagne's avatar
    Don Gagne committed
    #endif
    
    QString Vehicle::brandImageIndoor(void) const
    
        return _firmwarePlugin->brandImageIndoor(this);
    }
    
    QString Vehicle::brandImageOutdoor(void) const
    {
        return _firmwarePlugin->brandImageOutdoor(this);
    
    QStringList Vehicle::unhealthySensors(void) const
    {
        QStringList sensorList;
    
        struct sensorInfo_s {
            uint32_t    bit;
            const char* sensorName;
        };
    
        static const sensorInfo_s rgSensorInfo[] = {
            { MAV_SYS_STATUS_SENSOR_3D_GYRO,                "Gyro" },
            { MAV_SYS_STATUS_SENSOR_3D_ACCEL,               "Accelerometer" },
            { MAV_SYS_STATUS_SENSOR_3D_MAG,                 "Magnetometer" },
            { MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE,      "Absolute pressure" },
            { MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,  "Differential pressure" },
            { MAV_SYS_STATUS_SENSOR_GPS,                    "GPS" },
            { MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW,           "Optical flow" },
            { MAV_SYS_STATUS_SENSOR_VISION_POSITION,        "Computer vision position" },
            { MAV_SYS_STATUS_SENSOR_LASER_POSITION,         "Laser based position" },
            { MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH,  "External ground truth" },
            { MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL,   "Angular rate control" },
            { MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, "Attitude stabilization" },
            { MAV_SYS_STATUS_SENSOR_YAW_POSITION,           "Yaw position" },
            { MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL,     "Z/altitude control" },
            { MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL,    "X/Y position control" },
            { MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS,          "Motor outputs / control" },
            { MAV_SYS_STATUS_SENSOR_RC_RECEIVER,            "RC receiver" },
            { MAV_SYS_STATUS_SENSOR_3D_GYRO2,               "Gyro 2" },
            { MAV_SYS_STATUS_SENSOR_3D_ACCEL2,              "Accelerometer 2" },
            { MAV_SYS_STATUS_SENSOR_3D_MAG2,                "Magnetometer 2" },
            { MAV_SYS_STATUS_GEOFENCE,                      "GeoFence" },
            { MAV_SYS_STATUS_AHRS,                          "AHRS" },
            { MAV_SYS_STATUS_TERRAIN,                       "Terrain" },
            { MAV_SYS_STATUS_REVERSE_MOTOR,                 "Motors reversed" },
            { MAV_SYS_STATUS_LOGGING,                       "Logging" },
        };
    
        for (size_t i=0; i<sizeof(rgSensorInfo)/sizeof(sensorInfo_s); i++) {
            const sensorInfo_s* pSensorInfo = &rgSensorInfo[i];
            if ((_onboardControlSensorsEnabled & pSensorInfo->bit) && !(_onboardControlSensorsHealth & pSensorInfo->bit)) {
                sensorList << pSensorInfo->sensorName;
            }
        }
    
        return sensorList;
    }
    
    
    void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
    {
        if (_offlineEditingVehicle) {
            _defaultComponentId = defaultComponentId;
        } else {
            qWarning() << "Call to Vehicle::setOfflineEditingDefaultComponentId on vehicle which is not offline";
        }
    }
    
    Don Gagne's avatar
    Don Gagne committed
    void Vehicle::triggerCamera(void)
    {
    
    DonLakeFlyer's avatar
    DonLakeFlyer committed
        sendMavCommand(_defaultComponentId,
    
    Don Gagne's avatar
    Don Gagne committed
                       MAV_CMD_DO_DIGICAM_CONTROL,
                       true,                            // show errors
                       0.0, 0.0, 0.0, 0.0,              // param 1-4 unused
    
                       1.0,                             // trigger camera
                       0.0,                             // param 6 unused
                       1.0);                            // test shot flag
    
    const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
    const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";
    const char* VehicleGPSFactGroup::_courseOverGroundFactName =    "courseOverGround";
    const char* VehicleGPSFactGroup::_countFactName =               "count";
    const char* VehicleGPSFactGroup::_lockFactName =                "lock";
    
    Don Gagne's avatar
    Don Gagne committed
    
    VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
        : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent)
        , _hdopFact             (0, _hdopFactName,              FactMetaData::valueTypeDouble)
        , _vdopFact             (0, _vdopFactName,              FactMetaData::valueTypeDouble)
        , _courseOverGroundFact (0, _courseOverGroundFactName,  FactMetaData::valueTypeDouble)
        , _countFact            (0, _countFactName,             FactMetaData::valueTypeInt32)
        , _lockFact             (0, _lockFactName,              FactMetaData::valueTypeInt32)
    {
        _addFact(&_hdopFact,                _hdopFactName);
        _addFact(&_vdopFact,                _vdopFactName);
        _addFact(&_courseOverGroundFact,    _courseOverGroundFactName);
        _addFact(&_lockFact,                _lockFactName);
        _addFact(&_countFact,               _countFactName);
    
    
        _hdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
        _vdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
        _courseOverGroundFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    
    void Vehicle::startMavlinkLog()
    
        sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_START, false /* showError */);
    
    void Vehicle::stopMavlinkLog()
    
        sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_STOP, false /* showError */);
    
    void Vehicle::_ackMavlinkLogData(uint16_t sequence)
    
    {
        mavlink_message_t msg;
        mavlink_logging_ack_t ack;
    
    Don Gagne's avatar
    Don Gagne committed
        memset(&ack, 0, sizeof(ack));
    
        ack.target_component = _defaultComponentId;
    
        ack.target_system = id();
        mavlink_msg_logging_ack_encode_chan(
            _mavlink->getSystemId(),
            _mavlink->getComponentId(),
            priorityLink()->mavlinkChannel(),
            &msg,
            &ack);
        sendMessageOnLink(priorityLink(), msg);
    }
    
    
    void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
    
    {
        mavlink_logging_data_t log;
        mavlink_msg_logging_data_decode(&message, &log);
    
    Gus Grubba's avatar
    Gus Grubba committed
        emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
            log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
    
    void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
    
        mavlink_logging_data_acked_t log;
        mavlink_msg_logging_data_acked_decode(&message, &log);
    
        _ackMavlinkLogData(log.sequence);
    
    Gus Grubba's avatar
    Gus Grubba committed
        emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
            log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
    
    void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
    {
        firmwarePluginInstanceData->setParent(this);
        _firmwarePluginInstanceData = firmwarePluginInstanceData;
    }
    
    
    QString Vehicle::missionFlightMode(void) const
    {
        return _firmwarePlugin->missionFlightMode();
    }
    
    
    QString Vehicle::pauseFlightMode(void) const
    {
        return _firmwarePlugin->pauseFlightMode();
    }
    
    
    QString Vehicle::rtlFlightMode(void) const
    {
        return _firmwarePlugin->rtlFlightMode();
    }
    
    
    QString Vehicle::landFlightMode(void) const
    {
        return _firmwarePlugin->landFlightMode();
    }
    
    
    QString Vehicle::takeControlFlightMode(void) const
    {
        return _firmwarePlugin->takeControlFlightMode();
    }
    
    
    QString Vehicle::vehicleImageOpaque() const
    {
        if(_firmwarePlugin)
            return _firmwarePlugin->vehicleImageOpaque(this);
        else
            return QString();
    }
    
    QString Vehicle::vehicleImageOutline() const
    {
        if(_firmwarePlugin)
            return _firmwarePlugin->vehicleImageOutline(this);
        else
            return QString();
    }
    
    QString Vehicle::vehicleImageCompass() const
    {
        if(_firmwarePlugin)
            return _firmwarePlugin->vehicleImageCompass(this);
        else
            return QString();
    }
    
    
    const QVariantList& Vehicle::toolBarIndicators()
    
    {
        if(_firmwarePlugin) {
            return _firmwarePlugin->toolBarIndicators(this);
        }
        static QVariantList emptyList;
        return emptyList;
    }
    
    
    const QVariantList& Vehicle::cameraList(void) const
    {
        if (_firmwarePlugin) {
            return _firmwarePlugin->cameraList(this);
        }
        static QVariantList emptyList;
        return emptyList;
    }
    
    
    bool Vehicle::vehicleYawsToNextWaypointInMission(void) const
    {
        return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this);
    }
    
    void Vehicle::_setupAutoDisarmSignalling(void)
    {
        QString param = _firmwarePlugin->autoDisarmParameter(this);
    
        if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
            Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
            connect(fact, &Fact::rawValueChanged, this, &Vehicle::autoDisarmChanged);
            emit autoDisarmChanged();
        }
    }
    
    bool Vehicle::autoDisarm(void)
    {
        QString param = _firmwarePlugin->autoDisarmParameter(this);
    
        if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
            Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
            return fact->rawValue().toDouble() > 0;
        }
    
        return false;
    }
    
    
    //-----------------------------------------------------------------------------
    //-----------------------------------------------------------------------------
    
    
    const char* VehicleBatteryFactGroup::_voltageFactName =                     "voltage";
    const char* VehicleBatteryFactGroup::_percentRemainingFactName =            "percentRemaining";
    const char* VehicleBatteryFactGroup::_mahConsumedFactName =                 "mahConsumed";
    const char* VehicleBatteryFactGroup::_currentFactName =                     "current";
    const char* VehicleBatteryFactGroup::_temperatureFactName =                 "temperature";
    const char* VehicleBatteryFactGroup::_cellCountFactName =                   "cellCount";
    
    const char* VehicleBatteryFactGroup::_settingsGroup =                       "Vehicle.battery";
    
    const double VehicleBatteryFactGroup::_voltageUnavailable =           -1.0;
    const int    VehicleBatteryFactGroup::_percentRemainingUnavailable =  -1;
    const int    VehicleBatteryFactGroup::_mahConsumedUnavailable =       -1;
    const int    VehicleBatteryFactGroup::_currentUnavailable =           -1;
    const double VehicleBatteryFactGroup::_temperatureUnavailable =       -1.0;
    const int    VehicleBatteryFactGroup::_cellCountUnavailable =         -1.0;
    
    
    Don Gagne's avatar
    Don Gagne committed
    VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
        : FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent)
    
        , _voltageFact                  (0, _voltageFactName,                   FactMetaData::valueTypeDouble)
        , _percentRemainingFact         (0, _percentRemainingFactName,          FactMetaData::valueTypeInt32)
        , _mahConsumedFact              (0, _mahConsumedFactName,               FactMetaData::valueTypeInt32)
    
    Don Gagne's avatar
    Don Gagne committed
        , _currentFact                  (0, _currentFactName,                   FactMetaData::valueTypeFloat)
    
        , _temperatureFact              (0, _temperatureFactName,               FactMetaData::valueTypeDouble)
        , _cellCountFact                (0, _cellCountFactName,                 FactMetaData::valueTypeInt32)
    {
    
        _addFact(&_voltageFact,                 _voltageFactName);
        _addFact(&_percentRemainingFact,        _percentRemainingFactName);
        _addFact(&_mahConsumedFact,             _mahConsumedFactName);
        _addFact(&_currentFact,                 _currentFactName);
        _addFact(&_temperatureFact,             _temperatureFactName);
        _addFact(&_cellCountFact,               _cellCountFactName);
    
    Don Gagne's avatar
    Don Gagne committed
    
        // Start out as not available
        _voltageFact.setRawValue            (_voltageUnavailable);
        _percentRemainingFact.setRawValue   (_percentRemainingUnavailable);
        _mahConsumedFact.setRawValue        (_mahConsumedUnavailable);
        _currentFact.setRawValue            (_currentUnavailable);
        _temperatureFact.setRawValue        (_temperatureUnavailable);
        _cellCountFact.setRawValue          (_cellCountUnavailable);
    }
    
    
    const char* VehicleWindFactGroup::_directionFactName =      "direction";
    const char* VehicleWindFactGroup::_speedFactName =          "speed";
    const char* VehicleWindFactGroup::_verticalSpeedFactName =  "verticalSpeed";
    
    
    Don Gagne's avatar
    Don Gagne committed
    VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent)
        : FactGroup(1000, ":/json/Vehicle/WindFact.json", parent)
        , _directionFact    (0, _directionFactName,     FactMetaData::valueTypeDouble)
        , _speedFact        (0, _speedFactName,         FactMetaData::valueTypeDouble)
        , _verticalSpeedFact(0, _verticalSpeedFactName, FactMetaData::valueTypeDouble)
    {
        _addFact(&_directionFact,       _directionFactName);
        _addFact(&_speedFact,           _speedFactName);
        _addFact(&_verticalSpeedFact,   _verticalSpeedFactName);
    
        // Start out as not available "--.--"
        _directionFact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
        _speedFact.setRawValue          (std::numeric_limits<float>::quiet_NaN());
        _verticalSpeedFact.setRawValue  (std::numeric_limits<float>::quiet_NaN());
    }
    
    
    const char* VehicleVibrationFactGroup::_xAxisFactName =      "xAxis";
    const char* VehicleVibrationFactGroup::_yAxisFactName =      "yAxis";
    const char* VehicleVibrationFactGroup::_zAxisFactName =      "zAxis";
    const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1";
    const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2";
    const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3";
    
    
    VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent)
        : FactGroup(1000, ":/json/Vehicle/VibrationFact.json", parent)
        , _xAxisFact        (0, _xAxisFactName,         FactMetaData::valueTypeDouble)
        , _yAxisFact        (0, _yAxisFactName,         FactMetaData::valueTypeDouble)
        , _zAxisFact        (0, _zAxisFactName,         FactMetaData::valueTypeDouble)
        , _clipCount1Fact   (0, _clipCount1FactName,    FactMetaData::valueTypeUint32)
        , _clipCount2Fact   (0, _clipCount2FactName,    FactMetaData::valueTypeUint32)
        , _clipCount3Fact   (0, _clipCount3FactName,    FactMetaData::valueTypeUint32)
    {
        _addFact(&_xAxisFact,       _xAxisFactName);
        _addFact(&_yAxisFact,       _yAxisFactName);
        _addFact(&_zAxisFact,       _zAxisFactName);
        _addFact(&_clipCount1Fact,  _clipCount1FactName);
        _addFact(&_clipCount2Fact,  _clipCount2FactName);
        _addFact(&_clipCount3Fact,  _clipCount3FactName);
    
        // Start out as not available "--.--"
        _xAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
        _yAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
        _zAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    }
    
    
    
    const char* VehicleTemperatureFactGroup::_temperature1FactName =      "temperature1";
    const char* VehicleTemperatureFactGroup::_temperature2FactName =      "temperature2";
    const char* VehicleTemperatureFactGroup::_temperature3FactName =      "temperature3";
    
    VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent)
        : FactGroup(1000, ":/json/Vehicle/TemperatureFact.json", parent)
        , _temperature1Fact    (0, _temperature1FactName,     FactMetaData::valueTypeDouble)
        , _temperature2Fact    (0, _temperature2FactName,     FactMetaData::valueTypeDouble)
        , _temperature3Fact    (0, _temperature3FactName,     FactMetaData::valueTypeDouble)
    {
        _addFact(&_temperature1Fact,       _temperature1FactName);
        _addFact(&_temperature2Fact,       _temperature2FactName);
        _addFact(&_temperature3Fact,       _temperature3FactName);
    
        // Start out as not available "--.--"
        _temperature1Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
        _temperature2Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
        _temperature3Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
    }