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

   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[*] baro(file f) {
  40         string[...] x = {};
  41
  42         flush_input(f);
  43         fprintf (f, "B\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, "Altitude:") == 0)
  53                         break;
  54         }
  55         return x;
  56 }
  57
  58 string[*] find_baro(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_baro(file f) {
  67         string[*] i = baro(f);
  68         string[*] temp = find_baro(i, "Temperature");
  69         string[*] alt = find_baro(i, "Altitude");
  70
  71         real temperature = string_to_integer(temp[2]) / 100.0;
  72         real altitude = string_to_integer(alt[1]);
  73
  74         if (altitude < -50 || 3000 < altitude) {
  75                 printf ("weird altitude %f\n", altitude);
  76                 return false;
  77         }
  78
  79         if (temperature < 20 || 30 < temperature) {
  80                 printf ("weird temperature %f\n", temperature);
  81                 return false;
  82         }
  83
  84         printf ("altitude %f temperature %f\n", altitude, temperature);
  85
  86         return true;
  87 }
  88
  89 void main () {
  90         string  name = argv[1];
  91         file    f = open(name, "r+");
  92         bool ret = true;
  93
  94         if (!do_baro(f))
  95                 ret = false;
  96         exit (ret? 0 : 1);
  97 }
  98
  99 main();