7 #include <aversive/endian.h>
8 #include <aversive/wait.h>
14 #include "fat_config.h"
15 #include "partition.h"
17 #include "sd_raw_config.h"
20 /* global gps position */
21 struct gps_pos gps_pos;
23 /* note: enabling debug will make the code fail due to rx queue full */
24 #define debug_printf(fmt, a...) do { } while (0)
25 /* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */
27 /* display current GPS position stored in the global variable */
28 void display_gps_pos(void)
30 /* no need to call ntohs/ntohl because the frame is already fixed by
32 printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
33 "tow %.10"PRIu32"\t"),
40 printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
45 printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
47 (double)gps_pos.gdop/100.,
48 (double)gps_pos.pdop/100.,
49 (double)gps_pos.hdop/100.,
50 (double)gps_pos.vdop/100.,
51 (double)gps_pos.tdop/100.);
53 printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"),
54 (double)gps_pos.latitude/10000000.,
55 (double)gps_pos.longitude/10000000.,
56 (double)gps_pos.altitude/100.,
57 (double)gps_pos.sea_altitude/100.);
59 printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"),
60 (double)gps_pos.ecef_vx/100.,
61 (double)gps_pos.ecef_vy/100.,
62 (double)gps_pos.ecef_vz/100.);
65 void gps_get_pos(struct gps_pos *pos)
67 memcpy(pos, &gps_pos, sizeof(*pos));
70 int gps_log(uint8_t to_stdout)
82 /* get position (prevent modification of gps pos during copy) */
83 prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
85 callout_mgr_restore_prio(&imuboard.intr_cm, prio);
88 len = snprintf(buf, sizeof(buf),
90 "svnum %.2X lat %3.5f long %3.5f "
91 "alt %3.5f sea_alt %3.5f\n",
93 (double)gps_pos.latitude / 10000000.,
94 (double)gps_pos.longitude / 10000000.,
95 (double)gps_pos.altitude / 100.,
96 (double)gps_pos.sea_altitude / 100.);
99 if (!to_stdout && sd_log_enabled()) {
100 if (sd_log_write(buf, len) != len) {
101 printf_P(PSTR("error writing to file\n"));
105 else if (to_stdout) {
106 printf_P(PSTR("%s"), buf);