diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h index fece2683601328edd6d6f86b0efc5ba64ce36dc8..f0f1489059fa1a6e3a0e8d3cc420c2219c7276c4 100644 --- a/include/gnss/sensor/sensor_gnss.h +++ b/include/gnss/sensor/sensor_gnss.h @@ -52,6 +52,7 @@ struct ParamsSensorGnss : public ParamsSensorBase bool yaw_fixed = true; bool translation_fixed = true; Eigen::Vector3d ENU_latlonalt = Eigen::Vector3d::Zero(); + bool latlon_in_degrees = false; ~ParamsSensorGnss() override = default; @@ -71,7 +72,10 @@ struct ParamsSensorGnss : public ParamsSensorBase yaw_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/yaw_fixed"); translation_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/translation_fixed"); if (ENU_mode == "manual") - ENU_latlonalt = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU_latlonalt"); + { + ENU_latlonalt = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU/latlonalt"); + latlon_in_degrees = _server.getParam<bool>(prefix + _unique_name + "/ENU/latlon_in_degrees"); + } } else if (ENU_mode != "ECEF") { diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index b049301a73c3c95551f2604e3a734eb7ebe2dd42..6ae5a98afc6c312e908f59d99e6d47401dbdaa54 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/src/sensor/sensor_gnss.cpp @@ -99,7 +99,9 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3d& _ENU, bool _ECEF_coordinates) if (_ECEF_coordinates) GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF_, t_ENU_ECEF_); else - GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU), R_ENU_ECEF_, t_ENU_ECEF_); + GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU, not params_->latlon_in_degrees), + R_ENU_ECEF_, + t_ENU_ECEF_); ENU_defined_ = true;