diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c6e30bd5a4..570cfae930 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -256,24 +256,28 @@ AP_GPS::update(void) set HIL (hardware in the loop) status for a GPS instance */ void -AP_GPS::setHIL(GPS_Status _status, uint64_t time_epoch_ms, - Location &_location, Vector3f &_velocity, uint8_t _num_sats) +AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, + Location &_location, Vector3f &_velocity, uint8_t _num_sats, + uint16_t hdop, bool _have_vertical_velocity) { + if (instance >= GPS_MAX_INSTANCES) { + return; + } uint32_t tnow = hal.scheduler->millis(); - GPS_State &istate = state[0]; + GPS_State &istate = state[instance]; istate.status = _status; istate.location = _location; istate.location.options = 0; istate.velocity = _velocity; istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y); istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL; - istate.hdop = 0; + istate.hdop = hdop; istate.num_sats = _num_sats; - istate.have_vertical_velocity = false; + istate.have_vertical_velocity = _have_vertical_velocity; istate.last_gps_time_ms = tnow; istate.time_week = time_epoch_ms / (86400*7*(uint64_t)1000); istate.time_week_ms = time_epoch_ms - istate.time_week*(86400*7*(uint64_t)1000); - timing[0].last_message_time_ms = tnow; - timing[0].last_fix_time_ms = tnow; - _type[0].set(GPS_TYPE_HIL); + timing[instance].last_message_time_ms = tnow; + timing[instance].last_fix_time_ms = tnow; + _type[instance].set(GPS_TYPE_HIL); } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index beab3cd53e..3c7c1bc936 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -104,7 +104,7 @@ public: Location location; ///< last fix location float ground_speed; ///< ground speed in m/sec int32_t ground_course_cd; ///< ground course in 100ths of a degree - int16_t hdop; ///< horizontal dilution of precision in cm + uint16_t hdop; ///< horizontal dilution of precision in cm uint8_t num_sats; ///< Number of visible satelites Vector3f velocity; ///< 3D velocitiy in m/s, in NED format bool have_vertical_velocity:1; ///< does this GPS give vertical velocity? @@ -192,10 +192,10 @@ public: } // horizontal dilution of precision - int16_t get_hdop(uint8_t instance) const { + uint16_t get_hdop(uint8_t instance) const { return state[instance].hdop; } - int16_t get_hdop() const { + uint16_t get_hdop() const { return get_hdop(primary_instance()); } @@ -235,8 +235,9 @@ public: float get_lag() const { return 0.2f; } // set position for HIL - void setHIL(GPS_Status status, uint64_t time_epoch_ms, - Location &location, Vector3f &velocity, uint8_t num_sats); + void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms, + Location &location, Vector3f &velocity, uint8_t num_sats, + uint16_t hdop, bool _have_vertical_velocity); static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index f429bde6ae..f85c86b318 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -133,7 +133,7 @@ private: int32_t _new_altitude; ///< altitude parsed from a term int32_t _new_speed; ///< speed parsed from a term int32_t _new_course; ///< course parsed from a term - int16_t _new_hdop; ///< HDOP parsed from a term + uint16_t _new_hdop; ///< HDOP parsed from a term uint8_t _new_satellite_count; ///< satellite count parsed from a term /// @name Init strings