From a546c1294b2b97982aa6e4c7153a0f22780c91fe Mon Sep 17 00:00:00 2001 From: Andy Spencer Date: Mon, 10 Feb 2014 00:54:51 +0000 Subject: [PATCH] Get things running --- vis/config.py | 16 +-- vis/device.py | 327 ++++++++++++++++++++++++++++++-------------------- vis/visual.py | 16 +-- vis/visual.ui | 24 ++-- yue/main.cpp | 203 +++++++++++++++++-------------- 5 files changed, 340 insertions(+), 246 deletions(-) diff --git a/vis/config.py b/vis/config.py index be79e48..5e5ce4a 100644 --- a/vis/config.py +++ b/vis/config.py @@ -25,15 +25,15 @@ class Config: 'enable': { 'acc': True, 'mag': True, - 'touch': True, - 'light': True, + 'lgt': True, + 'tch': True, 'a2d': True, }, 'rate': { 'acc': 1, 'mag': 1, - 'touch': 1, - 'light': 1, + 'lgh': 1, + 'tch': 1, 'a2d': 1, }, } @@ -72,8 +72,8 @@ class Config: self.maxrate = int(get('xively', 'maxrate')) for key in list(self.defaults['enable'].keys()): - self.enable[key] = bin(get('enable', key)) - self.rate[key] = int(get('rate', key)) + self.enable[key] = bin(get('enable', key)) + self.rate[key] = float(get('rate', key)) def save(self): self.parser.set('serial', 'device', self.device) @@ -87,8 +87,8 @@ class Config: self.parser.set('xively', 'maxrate', str(int(self.maxrate))) for key in self.defaults['enable'].keys(): - self.parser.set('enable', key, str(bool(self.enable[key]))) - self.parser.set('rate', key, str( int(self.rate[key]))) + self.parser.set('enable', key, str( bool(self.enable[key]))) + self.parser.set('rate', key, str(float(self.rate[key]))) with open(self.filename, 'w') as fd: self.parser.write(fd) diff --git a/vis/device.py b/vis/device.py index ab08201..82b8b45 100644 --- a/vis/device.py +++ b/vis/device.py @@ -5,61 +5,171 @@ from serial import Serial from datetime import datetime from struct import * +class State: #information stored + acc = [None]*3 + mag = [None]*3 + lgt = [None]*1 + tch = [None]*2 + a2d = [None]*6 + time = None + def __init__(self): + self.time = datetime.utcnow() -#buildingFrame = 0; #if it is a start of new frame +class Frame: + # Sensor types + ACC_SNS = 0x00 + MAG_SNS = 0x01 + LGT_SNS = 0x02 + TCH_SNS = 0x03 + A2D_SNS = 0x04 -class Const: - HEADER = 0x02 - SNS_BITS = 5 - LGT_SNS = 0x00 - ACC_SNS = 0x01 - MAG_SNS = 0x02 - TCH_SNS = 0x03 - ADC_SNS_1 = 0x04 + SNS_NUM = 0x05 + SNS_SHIFT = 4 + SNS_MASK = 0xF0 - INT = 0 - LONG = 1 - FLOAT = 2 - DOUBLE = 3 - TAIL = 0x0A + # Data types + INT = 0 + LONG = 1 + FLOAT = 2 + DOUBLE = 3 - START = 0 - STOP = 1 - SET_INT = 2 - buildingFrame = 0 + TYP_NUM = 4 + TYP_SHIFT = 0 + TYP_MASK = 0x0F - snsCode = {'light':LGT_SNS,'touch':TCH_SNS,'acc':ACC_SNS,'mag':MAG_SNS,'a2d':ADC_SNS_1} - cmdCode = {'start':START, 'stop':STOP, 'set':SET_INT} - sizeMap = {INT:2, FLOAT:4, LONG:4, DOUBLE:8} - typeMap = {0:INT, 1:LONG, FLOAT:2, DOUBLE:3} + # Command codes + STOP = 0 + START = 1 + RATE = 2 -class Static: + OPER_NUM = 3 + OPER_SHIFT = 0 + OPER_MASK = 0x0F - count = 0 + # Frame information + HEADER = 0x02 + TAIL = 0x0A + HEADER_POS = 0 + BITS_POS = 1 + COUNT_POS = 2 + DATA_POS = 3 -class State: #information stored - acc = [None]*3 - mag = [None]*3 - touch = [None]*2 - light = [None]*1 - a2d = [None]*6 - time = None + # Maps + snsMap = {ACC_SNS: 'acc', + MAG_SNS: 'mag', + LGT_SNS: 'lgt', + TCH_SNS: 'tch', + A2D_SNS: 'a2d'} + + cmdMap = {START, 'start', + STOP, 'stop', + RATE, 'rate'} + sizeMap = {INT: 2, + LONG: 4, + FLOAT: 4, + DOUBLE: 8} + + fmtMap = {INT: 'h', + LONG: 'i', + FLOAT: 'f', + DOUBLE: 'd'} + + # Parser data + index = 0 # read index + count = 0 # number of items in frame + length = 0 # length of frame (in bytes) + bits_sns = 0 # sensor type + bits_typ = 0 # data type + binary = "" # binary read-in + values = [] # converted numeric data + + # Constructor def __init__(self): - self.time = datetime.utcnow() + pass + + # Converters + @staticmethod + def findCode(dataMap, name): + for code in dataMap: + if dataMap[code] == name: + return code + print("[ERROR] No code found") + + # Parse frame + def parse(self, byte): + # save current pos and increment read index + # if we have an error we cna reset index below + pos = self.index + self.index += 1 + + if pos == Frame.HEADER_POS: + if ord(byte) != Frame.HEADER: + self.index = 0 + #print('parse: header %02x' % ord(byte)) + + elif pos == Frame.BITS_POS: + self.bits_sns = (ord(byte) & Frame.SNS_MASK) >> Frame.SNS_SHIFT + self.bits_typ = (ord(byte) & Frame.TYP_MASK) >> Frame.TYP_SHIFT + if self.bits_sns >= Frame.SNS_NUM: + self.index = 0 + if self.bits_typ >= Frame.TYP_NUM: + self.index = 0 + #print('parse: bits sns=%d typ=%d' % + # (self.bits_sns, self.bits_typ)) + + elif pos == Frame.COUNT_POS: + wordsize = Frame.sizeMap[self.bits_typ] + self.count = ord(byte) + self.length = Frame.DATA_POS + self.count*wordsize + 1 + #print('parse: count cnt=%d len=%d' % + # (self.count, self.length)) + + elif pos < self.length-1: + self.binary += byte + #print('parse: data %02x @%d' % + # (ord(byte), pos-Frame.DATA_POS)) + + elif pos == self.length-1: + #print('parse: tail %02x' % ord(byte)) + if ord(byte) == Frame.TAIL: + state = self.convert() + else: + state = None + self.binary = "" + self.index = 0 + return state + + elif pos > self.length-1: + print('Error parsing') + + # Convert frame to state + def convert(self): + # Covnert data + fmt = Frame.fmtMap[self.bits_typ] * self.count + sns = Frame.snsMap[self.bits_sns] + self.values = unpack('<'+fmt, self.binary) + print('convert: %3s = \'%3s\'%%[%s] -> [%s]' % + (sns, fmt, hexDump(self.binary), fltDump(self.values))) + + # Create state + state = State() + setattr(state, sns, self.values) + return state class Device: # Constructors def __init__(self, config): - print("IN") + print('Defice.__init__') self.config = config self.serial = None + self.frame = Frame() # Methods def connect(self): - print("C") + print('Device.connect') buildingFrame = 0 try: self.inbuf = [] @@ -69,13 +179,16 @@ class Device: bytesize = self.config.databits, \ stopbits = self.config.stopbits, \ timeout = 0) - self.control() + for sns in self.config.rate: + self.set_rate(sns, self.config.rate[sns]) + for sns in self.config.enable: + self.set_enable(sns, self.config.enable[sns]) self.serial.flushInput() except Exception as ex: return str(ex) def disconnect(self): - print("DC") + print('Device.disconnect') if self.serial and self.serial.isOpen(): self.serial.close() @@ -86,71 +199,46 @@ class Device: return False return True - def control(self): - print("CT") - frame = [None]*7 - for key in list(self.config.enable.keys()): - state = self.config.enable[key] - rate = self.config.rate[key] - cmd = Const.cmdCode['start'] if state else Const.cmdCode['stop'] - sns = Const.snsCode[key] - frame[0] = chr(Const.HEADER) - frame[1] = chr(cmd<<(Const.SNS_BITS)|(0x1F&sns)) - frame[2:6] = pack('f',float(rate)) - frame[6] = Const.TAIL - self.serial.write(frame) - print('[SEND1] ',frame) - frame[1] = Const.cmdCode['set']<<(Const.SNS_BITS)|(0x1F&sns) - self.serial.write(frame) - print('[SEND2] ',frame) - self.serial.flush() + def set_rate(self, sensor, interval): + sns = Frame.findCode(Frame.snsMap, sensor) + bits = (sns << Frame.SNS_SHIFT) | \ + (Frame.RATE << Frame.OPER_SHIFT) + self._write_binary('Bf', bits, interval) + def set_enable(self, sensor, enabled): + sns = Frame.findCode(Frame.snsMap, sensor) + oper = Frame.START if enabled else Frame.STOP + bits = (sns << Frame.SNS_SHIFT) | \ + (oper << Frame.OPER_SHIFT) + self._write_binary('B', bits) def process(self): items = [] count = 0 limit = 1000 + if not self.running(): - return items; - if self.serial.readable(): - buildingFrame = 0 - while (self.serial.inWaiting() or count<(self.frame_len(self.inbuf)-1)): ###### - char = self.serial.read() - count +=1 - if char == chr(Const.TAIL) and buildingFrame and (len(self.inbuf))== self.frame_len(self.inbuf)-1: - self.inbuf.append(char) - #print("[TAIL]") - line = "".join(self.inbuf) - print(self.inbuf) - time.sleep(0.001) - #self.printHex(line) - item = self._parse_ascii(line) # analyze the received data - items.append(item) - buildingFrame = 0 # finished building one frame - #print ("BF set to 0") - self.inbuf = [] - count = 0 - elif char == chr(Const.HEADER) and buildingFrame==0: - self.inbuf = [] - buildingFrame = 1 - #print ("BF set to 1") - self.inbuf.append(char) - #print("[HEADER]") - #count +=1 - elif buildingFrame: - self.inbuf.append(char) - #print("[DT]") - #count +=1 - else: - #print("[ERROR] Byte Going Nowhere") - count = 0 - buildingFrame = 0 # reset the construction - #print ("BF set to 0!") - self.inbuf = [] + return items + + while self.serial.readable(): + try: + byte = self.serial.read() + except Exception as ex: + # Not sure why this is excepting + # if it says it's readable + break + if len(byte) == 0: + break + + state = self.frame.parse(byte) + if state: + items.append(state) if count > limit: - count = 0 - print("[ERROR] Exceeded Read Limit") + print('[ERROR] Exceeded Read Limit') break + + count += 1 + return items @@ -160,55 +248,34 @@ class Device: return -1 dataType_snsType = ord(frame[1]) dataNum = ord(frame[2]) - dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS - snsType = 0x1F&dataType_snsType - #print(dataType_snsType) - #print(dataType) - #print(snsType) - #print(dataNum) - dataSize = Const.sizeMap[Const.typeMap[dataType]] + dataType = (dataType_snsType & Frame.TYP_MASK) >> Frame.TYP_SHIFT + snsType = (dataType_snsType & Frame.SNS_MASK) >> Frame.SNS_SHIFT + dataSize = Frame.sizeMap[dataType] return (dataSize*dataNum+4) - def printHex(self, frame): - #print("PH") frameLen = self.frame_len(frame) i = 0 while i>Const.SNS_BITS - snsType = 0x1F&dataType_snsType - state = State() - if snsType == Const.ACC_SNS: - line = line[3:15] - state.acc = unpack('3f',line) - elif snsType == Const.MAG_SNS: - line = line[3:9] - state.mag = unpack('3h',line) - elif snsType == Const.LGT_SNS: - state.light[0] = ord(line[3]) - elif snsType == Const.TCH_SNS: - line = line[3:11] - state.touch = sunpack('2f',line) - elif snsType == Const.ADC_SNS_1: - line = line[3:15] - state.a2d = unpack('6h', line) - else: - print('[ERROR] Nothing Happened!') - return state +def hexDump(frame): + digits = ['%02x' % ord(byte) for byte in frame] + return ' '.join(digits) + +def fltDump(data): + digits = ['%5.2f' % flt for flt in data] + return ' '.join(digits) diff --git a/vis/visual.py b/vis/visual.py index 5580b5b..ac42208 100644 --- a/vis/visual.py +++ b/vis/visual.py @@ -33,8 +33,8 @@ class Visual: self.accs = get_objects(['accx', 'accy', 'accz']) self.mags = get_objects(['magx', 'magy', 'magz']) - self.touch = get_objects(['touch']) - self.light = get_objects(['light']) + self.lgts = get_objects(['lgt']) + self.tchs = get_objects(['tch']) self.a2ds = get_objects(['a2d0', 'a2d1', 'a2d2', 'a2d3', 'a2d4', 'a2d5']) # Signal handlers @@ -68,8 +68,8 @@ class Visual: name = name.replace('_btn', '') state = obj.get_active() if self.config.enable[name] != state: + self.device.set_enable(name, state) self.config.enable[name] = state - self.device.control() self.config.save() def on_rate(self, obj): @@ -77,8 +77,8 @@ class Visual: name = name.replace('_spin', '') value = obj.get_value() if self.config.rate[name] != value: + self.device.set_rate(name, value) self.config.rate[name] = value - self.device.control() self.config.save() def on_config(self, obj): @@ -136,16 +136,16 @@ class Visual: setxyz(self.accs, state.acc) setxyz(self.mags, state.mag) - setabs(self.touch, state.touch) - setabs(self.light, state.light) + setabs(self.lgts, state.lgt) + setabs(self.tchs, state.tch) setabs(self.a2ds, state.a2d) def debug(self, state): print('update: ' + str(state)) print('\tacc - ' + str(state.acc)) print('\tmag - ' + str(state.mag)) - print('\tlgt - ' + str(state.light)) - print('\ttch - ' + str(state.touch)) + print('\tlgt - ' + str(state.lgt)) + print('\ttch - ' + str(state.tch)) print('\ta2d - ' + str(state.a2d)) def timer(self): diff --git a/vis/visual.ui b/vis/visual.ui index 399dc9b..a9f1557 100644 --- a/vis/visual.ui +++ b/vis/visual.ui @@ -51,7 +51,7 @@ gtk-floppy - + 100 1 1 @@ -458,7 +458,7 @@ 1 10 - + 100 1 1 @@ -613,7 +613,7 @@ - + Tounch Sensor True True @@ -629,7 +629,7 @@ - + Light Sensor True True @@ -740,7 +740,7 @@ - + True True ● @@ -748,7 +748,7 @@ False True True - touch_adj + tch_adj 5 True @@ -762,7 +762,7 @@ - + True True ● @@ -770,7 +770,7 @@ False True True - light_adj + lgt_adj 5 True @@ -1032,7 +1032,7 @@ - + True False 0 @@ -1047,7 +1047,7 @@ - + True False 0 @@ -1090,7 +1090,7 @@ - + True False 0 @@ -1104,7 +1104,7 @@ - + True False 0 diff --git a/yue/main.cpp b/yue/main.cpp index c82b9ca..0b42bfd 100644 --- a/yue/main.cpp +++ b/yue/main.cpp @@ -33,6 +33,10 @@ #define MAX_FRAME_SIZE 256 +#define SNS_NUM 5 +#define TYPE_NUM 4 +#define OPER_NUM 3 + typedef enum { ACC_SNS, MAG_SNS, @@ -49,31 +53,31 @@ typedef enum { } type_t; typedef enum { - START, STOP, - SET_INT, + START, + RATE, } oper_t; // Data Frame Information typedef struct { uint8_t header; struct { - uint8_t sns : 5; - uint8_t type : 3; + uint8_t type : 4; + uint8_t sns : 4; } bits; uint8_t count; uint8_t data[]; -} state_t; +} __attribute__((__packed__)) state_t; // Command Frame Information typedef struct { uint8_t header; struct { - uint8_t sns : 5; - uint8_t oper : 3; + uint8_t oper : 4; + uint8_t sns : 4; } bits; float interval; -} command_t; +} __attribute__((__packed__)) command_t; // Define Devices & Pins MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS); @@ -104,9 +108,9 @@ bool tchEnable = false; bool a2dEnable = false; bool ledEnable = true; -bool useStr = true; +bool useStr = false; bool useHex = false; -bool useBin = false; +bool useBin = true; uint8_t txFrame[MAX_FRAME_SIZE]; uint8_t rxFrame[MAX_FRAME_SIZE]; @@ -115,6 +119,8 @@ uint8_t rxFrame[MAX_FRAME_SIZE]; void clock1_interrupt(void); void serialRx_interrupt(void); +void runCommand(command_t *cmd); + void sendAccInfo(void); void sendMagInfo(void); void sendLgtInfo(void); @@ -154,83 +160,44 @@ int main(void) { **********************/ void serialRx_interrupt(void) { // Byte version - clock1.detach(); // close the interrupt temporarily - int i = 0; - uint8_t ch = serial.getc(); - - while (ch!=HEADER){ - if (serial.readable()){ - ch = serial.getc(); - } - else{ - printStr("[ERROR] broken data!\r\n"); - clock1.attach(&clock1_interrupt, TIME_ACCURACY); - return; + static int state = 0; + static int index = 0; + static int length = 0; + + command_t *cmd = (command_t*)rxFrame; + + while (serial.readable()) { + rxFrame[index++] = serial.getc(); + switch (state) { + case 0: // Header + if (cmd->header == HEADER) + state = 1; + else + index = 0; + break; + case 1: // Bits + if (cmd->bits.sns >= SNS_NUM || + cmd->bits.oper >= OPER_NUM) { + state = index = 0; + } else if (cmd->bits.oper == RATE) { + length = 4; + state = 2; + } else { + state = 3; + } + break; + case 2: // Data + if (--length == 0) + state = 3; + break; + case 3: // Tail + if (rxFrame[index-1] == TAIL) + runCommand(cmd); + state = 0; + index = 0; + break; } } - - //TODO: ticker for time out - while (serial.readable()){ - rxFrame[i] = ch; - if (ch=='\n') - break; - ch = serial.getc(); - i++; - } - rxFrame[++i] = '\0'; - - // Cast to command and - command_t *cmd = (command_t *)rxFrame; - - // Validate interval - sns_t snsType = (sns_t )cmd->bits.sns; - oper_t operType = (oper_t)cmd->bits.oper; - float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE); - - // Save value to global data - switch(snsType){ - case ACC_SNS: - switch(operType){ - case START: accEnable = true; break; - case STOP: accEnable = false; break; - case SET_INT: accTmr = interval; break; - } - break; - - case MAG_SNS: - switch(operType){ - case START: magEnable = true; break; - case STOP: magEnable = false; break; - case SET_INT: magTmr = interval; break; - } - break; - - case LGT_SNS: - switch(operType){ - case START: lgtEnable = true; break; - case STOP: lgtEnable = false; break; - case SET_INT: lgtTmr = interval; break; - } - break; - - case TCH_SNS: - switch(operType){ - case START: tchEnable = true; break; - case STOP: tchEnable = false; break; - case SET_INT: tchTmr = interval; break; - } - break; - - case A2D_SNS: - switch(operType){ - case START: a2dEnable = true; break; - case STOP: a2dEnable = false; break; - case SET_INT: a2dTmr = interval; break; - } - break; - } - - clock1.attach(&clock1_interrupt, TIME_ACCURACY); } void clock1_interrupt(void){ @@ -279,6 +246,61 @@ void clock1_interrupt(void){ sdma_flush(); } +/******************* + * Command handler * + *******************/ + +void runCommand(command_t *cmd) +{ + // Validate interval + sns_t snsType = (sns_t )cmd->bits.sns; + oper_t operType = (oper_t)cmd->bits.oper; + float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE); + + // Save value to global data + switch(snsType){ + case ACC_SNS: + switch(operType){ + case STOP: accEnable = false; break; + case START: accEnable = true; break; + case RATE: accTmr = interval; break; + } + break; + + case MAG_SNS: + switch(operType){ + case STOP: magEnable = false; break; + case START: magEnable = true; break; + case RATE: magTmr = interval; break; + } + break; + + case LGT_SNS: + switch(operType){ + case STOP: lgtEnable = false; break; + case START: lgtEnable = true; break; + case RATE: lgtTmr = interval; break; + } + break; + + case TCH_SNS: + switch(operType){ + case STOP: tchEnable = false; break; + case START: tchEnable = true; break; + case RATE: tchTmr = interval; break; + } + break; + + case A2D_SNS: + switch(operType){ + case STOP: a2dEnable = false; break; + case START: a2dEnable = true; break; + case RATE: a2dTmr = interval; break; + } + break; + } +} + /******************* * Sensors reading * *******************/ @@ -337,7 +359,7 @@ void sendA2dInfo(void){ a2dData[3] = 0; a2dData[4] = 0; a2dData[5] = 0; - int len = packToFrame(txFrame, A2D_SNS, FLOAT, 2, a2dData); + int len = packToFrame(txFrame, A2D_SNS, FLOAT, 6, a2dData); printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n", a2dData[0], a2dData[1], a2dData[2], @@ -350,7 +372,7 @@ void sendA2dInfo(void){ * Helper functions * ********************/ -int calDataSize(uint8_t dataType){ +int calDataSize(type_t dataType){ switch(dataType){ case INT: return 2; case LONG: return 4; @@ -365,7 +387,12 @@ int calDataSize(uint8_t dataType){ *******************/ int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data){ - int size = dataNum + calDataSize(dataType); + //const char pattern[] = "\x80\x81\x82\x83" + // "\x84\x85\x86\x87" + // "\x88\x89\x8A\x8B" + // "\x8C\x8D\x8E\x8F"; + + int size = dataNum * calDataSize(dataType); state_t *state = (state_t*)frame; uint8_t *tail = &state->data[size]; -- 2.43.2