diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d119600849..7d5d74204f 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1877,7 +1877,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_gps_raw_decode(msg, &packet); // set gps hil sensor - g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + g_gps->setHIL(packet.usec/1000,packet.lat,packet.lon,packet.alt, packet.v,packet.hdg,0,0); if (gps_base_alt == 0) { gps_base_alt = packet.alt*100; @@ -1944,7 +1944,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100); // set gps hil sensor - g_gps->setHIL(packet.time_usec/1000.0, + g_gps->setHIL(packet.time_usec/1000, packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, vel*1.0e-2, cog*1.0e-2, 0, 10); @@ -2006,7 +2006,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_gps_raw_decode(msg, &packet); // set gps hil sensor - g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + g_gps->setHIL(packet.usec/1000,packet.lat,packet.lon,packet.alt, packet.v,packet.hdg,0,0); break; } diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 66dc206332..1ffa45c992 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -38,7 +38,7 @@ bool AP_GPS_HIL::read(void) return result; } -void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude, +void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { time = _time; diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index 939815d0d0..1b802c6ea4 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -30,7 +30,7 @@ public: * @param speed_3d - ground speed in meters/second * @param altitude - altitude in meters */ - virtual void setHIL(long time, float latitude, float longitude, float altitude, + virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); private: diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 7df3b0d673..ea7e336153 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -70,7 +70,7 @@ GPS::update(void) } void -GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude, +GPS::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index bd6410ce97..7cadcb8400 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -112,7 +112,7 @@ public: bool print_errors; ///< deprecated // HIL support - virtual void setHIL(long time, float latitude, float longitude, float altitude, + virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); /// Time in milliseconds after which we will assume the GPS is no longer