Merge pull request PX4#17 from wingtra/px4master

BugFix GPS update rate.
msamogh · Mar 17, 2016 · d7f998c · d7f998c
2 parents 6f0db3c + 4ea8f8d
commit d7f998c
Showing 1 changed file with 2 additions and 4 deletions.
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
@@ -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();
 
@@ -221,9 +221,7 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
     lon_rad = lon_zurich;
   }
 
-  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