imuboard/gps.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <string.h>
   5
   6 #include <aversive.h>
   7 #include <aversive/endian.h>
   8 #include <aversive/wait.h>
   9 #include <callout.h>
  10
  11 #include "main.h"
  12 #include "gps.h"
  13 #include "fat.h"
  14 #include "fat_config.h"
  15 #include "partition.h"
  16 #include "sd_raw.h"
  17 #include "sd_raw_config.h"
  18 #include "sd_log.h"
  19
  20 /* global gps position */
  21 struct gps_pos gps_pos;
  22
  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__) */
  26
  27 /* display current GPS position stored in the global variable */
  28 void display_gps_pos(void)
  29 {
  30         /* no need to call ntohs/ntohl because the frame is already fixed by
  31          * decode_gps_pos() */
  32         printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
  33                         "tow %.10"PRIu32"\t"),
  34                 gps_pos.msg_id,
  35                 gps_pos.mode,
  36                 gps_pos.sv_num,
  37                 gps_pos.gps_week,
  38                 gps_pos.tow);
  39
  40         printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
  41                 gps_pos.latitude,
  42                 gps_pos.longitude,
  43                 gps_pos.altitude);
  44
  45         printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
  46                         "tdop %3.3f\n"),
  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.);
  52
  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.);
  58
  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.);
  63 }
  64
  65 void gps_get_pos(struct gps_pos *pos)
  66 {
  67         memcpy(pos, &gps_pos, sizeof(*pos));
  68 }
  69
  70 int gps_log(uint8_t to_stdout)
  71 {
  72         uint32_t ms;
  73         uint8_t flags, prio;
  74         int16_t len;
  75         char buf[128];
  76         struct gps_pos pos;
  77
  78         IRQ_LOCK(flags);
  79         ms = global_ms;
  80         IRQ_UNLOCK(flags);
  81
  82         /* get position (prevent modification of gps pos during copy) */
  83         prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
  84         gps_get_pos(&pos);
  85         callout_mgr_restore_prio(&imuboard.intr_cm, prio);
  86
  87         /* XXX copy */
  88         len = snprintf(buf, sizeof(buf),
  89                 "%"PRIu32" "
  90                 "svnum %.2X lat %3.5f long %3.5f "
  91                 "alt %3.5f sea_alt %3.5f\n",
  92                 ms, gps_pos.sv_num,
  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.);
  97
  98
  99         if (!to_stdout && sd_log_enabled()) {
 100                 if (sd_log_write(buf, len) != len) {
 101                         printf_P(PSTR("error writing to file\n"));
 102                         return -1;
 103                 }
 104         }
 105         else if (to_stdout) {
 106                 printf_P(PSTR("%s"), buf);
 107         }
 108
 109         return 0;
 110 }