Commit 85f0b13c authored by Stephen Rees's avatar Stephen Rees
Browse files

Add Vandy location and patch to use gazebo world coordinates

parent b28258f1
......@@ -106,7 +106,7 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
_rotor_count = 5;
last_time_ = world_->GetSimTime();
last_gps_time_ = world_->GetSimTime();
double gps_update_interval_ = 200 * 1000000; // nanoseconds for 5Hz
gps_update_interval_ = 0.2; // in seconds for 5Hz
gravity_W_ = world_->GetPhysicsEngine()->GetGravity();
......@@ -189,34 +189,48 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
math::Vector3 velocity_current_W = model_->GetWorldLinearVel(); // Use the models' world position for GPS velocity.
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
// Zurich Irchel Park
const double lat_zurich = 47.397742 * M_PI / 180 ; // rad
const double lon_zurich = 8.545594 * M_PI / 180; // rad
// Seattle downtown (15 deg declination): 47.592182, -122.316031
// const double lat_zurich = 47.592182 * M_PI / 180 ; // rad
// const double lon_zurich = -122.316031 * M_PI / 180; // rad
const float earth_radius = 6353000; // m
velocity_current_W_xy.z = 0;
// Set default global reference point
// Zurich Irchel Park: 47.397742, 8.545594, 488m
const double lat_default = 47.397742 * M_PI / 180; // rad
const double lon_default = 8.545594 * M_PI / 180; // rad
const double alt_default = 488.0; // meters
const float radius_default = 6353000; // m
double lat_world = lat_default;
double lon_world = lon_default;
double alt_world = alt_default;
double radius_world = radius_default;
common::SphericalCoordinatesPtr spherical_coordinates = world_->GetSphericalCoordinates();
if (spherical_coordinates) {
lat_world = spherical_coordinates->LatitudeReference().Radian(); // rad
lon_world = spherical_coordinates->LongitudeReference().Radian(); // rad
alt_world = spherical_coordinates->GetElevationReference(); // m
switch (spherical_coordinates->GetSurfaceType()) {
case (common::SphericalCoordinates::EARTH_WGS84): {
radius_world = 6353000; // m
break;
}
}
}
// reproject local position to gps coordinates
double x_rad = pos_W_I.x / earth_radius;
double y_rad = -pos_W_I.y / earth_radius;
double x_rad = pos_W_I.x / radius_world;
double y_rad = -pos_W_I.y / radius_world;
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
if (c != 0.0) {
lat_rad = asin(cos_c * sin(lat_zurich) + (x_rad * sin_c * cos(lat_zurich)) / c);
lon_rad = (lon_zurich + atan2(y_rad * sin_c, c * cos(lat_zurich) * cos_c - x_rad * sin(lat_zurich) * sin_c));
lat_rad = asin(cos_c * sin(lat_world) + (x_rad * sin_c * cos(lat_world)) / c);
lon_rad = (lon_world + atan2(y_rad * sin_c, c * cos(lat_world) * cos_c - x_rad * sin(lat_world) * sin_c));
} else {
lat_rad = lat_zurich;
lon_rad = lon_zurich;
lat_rad = lat_world;
lon_rad = lon_world;
}
common::Time gps_update(gps_update_interval_);
if(current_time - last_gps_time_ > gps_update){ // 5Hz
if(current_time.Double() - last_gps_time_.Double() > gps_update_interval_){ // 5Hz
if(use_mavlink_udp){
// Raw UDP mavlink
......@@ -225,7 +239,7 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
hil_gps_msg.fix_type = 3;
hil_gps_msg.lat = lat_rad * 180 / M_PI * 1e7;
hil_gps_msg.lon = lon_rad * 180 / M_PI * 1e7;
hil_gps_msg.alt = pos_W_I.z * 1000;
hil_gps_msg.alt = (pos_W_I.z + alt_world) * 1000;
hil_gps_msg.eph = 100;
hil_gps_msg.epv = 100;
hil_gps_msg.vel = velocity_current_W_xy.GetLength() * 100;
......@@ -242,7 +256,7 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
hil_gps_msg_.set_fix_type(3);
hil_gps_msg_.set_lat(lat_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_lon(lon_rad * 180 / M_PI * 1e7);
hil_gps_msg_.set_alt(pos_W_I.z * 1000);
hil_gps_msg_.set_alt((pos_W_I.z + alt_world) * 1000);
hil_gps_msg_.set_eph(100);
hil_gps_msg_.set_epv(100);
hil_gps_msg_.set_vel(velocity_current_W_xy.GetLength() * 100);
......
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>36.143494</latitude_deg>
<longitude_deg>-86.798458</longitude_deg>
<elevation>182.0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<!-- A global light source -->
<include>
<uri>model://sun</uri>
......
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