@ -73,6 +73,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL";
@@ -73,6 +73,7 @@ const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL";
const char * Vehicle : : _flightDistanceFactName = " flightDistance " ;
const char * Vehicle : : _flightTimeFactName = " flightTime " ;
const char * Vehicle : : _distanceToHomeFactName = " distanceToHome " ;
const char * Vehicle : : _headingToNextWPFactName = " headingToNextWP " ;
const char * Vehicle : : _headingToHomeFactName = " headingToHome " ;
const char * Vehicle : : _distanceToGCSFactName = " distanceToGCS " ;
const char * Vehicle : : _hobbsFactName = " hobbs " ;
@ -203,6 +204,7 @@ Vehicle::Vehicle(LinkInterface* link,
@@ -203,6 +204,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _flightDistanceFact ( 0 , _flightDistanceFactName , FactMetaData : : valueTypeDouble )
, _flightTimeFact ( 0 , _flightTimeFactName , FactMetaData : : valueTypeElapsedTimeInSeconds )
, _distanceToHomeFact ( 0 , _distanceToHomeFactName , FactMetaData : : valueTypeDouble )
, _headingToNextWPFact ( 0 , _headingToNextWPFactName , FactMetaData : : valueTypeDouble )
, _headingToHomeFact ( 0 , _headingToHomeFactName , FactMetaData : : valueTypeDouble )
, _distanceToGCSFact ( 0 , _distanceToGCSFactName , FactMetaData : : valueTypeDouble )
, _hobbsFact ( 0 , _hobbsFactName , FactMetaData : : valueTypeString )
@ -404,6 +406,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
@@ -404,6 +406,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _flightDistanceFact ( 0 , _flightDistanceFactName , FactMetaData : : valueTypeDouble )
, _flightTimeFact ( 0 , _flightTimeFactName , FactMetaData : : valueTypeElapsedTimeInSeconds )
, _distanceToHomeFact ( 0 , _distanceToHomeFactName , FactMetaData : : valueTypeDouble )
, _headingToNextWPFact ( 0 , _headingToNextWPFactName , FactMetaData : : valueTypeDouble )
, _headingToHomeFact ( 0 , _headingToHomeFactName , FactMetaData : : valueTypeDouble )
, _distanceToGCSFact ( 0 , _distanceToGCSFactName , FactMetaData : : valueTypeDouble )
, _hobbsFact ( 0 , _hobbsFactName , FactMetaData : : valueTypeString )
@ -438,6 +441,7 @@ void Vehicle::_commonInit(void)
@@ -438,6 +441,7 @@ void Vehicle::_commonInit(void)
connect ( this , & Vehicle : : homePositionChanged , this , & Vehicle : : _updateDistanceHeadingToHome ) ;
connect ( this , & Vehicle : : hobbsMeterChanged , this , & Vehicle : : _updateHobbsMeter ) ;
connect ( _toolbox - > qgcPositionManager ( ) , & QGCPositionManager : : gcsPositionChanged , this , & Vehicle : : _updateDistanceToGCS ) ;
_missionManager = new MissionManager ( this ) ;
@ -447,6 +451,7 @@ void Vehicle::_commonInit(void)
@@ -447,6 +451,7 @@ void Vehicle::_commonInit(void)
connect ( _missionManager , & MissionManager : : newMissionItemsAvailable , this , & Vehicle : : _clearTrajectoryPoints ) ;
connect ( _missionManager , & MissionManager : : sendComplete , this , & Vehicle : : _clearCameraTriggerPoints ) ;
connect ( _missionManager , & MissionManager : : sendComplete , this , & Vehicle : : _clearTrajectoryPoints ) ;
connect ( _missionManager , & MissionManager : : currentIndexChanged , this , & Vehicle : : _updateHeadingToNextWP ) ;
_parameterManager = new ParameterManager ( this ) ;
connect ( _parameterManager , & ParameterManager : : parametersReadyChanged , this , & Vehicle : : _parametersReady ) ;
@ -481,6 +486,7 @@ void Vehicle::_commonInit(void)
@@ -481,6 +486,7 @@ void Vehicle::_commonInit(void)
_addFact ( & _flightDistanceFact , _flightDistanceFactName ) ;
_addFact ( & _flightTimeFact , _flightTimeFactName ) ;
_addFact ( & _distanceToHomeFact , _distanceToHomeFactName ) ;
_addFact ( & _headingToNextWPFact , _headingToNextWPFactName ) ;
_addFact ( & _headingToHomeFact , _headingToHomeFactName ) ;
_addFact ( & _distanceToGCSFact , _distanceToGCSFactName ) ;
_addFact ( & _throttlePctFact , _throttlePctFactName ) ;
@ -3795,6 +3801,23 @@ void Vehicle::_updateDistanceHeadingToHome(void)
@@ -3795,6 +3801,23 @@ void Vehicle::_updateDistanceHeadingToHome(void)
}
}
void Vehicle : : _updateHeadingToNextWP ( void )
{
const int _currentIndex = _missionManager - > currentIndex ( ) ;
MissionItem _currentItem ;
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 : : _updateDistanceToGCS ( void )
{
QGeoCoordinate gcsPosition = _toolbox - > qgcPositionManager ( ) - > gcsPosition ( ) ;