/* SITL handling This simulates a GPS on a serial port Andrew Tridgell November 2011 */ #include <unistd.h> #include <fcntl.h> #include <stdio.h> #include <stdlib.h> #include <string.h> #include <errno.h> #include <math.h> #include <AP_GPS.h> #include <AP_GPS_UBLOX.h> #include "desktop.h" #include "util.h" // state of GPS emulation static struct { /* pipe emulating UBLOX GPS serial stream */ int gps_fd; uint32_t last_update; // milliseconds } gps_state; /* hook for reading from the GPS pipe */ ssize_t sitl_gps_read(int fd, void *buf, size_t count) { return read(fd, buf, count); } /* setup GPS input pipe */ int sitl_gps_pipe(void) { int fd[2]; pipe(fd); gps_state.gps_fd = fd[1]; gps_state.last_update = millis(); set_nonblocking(gps_state.gps_fd); set_nonblocking(fd[0]); return fd[0]; } /* send a UBLOX GPS message */ static void gps_send(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 (uint8_t i=0; i<size; i++) { chk[1] += (chk[0] += buf[i]); } write(gps_state.gps_fd, hdr, sizeof(hdr)); write(gps_state.gps_fd, buf, size); write(gps_state.gps_fd, chk, sizeof(chk)); } /* possibly send a new GPS UBLOX packet */ void sitl_update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, bool have_lock) { struct ubx_nav_posllh { uint32_t time; // GPS msToW int32_t longitude; int32_t latitude; int32_t altitude_ellipsoid; int32_t altitude_msl; uint32_t horizontal_accuracy; uint32_t vertical_accuracy; } pos; struct ubx_nav_status { uint32_t time; // GPS msToW uint8_t fix_type; uint8_t fix_status; uint8_t differential_status; uint8_t res; uint32_t time_to_first_fix; uint32_t uptime; // milliseconds } status; struct ubx_nav_velned { uint32_t time; // GPS msToW int32_t ned_north; int32_t ned_east; int32_t ned_down; uint32_t speed_3d; uint32_t speed_2d; int32_t heading_2d; uint32_t speed_accuracy; uint32_t heading_accuracy; } velned; const uint8_t MSG_POSLLH = 0x2; const uint8_t MSG_STATUS = 0x3; const uint8_t MSG_VELNED = 0x12; double lon_scale; // 4Hz if (millis() - gps_state.last_update < 250) { return; } gps_state.last_update = millis(); pos.time = millis(); // FIX pos.longitude = longitude * 1.0e7; pos.latitude = latitude * 1.0e7; pos.altitude_ellipsoid = altitude*1000.0; pos.altitude_msl = altitude*1000.0; pos.horizontal_accuracy = 5; pos.vertical_accuracy = 10; status.time = pos.time; status.fix_type = have_lock?3:0; status.fix_status = have_lock?1:0; status.differential_status = 0; status.res = 0; status.time_to_first_fix = 0; status.uptime = millis(); velned.time = pos.time; velned.ned_north = 100.0 * speedN; velned.ned_east = 100.0 * speedE; velned.ned_down = 0; #define sqr(x) ((x)*(x)) velned.speed_2d = sqrt(sqr(speedN)+sqr(speedE)) * 100; velned.speed_3d = velned.speed_2d; lon_scale = cos(ToRad(latitude)); velned.heading_2d = ToDeg(atan2(lon_scale*speedE, speedN)) * 100000.0; if (velned.heading_2d < 0.0) { velned.heading_2d += 360.0 * 100000.0; } velned.speed_accuracy = 2; velned.heading_accuracy = 4; if (gps_state.gps_fd == 0) { return; } gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status)); gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); }