Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: handle multiple GPS in GPS setHIL()

Browse Source
This commit is contained in:
Andrew Tridgell 2014-04-02 07:49:29 +11:00
parent 79f5618f6f
commit e40a048009
3 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_GPS/AP_GPS.cpp
View File

@ -256,24 +256,28 @@ AP_GPS::update(void)
set HIL (hardware in the loop) status for a GPS instance set HIL (hardware in the loop) status for a GPS instance
*/ */
void void
AP_GPS::setHIL(GPS_Status _status, uint64_t time_epoch_ms, AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
Location &_location, Vector3f &_velocity, uint8_t _num_sats) 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(); uint32_t tnow = hal.scheduler->millis();
GPS_State &istate = state[0]; GPS_State &istate = state[instance];
istate.status = _status; istate.status = _status;
istate.location = _location; istate.location = _location;
istate.location.options = 0; istate.location.options = 0;
istate.velocity = _velocity; istate.velocity = _velocity;
istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y); istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);
istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL; 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.num_sats = _num_sats;
istate.have_vertical_velocity = false; istate.have_vertical_velocity = _have_vertical_velocity;
istate.last_gps_time_ms = tnow; istate.last_gps_time_ms = tnow;
istate.time_week = time_epoch_ms / (86400*7*(uint64_t)1000); 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); istate.time_week_ms = time_epoch_ms - istate.time_week*(86400*7*(uint64_t)1000);
timing[0].last_message_time_ms = tnow; timing[instance].last_message_time_ms = tnow;
timing[0].last_fix_time_ms = tnow; timing[instance].last_fix_time_ms = tnow;
_type[0].set(GPS_TYPE_HIL); _type[instance].set(GPS_TYPE_HIL);
} }

11
libraries/AP_GPS/AP_GPS.h
View File

@ -104,7 +104,7 @@ public:
Location location; ///< last fix location Location location; ///< last fix location
float ground_speed; ///< ground speed in m/sec float ground_speed; ///< ground speed in m/sec
int32_t ground_course_cd; ///< ground course in 100ths of a degree 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 uint8_t num_sats; ///< Number of visible satelites
Vector3f velocity; ///< 3D velocitiy in m/s, in NED format Vector3f velocity; ///< 3D velocitiy in m/s, in NED format
bool have_vertical_velocity:1; ///< does this GPS give vertical velocity? bool have_vertical_velocity:1; ///< does this GPS give vertical velocity?
@ -192,10 +192,10 @@ public:
} }
// horizontal dilution of precision // horizontal dilution of precision
int16_t get_hdop(uint8_t instance) const { uint16_t get_hdop(uint8_t instance) const {
return state[instance].hdop; return state[instance].hdop;
} }
int16_t get_hdop() const { uint16_t get_hdop() const {
return get_hdop(primary_instance()); return get_hdop(primary_instance());
} }
@ -235,8 +235,9 @@ public:
float get_lag() const { return 0.2f; } float get_lag() const { return 0.2f; }
// set position for HIL // set position for HIL
void setHIL(GPS_Status status, uint64_t time_epoch_ms, void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms,
Location &location, Vector3f &velocity, uint8_t num_sats); Location &location, Vector3f &velocity, uint8_t num_sats,
uint16_t hdop, bool _have_vertical_velocity);
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];

2
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -133,7 +133,7 @@ private:
int32_t _new_altitude; ///< altitude parsed from a term int32_t _new_altitude; ///< altitude parsed from a term
int32_t _new_speed; ///< speed parsed from a term int32_t _new_speed; ///< speed parsed from a term
int32_t _new_course; ///< course 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 uint8_t _new_satellite_count; ///< satellite count parsed from a term
/// @name Init strings /// @name Init strings