QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
VisualMissionItem.cc
Go to the documentation of this file.
1#include "VisualMissionItem.h"
2#include "TerrainQuery.h"
4#include "Vehicle.h"
5#include "QGC.h"
6
7// All VisualMissionItem derived classes are parented to masterController in order to tie their lifecycles together.
8
10 : QObject (masterController)
11 , _flyView (flyView)
12 , _masterController (masterController)
13 , _missionController(masterController->missionController())
14 , _controllerVehicle(masterController->controllerVehicle())
15{
16 _commonInit();
17}
18
20 : QObject (other._masterController)
21 , _flyView (flyView)
22{
23 *this = other;
24
25 _commonInit();
26}
27
28void VisualMissionItem::_commonInit(void)
29{
30 // Don't get terrain altitude information for submarines or boats
31 Vehicle* controllerVehicle = _masterController->controllerVehicle();
32 if (controllerVehicle->vehicleType() != MAV_TYPE_SUBMARINE && controllerVehicle->vehicleType() != MAV_TYPE_SURFACE_BOAT) {
33 _updateTerrainTimer.setInterval(500);
34 _updateTerrainTimer.setSingleShot(true);
35 connect(&_updateTerrainTimer, &QTimer::timeout, this, &VisualMissionItem::_reallyUpdateTerrainAltitude);
36
37 connect(this, &VisualMissionItem::coordinateChanged, this, &VisualMissionItem::_updateTerrainAltitude);
38 }
39}
40
61
65
73
81
82void VisualMissionItem::setDistance(double distance)
83{
87 }
88}
89
97
105
113
121
129
131{
135 }
136}
137
139{
140 if (!QGC::fuzzyCompare(missionFlightStatus.gimbalYaw, _missionGimbalYaw)) {
141 _missionGimbalYaw = missionFlightStatus.gimbalYaw;
143 }
144 if (missionFlightStatus.vtolMode != _previousVTOLMode) {
145 _previousVTOLMode = missionFlightStatus.vtolMode;
147 }
148}
149
151{
152 if (!QGC::fuzzyCompare(_missionVehicleYaw, vehicleYaw)) {
153 _missionVehicleYaw = vehicleYaw;
155 }
156}
157
158void VisualMissionItem::_updateTerrainAltitude(void)
159{
160 if (coordinate().latitude() == 0 && coordinate().longitude() == 0) {
161 // This is an intermediate state we don't react to
162 return;
163 }
164
165 _terrainAltitude = qQNaN();
166 emit terrainAltitudeChanged(qQNaN());
167
168 if (!_flyView && specifiesCoordinate() && coordinate().isValid()) {
169 // We use a timer so that any additional requests before the timer fires result in only a single request
170 _updateTerrainTimer.start();
171 }
172}
173
174void VisualMissionItem::_reallyUpdateTerrainAltitude(void)
175{
176 QGeoCoordinate coord = coordinate();
177 if (specifiesCoordinate() && coord.isValid() && (qIsNaN(_terrainAltitude) || !QGC::fuzzyCompare(_lastLatTerrainQuery, coord.latitude()) || !QGC::fuzzyCompare(_lastLonTerrainQuery, coord.longitude()))) {
178 _lastLatTerrainQuery = coord.latitude();
179 _lastLonTerrainQuery = coord.longitude();
180 if (_currentTerrainAtCoordinateQuery) {
181 disconnect(_currentTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &VisualMissionItem::_terrainDataReceived);
182 _currentTerrainAtCoordinateQuery = nullptr;
183 }
184 _currentTerrainAtCoordinateQuery = new TerrainAtCoordinateQuery(true /* autoDelet */);
185 connect(_currentTerrainAtCoordinateQuery, &TerrainAtCoordinateQuery::terrainDataReceived, this, &VisualMissionItem::_terrainDataReceived);
186 QList<QGeoCoordinate> rgCoord;
187 rgCoord.append(coordinate());
188 _currentTerrainAtCoordinateQuery->requestData(rgCoord);
189 }
190}
191
192void VisualMissionItem::_terrainDataReceived(bool success, QList<double> heights)
193{
194 _terrainAltitude = success ? heights[0] : qQNaN();
196 _currentTerrainAtCoordinateQuery = nullptr;
197}
198
200{
201 if (bc != _boundingCube) {
202 _boundingCube = bc;
203 emit boundingCubeChanged();
204 }
205}
206
208{
209 if (wizardMode != _wizardMode) {
212 }
213}
214
222
227
Master controller for mission, fence, rally.
Vehicle * controllerVehicle(void)
NOTE: TerrainAtCoordinateQuery is not thread safe. All instances/calls to ElevationProvider must be o...
void terrainDataReceived(bool success, const QList< double > &heights)
void requestData(const QList< QGeoCoordinate > &coordinates)
MAV_TYPE vehicleType() const
Definition Vehicle.h:428
void setIsCurrentItem(bool isCurrentItem)
void setDistanceFromStart(double distanceFromStart)
void setHasCurrentChildItem(bool hasCurrentChildItem)
PlanMasterController * _masterController
VisualMissionItem * _parentItem
bool _homePositionSpecialCase
true: This item is being used as a ui home position indicator
void distanceChanged(double distance)
void _setBoundingCube(QGCGeoBoundingCube bc)
void setTerrainCollision(bool terrainCollision)
void setAltDifference(double altDifference)
void setMissionVehicleYaw(double vehicleYaw)
bool hasCurrentChildItem(void) const
void amslExitAltChanged(double alt)
double altDifference(void) const
virtual double amslExitAlt(void) const =0
void setParentItem(VisualMissionItem *parentItem)
void terrainAltitudeChanged(double terrainAltitude)
void isCurrentItemChanged(bool isCurrentItem)
void terrainCollisionChanged(bool terrainCollision)
void azimuthChanged(double azimuth)
void setWizardMode(bool wizardMode)
bool isCurrentItem(void) const
double _distanceFromStart
Flight path cumalative horizontal distance from home point to this item.
double altPercent(void) const
void coordinateChanged(const QGeoCoordinate &coordinate)
double terrainPercent(void) const
void setDistance(double distance)
void missionGimbalYawChanged(double missionGimbalYaw)
virtual double amslEntryAlt(void) const =0
void setAltPercent(double altPercent)
double distance(void) const
void altDifferenceChanged(double altDifference)
void amslEntryAltChanged(double alt)
void setTerrainPercent(double terrainPercent)
virtual void setMissionFlightStatus(MissionController::MissionFlightStatus_t &missionFlightStatus)
double _terrainAltitude
Altitude of terrain at coordinate position, NaN for not known.
double _terrainPercent
Percent of terrain altitude for coordinate.
double _altDifference
Difference in altitude from previous waypoint.
void parentItemChanged(VisualMissionItem *parentItem)
double distanceFromStart(void) const
bool _wizardMode
true: Item editor is showing wizard completion panel
void _amslEntryAltChanged(void)
bool wizardMode(void) const
virtual QGeoCoordinate coordinate(void) const =0
void boundingCubeChanged(void)
void previousVTOLModeChanged(void)
QGCMAVLink::VehicleClass_t _previousVTOLMode
Generic == unknown.
VisualMissionItem(PlanMasterController *masterController, bool flyView)
virtual void setDirty(bool dirty)=0
double _distance
Distance to previous waypoint.
virtual bool specifiesCoordinate(void) const =0
void setAzimuth(double azimuth)
QGCGeoBoundingCube _boundingCube
The bounding "cube" of this element.
bool _terrainCollision
true: item collides with terrain
const VisualMissionItem & operator=(const VisualMissionItem &other)
void wizardModeChanged(bool wizardMode)
void hasCurrentChildItemChanged(bool hasCurrentChildItem)
void altPercentChanged(double altPercent)
double _altPercent
Percent of total altitude change in mission.
VisualMissionItem * parentItem(void)
double azimuth(void) const
bool terrainCollision(void) const
void missionVehicleYawChanged(double missionVehicleYaw)
double _azimuth
Azimuth to previous waypoint.
void terrainPercentChanged(double terrainPercent)
void distanceFromStartChanged(double distanceFromStart)
bool fuzzyCompare(double value1, double value2)
Returns true if the two values are equal or close. Correctly handles 0 and NaN values.
Definition QGC.cc:106
QGCMAVLink::VehicleClass_t vtolMode
Either VehicleClassFixedWing, VehicleClassMultiRotor, VehicleClassGeneric (mode unknown)
double gimbalYaw
NaN signals yaw was never changed.