diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index d2e0c61a0d84310d6941907fd196059909bbd2d6..d9c7adeaffc946e2ac1fa46adae632d18aae0882 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -76,6 +76,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"; @@ -205,6 +206,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) @@ -413,6 +415,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) @@ -458,6 +461,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); @@ -495,6 +499,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); @@ -3982,21 +3987,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 879c4baa2339630e30b596a91c2ba1ea0bf6f199..33cf7ed76ed2f276e0f9389ca6c311e4705597d9 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) @@ -981,6 +982,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; } @@ -1259,6 +1261,7 @@ private slots: void _sendMavCommandAgain (); void _clearCameraTriggerPoints (); void _updateDistanceHeadingToHome (); + void _updateMissionItemIndex (); void _updateHeadingToNextWP (); void _updateDistanceToGCS (); void _updateHobbsMeter (); @@ -1570,6 +1573,7 @@ private: Fact _flightDistanceFact; Fact _flightTimeFact; Fact _distanceToHomeFact; + Fact _missionItemIndexFact; Fact _headingToNextWPFact; Fact _headingToHomeFact; Fact _distanceToGCSFact; @@ -1604,6 +1608,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 07af7b11872e52139e35c51dcdb68d9ed971552e..41aaf28eaf1a95c994ba0a169ebcda9b7e3ba6ed 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -104,6 +104,11 @@ "decimalPlaces": 1, "units": "m" }, +{ + "name": "missionItemIndex", + "shortDescription": "Mission Item Index", + "type": "uint16" +}, { "name": "headingToNextWP", "shortDescription": "Next WP Heading",