Vehicle.cc

    _lonFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _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;
    memset(&ack, 0, sizeof(ack));
    ack.sequence = sequence;
    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);
    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);
    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::staticCameraList(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;
}

void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
{
    mavlink_adsb_vehicle_t adsbVehicle;
    static const int maxTimeSinceLastSeen = 15;

    mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicle);
    if (adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS) {
        if (_adsbICAOMap.contains(adsbVehicle.ICAO_address)) {
            if (adsbVehicle.tslc > maxTimeSinceLastSeen) {
                ADSBVehicle* vehicle = _adsbICAOMap[adsbVehicle.ICAO_address];
                _adsbVehicles.removeOne(vehicle);
                _adsbICAOMap.remove(adsbVehicle.ICAO_address);
                vehicle->deleteLater();
            } else {
                _adsbICAOMap[adsbVehicle.ICAO_address]->update(adsbVehicle);
            }
        } else if (adsbVehicle.tslc <= maxTimeSinceLastSeen) {
            ADSBVehicle* vehicle = new ADSBVehicle(adsbVehicle, this);
            _adsbICAOMap[adsbVehicle.ICAO_address] = vehicle;
            _adsbVehicles.append(vehicle);
        }
    }
}

void Vehicle::_updateDistanceToHome(void)
{
    if (coordinate().isValid() && homePosition().isValid()) {
        _distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition()));
    } else {
        _distanceToHomeFact.setRawValue(qQNaN());
    }
}

void Vehicle::_updateHobbsMeter(void)
{
    _hobbsFact.setRawValue(hobbsMeter());
}

void Vehicle::forceInitialPlanRequestComplete(void)
{
    _initialPlanRequestComplete = true;
    emit initialPlanRequestCompleteChanged(true);
}

void Vehicle::sendPlan(QString planFile)
{
    PlanMasterController::sendPlanToVehicle(this, planFile);
}

QString Vehicle::hobbsMeter()
{
    static const char* HOOBS_HI = "LND_FLIGHT_T_HI";
    static const char* HOOBS_LO = "LND_FLIGHT_T_LO";
    //-- TODO: Does this exist on non PX4?
    if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) &&
            _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) {
        Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI);
        Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO);
        uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000;
        int hours   = hobbsTimeSeconds / 3600;
        int minutes = (hobbsTimeSeconds % 3600) / 60;
        int seconds = hobbsTimeSeconds % 60;
        QString timeStr;
        timeStr.sprintf("%04d:%02d:%02d", hours, minutes, seconds);
        qCDebug(VehicleLog) << "Hobbs Meter:" << timeStr << "(" << factHi->rawValue().toUInt() << factLo->rawValue().toUInt() << ")";
        return timeStr;
    }
    return QString("0000:00:00");
}

void Vehicle::_vehicleParamLoaded(bool ready)
{
    //-- TODO: This seems silly but can you think of a better
    //   way to update this?
    if(ready) {
        emit hobbsMeterChanged();
    }
}

void Vehicle::_updateHighLatencyLink(void)
{
    if (_priorityLink->highLatency() != _highLatencyLink) {
        _highLatencyLink = _priorityLink->highLatency();
        _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
        emit highLatencyLinkChanged(_highLatencyLink);
    }
}

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

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::_instantPowerFactName =                "instantPower";

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;
const double VehicleBatteryFactGroup::_instantPowerUnavailable =      -1.0;

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)
    , _currentFact                  (0, _currentFactName,                   FactMetaData::valueTypeFloat)
    , _temperatureFact              (0, _temperatureFactName,               FactMetaData::valueTypeDouble)
    , _cellCountFact                (0, _cellCountFactName,                 FactMetaData::valueTypeInt32)
    , _instantPowerFact             (0, _instantPowerFactName,              FactMetaData::valueTypeFloat)
{
    _addFact(&_voltageFact,                 _voltageFactName);
    _addFact(&_percentRemainingFact,        _percentRemainingFactName);
    _addFact(&_mahConsumedFact,             _mahConsumedFactName);
    _addFact(&_currentFact,                 _currentFactName);
    _addFact(&_temperatureFact,             _temperatureFactName);
    _addFact(&_cellCountFact,               _cellCountFactName);
    _addFact(&_instantPowerFact,            _instantPowerFactName);

    // Start out as not available
    _voltageFact.setRawValue            (_voltageUnavailable);
    _percentRemainingFact.setRawValue   (_percentRemainingUnavailable);
    _mahConsumedFact.setRawValue        (_mahConsumedUnavailable);
    _currentFact.setRawValue            (_currentUnavailable);
    _temperatureFact.setRawValue        (_temperatureUnavailable);
    _cellCountFact.setRawValue          (_cellCountUnavailable);
    _instantPowerFact.setRawValue       (_instantPowerUnavailable);
}

const char* VehicleWindFactGroup::_directionFactName =      "direction";
const char* VehicleWindFactGroup::_speedFactName =          "speed";
const char* VehicleWindFactGroup::_verticalSpeedFactName =  "verticalSpeed";

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());
}

const char* VehicleClockFactGroup::_currentTimeFactName = "currentTime";
const char* VehicleClockFactGroup::_currentDateFactName = "currentDate";

VehicleClockFactGroup::VehicleClockFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/ClockFact.json", parent)
    , _currentTimeFact  (0, _currentTimeFactName,    FactMetaData::valueTypeString)
    , _currentDateFact  (0, _currentDateFactName,    FactMetaData::valueTypeString)
{
    _addFact(&_currentTimeFact, _currentTimeFactName);
    _addFact(&_currentDateFact, _currentDateFactName);

    // Start out as not available "--.--"
    _currentTimeFact.setRawValue    (std::numeric_limits<float>::quiet_NaN());
    _currentDateFact.setRawValue    (std::numeric_limits<float>::quiet_NaN());
}

