From 25e23f2aab2529dfe4ca46afa71daa5069034a93 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 23 Mar 2020 12:18:24 -0600 Subject: [PATCH] Added current mission item index as a fact for use with the Values Widget --- src/Vehicle/Vehicle.cc | 28 ++++++++++++++++++++++------ src/Vehicle/Vehicle.h | 5 +++++ src/Vehicle/VehicleFact.json | 6 ++++++ 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9bd2e9111..34cfb02b4 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -77,6 +77,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; const char* Vehicle::_flightDistanceFactName = "flightDistance"; const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; +const char* Vehicle::_missionItemIndexFactName = "missionItemIndex"; const char* Vehicle::_headingToNextWPFactName = "headingToNextWP"; const char* Vehicle::_headingToHomeFactName = "headingToHome"; const char* Vehicle::_distanceToGCSFactName = "distanceToGCS"; @@ -206,6 +207,7 @@ Vehicle::Vehicle(LinkInterface* link, , _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble) , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) + , _missionItemIndexFact (0, _missionItemIndexFactName, FactMetaData::valueTypeUint16) , _headingToNextWPFact (0, _headingToNextWPFactName, FactMetaData::valueTypeDouble) , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) @@ -409,6 +411,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds) , _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble) , _headingToNextWPFact (0, _headingToNextWPFactName, FactMetaData::valueTypeDouble) + , _missionItemIndexFact (0, _missionItemIndexFactName, FactMetaData::valueTypeUint16) , _headingToHomeFact (0, _headingToHomeFactName, FactMetaData::valueTypeDouble) , _distanceToGCSFact (0, _distanceToGCSFactName, FactMetaData::valueTypeDouble) , _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString) @@ -454,6 +457,7 @@ void Vehicle::_commonInit() connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints); connect(_missionManager, &MissionManager::sendComplete, this, &Vehicle::_clearCameraTriggerPoints); connect(_missionManager, &MissionManager::currentIndexChanged, this, &Vehicle::_updateHeadingToNextWP); + connect(_missionManager, &MissionManager::currentIndexChanged, this, &Vehicle::_updateMissionItemIndex); connect(_missionManager, &MissionManager::sendComplete, _trajectoryPoints, &TrajectoryPoints::clear); connect(_missionManager, &MissionManager::newMissionItemsAvailable, _trajectoryPoints, &TrajectoryPoints::clear); @@ -491,6 +495,7 @@ void Vehicle::_commonInit() _addFact(&_flightDistanceFact, _flightDistanceFactName); _addFact(&_flightTimeFact, _flightTimeFactName); _addFact(&_distanceToHomeFact, _distanceToHomeFactName); + _addFact(&_missionItemIndexFact, _missionItemIndexFactName); _addFact(&_headingToNextWPFact, _headingToNextWPFactName); _addFact(&_headingToHomeFact, _headingToHomeFactName); _addFact(&_distanceToGCSFact, _distanceToGCSFactName); @@ -3969,21 +3974,32 @@ void Vehicle::_updateDistanceHeadingToHome() void Vehicle::_updateHeadingToNextWP() { - const int _currentIndex = _missionManager->currentIndex(); - MissionItem _currentItem; + const int currentIndex = _missionManager->currentIndex(); QList llist = _missionManager->missionItems(); - if(llist.size()>_currentIndex && _currentIndex!=-1 - && llist[_currentIndex]->coordinate().longitude()!=0.0 - && coordinate().distanceTo(llist[_currentIndex]->coordinate())>5.0 ){ + 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())); + _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(); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index eb0d39b8f..92b221416 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -670,6 +670,7 @@ public: Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT) Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT) Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT) + Q_PROPERTY(Fact* missionItemIndex READ missionItemIndex CONSTANT) Q_PROPERTY(Fact* headingToNextWP READ headingToNextWP CONSTANT) Q_PROPERTY(Fact* headingToHome READ headingToHome CONSTANT) Q_PROPERTY(Fact* distanceToGCS READ distanceToGCS CONSTANT) @@ -984,6 +985,7 @@ public: Fact* altitudeAMSL () { return &_altitudeAMSLFact; } Fact* flightDistance () { return &_flightDistanceFact; } Fact* distanceToHome () { return &_distanceToHomeFact; } + Fact* missionItemIndex () { return &_missionItemIndexFact; } Fact* headingToNextWP () { return &_headingToNextWPFact; } Fact* headingToHome () { return &_headingToHomeFact; } Fact* distanceToGCS () { return &_distanceToGCSFact; } @@ -1265,6 +1267,7 @@ private slots: void _sendMavCommandAgain (); void _clearCameraTriggerPoints (); void _updateDistanceHeadingToHome (); + void _updateMissionItemIndex (); void _updateHeadingToNextWP (); void _updateDistanceToGCS (); void _updateHobbsMeter (); @@ -1566,6 +1569,7 @@ private: Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; + Fact _missionItemIndexFact; Fact _headingToNextWPFact; Fact _headingToHomeFact; Fact _distanceToGCSFact; @@ -1600,6 +1604,7 @@ private: static const char* _flightDistanceFactName; static const char* _flightTimeFactName; static const char* _distanceToHomeFactName; + static const char* _missionItemIndexFactName; static const char* _headingToNextWPFactName; static const char* _headingToHomeFactName; static const char* _distanceToGCSFactName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index 07af7b118..deb366c60 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -104,6 +104,12 @@ "decimalPlaces": 1, "units": "m" }, +{ + "name": "missionItemIndex", + "shortDescription": "Mission Item Index", + "type": "uint16", + "units": "" +}, { "name": "headingToNextWP", "shortDescription": "Next WP Heading", -- 2.22.0