Commit 00f02f07 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #4901 from DonLakeFlyer/BugFix

Pile of bugs fixes around map positioning
parents bd61552b 6c4e1c0c
......@@ -401,7 +401,7 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord
void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
if (!vehicle->homePositionAvailable()) {
if (!vehicle->homePosition().isValid()) {
qgcApp()->showMessage(tr("Unable to change altitude, home position unknown."));
return;
}
......
......@@ -133,6 +133,7 @@ FlightMap {
var visualItem = missionController.visualItems
if (visualItems && visualItems.count != 1) {
mapFitFunctions.fitMapViewportToMissionItems()
firstVehiclePositionReceived = true
}
}
}
......@@ -309,7 +310,7 @@ FlightMap {
map: flightMap
myGeoFenceController: geoFenceController
interactive: false
homePosition: _activeVehicle && _activeVehicle.homePositionAvailable ? _activeVehicle.homePosition : undefined
homePosition: _activeVehicle && _activeVehicle.homePosition.isValid ? _activeVehicle.homePosition : undefined
}
// Rally points on map
......
......@@ -112,7 +112,7 @@ Item {
QGCLabel {
anchors.horizontalCenter: parent.horizontalCenter
visible: _activeVehicle && !_activeVehicle.coordinateValid && _mainIsMap
visible: _activeVehicle && !_activeVehicle.coordinate.isValid && _mainIsMap
z: QGroundControl.zOrderTopMost
color: mapPal.text
font.pointSize: ScreenTools.largeFontPointSize
......
......@@ -54,7 +54,6 @@ Map {
function _possiblyCenterToVehiclePosition() {
if (!firstVehiclePositionReceived && allowVehicleLocationCenter && activeVehicleCoordinate.isValid) {
console.log("Moving to initial vehicle position", QGroundControl.flightMapInitialZoom)
firstVehiclePositionReceived = true
center = activeVehicleCoordinate
zoomLevel = QGroundControl.flightMapInitialZoom
......
......@@ -26,7 +26,7 @@ MapQuickItem {
anchorPoint.x: vehicleIcon.width / 2
anchorPoint.y: vehicleIcon.height / 2
visible: vehicle && vehicle.coordinateValid
visible: vehicle && vehicle.coordinate.isValid
sourceItem: Image {
id: vehicleIcon
......
......@@ -139,7 +139,7 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque
_visualItems = newControllerMissionItems;
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() || _visualItems->count() == 0) {
_addMissionSettings(_activeVehicle, _visualItems, _visualItems->count() > 0 /* addToCenter */);
_addMissionSettings(_activeVehicle, _visualItems, _editMode && _visualItems->count() > 0 /* addToCenter */);
}
_missionItemsRequested = false;
......@@ -753,7 +753,10 @@ void MissionController::saveToFile(const QString& filename)
file.write(saveDoc.toJson());
}
_visualItems->setDirty(false);
// If we are connected to a real vehicle, don't clear dirty bit on saving to file since vehicle is still out of date
if (_activeVehicle->isOfflineEditingVehicle()) {
_visualItems->setDirty(false);
}
}
void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
......@@ -764,10 +767,6 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
// Convert to fixed altitudes
qCDebug(MissionControllerLog) << homeAlt
<< currentItem->coordinateHasRelativeAltitude() << currentItem->coordinate().altitude()
<< prevItem->exitCoordinateHasRelativeAltitude() << prevItem->exitCoordinate().altitude();
distanceOk = true;
if (currentItem->coordinateHasRelativeAltitude()) {
currentCoord.setAltitude(homeAlt + currentCoord.altitude());
......@@ -776,8 +775,6 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
prevCoord.setAltitude(homeAlt + prevCoord.altitude());
}
qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;
if (distanceOk) {
*altDifference = currentCoord.altitude() - prevCoord.altitude();
*distance = prevCoord.distanceTo(currentCoord);
......@@ -797,8 +794,6 @@ double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, Vi
distanceOk = true;
qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;
return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0;
}
......@@ -809,7 +804,7 @@ void MissionController::_recalcWaypointLines(void)
bool showHomePosition = _settingsItem->coordinate().isValid();
qCDebug(MissionControllerLog) << "_recalcWaypointLines";
qCDebug(MissionControllerLog) << "_recalcWaypointLines showHomePosition" << showHomePosition;
CoordVectHashTable old_table = _linesTable;
_linesTable.clear();
......@@ -1178,7 +1173,9 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(void)
void MissionController::_recalcAll(void)
{
_setPlannedHomePositionFromFirstCoordinate();
if (_editMode) {
_setPlannedHomePositionFromFirstCoordinate();
}
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
......
......@@ -412,8 +412,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
mavlink_msg_mission_item_int_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
......@@ -428,8 +428,8 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
mavlink_msg_mission_item_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
......@@ -440,6 +440,13 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m
isCurrentItem = missionItem.current;
seq = missionItem.seq;
}
// We don't support editing ALT_INT frames so change on the way in.
if (frame == MAV_FRAME_GLOBAL_INT) {
frame = MAV_FRAME_GLOBAL;
} else if (frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << seq << command;
......
......@@ -364,6 +364,9 @@ void MissionSettingsItem::_setDirty(void)
void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate)
{
if (coordinate.isValid()) {
qDebug() << "MissionSettingsItem::setCoordinate" << coordinate.isValid();
}
if (_plannedHomePositionCoordinate != coordinate) {
_plannedHomePositionCoordinate = coordinate;
emit coordinateChanged(coordinate);
......
......@@ -502,11 +502,6 @@ bool SimpleMissionItem::friendlyEditAllowed(void) const
switch (frame) {
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
#if 0
Coming soon
case MAV_FRAME_GLOBAL_INT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
#endif
return true;
break;
......
......@@ -57,8 +57,6 @@ const char* Vehicle::_windFactGroupName = "wind";
const char* Vehicle::_vibrationFactGroupName = "vibration";
const char* Vehicle::_temperatureFactGroupName = "temperature";
const int Vehicle::_lowBatteryAnnounceRepeatMSecs = 30 * 1000;
Vehicle::Vehicle(LinkInterface* link,
int vehicleId,
int defaultComponentId,
......@@ -82,9 +80,6 @@ Vehicle::Vehicle(LinkInterface* link,
, _joystickMode(JoystickModeRC)
, _joystickEnabled(false)
, _uas(NULL)
, _coordinate(37.803784, -122.462276)
, _coordinateValid(false)
, _homePositionAvailable(false)
, _mav(NULL)
, _currentMessageCount(0)
, _messageCount(0)
......@@ -141,6 +136,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _firmwareMinorVersion(versionNotSetValue)
, _firmwarePatchVersion(versionNotSetValue)
, _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
, _lastAnnouncedLowBatteryPercent(100)
, _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble)
, _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble)
, _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble)
......@@ -220,9 +216,6 @@ Vehicle::Vehicle(LinkInterface* link,
_mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints);
connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint);
// Invalidate the timer to signal first announce
_lowBatteryAnnounceTimer.invalidate();
}
// Disconnected Vehicle for offline editing
......@@ -246,9 +239,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _joystickMode(JoystickModeRC)
, _joystickEnabled(false)
, _uas(NULL)
, _coordinate(37.803784, -122.462276)
, _coordinateValid(false)
, _homePositionAvailable(false)
, _mav(NULL)
, _currentMessageCount(0)
, _messageCount(0)
......@@ -298,6 +288,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _firmwareMinorVersion(versionNotSetValue)
, _firmwarePatchVersion(versionNotSetValue)
, _gitHash(versionNotSetValue)
, _lastAnnouncedLowBatteryPercent(100)
, _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble)
, _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble)
, _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble)
......@@ -636,10 +627,6 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
_gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.epv / 100.0);
_gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.cog / 100.0);
_gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type);
if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
_setCoordinateValid(true);
}
}
void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
......@@ -854,11 +841,11 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message)
}
_batteryFactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining);
if (sysStatus.battery_remaining > 0 && sysStatus.battery_remaining < _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt()) {
if (!_lowBatteryAnnounceTimer.isValid() || _lowBatteryAnnounceTimer.elapsed() > _lowBatteryAnnounceRepeatMSecs) {
_lowBatteryAnnounceTimer.restart();
_say(QString("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining));
}
if (sysStatus.battery_remaining > 0 &&
sysStatus.battery_remaining < _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt() &&
sysStatus.battery_remaining < _lastAnnouncedLowBatteryPercent) {
_lastAnnouncedLowBatteryPercent = sysStatus.battery_remaining;
_say(QString("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining));
}
_onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present;
......@@ -903,9 +890,6 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message)
void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
bool emitHomePositionChanged = false;
bool emitHomePositionAvailableChanged = false;
mavlink_home_position_t homePos;
mavlink_msg_home_position_decode(&message, &homePos);
......@@ -913,23 +897,10 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message)
QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
homePos.longitude / 10000000.0,
homePos.altitude / 1000.0);
if (!_homePositionAvailable || newHomePosition != _homePosition) {
emitHomePositionChanged = true;
if (newHomePosition != _homePosition) {
_homePosition = newHomePosition;
}
if (!_homePositionAvailable) {
emitHomePositionAvailableChanged = true;
_homePositionAvailable = true;
}
if (emitHomePositionChanged) {
qCDebug(VehicleLog) << "New home position" << newHomePosition;
emit homePositionChanged(_homePosition);
}
if (emitHomePositionAvailableChanged) {
emit homePositionAvailableChanged(true);
}
}
void Vehicle::_handleHeartbeat(mavlink_message_t& message)
......@@ -1173,22 +1144,6 @@ void Vehicle::_updatePriorityLink(void)
}
}
void Vehicle::setLatitude(double latitude)
{
_coordinate.setLatitude(latitude);
emit coordinateChanged(_coordinate);
}
void Vehicle::setLongitude(double longitude){
_coordinate.setLongitude(longitude);
emit coordinateChanged(_coordinate);
}
void Vehicle::setAltitude(double altitude){
_coordinate.setAltitude(altitude);
emit coordinateChanged(_coordinate);
}
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
if (qIsInf(roll)) {
......@@ -1476,13 +1431,9 @@ void Vehicle::setActive(bool active)
_startJoystick(_active);
}
bool Vehicle::homePositionAvailable(void)
{
return _homePositionAvailable;
}
QGeoCoordinate Vehicle::homePosition(void)
{
qDebug() << "Vehicle::homePosition" << _homePosition.isValid();
return _homePosition;
}
......@@ -1852,14 +1803,6 @@ bool Vehicle::supportsMotorInterference(void) const
return _firmwarePlugin->supportsMotorInterference();
}
void Vehicle::_setCoordinateValid(bool coordinateValid)
{
if (coordinateValid != _coordinateValid) {
_coordinateValid = coordinateValid;
emit coordinateValidChanged(_coordinateValid);
}
}
QString Vehicle::vehicleTypeName() const {
static QMap<int, QString> typeNames = {
{ MAV_TYPE_GENERIC, tr("Generic micro air vehicle" )},
......
......@@ -238,8 +238,6 @@ public:
Q_PROPERTY(int id READ id CONSTANT)
Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT)
Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged)
Q_PROPERTY(bool coordinateValid READ coordinateValid NOTIFY coordinateValidChanged)
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)
......@@ -436,8 +434,6 @@ public:
// Property accessors
QGeoCoordinate coordinate(void) { return _coordinate; }
bool coordinateValid(void) { return _coordinateValid; }
void _setCoordinateValid(bool coordinateValid);
typedef enum {
JoystickModeRC, ///< Joystick emulates an RC Transmitter
......@@ -494,7 +490,6 @@ public:
GeoFenceManager* geoFenceManager(void) { return _geoFenceManager; }
RallyPointManager* rallyPointManager(void) { return _rallyPointManager; }
bool homePositionAvailable(void);
QGeoCoordinate homePosition(void);
bool armed(void) { return _armed; }
......@@ -672,22 +667,13 @@ public:
/// @true: When flying a mission the vehicle is always facing towards the next waypoint
bool vehicleYawsToNextWaypointInMission(void) const;
public slots:
/// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle
/// and destroyed when the vehicle goes away.
void setLatitude (double latitude);
void setLongitude (double longitude);
void setAltitude (double altitude);
signals:
void allLinksInactive(Vehicle* vehicle);
void coordinateChanged(QGeoCoordinate coordinate);
void coordinateValidChanged(bool coordinateValid);
void joystickModeChanged(int mode);
void joystickEnabledChanged(bool enabled);
void activeChanged(bool active);
void mavlinkMessageReceived(const mavlink_message_t& message);
void homePositionAvailableChanged(bool homePositionAvailable);
void homePositionChanged(const QGeoCoordinate& homePosition);
void armedChanged(bool armed);
void flightModeChanged(const QString& flightMode);
......@@ -859,9 +845,6 @@ private:
UAS* _uas;
QGeoCoordinate _coordinate;
bool _coordinateValid; ///< true: vehicle has 3d lock and therefore valid location
bool _homePositionAvailable;
QGeoCoordinate _homePosition;
UASInterface* _mav;
......@@ -976,8 +959,7 @@ private:
QString _gitHash;
static const int _lowBatteryAnnounceRepeatMSecs; // Amount of time in between each low battery announcement
QElapsedTimer _lowBatteryAnnounceTimer;
int _lastAnnouncedLowBatteryPercent;
SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering
......
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