/* * gps-synth: synthesize GPS (and other NMEA) info. * * Usage: gps-synth [-speed BAUDRATE] SERIAL-PORT * * Runs with a user-interface command line, synthesizing GPS data out * the serial port. * * We know how to generate GGA, GLL, RMC, VTG, and ZDA sentences, and, * by default, generate all of them. They can be selectively enabled * and disabled from the command line. * * If the semimajor axis is a and the semiminor axis is b, the * flattening, f, is (a-b)/a. According to Wikipedia, for the WGS 84 * spheroid, a is 6378137.0, b is 6356752.3142, and 1/f is * 1/298.257223563. These cannot all three be correct; the wording * leads me to think that a and f are the defining values, and, * indeed, the b that I get from that assumption matches the above to * the limit of precision of the number given (it's * 6356752.3142451795-). * * However.... * * I don't know whether the latitude numbers expected here are geodetic * latitude (angle between equatorial plane and a normal to the * ellipsoid) or geocentric latitude (angle between equatorial plane * and a line from the centre of the ellipsoid). Since the flattening * is only about 1/300, and this is all fake data anyway, we ignore * the whole issue and treat the earth as a sphere with radius 6371km; * the IUGG defines the mean radius to be (2a+b)/3 in the above terms, * giving 6371.0087714km. The authalic radius (sphere with same * surface area as the Earth) is 6371.0072km; the volumetric radius * (sphere with same volume as the Earth) is 6371.0008km. So a 6371km * sphere is a reasonable approximation for our purposes. * * We operate in MKS units internally, with conversion to and from * other units (eg, between km/s and knots) happening at the * interfaces. Location is kept as a 3-space point on the surface of * a unit sphere (the globe scaled down by its radius), with heading a * 3-space vector which is always tangent to the globe at that point. * The redundancy here allows us to avoid internal hiccups at the * poles (not that we are likely to get close to the poles in most * cases). The coordinate system has its origin at the centre of the * earth sphere, its X axis through the point where the equator and * the Prime Meridian cross, and the Z axis through the North Pole. * This puts the Y axis through 0°N 90°E, which happens to be a spot a * bit ESE of Sri Lanka and west of Singapore - roughly 40% of the way * from Sri Lanka to Jakarta. We store latitude and longitude as * degrees north of the equator and east of the Prime Meridian. In * particular, this means that, ignoring units, longitude is * atan2(y,x). * * We can generate three kinds of motion: * * 1) No motion. We are stationary at a particular spot. * * 2) Circling. We are moving in circles at a fixed speed * about a fixed point. Note: the code actually circles * on the plane tangent to the earth at the circle centre, * projecting the resulting circle onto the globe. If the * circle radius is a significant fraction of the earth's * radius, this can make a visible difference. * * 3) Heading for a point. We are moving along a great * circle at a fixed speed towards a fixed point. When we * get close to the point, we slow down and stop, shifting * to mode (1). * * There are also various transition states - for example, if we are * moving north from 0°N 0°W and are told to start circling with a * 10km radius about 45°N 90°W, we will have to move quite a bit to * get there: we will turn left and take a great circle path that's * tangent to the specified circle. For code simplicity, we handle * transitions by decelerating to a stop, turning, and accelerating. * * There are some ambiguities possible. As a simple example, if we are * stationary at 0°N 90°W and are told to start circling centred at * 0°N 90°E, all directions are equally good to get there. Such * ambiguities are resolved however the code finds convenient. * * We generate whichever sentences are selected at whatever interval is * selected at whatever baud rate is selected. By default, all * sentences are selected, the interval is one second, and the baud * rate is 4800. If these settings are such that it takes longer than * the interval to send one interval's sentences, we thin the * intervals as necessary, effectively multiplying the interval by an * integer to make time (we print a message when we do this). We add * a slop factor of 10-15 percent to compensate for serial port clocks * not being quite exact. * * On startup, values are set for being stationary at 45°N 45°W. */ #include #include #include #include #include #include #include #include #include #include #include #include #include extern const char *__progname; #define SECUSEC 1000000 /* * A knot is defined as 1.852 km/hr. Since we operate in MKS (when we * use units at all), we want the value in m/sec: * * km 1 hr 1000 m * 1.852 -- * -------- * ------ * hr 3600 sec 1 km */ #define KNOT (1.852 / 3.6) /* * The earth does not have a single well-defined radius - see the file * header comment. But we approximate it by an exact sphere; see the * file header comment again. This is the radius of that sphere, in * metres. */ #define RAD 6371000 /* * A distance of 1m on the earth is a distance of 1/RAD at the * unit-sphere scale, because RAD is in metres. (1.0 rather than 1 in * case RAD is syntactically integral instead of floating.) It's a * curious coincidence that this number is close to one ULP for an * IEEE single-float with value 1 - and that's one reason we work with * doubles instead of floats. */ #define ONE_METRE (1.0 / RAD) /* * cos(89°). This is used to ensure that, when working with circle * motion, various things are on the same hemisphere as one another. * (This removes the need for various special cases, and doesn't * impact the designed-for uses of circle motion.) * * This value doesn't have to be exact. Anything from about cos(80°) * to cos(90°-epsilon) would probably do. All it affects is how much * of the globe counts as "in the same hemisphere" when working with * circle paths. */ #define COS89 .01745240632728359 typedef struct pt3 PT3; typedef struct ckfstate CKFSTATE; typedef struct latlon LATLON; typedef struct stype STYPE; typedef struct motion MOTION; typedef struct oqfp OQFP; struct oqfp { AIO_OQ *q; } ; struct motion { void (*move)(void); void (*show)(FILE *); } ; #define MOTION_INIT(name) { &move_##name, &show_##name } struct latlon { double lat; double lon; } ; struct ckfstate { FILE *inner; unsigned char cksum; } ; struct pt3 { double x; double y; double z; } ; #define PT3_PX ((PT3){ 1,0,0}) #define PT3_MX ((PT3){-1,0,0}) #define PT3_PY ((PT3){0, 1,0}) #define PT3_MY ((PT3){0,-1,0}) #define PT3_PZ ((PT3){0,0, 1}) #define PT3_MZ ((PT3){0,0,-1}) struct stype { const char * const name; void (*const gen)(FILE *); unsigned int namelen; unsigned int bit; } ; static STYPE stypes[]; // forward static const char *ser_path; static int ser_fd; static unsigned int enable; static AIO_OQ ser_oq; static int ser_baud = 4800; static AIO_OQ stdout_oq; static int timer_fd; static double interval; static int thin; static int thincount; static int prompted; static PT3 at; static PT3 dir; static double speedset; static double speedcur; static double hdg; static int nohdg = 1; static MOTION *motion; // forwards static MOTION motion_still; static MOTION motion_headfor; static MOTION motion_circle; /* * circle_rad actually applies on the plane tangent to the earth at * circle_ctr, which means the value after projection back onto the * earth is lower. We assume the circles are small enough that the * difference doesn't matter in practice. * * circle_ctr and circle_rad are both in unit-sphere units, ie, scaled * so the earth is a unit sphere. */ static PT3 circle_ctr; static double circle_rad; static int circle_cw; // 1 for CW, 0 for CCW static PT3 head_pt; static FILE *outf; static char *outb; static int outl; static unsigned int genhave; #define GH_TIME 0x00000001 #define GH_LAT 0x00000002 #define GH_LON 0x00000004 #define GH_HDG 0x00000008 static char gen_hhmmss[7]; static char gen_dd[3]; static char gen_mm[3]; static char gen_yyyy[5]; static char gen_ddmmyy[7]; static char gen_lat_text[10]; static char gen_lat_ns; static char gen_lon_text[11]; static char gen_lon_ew; static char gen_hdg[16]; #define sindeg(x) sin((x)*(M_PI/180)) #define cosdeg(x) cos((x)*(M_PI/180)) #define asindeg(x) (asin((x))*(180/M_PI)) #define acosdeg(x) (acos((x))*(180/M_PI)) #define atan2deg(y,x) (atan2((y),(x))*(180/M_PI)) #define Cisspace(x) isspace((unsigned char)(x)) static int oq_f_w(void *pv, const char *data, int len) { OQFP *p; p = pv; aio_oq_queue_copy(p->q,data,len); return(len); } static FILE *open_oq(AIO_OQ *q) { OQFP *p; FILE *f; p = malloc(sizeof(OQFP)); if (! p) return(0); f = funopen(p,0,&oq_f_w,0,0); if (! f) { free(p); return(0); } p->q = q; return(f); } /* * We can't use the sqrt trick for x and y because they have to support * the full two-pi range of angles. We can get away with it for zf * only because the range of latitude is -90 to 90, so the cosine is * always positive. But x and y each have to be able to go negative. * (I suppose we could do sqrt and then negate it conditionally based * on the argument. But that's getting into pipeline flushes and the * like, quite possibly getting slower than just doing the trig - * especially if the FPU in use has direct trig support, which, * looking at /usr/src/lib/libm/arch/i387, looks probable.) */ static PT3 loc_ll(LATLON ll) { double zf; double x; double y; double z; z = sindeg(ll.lat); zf = sqrt(1-(z*z)); // aka cosdeg(ll.lat) x = cosdeg(ll.lon); y = sindeg(ll.lon); return((PT3){.x = x * zf, .y = y * zf, .z = z}); } /* * | i j k | * a × b = | ax ay az | * | bx by bz | * = (ay bz - az by) i + (az bx - ax bz) j + (ax by - ay bx) k */ static PT3 cross(PT3 a, PT3 b) { return((PT3) { .x = (a.y * b.z) - (a.z * b.y), .y = (a.z * b.x) - (a.x * b.z), .z = (a.x * b.y) - (a.y * b.x) }); } static double norm3(PT3 v) { return(sqrt((v.x*v.x)+(v.y*v.y)+(v.z*v.z))); } static PT3 scale3(PT3 v, double s) { return((PT3) { .x = s * v.x, .y = s * v.y, .z = s * v.z }); } static PT3 unit3(PT3 v) { return(scale3(v,1/norm3(v))); } static double dot3(PT3 a, PT3 b) { return((a.x*b.x)+(a.y*b.y)+(a.z*b.z)); } static PT3 add3(PT3 a, PT3 b) { return((PT3){.x=a.x+b.x,.y=a.y+b.y,.z=a.z+b.z}); } static PT3 sub3(PT3 a, PT3 b) { return((PT3){.x=a.x-b.x,.y=a.y-b.y,.z=a.z-b.z}); } /* * To rotate vector X by angle A around vector Y, conceptually, * * (1) Y^ = Y / |Y| * (2) parX = (X · Y) Y^ * (3) perpX = X - parX * (4) pX^ = perpX / |perpX| * (5) Z^ = pX^ × Y^ * (6) rotperp^ = cos(A) pX^ + sin(A) Z^ * (7) rotperp = |perpX| rotperp^ * (8) rotX = parX + rotperp * * But, noting that steps (4) and (5) are linear, in that * multiplication by a scalar constant passes through unchanged, we * can transform these into * * (5') Z^ = (perpX × Y^) / |perpX| * (5'') Z = perpX × Y^ = Z^ |perpX| * (6') rotperp^ |perpX| = cos(A) pX^ |perpX| + sin(A) Z^ |perpX| * = cos(A) perpX + sin(A) Z * * and then collapse 6' with 7, leading to * * (1) Y^ = Y / |Y| * (2) parX = (X · Y) Y^ * (3) perpX = X - parX * (5'') Z = perpX × Y^ * (7') rotperp = cos(A) perpX + sin(A) Z * (8) rotX = parX + rotperp * * which is, usefully, numerically stable even when X is parallel to * Y, or close to. The only division involved is the one in line (1), * and, for our purposes, that can go away because we present an API * requiring that Y be a unit vector on input. We actually collapse * 5'', 7', and 8 into a single line, since Z and rotperp are each * used only once: * * (9) rotX = parX + (cos(A) perpX + sin(A) (perpX × Y^)) * * We assume axis is a unit vector. ang is in radians. * * A positive angle rotates counterclockwise when facing in the same * direction as axis. For example, calling with vec=(0,0,1), * axis=(1,0,0), ang=pi/4 (45°) returns (0,sqrt(2),sqrt(2)). */ static PT3 rot3(PT3 vec, PT3 axis, double ang) { PT3 par; PT3 perp; par = scale3(axis,dot3(vec,axis)); perp = sub3(vec,par); return(add3(par,add3(scale3(perp,cos(ang)),scale3(cross(perp,axis),sin(ang))))); } static int wtest_sero(void *arg __attribute__((__unused__))) { return(aio_oq_nonempty(&ser_oq)); } static int wtest_stdout(void *arg __attribute__((__unused__))) { return(aio_oq_nonempty(&stdout_oq)); } static void ser_w(void *arg __attribute__((__unused__))) { int n; n = aio_oq_writev(&ser_oq,ser_fd,-1); if (n < 0) { if (n == AIO_WRITEV_ERROR) { fprintf(stderr,"%s: write to %s: %s\n",__progname,ser_path,strerror(errno)); exit(1); } fprintf(stderr,"%s: impossible aio_oq_writev return %d\n",__progname,n); exit(1); } aio_oq_dropdata(&ser_oq,n); } static void stdout_w(void *arg __attribute__((__unused__))) { int n; n = aio_oq_writev(&stdout_oq,1,-1); if (n < 0) { if (n == AIO_WRITEV_ERROR) { fprintf(stderr,"%s: write to standard output: %s\n",__progname,strerror(errno)); exit(1); } fprintf(stderr,"%s: impossible aio_oq_writev return %d\n",__progname,n); exit(1); } aio_oq_dropdata(&stdout_oq,n); } static void set_interval(void) { struct itimerspec its; int s; int us; s = interval; us = (interval - s) * SECUSEC; if (us < 0) { us += SECUSEC; s --; } if (us >= SECUSEC) { us -= SECUSEC; s ++; } if ((us < 0) || (us >= SECUSEC) || (s < 0)) abort(); its.it_interval.tv_sec = s; its.it_interval.tv_nsec = us * 1000; its.it_value = its.it_interval; timerfd_settime(timer_fd,0,&its,0); thin = 1; thincount = 0; } static int out_w(void *cookie __attribute__((__unused__)), const char *buf, int len) { outb = realloc(outb,outl+len); bcopy(buf,outb+outl,len); outl += len; return(len); } static int cksum_w(void *sv, const char *buf, int len) { CKFSTATE *s; int i; s = sv; for (i=0;icksum ^= (unsigned char)buf[i]; fwrite(buf,1,len,s->inner); return(len); } static int cksum_c(void *sv) { CKFSTATE *s; s = sv; fprintf(s->inner,"*%02X",s->cksum); free(s); return(0); } static FILE *fopen_cksum(FILE *inner) { CKFSTATE *s; FILE *f; s = malloc(sizeof(CKFSTATE)); if (! s) return(0); f = funopen(s,0,&cksum_w,0,&cksum_c); if (! f) { free(s); return(0); } s->inner = inner; s->cksum = 0; return(f); } static void gen_get_time(void) { if (! (genhave & GH_TIME)) { time_t t; struct tm *tm; t = time(0); tm = gmtime(&t); sprintf(&gen_hhmmss[0],"%02d%02d%02d",tm->tm_hour,tm->tm_min,tm->tm_sec); sprintf(&gen_dd[0],"%02d",tm->tm_mday); sprintf(&gen_mm[0],"%02d",tm->tm_mon+1); sprintf(&gen_yyyy[0],"%04d",tm->tm_year+1900); sprintf(&gen_ddmmyy[0],"%02d%02d%.2s",tm->tm_mday,tm->tm_mon+1,&gen_yyyy[2]); genhave |= GH_TIME; } } static void gen_get_hdg(void) { if (! (genhave & GH_HDG)) { if (nohdg) { gen_hdg[0] = '\0'; } else { sprintf(&gen_hdg[0],"%.2f",hdg); } genhave |= GH_HDG; } } static const char *g_hhmmss(void) { gen_get_time(); return(&gen_hhmmss[0]); } static const char *g_dd(void) { gen_get_time(); return(&gen_dd[0]); } static const char *g_mm(void) { gen_get_time(); return(&gen_mm[0]); } static const char *g_yyyy(void) { gen_get_time(); return(&gen_yyyy[0]); } static const char *g_ddmmyy(void) { gen_get_time(); return(&gen_ddmmyy[0]); } static const char *g_hdg(void) { gen_get_hdg(); return(&gen_hdg[0]); } static LATLON pt3_to_ll(PT3 p) { p = unit3(p); return((LATLON){.lat = asindeg(p.z), .lon = atan2deg(p.y,p.x)}); } static void setup_latlon(char *buf, int dd, char *chp, double a, char chplus, char chminus) { int deg; int minxxxx; if (a < 0) { *chp = chminus; a = - a; } else { *chp = chplus; } deg = a; minxxxx = ((a - deg) * 60 * 10000) + .5; if (minxxxx < 0) { minxxxx += 60 * 10000; deg --; } if (minxxxx >= 60*10000) { minxxxx -= 60 * 10000; deg ++; } if ((minxxxx < 0) || (minxxxx >= 60*10000)) abort(); sprintf(buf,"%0*d%02d.%04d",dd,deg,minxxxx/10000,minxxxx%10000); } static void gen_get_lat(void) { setup_latlon(&gen_lat_text[0],2,&gen_lat_ns,asindeg(at.z),'N','S'); genhave |= GH_LAT; } static void gen_get_lon(void) { setup_latlon(&gen_lon_text[0],3,&gen_lon_ew,atan2deg(at.y,at.x),'E','W'); genhave |= GH_LON; } static const char *g_lat(void) { if (! (genhave & GH_LAT)) { gen_get_lat(); } return(&gen_lat_text[0]); } static char g_lat_ns(void) { if (! (genhave & GH_LAT)) { gen_get_lat(); } return(gen_lat_ns); } static const char *g_lon(void) { if (! (genhave & GH_LON)) { gen_get_lon(); } return(&gen_lon_text[0]); } static char g_lon_ew(void) { if (! (genhave & GH_LON)) { gen_get_lon(); } return(gen_lon_ew); } /* * According to Wikipedia, the NMEA spec is pay-to-play - presumably * they'd rather have people implementing based on reverse engineering * and guesses. * * Messages, for our purposes, begin with $ (can also be !, for * messages with special encapsulation). Then two bytes identify the * talker and three the type of message. Commas separate fields. *XX * checksum may be appended "but is compulsory for RMA, RMB, and RMC * (among others)". * * Message types, according to a jmu.edu webpage, include: * * AAM Waypoint arrival alarm * APA Autopilot format A * APB Autopilot format B * BOD Bearing, origin to destination * BWC Bearing and distance to waypoint, great circle * BWR Bearing and distance to waypoint, rhumb line * DBT Depth below transducer * DPT Depth of water * GGA GPS fix data * GLL Geographic position, latitude/longitude (and time) * GSA GPS DOP and active satellites * GSV Satellites in view * HDM Heading, magnetic north * HDT Heading, true north * HSC Steer to heading * MTW Mean water temperature * RMA Recommended minimum specific Loran-C data * RMB Recommended minimum navigation info * R00 List of waypoints in active route * RMC Recommended minimum specific GPS/Transit data * RTE Routes * STN Multiple data ID * TRF Transit fix data * VBW Dual ground/water speed * VTG Track made good and ground speed * WCV Waypoint closure velocity * WPL Waypoint location * XTE Cross-track error * XTR Cross-track error, dead reckoning * ZDA Date & time * * Talker fields beginning P indicate vendor-proprietary sentences. * * GGA looks like * * $GPGGA,131416,4513.9648,N,07507.9376,E,1,10,1.6,5,M,37,M,,*4B * * where the pieces are * * $GPGGA, Header * 131416, UTC time, hhmmss (optional .ff hundredths) * 4513.9648,N, Latitude (DDmm.mmmm) * 07507.9376,E, Longitude (DDDmm.mmmm) * 1, Quality * 10, # satellites in use * 1.6, Horizontal dilution of precision * 5, Antenna altitude above/below mean sea level * M, Units of previous field (M = metres) * 37, Undulation (mean sea level vs WGS84 ellipsoid) * M, Units of previous field (M = metres) * , Age of correction data, seconds, max 99 * (empty = no differential data) * Differential base station ID * (empty = no differential data) * *4B Checksum * * Quality values: * * One reference: * 0 No fix * 1 GPS fix * 2 DGPS fix * Another reference: * 0 Fix unavailable or invalid * 1 Single point * Converging PPP (TerraStar-L) * 2 Pseudorange differential * Converged PPP (TerraStar-L) * Converging PPP (TerraStar-C, -C PRO, or -X) * 4 RTK fixed ambiguity solution * 5 RTK floating ambiguity solution * Converted PPP (TerraStar-C, -C PRO, or -X) * 6 Dead reckoning mode * 7 Manual input mode, fixed position * 8 Simulator mode * 9 WAAS (SBAS) * * GLL looks like * * $GPGLL,4513.9648,N,07507.9376,E,131416,A*24 * * where the pieces are * * $GPGLL, Header * 4513.9648,N, Latitude (DDmm.mmmm) * 07507.9376,E, Longitude (DDDmm.mmmm) * 131416, UTC time (hhmmss) * A Valid * *24 Checksum * * RMC looks like * * $GPRMC,131416,A,4513.9648,N,07507.9376,E,10.0,2.9,081008,15,E*69 * * where the pieces are * * $GPRMC, Header * 131416, UTC time, hhmmss * A, Position status (A valid, V invalid) * 4513.9648,N, Latitude (DDmm.mmmm) * 07507.9376,E, Longitude (DDmm.mmmm) * 10.0, Speed (knots) * 2.9, Heading (units?) * 081008, Date (DDMMYY) * 15, Magnetic variation * E Magnetic variation direction * *69 Checksum * * VTG looks like * * $GPVTG,,T,347.9,M,10.0,N,18.5,K*64 * * where the pieces are * * $GPVTG, Header * ,T, True track made good (degrees) * 347.9,M, Magnetic track made good * 10.0,N, Ground speed, knots * 18.5,K Ground speed, km/hr * *64 Checksum * * ZDA looks like * * $GPZDA,131416,08,10,2008,0,0*4B * * where the pieces are * * $GPZDA, Header * 131416, UTC time, hhmmss (optional .ff hundredths) * 08, Day-of-month (01-31) * 10, Month (01-12) * 2008, Year (with century) * 0, Local zone, hours (00 to ±13) * 0 Local zone, minutes (same sign as hours) * *4B Checksum */ static void gen_GGA(FILE *f) { fprintf(f,"GPGGA,%s,%s,%c,%s,%c,1,10,1.6,5,M,37,M,,",g_hhmmss(),g_lat(),g_lat_ns(),g_lon(),g_lon_ew()); } static void gen_GLL(FILE *f) { fprintf(f,"GPGLL,%s,%c,%s,%c,%s,A",g_lat(),g_lat_ns(),g_lon(),g_lon_ew(),g_hhmmss()); } static void gen_RMC(FILE *f) { fprintf(f,"GPRMC,%s,A,%s,%c,%s,%c,%.1f,%s,%s,,",g_hhmmss(),g_lat(),g_lat_ns(),g_lon(),g_lon_ew(),speedcur,g_hdg(),g_ddmmyy()); } static void gen_VTG(FILE *f) { fprintf(f,"GPVTG,,T,,M,%.1f,N,%.1f,K",speedcur,speedcur*(KNOT*3.6)); } static void gen_ZDA(FILE *f) { fprintf(f,"GPZDA,%s,%s,%s,%s,0,0",g_hhmmss(),g_dd(),g_mm(),g_yyyy()); } static STYPE stypes[] = { { "GGA", &gen_GGA }, { "GLL", &gen_GLL }, { "RMC", &gen_RMC }, { "VTG", &gen_VTG }, { "ZDA", &gen_ZDA }, { 0 } }; static void gen(void (*fn)(FILE *)) { FILE *ckf; putc('$',outf); ckf = fopen_cksum(outf); (*fn)(ckf); fclose(ckf); fprintf(outf,"\r\n"); } static void gen_sentences(void) { double s; int tf; int i; outb = 0; outl = 0; outf = fwopen(0,&out_w); genhave = 0; for (i=0;stypes[i].name;i++) if (enable & stypes[i].bit) gen(stypes[i].gen); fclose(outf); /* * Check to see if we're generating too much for the current baudrate * and interval. We assume 8-bit data bytes with two framing bits * (one start, one stop), for a total of 10 bit times per output * byte. ser_baud is bits per second, so with conversion factors and * units (but not explicitly representing the 10% factor), we have * (BR = "baud rate", ser_baud) * 10 bits 1 second * bytes * ------- * -------- = seconds * 1 byte BR bits * There is some idle margin; see the comments below. */ s = (outl * 10) / (double)ser_baud; if (s*1.1 > interval) { /* * If s+10% is more than interval, compute the thinning factor. * * We want the smallest integer i such that s+10% > i*interval. * Since both interval and s are positive, this is * ceil(s*1.1/interval). */ tf = ceil(s*1.1/interval); /* * If this is more than the current thinning factor, push it up. * We want to pull the thinning factor back down when we can, but * we want to avoid stuttering when we're right on the edge and * plus or minus a byte pushes the factor up or down. So, * instead, we say, if the thinning factor resulting from 15% idle * time, instead of 10%, is still less than the current factor, * then decrement it. We know that s*1.1 > interval, so s*1.15 * is, if anything, even more > interval, so pushing thin below * one is a can't-happen. */ if (tf > thin) thin = tf; if (ceil(s*1.15/interval) < thin) { thin --; if (thin < 0) abort(); } } else if (s*1.15 <= interval) { /* * If s+10% is <= interval, we should be fine with thin set bto 1. * To avoid stuttering, as sketched above, we actually use s+15% * instead, which is why 1.15 instead of 1.1 in the test above. */ thin = 1; } aio_oq_queue_free(&ser_oq,outb,outl); } static void move_still(void) { speedcur = 0; } /* * This approximation assumes speeds are low, or more precisely that * each step is short enough that "the earth is locally flat" is a * workable approximation. */ static void move_headfor(void) { double d; double sf; PT3 delta; if (speedset < 1e-6) { speedset = 0; speedcur = 0; return; } sf = (speedset * interval * KNOT) / RAD; d = norm3(sub3(at,head_pt)); if (d < sf) { motion = &motion_still; at = head_pt; speedcur = 0; return; } at = unit3(add3(at,scale3(dir,(speedset*interval*KNOT)/RAD))); delta = unit3(sub3(head_pt,at)); dir = unit3(sub3(delta,scale3(at,dot3(delta,at)))); } /* * See the comment on circle_ctr and circle_rad, above. We project out * to the tangent plane, advance around the circle based on the speed * figure, then project back again. In a sense, we ought to scale the * speed by the ratio between the tangent-plane radius and the * three-space radius, but circles large enough that the difference is * significant are well outside the design-for use cases. */ static void move_circle(void) { double d; PT3 t_at; PT3 t_rad; double r; d = dot3(at,circle_ctr); if (d < COS89) { // Can this happen? I can't see how. aio_oq_queue_copy(&stdout_oq,"Centre too far away\n",AIO_STRLEN); return; } t_at = scale3(at,1/d); t_rad = sub3(t_at,circle_ctr); r = norm3(t_rad); if (fabs(r-circle_rad) > ONE_METRE) { if (r < ONE_METRE) { // We're too close to the centre to have a well-defined // direction. Pick one. if ((fabs(circle_ctr.z) < fabs(circle_ctr.x)) && (fabs(circle_ctr.z) < fabs(circle_ctr.y))) { t_rad = PT3_PZ; } else if (fabs(circle_ctr.y) < fabs(circle_ctr.x)) { t_rad = PT3_PY; } else { t_rad = PT3_PX; } t_rad = sub3(t_rad,scale3(circle_ctr,dot3(t_rad,circle_ctr))); } head_pt = unit3(add3(scale3(t_rad,circle_rad/norm3(t_rad)),circle_ctr)); move_headfor(); // and, in case move_headfor() set motion_still.... motion = &motion_circle; speedcur = speedset; } else { t_rad = rot3(t_rad,circle_ctr,(speedset*interval*KNOT*(circle_cw?1:-1))/(circle_rad*RAD)); t_rad = scale3(t_rad,circle_rad/norm3(t_rad)); at = unit3(add3(circle_ctr,t_rad)); } } static void show_still(FILE *f) { fprintf(f,"Still\n"); } static void show_headfor(FILE *f) { LATLON ll; double d; ll = pt3_to_ll(head_pt); fprintf(f,"Heading for %f%c %f%c",fabs(ll.lat),(ll.lat<0)?'S':'N',fabs(ll.lon),(ll.lon<0)?'W':'E'); d = norm3(sub3(head_pt,at)); if (d >= 10000) { fprintf(f," (%gkm",d/1000); } else { fprintf(f," (%gm",d); } fprintf(f," chord)\n"); } /* * See the comment on circle_ctr and circle_rad, above. We project out * to the tangent plane, advance around the circle based on the speed * figure, then project back again. In a sense, we ought to scale the * speed by the ratio between the tangent-plane radius and the * three-space radius, but circles large enough that the difference is * significant are well outside the design-for use cases. */ static void show_circle(FILE *f) { LATLON ll; ll = pt3_to_ll(circle_ctr); fprintf(f,"Circling %f%c %f%c, radius ",fabs(ll.lat),(ll.lat<0)?'S':'N',fabs(ll.lon),(ll.lon<0)?'W':'E'); if (circle_rad >= 10000) { fprintf(f,"%gkm",(circle_rad*RAD)/1000); } else { fprintf(f,"%gm",circle_rad*RAD); } fprintf(f,", %s\n",circle_cw?"clockwise":"counter-clockwise"); } static MOTION motion_still = MOTION_INIT(still); static MOTION motion_headfor = MOTION_INIT(headfor); static MOTION motion_circle = MOTION_INIT(circle); static void move_step(void) { PT3 pre; PT3 d; double n; PT3 north; PT3 east; pre = at; speedcur = speedset; (*motion->move)(); d = sub3(at,pre); if (norm3(d) < ONE_METRE/100) { // stationary - don't change hdg or nohdg } else if (hypot(at.x,at.y) < ONE_METRE) { // at N or S pole - set nohdg nohdg = 1; } else { d = sub3(d,scale3(at,dot3(at,d))); n = norm3(d); if (n < ONE_METRE/100) { // blinked between antipodes - set nohdg nohdg = 1; } else { d = scale3(d,1/n); north = unit3(sub3(PT3_PZ,scale3(at,dot3(at,PT3_PZ)))); east = unit3(cross(north,at)); hdg = atan2deg(dot3(d,east),dot3(d,north)); if (hdg < 0) hdg += 360; nohdg = 0; } } } static void move_steps(int n) { for (;n>0;n--) move_step(); } static void timer_events(int n) { move_steps(n); thincount += n; if (thincount < thin) return; thincount = 0; gen_sentences(); } static void timer_r(void *arg __attribute__((__unused__))) { uint64_t n; int nb; nb = read(timer_fd,&n,sizeof(n)); if (nb < 0) { if ((errno == EINTR) || (errno == EWOULDBLOCK)) return; fprintf(stderr,"%s: read from timer socket: %s\n",__progname,strerror(errno)); exit(1); } if (nb != sizeof(n)) { fprintf(stderr,"%s: read from timer socket: returned %d, expected %d\n",__progname,nb,(int)sizeof(n)); exit(1); } timer_events(n); } static int show_prompt(void *arg __attribute__((__unused__))) { if (prompted) return(AIO_BLOCK_NIL); aio_oq_queue_point(&stdout_oq,"> ",2); prompted = 1; return(AIO_BLOCK_LOOP); } static int scan_ll(const char *s0, const char **epp, LATLON *ll) { int havelat; int havelon; int n; int v[7]; char c[8]; double d; const char *s; havelat = 0; havelon = 0; s = s0; n = -1; sscanf(s," %5s%n",&c[0],&n); if ((n >= 0) && !strcasecmp(&c[0],"here")) { *ll = pt3_to_ll(at); *epp = s + n; return(0); } while (1) { if (havelat && havelon) break; n = -1; sscanf(s," %*c%n",&n); if (n < 0) break; do { /* * We don't use floating-point scanf formats for the decimal * fractions because the numbers can be followed by an E that * is not part of the number but would likely be mistaken for * an exponent marker - and because we don't want to accept * exponential notation in any case. But scanf is very * aggressive at skipping whitespace, which is why we scan the * dot in decimal fractions with "%n . %n" - so we can check * there's no whitespace around it. */ n = -1; // degrees, minutes, decimal seconds, with °, ', and " sscanf(s,"%d°%d'%d%n . %n%d%n\" %1[nNsSeEwW]%n", // 0 1 2 3 4 5 6 c n &v[0],&v[1],&v[2],&v[3],&v[4],&v[5],&v[6],&c[0],&n); if ( (n >= 0) && (v[3]+1 == v[4]) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) && (v[2] >= 0) && (v[2] <= 60) ) { d = v[0] + (v[1] / 60.0) + ((v[2] + (v[5] * pow(10,v[4]-v[6]))) / 3600.0); break; } n = -1; // degrees, minutes, integer seconds, with °, ', and " sscanf(s,"%d°%d'%d\" %1[nNsSeEwW]%n", // 0 1 2 c n &v[0],&v[1],&v[2],&c[0],&n); if ( (n >= 0) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) && (v[2] >= 0) && (v[2] <= 60) ) { d = v[0] + (v[1] / 60.0) + (v[2] / 3600.0); break; } n = -1; // degrees, minutes, decimal seconds, with ° and ' but no " sscanf(s,"%d°%d'%d%n . %n%d%n %1[nNsSeEwW]%n", // 0 1 2 3 4 5 6 c n &v[0],&v[1],&v[2],&v[3],&v[4],&v[5],&v[6],&c[0],&n); if ( (n >= 0) && (v[3]+1 == v[4]) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) && (v[2] >= 0) && (v[2] <= 60) ) { d = v[0] + (v[1] / 60.0) + ((v[2] + (v[5] * pow(10,v[4]-v[6]))) / 3600.0); break; } n = -1; // degrees, minutes, integer seconds, with ° and ', but no " sscanf(s,"%d°%d'%d %1[nNsSeEwW]%n", // 0 1 2 c n &v[0],&v[1],&v[2],&c[0],&n); if ( (n >= 0) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) && (v[2] >= 0) && (v[2] <= 60) ) { d = v[0] + (v[1] / 60.0) + (v[2] / 3600.0); break; } n = -1; // degrees and decimal minutes, with ° and ' sscanf(s,"%d°%d%n . %n%d%n' %1[nNsSeEwW]%n", // 0 1 2 3 4 5 c n &v[0],&v[1],&v[2],&v[3],&v[4],&v[5],&c[0],&n); if ( (n >= 0) && (v[2]+1 == v[3]) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) ) { d = v[0] + ((v[1] + (v[4] * pow(10,v[3]-v[5]))) / 60.0); break; } n = -1; // degrees and integer minutes, with ° and ' sscanf(s,"%d°%d' %1[nNsSeEwW]%n", // 0 1 c n &v[0],&v[1],&c[0],&n); if ( (n >= 0) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) ) { d = v[0] + (v[1] / 60.0); break; } n = -1; // degrees and decimal minutes, with ° but no ' sscanf(s,"%d°%d%n . %n%d%n %1[nNsSeEwW]%n", // 0 1 2 3 4 5 c n &v[0],&v[1],&v[2],&v[3],&v[4],&v[5],&c[0],&n); if ( (n >= 0) && (v[2]+1 == v[3]) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) ) { d = v[0] + ((v[1] + (v[4] * pow(10,v[3]-v[5]))) / 60.0); break; } n = -1; // degrees and integer minutes, with ° but no ' sscanf(s,"%d°%d %1[nNsSeEwW]%n", // 0 1 c n &v[0],&v[1],&c[0],&n); if ( (n >= 0) && (v[0] >= 0) && (v[0] <= 180) && (v[1] >= 0) && (v[1] <= 60) ) { d = v[0] + (v[1] / 60.0); break; } n = -1; // decimal degrees, with ° sscanf(s,"%d%n . %n%d%n° %1[nNsSeEwW]%n", // 0 1 2 3 4 c n &v[0],&v[1],&v[2],&v[3],&v[4],&c[0],&n); if ( (n >= 0) && (v[1]+1 == v[2]) && (v[0] >= 0) && (v[0] <= 180) ) { d = v[0] + (v[3] * pow(10,v[2]-v[4])); break; } n = -1; // decimal degrees, with no ° sscanf(s,"%d%n . %n%d%n %1[nNsSeEwW]%n", // 0 1 2 3 4 c n &v[0],&v[1],&v[2],&v[3],&v[4],&c[0],&n); if ((n >= 0) && (v[1]+1 == v[2])) { d = v[0] + (v[3] * pow(10,v[2]-v[4])); break; } n = -1; // integer degrees, with ° sscanf(s,"%d° %1[nNsSeEwW]%n", // 0 c n &v[0],&c[0],&n); if (n >= 0) { d = v[0]; break; } n = -1; // integer degrees, no ° sscanf(s,"%d %1[nNsSeEwW]%n", // 0 c n &v[0],&c[0],&n); if (n >= 0) { d = v[0]; break; } // None of the above? Fail. aio_oq_queue_printf(&stdout_oq,"Can't parse string, at: %s\n",s); return(1); } while (0); switch (c[0]) { case 's': case 'S': d = - d; // fall through case 'n': case 'N': if (havelat) { aio_oq_queue_point(&stdout_oq,"Latitude specified multiple times\n",AIO_STRLEN); return(1); } ll->lat = d; havelat = 1; break; case 'w': case 'W': d = - d; // fall through case 'e': case 'E': if (havelon) { aio_oq_queue_point(&stdout_oq,"Longitude specified multiple times\n",AIO_STRLEN); return(1); } ll->lon = d; havelon = 1; break; } s += n; } if (! havelat) { if (! havelon) { aio_oq_queue_point(&stdout_oq,"No coordinates specified\n",AIO_STRLEN); } else { aio_oq_queue_point(&stdout_oq,"No latitude specified\n",AIO_STRLEN); } return(1); } if (! havelon) { aio_oq_queue_point(&stdout_oq,"No longitude specified\n",AIO_STRLEN); return(1); } *epp = s; return(0); } static void cmd_quit(void) { exit(0); } static void cmd_tick(const char *arg) { double v; v = atof(arg); if ((v < .01) || (v > 3600)) { aio_oq_queue_printf(&stdout_oq,"Unreasonable tick interval %g\n",v); return; } interval = v; set_interval(); } static void cmd_speed(const char *arg) { double v; v = atof(arg); if ((v < 0) || (v > 1000000)) { aio_oq_queue_printf(&stdout_oq,"Unreasonable speed %g\n",v); return; } speedset = v; aio_oq_queue_printf(&stdout_oq,"Speed %gkt, ",speedset); v *= KNOT; if (v > 10000) { aio_oq_queue_printf(&stdout_oq,"%gkm/s\n",v/1000); } else { aio_oq_queue_printf(&stdout_oq,"%gm/s\n",v); } } static void print_d_dm(FILE *f, double a) { int d; double m; d = a; m = (a - d) * 60; if (m < 0) { m += 60; d --; } if (m >= 60) { m -= 60; d ++; } fprintf(f,"%d°%f'",d,m); } static void print_d_m_ds(FILE *f, double a) { int d; int m; double s; a *= 60; m = a; s = (a - m) * 60; if (s < 0) { s += 60; m --; } if (s >= 60) { s -= 60; m ++; } d = m / 60; m %= 60; fprintf(f,"%d°%d'%f\"",d,m,s); } static void cmd_show(void) { LATLON ll; int i; int any; FILE *f; ll = pt3_to_ll(at); f = open_oq(&stdout_oq); fprintf(f,"At %f%c %f%c\n",fabs(ll.lat),(ll.lat<0)?'S':'N',fabs(ll.lon),(ll.lon<0)?'W':'E'); fprintf(f," "); print_d_dm(f,fabs(ll.lat)); fprintf(f,"%c ",(ll.lat<0)?'S':'N'); print_d_dm(f,fabs(ll.lon)); fprintf(f,"%c\n",(ll.lon<0)?'W':'E'); fprintf(f," "); print_d_m_ds(f,fabs(ll.lat)); fprintf(f,"%c ",(ll.lat<0)?'S':'N'); print_d_m_ds(f,fabs(ll.lon)); fprintf(f,"%c\n",(ll.lon<0)?'W':'E'); fprintf(f," (%f,%f,%f)\n",at.x,at.y,at.z); fprintf(f,"Speed set %gkt (cur %gkt)\n",speedset,speedcur); fprintf(f,"Interval %gs\n",interval); fprintf(f,"Generating"); any = 0; for (i=0;stypes[i].name;i++) { if (enable & stypes[i].bit) { fprintf(f," %s",stypes[i].name); any = 1; } } if (! any) fprintf(f," nothing"); fprintf(f,"\n"); (*motion->show)(f); fclose(f); } static void cmd_go(const char *s) { LATLON ll; PT3 dst; PT3 to; double d; const char *e; if (scan_ll(s,&e,&ll)) return; while (*e && Cisspace(*e)) e ++; if (*e) { aio_oq_queue_printf(&stdout_oq,"Junk after lat/lon: %s\n",e); return; } aio_oq_queue_printf(&stdout_oq,"Lat %g lon %g\n",ll.lat,ll.lon); dst = loc_ll(ll); to = sub3(dst,at); d = norm3(to); if (d < ONE_METRE) { aio_oq_queue_point(&stdout_oq,"Already there\n",AIO_STRLEN); at = dst; motion = &motion_still; return; } /* * The direction we want to head is along the shorter great circle arc * going through our location and the dstination. We find this * direction by taking the 3-space vector from our current location * to the destination, projecting it onto the tangent plane where we * are, and normalizing. But... */ to = scale3(to,1/d); to = sub3(to,scale3(at,dot3(to,at))); /* * ...if the resulting vector is zero or tiny, we must be heading for * a point on or very close to the line through our current point and * the centre of the earth. Of the two such possible areas, one is * eliminated by the check above, so we must be heading for our * antipodal point. Doesn't much matter what direction we pick in * this case. We use whichever of the three princpal axes gives the * largest vector. * * Using ONE_METRE/2 here instead of ONE_METRE is a defense against * numerical errors; if we used ONE_METRE for each, numerical errors * could make the above test pass but this one fail when we're * heading for a point almost exactly 1m from our current location. */ if (norm3(to) < ONE_METRE/2) { if ((fabs(at.z) < fabs(at.x)) && (fabs(at.z) < fabs(at.y))) { to = PT3_PZ; } else if (fabs(at.y) < fabs(at.x)) { to = PT3_PY; } else { to = PT3_PX; } to = sub3(to,scale3(at,dot3(to,at))); } motion = &motion_headfor; head_pt = dst; dir = unit3(to); } static void cmd_on_off(const char *arg, int on) { const char *sp0; const char *sp; int l; int i; sp = arg; while (*sp && Cisspace(*sp)) sp ++; if (! *sp) { aio_oq_queue_point(&stdout_oq,"Currently on:",AIO_STRLEN); for (i=0;stypes[i].name;i++) { if (enable & stypes[i].bit) aio_oq_queue_printf(&stdout_oq," %s",stypes[i].name); } aio_oq_queue_point(&stdout_oq,"\nCurrently off:",AIO_STRLEN); for (i=0;stypes[i].name;i++) { if (! (enable & stypes[i].bit)) aio_oq_queue_printf(&stdout_oq," %s",stypes[i].name); } aio_oq_queue_point(&stdout_oq,"\n",AIO_STRLEN); return; } while <"keys"> (1) { while (*sp && Cisspace(*sp)) sp ++; sp0 = sp; while (*sp && !Cisspace(*sp)) sp ++; l = sp - sp0; if (l < 1) break; for (i=0;stypes[i].name;i++) { if ((stypes[i].namelen == l) && !strncasecmp(sp0,stypes[i].name,l)) { if (on) enable |= stypes[i].bit; else enable &= ~stypes[i].bit; continue <"keys">; } } aio_oq_queue_printf(&stdout_oq,"Don't recognize `%.*s'\n",l,sp0); } } static void cmd_warp(const char *s) { LATLON ll; const char *e; if (scan_ll(s,&e,&ll)) return; while (*e && Cisspace(*e)) e ++; if (*e) { aio_oq_queue_printf(&stdout_oq,"Junk after lat/lon: %s\n",e); return; } at = loc_ll(ll); motion = &motion_still; } /* * Because of the "project onto the tangent plane" implementation, a * naïve implementation will malfunction when deriving a radius from a * location if the location is >= 90° of great-circle distance from * the circle centre. We actually put the limit at 89° to give us * some safety margin. For the design-for uses, even this is severe * overkill. * * A radius can be given as * to explicitly get it from our current * location. * * A direction can be CW or CCW, or if omitted it comes from the * current direction, or CW if the current tangentional velocity is * zero or tiny. */ static void cmd_circle(const char *s0) { const char *s; LATLON cll; double radius; int rad_from_loc; int dir_from_hdg; int dir_cw; PT3 cloc; const char *e; char *nce; double d; char c[8]; int n; if (scan_ll(s0,&s,&cll)) return; cloc = loc_ll(cll); d = dot3(at,cloc); if (d < COS89) { aio_oq_queue_point(&stdout_oq,"Centre too far away\n",AIO_STRLEN); return; } radius = strtod(s,&nce); e = nce; rad_from_loc = 0; if (e == s) { n = -1; sscanf(e," %5s%n",&c[0],&n); if ((n >= 0) && (!strcasecmp(&c[0],"auto") || !strcmp(&c[0],"*"))) { rad_from_loc = 1; e += n; } else if (n < 0) { rad_from_loc = 1; } else { aio_oq_queue_printf(&stdout_oq,"Can't scan radius: %s\n",s); return; } } n = -1; sscanf(e," %5s%n",&c[0],&n); if (n >= 0) { if (!strcasecmp(&c[0],"cw")) { dir_cw = 1; dir_from_hdg = 0; e += n; } else if (!strcasecmp(&c[0],"ccw")) { dir_cw = 0; dir_from_hdg = 0; e += n; } else if (!strcasecmp(&c[0],"auto")) { e += n; dir_from_hdg = 1; } } else { dir_from_hdg = 1; } n = -1; sscanf(e," %*c%n",&n); if (n >= 0) { aio_oq_queue_printf(&stdout_oq,"Junk after parameters: %s\n",e); return; } if (rad_from_loc) { radius = norm3(sub3(scale3(at,1/d),cloc)); } else { // Radius is specified in metres radius /= RAD; } if (dir_from_hdg) { PT3 perpat; perpat = sub3(at,scale3(cloc,dot3(at,cloc))); d = norm3(perpat); if (d < ONE_METRE) { if (dot3(at,cloc) > 0) { aio_oq_queue_printf(&stdout_oq,"Centre too close to current location to get direction from heading (d=%g)\n",d); } else { aio_oq_queue_printf(&stdout_oq,"Centre too close to current antipode to get direction from heading (d=%g)\n",d); } return; } d = dot3(cross(perpat,dir),cloc); /* * If d is tiny, semi-arbitrarily pick CW. * If d is non-tiny negative, use CW. * If d is non-tiny positive, use CCW. * So.... */ dir_cw = (d < 1e-6); } motion = &motion_circle; circle_ctr = cloc; circle_rad = radius; circle_cw = dir_cw; aio_oq_queue_printf(&stdout_oq,"ctr (%g,%g,%g), rad %gm, cw %d\n", circle_ctr.x, circle_ctr.y, circle_ctr.z, circle_rad/ONE_METRE, circle_cw); } static void cmd_d(const char *s0) { const char *s; LATLON ll; PT3 cloc; double d; if (scan_ll(s0,&s,&ll)) return; cloc = loc_ll(ll); d = norm3(sub3(cloc,at)) / ONE_METRE; if (d >= 10000) { aio_oq_queue_printf(&stdout_oq,"distance %gkm\n",d/1000); } else { aio_oq_queue_printf(&stdout_oq,"distance %gm\n",d); } } static void cmdline_run(const char *l) { const char *sp; const char *sp0; int len; sp = l; while (*sp && Cisspace(*sp)) sp ++; if (*sp == '?') { aio_oq_queue_point(&stdout_oq,"\n\ ?\n\ This help.\n\ d LAT-LON\n\ Print the distance from the current location to LAT-LON.\n\ go LAT-LON\n\ Start heading for LAT-LON. If speed is set above zero, will\n\ actually start moving.\n\ on TYPE\n\ off TYPE\n\ Turn sentence type TYPE on or off.\n\ quit\n\ Quit.\n\ show\n\ Show current status.\n\ tick S\n\ Generate data every S seconds.\n\ warp LAT-LON\n\ Jump instantly to being stationary at LAT-LON.\n\ speed K\n\ Move at K knots (if instructed to move).\n\ circle LAT-LON RADIUS DIRECTION\n\ Start circling. The centre of the circle is at LAT-LON, its\n\ radius is RADIUS (in metres), or \"auto\" or \"*\" to make the\n\ radius equal to the distance from the centre to the current\n\ location, and DIRECTION is \"cw\" or \"ccw\" to circle clockwise or\n\ counterclockwise, or \"auto\" or omitted to circle whichever\n\ direction the current motion is going.\n\ \n\ LAT-LON is a latitude and longitude, in either order. Each one can be\n\ deg.fff, deg.fff°, deg°min.fff', or deg°min'sec.fff\", where .fff\n\ represents an optional decimal fraction. In each case, this needs to\n\ be followed by N, S, E, or W.\n\ ",AIO_STRLEN); return; } sp0 = sp; while (*sp && !Cisspace(*sp)) sp ++; len = sp - sp0; while (*sp && Cisspace(*sp)) sp ++; switch (len) { case 0: return; break; case 1: if (! strncmp(sp0,"d",1)) { cmd_d(sp); return; } break; case 2: switch (*sp0) { case 'g': if (! strncmp(sp0,"go",2)) { cmd_go(sp); return; } break; case 'o': if (! strncmp(sp0,"on",2)) { cmd_on_off(sp,1); return; } break; } break; case 3: if (! strncmp(sp0,"off",3)) { cmd_on_off(sp,0); return; } break; case 4: switch (*sp0) { case 'q': if (!strncmp(sp0,"quit",4)) { cmd_quit(); return; } break; case 's': if (!strncmp(sp0,"show",4)) { cmd_show(); return; } break; case 't': if (!strncmp(sp0,"tick",4)) { cmd_tick(sp); return; } break; case 'w': if (!strncmp(sp0,"warp",4)) { cmd_warp(sp); return; } break; } break; case 5: if (! strncmp(sp0,"speed",5)) { cmd_speed(sp); return; } break; case 6: if (! strncmp(sp0,"circle",6)) { cmd_circle(sp); return; } break; } aio_oq_queue_printf(&stdout_oq,"`%.*s' unrecognized (? for help)\n",len,sp0); } static void cmdline_char(char c) { static char *cb = 0; static int ca = 0; static int cl = 0; if (cl >= ca) cb = realloc(cb,ca=cl+8); if (c == '\n') { prompted = 0; cb[cl++] = '\0'; cmdline_run(cb); cl = 0; } else { cb[cl++] = c; } } static void stdin_r(void *arg __attribute__((__unused__))) { char rbuf[512]; int nr; int i; nr = read(0,&rbuf[0],sizeof(rbuf)); if (nr < 0) { if ((errno == EINTR) || (errno == EWOULDBLOCK)) return; fprintf(stderr,"%s: read from stdin: %s\n",__progname,strerror(errno)); exit(1); } if (nr == 0) exit(0); for (i=0;i 0) { skip --; continue; } if (**av != '-') { if (! ser_path) { ser_path = *av; } else { fprintf(stderr,"%s: extra argument `%s'\n",__progname,*av); errs ++; } continue; } if (0) { needarg:; fprintf(stderr,"%s: %s needs a following argument\n",__progname,*av); errs ++; continue; } #define WANTARG() do { if (++skip >= ac) goto needarg; } while (0) if (!strcmp(*av,"-speed")) { WANTARG(); ser_baud = atoi(av[skip]); continue; } #undef WANTARG fprintf(stderr,"%s: unrecognized option `%s'\n",__progname,*av); errs ++; } if (errs) exit(1); } int main(int, char **); int main(int ac, char **av) { handleargs(ac,av); aio_poll_init(); at = loc_ll((LATLON){.lat=45,.lon=-45}); dir = unit3(cross(at,PT3_PZ)); speedset = 0; motion = &motion_still; startup(); aio_event_loop(); exit(1); }