diff --git a/src/HomePositionManager.cc b/src/HomePositionManager.cc index 14b06ba1b72d5006dc64174f37657c3a1d8cf079..01f39b4ddcb20ce7d5289d32de9b9f6634728d56 100644 --- a/src/HomePositionManager.cc +++ b/src/HomePositionManager.cc @@ -117,62 +117,7 @@ void HomePositionManager::_loadSettings(void) if (_homePositions.count() == 0) { _homePositions.append(new HomePosition("ETH Campus", QGeoCoordinate(47.3769, 8.549444, 470.0), this)); - } - - // Deprecated settings for old editor - - settings.beginGroup("QGC_UASMANAGER"); - bool changed = setHomePosition(settings.value("HOMELAT", homeLat).toDouble(), - settings.value("HOMELON", homeLon).toDouble(), - settings.value("HOMEALT", homeAlt).toDouble()); - - // Make sure to fire the change - this will - // make sure widgets get the signal once - if (!changed) - { - emit homePositionChanged(homeLat, homeLon, homeAlt); - } - - settings.endGroup(); -} - -bool HomePositionManager::setHomePosition(double lat, double lon, double alt) -{ - // Checking for NaN and infitiny - // and checking for borders - bool changed = false; - if (!isnan(lat) && !isnan(lon) && !isnan(alt) - && !isinf(lat) && !isinf(lon) && !isinf(alt) - && lat <= 90.0 && lat >= -90.0 && lon <= 180.0 && lon >= -180.0) - { - - if (fabs(homeLat - lat) > 1e-7) changed = true; - if (fabs(homeLon - lon) > 1e-7) changed = true; - if (fabs(homeAlt - alt) > 0.5f) changed = true; - - if (changed) - { - homeLat = lat; - homeLon = lon; - homeAlt = alt; - - emit homePositionChanged(homeLat, homeLon, homeAlt); - } - } - return changed; -} - -bool HomePositionManager::setHomePositionAndNotify(double lat, double lon, double alt) -{ - // Checking for NaN and infitiny - // and checking for borders - bool changed = setHomePosition(lat, lon, alt); - - if (changed) { - qgcApp()->toolbox()->multiVehicleManager()->setHomePositionForAllVehicles(homeLat, homeLon, homeAlt); - } - - return changed; + } } void HomePositionManager::updateHomePosition(const QString& name, const QGeoCoordinate& coordinate) diff --git a/src/HomePositionManager.h b/src/HomePositionManager.h index 0b3d2d5e42b1bf04aabafe4fe51b18ed5591d548..e9158178a99589393d37788a6c0fd33545182ffb 100644 --- a/src/HomePositionManager.h +++ b/src/HomePositionManager.h @@ -112,21 +112,6 @@ public: return homeAlt; } -public slots: - - // Deprecated methods - - /** @brief Set the current home position, but do not change it on the UAVs */ - bool setHomePosition(double lat, double lon, double alt); - - /** @brief Set the current home position on all UAVs*/ - bool setHomePositionAndNotify(double lat, double lon, double alt); - - -signals: - /** @brief Current home position changed */ - void homePositionChanged(double lat, double lon, double alt); - protected: double homeLat; double homeLon; diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 026edc8247526c41c2eb97332697cbc595d0afb0..bdfb8ca3a3fbf42841b9c3d0808c3f6ea447c3a6 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -212,13 +212,6 @@ void MultiVehicleManager::_autopilotParametersReadyChanged(bool parametersReady) } } -void MultiVehicleManager::setHomePositionForAllVehicles(double lat, double lon, double alt) -{ - for (int i=0; i< _vehicles.count(); i++) { - qobject_cast(_vehicles[i])->uas()->setHomePosition(lat, lon, alt); - } -} - void MultiVehicleManager::saveSetting(const QString &name, const QString& value) { QSettings settings; diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index badc782ca9171e6c08c9a1872da06168e361cec9..b24cd0c28d21e1495cc8e79cfb39f902e85c8f9c 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -65,8 +65,6 @@ public: Q_INVOKABLE Vehicle* getVehicleById(int vehicleId); - void setHomePositionForAllVehicles(double lat, double lon, double alt); - UAS* activeUas(void) { return _activeVehicle ? _activeVehicle->uas() : NULL; } QList vehicles(void); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 90d486ea7859054783c5838e04adc532cc383031..8651db6837ae18252b01c305af5c862d38e3d2ef 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1010,42 +1010,6 @@ void UAS::receiveMessage(mavlink_message_t message) } } -/** -* Set the home position of the UAS. -* @param lat The latitude fo the home position -* @param lon The longitude of the home position -* @param alt The altitude of the home position -*/ -void UAS::setHomePosition(double lat, double lon, double alt) -{ - if (!_vehicle || blockHomePositionChanges) - return; - - QMessageBox::StandardButton button = QGCMessageBox::question(tr("Set a new home position for vehicle %1").arg(getUASID()), - tr("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"), - QMessageBox::Yes | QMessageBox::Cancel, - QMessageBox::Cancel); - if (button == QMessageBox::Yes) - { - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_DO_SET_HOME, 1, 0, 0, 0, 0, lat, lon, alt); - // Send message twice to increase chance that it reaches its goal - _vehicle->sendMessage(msg); - - // Send new home position to UAS - mavlink_set_gps_global_origin_t home; - home.target_system = uasId; - home.latitude = lat*1E7; - home.longitude = lon*1E7; - home.altitude = alt*1000; - qDebug() << "lat:" << home.latitude << " lon:" << home.longitude; - mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home); - _vehicle->sendMessage(msg); - } else { - blockHomePositionChanges = true; - } -} - void UAS::startCalibration(UASInterface::StartCalibrationType calType) { if (!_vehicle) { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index f8aedc93454bfefc9aa6a470684cf7166f412ae7..45ab119d7f9e5597fd97819156bf42e3b2470ed4 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -570,9 +570,6 @@ public slots: /** @brief Update the system state */ void updateState(); - /** @brief Set world frame origin / home position at this GPS position */ - void setHomePosition(double lat, double lon, double alt); - void startCalibration(StartCalibrationType calType); void stopCalibration(void); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index fec8ca1a8e7a87d3c6e2e64fde84993e6a242200..24f5b8148c792b56e540e20626539cb828ec1da2 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -119,9 +119,6 @@ public: return color; } - static const unsigned int WAYPOINT_RADIUS_DEFAULT_FIXED_WING = 25; - static const unsigned int WAYPOINT_RADIUS_DEFAULT_ROTARY_WING = 5; - enum StartCalibrationType { StartCalibrationRadio, StartCalibrationGyro, @@ -158,8 +155,6 @@ public slots: /** @brief Order the robot to pair its receiver **/ virtual void pairRX(int rxType, int rxSubType) = 0; - - virtual void setHomePosition(double lat, double lon, double alt) = 0; /** @brief Send the full HIL state to the MAV */ #ifndef __mobile__