Vehicle.cc

        _defaultComponentId,
        MAV_CMD_DO_MOUNT_CONTROL,
        false,                               // show errors
        static_cast<float>(pitch),           // Pitch 0 - 90
        0,                                   // Roll (not used)
        static_cast<float>(yaw),             // Yaw -180 - 180
        0,                                   // Altitude (not used)
        0,                                   // Latitude (not used)
        0,                                   // Longitude (not used)
        MAV_MOUNT_MODE_MAVLINK_TARGETING);   // MAVLink Roll,Pitch,Yaw
}

void Vehicle::gimbalPitchStep(int direction)
{
    if(_haveGimbalData) {
        //qDebug() << "Pitch:" << _curGimbalPitch << direction << (_curGimbalPitch + direction);
        double p = static_cast<double>(_curGimbalPitch + direction);
        gimbalControlValue(p, static_cast<double>(_curGinmbalYaw));
    }
}

void Vehicle::gimbalYawStep(int direction)
{
    if(_haveGimbalData) {
        //qDebug() << "Yaw:" << _curGinmbalYaw << direction << (_curGinmbalYaw + direction);
        double y = static_cast<double>(_curGinmbalYaw + direction);
        gimbalControlValue(static_cast<double>(_curGimbalPitch), y);
    }
}

void Vehicle::centerGimbal()
{
    if(_haveGimbalData) {
        gimbalControlValue(0.0, 0.0);
    }
}

void Vehicle::_handleGimbalOrientation(const mavlink_message_t& message)
{
    mavlink_mount_orientation_t o;
    mavlink_msg_mount_orientation_decode(&message, &o);
    if(fabsf(_curGimbalRoll - o.roll) > 0.5f) {
        _curGimbalRoll = o.roll;
        emit gimbalRollChanged();
    }
    if(fabsf(_curGimbalPitch - o.pitch) > 0.5f) {
        _curGimbalPitch = o.pitch;
        emit gimbalPitchChanged();
    }
    if(fabsf(_curGinmbalYaw - o.yaw) > 0.5f) {
        _curGinmbalYaw = o.yaw;
        emit gimbalYawChanged();
    }
    if(!_haveGimbalData) {
        _haveGimbalData = true;
        emit gimbalDataChanged();
    }
}

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

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::_timeRemainingFactName =               "timeRemaining";
const char* VehicleBatteryFactGroup::_chargeStateFactName =                 "chargeState";

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

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

const char* VehicleEstimatorStatusFactGroup::_goodAttitudeEstimateFactName =        "goodAttitudeEsimate";
const char* VehicleEstimatorStatusFactGroup::_goodHorizVelEstimateFactName =        "goodHorizVelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodVertVelEstimateFactName =         "goodVertVelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodHorizPosRelEstimateFactName =     "goodHorizPosRelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodHorizPosAbsEstimateFactName =     "goodHorizPosAbsEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodVertPosAbsEstimateFactName =      "goodVertPosAbsEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodVertPosAGLEstimateFactName =      "goodVertPosAGLEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodConstPosModeEstimateFactName =    "goodConstPosModeEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodPredHorizPosRelEstimateFactName = "goodPredHorizPosRelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodPredHorizPosAbsEstimateFactName = "goodPredHorizPosAbsEstimate";
const char* VehicleEstimatorStatusFactGroup::_gpsGlitchFactName =                   "gpsGlitch";
const char* VehicleEstimatorStatusFactGroup::_accelErrorFactName =                  "accelError";
const char* VehicleEstimatorStatusFactGroup::_velRatioFactName =                    "velRatio";
const char* VehicleEstimatorStatusFactGroup::_horizPosRatioFactName =               "horizPosRatio";
const char* VehicleEstimatorStatusFactGroup::_vertPosRatioFactName =                "vertPosRatio";
const char* VehicleEstimatorStatusFactGroup::_magRatioFactName =                    "magRatio";
const char* VehicleEstimatorStatusFactGroup::_haglRatioFactName =                   "haglRatio";
const char* VehicleEstimatorStatusFactGroup::_tasRatioFactName =                    "tasRatio";
const char* VehicleEstimatorStatusFactGroup::_horizPosAccuracyFactName =            "horizPosAccuracy";
const char* VehicleEstimatorStatusFactGroup::_vertPosAccuracyFactName =             "vertPosAccuracy";

