Newer
Older
void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
{
mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicleMsg);
if (adsbVehicleMsg.flags | ADSB_FLAGS_VALID_COORDS && adsbVehicleMsg.tslc <= maxTimeSinceLastSeen) {
ADSBVehicle::VehicleInfo_t vehicleInfo;
vehicleInfo.availableFlags = 0;
vehicleInfo.icaoAddress = adsbVehicleMsg.ICAO_address;
vehicleInfo.location.setLongitude(adsbVehicleMsg.lon / 1e7);
vehicleInfo.availableFlags |= ADSBVehicle::LocationAvailable;
vehicleInfo.callsign = adsbVehicleMsg.callsign;
vehicleInfo.availableFlags |= ADSBVehicle::CallsignAvailable;
if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_ALTITUDE) {
vehicleInfo.altitude = (double)adsbVehicleMsg.altitude / 1e3;
vehicleInfo.availableFlags |= ADSBVehicle::AltitudeAvailable;
}
if (adsbVehicleMsg.flags & ADSB_FLAGS_VALID_HEADING) {
vehicleInfo.heading = (double)adsbVehicleMsg.heading / 100.0;
vehicleInfo.availableFlags |= ADSBVehicle::HeadingAvailable;
void Vehicle::_updateDistanceHeadingToHome()
{
if (coordinate().isValid() && homePosition().isValid()) {
_distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition()));
if (_distanceToHomeFact.rawValue().toDouble() > 1.0) {
_headingToHomeFact.setRawValue(coordinate().azimuthTo(homePosition()));
} else {
_headingToHomeFact.setRawValue(qQNaN());
}
} else {
_distanceToHomeFact.setRawValue(qQNaN());
void Vehicle::_updateHeadingToNextWP()
const int currentIndex = _missionManager->currentIndex();
QList<MissionItem*> llist = _missionManager->missionItems();
if(llist.size()>currentIndex && currentIndex!=-1
&& llist[currentIndex]->coordinate().longitude()!=0.0
&& coordinate().distanceTo(llist[currentIndex]->coordinate())>5.0 ){
_headingToNextWPFact.setRawValue(coordinate().azimuthTo(llist[currentIndex]->coordinate()));
}
else{
_headingToNextWPFact.setRawValue(qQNaN());
}
}
void Vehicle::_updateMissionItemIndex()
{
const int currentIndex = _missionManager->currentIndex();
unsigned offset = 0;
if (!_firmwarePlugin->sendHomePositionToVehicle()) {
offset = 1;
}
_missionItemIndexFact.setRawValue(currentIndex + offset);
}
void Vehicle::_updateDistanceToGCS()
{
QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition();
if (coordinate().isValid() && gcsPosition.isValid()) {
_distanceToGCSFact.setRawValue(coordinate().distanceTo(gcsPosition));
} else {
_distanceToGCSFact.setRawValue(qQNaN());
}
}
{
_hobbsFact.setRawValue(hobbsMeter());
}
void Vehicle::forceInitialPlanRequestComplete()
{
_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);
Gus Grubba
committed
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 = QString::asprintf("%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(bool sendCommand)
if (!_priorityLink) {
return;
}
if (_priorityLink->highLatency() != _highLatencyLink) {
_highLatencyLink = _priorityLink->highLatency();
_mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
emit highLatencyLinkChanged(_highLatencyLink);
if (sendCommand) {
sendMavCommand(defaultComponentId(),
MAV_CMD_CONTROL_HIGH_LATENCY,
true,
_highLatencyLink ? 1.0f : 0.0f); // request start/stop transmitting over high latency telemetry
void Vehicle::_trafficUpdate(bool /*alert*/, QString /*traffic_id*/, QString /*vehicle_id*/, QGeoCoordinate /*location*/, float /*heading*/)
#if 0
// This is ifdef'ed out for now since this code doesn't mesh with the recent ADSB manager changes. Also airmap isn't in any
// released build. So not going to waste time trying to fix up unused code.
if (_trafficVehicleMap.contains(traffic_id)) {
_trafficVehicleMap[traffic_id]->update(alert, location, heading);
ADSBVehicle* vehicle = new ADSBVehicle(location, heading, alert, this);
_trafficVehicleMap[traffic_id] = vehicle;
_adsbVehicles.append(vehicle);
}
void Vehicle::_mavlinkMessageStatus(int uasId, uint64_t totalSent, uint64_t totalReceived, uint64_t totalLoss, float lossPercent)
{
if(uasId == _id) {
_mavlinkSentCount = totalSent;
_mavlinkReceivedCount = totalReceived;
_mavlinkLossCount = totalLoss;
_mavlinkLossPercent = lossPercent;
emit mavlinkStatusChanged();
}
}
int Vehicle::versionCompare(QString& compare)
{
return _firmwarePlugin->versionCompare(this, compare);
}
int Vehicle::versionCompare(int major, int minor, int patch)
{
return _firmwarePlugin->versionCompare(this, major, minor, patch);
}
if (_pidTuningWaitingForRates) {
mavlink_message_interval_t messageInterval;
mavlink_msg_message_interval_decode(&message, &messageInterval);
int msgId = messageInterval.message_id;
if (_pidTuningMessages.contains(msgId)) {
_pidTuningMessageRatesUsecs[msgId] = messageInterval.interval_us;
}
if (_pidTuningMessageRatesUsecs.count() == _pidTuningMessages.count()) {
// We have back all the rates we requested
_pidTuningWaitingForRates = false;
_pidTuningAdjustRates(true);
}
}
}
void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
{
4215
4216
4217
4218
4219
4220
4221
4222
4223
4224
4225
4226
4227
4228
4229
4230
4231
4232
4233
4234
4235
4236
4237
4238
4239
4240
4241
4242
4243
4244
4245
4246
if (!_pidTuningTelemetryMode) {
// First step is to get the current message rates before we adjust them
_pidTuningTelemetryMode = true;
_pidTuningWaitingForRates = true;
_pidTuningMessageRatesUsecs.clear();
for (int telemetry: _pidTuningMessages) {
sendMavCommand(defaultComponentId(),
MAV_CMD_GET_MESSAGE_INTERVAL,
true, // show error
telemetry);
}
}
} else {
if (_pidTuningTelemetryMode) {
_pidTuningTelemetryMode = false;
if (_pidTuningWaitingForRates) {
// We never finished waiting for previous rates
_pidTuningWaitingForRates = false;
} else {
_pidTuningAdjustRates(false);
}
}
}
}
void Vehicle::_pidTuningAdjustRates(bool setRatesForTuning)
{
int requestedRate = (int)(1000000.0 / 30.0); // 30 Hz in usecs
for (int telemetry: _pidTuningMessages) {
if (requestedRate < _pidTuningMessageRatesUsecs[telemetry]) {
sendMavCommand(defaultComponentId(),
MAV_CMD_SET_MESSAGE_INTERVAL,
true, // show error
telemetry,
setRatesForTuning ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]);
setLiveUpdates(setRatesForTuning);
_setpointFactGroup.setLiveUpdates(setRatesForTuning);
void Vehicle::_initializeCsv()
{
if(!_toolbox->settingsManager()->appSettings()->saveCsvTelemetry()->rawValue().toBool()){
return;
}
4264
4265
4266
4267
4268
4269
4270
4271
4272
4273
4274
4275
4276
4277
4278
4279
4280
4281
4282
4283
4284
4285
4286
4287
QString now = QDateTime::currentDateTime().toString("yyyy-MM-dd hh-mm-ss");
QString fileName = QString("%1 vehicle%2.csv").arg(now).arg(_id);
QDir saveDir(_toolbox->settingsManager()->appSettings()->telemetrySavePath());
_csvLogFile.setFileName(saveDir.absoluteFilePath(fileName));
if (!_csvLogFile.open(QIODevice::Append)) {
qCWarning(VehicleLog) << "unable to open file for csv logging, Stopping csv logging!";
return;
}
QTextStream stream(&_csvLogFile);
QStringList allFactNames;
allFactNames << factNames();
for (const QString& groupName: factGroupNames()) {
for(const QString& factName: getFactGroup(groupName)->factNames()){
allFactNames << QString("%1.%2").arg(groupName, factName);
}
}
qCDebug(VehicleLog) << "Facts logged to csv:" << allFactNames;
stream << "Timestamp," << allFactNames.join(",") << "\n";
}
void Vehicle::_writeCsvLine()
{
// Only save the logs after the the vehicle gets armed, unless "Save logs even if vehicle was not armed" is checked
if(!_csvLogFile.isOpen() &&
(_armed || _toolbox->settingsManager()->appSettings()->telemetrySaveNotArmed()->rawValue().toBool())){
_initializeCsv();
}
if(!_csvLogFile.isOpen()){
return;
}
QStringList allFactValues;
QTextStream stream(&_csvLogFile);
// Write timestamp to csv file
allFactValues << QDateTime::currentDateTime().toString(QStringLiteral("yyyy-MM-dd hh:mm:ss.zzz"));
// Write Vehicle's own facts
for (const QString& factName : factNames()) {
allFactValues << getFact(factName)->cookedValueString();
}
// write facts from Vehicle's FactGroups
for (const QString& groupName: factGroupNames()) {
for (const QString& factName : getFactGroup(groupName)->factNames()) {
allFactValues << getFactGroup(groupName)->getFact(factName)->cookedValueString();
}
}
stream << allFactValues.join(",") << "\n";
}
{
sendMavCommand(defaultComponentId(),
MAV_CMD_FLASH_BOOTLOADER,
true, // show error
0, 0, 0, 0, // param 1-4 not used
290876); // magic number
}
#endif
void Vehicle::gimbalControlValue(double pitch, double yaw)
{
sendMavCommand(
_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
}
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()
{
4366
4367
4368
4369
4370
4371
4372
4373
4374
4375
4376
4377
4378
4379
4380
4381
4382
4383
4384
4385
4386
4387
4388
4389
4390
4391
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();
}
}
void Vehicle::_handleObstacleDistance(const mavlink_message_t& message)
{
mavlink_obstacle_distance_t o;
mavlink_msg_obstacle_distance_decode(&message, &o);
_objectAvoidance->update(&o);
{
_flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + distance);
}
4404
4405
4406
4407
4408
4409
4410
4411
4412
4413
4414
4415
4416
4417
4418
4419
4420
4421
4422
4423
4424
4425
4426
4427
4428
4429
4430
4431
4432
4433
4434
4435
4436
4437
4438
4439
4440
4441
4442
4443
4444
4445
4446
4447
4448
4449
4450
4451
void Vehicle::sendParamMapRC(const QString& paramName, double scale, double centerValue, int tuningID, double minValue, double maxValue)
{
mavlink_message_t message;
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
// Copy string into buffer, ensuring not to exceed the buffer size
for (unsigned int i = 0; i < sizeof(param_id_cstr); i++) {
if ((int)i < paramName.length()) {
param_id_cstr[i] = paramName.toLatin1()[i];
}
}
mavlink_msg_param_map_rc_pack_chan(static_cast<uint8_t>(_mavlink->getSystemId()),
static_cast<uint8_t>(_mavlink->getComponentId()),
priorityLink()->mavlinkChannel(),
&message,
_id,
MAV_COMP_ID_AUTOPILOT1,
param_id_cstr,
-1, // parameter name specified as string in previous argument
static_cast<uint8_t>(tuningID),
static_cast<float>(scale),
static_cast<float>(centerValue),
static_cast<float>(minValue),
static_cast<float>(maxValue));
sendMessageOnLink(priorityLink(), message);
}
void Vehicle::clearAllParamMapRC(void)
{
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
for (int i = 0; i < 3; i++) {
mavlink_message_t message;
mavlink_msg_param_map_rc_pack_chan(static_cast<uint8_t>(_mavlink->getSystemId()),
static_cast<uint8_t>(_mavlink->getComponentId()),
priorityLink()->mavlinkChannel(),
&message,
_id,
MAV_COMP_ID_AUTOPILOT1,
param_id_cstr,
-2, // Disable map for specified tuning id
i, // tuning id
0, 0, 0, 0); // unused
sendMessageOnLink(priorityLink(), message);
}
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
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::_instantPowerFactName = "instantPower";
const char* VehicleBatteryFactGroup::_timeRemainingFactName = "timeRemaining";
const char* VehicleBatteryFactGroup::_chargeStateFactName = "chargeState";
const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery";
VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent)
, _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble)
, _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeDouble)
, _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeDouble)
, _currentFact (0, _currentFactName, FactMetaData::valueTypeDouble)
, _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble)
, _instantPowerFact (0, _instantPowerFactName, FactMetaData::valueTypeDouble)
, _timeRemainingFact (0, _timeRemainingFactName, FactMetaData::valueTypeDouble)
, _chargeStateFact (0, _chargeStateFactName, FactMetaData::valueTypeUint8)
_addFact(&_voltageFact, _voltageFactName);
_addFact(&_percentRemainingFact, _percentRemainingFactName);
_addFact(&_mahConsumedFact, _mahConsumedFactName);
_addFact(&_currentFact, _currentFactName);
_addFact(&_temperatureFact, _temperatureFactName);
_addFact(&_instantPowerFact, _instantPowerFactName);
_addFact(&_timeRemainingFact, _timeRemainingFactName);
_addFact(&_chargeStateFact, _chargeStateFactName);
_voltageFact.setRawValue (qQNaN());
_percentRemainingFact.setRawValue (qQNaN());
_mahConsumedFact.setRawValue (qQNaN());
_currentFact.setRawValue (qQNaN());
_temperatureFact.setRawValue (qQNaN());
_instantPowerFact.setRawValue (qQNaN());
_timeRemainingFact.setRawValue (qQNaN());
_chargeStateFact.setRawValue (MAV_BATTERY_CHARGE_STATE_UNDEFINED);
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";
4524
4525
4526
4527
4528
4529
4530
4531
4532
4533
4534
4535
4536
4537
4538
4539
4540
4541
4542
4543
4544
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()
{
_currentTimeFact.setRawValue(QTime::currentTime().toString());
_currentDateFact.setRawValue(QDateTime::currentDateTime().toString(QLocale::system().dateFormat(QLocale::ShortFormat)));
FactGroup::_updateAllValues();
}
4589
4590
4591
4592
4593
4594
4595
4596
4597
4598
4599
4600
4601
4602
4603
4604
4605
4606
4607
4608
4609
4610
4611
4612
4613
4614
4615
4616
4617
4618
4619
4620
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)
{
_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());
4668
4669
4670
4671
4672
4673
4674
4675
4676
4677
4678
4679
4680
4681
4682
4683
4684
4685
4686
4687
4688
4689
4690
4691
4692
4693
4694
4695
4696
4697
4698
4699
4700
4701
4702
4703
4704
4705
4706
4707
4708
4709
4710
4711
4712
4713
4714
4715
4716
4717
4718
4719
4720
4721
4722
4723
4724
4725
4726
4727
4728
4729
4730
4731
4732
4733
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);
}