host/app/dadpyscan/dadpyscan.py

   1 #!/usr/bin/python
   2
   3 import sys
   4 import serial
   5 import time
   6 import numpy
   7 import argparse
   8 import random
   9
  10 class rdevice(object):
  11   def __init__(self, rcon = None):
  12     self.rcon = rcon;
  13     self.wait_stamp_sqn = random.randint(1, 1000)
  14
  15   def set_rcon(self, rcon):
  16     if self.rcon is not None:
  17       self.rcon.close();
  18       self.rcon = None;
  19     self.rcon = rcon;
  20
  21   def init_communication(self):
  22     s = self.rcon.read(1024)
  23
  24     self.rcon.write("InvBuff\n")
  25     s = self.rcon.read(1024)
  26
  27     self.rcon.write("ECHO:0\n")
  28     s = self.rcon.read(1024)
  29     self.rcon.write("VER?\n")
  30     s = self.rcon.read(1024)
  31     print(s);
  32
  33     # Purge
  34     s = self.rcon.read(2000)
  35     print(s);
  36
  37   def set_int_param(self, param, val):
  38     valstr=str(int(val))
  39     self.rcon.write(param+':'+valstr+'\n')
  40
  41   def wait_stamp(self):
  42     s = self.rcon.read(1024)
  43     self.wait_stamp_sqn = self.wait_stamp_sqn + 1
  44     sqnstr = str(int(self.wait_stamp_sqn))
  45     self.rcon.write("STAMP:"+sqnstr+"\n")
  46     while True:
  47       s = self.rcon.read(1024)
  48       for sl in s.splitlines():
  49         if (sl == "STAMP="+sqnstr):
  50           return
  51       time.sleep(1)
  52
  53   def get_multiline(self, last_line = None, try_cycles = 10):
  54     res = ''
  55     try_cnt = 0;
  56     while True:
  57       s = self.rcon.read(10*1024)
  58       if len(s) == 0:
  59         if last_line is None:
  60           return res
  61         else:
  62            if try_cnt > try_cycles:
  63              return None
  64            try_cnt = try_cnt + 1
  65       try_cnt = 0
  66       res = res + s
  67       if last_line is not None:
  68         for sl in res.splitlines():
  69           if (sl == last_line):
  70             return res
  71
  72 def rdevice_com_test(rcon):
  73   rcon.write("init\n");
  74   rcon.write("run\n");
  75   wait_stamp(rcon)
  76   print("Wait stamp done")
  77
  78
  79 if __name__ == '__main__':
  80   help_msg = '''SYNOPSIS: dadpyscan.py [-d /dev/ttyXXX]
  81         Control LX DAD'''
  82
  83   parser = argparse.ArgumentParser(description='BlueBot rdevice commander')
  84   parser.add_argument('-s', '--skip-setup', dest='skip_setup', action='store_true',
  85                       default=False, help='skip hard-home inicialization of rdevice')
  86   parser.add_argument('-d', '--tty-device', dest='tty_dev', type=str,
  87                     default='/dev/ttyACM0', help='tty line/device to rdevice')
  88   parser.add_argument('-a', '--action', dest='action', type = str,
  89                       default=None, help='action to run')
  90
  91   args = parser.parse_args()
  92
  93   tty_dev = args.tty_dev
  94   skip_setup = args.skip_setup
  95   action = args.action
  96   scan_count = 20
  97
  98   if action is None:
  99     action = 'cyclic'
 100
 101   random.seed()
 102
 103   r = rdevice();
 104
 105   if True:
 106     print("Opening %s ...\n" % tty_dev)
 107     ser = serial.Serial(tty_dev,
 108           baudrate=57600,
 109           bytesize=serial.EIGHTBITS,
 110           parity=serial.PARITY_NONE,
 111           stopbits=serial.STOPBITS_ONE,
 112           timeout=0.1)
 113
 114     r.set_rcon(ser)
 115
 116     #ser.open()
 117
 118     #ser.write("SPIMST0:4(%02X,%02X)\n"%((output_data>>8)&0xFF,(output_data)&0xFF))
 119     #s = ser.read(1000)
 120     #print(s)
 121
 122     r.init_communication()
 123
 124   if not skip_setup or (action == 'home'):
 125     print("Setup")
 126   else:
 127     print("Setup skipped")
 128
 129   print('action:'+action)
 130
 131   if action == 'cyclic':
 132     r.wait_stamp()
 133     r.rcon.write("init\n");
 134     scan_cnt = 0
 135     scan_all=numpy.ndarray(shape=(0,0), dtype=float)
 136     while True:
 137       r.wait_stamp()
 138       r.rcon.write("run\n");
 139       s = r.get_multiline(last_line = 'end')
 140       if s is None:
 141         print("scan read timeout")
 142         continue
 143       si = s.find('bank')
 144       ei = s.find('end')
 145       if (si == -1) or (ei == -1):
 146         print("bank, end not found")
 147         continue
 148       si = s.find('\n', si, ei)
 149       scan_act=numpy.fromstring(s[si:ei], dtype=float, count=-1, sep=' ')
 150       print(scan_act);
 151       if scan_all.size == 0:
 152         scan_all=numpy.ndarray(shape=(scan_count,scan_act.size), dtype=float)
 153       scan_all[scan_cnt,:]=scan_act
 154       scan_cnt = scan_cnt + 1
 155       if scan_cnt >= scan_count:
 156         F=open('scan.dat', mode='wt')
 157         numpy.savetxt(F, scan_all, fmt='%8g')
 158         F.write('min:')
 159         numpy.savetxt(F, numpy.amin(scan_all, axis = 0), fmt='%8g', newline = ' ')
 160         F.write('\n')
 161         F.write('max:')
 162         numpy.savetxt(F, numpy.amax(scan_all, axis = 0), fmt='%8g', newline = ' ')
 163         F.write('\n')
 164         F.write('std:')
 165         numpy.savetxt(F, numpy.std(scan_all, axis = 0), fmt='%8g', newline = ' ')
 166         F.write('\n')
 167         F.close()
 168         exit(0)