void VehicleClockFactGroup::_updateAllValues(void)
{
    _currentTimeFact.setRawValue(QTime::currentTime().toString());
    _currentDateFact.setRawValue(QDateTime::currentDateTime().toString(QLocale::system().dateFormat(QLocale::ShortFormat)));

    FactGroup::_updateAllValues();
}

const char* VehicleSetpointFactGroup::_rollFactName =       "roll";
const char* VehicleSetpointFactGroup::_pitchFactName =      "pitch";
const char* VehicleSetpointFactGroup::_yawFactName =        "yaw";
const char* VehicleSetpointFactGroup::_rollRateFactName =   "rollRate";
const char* VehicleSetpointFactGroup::_pitchRateFactName =  "pitchRate";
const char* VehicleSetpointFactGroup::_yawRateFactName =    "yawRate";

VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent)
    : FactGroup     (1000, ":/json/Vehicle/SetpointFact.json", parent)
    , _rollFact     (0, _rollFactName,      FactMetaData::valueTypeDouble)
    , _pitchFact    (0, _pitchFactName,     FactMetaData::valueTypeDouble)
    , _yawFact      (0, _yawFactName,       FactMetaData::valueTypeDouble)
    , _rollRateFact (0, _rollRateFactName,  FactMetaData::valueTypeDouble)
    , _pitchRateFact(0, _pitchRateFactName, FactMetaData::valueTypeDouble)
    , _yawRateFact  (0, _yawRateFactName,   FactMetaData::valueTypeDouble)
{
    _addFact(&_rollFact,        _rollFactName);
    _addFact(&_pitchFact,       _pitchFactName);
    _addFact(&_yawFact,         _yawFactName);
    _addFact(&_rollRateFact,    _rollRateFactName);
    _addFact(&_pitchRateFact,   _pitchRateFactName);
    _addFact(&_yawRateFact,     _yawRateFactName);

    // Start out as not available "--.--"
    _rollFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _pitchFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _yawFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rollRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _pitchRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _yawRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}

const char* VehicleDistanceSensorFactGroup::_rotationNoneFactName =     "rotationNone";
const char* VehicleDistanceSensorFactGroup::_rotationYaw45FactName =    "rotationYaw45";
const char* VehicleDistanceSensorFactGroup::_rotationYaw90FactName =    "rotationYaw90";
const char* VehicleDistanceSensorFactGroup::_rotationYaw135FactName =   "rotationYaw135";
const char* VehicleDistanceSensorFactGroup::_rotationYaw180FactName =   "rotationYaw180";
const char* VehicleDistanceSensorFactGroup::_rotationYaw225FactName =   "rotationYaw225";
const char* VehicleDistanceSensorFactGroup::_rotationYaw270FactName =   "rotationYaw270";
const char* VehicleDistanceSensorFactGroup::_rotationYaw315FactName =   "rotationYaw315";
const char* VehicleDistanceSensorFactGroup::_rotationPitch90FactName =  "rotationPitch90";
const char* VehicleDistanceSensorFactGroup::_rotationPitch270FactName = "rotationPitch270";

VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent)
    : FactGroup             (1000, ":/json/Vehicle/DistanceSensorFact.json", parent)
    , _rotationNoneFact     (0, _rotationNoneFactName,      FactMetaData::valueTypeDouble)
    , _rotationYaw45Fact    (0, _rotationYaw45FactName,     FactMetaData::valueTypeDouble)
    , _rotationYaw90Fact    (0, _rotationYaw90FactName,     FactMetaData::valueTypeDouble)
    , _rotationYaw135Fact   (0, _rotationYaw135FactName,    FactMetaData::valueTypeDouble)
    , _rotationYaw180Fact   (0, _rotationYaw180FactName,    FactMetaData::valueTypeDouble)
    , _rotationYaw225Fact   (0, _rotationYaw225FactName,    FactMetaData::valueTypeDouble)
    , _rotationYaw270Fact   (0, _rotationYaw270FactName,    FactMetaData::valueTypeDouble)
    , _rotationYaw315Fact   (0, _rotationYaw315FactName,    FactMetaData::valueTypeDouble)
    , _rotationPitch90Fact  (0, _rotationPitch90FactName,   FactMetaData::valueTypeDouble)
    , _rotationPitch270Fact (0, _rotationPitch270FactName,  FactMetaData::valueTypeDouble)
    , _idSet                (false)
    , _id                   (0)
{
    _addFact(&_rotationNoneFact,        _rotationNoneFactName);
    _addFact(&_rotationYaw45Fact,       _rotationYaw45FactName);
    _addFact(&_rotationYaw90Fact,       _rotationYaw90FactName);
    _addFact(&_rotationYaw135Fact,      _rotationYaw135FactName);
    _addFact(&_rotationYaw180Fact,      _rotationYaw180FactName);
    _addFact(&_rotationYaw225Fact,      _rotationYaw225FactName);
    _addFact(&_rotationYaw270Fact,      _rotationYaw270FactName);
    _addFact(&_rotationYaw315Fact,      _rotationYaw315FactName);
    _addFact(&_rotationPitch90Fact,     _rotationPitch90FactName);
    _addFact(&_rotationPitch270Fact,    _rotationPitch270FactName);

    // Start out as not available "--.--"
    _rotationNoneFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationYaw45Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationYaw135Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationYaw90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationYaw180Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationYaw225Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationYaw270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationPitch90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _rotationPitch270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}