diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 61eeaaecc7..9ec9484a44 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -12,13 +12,19 @@ #include -#include #include #include #include #include -#include -#include + +#include "SIM_GPS_FILE.h" +#include "SIM_GPS_GSOF.h" +#include "SIM_GPS_MSP.h" +#include "SIM_GPS_NMEA.h" +#include "SIM_GPS_NOVA.h" +#include "SIM_GPS_SBP2.h" +#include "SIM_GPS_SBP.h" +#include "SIM_GPS_UBLOX.h" // the number of GPS leap seconds - copied from AP_GPS.h #define GPS_LEAPSECONDS_MILLIS 18000ULL @@ -27,13 +33,6 @@ extern const AP_HAL::HAL& hal; using namespace SITL; -struct GPS_TOW { - // Number of weeks since midnight 5-6 January 1980 - uint16_t week; - // Time since start of the GPS week [mS] - uint32_t ms; -}; - // ensure the backend we have allocated matches the one that's configured: GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance) : front{_front}, @@ -114,7 +113,7 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const /* get timeval using simulation time */ -static void simulation_timeval(struct timeval *tv) +void GPS_Backend::simulation_timeval(struct timeval *tv) { uint64_t now = AP_HAL::micros64(); static uint64_t first_usec; @@ -130,37 +129,10 @@ static void simulation_timeval(struct timeval *tv) tv->tv_usec = new_usec % 1000000ULL; } -/* - send a UBLOX GPS message - */ -void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) -{ - const uint8_t PREAMBLE1 = 0xb5; - const uint8_t PREAMBLE2 = 0x62; - const uint8_t CLASS_NAV = 0x1; - uint8_t hdr[6], chk[2]; - hdr[0] = PREAMBLE1; - hdr[1] = PREAMBLE2; - hdr[2] = CLASS_NAV; - hdr[3] = msgid; - hdr[4] = size & 0xFF; - hdr[5] = size >> 8; - chk[0] = chk[1] = hdr[2]; - chk[1] += (chk[0] += hdr[3]); - chk[1] += (chk[0] += hdr[4]); - chk[1] += (chk[0] += hdr[5]); - for (uint16_t i=0; ilongitude * 1.0e7; - pos.latitude = d->latitude * 1.0e7; - pos.altitude_ellipsoid = d->altitude * 1000.0f; - pos.altitude_msl = d->altitude * 1000.0f; - pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000; - - status.time = gps_tow.ms; - status.fix_type = d->have_lock?3:0; - status.fix_status = d->have_lock?1:0; - status.differential_status = 0; - status.res = 0; - status.time_to_first_fix = 0; - status.uptime = AP_HAL::millis(); - - velned.time = gps_tow.ms; - velned.ned_north = 100.0f * d->speedN; - velned.ned_east = 100.0f * d->speedE; - velned.ned_down = 100.0f * d->speedD; - velned.speed_2d = norm(d->speedN, d->speedE) * 100; - velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100; - velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; - if (velned.heading_2d < 0.0f) { - velned.heading_2d += 360.0f * 100000.0f; - } - velned.speed_accuracy = 40; - velned.heading_accuracy = 4; - - memset(&sol, 0, sizeof(sol)); - sol.fix_type = d->have_lock?3:0; - sol.fix_status = 221; - sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3; - sol.time = gps_tow.ms; - sol.week = gps_tow.week; - - dop.time = gps_tow.ms; - dop.gDOP = 65535; - dop.pDOP = 65535; - dop.tDOP = 65535; - dop.vDOP = 200; - dop.hDOP = 121; - dop.nDOP = 65535; - dop.eDOP = 65535; - - pvt.itow = gps_tow.ms; - pvt.year = 0; - pvt.month = 0; - pvt.day = 0; - pvt.hour = 0; - pvt.min = 0; - pvt.sec = 0; - pvt.valid = 0; // invalid utc date - pvt.t_acc = 0; - pvt.nano = 0; - pvt.fix_type = d->have_lock? 0x3 : 0; - pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok - pvt.flags2 =0; - pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3; - pvt.lon = d->longitude * 1.0e7; - pvt.lat = d->latitude * 1.0e7; - pvt.height = d->altitude * 1000.0f; - pvt.h_msl = d->altitude * 1000.0f; - pvt.h_acc = _sitl->gps_accuracy[instance] * 1000; - pvt.v_acc = _sitl->gps_accuracy[instance] * 1000; - pvt.velN = 1000.0f * d->speedN; - pvt.velE = 1000.0f * d->speedE; - pvt.velD = 1000.0f * d->speedD; - pvt.gspeed = norm(d->speedN, d->speedE) * 1000; - pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5; - pvt.s_acc = 40; - pvt.head_acc = 38 * 1.0e5; - pvt.p_dop = 65535; - memset(pvt.reserved1, '\0', ARRAY_SIZE(pvt.reserved1)); - pvt.headVeh = 0; - memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2)); - - if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { - const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get(); - const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get(); - Vector3f rel_antenna_pos = ant2_pos - ant1_pos; - Matrix3f rot; - // project attitude back using gyros to get antenna orientation at time of GPS sample - Vector3f gyro(radians(_sitl->state.rollRate), - radians(_sitl->state.pitchRate), - radians(_sitl->state.yawRate)); - rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg)); - const float lag = _sitl->gps_delay_ms[instance] * 0.001; - rot.rotate(gyro * (-lag)); - rel_antenna_pos = rot * rel_antenna_pos; - relposned.version = 1; - relposned.iTOW = gps_tow.ms; - relposned.relPosN = rel_antenna_pos.x * 100; - relposned.relPosE = rel_antenna_pos.y * 100; - relposned.relPosD = rel_antenna_pos.z * 100; - relposned.relPosLength = rel_antenna_pos.length() * 100; - relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5; - relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid; - } - - send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); - send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); - send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); - send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); - send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); - send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); - if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { - send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned)); - } - - if (gps_tow.ms > _next_nav_sv_info_time) { - svinfo.itow = gps_tow.ms; - svinfo.numCh = 32; - svinfo.globalFlags = 4; // u-blox 8/M8 - // fill in the SV's with some data even though firmware does not currently use it - // note that this is not using num_sats as we aren't dynamically creating this to match - for (uint8_t i = 0; i < SV_COUNT; i++) { - svinfo.sv[i].chn = i; - svinfo.sv[i].svid = i; - svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information - svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized - svinfo.sv[i].cno = MAX(20, 30 - i); - svinfo.sv[i].elev = MAX(30, 90 - i); - svinfo.sv[i].azim = i; - // not bothering to fill in prRes - } - send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo)); - _next_nav_sv_info_time = gps_tow.ms + 10000; // 10 second delay - } -} - -/* - formatted print of NMEA message, with checksum appended - */ -void GPS_NMEA::nmea_printf(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - char *s = nmea_vaprintf(fmt, ap); - va_end(ap); - if (s != nullptr) { - write_to_autopilot((const char*)s, strlen(s)); - free(s); - } -} - - -/* - send a new GPS NMEA packet - */ -void GPS_NMEA::publish(const GPS_Data *d) -{ - struct timeval tv; - struct tm *tm; - char tstring[20]; - char dstring[20]; - char lat_string[20]; - char lng_string[20]; - - simulation_timeval(&tv); - - tm = gmtime(&tv.tv_sec); - - // format time string - snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + tv.tv_usec*1.0e-6); - - // format date string - snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); - - // format latitude - double deg = fabs(d->latitude); - snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", - (unsigned)deg, - (deg - int(deg))*60, - d->latitude<0?'S':'N'); - - // format longitude - deg = fabs(d->longitude); - snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", - (unsigned)deg, - (deg - int(deg))*60, - d->longitude<0?'W':'E'); - - nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", - tstring, - lat_string, - lng_string, - d->have_lock?1:0, - d->have_lock?_sitl->gps_numsats[instance]:3, - 1.2, - d->altitude); - - const float speed_mps = d->speed_2d(); - const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS; - const auto heading_rad = d->heading(); - - //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24 - nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A", - tstring, - heading_rad, - heading_rad, - speed_knots, - speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6); - - nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", - tstring, - d->have_lock?'A':'V', - lat_string, - lng_string, - speed_knots, - heading_rad, - dstring); - - if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) { - nmea_printf("$GPHDT,%.2f,T", d->yaw_deg); - } - else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) { - nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V'); - } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) { - // Unicore support - // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D - nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,", - tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4), - d->longitude, d->latitude, - d->altitude, - wrap_360(d->yaw_deg), - d->pitch_deg, - heading_rad, - speed_mps, - d->roll_deg, - d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed, - 3, // fixed rtk yaw solution, - d->have_lock?_sitl->gps_numsats[instance]:3, - d->have_lock?_sitl->gps_numsats[instance]:3, - d->speedE * 3.6, - d->speedN * 3.6, - -d->speedD * 3.6); - } -} - -void GPS_SBP_Common::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; - write_to_autopilot((char*)&preamble, 1); - write_to_autopilot((char*)&msg_type, 2); - write_to_autopilot((char*)&sender_id, 2); - write_to_autopilot((char*)&len, 1); - if (len > 0) { - write_to_autopilot((char*)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); - write_to_autopilot((char*)&crc, 2); -} - -void GPS_SBP::publish(const GPS_Data *d) -{ - struct sbp_heartbeat_t { - bool sys_error : 1; - bool io_error : 1; - bool nap_error : 1; - uint8_t res : 5; - uint8_t protocol_minor : 8; - uint8_t protocol_major : 8; - uint8_t res2 : 7; - bool ext_antenna : 1; - } hb; // 4 bytes - - 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 - uint8_t flags; //< Status flags (reserved) - } dops; - - static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; - 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_VEL_NED_MSGTYPE = 0x0205; - - const auto gps_tow = gps_time(); - - t.wn = gps_tow.week; - t.tow = gps_tow.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 = gps_tow.ms; - pos.lon = d->longitude; - pos.lat= d->latitude; - pos.height = d->altitude; - pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; - - // Send single point position solution - pos.flags = 0; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - // Send "pseudo-absolute" RTK position solution - pos.flags = 1; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - - velned.tow = gps_tow.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 = d->have_lock ? _sitl->gps_numsats[instance] : 3; - velned.flags = 0; - sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); - - static uint32_t do_every_count = 0; - if (do_every_count % 5 == 0) { - - dops.tow = gps_tow.ms; - dops.gdop = 1; - dops.pdop = 1; - dops.tdop = 1; - dops.hdop = 100; - dops.vdop = 1; - dops.flags = 1; - sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), - (uint8_t*)&dops); - - hb.protocol_major = 0; //Sends protocol version 0 - sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), - (uint8_t*)&hb); - - } - do_every_count++; -} - - -void GPS_SBP2::publish(const GPS_Data *d) -{ - struct sbp_heartbeat_t { - bool sys_error : 1; - bool io_error : 1; - bool nap_error : 1; - uint8_t res : 5; - uint8_t protocol_minor : 8; - uint8_t protocol_major : 8; - uint8_t res2 : 7; - bool ext_antenna : 1; - } hb; // 4 bytes - - 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 - uint8_t flags; //< Status flags (reserved) - } dops; - - static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; - static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102; - static const uint16_t SBP_DOPS_MSGTYPE = 0x0208; - static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A; - static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E; - - const auto gps_tow = gps_time(); - - t.wn = gps_tow.week; - t.tow = gps_tow.ms; - t.ns = 0; - t.flags = 1; - sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); - - if (!d->have_lock) { - return; - } - - pos.tow = gps_tow.ms; - pos.lon = d->longitude; - pos.lat= d->latitude; - pos.height = d->altitude; - pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; - - // Send single point position solution - pos.flags = 1; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - // Send "pseudo-absolute" RTK position solution - pos.flags = 4; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - - velned.tow = gps_tow.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 = d->have_lock ? _sitl->gps_numsats[instance] : 3; - velned.flags = 1; - sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); - - static uint32_t do_every_count = 0; - if (do_every_count % 5 == 0) { - - dops.tow = gps_tow.ms; - dops.gdop = 1; - dops.pdop = 1; - dops.tdop = 1; - dops.hdop = 100; - dops.vdop = 1; - dops.flags = 1; - sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), - (uint8_t*)&dops); - - hb.protocol_major = 2; //Sends protocol version 2.0 - sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), - (uint8_t*)&hb); - } - do_every_count++; -} - -void GPS_NOVA::publish(const GPS_Data *d) -{ - static struct PACKED nova_header - { - // 0 - uint8_t preamble[3]; - // 3 - uint8_t headerlength; - // 4 - uint16_t messageid; - // 6 - uint8_t messagetype; - //7 - uint8_t portaddr; - //8 - uint16_t messagelength; - //10 - uint16_t sequence; - //12 - uint8_t idletime; - //13 - uint8_t timestatus; - //14 - uint16_t week; - //16 - uint32_t tow; - //20 - uint32_t recvstatus; - // 24 - uint16_t resv; - //26 - uint16_t recvswver; - } header; - - struct PACKED psrdop - { - float gdop; - float pdop; - float hdop; - float htdop; - float tdop; - float cutoff; - uint32_t svcount; - // extra data for individual prns - } psrdop {}; - - struct PACKED bestpos - { - uint32_t solstat; - uint32_t postype; - double lat; - double lng; - double hgt; - float undulation; - uint32_t datumid; - float latsdev; - float lngsdev; - float hgtsdev; - // 4 bytes - uint8_t stnid[4]; - float diffage; - float sol_age; - uint8_t svstracked; - uint8_t svsused; - uint8_t svsl1; - uint8_t svsmultfreq; - uint8_t resv; - uint8_t extsolstat; - uint8_t galbeisigmask; - uint8_t gpsglosigmask; - } bestpos {}; - - struct PACKED bestvel - { - uint32_t solstat; - uint32_t veltype; - float latency; - float age; - double horspd; - double trkgnd; - // + up - double vertspd; - float resv; - } bestvel {}; - - const auto gps_tow = gps_time(); - - header.preamble[0] = 0xaa; - header.preamble[1] = 0x44; - header.preamble[2] = 0x12; - header.headerlength = sizeof(header); - header.week = gps_tow.week; - header.tow = gps_tow.ms; - - header.messageid = 174; - header.messagelength = sizeof(psrdop); - header.sequence += 1; - - psrdop.hdop = 1.20; - psrdop.htdop = 1.20; - nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop)); - - - header.messageid = 99; - header.messagelength = sizeof(bestvel); - header.sequence += 1; - - bestvel.horspd = norm(d->speedN, d->speedE); - bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN)); - bestvel.vertspd = -d->speedD; - - nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel)); - - - header.messageid = 42; - header.messagelength = sizeof(bestpos); - header.sequence += 1; - - bestpos.lat = d->latitude; - bestpos.lng = d->longitude; - bestpos.hgt = d->altitude; - bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3; - bestpos.latsdev=0.2; - bestpos.lngsdev=0.2; - bestpos.hgtsdev=0.2; - bestpos.solstat=0; - bestpos.postype=32; - - nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos)); -} - -void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen) -{ - write_to_autopilot((char*)header, headerlength); -write_to_autopilot((char*)payload, payloadlen); - - uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0); - crc = CalculateBlockCRC32(payloadlen, payload, crc); - - write_to_autopilot((char*)&crc, 4); -} - -#define CRC32_POLYNOMIAL 0xEDB88320L -uint32_t GPS_NOVA::CRC32Value(uint32_t icrc) -{ - int i; - uint32_t crc = icrc; - for ( i = 8 ; i > 0; i-- ) - { - if ( crc & 1 ) - crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; - else - crc >>= 1; - } - return crc; -} - -uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) -{ - while ( length-- != 0 ) - { - crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); - } - return( crc ); -} - -void GPS_GSOF::publish(const GPS_Data *d) -{ - // This logic is to populate output buffer only with enabled channels. - // It also only sends each channel at the configured rate. - const uint64_t now = AP_HAL::millis(); - uint8_t buf[MAX_PAYLOAD_SIZE] = {}; - uint8_t payload_sz = 0; - uint8_t offset = 0; - if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){ - const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; - const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; - - if (now - last_time > RateToPeriodMs(desired_rate)) { - - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 - constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; - // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE - const uint8_t bootcount = 17; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 - enum class POS_FLAGS_1 : uint8_t { - NEW_POSITION = 1U << 0, - CLOCK_FIX_CALULATED = 1U << 1, - HORIZ_FROM_THIS_POS = 1U << 2, - HEIGHT_FROM_THIS_POS = 1U << 3, - RESERVED_4 = 1U << 4, - LEAST_SQ_POSITION = 1U << 5, - RESERVED_6 = 1U << 6, - POSITION_L1_PSEUDORANGES = 1U << 7 - }; - const uint8_t pos_flags_1 { - uint8_t(POS_FLAGS_1::NEW_POSITION) | - uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | - uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | - uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | - uint8_t(POS_FLAGS_1::RESERVED_4) | - uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | - uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) - }; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 - enum class POS_FLAGS_2 : uint8_t { - DIFFERENTIAL_POS = 1U << 0, - DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, - POSITION_METHOD_FIXED_PHASE = 1U << 2, - OMNISTAR_ACTIVE = 1U << 3, - DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, - NETWORK_RTK = 1U << 5, - DITHERED_RTK = 1U << 6, - BEACON_DGNSS = 1U << 7, - }; - - // Simulate a GPS without RTK in SIM since there is no RTK SIM params. - // This means these flags are unset: - // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS - uint8_t pos_flags_2 {0}; - if(d->have_lock) { - pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); - pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); - pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); - pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); - pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); - } - - const auto gps_tow = gps_time(); - const struct PACKED gsof_pos_time { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - uint32_t time_week_ms; - uint16_t time_week; - uint8_t num_sats; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 - uint8_t pos_flags_1; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 - uint8_t pos_flags_2; - uint8_t initialized_num; - } pos_time { - uint8_t(Gsof_Msg_Record_Type::POSITION_TIME), - GSOF_POS_TIME_LEN, - htobe32(gps_tow.ms), - htobe16(gps_tow.week), - d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), - pos_flags_1, - pos_flags_2, - bootcount - }; - static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); - - payload_sz += sizeof(pos_time); - memcpy(&buf[offset], &pos_time, sizeof(pos_time)); - offset += sizeof(pos_time); - } - } - - if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){ - const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)]; - const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)]; - - if (now - last_time > RateToPeriodMs(desired_rate)) { - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20 - constexpr uint8_t GSOF_POS_LEN = 0x18; - - const struct PACKED gsof_pos { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - uint64_t lat; - uint64_t lng; - uint64_t alt; - } pos { - uint8_t(Gsof_Msg_Record_Type::LLH), - GSOF_POS_LEN, - gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), - gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), - gsof_pack_double(static_cast(d->altitude)) - }; - static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); - - payload_sz += sizeof(pos); - memcpy(&buf[offset], &pos, sizeof(pos)); - offset += sizeof(pos); - } - } - - if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){ - const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; - const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; - - if (now - last_time > RateToPeriodMs(desired_rate)) { - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html - // use the smaller packet by ignoring local coordinate system - constexpr uint8_t GSOF_VEL_LEN = 0x0D; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags - enum class VEL_FIELDS : uint8_t { - VALID = 1U << 0, - CONSECUTIVE_MEASUREMENTS = 1U << 1, - HEADING_VALID = 1U << 2, - RESERVED_3 = 1U << 3, - RESERVED_4 = 1U << 4, - RESERVED_5 = 1U << 5, - RESERVED_6 = 1U << 6, - RESERVED_7 = 1U << 7, - }; - uint8_t vel_flags {0}; - if(d->have_lock) { - vel_flags |= uint8_t(VEL_FIELDS::VALID); - vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); - vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); - } - - const struct PACKED gsof_vel { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags - uint8_t flags; - uint32_t horiz_m_p_s; - uint32_t heading_rad; - uint32_t vertical_m_p_s; - } vel { - uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA), - GSOF_VEL_LEN, - vel_flags, - gsof_pack_float(d->speed_2d()), - gsof_pack_float(d->heading()), - // Trimble API has ambiguous direction here. - // Intentionally narrow from double. - gsof_pack_float(static_cast(d->speedD)) - }; - static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); - - payload_sz += sizeof(vel); - memcpy(&buf[offset], &vel, sizeof(vel)); - offset += sizeof(vel); - } - } - if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){ - const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; - const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; - - if (now - last_time > RateToPeriodMs(desired_rate)) { - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 - constexpr uint8_t GSOF_DOP_LEN = 0x10; - const struct PACKED gsof_dop { - const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) }; - const uint8_t RECORD_LEN { GSOF_DOP_LEN }; - uint32_t pdop = htobe32(1); - uint32_t hdop = htobe32(1); - uint32_t vdop = htobe32(1); - uint32_t tdop = htobe32(1); - } dop {}; - // Check the payload size calculation in the compiler - constexpr auto dop_size = sizeof(gsof_dop); - static_assert(dop_size == 18); - constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); - static_assert(dop_record_type_size == 1); - constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); - static_assert(len_size == 1); - constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); - static_assert(dop_payload_size == GSOF_DOP_LEN); - - payload_sz += sizeof(dop); - memcpy(&buf[offset], &dop, sizeof(dop)); - offset += sizeof(dop); - } - } - if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){ - const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; - const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; - if (now - last_time > RateToPeriodMs(desired_rate)) { - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html - constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; - const struct PACKED gsof_pos_sigma { - const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) }; - const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; - uint32_t pos_rms = htobe32(0); - uint32_t sigma_e = htobe32(0); - uint32_t sigma_n = htobe32(0); - uint32_t cov_en = htobe32(0); - uint32_t sigma_up = htobe32(0); - uint32_t semi_major_axis = htobe32(0); - uint32_t semi_minor_axis = htobe32(0); - uint32_t orientation = htobe32(0); - uint32_t unit_variance = htobe32(0); - uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. - } pos_sigma {}; - static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); - payload_sz += sizeof(pos_sigma); - memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); - offset += sizeof(pos_sigma); - } - } - - assert(offset == payload_sz); - - // Don't send empy frames when all channels are disabled; - if (payload_sz > 0) { - send_gsof(buf, payload_sz); - } - -} - -bool DCOL_Parser::dcol_parse(const char data_in) { - bool ret = false; - switch (parse_state) { - case Parse_State::WAITING_ON_STX: - if (data_in == STX) { - reset(); - parse_state = Parse_State::WAITING_ON_STATUS; - } - break; - case Parse_State::WAITING_ON_STATUS: - if (data_in != (uint8_t)Status::OK) { - // Invalid, status should always be OK. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } else { - status = static_cast(data_in); - parse_state = Parse_State::WAITING_ON_PACKET_TYPE; - } - break; - case Parse_State::WAITING_ON_PACKET_TYPE: - if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) { - packet_type = Packet_Type::COMMAND_APPFILE; - } else { - // Unsupported command in this simulator. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - parse_state = Parse_State::WAITING_ON_LENGTH; - break; - case Parse_State::WAITING_ON_LENGTH: - expected_payload_length = data_in; - parse_state = Parse_State::WAITING_ON_PACKET_DATA; - break; - case Parse_State::WAITING_ON_PACKET_DATA: - payload[cur_payload_idx] = data_in; - if (++cur_payload_idx == expected_payload_length) { - parse_state = Parse_State::WAITING_ON_CSUM; - } - break; - case Parse_State::WAITING_ON_CSUM: - expected_csum = data_in; - parse_state = Parse_State::WAITING_ON_ETX; - break; - case Parse_State::WAITING_ON_ETX: - if (data_in != ETX) { - reset(); - } - if (!valid_csum()) { - // GSOF driver sent a packet with invalid CSUM. - // In real GSOF driver, the packet is simply ignored with no reply. - // In the SIM, treat this as a coding error. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } else { - ret = parse_payload(); - } - reset(); - break; - } - - return ret; -} - -uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) { - uint32_t min_period_ms = 0; - switch (rate) { - case Output_Rate::OFF: - min_period_ms = 0; - break; - case Output_Rate::FREQ_10_HZ: - min_period_ms = 100; - break; - case Output_Rate::FREQ_50_HZ: - min_period_ms = 20; - break; - case Output_Rate::FREQ_100_HZ: - min_period_ms = 10; - break; - } - return min_period_ms; -} - - -bool DCOL_Parser::valid_csum() { - uint8_t sum = (uint8_t)status; - sum += (uint8_t)packet_type; - sum += expected_payload_length; - sum += crc_sum_of_bytes(payload, expected_payload_length); - return sum == expected_csum; -} - -bool DCOL_Parser::parse_payload() { - bool success = false; - if (packet_type == Packet_Type::COMMAND_APPFILE) { - success = parse_cmd_appfile(); - } - - return success; -} - -bool DCOL_Parser::parse_cmd_appfile() { - // A file control info block contains: - // * version - // * device type - // * start application file flag - // * factory settings flag - constexpr uint8_t file_control_info_block_sz = 4; - // An appfile header contains: - // * transmisison number - // * page index - // * max page index - constexpr uint8_t appfile_header_sz = 3; - constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz; - if (expected_payload_length < min_cmd_appfile_sz) { - return false; - } - - // For now, ignore appfile_trans_num, return success regardless. - // If the driver doesn't send the right value, it's not clear what the behavior is supposed to be. - // Also would need to handle re-sync. - // For now, just store it for debugging. - appfile_trans_num = payload[0]; - - // File control information block parsing: - // https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html - constexpr uint8_t expected_app_file_spec_version = 0x03; - constexpr uint8_t file_ctrl_idx = appfile_header_sz; - if (payload[file_ctrl_idx] != expected_app_file_spec_version) { - // Only version 3 is supported at this time. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - constexpr uint8_t expected_dev_type = 0x00; - if (payload[file_ctrl_idx+1] != expected_dev_type) { - // Only "all" device type is supported. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - constexpr uint8_t expected_start_flag = 0x01; - if (payload[file_ctrl_idx+2] != expected_start_flag) { - // Only "immediate" start type is supported. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - - constexpr uint8_t expected_factory_settings_flag = 0x00; - if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) { - // Factory settings restore before appfile is not supported. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz; - const uint8_t record_type = payload[app_file_records_idx]; - if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) { - // Serial port baud/format - // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html - - // Ignore serial port index (COM Port) since there's only one in SITL. - // Ignore baud rate because you can't change baud yet in SITL. - // Ignore parity because it can't be changed in SITL. - // Ignore flow control because it's not yet in SITL. - } else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){ - // Output Message - // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html - - - // Ignore record length to save flash. - // Ignore port index since SITL is only one port. - if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) { - const auto gsof_submessage_type = payload[app_file_records_idx + 6]; - const auto rate = payload[app_file_records_idx + 4]; - if (rate == (uint8_t)Output_Rate::OFF) { - channel_rates[gsof_submessage_type] = static_cast(rate); - } else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) { - channel_rates[gsof_submessage_type] = static_cast(rate); - } else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) { - channel_rates[gsof_submessage_type] = static_cast(rate); - } else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) { - channel_rates[gsof_submessage_type] = static_cast(rate); - } else { - // Unsupported GSOF rate in SITL. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - } else { - // Only some data output protocols are supported in SITL. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - } else { - // Other application file packets are not yet supported. - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } - - return true; -} - -void DCOL_Parser::reset() { - cur_payload_idx = 0; - expected_payload_length = 0; - parse_state = Parse_State::WAITING_ON_STX; - // To be pedantic, one could zero the bytes in the payload, - // but this is skipped because it's extra CPU. - - // Note - appfile_trans_number is intended to persist over parser resets. -} - - -void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) -{ - // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields: - // * A fixed-length packet header (dcol_header) - // * A variable-length data frame (buf) - // * A fixed-length packet trailer (dcol_trailer) - // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0 - - // status bitfield - // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1 - const uint8_t STATUS = 0xa8; - const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT) - - // Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame. - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2 - - static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number - // Most messages, even GSOF49, only take one page. For SIM, assume it. - assert(size < 0xFA); // GPS SIM doesn't yet support paging - constexpr uint8_t PAGE_INDEX = 0; - constexpr uint8_t MAX_PAGE_INDEX = 0; - const uint8_t gsof_header[3] = { - TRANSMISSION_NUMBER, - PAGE_INDEX, - MAX_PAGE_INDEX, - }; - ++TRANSMISSION_NUMBER; - - // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. - // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. - // Thus, the following 5 bytes are not counted. - // 1) STX - // 2) STATUS - // 3) PACKET TYPE - // 4) LENGTH - // 5) CHECKSUM - // 6) ETX - // This aligns with manual's idea of data bytes: - // "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer." - // Thus, for this implementation with single-page single-record per DCOL packet, - // the length is simply the sum of data packet size, the gsof_header size. - const uint8_t length = size + sizeof(gsof_header); - const uint8_t dcol_header[4] { - STX, - STATUS, - PACKET_TYPE, - length - }; - - - // Sum bytes (status + type + length + data bytes) and modulo 256 the summation - // Because it's a uint8, use natural overflow - uint8_t csum = STATUS + PACKET_TYPE + length; - for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) { - csum += gsof_header[i]; - } - for (size_t i = 0; i < size; i++) { - csum += buf[i]; - } - - const uint8_t dcol_trailer[2] = { - csum, - ETX - }; - - write_to_autopilot((char*)dcol_header, sizeof(dcol_header)); - write_to_autopilot((char*)gsof_header, sizeof(gsof_header)); - write_to_autopilot((char*)buf, size); - write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer)); - const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer); - // Validate length based on everything but DCOL h - if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } -} - -uint64_t GPS_GSOF::gsof_pack_double(const double& src) -{ - uint64_t dst; - static_assert(sizeof(src) == sizeof(dst)); - memcpy(&dst, &src, sizeof(dst)); - dst = htobe64(dst); - return dst; -} - -uint32_t GPS_GSOF::gsof_pack_float(const float& src) -{ - uint32_t dst; - static_assert(sizeof(src) == sizeof(dst)); - memcpy(&dst, &src, sizeof(dst)); - dst = htobe32(dst); - return dst; -} - -void GPS_GSOF::update_read() { - // Technically, the max command is slightly larger. - // This will only slightly slow the response for packets that big. - char c[MAX_PAYLOAD_SIZE]; - const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE); - if (n_read > 0) { - for (uint8_t i = 0; i < n_read; i++) { - if (dcol_parse(c[i])) { - constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK}; - write_to_autopilot((char*)response, sizeof(response)); - } - // TODO handle NACK for failure - } - } -} - -/* - send MSP GPS data - */ -void GPS_MSP::publish(const GPS_Data *d) -{ - struct PACKED { - // header - struct PACKED { - uint8_t dollar = '$'; - uint8_t magic = 'X'; - uint8_t code = '<'; - uint8_t flags; - uint16_t cmd = 0x1F03; // GPS - uint16_t size = 52; - } hdr; - uint8_t instance; - uint16_t gps_week; - uint32_t ms_tow; - uint8_t fix_type; - uint8_t satellites_in_view; - uint16_t horizontal_pos_accuracy; // [cm] - uint16_t vertical_pos_accuracy; // [cm] - uint16_t horizontal_vel_accuracy; // [cm/s] - uint16_t hdop; - int32_t longitude; - int32_t latitude; - int32_t msl_altitude; // cm - int32_t ned_vel_north; // cm/s - int32_t ned_vel_east; - int32_t ned_vel_down; - uint16_t ground_course; // deg * 100, 0..36000 - uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - - // footer CRC - uint8_t crc; - } msp_gps {}; - - auto t = gps_time(); - struct timeval tv; - simulation_timeval(&tv); - auto *tm = gmtime(&tv.tv_sec); - - msp_gps.gps_week = t.week; - msp_gps.ms_tow = t.ms; - msp_gps.fix_type = d->have_lock?3:0; - msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3; - msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100; - msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100; - msp_gps.horizontal_vel_accuracy = 30; - msp_gps.hdop = 100; - msp_gps.longitude = d->longitude * 1.0e7; - msp_gps.latitude = d->latitude * 1.0e7; - msp_gps.msl_altitude = d->altitude * 100; - msp_gps.ned_vel_north = 100 * d->speedN; - msp_gps.ned_vel_east = 100 * d->speedE; - msp_gps.ned_vel_down = 100 * d->speedD; - msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100; - msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw - msp_gps.year = tm->tm_year; - msp_gps.month = tm->tm_mon; - msp_gps.day = tm->tm_mday; - msp_gps.hour = tm->tm_hour; - msp_gps.min = tm->tm_min; - msp_gps.sec = tm->tm_sec; - - // CRC is over packet without first 3 bytes and trailing CRC byte - msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4); - - write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps)); -} - -/* - read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED - */ -#if AP_SIM_GPS_FILE_ENABLED -void GPS_FILE::publish(const GPS_Data *d) -{ - static int fd[2] = {-1,-1}; - static uint32_t base_time[2]; - const uint16_t lognum = uint16_t(_sitl->gps_log_num.get()); - if (instance > 1) { - return; - } - if (fd[instance] == -1) { - char fname[] = "gpsN_NNN.log"; - hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum); - fd[instance] = open(fname, O_RDONLY|O_CLOEXEC); - if (fd[instance] == -1) { - return; - } - } - const uint32_t magic = 0x7fe53b04; - struct { - uint32_t magic; - uint32_t time_ms; - uint32_t n; - } header; - uint8_t *buf = nullptr; - while (true) { - if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) || - header.magic != magic) { - goto rewind_file; - } - if (header.time_ms+base_time[instance] > AP_HAL::millis()) { - // not ready for this data yet - ::lseek(fd[instance], -sizeof(header), SEEK_CUR); - return; - } - buf = new uint8_t[header.n]; - if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) { - write_to_autopilot((const char *)buf, header.n); - delete[] buf; - buf = nullptr; - continue; - } - goto rewind_file; - } - -rewind_file: - ::printf("GPS[%u] rewind\n", unsigned(instance)); - base_time[instance] = AP_HAL::millis(); - ::lseek(fd[instance], 0, SEEK_SET); - delete[] buf; -} -#endif // AP_SIM_GPS_FILE_ENABLED - void GPS::check_backend_allocation() { const Type configured_type = Type(_sitl->gps_type[instance].get()); @@ -1691,33 +165,47 @@ void GPS::check_backend_allocation() // no GPS attached break; +#if AP_SIM_GPS_UBLOX_ENABLED case Type::UBLOX: backend = new GPS_UBlox(*this, instance); break; +#endif +#if AP_SIM_GPS_NMEA_ENABLED case Type::NMEA: backend = new GPS_NMEA(*this, instance); break; +#endif +#if AP_SIM_GPS_SBP_ENABLED case Type::SBP: backend = new GPS_SBP(*this, instance); break; +#endif +#if AP_SIM_GPS_SBP2_ENABLED case Type::SBP2: backend = new GPS_SBP2(*this, instance); break; +#endif +#if AP_SIM_GPS_NOVA_ENABLED case Type::NOVA: backend = new GPS_NOVA(*this, instance); break; +#endif +#if AP_SIM_GPS_MSP_ENABLED case Type::MSP: backend = new GPS_MSP(*this, instance); break; +#endif +#if AP_SIM_GPS_GSOF_ENABLED case Type::GSOF: backend = new GPS_GSOF(*this, instance); break; +#endif #if AP_SIM_GPS_FILE_ENABLED case Type::FILE: diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index c9f8c34e41..8e6ccad06b 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -23,19 +23,10 @@ param set SERIAL5_PROTOCOL 5 #pragma once -#include - -#ifndef HAL_SIM_GPS_ENABLED -#define HAL_SIM_GPS_ENABLED AP_SIM_ENABLED -#endif +#include "SIM_config.h" #if HAL_SIM_GPS_ENABLED -#ifndef AP_SIM_GPS_FILE_ENABLED -// really need to use AP_FileSystem for this. -#define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) -#endif - #include "SIM_SerialDevice.h" namespace SITL { @@ -87,237 +78,15 @@ protected: class SIM *_sitl; -}; - -class GPS_FILE : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_FILE); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; -}; - -class DCOL_Parser { - // The DCOL parser is used by Trimble GSOF devices. - // It's used for doing configuration. - // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html -public: - // Feed data in to the DCOL parser. - // If the data reaches a parse state that needs to write ACK/NACK back out, - // the function returns true with a populated data_out value. - // Otherwise, it returns false waiting for more data. - bool dcol_parse(const char data_in); - - static constexpr uint8_t STX = 0x02; - static constexpr uint8_t ETX = 0x03; - - // Receiver status code - enum class Status : uint8_t { - OK = 0x00, + struct GPS_TOW { + // Number of weeks since midnight 5-6 January 1980 + uint16_t week; + // Time since start of the GPS week [mS] + uint32_t ms; }; - // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html - enum class Command_Response : uint8_t { - ACK = 0x06, - NACK = 0x15, - }; - - // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc - enum class Output_Rate : uint8_t { - OFF = 0, - FREQ_10_HZ = 1, - FREQ_50_HZ = 15, - FREQ_100_HZ = 16, - }; - - // https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0 - enum class Packet_Type : uint8_t { - COMMAND_APPFILE = 0x64, - }; - - // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html - enum class Appfile_Record_Type : uint8_t { - SERIAL_PORT_BAUD_RATE_FORMAT = 0x02, - OUTPUT_MESSAGE = 0x07, - }; - - // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output - enum class Output_Msg_Msg_Type : uint8_t { - GSOF = 10, - }; - - // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 - enum class Gsof_Msg_Record_Type : uint8_t { - POSITION_TIME = 1, - LLH = 2, - VELOCITY_DATA = 8, - PDOP_INFO = 9, - POSITION_SIGMA_INFO = 12, - }; - -protected: - // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html - static constexpr uint8_t MAX_PAYLOAD_SIZE = 255; - - // GSOF supports this many different packet types. - // Only a fraction are supported by the simulator. - // Waste some RAM and allocate arrays for the whole set. - // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 - static constexpr uint8_t MAX_CHANNEL_NUM = 70; - // Rates of dynamically enabled channels. - // Assume factory behavior of no enabled channels. - // Each channel can send data out at its own rate. - Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF}; - - // Last publish time of dynamically enabled channels. - uint32_t last_publish_ms[MAX_CHANNEL_NUM]; - - static uint32_t RateToPeriodMs(const Output_Rate rate); - -private: - - // Internal parser implementation state - enum class Parse_State { - WAITING_ON_STX, - WAITING_ON_STATUS, - WAITING_ON_PACKET_TYPE, - WAITING_ON_LENGTH, - WAITING_ON_PACKET_DATA, - WAITING_ON_CSUM, - WAITING_ON_ETX, - }; - - bool valid_csum(); - bool parse_payload(); - // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html - bool parse_cmd_appfile(); - - - // states for currently parsing packet - Status status; - Parse_State parse_state = {Parse_State::WAITING_ON_STX}; - Packet_Type packet_type; - // This is the length in the header. - uint8_t expected_payload_length; - // This is the increasing tally of bytes per packet. - uint8_t cur_payload_idx; - // This is the expected packet checksum in the trailer. - uint8_t expected_csum; - - // The application file record transmission number - uint8_t appfile_trans_num; - - uint8_t payload[MAX_PAYLOAD_SIZE]; - - // Clear all parser state/flags for handling a fresh packet. - void reset(); -}; - -class GPS_GSOF : public GPS_Backend, public DCOL_Parser { -public: - CLASS_NO_COPY(GPS_GSOF); - - using GPS_Backend::GPS_Backend; - - - // GPS_Backend overrides - void publish(const GPS_Data *d) override; - void update_read() override; - -private: - void send_gsof(const uint8_t *buf, const uint16_t size); - - // These packing utilities for GSOF perform a type-safe floating point byteswap. - // They return integer types because returning floating points would involve an extra copy. - uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED; - uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED; -}; - -class GPS_NMEA : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_NMEA); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; - -private: - - uint8_t nmea_checksum(const char *s); - void nmea_printf(const char *fmt, ...); - void update_nmea(const GPS_Data *d); - -}; - -class GPS_NOVA : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_NOVA); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; - - uint32_t device_baud() const override { return 19200; } - -private: - - void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); - uint32_t CRC32Value(uint32_t icrc); - uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); -}; - -class GPS_MSP : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_MSP); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; -}; - -class GPS_SBP_Common : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_SBP_Common); - - using GPS_Backend::GPS_Backend; - -protected: - - void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); - -}; - -class GPS_SBP : public GPS_SBP_Common { -public: - CLASS_NO_COPY(GPS_SBP); - - using GPS_SBP_Common::GPS_SBP_Common; - - void publish(const GPS_Data *d) override; - -}; - -class GPS_SBP2 : public GPS_SBP_Common { -public: - CLASS_NO_COPY(GPS_SBP2); - - using GPS_SBP_Common::GPS_SBP_Common; - - void publish(const GPS_Data *d) override; - -}; - -class GPS_UBlox : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_UBlox); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; - -private: - void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); + static GPS_TOW gps_time(); + static void simulation_timeval(struct timeval *tv); }; class GPS : public SerialDevice { @@ -327,16 +96,30 @@ public: enum Type { NONE = 0, +#if AP_SIM_GPS_UBLOX_ENABLED UBLOX = 1, +#endif +#if AP_SIM_GPS_NMEA_ENABLED NMEA = 5, +#endif +#if AP_SIM_GPS_SBP_ENABLED SBP = 6, +#endif #if AP_SIM_GPS_FILE_ENABLED FILE = 7, #endif +#if AP_SIM_GPS_NOVA_ENABLED NOVA = 8, +#endif +#if AP_SIM_GPS_SBP2_ENABLED SBP2 = 9, +#endif +#if AP_SIM_GPS_GSOF_ENABLED GSOF = 11, // matches GPS_TYPE +#endif +#if AP_SIM_GPS_MSP_ENABLED MSP = 19, +#endif }; GPS(uint8_t _instance); @@ -352,8 +135,6 @@ private: uint8_t instance; - int ext_fifo_fd; - // The last time GPS data was written [mS] uint32_t last_write_update_ms; diff --git a/libraries/SITL/SIM_GPS_FILE.cpp b/libraries/SITL/SIM_GPS_FILE.cpp new file mode 100644 index 0000000000..e4c096f1eb --- /dev/null +++ b/libraries/SITL/SIM_GPS_FILE.cpp @@ -0,0 +1,67 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_FILE_ENABLED + +#include "SIM_GPS_FILE.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace SITL; + +/* + read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED + */ +void GPS_FILE::publish(const GPS_Data *d) +{ + static int fd[2] = {-1,-1}; + static uint32_t base_time[2]; + const uint16_t lognum = uint16_t(_sitl->gps_log_num.get()); + if (instance > 1) { + return; + } + if (fd[instance] == -1) { + char fname[] = "gpsN_NNN.log"; + hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum); + fd[instance] = open(fname, O_RDONLY|O_CLOEXEC); + if (fd[instance] == -1) { + return; + } + } + const uint32_t magic = 0x7fe53b04; + struct { + uint32_t magic; + uint32_t time_ms; + uint32_t n; + } header; + uint8_t *buf = nullptr; + while (true) { + if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) || + header.magic != magic) { + goto rewind_file; + } + if (header.time_ms+base_time[instance] > AP_HAL::millis()) { + // not ready for this data yet + ::lseek(fd[instance], -sizeof(header), SEEK_CUR); + return; + } + buf = new uint8_t[header.n]; + if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) { + write_to_autopilot((const char *)buf, header.n); + delete[] buf; + buf = nullptr; + continue; + } + goto rewind_file; + } + +rewind_file: + ::printf("GPS[%u] rewind\n", unsigned(instance)); + base_time[instance] = AP_HAL::millis(); + ::lseek(fd[instance], 0, SEEK_SET); + delete[] buf; +} + +#endif // AP_SIM_GPS_FILE_ENABLED diff --git a/libraries/SITL/SIM_GPS_FILE.h b/libraries/SITL/SIM_GPS_FILE.h new file mode 100644 index 0000000000..980f9602ad --- /dev/null +++ b/libraries/SITL/SIM_GPS_FILE.h @@ -0,0 +1,20 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_FILE_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_FILE : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_FILE); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; +}; + +}; + +#endif // AP_SIM_GPS_FILE_ENABLED diff --git a/libraries/SITL/SIM_GPS_GSOF.cpp b/libraries/SITL/SIM_GPS_GSOF.cpp new file mode 100644 index 0000000000..477ef5a87a --- /dev/null +++ b/libraries/SITL/SIM_GPS_GSOF.cpp @@ -0,0 +1,569 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_GSOF_ENABLED + +#include "SIM_GPS_GSOF.h" + +#include +#include + +#include + +using namespace SITL; + +void GPS_GSOF::publish(const GPS_Data *d) +{ + // This logic is to populate output buffer only with enabled channels. + // It also only sends each channel at the configured rate. + const uint64_t now = AP_HAL::millis(); + uint8_t buf[MAX_PAYLOAD_SIZE] = {}; + uint8_t payload_sz = 0; + uint8_t offset = 0; + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 + constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; + // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE + const uint8_t bootcount = 17; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + enum class POS_FLAGS_1 : uint8_t { + NEW_POSITION = 1U << 0, + CLOCK_FIX_CALULATED = 1U << 1, + HORIZ_FROM_THIS_POS = 1U << 2, + HEIGHT_FROM_THIS_POS = 1U << 3, + RESERVED_4 = 1U << 4, + LEAST_SQ_POSITION = 1U << 5, + RESERVED_6 = 1U << 6, + POSITION_L1_PSEUDORANGES = 1U << 7 + }; + const uint8_t pos_flags_1 { + uint8_t(POS_FLAGS_1::NEW_POSITION) | + uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | + uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::RESERVED_4) | + uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | + uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) + }; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + enum class POS_FLAGS_2 : uint8_t { + DIFFERENTIAL_POS = 1U << 0, + DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, + POSITION_METHOD_FIXED_PHASE = 1U << 2, + OMNISTAR_ACTIVE = 1U << 3, + DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, + NETWORK_RTK = 1U << 5, + DITHERED_RTK = 1U << 6, + BEACON_DGNSS = 1U << 7, + }; + + // Simulate a GPS without RTK in SIM since there is no RTK SIM params. + // This means these flags are unset: + // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS + uint8_t pos_flags_2 {0}; + if(d->have_lock) { + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); + pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); + } + + const auto gps_tow = gps_time(); + const struct PACKED gsof_pos_time { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint32_t time_week_ms; + uint16_t time_week; + uint8_t num_sats; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + uint8_t pos_flags_1; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + uint8_t pos_flags_2; + uint8_t initialized_num; + } pos_time { + uint8_t(Gsof_Msg_Record_Type::POSITION_TIME), + GSOF_POS_TIME_LEN, + htobe32(gps_tow.ms), + htobe16(gps_tow.week), + d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), + pos_flags_1, + pos_flags_2, + bootcount + }; + static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); + + payload_sz += sizeof(pos_time); + memcpy(&buf[offset], &pos_time, sizeof(pos_time)); + offset += sizeof(pos_time); + } + } + + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20 + constexpr uint8_t GSOF_POS_LEN = 0x18; + + const struct PACKED gsof_pos { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint64_t lat; + uint64_t lng; + uint64_t alt; + } pos { + uint8_t(Gsof_Msg_Record_Type::LLH), + GSOF_POS_LEN, + gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(static_cast(d->altitude)) + }; + static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); + + payload_sz += sizeof(pos); + memcpy(&buf[offset], &pos, sizeof(pos)); + offset += sizeof(pos); + } + } + + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html + // use the smaller packet by ignoring local coordinate system + constexpr uint8_t GSOF_VEL_LEN = 0x0D; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + enum class VEL_FIELDS : uint8_t { + VALID = 1U << 0, + CONSECUTIVE_MEASUREMENTS = 1U << 1, + HEADING_VALID = 1U << 2, + RESERVED_3 = 1U << 3, + RESERVED_4 = 1U << 4, + RESERVED_5 = 1U << 5, + RESERVED_6 = 1U << 6, + RESERVED_7 = 1U << 7, + }; + uint8_t vel_flags {0}; + if(d->have_lock) { + vel_flags |= uint8_t(VEL_FIELDS::VALID); + vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); + vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); + } + + const struct PACKED gsof_vel { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + uint8_t flags; + uint32_t horiz_m_p_s; + uint32_t heading_rad; + uint32_t vertical_m_p_s; + } vel { + uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA), + GSOF_VEL_LEN, + vel_flags, + gsof_pack_float(d->speed_2d()), + gsof_pack_float(d->heading()), + // Trimble API has ambiguous direction here. + // Intentionally narrow from double. + gsof_pack_float(static_cast(d->speedD)) + }; + static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); + + payload_sz += sizeof(vel); + memcpy(&buf[offset], &vel, sizeof(vel)); + offset += sizeof(vel); + } + } + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 + constexpr uint8_t GSOF_DOP_LEN = 0x10; + const struct PACKED gsof_dop { + const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) }; + const uint8_t RECORD_LEN { GSOF_DOP_LEN }; + uint32_t pdop = htobe32(1); + uint32_t hdop = htobe32(1); + uint32_t vdop = htobe32(1); + uint32_t tdop = htobe32(1); + } dop {}; + // Check the payload size calculation in the compiler + constexpr auto dop_size = sizeof(gsof_dop); + static_assert(dop_size == 18); + constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); + static_assert(dop_record_type_size == 1); + constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); + static_assert(len_size == 1); + constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); + static_assert(dop_payload_size == GSOF_DOP_LEN); + + payload_sz += sizeof(dop); + memcpy(&buf[offset], &dop, sizeof(dop)); + offset += sizeof(dop); + } + } + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html + constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; + const struct PACKED gsof_pos_sigma { + const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) }; + const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; + uint32_t pos_rms = htobe32(0); + uint32_t sigma_e = htobe32(0); + uint32_t sigma_n = htobe32(0); + uint32_t cov_en = htobe32(0); + uint32_t sigma_up = htobe32(0); + uint32_t semi_major_axis = htobe32(0); + uint32_t semi_minor_axis = htobe32(0); + uint32_t orientation = htobe32(0); + uint32_t unit_variance = htobe32(0); + uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. + } pos_sigma {}; + static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); + payload_sz += sizeof(pos_sigma); + memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); + offset += sizeof(pos_sigma); + } + } + + assert(offset == payload_sz); + + // Don't send empy frames when all channels are disabled; + if (payload_sz > 0) { + send_gsof(buf, payload_sz); + } + +} + +bool DCOL_Parser::dcol_parse(const char data_in) { + bool ret = false; + switch (parse_state) { + case Parse_State::WAITING_ON_STX: + if (data_in == STX) { + reset(); + parse_state = Parse_State::WAITING_ON_STATUS; + } + break; + case Parse_State::WAITING_ON_STATUS: + if (data_in != (uint8_t)Status::OK) { + // Invalid, status should always be OK. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } else { + status = static_cast(data_in); + parse_state = Parse_State::WAITING_ON_PACKET_TYPE; + } + break; + case Parse_State::WAITING_ON_PACKET_TYPE: + if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) { + packet_type = Packet_Type::COMMAND_APPFILE; + } else { + // Unsupported command in this simulator. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + parse_state = Parse_State::WAITING_ON_LENGTH; + break; + case Parse_State::WAITING_ON_LENGTH: + expected_payload_length = data_in; + parse_state = Parse_State::WAITING_ON_PACKET_DATA; + break; + case Parse_State::WAITING_ON_PACKET_DATA: + payload[cur_payload_idx] = data_in; + if (++cur_payload_idx == expected_payload_length) { + parse_state = Parse_State::WAITING_ON_CSUM; + } + break; + case Parse_State::WAITING_ON_CSUM: + expected_csum = data_in; + parse_state = Parse_State::WAITING_ON_ETX; + break; + case Parse_State::WAITING_ON_ETX: + if (data_in != ETX) { + reset(); + } + if (!valid_csum()) { + // GSOF driver sent a packet with invalid CSUM. + // In real GSOF driver, the packet is simply ignored with no reply. + // In the SIM, treat this as a coding error. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } else { + ret = parse_payload(); + } + reset(); + break; + } + + return ret; +} + +uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) { + uint32_t min_period_ms = 0; + switch (rate) { + case Output_Rate::OFF: + min_period_ms = 0; + break; + case Output_Rate::FREQ_10_HZ: + min_period_ms = 100; + break; + case Output_Rate::FREQ_50_HZ: + min_period_ms = 20; + break; + case Output_Rate::FREQ_100_HZ: + min_period_ms = 10; + break; + } + return min_period_ms; +} + + +bool DCOL_Parser::valid_csum() { + uint8_t sum = (uint8_t)status; + sum += (uint8_t)packet_type; + sum += expected_payload_length; + sum += crc_sum_of_bytes(payload, expected_payload_length); + return sum == expected_csum; +} + +bool DCOL_Parser::parse_payload() { + bool success = false; + if (packet_type == Packet_Type::COMMAND_APPFILE) { + success = parse_cmd_appfile(); + } + + return success; +} + +bool DCOL_Parser::parse_cmd_appfile() { + // A file control info block contains: + // * version + // * device type + // * start application file flag + // * factory settings flag + constexpr uint8_t file_control_info_block_sz = 4; + // An appfile header contains: + // * transmisison number + // * page index + // * max page index + constexpr uint8_t appfile_header_sz = 3; + constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz; + if (expected_payload_length < min_cmd_appfile_sz) { + return false; + } + + // For now, ignore appfile_trans_num, return success regardless. + // If the driver doesn't send the right value, it's not clear what the behavior is supposed to be. + // Also would need to handle re-sync. + // For now, just store it for debugging. + appfile_trans_num = payload[0]; + + // File control information block parsing: + // https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html + constexpr uint8_t expected_app_file_spec_version = 0x03; + constexpr uint8_t file_ctrl_idx = appfile_header_sz; + if (payload[file_ctrl_idx] != expected_app_file_spec_version) { + // Only version 3 is supported at this time. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t expected_dev_type = 0x00; + if (payload[file_ctrl_idx+1] != expected_dev_type) { + // Only "all" device type is supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t expected_start_flag = 0x01; + if (payload[file_ctrl_idx+2] != expected_start_flag) { + // Only "immediate" start type is supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + + constexpr uint8_t expected_factory_settings_flag = 0x00; + if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) { + // Factory settings restore before appfile is not supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz; + const uint8_t record_type = payload[app_file_records_idx]; + if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) { + // Serial port baud/format + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html + + // Ignore serial port index (COM Port) since there's only one in SITL. + // Ignore baud rate because you can't change baud yet in SITL. + // Ignore parity because it can't be changed in SITL. + // Ignore flow control because it's not yet in SITL. + } else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){ + // Output Message + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html + + + // Ignore record length to save flash. + // Ignore port index since SITL is only one port. + if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) { + const auto gsof_submessage_type = payload[app_file_records_idx + 6]; + const auto rate = payload[app_file_records_idx + 4]; + if (rate == (uint8_t)Output_Rate::OFF) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else { + // Unsupported GSOF rate in SITL. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + } else { + // Only some data output protocols are supported in SITL. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + } else { + // Other application file packets are not yet supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + return true; +} + +void DCOL_Parser::reset() { + cur_payload_idx = 0; + expected_payload_length = 0; + parse_state = Parse_State::WAITING_ON_STX; + // To be pedantic, one could zero the bytes in the payload, + // but this is skipped because it's extra CPU. + + // Note - appfile_trans_number is intended to persist over parser resets. +} + + +void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) +{ + // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields: + // * A fixed-length packet header (dcol_header) + // * A variable-length data frame (buf) + // * A fixed-length packet trailer (dcol_trailer) + // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0 + + // status bitfield + // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1 + const uint8_t STATUS = 0xa8; + const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT) + + // Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame. + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2 + + static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number + // Most messages, even GSOF49, only take one page. For SIM, assume it. + assert(size < 0xFA); // GPS SIM doesn't yet support paging + constexpr uint8_t PAGE_INDEX = 0; + constexpr uint8_t MAX_PAGE_INDEX = 0; + const uint8_t gsof_header[3] = { + TRANSMISSION_NUMBER, + PAGE_INDEX, + MAX_PAGE_INDEX, + }; + ++TRANSMISSION_NUMBER; + + // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. + // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. + // Thus, the following 5 bytes are not counted. + // 1) STX + // 2) STATUS + // 3) PACKET TYPE + // 4) LENGTH + // 5) CHECKSUM + // 6) ETX + // This aligns with manual's idea of data bytes: + // "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer." + // Thus, for this implementation with single-page single-record per DCOL packet, + // the length is simply the sum of data packet size, the gsof_header size. + const uint8_t length = size + sizeof(gsof_header); + const uint8_t dcol_header[4] { + STX, + STATUS, + PACKET_TYPE, + length + }; + + + // Sum bytes (status + type + length + data bytes) and modulo 256 the summation + // Because it's a uint8, use natural overflow + uint8_t csum = STATUS + PACKET_TYPE + length; + for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) { + csum += gsof_header[i]; + } + for (size_t i = 0; i < size; i++) { + csum += buf[i]; + } + + const uint8_t dcol_trailer[2] = { + csum, + ETX + }; + + write_to_autopilot((char*)dcol_header, sizeof(dcol_header)); + write_to_autopilot((char*)gsof_header, sizeof(gsof_header)); + write_to_autopilot((char*)buf, size); + write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer)); + const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer); + // Validate length based on everything but DCOL h + if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } +} + +uint64_t GPS_GSOF::gsof_pack_double(const double& src) +{ + uint64_t dst; + static_assert(sizeof(src) == sizeof(dst)); + memcpy(&dst, &src, sizeof(dst)); + dst = htobe64(dst); + return dst; +} + +uint32_t GPS_GSOF::gsof_pack_float(const float& src) +{ + uint32_t dst; + static_assert(sizeof(src) == sizeof(dst)); + memcpy(&dst, &src, sizeof(dst)); + dst = htobe32(dst); + return dst; +} + +void GPS_GSOF::update_read() { + // Technically, the max command is slightly larger. + // This will only slightly slow the response for packets that big. + char c[MAX_PAYLOAD_SIZE]; + const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE); + if (n_read > 0) { + for (uint8_t i = 0; i < n_read; i++) { + if (dcol_parse(c[i])) { + constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK}; + write_to_autopilot((char*)response, sizeof(response)); + } + // TODO handle NACK for failure + } + } +} + +#endif // AP_SIM_GPS_GSOF_ENABLED diff --git a/libraries/SITL/SIM_GPS_GSOF.h b/libraries/SITL/SIM_GPS_GSOF.h new file mode 100644 index 0000000000..1780781537 --- /dev/null +++ b/libraries/SITL/SIM_GPS_GSOF.h @@ -0,0 +1,149 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_GSOF_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class DCOL_Parser { + // The DCOL parser is used by Trimble GSOF devices. + // It's used for doing configuration. + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html +public: + // Feed data in to the DCOL parser. + // If the data reaches a parse state that needs to write ACK/NACK back out, + // the function returns true with a populated data_out value. + // Otherwise, it returns false waiting for more data. + bool dcol_parse(const char data_in); + + static constexpr uint8_t STX = 0x02; + static constexpr uint8_t ETX = 0x03; + + // Receiver status code + enum class Status : uint8_t { + OK = 0x00, + }; + + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html + enum class Command_Response : uint8_t { + ACK = 0x06, + NACK = 0x15, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc + enum class Output_Rate : uint8_t { + OFF = 0, + FREQ_10_HZ = 1, + FREQ_50_HZ = 15, + FREQ_100_HZ = 16, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0 + enum class Packet_Type : uint8_t { + COMMAND_APPFILE = 0x64, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + enum class Appfile_Record_Type : uint8_t { + SERIAL_PORT_BAUD_RATE_FORMAT = 0x02, + OUTPUT_MESSAGE = 0x07, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output + enum class Output_Msg_Msg_Type : uint8_t { + GSOF = 10, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + enum class Gsof_Msg_Record_Type : uint8_t { + POSITION_TIME = 1, + LLH = 2, + VELOCITY_DATA = 8, + PDOP_INFO = 9, + POSITION_SIGMA_INFO = 12, + }; + +protected: + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html + static constexpr uint8_t MAX_PAYLOAD_SIZE = 255; + + // GSOF supports this many different packet types. + // Only a fraction are supported by the simulator. + // Waste some RAM and allocate arrays for the whole set. + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + static constexpr uint8_t MAX_CHANNEL_NUM = 70; + // Rates of dynamically enabled channels. + // Assume factory behavior of no enabled channels. + // Each channel can send data out at its own rate. + Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF}; + + // Last publish time of dynamically enabled channels. + uint32_t last_publish_ms[MAX_CHANNEL_NUM]; + + static uint32_t RateToPeriodMs(const Output_Rate rate); + +private: + + // Internal parser implementation state + enum class Parse_State { + WAITING_ON_STX, + WAITING_ON_STATUS, + WAITING_ON_PACKET_TYPE, + WAITING_ON_LENGTH, + WAITING_ON_PACKET_DATA, + WAITING_ON_CSUM, + WAITING_ON_ETX, + }; + + bool valid_csum(); + bool parse_payload(); + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + bool parse_cmd_appfile(); + + + // states for currently parsing packet + Status status; + Parse_State parse_state = {Parse_State::WAITING_ON_STX}; + Packet_Type packet_type; + // This is the length in the header. + uint8_t expected_payload_length; + // This is the increasing tally of bytes per packet. + uint8_t cur_payload_idx; + // This is the expected packet checksum in the trailer. + uint8_t expected_csum; + + // The application file record transmission number + uint8_t appfile_trans_num; + + uint8_t payload[MAX_PAYLOAD_SIZE]; + + // Clear all parser state/flags for handling a fresh packet. + void reset(); +}; + +class GPS_GSOF : public GPS_Backend, public DCOL_Parser { +public: + CLASS_NO_COPY(GPS_GSOF); + + using GPS_Backend::GPS_Backend; + + + // GPS_Backend overrides + void publish(const GPS_Data *d) override; + void update_read() override; + +private: + void send_gsof(const uint8_t *buf, const uint16_t size); + + // These packing utilities for GSOF perform a type-safe floating point byteswap. + // They return integer types because returning floating points would involve an extra copy. + uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED; + uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED; +}; + +}; + +#endif // AP_SIM_GPS_GSOF_ENABLED diff --git a/libraries/SITL/SIM_GPS_MSP.cpp b/libraries/SITL/SIM_GPS_MSP.cpp new file mode 100644 index 0000000000..1751b5bdbd --- /dev/null +++ b/libraries/SITL/SIM_GPS_MSP.cpp @@ -0,0 +1,88 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_MSP_ENABLED + +#include "SIM_GPS_MSP.h" + +#include + +using namespace SITL; + +/* + send MSP GPS data + */ +void GPS_MSP::publish(const GPS_Data *d) +{ + struct PACKED { + // header + struct PACKED { + uint8_t dollar = '$'; + uint8_t magic = 'X'; + uint8_t code = '<'; + uint8_t flags; + uint16_t cmd = 0x1F03; // GPS + uint16_t size = 52; + } hdr; + uint8_t instance; + uint16_t gps_week; + uint32_t ms_tow; + uint8_t fix_type; + uint8_t satellites_in_view; + uint16_t horizontal_pos_accuracy; // [cm] + uint16_t vertical_pos_accuracy; // [cm] + uint16_t horizontal_vel_accuracy; // [cm/s] + uint16_t hdop; + int32_t longitude; + int32_t latitude; + int32_t msl_altitude; // cm + int32_t ned_vel_north; // cm/s + int32_t ned_vel_east; + int32_t ned_vel_down; + uint16_t ground_course; // deg * 100, 0..36000 + uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + + // footer CRC + uint8_t crc; + } msp_gps {}; + + auto t = gps_time(); + struct timeval tv; + simulation_timeval(&tv); + auto *tm = gmtime(&tv.tv_sec); + + msp_gps.gps_week = t.week; + msp_gps.ms_tow = t.ms; + msp_gps.fix_type = d->have_lock?3:0; + msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3; + msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100; + msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100; + msp_gps.horizontal_vel_accuracy = 30; + msp_gps.hdop = 100; + msp_gps.longitude = d->longitude * 1.0e7; + msp_gps.latitude = d->latitude * 1.0e7; + msp_gps.msl_altitude = d->altitude * 100; + msp_gps.ned_vel_north = 100 * d->speedN; + msp_gps.ned_vel_east = 100 * d->speedE; + msp_gps.ned_vel_down = 100 * d->speedD; + msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100; + msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw + msp_gps.year = tm->tm_year; + msp_gps.month = tm->tm_mon; + msp_gps.day = tm->tm_mday; + msp_gps.hour = tm->tm_hour; + msp_gps.min = tm->tm_min; + msp_gps.sec = tm->tm_sec; + + // CRC is over packet without first 3 bytes and trailing CRC byte + msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4); + + write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps)); +} + +#endif // AP_SIM_GPS_MSP_ENABLED diff --git a/libraries/SITL/SIM_GPS_MSP.h b/libraries/SITL/SIM_GPS_MSP.h new file mode 100644 index 0000000000..ae8ca8fa1a --- /dev/null +++ b/libraries/SITL/SIM_GPS_MSP.h @@ -0,0 +1,20 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_MSP_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_MSP : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_MSP); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; +}; + +} + +#endif // AP_SIM_GPS_MSP_ENABLED diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp new file mode 100644 index 0000000000..cb15ce32d5 --- /dev/null +++ b/libraries/SITL/SIM_GPS_NMEA.cpp @@ -0,0 +1,125 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_NMEA_ENABLED + +#include "SIM_GPS_NMEA.h" + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace SITL; + +/* + formatted print of NMEA message, with checksum appended + */ +void GPS_NMEA::nmea_printf(const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + char *s = nmea_vaprintf(fmt, ap); + va_end(ap); + if (s != nullptr) { + write_to_autopilot((const char*)s, strlen(s)); + free(s); + } +} + + +/* + send a new GPS NMEA packet + */ +void GPS_NMEA::publish(const GPS_Data *d) +{ + struct timeval tv; + struct tm *tm; + char tstring[20]; + char dstring[20]; + char lat_string[20]; + char lng_string[20]; + + simulation_timeval(&tv); + + tm = gmtime(&tv.tv_sec); + + // format time string + hal.util->snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + tv.tv_usec*1.0e-6); + + // format date string + hal.util->snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); + + // format latitude + double deg = fabs(d->latitude); + hal.util->snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", + (unsigned)deg, + (deg - int(deg))*60, + d->latitude<0?'S':'N'); + + // format longitude + deg = fabs(d->longitude); + hal.util->snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", + (unsigned)deg, + (deg - int(deg))*60, + d->longitude<0?'W':'E'); + + nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", + tstring, + lat_string, + lng_string, + d->have_lock?1:0, + d->have_lock?_sitl->gps_numsats[instance]:3, + 1.2, + d->altitude); + + const float speed_mps = d->speed_2d(); + const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS; + const auto heading_rad = d->heading(); + + //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24 + nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A", + tstring, + heading_rad, + heading_rad, + speed_knots, + speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6); + + nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", + tstring, + d->have_lock?'A':'V', + lat_string, + lng_string, + speed_knots, + heading_rad, + dstring); + + if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) { + nmea_printf("$GPHDT,%.2f,T", d->yaw_deg); + } + else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) { + nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V'); + } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) { + // Unicore support + // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D + nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,", + tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4), + d->longitude, d->latitude, + d->altitude, + wrap_360(d->yaw_deg), + d->pitch_deg, + heading_rad, + speed_mps, + d->roll_deg, + d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed, + 3, // fixed rtk yaw solution, + d->have_lock?_sitl->gps_numsats[instance]:3, + d->have_lock?_sitl->gps_numsats[instance]:3, + d->speedE * 3.6, + d->speedN * 3.6, + -d->speedD * 3.6); + } +} + +#endif // AP_SIM_GPS_NMEA_ENABLED diff --git a/libraries/SITL/SIM_GPS_NMEA.h b/libraries/SITL/SIM_GPS_NMEA.h new file mode 100644 index 0000000000..471f297857 --- /dev/null +++ b/libraries/SITL/SIM_GPS_NMEA.h @@ -0,0 +1,29 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_NMEA_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_NMEA : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_NMEA); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + +private: + + uint8_t nmea_checksum(const char *s); + void nmea_printf(const char *fmt, ...); + void update_nmea(const GPS_Data *d); + +}; + +}; + +#endif // AP_SIM_GPS_NMEA_ENABLED diff --git a/libraries/SITL/SIM_GPS_NOVA.cpp b/libraries/SITL/SIM_GPS_NOVA.cpp new file mode 100644 index 0000000000..fd6b3da08e --- /dev/null +++ b/libraries/SITL/SIM_GPS_NOVA.cpp @@ -0,0 +1,177 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_NOVA_ENABLED + +#include "SIM_GPS_NOVA.h" + +#include + +using namespace SITL; + +void GPS_NOVA::publish(const GPS_Data *d) +{ + static struct PACKED nova_header + { + // 0 + uint8_t preamble[3]; + // 3 + uint8_t headerlength; + // 4 + uint16_t messageid; + // 6 + uint8_t messagetype; + //7 + uint8_t portaddr; + //8 + uint16_t messagelength; + //10 + uint16_t sequence; + //12 + uint8_t idletime; + //13 + uint8_t timestatus; + //14 + uint16_t week; + //16 + uint32_t tow; + //20 + uint32_t recvstatus; + // 24 + uint16_t resv; + //26 + uint16_t recvswver; + } header; + + struct PACKED psrdop + { + float gdop; + float pdop; + float hdop; + float htdop; + float tdop; + float cutoff; + uint32_t svcount; + // extra data for individual prns + } psrdop {}; + + struct PACKED bestpos + { + uint32_t solstat; + uint32_t postype; + double lat; + double lng; + double hgt; + float undulation; + uint32_t datumid; + float latsdev; + float lngsdev; + float hgtsdev; + // 4 bytes + uint8_t stnid[4]; + float diffage; + float sol_age; + uint8_t svstracked; + uint8_t svsused; + uint8_t svsl1; + uint8_t svsmultfreq; + uint8_t resv; + uint8_t extsolstat; + uint8_t galbeisigmask; + uint8_t gpsglosigmask; + } bestpos {}; + + struct PACKED bestvel + { + uint32_t solstat; + uint32_t veltype; + float latency; + float age; + double horspd; + double trkgnd; + // + up + double vertspd; + float resv; + } bestvel {}; + + const auto gps_tow = gps_time(); + + header.preamble[0] = 0xaa; + header.preamble[1] = 0x44; + header.preamble[2] = 0x12; + header.headerlength = sizeof(header); + header.week = gps_tow.week; + header.tow = gps_tow.ms; + + header.messageid = 174; + header.messagelength = sizeof(psrdop); + header.sequence += 1; + + psrdop.hdop = 1.20; + psrdop.htdop = 1.20; + nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop)); + + + header.messageid = 99; + header.messagelength = sizeof(bestvel); + header.sequence += 1; + + bestvel.horspd = norm(d->speedN, d->speedE); + bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN)); + bestvel.vertspd = -d->speedD; + + nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel)); + + + header.messageid = 42; + header.messagelength = sizeof(bestpos); + header.sequence += 1; + + bestpos.lat = d->latitude; + bestpos.lng = d->longitude; + bestpos.hgt = d->altitude; + bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3; + bestpos.latsdev=0.2; + bestpos.lngsdev=0.2; + bestpos.hgtsdev=0.2; + bestpos.solstat=0; + bestpos.postype=32; + + nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos)); +} + +void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen) +{ + write_to_autopilot((char*)header, headerlength); +write_to_autopilot((char*)payload, payloadlen); + + uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0); + crc = CalculateBlockCRC32(payloadlen, payload, crc); + + write_to_autopilot((char*)&crc, 4); +} + +#define CRC32_POLYNOMIAL 0xEDB88320L +uint32_t GPS_NOVA::CRC32Value(uint32_t icrc) +{ + int i; + uint32_t crc = icrc; + for ( i = 8 ; i > 0; i-- ) + { + if ( crc & 1 ) + crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; + else + crc >>= 1; + } + return crc; +} + +uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) +{ + while ( length-- != 0 ) + { + crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); + } + return( crc ); +} + +#endif // AP_SIM_GPS_NOVA_ENABLED diff --git a/libraries/SITL/SIM_GPS_NOVA.h b/libraries/SITL/SIM_GPS_NOVA.h new file mode 100644 index 0000000000..4df22b32d8 --- /dev/null +++ b/libraries/SITL/SIM_GPS_NOVA.h @@ -0,0 +1,30 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_NOVA_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_NOVA : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_NOVA); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + + uint32_t device_baud() const override { return 19200; } + +private: + + void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); + uint32_t CRC32Value(uint32_t icrc); + uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); +}; + +}; + +#endif // AP_SIM_GPS_NOVA_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP.cpp b/libraries/SITL/SIM_GPS_SBP.cpp new file mode 100644 index 0000000000..b3b8e77d89 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP.cpp @@ -0,0 +1,122 @@ +#include "SIM_GPS_SBP.h" + +#if AP_SIM_GPS_SBP_ENABLED + +#include + +using namespace SITL; + +void GPS_SBP::publish(const GPS_Data *d) +{ + struct sbp_heartbeat_t { + bool sys_error : 1; + bool io_error : 1; + bool nap_error : 1; + uint8_t res : 5; + uint8_t protocol_minor : 8; + uint8_t protocol_major : 8; + uint8_t res2 : 7; + bool ext_antenna : 1; + } hb; // 4 bytes + + 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 + uint8_t flags; //< Status flags (reserved) + } dops; + + static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; + 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_VEL_NED_MSGTYPE = 0x0205; + + const auto gps_tow = gps_time(); + + t.wn = gps_tow.week; + t.tow = gps_tow.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 = gps_tow.ms; + pos.lon = d->longitude; + pos.lat= d->latitude; + pos.height = d->altitude; + pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; + + // Send single point position solution + pos.flags = 0; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + // Send "pseudo-absolute" RTK position solution + pos.flags = 1; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + + velned.tow = gps_tow.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 = d->have_lock ? _sitl->gps_numsats[instance] : 3; + velned.flags = 0; + sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); + + static uint32_t do_every_count = 0; + if (do_every_count % 5 == 0) { + + dops.tow = gps_tow.ms; + dops.gdop = 1; + dops.pdop = 1; + dops.tdop = 1; + dops.hdop = 100; + dops.vdop = 1; + dops.flags = 1; + sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), + (uint8_t*)&dops); + + hb.protocol_major = 0; //Sends protocol version 0 + sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), + (uint8_t*)&hb); + + } + do_every_count++; +} + +#endif // AP_SIM_GPS_SBP_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP.h b/libraries/SITL/SIM_GPS_SBP.h new file mode 100644 index 0000000000..e0f1f58eab --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP.h @@ -0,0 +1,23 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_SBP_ENABLED + +#include "SIM_GPS_SBP_Common.h" + +namespace SITL { + +class GPS_SBP : public GPS_SBP_Common { +public: + CLASS_NO_COPY(GPS_SBP); + + using GPS_SBP_Common::GPS_SBP_Common; + + void publish(const GPS_Data *d) override; + +}; + +}; + +#endif // AP_SIM_GPS_SBP_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP2.cpp b/libraries/SITL/SIM_GPS_SBP2.cpp new file mode 100644 index 0000000000..1596d330af --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP2.cpp @@ -0,0 +1,121 @@ +#include "SIM_GPS_SBP2.h" + +#if AP_SIM_GPS_SBP2_ENABLED + +#include + +using namespace SITL; + +void GPS_SBP2::publish(const GPS_Data *d) +{ + struct sbp_heartbeat_t { + bool sys_error : 1; + bool io_error : 1; + bool nap_error : 1; + uint8_t res : 5; + uint8_t protocol_minor : 8; + uint8_t protocol_major : 8; + uint8_t res2 : 7; + bool ext_antenna : 1; + } hb; // 4 bytes + + 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 + uint8_t flags; //< Status flags (reserved) + } dops; + + static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; + static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102; + static const uint16_t SBP_DOPS_MSGTYPE = 0x0208; + static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A; + static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E; + + const auto gps_tow = gps_time(); + + t.wn = gps_tow.week; + t.tow = gps_tow.ms; + t.ns = 0; + t.flags = 1; + sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); + + if (!d->have_lock) { + return; + } + + pos.tow = gps_tow.ms; + pos.lon = d->longitude; + pos.lat= d->latitude; + pos.height = d->altitude; + pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; + + // Send single point position solution + pos.flags = 1; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + // Send "pseudo-absolute" RTK position solution + pos.flags = 4; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + + velned.tow = gps_tow.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 = d->have_lock ? _sitl->gps_numsats[instance] : 3; + velned.flags = 1; + sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); + + static uint32_t do_every_count = 0; + if (do_every_count % 5 == 0) { + + dops.tow = gps_tow.ms; + dops.gdop = 1; + dops.pdop = 1; + dops.tdop = 1; + dops.hdop = 100; + dops.vdop = 1; + dops.flags = 1; + sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), + (uint8_t*)&dops); + + hb.protocol_major = 2; //Sends protocol version 2.0 + sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), + (uint8_t*)&hb); + } + do_every_count++; +} + +#endif // AP_SIM_GPS_SBP2_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP2.h b/libraries/SITL/SIM_GPS_SBP2.h new file mode 100644 index 0000000000..580f430fea --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP2.h @@ -0,0 +1,23 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_SBP2_ENABLED + +#include "SIM_GPS_SBP_Common.h" + +namespace SITL { + +class GPS_SBP2 : public GPS_SBP_Common { +public: + CLASS_NO_COPY(GPS_SBP2); + + using GPS_SBP_Common::GPS_SBP_Common; + + void publish(const GPS_Data *d) override; + +}; + +}; + +#endif // AP_SIM_GPS_SBP2_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP_Common.cpp b/libraries/SITL/SIM_GPS_SBP_Common.cpp new file mode 100644 index 0000000000..90116d76da --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP_Common.cpp @@ -0,0 +1,34 @@ +#include "SIM_config.h" + +#if HAL_SIM_GPS_ENABLED + +#include "SIM_GPS_SBP_Common.h" + +#include + +using namespace SITL; + +void GPS_SBP_Common::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; + write_to_autopilot((char*)&preamble, 1); + write_to_autopilot((char*)&msg_type, 2); + write_to_autopilot((char*)&sender_id, 2); + write_to_autopilot((char*)&len, 1); + if (len > 0) { + write_to_autopilot((char*)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); + write_to_autopilot((char*)&crc, 2); +} + +#endif diff --git a/libraries/SITL/SIM_GPS_SBP_Common.h b/libraries/SITL/SIM_GPS_SBP_Common.h new file mode 100644 index 0000000000..5cb13e930f --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP_Common.h @@ -0,0 +1,25 @@ +#pragma once + +#include "SIM_config.h" + +#if HAL_SIM_GPS_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_SBP_Common : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_SBP_Common); + + using GPS_Backend::GPS_Backend; + +protected: + + void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); + +}; + +}; + +#endif diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp new file mode 100644 index 0000000000..7bcab4b376 --- /dev/null +++ b/libraries/SITL/SIM_GPS_UBLOX.cpp @@ -0,0 +1,324 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_UBLOX_ENABLED + +#include "SIM_GPS_UBLOX.h" + +#include + +using namespace SITL; + +/* + send a UBLOX GPS message + */ +void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) +{ + const uint8_t PREAMBLE1 = 0xb5; + const uint8_t PREAMBLE2 = 0x62; + const uint8_t CLASS_NAV = 0x1; + uint8_t hdr[6], chk[2]; + hdr[0] = PREAMBLE1; + hdr[1] = PREAMBLE2; + hdr[2] = CLASS_NAV; + hdr[3] = msgid; + hdr[4] = size & 0xFF; + hdr[5] = size >> 8; + chk[0] = chk[1] = hdr[2]; + chk[1] += (chk[0] += hdr[3]); + chk[1] += (chk[0] += hdr[4]); + chk[1] += (chk[0] += hdr[5]); + for (uint16_t i=0; ilongitude * 1.0e7; + pos.latitude = d->latitude * 1.0e7; + pos.altitude_ellipsoid = d->altitude * 1000.0f; + pos.altitude_msl = d->altitude * 1000.0f; + pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000; + + status.time = gps_tow.ms; + status.fix_type = d->have_lock?3:0; + status.fix_status = d->have_lock?1:0; + status.differential_status = 0; + status.res = 0; + status.time_to_first_fix = 0; + status.uptime = AP_HAL::millis(); + + velned.time = gps_tow.ms; + velned.ned_north = 100.0f * d->speedN; + velned.ned_east = 100.0f * d->speedE; + velned.ned_down = 100.0f * d->speedD; + velned.speed_2d = norm(d->speedN, d->speedE) * 100; + velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100; + velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; + if (velned.heading_2d < 0.0f) { + velned.heading_2d += 360.0f * 100000.0f; + } + velned.speed_accuracy = 40; + velned.heading_accuracy = 4; + + memset(&sol, 0, sizeof(sol)); + sol.fix_type = d->have_lock?3:0; + sol.fix_status = 221; + sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3; + sol.time = gps_tow.ms; + sol.week = gps_tow.week; + + dop.time = gps_tow.ms; + dop.gDOP = 65535; + dop.pDOP = 65535; + dop.tDOP = 65535; + dop.vDOP = 200; + dop.hDOP = 121; + dop.nDOP = 65535; + dop.eDOP = 65535; + + pvt.itow = gps_tow.ms; + pvt.year = 0; + pvt.month = 0; + pvt.day = 0; + pvt.hour = 0; + pvt.min = 0; + pvt.sec = 0; + pvt.valid = 0; // invalid utc date + pvt.t_acc = 0; + pvt.nano = 0; + pvt.fix_type = d->have_lock? 0x3 : 0; + pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok + pvt.flags2 =0; + pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3; + pvt.lon = d->longitude * 1.0e7; + pvt.lat = d->latitude * 1.0e7; + pvt.height = d->altitude * 1000.0f; + pvt.h_msl = d->altitude * 1000.0f; + pvt.h_acc = _sitl->gps_accuracy[instance] * 1000; + pvt.v_acc = _sitl->gps_accuracy[instance] * 1000; + pvt.velN = 1000.0f * d->speedN; + pvt.velE = 1000.0f * d->speedE; + pvt.velD = 1000.0f * d->speedD; + pvt.gspeed = norm(d->speedN, d->speedE) * 1000; + pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5; + pvt.s_acc = 40; + pvt.head_acc = 38 * 1.0e5; + pvt.p_dop = 65535; + memset(pvt.reserved1, '\0', ARRAY_SIZE(pvt.reserved1)); + pvt.headVeh = 0; + memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2)); + + if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { + const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get(); + const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get(); + Vector3f rel_antenna_pos = ant2_pos - ant1_pos; + Matrix3f rot; + // project attitude back using gyros to get antenna orientation at time of GPS sample + Vector3f gyro(radians(_sitl->state.rollRate), + radians(_sitl->state.pitchRate), + radians(_sitl->state.yawRate)); + rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg)); + const float lag = _sitl->gps_delay_ms[instance] * 0.001; + rot.rotate(gyro * (-lag)); + rel_antenna_pos = rot * rel_antenna_pos; + relposned.version = 1; + relposned.iTOW = gps_tow.ms; + relposned.relPosN = rel_antenna_pos.x * 100; + relposned.relPosE = rel_antenna_pos.y * 100; + relposned.relPosD = rel_antenna_pos.z * 100; + relposned.relPosLength = rel_antenna_pos.length() * 100; + relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5; + relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid; + } + + send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); + send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); + send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); + send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); + send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); + send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); + if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { + send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned)); + } + + if (gps_tow.ms > _next_nav_sv_info_time) { + svinfo.itow = gps_tow.ms; + svinfo.numCh = 32; + svinfo.globalFlags = 4; // u-blox 8/M8 + // fill in the SV's with some data even though firmware does not currently use it + // note that this is not using num_sats as we aren't dynamically creating this to match + for (uint8_t i = 0; i < SV_COUNT; i++) { + svinfo.sv[i].chn = i; + svinfo.sv[i].svid = i; + svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information + svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized + svinfo.sv[i].cno = MAX(20, 30 - i); + svinfo.sv[i].elev = MAX(30, 90 - i); + svinfo.sv[i].azim = i; + // not bothering to fill in prRes + } + send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo)); + _next_nav_sv_info_time = gps_tow.ms + 10000; // 10 second delay + } +} + +#endif // AP_SIM_GPS_UBLOX_ENABLED diff --git a/libraries/SITL/SIM_GPS_UBLOX.h b/libraries/SITL/SIM_GPS_UBLOX.h new file mode 100644 index 0000000000..6ab5d01eea --- /dev/null +++ b/libraries/SITL/SIM_GPS_UBLOX.h @@ -0,0 +1,23 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_UBLOX_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_UBlox : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_UBlox); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + +private: + void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); +}; + +}; + +#endif // AP_SIM_GPS_UBLOX_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 726b903276..7c760ee4bf 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -37,3 +37,44 @@ #ifndef AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED #define AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED 0 #endif + +#ifndef HAL_SIM_GPS_ENABLED +#define HAL_SIM_GPS_ENABLED AP_SIM_ENABLED +#endif + +#ifndef AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#define AP_SIM_GPS_BACKEND_DEFAULT_ENABLED AP_SIM_ENABLED +#endif + +#ifndef AP_SIM_GPS_FILE_ENABLED +// really need to use AP_FileSystem for this. +#define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) +#endif + +#ifndef AP_SIM_GPS_GSOF_ENABLED +#define AP_SIM_GPS_GSOF_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_MSP_ENABLED +#define AP_SIM_GPS_MSP_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_NMEA_ENABLED +#define AP_SIM_GPS_NMEA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_NOVA_ENABLED +#define AP_SIM_GPS_NOVA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_SBP2_ENABLED +#define AP_SIM_GPS_SBP2_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_SBP_ENABLED +#define AP_SIM_GPS_SBP_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_UBLOX_ENABLED +#define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif