diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index edd5456ffb..58327511b0 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -163,9 +163,6 @@ private: void _gps_write(const uint8_t *p, uint16_t size, uint8_t instance); void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size, uint8_t instance); void _update_gps_ubx(const struct gps_data *d, uint8_t instance); - void _update_gps_mtk(const struct gps_data *d, uint8_t instance); - void _update_gps_mtk16(const struct gps_data *d, uint8_t instance); - void _update_gps_mtk19(const struct gps_data *d, uint8_t instance); uint8_t _gps_nmea_checksum(const char *s); void _gps_nmea_printf(uint8_t instance, const char *fmt, ...); void _update_gps_nmea(const struct gps_data *d, uint8_t instance); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 1e13e737cb..a692262d96 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -475,190 +475,6 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance) } } -/* - MTK type simple checksum - */ -static void mtk_checksum(const uint8_t *data, uint8_t n, uint8_t *ck_a, uint8_t *ck_b) -{ - *ck_a = *ck_b = 0; - while (n--) { - *ck_a += *data++; - *ck_b += *ck_a; - } -} - - -/* - send a new GPS MTK packet - */ -void SITL_State::_update_gps_mtk(const struct gps_data *d, uint8_t instance) -{ - struct PACKED mtk_msg { - uint8_t preamble1; - uint8_t preamble2; - uint8_t msg_class; - uint8_t msg_id; - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_time; - uint8_t ck_a; - uint8_t ck_b; - } p; - - p.preamble1 = 0xb5; - p.preamble2 = 0x62; - p.msg_class = 1; - p.msg_id = 5; - p.latitude = htonl(d->latitude * 1.0e6); - p.longitude = htonl(d->longitude * 1.0e6); - p.altitude = htonl(d->altitude * 100); - p.ground_speed = htonl(norm(d->speedN, d->speedE) * 100); - p.ground_course = htonl(ToDeg(atan2f(d->speedE, d->speedN)) * 1000000.0f); - if (p.ground_course < 0.0f) { - p.ground_course += 360.0f * 1000000.0f; - } - p.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; - p.fix_type = d->have_lock?3:1; - - // the spec is not very clear, but the time field seems to be - // milliseconds since the start of the day in UTC time, - // done in powers of 100. - // The date is powers of 100 as well, but in days since 1/1/2000 - struct tm tm; - struct timeval tv; - - simulation_timeval(&tv); - tm = *gmtime(&tv.tv_sec); - uint32_t hsec = (tv.tv_usec / (10000*20)) * 20; // always multiple of 20 - - p.utc_time = htonl(hsec + tm.tm_sec*100 + tm.tm_min*100*100 + tm.tm_hour*100*100*100); - - mtk_checksum(&p.msg_class, sizeof(p)-4, &p.ck_a, &p.ck_b); - - _gps_write((uint8_t*)&p, sizeof(p), instance); -} - -/* - send a new GPS MTK 1.6 packet - */ -void SITL_State::_update_gps_mtk16(const struct gps_data *d, uint8_t instance) -{ - struct PACKED mtk_msg { - uint8_t preamble1; - uint8_t preamble2; - uint8_t size; - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_date; - uint32_t utc_time; - uint16_t hdop; - uint8_t ck_a; - uint8_t ck_b; - } p; - - p.preamble1 = 0xd0; - p.preamble2 = 0xdd; - p.size = sizeof(p) - 5; - p.latitude = d->latitude * 1.0e6; - p.longitude = d->longitude * 1.0e6; - p.altitude = d->altitude * 100; - p.ground_speed = norm(d->speedN, d->speedE) * 100; - p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; - if (p.ground_course < 0.0f) { - p.ground_course += 360.0f * 100.0f; - } - p.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; - p.fix_type = d->have_lock?3:1; - - // the spec is not very clear, but the time field seems to be - // milliseconds since the start of the day in UTC time, - // done in powers of 100. - // The date is powers of 100 as well, but in days since 1/1/2000 - struct tm tm; - struct timeval tv; - - simulation_timeval(&tv); - tm = *gmtime(&tv.tv_sec); - uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200 - - p.utc_date = (tm.tm_year-100) + ((tm.tm_mon+1)*100) + (tm.tm_mday*100*100); - p.utc_time = millisec + tm.tm_sec*1000 + tm.tm_min*1000*100 + tm.tm_hour*1000*100*100; - - p.hdop = 115; - - mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b); - - _gps_write((uint8_t*)&p, sizeof(p), instance); -} - -/* - send a new GPS MTK 1.9 packet - */ -void SITL_State::_update_gps_mtk19(const struct gps_data *d, uint8_t instance) -{ - struct PACKED mtk_msg { - uint8_t preamble1; - uint8_t preamble2; - uint8_t size; - int32_t latitude; - int32_t longitude; - int32_t altitude; - int32_t ground_speed; - int32_t ground_course; - uint8_t satellites; - uint8_t fix_type; - uint32_t utc_date; - uint32_t utc_time; - uint16_t hdop; - uint8_t ck_a; - uint8_t ck_b; - } p; - - p.preamble1 = 0xd1; - p.preamble2 = 0xdd; - p.size = sizeof(p) - 5; - p.latitude = d->latitude * 1.0e7; - p.longitude = d->longitude * 1.0e7; - p.altitude = d->altitude * 100; - p.ground_speed = norm(d->speedN, d->speedE) * 100; - p.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100.0f; - if (p.ground_course < 0.0f) { - p.ground_course += 360.0f * 100.0f; - } - p.satellites = d->have_lock?_sitl->gps_numsats[instance]:3; - p.fix_type = d->have_lock?3:1; - - // the spec is not very clear, but the time field seems to be - // milliseconds since the start of the day in UTC time, - // done in powers of 100. - // The date is powers of 100 as well, but in days since 1/1/2000 - struct tm tm; - struct timeval tv; - - simulation_timeval(&tv); - tm = *gmtime(&tv.tv_sec); - uint32_t millisec = (tv.tv_usec / (1000*200)) * 200; // always multiple of 200 - - p.utc_date = (tm.tm_year-100) + ((tm.tm_mon+1)*100) + (tm.tm_mday*100*100); - p.utc_time = millisec + tm.tm_sec*1000 + tm.tm_min*1000*100 + tm.tm_hour*1000*100*100; - - p.hdop = 115; - - mtk_checksum(&p.size, sizeof(p)-4, &p.ck_a, &p.ck_b); - - _gps_write((uint8_t*)&p, sizeof(p), instance); -} - /* formatted print of NMEA message, with checksum appended */ @@ -1357,18 +1173,6 @@ void SITL_State::_update_gps_instance(SITL::SIM::GPSType gps_type, const struct _update_gps_ubx(data, instance); break; - case SITL::SIM::GPS_TYPE_MTK: - _update_gps_mtk(data, instance); - break; - - case SITL::SIM::GPS_TYPE_MTK16: - _update_gps_mtk16(data, instance); - break; - - case SITL::SIM::GPS_TYPE_MTK19: - _update_gps_mtk19(data, instance); - break; - case SITL::SIM::GPS_TYPE_NMEA: _update_gps_nmea(data, instance); break;