'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,
},
}
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)
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)
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 = []
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()
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
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<frameLen:
print(hex(ord(frame[i])))
i+=1
-
# Private methods
- def _write_ascii(self, line): # to be changed
- #print("WA")
+ def _write_binary(self, fmt, *args):
+ #print('Device._write_binary')
if self.serial:
- print('write: [' + line + ']')
- self.serial.write(line + '\n')
+ fmt = 'B' + fmt + 'B'
+ args = [Frame.HEADER] + list(args) + [Frame.TAIL]
+ frame = pack('<'+fmt, *args)
+ print('write: bin:[' + hexDump(frame) + ']')
+ self.serial.write(frame)
self.serial.flush()
time.sleep(0.1)
- def _parse_ascii(self, line):##############
- #print("PA")
- dataType_snsType = ord(line[1]);
- dataNum = ord(line[2]);
- dataType = (0xE0&dataType_snsType)>>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)
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
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):
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):
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):
<property name="stock_id">gtk-floppy</property>
<signal name="activate" handler="on_flush" swapped="no"/>
</object>
- <object class="GtkAdjustment" id="light_adj">
+ <object class="GtkAdjustment" id="lgt_adj">
<property name="upper">100</property>
<property name="value">1</property>
<property name="step_increment">1</property>
<property name="step_increment">1</property>
<property name="page_increment">10</property>
</object>
- <object class="GtkAdjustment" id="touch_adj">
+ <object class="GtkAdjustment" id="tch_adj">
<property name="upper">100</property>
<property name="value">1</property>
<property name="step_increment">1</property>
</packing>
</child>
<child>
- <object class="GtkCheckButton" id="touch_btn">
+ <object class="GtkCheckButton" id="tch_btn">
<property name="label" translatable="yes">Tounch Sensor</property>
<property name="visible">True</property>
<property name="can_focus">True</property>
</packing>
</child>
<child>
- <object class="GtkCheckButton" id="light_btn">
+ <object class="GtkCheckButton" id="lgt_btn">
<property name="label" translatable="yes">Light Sensor</property>
<property name="visible">True</property>
<property name="can_focus">True</property>
</packing>
</child>
<child>
- <object class="GtkSpinButton" id="touch_spin">
+ <object class="GtkSpinButton" id="tch_spin">
<property name="visible">True</property>
<property name="can_focus">True</property>
<property name="invisible_char">●</property>
<property name="secondary_icon_activatable">False</property>
<property name="primary_icon_sensitive">True</property>
<property name="secondary_icon_sensitive">True</property>
- <property name="adjustment">touch_adj</property>
+ <property name="adjustment">tch_adj</property>
<property name="digits">5</property>
<property name="numeric">True</property>
<signal name="value-changed" handler="on_rate" swapped="no"/>
</packing>
</child>
<child>
- <object class="GtkSpinButton" id="light_spin">
+ <object class="GtkSpinButton" id="lgt_spin">
<property name="visible">True</property>
<property name="can_focus">True</property>
<property name="invisible_char">●</property>
<property name="secondary_icon_activatable">False</property>
<property name="primary_icon_sensitive">True</property>
<property name="secondary_icon_sensitive">True</property>
- <property name="adjustment">light_adj</property>
+ <property name="adjustment">lgt_adj</property>
<property name="digits">5</property>
<property name="numeric">True</property>
<signal name="value-changed" handler="on_rate" swapped="no"/>
</packing>
</child>
<child>
- <object class="GtkLabel" id="touch">
+ <object class="GtkLabel" id="tch">
<property name="visible">True</property>
<property name="can_focus">False</property>
<property name="xalign">0</property>
</packing>
</child>
<child>
- <object class="GtkLabel" id="light">
+ <object class="GtkLabel" id="lgt">
<property name="visible">True</property>
<property name="can_focus">False</property>
<property name="xalign">0</property>
</packing>
</child>
<child>
- <object class="GtkLabel" id="touch_lbl">
+ <object class="GtkLabel" id="tch_lbl">
<property name="visible">True</property>
<property name="can_focus">False</property>
<property name="xalign">0</property>
</packing>
</child>
<child>
- <object class="GtkLabel" id="light_lbl">
+ <object class="GtkLabel" id="lgt_lbl">
<property name="visible">True</property>
<property name="can_focus">False</property>
<property name="xalign">0</property>
\r
#define MAX_FRAME_SIZE 256\r
\r
+#define SNS_NUM 5\r
+#define TYPE_NUM 4\r
+#define OPER_NUM 3\r
+\r
typedef enum {\r
ACC_SNS,\r
MAG_SNS,\r
} type_t;\r
\r
typedef enum {\r
- START,\r
STOP,\r
- SET_INT,\r
+ START,\r
+ RATE,\r
} oper_t;\r
\r
// Data Frame Information\r
typedef struct {\r
uint8_t header;\r
struct {\r
- uint8_t sns : 5;\r
- uint8_t type : 3;\r
+ uint8_t type : 4;\r
+ uint8_t sns : 4;\r
} bits;\r
uint8_t count;\r
uint8_t data[];\r
-} state_t;\r
+} __attribute__((__packed__)) state_t;\r
\r
// Command Frame Information\r
typedef struct {\r
uint8_t header;\r
struct {\r
- uint8_t sns : 5;\r
- uint8_t oper : 3;\r
+ uint8_t oper : 4;\r
+ uint8_t sns : 4;\r
} bits;\r
float interval;\r
-} command_t;\r
+} __attribute__((__packed__)) command_t;\r
\r
// Define Devices & Pins\r
MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
bool a2dEnable = false;\r
bool ledEnable = true;\r
\r
-bool useStr = true;\r
+bool useStr = false;\r
bool useHex = false;\r
-bool useBin = false;\r
+bool useBin = true;\r
\r
uint8_t txFrame[MAX_FRAME_SIZE];\r
uint8_t rxFrame[MAX_FRAME_SIZE];\r
void clock1_interrupt(void);\r
void serialRx_interrupt(void);\r
\r
+void runCommand(command_t *cmd);\r
+\r
void sendAccInfo(void);\r
void sendMagInfo(void);\r
void sendLgtInfo(void);\r
**********************/\r
\r
void serialRx_interrupt(void) { // Byte version\r
- clock1.detach(); // close the interrupt temporarily\r
- int i = 0;\r
- uint8_t ch = serial.getc();\r
-\r
- while (ch!=HEADER){\r
- if (serial.readable()){\r
- ch = serial.getc();\r
- }\r
- else{\r
- printStr("[ERROR] broken data!\r\n");\r
- clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
- return;\r
+ static int state = 0;\r
+ static int index = 0;\r
+ static int length = 0;\r
+\r
+ command_t *cmd = (command_t*)rxFrame;\r
+\r
+ while (serial.readable()) {\r
+ rxFrame[index++] = serial.getc();\r
+ switch (state) {\r
+ case 0: // Header\r
+ if (cmd->header == HEADER)\r
+ state = 1;\r
+ else\r
+ index = 0;\r
+ break;\r
+ case 1: // Bits\r
+ if (cmd->bits.sns >= SNS_NUM ||\r
+ cmd->bits.oper >= OPER_NUM) {\r
+ state = index = 0;\r
+ } else if (cmd->bits.oper == RATE) {\r
+ length = 4;\r
+ state = 2;\r
+ } else {\r
+ state = 3;\r
+ }\r
+ break;\r
+ case 2: // Data\r
+ if (--length == 0)\r
+ state = 3;\r
+ break;\r
+ case 3: // Tail\r
+ if (rxFrame[index-1] == TAIL)\r
+ runCommand(cmd);\r
+ state = 0;\r
+ index = 0;\r
+ break;\r
}\r
}\r
-\r
- //TODO: ticker for time out\r
- while (serial.readable()){\r
- rxFrame[i] = ch;\r
- if (ch=='\n')\r
- break;\r
- ch = serial.getc();\r
- i++;\r
- }\r
- rxFrame[++i] = '\0';\r
-\r
- // Cast to command and\r
- command_t *cmd = (command_t *)rxFrame;\r
-\r
- // Validate interval\r
- sns_t snsType = (sns_t )cmd->bits.sns;\r
- oper_t operType = (oper_t)cmd->bits.oper;\r
- float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);\r
-\r
- // Save value to global data\r
- switch(snsType){\r
- case ACC_SNS:\r
- switch(operType){\r
- case START: accEnable = true; break;\r
- case STOP: accEnable = false; break;\r
- case SET_INT: accTmr = interval; break;\r
- }\r
- break;\r
-\r
- case MAG_SNS:\r
- switch(operType){\r
- case START: magEnable = true; break;\r
- case STOP: magEnable = false; break;\r
- case SET_INT: magTmr = interval; break;\r
- }\r
- break;\r
-\r
- case LGT_SNS:\r
- switch(operType){\r
- case START: lgtEnable = true; break;\r
- case STOP: lgtEnable = false; break;\r
- case SET_INT: lgtTmr = interval; break;\r
- }\r
- break;\r
-\r
- case TCH_SNS:\r
- switch(operType){\r
- case START: tchEnable = true; break;\r
- case STOP: tchEnable = false; break;\r
- case SET_INT: tchTmr = interval; break;\r
- }\r
- break;\r
-\r
- case A2D_SNS:\r
- switch(operType){\r
- case START: a2dEnable = true; break;\r
- case STOP: a2dEnable = false; break;\r
- case SET_INT: a2dTmr = interval; break;\r
- }\r
- break;\r
- }\r
-\r
- clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
}\r
\r
void clock1_interrupt(void){\r
sdma_flush();\r
}\r
\r
+/*******************\r
+ * Command handler *\r
+ *******************/\r
+\r
+void runCommand(command_t *cmd)\r
+{\r
+ // Validate interval\r
+ sns_t snsType = (sns_t )cmd->bits.sns;\r
+ oper_t operType = (oper_t)cmd->bits.oper;\r
+ float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);\r
+\r
+ // Save value to global data\r
+ switch(snsType){\r
+ case ACC_SNS:\r
+ switch(operType){\r
+ case STOP: accEnable = false; break;\r
+ case START: accEnable = true; break;\r
+ case RATE: accTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case MAG_SNS:\r
+ switch(operType){\r
+ case STOP: magEnable = false; break;\r
+ case START: magEnable = true; break;\r
+ case RATE: magTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case LGT_SNS:\r
+ switch(operType){\r
+ case STOP: lgtEnable = false; break;\r
+ case START: lgtEnable = true; break;\r
+ case RATE: lgtTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case TCH_SNS:\r
+ switch(operType){\r
+ case STOP: tchEnable = false; break;\r
+ case START: tchEnable = true; break;\r
+ case RATE: tchTmr = interval; break;\r
+ }\r
+ break;\r
+\r
+ case A2D_SNS:\r
+ switch(operType){\r
+ case STOP: a2dEnable = false; break;\r
+ case START: a2dEnable = true; break;\r
+ case RATE: a2dTmr = interval; break;\r
+ }\r
+ break;\r
+ }\r
+}\r
+\r
/*******************\r
* Sensors reading *\r
*******************/\r
a2dData[3] = 0;\r
a2dData[4] = 0;\r
a2dData[5] = 0;\r
- int len = packToFrame(txFrame, A2D_SNS, FLOAT, 2, a2dData);\r
+ int len = packToFrame(txFrame, A2D_SNS, FLOAT, 6, a2dData);\r
\r
printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",\r
a2dData[0], a2dData[1], a2dData[2],\r
* Helper functions *\r
********************/\r
\r
-int calDataSize(uint8_t dataType){\r
+int calDataSize(type_t dataType){\r
switch(dataType){\r
case INT: return 2;\r
case LONG: return 4;\r
*******************/\r
\r
int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data){\r
- int size = dataNum + calDataSize(dataType);\r
+ //const char pattern[] = "\x80\x81\x82\x83"\r
+ // "\x84\x85\x86\x87"\r
+ // "\x88\x89\x8A\x8B"\r
+ // "\x8C\x8D\x8E\x8F";\r
+\r
+ int size = dataNum * calDataSize(dataType);\r
state_t *state = (state_t*)frame;\r
uint8_t *tail = &state->data[size];\r
\r