void ugear_event( void ) {
if (UgearBuffer()){
ReadUgearBuffer();
}
if (ugear_msg_received){
parse_ugear_msg();
ugear_msg_received = FALSE;
if (gps_pos_available){
//gps_downlink();
gps_verbose_downlink = !launch;
UseGpsPosNoSend(estimator_update_state_gps);
gps_msg_received_counter = gps_msg_received_counter+1;
#ifdef GX2
if (gps_msg_received_counter == 1){
gps_send();
gps_msg_received_counter = 0;
}
#endif
#ifdef XSENSDL
if (gps_msg_received_counter == 25){
gps_send();
gps_msg_received_counter = 0;
}
#endif
gps_pos_available = FALSE;
}
}
}
int gps_sock_stream(struct gps_data_t *gpsdata, unsigned int flags, void *d)
/* ask gpsd to stream reports at you, hiding the command details */
{
char buf[GPS_JSON_COMMAND_MAX];
if ((flags & (WATCH_JSON | WATCH_NMEA | WATCH_RAW)) == 0) {
flags |= WATCH_JSON;
}
if ((flags & WATCH_DISABLE) != 0) {
(void)strlcpy(buf, "?WATCH={\"enable\":false,", sizeof(buf));
if (flags & WATCH_JSON)
(void)strlcat(buf, "\"json\":false,", sizeof(buf));
if (flags & WATCH_NMEA)
(void)strlcat(buf, "\"nmea\":false,", sizeof(buf));
if (flags & WATCH_RAW)
(void)strlcat(buf, "\"raw\":1,", sizeof(buf));
if (flags & WATCH_RARE)
(void)strlcat(buf, "\"raw\":0,", sizeof(buf));
if (flags & WATCH_SCALED)
(void)strlcat(buf, "\"scaled\":false,", sizeof(buf));
if (flags & WATCH_TIMING)
(void)strlcat(buf, "\"timing\":false,", sizeof(buf));
if (flags & WATCH_SPLIT24)
(void)strlcat(buf, "\"split24\":false,", sizeof(buf));
if (flags & WATCH_PPS)
(void)strlcat(buf, "\"pps\":false,", sizeof(buf));
str_rstrip_char(buf, ',');
(void)strlcat(buf, "};", sizeof(buf));
libgps_debug_trace((DEBUG_CALLS, "gps_stream() disable command: %s\n", buf));
return gps_send(gpsdata, buf);
} else { /* if ((flags & WATCH_ENABLE) != 0) */
(void)strlcpy(buf, "?WATCH={\"enable\":true,", sizeof(buf));
if (flags & WATCH_JSON)
(void)strlcat(buf, "\"json\":true,", sizeof(buf));
if (flags & WATCH_NMEA)
(void)strlcat(buf, "\"nmea\":true,", sizeof(buf));
if (flags & WATCH_RARE)
(void)strlcat(buf, "\"raw\":1,", sizeof(buf));
if (flags & WATCH_RAW)
(void)strlcat(buf, "\"raw\":2,", sizeof(buf));
if (flags & WATCH_SCALED)
(void)strlcat(buf, "\"scaled\":true,", sizeof(buf));
if (flags & WATCH_TIMING)
(void)strlcat(buf, "\"timing\":true,", sizeof(buf));
if (flags & WATCH_SPLIT24)
(void)strlcat(buf, "\"split24\":true,", sizeof(buf));
if (flags & WATCH_PPS)
(void)strlcat(buf, "\"pps\":true,", sizeof(buf));
if (flags & WATCH_DEVICE)
str_appendf(buf, sizeof(buf), "\"device\":\"%s\",", (char *)d);
str_rstrip_char(buf, ',');
(void)strlcat(buf, "};", sizeof(buf));
libgps_debug_trace((DEBUG_CALLS, "gps_stream() enable command: %s\n", buf));
return gps_send(gpsdata, buf);
}
}
struct gps_data_t* gpsmm::send(const char *request)
{
if (gps_send(gps_state(),request)==-1) {
return NULL;
}
else {
return backup();
}
}
/*
* Send a nak for the given packet type. Return 1 if the packet
* sent OK, othewise -1.
*/
int
gps_send_nak(gps_handle gps, u_char type)
{
u_char buf[4];
buf[0] = nak;
buf[1] = type;
buf[2] = 0;
return gps_send(gps, buf, 3);
}
int main(int argc, char *argv[])
{
struct gps_data_t collect;
struct fixsource_t source;
char buf[BUFSIZ];
int option;
bool batchmode = false;
bool forwardmode = false;
char *fmsg = NULL;
#ifdef CLIENTDEBUG_ENABLE
int debug = 0;
#endif
(void)signal(SIGSEGV, onsig);
(void)signal(SIGBUS, onsig);
while ((option = getopt(argc, argv, "bf:hsD:?")) != -1) {
switch (option) {
case 'b':
batchmode = true;
break;
case 'f':
forwardmode = true;
fmsg = optarg;
break;
case 's':
(void)
printf
("Sizes: fix=%zd gpsdata=%zd rtcm2=%zd rtcm3=%zd ais=%zd compass=%zd raw=%zd devices=%zd policy=%zd version=%zd, noise=%zd\n",
sizeof(struct gps_fix_t),
sizeof(struct gps_data_t), sizeof(struct rtcm2_t),
sizeof(struct rtcm3_t), sizeof(struct ais_t),
sizeof(struct attitude_t), sizeof(struct rawdata_t),
sizeof(collect.devices), sizeof(struct policy_t),
sizeof(struct version_t), sizeof(struct gst_t));
exit(EXIT_SUCCESS);
#ifdef CLIENTDEBUG_ENABLE
case 'D':
debug = atoi(optarg);
break;
#endif
case '?':
case 'h':
default:
(void)fputs("usage: test_libgps [-b] [-f fwdmsg] [-D lvl] [-s] [server[:port:[device]]]\n", stderr);
exit(EXIT_FAILURE);
}
}
/* Grok the server, port, and device. */
if (optind < argc) {
gpsd_source_spec(argv[optind], &source);
} else
gpsd_source_spec(NULL, &source);
#ifdef CLIENTDEBUG_ENABLE
gps_enable_debug(debug, stdout);
#endif
if (batchmode) {
#ifdef SOCKET_EXPORT_ENABLE
while (fgets(buf, sizeof(buf), stdin) != NULL) {
if (buf[0] == '{' || isalpha(buf[0])) {
gps_unpack(buf, &gpsdata);
libgps_dump_state(&gpsdata);
}
}
#endif
} else if (gps_open(source.server, source.port, &collect) != 0) {
(void)fprintf(stderr,
"test_libgps: no gpsd running or network error: %d, %s\n",
errno, gps_errstr(errno));
exit(EXIT_FAILURE);
} else if (forwardmode) {
(void)gps_send(&collect, fmsg);
(void)gps_read(&collect);
#ifdef SOCKET_EXPORT_ENABLE
libgps_dump_state(&collect);
#endif
(void)gps_close(&collect);
} else {
int tty = isatty(0);
if (tty)
(void)fputs("This is the gpsd exerciser.\n", stdout);
for (;;) {
if (tty)
(void)fputs("> ", stdout);
if (fgets(buf, sizeof(buf), stdin) == NULL) {
if (tty)
putchar('\n');
break;
}
collect.set = 0;
(void)gps_send(&collect, buf);
(void)gps_read(&collect);
#ifdef SOCKET_EXPORT_ENABLE
libgps_dump_state(&collect);
#endif
}
(void)gps_close(&collect);
}
return 0;
}
int main(int argc, char *argv[])
{
struct gps_data_t collect;
char buf[BUFSIZ];
int option;
bool batchmode = false;
int debug = 0;
(void)signal(SIGSEGV, onsig);
(void)signal(SIGBUS, onsig);
while ((option = getopt(argc, argv, "bhsD:?")) != -1) {
switch (option) {
case 'b':
batchmode = true;
break;
case 's':
(void)
printf
("Sizes: fix=%zd gpsdata=%zd rtcm2=%zd rtcm3=%zd ais=%zd compass=%zd raw=%zd devices=%zd policy=%zd version=%zd, noise=%zd\n",
sizeof(struct gps_fix_t),
sizeof(struct gps_data_t), sizeof(struct rtcm2_t),
sizeof(struct rtcm3_t), sizeof(struct ais_t),
sizeof(struct attitude_t), sizeof(struct rawdata_t),
sizeof(collect.devices), sizeof(struct policy_t),
sizeof(struct version_t), sizeof(struct gst_t));
exit(EXIT_SUCCESS);
case 'D':
debug = atoi(optarg);
break;
case '?':
case 'h':
default:
(void)fputs("usage: test_libgps [-b] [-D lvl] [-s]\n", stderr);
exit(EXIT_FAILURE);
}
}
gps_enable_debug(debug, stdout);
if (batchmode) {
while (fgets(buf, sizeof(buf), stdin) != NULL) {
if (buf[0] == '{' || isalpha(buf[0])) {
gps_unpack(buf, &gpsdata);
libgps_dump_state(&gpsdata);
}
}
} else if (gps_open(NULL, 0, &collect) <= 0) {
(void)fputs("Daemon is not running.\n", stdout);
exit(EXIT_FAILURE);
} else if (optind < argc) {
(void)strlcpy(buf, argv[optind], BUFSIZ);
(void)strlcat(buf, "\n", BUFSIZ);
(void)gps_send(&collect, buf);
(void)gps_read(&collect);
libgps_dump_state(&collect);
(void)gps_close(&collect);
} else {
int tty = isatty(0);
if (tty)
(void)fputs("This is the gpsd exerciser.\n", stdout);
for (;;) {
if (tty)
(void)fputs("> ", stdout);
if (fgets(buf, sizeof(buf), stdin) == NULL) {
if (tty)
putchar('\n');
break;
}
collect.set = 0;
(void)gps_send(&collect, buf);
(void)gps_read(&collect);
libgps_dump_state(&collect);
}
(void)gps_close(&collect);
}
return 0;
}
/**
* Thread reading from gpsd.
*/
static void * cgps_thread (void * pData)
{
struct gps_data_t gpsd_conn;
unsigned int err_count;
cgps_thread_running = CGPS_TRUE;
while (CGPS_TRUE)
{
pthread_mutex_lock (&cgps_thread_lock);
if (cgps_thread_shutdown == CGPS_TRUE)
{
goto quit;
}
pthread_mutex_unlock (&cgps_thread_lock);
err_count = 0;
#if GPSD_API_MAJOR_VERSION > 4
int status = gps_open (cgps_config_data.host, cgps_config_data.port, &gpsd_conn);
#else
int status = gps_open_r (cgps_config_data.host, cgps_config_data.port, &gpsd_conn);
#endif
if (status < 0)
{
WARNING ("gps plugin: connecting to %s:%s failed: %s",
cgps_config_data.host, cgps_config_data.port, gps_errstr (status));
// Here we make a pause until a new tentative to connect, we check also if
// the thread does not need to stop.
if (cgps_thread_pause(cgps_config_data.pause_connect) == CGPS_FALSE)
{
goto quit;
}
continue;
}
gps_stream (&gpsd_conn, WATCH_ENABLE | WATCH_JSON | WATCH_NEWSTYLE, NULL);
gps_send (&gpsd_conn, CGPS_CONFIG);
while (CGPS_TRUE)
{
pthread_mutex_lock (&cgps_thread_lock);
if (cgps_thread_shutdown == CGPS_TRUE)
{
goto stop;
}
pthread_mutex_unlock (&cgps_thread_lock);
#if GPSD_API_MAJOR_VERSION > 4
long timeout_us = CDTIME_T_TO_US (cgps_config_data.timeout);
if (!gps_waiting (&gpsd_conn, (int) timeout_us ))
#else
if (!gps_waiting (&gpsd_conn))
#endif
{
continue;
}
if (gps_read (&gpsd_conn) == -1)
{
WARNING ("gps plugin: incorrect data! (err_count: %d)", err_count);
err_count++;
if (err_count > CGPS_MAX_ERROR)
{
// Server is not responding ...
if (gps_send (&gpsd_conn, CGPS_CONFIG) == -1)
{
WARNING ("gps plugin: gpsd seems to be down, reconnecting");
gps_close (&gpsd_conn);
break;
}
// Server is responding ...
else
{
err_count = 0;
}
}
continue;
}
pthread_mutex_lock (&cgps_data_lock);
// Number of sats in view:
cgps_data.sats_used = (gauge_t) gpsd_conn.satellites_used;
cgps_data.sats_visible = (gauge_t) gpsd_conn.satellites_visible;
// dilution of precision:
cgps_data.vdop = NAN;
cgps_data.hdop = NAN;
if (cgps_data.sats_used > 0)
{
cgps_data.hdop = gpsd_conn.dop.hdop;
cgps_data.vdop = gpsd_conn.dop.vdop;
}
DEBUG ("gps plugin: %.0f sats used (of %.0f visible), hdop = %.3f, vdop = %.3f",
cgps_data.sats_used, cgps_data.sats_visible, cgps_data.hdop, cgps_data.vdop);
pthread_mutex_unlock (&cgps_data_lock);
}
}
stop:
DEBUG ("gps plugin: thread closing gpsd connection ... ");
gps_stream (&gpsd_conn, WATCH_DISABLE, NULL);
gps_close (&gpsd_conn);
quit:
DEBUG ("gps plugin: thread shutting down ... ");
cgps_thread_running = CGPS_FALSE;
pthread_mutex_unlock (&cgps_thread_lock);
pthread_exit (NULL);
}
int gps_stream(struct gps_data_t *gpsdata, unsigned int flags,
/*@null@*/ void *d)
/* ask gpsd to stream reports at you, hiding the command details */
{
char buf[GPS_JSON_COMMAND_MAX];
if ((flags & (WATCH_JSON | WATCH_OLDSTYLE | WATCH_NMEA | WATCH_RAW)) == 0) {
flags |= WATCH_JSON;
}
if ((flags & WATCH_DISABLE) != 0) {
if ((flags & WATCH_OLDSTYLE) != 0) {
(void)strlcpy(buf, "w-", sizeof(buf));
if ((flags & WATCH_NMEA) != 0)
(void)strlcat(buf, "r-", sizeof(buf));
} else {
(void)strlcpy(buf, "?WATCH={\"enable\":false,", sizeof(buf));
if (flags & WATCH_JSON)
(void)strlcat(buf, "\"json\":false,", sizeof(buf));
if (flags & WATCH_NMEA)
(void)strlcat(buf, "\"nmea\":false,", sizeof(buf));
if (flags & WATCH_RAW)
(void)strlcat(buf, "\"raw\":1,", sizeof(buf));
if (flags & WATCH_RARE)
(void)strlcat(buf, "\"raw\":0,", sizeof(buf));
if (flags & WATCH_SCALED)
(void)strlcat(buf, "\"scaled\":false,", sizeof(buf));
if (flags & WATCH_TIMING)
(void)strlcat(buf, "\"timing\":false,", sizeof(buf));
if (buf[strlen(buf) - 1] == ',')
buf[strlen(buf) - 1] = '\0';
(void)strlcat(buf, "};", sizeof(buf));
}
libgps_debug_trace((DEBUG_CALLS, "gps_stream() disable command: %s\n", buf));
return gps_send(gpsdata, buf);
} else { /* if ((flags & WATCH_ENABLE) != 0) */
if ((flags & WATCH_OLDSTYLE) != 0) {
(void)strlcpy(buf, "w+x", sizeof(buf));
if ((flags & WATCH_NMEA) != 0)
(void)strlcat(buf, "r+", sizeof(buf));
} else {
(void)strlcpy(buf, "?WATCH={\"enable\":true,", sizeof(buf));
if (flags & WATCH_JSON)
(void)strlcat(buf, "\"json\":true,", sizeof(buf));
if (flags & WATCH_NMEA)
(void)strlcat(buf, "\"nmea\":true,", sizeof(buf));
if (flags & WATCH_RARE)
(void)strlcat(buf, "\"raw\":1,", sizeof(buf));
if (flags & WATCH_RAW)
(void)strlcat(buf, "\"raw\":2,", sizeof(buf));
if (flags & WATCH_SCALED)
(void)strlcat(buf, "\"scaled\":true,", sizeof(buf));
if (flags & WATCH_TIMING)
(void)strlcat(buf, "\"timing\":true,", sizeof(buf));
/*@-nullpass@*//* shouldn't be needed, splint has a bug */
if (flags & WATCH_DEVICE)
(void)snprintf(buf + strlen(buf), sizeof(buf) - strlen(buf),
"\"device\":\"%s\",", (char *)d);
/*@+nullpass@*/
if (buf[strlen(buf) - 1] == ',')
buf[strlen(buf) - 1] = '\0';
(void)strlcat(buf, "};", sizeof(buf));
}
libgps_debug_trace((DEBUG_CALLS, "gps_stream() enable command: %s\n", buf));
return gps_send(gpsdata, buf);
}
}
/*
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;
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));
}