VehicleEstimatorStatusFactGroup::VehicleEstimatorStatusFactGroup(QObject* parent)
    : FactGroup                         (500, ":/json/Vehicle/EstimatorStatusFactGroup.json", parent)
    , _goodAttitudeEstimateFact         (0, _goodAttitudeEstimateFactName,          FactMetaData::valueTypeBool)
    , _goodHorizVelEstimateFact         (0, _goodHorizVelEstimateFactName,          FactMetaData::valueTypeBool)
    , _goodVertVelEstimateFact          (0, _goodVertVelEstimateFactName,           FactMetaData::valueTypeBool)
    , _goodHorizPosRelEstimateFact      (0, _goodHorizPosRelEstimateFactName,       FactMetaData::valueTypeBool)
    , _goodHorizPosAbsEstimateFact      (0, _goodHorizPosAbsEstimateFactName,       FactMetaData::valueTypeBool)
    , _goodVertPosAbsEstimateFact       (0, _goodVertPosAbsEstimateFactName,        FactMetaData::valueTypeBool)
    , _goodVertPosAGLEstimateFact       (0, _goodVertPosAGLEstimateFactName,        FactMetaData::valueTypeBool)
    , _goodConstPosModeEstimateFact     (0, _goodConstPosModeEstimateFactName,      FactMetaData::valueTypeBool)
    , _goodPredHorizPosRelEstimateFact  (0, _goodPredHorizPosRelEstimateFactName,   FactMetaData::valueTypeBool)
    , _goodPredHorizPosAbsEstimateFact  (0, _goodPredHorizPosAbsEstimateFactName,   FactMetaData::valueTypeBool)
    , _gpsGlitchFact                    (0, _gpsGlitchFactName,                     FactMetaData::valueTypeBool)
    , _accelErrorFact                   (0, _accelErrorFactName,                    FactMetaData::valueTypeBool)
    , _velRatioFact                     (0, _velRatioFactName,                      FactMetaData::valueTypeFloat)
    , _horizPosRatioFact                (0, _horizPosRatioFactName,                 FactMetaData::valueTypeFloat)
    , _vertPosRatioFact                 (0, _vertPosRatioFactName,                  FactMetaData::valueTypeFloat)
    , _magRatioFact                     (0, _magRatioFactName,                      FactMetaData::valueTypeFloat)
    , _haglRatioFact                    (0, _haglRatioFactName,                     FactMetaData::valueTypeFloat)
    , _tasRatioFact                     (0, _tasRatioFactName,                      FactMetaData::valueTypeFloat)
    , _horizPosAccuracyFact             (0, _horizPosAccuracyFactName,              FactMetaData::valueTypeFloat)
    , _vertPosAccuracyFact              (0, _vertPosAccuracyFactName,               FactMetaData::valueTypeFloat)
{
    _addFact(&_goodAttitudeEstimateFact,        _goodAttitudeEstimateFactName);
    _addFact(&_goodHorizVelEstimateFact,        _goodHorizVelEstimateFactName);
    _addFact(&_goodVertVelEstimateFact,         _goodVertVelEstimateFactName);
    _addFact(&_goodHorizPosRelEstimateFact,     _goodHorizPosRelEstimateFactName);
    _addFact(&_goodHorizPosAbsEstimateFact,     _goodHorizPosAbsEstimateFactName);
    _addFact(&_goodVertPosAbsEstimateFact,      _goodVertPosAbsEstimateFactName);
    _addFact(&_goodVertPosAGLEstimateFact,      _goodVertPosAGLEstimateFactName);
    _addFact(&_goodConstPosModeEstimateFact,    _goodConstPosModeEstimateFactName);
    _addFact(&_goodPredHorizPosRelEstimateFact, _goodPredHorizPosRelEstimateFactName);
    _addFact(&_goodPredHorizPosAbsEstimateFact, _goodPredHorizPosAbsEstimateFactName);
    _addFact(&_gpsGlitchFact,                   _gpsGlitchFactName);
    _addFact(&_accelErrorFact,                  _accelErrorFactName);
    _addFact(&_velRatioFact,                    _velRatioFactName);
    _addFact(&_horizPosRatioFact,               _horizPosRatioFactName);
    _addFact(&_vertPosRatioFact,                _vertPosRatioFactName);
    _addFact(&_magRatioFact,                    _magRatioFactName);
    _addFact(&_haglRatioFact,                   _haglRatioFactName);
    _addFact(&_tasRatioFact,                    _tasRatioFactName);
    _addFact(&_horizPosAccuracyFact,            _horizPosAccuracyFactName);
    _addFact(&_vertPosAccuracyFact,             _vertPosAccuracyFactName);
}