void
toWGS84(const IMC::EstimatedState& estate, double& lat, double& lon)
{
// Use reference height.
float hae = estate.height;
toWGS84(estate, lat, lon, hae);
}
GlobalReference& GlobalReference::setCurrentPosition(const State& state, double new_latitude, double new_longitude) {
State::ConstPositionType position = state.getPosition();
// set reference to new latitude/longitude first (intermediate reference)
setPosition(new_latitude, new_longitude, true);
// convert current position back to WGS84 using the new reference position
// and reset the reference position so that current position in x/y coordinates remains the same
// This will work unless the radii at the origin and the x/y position of the robot differ too much
toWGS84(-position.x(), -position.y(), new_latitude, new_longitude);
setPosition(new_latitude, new_longitude);
return *this;
}
GlobalReference& GlobalReference::setCurrentHeading(const State& state, double new_heading) {
// get current yaw angle
double current_yaw = state.getYaw();
State::ConstPositionType position = state.getPosition();
// get current position in WGS84
double current_latitude, current_longitude;
if (hasPosition()) {
toWGS84(position.x(), position.y(), current_latitude, current_longitude);
}
// set the new reference heading
setHeading(new_heading - (-current_yaw));
// set the new reference position so that current position in WGS84 coordinates remains the same as before
if (hasPosition()) {
setCurrentPosition(state, current_latitude, current_longitude);
}
return *this;
}
