From 9331f8616cf1bd2a051bd51b1c2390cc09d74e40 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Wed, 24 Nov 2010 16:32:30 +0000 Subject: [PATCH] Fixed several HIL class bugs. git-svn-id: https://arducopter.googlecode.com/svn/trunk@914 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS_HIL.cpp | 24 ++++++++++++++---------- libraries/AP_GPS/AP_GPS_HIL.h | 4 ++-- 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index e21ee4ea21..7e153c86e7 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -30,20 +30,24 @@ void AP_GPS_HIL::update(void) int AP_GPS_HIL::status(void) { - return 2; // gps locked - // TODO: show as locked after first packet received + if (valid_read) + { + if (fix) return 2; + else return 1; + } + else return 0; } -void AP_GPS_HIL::setHIL(long _time, long _latitude, long _longitude, long _altitude, - long _ground_speed, long _ground_course, long _speed_3d, uint8_t _num_sats) +void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude, + float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) { time = _time; - latitude = _latitude*1e7; - longitude = _longitude*1e7; - altitude = _altitude*1e2; - ground_speed = _ground_speed*1e2; - ground_course = _ground_course*1e2; - speed_3d = _speed_3d*1e2; + latitude = _latitude*1.0e7; + longitude = _longitude*1.0e7; + altitude = _altitude*1.0e2; + ground_speed = _ground_speed*1.0e2; + ground_course = _ground_course*1.0e2; + speed_3d = _speed_3d*1.0e2; num_sats = _num_sats; new_data = true; fix = true; diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index ad31bb29b4..fd25cd06c2 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -30,8 +30,8 @@ public: * @param speed_3d - ground speed in meters/second * @param altitude - altitude in meters */ - void setHIL(long time, long latitude, long longitude, long altitude, - long ground_speed, long ground_course, long speed_3d, uint8_t num_sats); + void setHIL(long time, float latitude, float longitude, float altitude, + float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); }; #endif // AP_GPS_HIL_H