From 957710b4d3a07a64f0f930b759b03a3055523cdb Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 24 Oct 2015 12:37:31 -0700 Subject: [PATCH] Editor moves to vehicle position once it gets gps lock --- src/FlightDisplay/FlightDisplayView.qml | 2 +- .../FlightDisplayViewDelayLoadInner.qml | 3 +- src/FlightMap/MapItems/VehicleMapItem.qml | 2 +- src/MissionEditor/MissionEditor.qml | 16 ++++++- src/Vehicle/Vehicle.cc | 30 +++++++------ src/Vehicle/Vehicle.h | 45 +++++++++---------- src/uas/UAS.cc | 5 ++- 7 files changed, 60 insertions(+), 43 deletions(-) diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index ec7d26443..c39f30657 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -73,7 +73,7 @@ Item { property real _pitch: _activeVehicle ? (isNaN(_activeVehicle.pitch) ? _defaultPitch : _activeVehicle.pitch) : _defaultPitch property real _heading: _activeVehicle ? (isNaN(_activeVehicle.heading) ? _defaultHeading : _activeVehicle.heading) : _defaultHeading - property var _vehicleCoordinate: _activeVehicle ? (_activeVehicle.satelliteLock >= 2 ? _activeVehicle.coordinate : _defaultVehicleCoordinate) : _defaultVehicleCoordinate + property var _vehicleCoordinate: _activeVehicle ? (_activeVehicle.coordinateValid ? _activeVehicle.coordinate : _defaultVehicleCoordinate) : _defaultVehicleCoordinate property real _altitudeWGS84: _activeVehicle ? _activeVehicle.altitudeWGS84 : _defaultAltitudeWGS84 property real _groundSpeed: _activeVehicle ? _activeVehicle.groundSpeed : _defaultGroundSpeed diff --git a/src/FlightDisplay/FlightDisplayViewDelayLoadInner.qml b/src/FlightDisplay/FlightDisplayViewDelayLoadInner.qml index b530032ba..50fb3239f 100644 --- a/src/FlightDisplay/FlightDisplayViewDelayLoadInner.qml +++ b/src/FlightDisplay/FlightDisplayViewDelayLoadInner.qml @@ -51,7 +51,7 @@ Item { QGCLabel { width: gpsLockColumn.width horizontalAlignment: Text.AlignHCenter - visible: object.satelliteLock < 2 + visible: !object.coordinateValid text: "No GPS Lock for Vehicle #" + object.id z: QGroundControl.zOrderMapItems - 2 } @@ -65,7 +65,6 @@ Item { anchors.top: parent.top size: ScreenTools.defaultFontPixelSize * (13.3) heading: _heading - active: multiVehicleManager.activeVehicleAvailable z: QGroundControl.zOrderWidgets } diff --git a/src/FlightMap/MapItems/VehicleMapItem.qml b/src/FlightMap/MapItems/VehicleMapItem.qml index 2b8dcea16..c3b578be3 100644 --- a/src/FlightMap/MapItems/VehicleMapItem.qml +++ b/src/FlightMap/MapItems/VehicleMapItem.qml @@ -38,7 +38,7 @@ MapQuickItem { anchorPoint.x: vehicleIcon.width / 2 anchorPoint.y: vehicleIcon.height / 2 - visible: vehicle.satelliteLock >= 2 // 2D lock + visible: vehicle.coordinateValid sourceItem: Image { id: vehicleIcon diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index b53e58f0c..0852f3a75 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -56,7 +56,10 @@ QGCView { readonly property int _addMissionItemsButtonAutoOffTimeout: 10000 readonly property var _defaultVehicleCoordinate: QtPositioning.coordinate(37.803784, -122.462276) - property var _missionItems: controller.missionItems + property var _missionItems: controller.missionItems + + property bool gpsLock: _activeVehicle ? _activeVehicle.coordinateValid : false + property bool _firstGpsLock: true //property var _homePositionManager: QGroundControl.homePositionManager //property string _homePositionName: _homePositionManager.homePositions.get(0).name @@ -71,6 +74,17 @@ QGCView { property bool _showHelp: QGroundControl.flightMapSettings.loadBoolMapSetting(editorMap.mapName, _showHelpKey, true) + onGpsLockChanged: updateMapToVehiclePosition() + Component.onCompleted: updateMapToVehiclePosition() + + function updateMapToVehiclePosition() { + if (gpsLock && _firstGpsLock) { + _firstGpsLock = false + editorMap.latitude = _activeVehicle.latitude + editorMap.longitude = _activeVehicle.longitude + } + } + MissionController { id: controller diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 824b860b9..1991995ea 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -53,6 +53,8 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, , _joystickMode(JoystickModeRC) , _joystickEnabled(false) , _uas(NULL) + , _coordinate(37.803784, -122.462276) + , _coordinateValid(false) , _homePositionAvailable(false) , _mav(NULL) , _currentMessageCount(0) @@ -74,8 +76,6 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType, , _navigationSpeedError(0.0f) , _navigationCrosstrackError(0.0f) , _navigationTargetBearing(0.0f) - , _latitude(DEFAULT_LAT) - , _longitude(DEFAULT_LON) , _refreshTimer(new QTimer(this)) , _batteryVoltage(-1.0f) , _batteryPercent(0.0) @@ -322,13 +322,13 @@ QList Vehicle::links(void) void Vehicle::setLatitude(double latitude) { - _geoCoordinate.setLatitude(latitude); - emit coordinateChanged(_geoCoordinate); + _coordinate.setLatitude(latitude); + emit coordinateChanged(_coordinate); } void Vehicle::setLongitude(double longitude){ - _geoCoordinate.setLongitude(longitude); - emit coordinateChanged(_geoCoordinate); + _coordinate.setLongitude(longitude); + emit coordinateChanged(_coordinate); } void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64) @@ -498,13 +498,11 @@ void Vehicle::_checkUpdate() { // Update current location if(_mav) { - if(_latitude != _mav->getLatitude()) { - _latitude = _mav->getLatitude(); - emit latitudeChanged(); + if(latitude() != _mav->getLatitude()) { + setLatitude(_mav->getLatitude()); } - if(_longitude != _mav->getLongitude()) { - _longitude = _mav->getLongitude(); - emit longitudeChanged(); + if(longitude() != _mav->getLongitude()) { + setLongitude(_mav->getLongitude()); } } // The timer rate is 20Hz for the coordinates above. These below we only check @@ -705,6 +703,10 @@ void Vehicle::_setSatLoc(UASInterface*, int fix) { // fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock if(_satelliteLock != fix) { + if (fix > 2) { + _coordinateValid = true; + emit coordinateValidChanged(true); + } _satelliteLock = fix; emit satelliteLockChanged(); } @@ -1086,10 +1088,10 @@ void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg) void Vehicle::_addNewMapTrajectoryPoint(void) { if (_mapTrajectoryHaveFirstCoordinate) { - _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _geoCoordinate, this)); + _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this)); } _mapTrajectoryHaveFirstCoordinate = true; - _mapTrajectoryLastCoordinate = _geoCoordinate; + _mapTrajectoryLastCoordinate = _coordinate; } void Vehicle::_mapTrajectoryStart(void) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2377081bc..1a3a6ee72 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -56,23 +56,23 @@ public: Q_PROPERTY(int id READ id CONSTANT) Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) - Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged) - Q_PROPERTY(MissionManager* missionManager MEMBER _missionManager CONSTANT) - - Q_PROPERTY(bool homePositionAvailable READ homePositionAvailable NOTIFY homePositionAvailableChanged) - Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged) - - Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged) + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged) + Q_PROPERTY(bool coordinateValid READ coordinateValid NOTIFY coordinateValidChanged) + Q_PROPERTY(MissionManager* missionManager MEMBER _missionManager CONSTANT) + Q_PROPERTY(bool homePositionAvailable READ homePositionAvailable NOTIFY homePositionAvailableChanged) + Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged) + Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged) + Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT) + Q_PROPERTY(QStringList flightModes READ flightModes CONSTANT) + Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) + Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) + Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) + Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) - Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT) - Q_PROPERTY(QStringList flightModes READ flightModes CONSTANT) - Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged) + // Property accessors - Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged) - - Q_PROPERTY(bool missingParameters READ missingParameters NOTIFY missingParametersChanged) - - Q_PROPERTY(QmlObjectListModel* trajectoryPoints READ trajectoryPoints CONSTANT) + QGeoCoordinate coordinate(void) { return _coordinate; } + bool coordinateValid(void) { return _coordinateValid; } Q_INVOKABLE QString getMavIconColor(); @@ -94,8 +94,8 @@ public: Q_PROPERTY(float altitudeRelative READ altitudeRelative NOTIFY altitudeRelativeChanged) Q_PROPERTY(float altitudeWGS84 READ altitudeWGS84 NOTIFY altitudeWGS84Changed) Q_PROPERTY(float altitudeAMSL READ altitudeAMSL NOTIFY altitudeAMSLChanged) - Q_PROPERTY(float latitude READ latitude NOTIFY latitudeChanged) - Q_PROPERTY(float longitude READ longitude NOTIFY longitudeChanged) + Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) + Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) Q_PROPERTY(double batteryVoltage READ batteryVoltage NOTIFY batteryVoltageChanged) Q_PROPERTY(double batteryPercent READ batteryPercent NOTIFY batteryPercentChanged) Q_PROPERTY(double batteryConsumed READ batteryConsumed NOTIFY batteryConsumedChanged) @@ -234,8 +234,8 @@ public: float altitudeRelative () { return _altitudeRelative; } float altitudeWGS84 () { return _altitudeWGS84; } float altitudeAMSL () { return _altitudeAMSL; } - float latitude () { return _latitude; } - float longitude () { return _longitude; } + float latitude () { return _coordinate.latitude(); } + float longitude () { return _coordinate.longitude(); } bool mavPresent () { return _mav != NULL; } int satelliteCount () { return _satelliteCount; } double batteryVoltage () { return _batteryVoltage; } @@ -256,6 +256,7 @@ public slots: signals: void allLinksDisconnected(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); + void coordinateValidChanged(bool coordinateValid); void joystickModeChanged(int mode); void joystickEnabledChanged(bool enabled); void activeChanged(bool active); @@ -283,7 +284,6 @@ signals: void altitudeRelativeChanged(); void altitudeWGS84Changed (); void altitudeAMSLChanged (); - void latitudeChanged (); void longitudeChanged (); void batteryVoltageChanged (); void batteryPercentChanged (); @@ -366,7 +366,8 @@ private: UAS* _uas; - QGeoCoordinate _geoCoordinate; + QGeoCoordinate _coordinate; + bool _coordinateValid; ///< true: vehicle has 3d lock and therefore valid location bool _homePositionAvailable; QGeoCoordinate _homePosition; @@ -392,8 +393,6 @@ private: float _navigationSpeedError; float _navigationCrosstrackError; float _navigationTargetBearing; - float _latitude; - float _longitude; QTimer* _refreshTimer; QList _changes; double _batteryVoltage; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 142b24750..230667be6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -758,7 +758,6 @@ void UAS::receiveMessage(mavlink_message_t message) { loc_type = 0; } - emit localizationChanged(this, loc_type); setSatelliteCount(pos.satellites_visible); if (pos.fix_type > 2) @@ -788,6 +787,10 @@ void UAS::receiveMessage(mavlink_message_t message) } } } + + // Emit this signal after the above signals. This way a trigger on gps lock signal which then asks for vehicle position + // gets a good position. + emit localizationChanged(this, loc_type); } break; case MAVLINK_MSG_ID_GPS_STATUS: -- 2.22.0