Commit 25e23f2a authored by Jacob Dahl's avatar Jacob Dahl

Added current mission item index as a fact for use with the Values Widget

parent 6127e4bf
......@@ -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<MissionItem*> 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();
......
......@@ -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;
......
......@@ -104,6 +104,12 @@
"decimalPlaces": 1,
"units": "m"
},
{
"name": "missionItemIndex",
"shortDescription": "Mission Item Index",
"type": "uint16",
"units": ""
},
{
"name": "headingToNextWP",
"shortDescription": "Next WP Heading",
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment