git.gag.com Git - fw/altos/blob - ao-bringup/test-gps

   1 #!/usr/bin/nickle
   2
   3 import File;
   4
   5 string timed_read(file f, int timeout) {
   6         thread reader = fork func() {
   7                 try {
   8                         return fgets(f);
   9                 } catch Thread::signal(int i) {
  10                         return "";
  11                 }
  12         }();
  13
  14         thread killer = fork func() {
  15                 try {
  16                         sleep (timeout);
  17                         Thread::send_signal(reader, 1);
  18                 } catch Thread::signal(int i) {
  19                         return;
  20                 }
  21         }();
  22
  23         poly v = Thread::join(reader);
  24         Thread::send_signal(killer, 1);
  25         Thread::join(killer);
  26         if (is_string(v))
  27                 return v;
  28         return "";
  29 }
  30
  31 void flush_input(file f) {
  32         for (;;) {
  33                 string s = timed_read(f, 200);
  34                 if (s == "")
  35                         break;
  36         }
  37 }
  38
  39 string[*] gps(file f) {
  40         string[...] x = {};
  41
  42         flush_input(f);
  43         fprintf (f, "g\nv\n");
  44         flush(f);
  45         for (;;) {
  46                 string l = timed_read(f, 1000);
  47                 if (l == "") {
  48                         File::fprintf(stderr, "Read timedout\n");
  49                         exit(1);
  50                 }
  51                 x[dim(x)] = l;
  52                 if (String::index(l, "software-version") == 0)
  53                         break;
  54         }
  55         return x;
  56 }
  57
  58 string[*] find_gps(string[*] s, string match) {
  59         for (int i = 0; i < dim(s); i++)
  60                 if (String::index(s[i], match) >= 0)
  61                         return String::wordsplit(s[i], " ");
  62         return (string[*]) {};
  63 }
  64
  65 bool
  66 do_gps(file f) {
  67
  68         string[*] i = gps(f);
  69         string[*] flags = find_gps(i, "Flags:");
  70         string[*] sats = find_gps(i, "Sats:");
  71
  72         int actual_flags = string_to_integer(flags[1]);
  73         int actual_sats = string_to_integer(sats[1]);
  74
  75         while ((actual_flags & (1 << 4)) == 0) {
  76                 printf("Flags: %s\n", flags[1]);
  77                 printf("Sats: %s\n", sats[1]);
  78
  79                 sleep(1000);
  80                 i = gps(f);
  81                 flags = find_gps(i, "Flags:");
  82                 sats = find_gps(i, "Sats:");
  83
  84                 actual_flags = string_to_integer(flags[1]);
  85         }
  86
  87         printf("Flags: %s\n", flags[1]);
  88         printf("Sats: %s\n", sats[1]);
  89         printf("GPS locked\n");
  90         return true;
  91 }
  92
  93 void main () {
  94         string  name = argv[1];
  95         file    f = open(name, "r+");
  96         bool ret = true;
  97
  98         if (!do_gps(f))
  99                 ret = false;
 100         exit (ret? 0 : 1);
 101 }
 102
 103 main();