zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

QGC4.4
Jacob Dahl 5 years ago
parent
6127e4bf11
commit
25e23f2aab
3 changed files with 33 additions and 6 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 28
      src/Vehicle/Vehicle.cc
  2. 5
      src/Vehicle/Vehicle.h
  3. 6
      src/Vehicle/VehicleFact.json

28
src/Vehicle/Vehicle.cc
Unescape View File

@ -77,6 +77,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL"; @@ -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, @@ -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, @@ -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() @@ -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() @@ -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() @@ -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();

5
src/Vehicle/Vehicle.h
Unescape View File

@ -670,6 +670,7 @@ public: @@ -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: @@ -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: @@ -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: @@ -1566,6 +1569,7 @@ private:
Fact _flightDistanceFact;
Fact _flightTimeFact;
Fact _distanceToHomeFact;
Fact _missionItemIndexFact;
Fact _headingToNextWPFact;
Fact _headingToHomeFact;
Fact _distanceToGCSFact;
@ -1600,6 +1604,7 @@ private: @@ -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;

6
src/Vehicle/VehicleFact.json
Unescape View File

@ -105,6 +105,12 @@ @@ -105,6 +105,12 @@
"units": "m"
},
{
"name": "missionItemIndex",
"shortDescription": "Mission Item Index",
"type": "uint16",
"units": ""
},
{
"name": "headingToNextWP",
"shortDescription": "Next WP Heading",
"type": "double",

Write Preview
Loading…
Cancel
Save