git.gag.com Git - fw/altos/blob - ao-bringup/cal-accel

   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[*] settings(file f) {
  40         string[...] x = {};
  41
  42         flush_input(f);
  43         fprintf (f, "c s\nv\n");
  44         flush(f);
  45         for (;;) {
  46                 string l = File::fgets(f);
  47                 x[dim(x)] = l;
  48                 if (String::index(l, "software-version") == 0)
  49                         break;
  50         }
  51         return x;
  52 }
  53
  54 string[*] find_setting(string[*] s, string match) {
  55         for (int i = 0; i < dim(s); i++)
  56                 if (String::index(s[i], match) == 0)
  57                         return String::split(s[i], " ");
  58         return (string[*]) {};
  59 }
  60
  61 bool
  62 do_cal(file f) {
  63         flush_input(f);
  64         fprintf(f, "E 1\nc a 0\n");
  65         flush(f);
  66         string s = "";
  67         bool worked = true;
  68         bool running = false;
  69
  70         thread put = fork func() {
  71                 try {
  72                         for (;;) {
  73                                 putc(getchar(), f);
  74                                 flush(f);
  75                         }
  76                 } catch Thread::signal(int i) {
  77                         return;
  78                 }
  79         }();
  80
  81         for (;;) {
  82                 int c = getc(f);
  83                 if (c == '\n')
  84                         s = "";
  85                 else
  86                         s = s + String::new(c);
  87                 putchar(c); flush(stdout);
  88                 if (String::index(s, "press a key...") >= 0)
  89                         running = true;
  90                 if (String::index(s, "Invalid") >= 0)
  91                         worked = false;
  92                 if (running && String::index(s, ">") >= 0)
  93                         break;
  94         }
  95         fprintf (f, "E 0\n");
  96         if (worked)
  97                 fprintf (f, "c w\n");
  98         sleep(200);
  99         Thread::send_signal(put, 1);
 100         Thread::join(put);
 101
 102         return worked;
 103 }
 104
 105 void main () {
 106         string  name = argv[1];
 107         file    f = open(name, "r+");
 108
 109         if (do_cal(f)) {
 110                 string[*] s = settings(f);
 111                 string[*] ac = find_setting(s, "Accel cal");
 112                 printf ("Calibration value +1g %s -1g %s saved\n", ac[3], ac[5]);
 113                 exit (0);
 114         } else {
 115                 printf ("Calibration failed\n");
 116                 exit (1);
 117         }
 118 }
 119
 120 main();