diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index f69a3d3aa9..a35f2050e4 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -75,6 +75,8 @@ private: #define MAX_GPS_DELAY 100 static gps_data _gps_data[MAX_GPS_DELAY]; + static bool _gps_has_basestation_position; + static gps_data _gps_basestation_data; static void _gps_write(const uint8_t *p, uint16_t size); static void _gps_send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); static void _update_gps_ubx(const struct gps_data *d); @@ -84,6 +86,8 @@ private: static uint16_t _gps_nmea_checksum(const char *s); static void _gps_nmea_printf(const char *fmt, ...); static void _update_gps_nmea(const struct gps_data *d); + static void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); + static void _update_gps_sbp(const struct gps_data *d, bool sim_rtk); static void _update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, double speedD, bool have_lock); diff --git a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp index c410bdff58..2b03497fc5 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_gps.cpp @@ -33,6 +33,8 @@ extern const AP_HAL::HAL& hal; static uint8_t next_gps_index; static uint8_t gps_delay; SITL_State::gps_data SITL_State::_gps_data[MAX_GPS_DELAY]; +bool SITL_State::_gps_has_basestation_position = false; +SITL_State::gps_data SITL_State::_gps_basestation_data; // state of GPS emulation static struct gps_state { @@ -550,6 +552,159 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d) dstring); } +void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload) +{ + if (len != 0 && payload == 0) { + return; //SBP_NULL_ERROR; + } + + uint8_t preamble = 0x55; + _gps_write(&preamble, 1); + _gps_write((uint8_t*)&msg_type, 2); + _gps_write((uint8_t*)&sender_id, 2); + _gps_write(&len, 1); + if (len > 0) { + _gps_write((uint8_t*)payload, len); + } + + uint16_t crc; + crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0); + crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc); + crc = crc16_ccitt(&(len), 1, crc); + crc = crc16_ccitt(payload, len, crc); + _gps_write((uint8_t*)&crc, 2); +} + + +void SITL_State::_update_gps_sbp(const struct gps_data *d, bool sim_rtk) +{ + struct PACKED sbp_gps_time_t { + uint16_t wn; //< GPS week number + uint32_t tow; //< GPS Time of Week rounded to the nearest ms + int32_t ns; //< Nanosecond remainder of rounded tow + uint8_t flags; //< Status flags (reserved) + } t; + struct PACKED sbp_pos_llh_t { + uint32_t tow; //< GPS Time of Week + double lat; //< Latitude + double lon; //< Longitude + double height; //< Height + uint16_t h_accuracy; //< Horizontal position accuracy estimate + uint16_t v_accuracy; //< Vertical position accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags + } pos; + struct PACKED sbp_vel_ned_t { + uint32_t tow; //< GPS Time of Week + int32_t n; //< Velocity North coordinate + int32_t e; //< Velocity East coordinate + int32_t d; //< Velocity Down coordinate + uint16_t h_accuracy; //< Horizontal velocity accuracy estimate + uint16_t v_accuracy; //< Vertical velocity accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags (reserved) + } velned; + struct PACKED sbp_dops_t { + uint32_t tow; //< GPS Time of Week + uint16_t gdop; //< Geometric Dilution of Precision + uint16_t pdop; //< Position Dilution of Precision + uint16_t tdop; //< Time Dilution of Precision + uint16_t hdop; //< Horizontal Dilution of Precision + uint16_t vdop; //< Vertical Dilution of Precision + } dops; + struct PACKED sbp_baseline_ecef_t { + uint32_t tow; //< GPS Time of Week + int32_t x; //< Baseline ECEF X coordinate + int32_t y; //< Baseline ECEF Y coordinate + int32_t z; //< Baseline ECEF Z coordinate + uint16_t accuracy; //< Position accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags (reserved) + } baseline; + static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100; + static const uint16_t SBP_DOPS_MSGTYPE = 0x0206; + static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201; + static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202; + static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205; + + uint16_t time_week; + uint32_t time_week_ms; + + gps_time(&time_week, &time_week_ms); + + t.wn = time_week; + t.tow = time_week_ms; + t.ns = 0; + t.flags = 0; + _sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); + + if (!d->have_lock) { + return; + } + + pos.tow = time_week_ms; + pos.lon = d->longitude; + pos.lat= d->latitude; + pos.height = d->altitude; + pos.h_accuracy = 5e3; + pos.v_accuracy = 10e3; + pos.n_sats = _sitl->gps_numsats; + pos.flags = 0; + _sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + + velned.tow = time_week_ms; + velned.n = 1e3 * d->speedN; + velned.e = 1e3 * d->speedE; + velned.d = 1e3 * d->speedD; + velned.h_accuracy = 5e3; + velned.v_accuracy = 5e3; + velned.n_sats = _sitl->gps_numsats; + velned.flags = 0; + _sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); + + dops.tow = time_week_ms; + dops.gdop = 1; + dops.pdop = 1; + dops.tdop = 1; + dops.hdop = 100; + dops.vdop = 1; + _sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), (uint8_t*)&dops); + + //Also send baseline messages + if (sim_rtk && _gps_has_basestation_position) { + + Vector3d homeLLH; + Vector3d currentLLH; + Vector3d homeECEF; + Vector3d currentECEF; + Vector3d baselineVector; + + homeLLH[0] = _gps_basestation_data.latitude * DEG_TO_RAD_DOUBLE; + homeLLH[1] = _gps_basestation_data.longitude * DEG_TO_RAD_DOUBLE; + homeLLH[2] = _gps_basestation_data.altitude; + + currentLLH[0] = d->latitude * DEG_TO_RAD_DOUBLE; + currentLLH[1] = d->longitude * DEG_TO_RAD_DOUBLE; + currentLLH[2] = d->altitude; + + wgsllh2ecef(homeLLH, homeECEF); + wgsllh2ecef(currentLLH, currentECEF); + + baselineVector = currentECEF - homeECEF; + + baseline.tow = time_week_ms; + baseline.x = (int32_t) (baselineVector[0]*1e3); //Convert to MM + baseline.y = (int32_t) (baselineVector[1]*1e3); //Convert to MM + baseline.z = (int32_t) (baselineVector[2]*1e3); //Convert to MM + baseline.accuracy = 5e3; + baseline.n_sats = _sitl->gps_numsats; + baseline.flags = 0; + //printf("Sending baseline with length %f\n",baselineVector.length()); + _sbp_send_message(SBP_BASELINE_ECEF_MSGTYPE, 0x2222, sizeof(baseline), (uint8_t*)&baseline); + } + +} + /* possibly send a new GPS packet */ @@ -560,6 +715,20 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, char c; Vector3f glitch_offsets = _sitl->gps_glitch; + //Capture current position as basestation location for + if (!_gps_has_basestation_position) { + if (have_lock) { + _gps_basestation_data.latitude = latitude; + _gps_basestation_data.longitude = longitude; + _gps_basestation_data.altitude = altitude; + _gps_basestation_data.speedN = speedN; + _gps_basestation_data.speedE = speedE; + _gps_basestation_data.speedD = speedD; + _gps_basestation_data.have_lock = have_lock; + _gps_has_basestation_position = true; + } + } + // run at configured GPS rate (default 5Hz) if ((hal.scheduler->millis() - gps_state.last_update) < (uint32_t)(1000/_sitl->gps_hertz)) { return; @@ -628,6 +797,15 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, case SITL::GPS_TYPE_NMEA: _update_gps_nmea(&d); break; + + case SITL::GPS_TYPE_SBP: + _update_gps_sbp(&d, false); + break; + + case SITL::GPS_TYPE_SBP_RTK: + _update_gps_sbp(&d, true); + break; + } }