|
|
|
@ -1143,55 +1143,6 @@ void AP_GPS::handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt)
@@ -1143,55 +1143,6 @@ void AP_GPS::handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt)
|
|
|
|
|
} |
|
|
|
|
#endif // HAL_EXTERNAL_AHRS_ENABLED
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
set HIL (hardware in the loop) status for a GPS instance |
|
|
|
|
*/ |
|
|
|
|
void AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, |
|
|
|
|
const Location &_location, const Vector3f &_velocity, uint8_t _num_sats, |
|
|
|
|
uint16_t hdop) |
|
|
|
|
{ |
|
|
|
|
if (instance >= GPS_MAX_RECEIVERS) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
const uint32_t tnow = AP_HAL::millis(); |
|
|
|
|
GPS_State &istate = state[instance]; |
|
|
|
|
istate.status = _status; |
|
|
|
|
istate.location = _location; |
|
|
|
|
istate.velocity = _velocity; |
|
|
|
|
istate.ground_speed = norm(istate.velocity.x, istate.velocity.y); |
|
|
|
|
istate.ground_course = wrap_360(degrees(atan2f(istate.velocity.y, istate.velocity.x))); |
|
|
|
|
istate.hdop = hdop; |
|
|
|
|
istate.num_sats = _num_sats; |
|
|
|
|
istate.last_gps_time_ms = tnow; |
|
|
|
|
uint64_t gps_time_ms = time_epoch_ms - UNIX_OFFSET_MSEC; |
|
|
|
|
istate.time_week = gps_time_ms / AP_MSEC_PER_WEEK; |
|
|
|
|
istate.time_week_ms = gps_time_ms - istate.time_week * AP_MSEC_PER_WEEK; |
|
|
|
|
timing[instance].last_message_time_ms = tnow; |
|
|
|
|
timing[instance].last_fix_time_ms = tnow; |
|
|
|
|
_type[instance].set(GPS_TYPE_HIL); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// set accuracy for HIL
|
|
|
|
|
void AP_GPS::setHIL_Accuracy(uint8_t instance, float vdop, float hacc, float vacc, float sacc, bool _have_vertical_velocity, uint32_t sample_ms) |
|
|
|
|
{ |
|
|
|
|
if (instance >= GPS_MAX_RECEIVERS) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
GPS_State &istate = state[instance]; |
|
|
|
|
istate.vdop = vdop * 100; |
|
|
|
|
istate.horizontal_accuracy = hacc; |
|
|
|
|
istate.vertical_accuracy = vacc; |
|
|
|
|
istate.speed_accuracy = sacc; |
|
|
|
|
istate.have_horizontal_accuracy = true; |
|
|
|
|
istate.have_vertical_accuracy = true; |
|
|
|
|
istate.have_speed_accuracy = true; |
|
|
|
|
istate.have_vertical_velocity |= _have_vertical_velocity; |
|
|
|
|
if (sample_ms != 0) { |
|
|
|
|
timing[instance].last_message_time_ms = sample_ms; |
|
|
|
|
timing[instance].last_fix_time_ms = sample_ms; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
Lock a GPS port, preventing the GPS driver from using it. This can |
|
|
|
|
be used to allow a user to control a GPS port via the |
|
|
|
|