Parent Directory
| 
Revision Log
Parent Directory
| Revision Log
Added gpsemu tool
| 1 | /* |
| 2 | * gpstest.c |
| 3 | * |
| 4 | * simple tool to fake a gpsd |
| 5 | */ |
| 6 | |
| 7 | #include <stdlib.h> |
| 8 | #include <gtk/gtk.h> |
| 9 | #include <math.h> |
| 10 | |
| 11 | #include <sys/socket.h> /* for socket(), bind(), and connect() */ |
| 12 | #include <arpa/inet.h> /* for sockaddr_in and inet_ntoa() */ |
| 13 | #include <string.h> /* for memset() */ |
| 14 | |
| 15 | typedef struct { |
| 16 | double latitude, longitude; |
| 17 | } pos_t; |
| 18 | |
| 19 | #define NAN (0.0/0.0) |
| 20 | |
| 21 | GtkWidget *window = NULL; |
| 22 | GThread* thread_p = NULL; |
| 23 | GMutex *mutex = NULL; |
| 24 | |
| 25 | /* initial position */ |
| 26 | pos_t cur_pos = { 48.8812, 8.50773 }; |
| 27 | float altitude = 100.1; |
| 28 | gboolean fix = TRUE, fix3d = TRUE; |
| 29 | |
| 30 | #define PORT 2947 |
| 31 | |
| 32 | pos_t geomath_translate(pos_t p, double dist, double angle) { |
| 33 | pos_t result; |
| 34 | |
| 35 | double gcrad = 6371000.0; // great circle in meters |
| 36 | |
| 37 | // from: http://www.movable-type.co.uk/scripts/latlong.html |
| 38 | result.latitude = asin(sin(p.latitude/180*M_PI) * cos(dist/gcrad) + |
| 39 | cos(p.latitude/180*M_PI) * sin(dist/gcrad) * |
| 40 | cos(angle/180*M_PI) )/M_PI*180; |
| 41 | result.longitude = p.longitude + atan2(sin(angle/180*M_PI)*sin(dist/gcrad)* |
| 42 | cos(p.latitude/180*M_PI), |
| 43 | cos(dist/gcrad)-sin(p.latitude/180*M_PI)* |
| 44 | sin(result.latitude/180*M_PI))/M_PI*180; |
| 45 | // normalise to -180...+180 |
| 46 | result.longitude = fmodf(result.longitude+180,360) - 180; |
| 47 | |
| 48 | return result; |
| 49 | } |
| 50 | |
| 51 | void pos_lon_str(char *str, int len, double longitude) { |
| 52 | char c = 'E'; |
| 53 | double integral, fractional; |
| 54 | |
| 55 | if(longitude < 0) { longitude = fabs(longitude); c = 'W'; } |
| 56 | fractional = modf(longitude, &integral); |
| 57 | |
| 58 | snprintf(str, len, "%c %03d° %06.3f'", c, (int)integral, fractional*60.0); |
| 59 | } |
| 60 | |
| 61 | void pos_lat_str(char *str, int len, double latitude) { |
| 62 | char c = 'N'; |
| 63 | double integral, fractional; |
| 64 | |
| 65 | if(latitude < 0) { latitude = fabs(latitude); c = 'S'; } |
| 66 | fractional = modf(latitude, &integral); |
| 67 | |
| 68 | snprintf(str, len, "%c %02d° %06.3f'", c, (int)integral, fractional*60.0); |
| 69 | } |
| 70 | |
| 71 | void pos_alt_str(char *str, int len, double altitude) { |
| 72 | snprintf(str, len, "%.02fm", altitude); |
| 73 | } |
| 74 | |
| 75 | |
| 76 | GtkWidget *pos_lat(double latitude) { |
| 77 | char str[32]; |
| 78 | pos_lat_str(str, sizeof(str), latitude); |
| 79 | return gtk_label_new(str); |
| 80 | } |
| 81 | |
| 82 | GtkWidget *pos_lon(double longitude) { |
| 83 | char str[32]; |
| 84 | pos_lon_str(str, sizeof(str), longitude); |
| 85 | return gtk_label_new(str); |
| 86 | } |
| 87 | |
| 88 | GtkWidget *pos_alt(double altitude) { |
| 89 | char str[32]; |
| 90 | pos_alt_str(str, sizeof(str), altitude); |
| 91 | return gtk_label_new(str); |
| 92 | } |
| 93 | |
| 94 | #define DEFAULT_DIST 10.0 |
| 95 | #define DEFAULT_DIR 0.0 |
| 96 | GtkWidget *navtool_dist, *navtool_dir, *navtool_go; |
| 97 | GtkWidget *navtool_lat, *navtool_lon, *navtool_fix, *navtool_fix3d; |
| 98 | GtkWidget *navtool_alt; |
| 99 | |
| 100 | void go_clicked(GtkButton *button, gpointer data) { |
| 101 | char str[32]; |
| 102 | float dist, dir; |
| 103 | char *p = (char*)gtk_entry_get_text(GTK_ENTRY(navtool_dist)); |
| 104 | if(sscanf(p, "%f", &dist) != 1) |
| 105 | dist = NAN; |
| 106 | |
| 107 | p = (char*)gtk_entry_get_text(GTK_ENTRY(navtool_dir)); |
| 108 | if(sscanf(p, "%f", &dir) != 1) |
| 109 | dir = NAN; |
| 110 | |
| 111 | printf("%s: going %.2fm heading %.1f°\n", __func__, dist, dir); |
| 112 | |
| 113 | g_mutex_lock(mutex); |
| 114 | cur_pos = geomath_translate(cur_pos, dist, dir); |
| 115 | g_mutex_unlock(mutex); |
| 116 | |
| 117 | pos_lat_str(str, sizeof(str), cur_pos.latitude); |
| 118 | gtk_label_set_text(GTK_LABEL(navtool_lat), str); |
| 119 | pos_lon_str(str, sizeof(str), cur_pos.longitude); |
| 120 | gtk_label_set_text(GTK_LABEL(navtool_lon), str); |
| 121 | } |
| 122 | |
| 123 | void fix_toggled(GtkToggleButton *togglebutton, gpointer user_data) { |
| 124 | fix = gtk_toggle_button_get_active(togglebutton); |
| 125 | } |
| 126 | |
| 127 | void fix3d_toggled(GtkToggleButton *togglebutton, gpointer user_data) { |
| 128 | fix3d = gtk_toggle_button_get_active(togglebutton); |
| 129 | } |
| 130 | |
| 131 | GtkWidget *new_navtool(void) { |
| 132 | char str[32]; |
| 133 | GtkWidget *hbox, *table = gtk_table_new(3,3,FALSE); |
| 134 | |
| 135 | hbox = gtk_hbox_new(FALSE,0); |
| 136 | gtk_box_pack_start_defaults(GTK_BOX(hbox), navtool_dist = gtk_entry_new()); |
| 137 | gtk_box_pack_start_defaults(GTK_BOX(hbox), gtk_label_new("m")); |
| 138 | gtk_table_attach_defaults(GTK_TABLE(table), hbox, 0, 1, 0, 1); |
| 139 | snprintf(str, sizeof(str), "%.2f", DEFAULT_DIST); |
| 140 | gtk_entry_set_text(GTK_ENTRY(navtool_dist), str); |
| 141 | gtk_entry_set_width_chars(GTK_ENTRY(navtool_dist), 6); |
| 142 | |
| 143 | hbox = gtk_hbox_new(FALSE,0); |
| 144 | gtk_box_pack_start_defaults(GTK_BOX(hbox), navtool_dir = gtk_entry_new()); |
| 145 | gtk_box_pack_start_defaults(GTK_BOX(hbox), gtk_label_new("°")); |
| 146 | gtk_table_attach_defaults(GTK_TABLE(table), hbox, 0, 1, 1, 2); |
| 147 | snprintf(str, sizeof(str), "%.1f", DEFAULT_DIR); |
| 148 | gtk_entry_set_text(GTK_ENTRY(navtool_dir), str); |
| 149 | gtk_entry_set_width_chars(GTK_ENTRY(navtool_dir), 6); |
| 150 | |
| 151 | gtk_table_attach_defaults(GTK_TABLE(table), |
| 152 | navtool_go = gtk_button_new_with_label("Go"), 1, 2, 0, 2); |
| 153 | gtk_signal_connect(GTK_OBJECT(navtool_go), "clicked", |
| 154 | (GtkSignalFunc)go_clicked, NULL); |
| 155 | |
| 156 | gtk_table_attach_defaults(GTK_TABLE(table), |
| 157 | navtool_lat = pos_lat(cur_pos.latitude), 2, 3, 0, 1); |
| 158 | |
| 159 | gtk_table_attach_defaults(GTK_TABLE(table), |
| 160 | navtool_lon = pos_lon(cur_pos.longitude), 2, 3, 1, 2); |
| 161 | |
| 162 | navtool_fix = gtk_check_button_new_with_label("Fix"); |
| 163 | gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(navtool_fix), fix); |
| 164 | g_signal_connect(navtool_fix, "toggled", |
| 165 | G_CALLBACK(fix_toggled), NULL); |
| 166 | gtk_table_attach_defaults(GTK_TABLE(table), |
| 167 | navtool_fix, 0, 1, 2, 3); |
| 168 | |
| 169 | navtool_fix3d = gtk_check_button_new_with_label("Fix3d"); |
| 170 | gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(navtool_fix3d), fix3d); |
| 171 | g_signal_connect(navtool_fix3d, "toggled", |
| 172 | G_CALLBACK(fix3d_toggled), NULL); |
| 173 | gtk_table_attach_defaults(GTK_TABLE(table), |
| 174 | navtool_fix3d, 1, 2, 2, 3); |
| 175 | |
| 176 | gtk_table_attach_defaults(GTK_TABLE(table), |
| 177 | navtool_alt = pos_alt(altitude), 2, 3, 2, 3); |
| 178 | |
| 179 | |
| 180 | return table; |
| 181 | } |
| 182 | |
| 183 | pos_t gui_navtool_get_pos(void) { |
| 184 | g_mutex_lock(mutex); |
| 185 | pos_t cpos = cur_pos; |
| 186 | g_mutex_unlock(mutex); |
| 187 | |
| 188 | return cpos; |
| 189 | } |
| 190 | |
| 191 | |
| 192 | /* |
| 193 | * Terminate the main loop. |
| 194 | */ |
| 195 | static void on_destroy (GtkWidget * widget, gpointer data) { |
| 196 | gtk_main_quit (); |
| 197 | } |
| 198 | |
| 199 | gpointer comm_thread(gpointer data) { |
| 200 | char buf[1024]; |
| 201 | int sock; /* socket to create */ |
| 202 | struct sockaddr_in saddr; /* Local address */ |
| 203 | |
| 204 | /* open a listen socket */ |
| 205 | /* Create socket for incoming connections */ |
| 206 | if ((sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0) { |
| 207 | perror("socket()"); |
| 208 | exit(1); |
| 209 | } |
| 210 | |
| 211 | /* Construct local address structure */ |
| 212 | memset(&saddr, 0, sizeof(saddr)); /* Zero out structure */ |
| 213 | saddr.sin_family = AF_INET; /* Internet address family */ |
| 214 | saddr.sin_addr.s_addr = htonl(INADDR_ANY); /* Any incoming interface */ |
| 215 | saddr.sin_port = htons(PORT); /* Local port */ |
| 216 | |
| 217 | /* Bind to the local address */ |
| 218 | if (bind(sock, (struct sockaddr *) &saddr, sizeof(saddr)) < 0) { |
| 219 | perror("bind()"); |
| 220 | exit(1); |
| 221 | } |
| 222 | |
| 223 | /* Mark the socket so it will listen for incoming connections */ |
| 224 | if (listen(sock, 1) < 0) { |
| 225 | perror("listen()"); |
| 226 | exit(1); |
| 227 | } |
| 228 | |
| 229 | printf("Server listening on port %d\n", PORT); |
| 230 | |
| 231 | while(1) { |
| 232 | printf("Waiting for client ...\n"); |
| 233 | |
| 234 | int csock; /* Socket descriptor for client */ |
| 235 | struct sockaddr_in caddr; /* Client address */ |
| 236 | unsigned int clen; /* Length of client address data structure */ |
| 237 | |
| 238 | /* Set the size of the in-out parameter */ |
| 239 | clen = sizeof(caddr); |
| 240 | |
| 241 | /* Wait for a client to connect */ |
| 242 | if((csock = accept(sock, (struct sockaddr *) &caddr, &clen)) < 0) { |
| 243 | perror("accept()"); |
| 244 | exit(1); |
| 245 | } |
| 246 | |
| 247 | printf("Connected by client %s\n", inet_ntoa(caddr.sin_addr)); |
| 248 | |
| 249 | int reqlen = read(csock, buf, sizeof(buf)); |
| 250 | while(reqlen > 0) { |
| 251 | buf[reqlen] = 0; |
| 252 | printf("Got %d bytes request (%s)\n", reqlen, buf); |
| 253 | |
| 254 | int i; |
| 255 | for(i=0;i<reqlen;i++) { |
| 256 | switch(buf[i]) { |
| 257 | char sbuf[512]; |
| 258 | |
| 259 | case 0: |
| 260 | case '\r': |
| 261 | case '\n': |
| 262 | case '+': |
| 263 | break; |
| 264 | |
| 265 | case 'o': |
| 266 | case 'O': |
| 267 | { pos_t pos = gui_navtool_get_pos(); |
| 268 | |
| 269 | if(fix) { |
| 270 | char alt_str[32]; |
| 271 | snprintf(alt_str, sizeof(alt_str), "%.1f", altitude); |
| 272 | |
| 273 | // http://gpsd.berlios.de/gpsd.html |
| 274 | snprintf(sbuf, sizeof(sbuf), "GPSD O=%s %d %d " |
| 275 | "%.6f %.6f %s %.1f %.1f %.1f " |
| 276 | "%.1f %.1f %.1f " |
| 277 | "%.1f %.1f %.1f " |
| 278 | "%d\n", |
| 279 | "XXX", /* TAG */ |
| 280 | time(NULL), /* timestr */ |
| 281 | 1, /* ept, error time */ |
| 282 | pos.latitude, pos.longitude, |
| 283 | fix3d?alt_str:"?", /* altitude */ |
| 284 | 10.0, 10.0, /* eph, epv */ |
| 285 | 0.0, 0.0, 0.0, /* track, speed, climb */ |
| 286 | 1.0, 1.0, 1.0, /* epd, eps, epc */ |
| 287 | 3); /* mode */ |
| 288 | } else |
| 289 | strcpy(sbuf, "GPSD O=?\n"); |
| 290 | |
| 291 | // printf("sending %s\n", sbuf); |
| 292 | |
| 293 | write(csock, sbuf, strlen(sbuf)); |
| 294 | } break; |
| 295 | |
| 296 | |
| 297 | default: |
| 298 | printf("insupported request %c\n", buf[i]); |
| 299 | snprintf(sbuf, sizeof(sbuf), "GPSD %c=?\n", buf[i]); |
| 300 | write(csock, sbuf, strlen(sbuf)); |
| 301 | break; |
| 302 | } |
| 303 | } |
| 304 | reqlen = read(csock, buf, sizeof(buf)); |
| 305 | } |
| 306 | |
| 307 | perror("read()"); |
| 308 | |
| 309 | printf("Closing connection\n"); |
| 310 | close(csock); |
| 311 | } |
| 312 | } |
| 313 | |
| 314 | int |
| 315 | main (int argc, char *argv[]) { |
| 316 | |
| 317 | gtk_init (&argc, &argv); |
| 318 | |
| 319 | g_thread_init(NULL); |
| 320 | |
| 321 | /* create the main, top level, window */ |
| 322 | window = gtk_window_new (GTK_WINDOW_TOPLEVEL); |
| 323 | |
| 324 | /* give the window a 20px wide border */ |
| 325 | gtk_container_set_border_width (GTK_CONTAINER (window), 20); |
| 326 | |
| 327 | /* give it the title */ |
| 328 | gtk_window_set_title (GTK_WINDOW (window), APP); |
| 329 | |
| 330 | /* open it a bit wider so that both the label and title show up */ |
| 331 | // gtk_window_set_default_size (GTK_WINDOW (window), 200, 300); |
| 332 | |
| 333 | /* Connect the destroy event of the window with our on_destroy function |
| 334 | * When the window is about to be destroyed we get a notificaiton and |
| 335 | * stop the main GTK loop |
| 336 | */ |
| 337 | g_signal_connect(G_OBJECT(window), "destroy", G_CALLBACK(on_destroy), NULL); |
| 338 | |
| 339 | gtk_container_add(GTK_CONTAINER(window), new_navtool()); |
| 340 | |
| 341 | /* make sure that everything, window and label, are visible */ |
| 342 | gtk_widget_show_all (window); |
| 343 | |
| 344 | /* start communication thread */ |
| 345 | mutex = g_mutex_new(); |
| 346 | thread_p = g_thread_create(comm_thread, NULL, FALSE, NULL); |
| 347 | |
| 348 | /* start the main loop */ |
| 349 | gtk_main(); |
| 350 | |
| 351 | return 0; |
| 352 | } |
| 353 |