From d7b2a09919339f97e0dc9cf6b9dbd91be83ac83e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Feb 2014 21:10:30 +1100 Subject: [PATCH] AP_GPS: added fix type to setHIL --- libraries/AP_GPS/AP_GPS_HIL.cpp | 5 +++-- libraries/AP_GPS/AP_GPS_HIL.h | 3 ++- libraries/AP_GPS/GPS.cpp | 3 ++- libraries/AP_GPS/GPS.h | 3 ++- 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 430948e206..c183371294 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -46,7 +46,8 @@ bool AP_GPS_HIL::read(void) return result; } -void AP_GPS_HIL::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude, +void AP_GPS_HIL::setHIL(Fix_Status fix_status, + uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { time_week = _time_epoch_ms / (86400*7*(uint64_t)1000); @@ -58,7 +59,7 @@ void AP_GPS_HIL::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitu ground_course_cd = _ground_course*1.0e2f; speed_3d_cm = _speed_3d*1.0e2f; num_sats = _num_sats; - fix = num_sats>5?FIX_3D:FIX_NONE; + fix = fix_status, hdop = 200; _updated = true; } diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index b09ea054af..08386c00b4 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -45,7 +45,8 @@ public: * @param speed_3d - ground speed in meters/second * @param altitude - altitude in meters */ - virtual void setHIL(uint64_t time_epoch_ms, float latitude, float longitude, float altitude, + virtual void setHIL(Fix_Status fix_status, + uint64_t time_epoch_ms, 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 1d3d99f33c..bd4ecfbde1 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -113,7 +113,8 @@ GPS::update(void) } void -GPS::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude, +GPS::setHIL(Fix_Status fix_status, + uint64_t _time_epoch_ms, 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 c6fe1519fe..c2b801389c 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -105,7 +105,8 @@ public: bool print_errors; ///< deprecated // HIL support - virtual void setHIL(uint64_t time_epoch_ms, float latitude, float longitude, float altitude, + virtual void setHIL(Fix_Status fix_status, + uint64_t time_epoch_ms, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); // components of velocity in 2D, in m/s