from re import compile
from serial import Serial
from datetime import datetime
+from struct import *
-class State:
+
+
+#buildingFrame = 0; #if it is a start of new frame
+
+class Const:
+ HEADER = 0x02
+ SNS_BITS = 5
+ LGT_SNS = 0x00
+ ACC_SNS = 0x01
+ MAG_SNS = 0x02
+ TCH_SNS = 0x03
+ ADC_SNS_1 = 0x04
+
+ INT = 0
+ LONG = 1
+ FLOAT = 2
+ DOUBLE = 3
+ TAIL = 0x0A
+
+ START = 0
+ STOP = 1
+ SET_INT = 2
+ buildingFrame = 0
+
+ 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}
+
+class Static:
+
+ count = 0
+
+
+class State: #information stored
acc = [None]*3
mag = [None]*3
touch = [None]*2
self.time = datetime.utcnow()
class Device:
- # Attributes
- port = "/dev/ttyACM0"
-
# Constructors
def __init__(self, config):
+ print("IN")
self.config = config
self.serial = None
# Methods
def connect(self):
+ print("C")
+ buildingFrame = 0
try:
self.inbuf = []
- self.serial = Serial(self.port, \
+ self.serial = Serial(self.config.device, \
baudrate = self.config.baudrate, \
parity = self.config.parity, \
bytesize = self.config.databits, \
return str(ex)
def disconnect(self):
+ print("DC")
if self.serial and self.serial.isOpen():
self.serial.close()
- def running(self):
+ def running(self): # isRunning
if self.serial == None:
return False
if self.serial.isOpen() == False:
return False
return True
- def control(self):
+ 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 = 'start' if state else 'stop'
- self._write_ascii('%s %s' % (cmd, key))
- self._write_ascii('set %s int %d' % (key, rate))
+ 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 process(self):
items = []
count = 0
limit = 1000
- if not self.running():
- return items
- while self.serial.readable():
- try:
- char = self.serial.read().decode()
- except Exception as ex:
- char = ''
- if len(char) == 0:
- break
- if char == '\r' or char == '\n':
- if len(self.inbuf) == 0:
- continue
- line = "".join(self.inbuf)
- print('read: [' + line + ']')
- item = self._parse_ascii(line)
- items.append(item)
- self.inbuf = []
- else:
- self.inbuf.append(char)
- if count > limit:
- print("Error: exceeded read limit")
- break
- count += 1
- return items
+ 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 = []
+ if count > limit:
+ count = 0
+ print("[ERROR] Exceeded Read Limit")
+ break
+ return items
+
+
+ # auxilary function
+ def frame_len(self, frame):
+ if len(frame) < 3:
+ 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]]
+ 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):
+ def _write_ascii(self, line): # to be changed
+ #print("WA")
if self.serial:
print('write: [' + line + ']')
- data = bytes(line+'\n', 'UTF-8')
- self.serial.write(data)
+ self.serial.write(line + '\n')
self.serial.flush()
time.sleep(0.1)
- def _parse_ascii(self, line):
- acc_re = compile("\[ACC\] accX=(.*) accY=(.*) accZ=(.*)")
- mag_re = compile("\[MAG\] magX=(.*) magY=(.*) magZ=(.*)")
- lgt_re = compile("\[LGT\] Light Intensity=(.*)")
- tch_re = compile("\[TCH\] Force=(.*) Distance=(.*)")
- a2d_re = compile("\[A2D\] ...")
-
- acc_m = acc_re.match(line)
- mag_m = mag_re.match(line)
- lgt_m = lgt_re.match(line)
- tch_m = tch_re.match(line)
- a2d_m = a2d_re.match(line)
-
- state = State()
- if acc_m:
- state.acc[0] = float(acc_m.group(1))
- state.acc[1] = float(acc_m.group(2))
- state.acc[2] = float(acc_m.group(3))
- if mag_m:
- state.mag[0] = float(mag_m.group(1))
- state.mag[1] = float(mag_m.group(2))
- state.mag[2] = float(mag_m.group(3))
- if lgt_m:
- state.light[0] = float(lgt_m.group(1))
- if tch_m:
- state.touch[0] = float(tch_m.group(1))
- if a2d_m:
- state.a2d[0] = float(tch_m.group(1))
- state.a2d[1] = float(tch_m.group(2))
- state.a2d[2] = float(tch_m.group(3))
- state.a2d[3] = float(tch_m.group(4))
- state.a2d[4] = float(tch_m.group(5))
- state.a2d[5] = float(tch_m.group(6))
-
- return state
+ 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
-#include "mbed.h"
-#include "TSISensor.h" // Touch Sensor
-#include "MAG3110.h" // Magnetic Sensor
-#include "MMA8451Q.h" // AcceleroMeter
-
-
-#define MMA8451_I2C_ADDRESS (0x1d<<1) // acc sensor address
-#define TIME_ACCURACY 0.1
-#define LIGHT_SNS_DEFAULT 1 // default collecting interval in seconds
-#define ACC_SNS_DEFAULT 1
-#define MAG_SNS_DEFAULT 1
-#define TOUCH_SNS_DEFAULT 1
-
-// Define Devices & Pins
-MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
-TSISensor touchSensor;
-MAG3110 magSensor(PTE25, PTE24);
-Serial serial(USBTX, USBRX);
-Ticker clock1;
-AnalogIn lightSensor(PTE22);
-
-
-// Global Variables
-// Initial interval: in seconds
-float lightTmr = LIGHT_SNS_DEFAULT;
-float accTmr = ACC_SNS_DEFAULT;
-float magTmr = MAG_SNS_DEFAULT;
-float touchTmr = TOUCH_SNS_DEFAULT;
-
-bool lightEnable = true;
-bool accEnable = true;
-bool magEnable = true;
-bool touchEnable = true;
-
-void clock1_interrupt();
-void serialRx_interrupt();
-void sendLightInfo();
-void sendAccInfo();
-void sendMagInfo();
-void sendTouchInfo();
-
-
-int main() {
- // Initialization
- // Interruption Declarations
- clock1.attach(&clock1_interrupt, TIME_ACCURACY); // maximun accuracy be 0.1s
- serial.attach(&serialRx_interrupt, Serial::RxIrq); // receive interrupt for serialS
- serial.baud(921600);
- magSensor.begin();
-
- serial.printf("\r\n============= Start of the program ============\r\n");
- while(1){
- wait(3);
- }
-}
-
-
-/*---------------------------------------------------------------
- ## Receive Interruption of the Serial ##
- -> used to receive & process user command
- -> and configure the board accordingly
- ---------------------------------------------------------------*/
-void serialRx_interrupt(){
- clock1.detach(); // close the interrupt
- serial.printf("\r\n");
-
- // Receive the Serial Input
- float interval;
- char buffer[255];
- char temp[255];
- char ch = serial.getc();
- int i;
- for (i=0 ; ch!='\n' && ch!='\r'; i++){
- serial.putc(ch);
- buffer[i] = ch;
- if (ch==127){ // BackSpace
- i--;
- i--;
- }
- ch = serial.getc();
- }
- buffer[i] = '\0';
- serial.printf("\r\nBUFFER: %s %d\r\n", buffer,i);
- // TODO: buffer -> lower case
-
- // Process the Serial Input
- // Set-Interval Command
- if (strstr(buffer, "set")!=NULL && strstr(buffer, "int")!= NULL){
- sscanf(buffer, "%*[^0123456789.]%s", temp); // find the number in buffer
- sscanf(temp, "%f", &interval); // translate into float
- if (interval<0.1 || interval>5){
- interval = 1;
- }
- if (strstr(buffer, "acc")){
- accTmr = interval;
- }
- if (strstr(buffer, "mag")){
- magTmr = interval;
- }
- if (strstr(buffer, "light")){
- lightTmr = interval;
- }
- if (strstr(buffer, "touch")){
- touchTmr = interval;
- }
- }
-
- // Stop Command
- else if (strstr(buffer, "stop")!= NULL){
- serial.printf("STOP\r\n");
- if (strstr(buffer, "acc")){
- accEnable = false;
- accTmr = ACC_SNS_DEFAULT;
- }
- if (strstr(buffer, "mag")){
- magEnable = false;
- magTmr = MAG_SNS_DEFAULT;
- }
- if (strstr(buffer, "light")){
- lightEnable = false;
- lightTmr = LIGHT_SNS_DEFAULT;
- }
- if (strstr(buffer, "touch")){
- touchEnable = false;
- touchTmr = TOUCH_SNS_DEFAULT;
- }
- }
-
- // Start Command
- else if (strstr(buffer, "start")!=NULL){
- if (strstr(buffer, "acc") && !accEnable){
- accEnable = true;
- accTmr = ACC_SNS_DEFAULT;
- }
- if (strstr(buffer, "mag") && !magEnable){
- magEnable = true;
- magTmr = MAG_SNS_DEFAULT;
- }
- if (strstr(buffer, "light") && !lightEnable){
- lightEnable = true;
- lightTmr = LIGHT_SNS_DEFAULT;
- }
- if (strstr(buffer, "touch") && !touchEnable){
- touchEnable = true;
- touchTmr = TOUCH_SNS_DEFAULT;
- }
- }
- clock1.attach(&clock1_interrupt,0.1);
-}
-
-
-void clock1_interrupt(){
- static int accCnt;
- static int magCnt;
- static int lightCnt;
- static int touchCnt;
-
- accCnt++;
- magCnt++;
- lightCnt++;
- touchCnt++;
-
- // TODO: send data through Serial
- if (lightEnable && (lightCnt<0 || lightCnt>=lightTmr/TIME_ACCURACY)){
- sendLightInfo();
- lightCnt = 0;
- }
- if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){
- sendMagInfo();
- magCnt = 0;
- }
- if (touchEnable && (touchCnt<0 || touchCnt>=touchTmr/TIME_ACCURACY)){
- sendTouchInfo();
- touchCnt = 0;
- }
- if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
- sendAccInfo();
- accCnt = 0;
- }
-}
-
-void sendLightInfo(){
- serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());
-}
-
-
-void sendAccInfo(){
- // get acc data
- float accX = accSensor.getAccX();
- float accY = accSensor.getAccY();
- float accZ = accSensor.getAccZ();
-
- // send acc data
- serial.printf("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",accX,accY,accZ);
-}
-
-void sendTouchInfo(){
- // get data
- float touchForce = touchSensor.readPercentage();
- float distance = touchSensor.readDistance();
-
- // send data
- serial.printf("[TCH] Force=%0.4f Distance=%2.2f\r\n", touchForce, distance);
-}
-
-void sendMagInfo(){
- // get data
- int magX, magY, magZ;
- magSensor.getValues(&magX, &magY, &magZ);
-
- // send data
- serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magX,magY,magZ);
-}
+#include "mbed.h"\r
+#include "TSISensor.h" // Touch Sensor\r
+#include "MAG3110.h" // Magnetic Sensor\r
+#include "MMA8451Q.h" // AcceleroMeter\r
+//#include "DataFrame.h"\r
+\r
+\r
+\r
+#define MMA8451_I2C_ADDRESS (0x1d<<1) // acc sensor address\r
+#define TIME_ACCURACY 0.1\r
+#define LGT_SNS_DEFAULT 2 // default collecting interval in seconds\r
+#define ACC_SNS_DEFAULT 2\r
+#define MAG_SNS_DEFAULT 2\r
+#define TCH_SNS_DEFAULT 2\r
+\r
+// Parameters in Data Frame\r
+// TODO: need to sync with the pc program\r
+\r
+#define HEADER 0x02\r
+\r
+#define SNS_BITS 5\r
+#define LGT_SNS 0x00\r
+#define ACC_SNS 0x01\r
+#define MAG_SNS 0x02\r
+#define TCH_SNS 0x03\r
+#define ADC_SNS_1 0x04\r
+\r
+#define INT 0\r
+#define LONG 1\r
+#define FLOAT 2\r
+#define DOUBLE 3\r
+#define TAIL 0x0A // '\n'\r
+\r
+// Command Frame\r
+#define START 0\r
+#define STOP 1\r
+#define SET_INT 2\r
+\r
+\r
+\r
+#define MAX_FRAME_LEN 255\r
+// Define Devices & Pins\r
+MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
+TSISensor touchSensor;\r
+MAG3110 magSensor(PTE25, PTE24);\r
+Serial serial(USBTX, USBRX);\r
+Ticker clock1;\r
+AnalogIn lightSensor(PTE22);\r
+\r
+\r
+// Global Variables\r
+// Initial interval: in seconds\r
+float lgtTmr = LGT_SNS_DEFAULT;\r
+float accTmr = ACC_SNS_DEFAULT;\r
+float magTmr = MAG_SNS_DEFAULT;\r
+float tchTmr = TCH_SNS_DEFAULT;\r
+\r
+bool lgtEnable = false;\r
+bool accEnable = false;\r
+bool magEnable = true;\r
+bool tchEnable = false;\r
+\r
+void clock1_interrupt();\r
+void serialRx_interrupt();\r
+void sendLightInfo();\r
+void sendAccInfo();\r
+void sendMagInfo();\r
+void sendTouchInfo();\r
+int packToFrame(char*, char, char, int, void*);\r
+char* floatToByte(float);\r
+float byteToFloat(char*);\r
+int calDataSize(char);\r
+void printHex(char*);\r
+void sendFrame(char*, int);\r
+\r
+\r
+int main() {\r
+ // Initialization\r
+ // Interruption Declarations\r
+ clock1.attach(&clock1_interrupt, TIME_ACCURACY); // maximun accuracy be 0.1s\r
+ serial.attach(&serialRx_interrupt, Serial::RxIrq); // receive interrupt for serialS\r
+ magSensor.begin();\r
+\r
+ serial.printf("\r\n============= Start of the program ============\r\n");\r
+ while(1){\r
+ wait(3);\r
+ }\r
+}\r
+\r
+\r
+/*---------------------------------------------------------------\r
+ ## Receive Interruption of the Serial ##\r
+ -> used to receive & process user command\r
+ -> and configure the board accordingly\r
+ ---------------------------------------------------------------*/\r
+/*void serialRx_interrupt_userFriendlyVersion(){\r
+ clock1.detach(); // close the interrupt\r
+ serial.printf("\r\n");\r
+\r
+ // Receive the Serial Input\r
+ float interval;\r
+ char buffer[255];\r
+ char temp[255];\r
+ char ch = serial.getc();\r
+ int i;\r
+ for (i=0 ; ch!='\n' && ch!='\r'; i++){\r
+ serial.putc(ch);\r
+ buffer[i] = ch;\r
+ if (ch==127){ // BackSpace\r
+ i--;\r
+ i--;\r
+ }\r
+ ch = serial.getc();\r
+ }\r
+ buffer[i] = '\0';\r
+ serial.printf("\r\nBUFFER: %s %d\r\n", buffer,i);\r
+ // TODO: buffer -> lower case\r
+\r
+ // Process the Serial Input\r
+ // Set-Interval Command\r
+ if (strstr(buffer, "set")!=NULL && strstr(buffer, "int")!= NULL){\r
+ sscanf(buffer, "%*[^0123456789.]%s", temp); // find the number in buffer\r
+ sscanf(temp, "%f", &interval); // translate into float\r
+ if (interval<0.1 || interval>5){\r
+ interval = 1;\r
+ }\r
+ if (strstr(buffer, "acc")){\r
+ accTmr = interval;\r
+ }\r
+ if (strstr(buffer, "mag")){\r
+ magTmr = interval;\r
+ }\r
+ if (strstr(buffer, "light")){\r
+ lgtTmr = interval;\r
+ }\r
+ if (strstr(buffer, "touch")){\r
+ tchTmr = interval;\r
+ }\r
+ }\r
+\r
+ // Stop Command\r
+ else if (strstr(buffer, "stop")!= NULL){\r
+ serial.printf("STOP\r\n");\r
+ if (strstr(buffer, "acc")){\r
+ accEnable = false;\r
+ accTmr = ACC_SNS_DEFAULT;\r
+ }\r
+ if (strstr(buffer, "mag")){\r
+ magEnable = false;\r
+ magTmr = MAG_SNS_DEFAULT;\r
+ }\r
+ if (strstr(buffer, "light")){\r
+ lgtEnable = false;\r
+ lgtTmr = LGT_SNS_DEFAULT;\r
+ }\r
+ if (strstr(buffer, "touch")){\r
+ tchEnable = false;\r
+ tchTmr = TCH_SNS_DEFAULT;\r
+ }\r
+ }\r
+\r
+ // Start Command\r
+ else if (strstr(buffer, "start")!=NULL){\r
+ if (strstr(buffer, "acc") && !accEnable){\r
+ accEnable = true;\r
+ accTmr = ACC_SNS_DEFAULT;\r
+ }\r
+ if (strstr(buffer, "mag") && !magEnable){\r
+ magEnable = true;\r
+ magTmr = MAG_SNS_DEFAULT;\r
+ }\r
+ if (strstr(buffer, "light") && !lgtEnable){\r
+ lgtEnable = true;\r
+ lgtTmr = LGT_SNS_DEFAULT;\r
+ }\r
+ if (strstr(buffer, "touch") && !tchEnable){\r
+ tchEnable = true;\r
+ tchTmr = TCH_SNS_DEFAULT;\r
+ }\r
+ }\r
+ clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+}\r
+*/\r
+\r
+void serialRx_interrupt(){ // Byte version\r
+ clock1.detach(); // close the interrupt temporarily\r
+ int i = 0;\r
+ char frame[MAX_FRAME_LEN];\r
+ char ch = serial.getc();\r
+ while (ch!=HEADER){\r
+ if (serial.readable()){\r
+ ch = serial.getc();\r
+ }\r
+ else{\r
+ serial.printf("[ERROR] broken data!\r\n");\r
+ clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+ return;\r
+ }\r
+ }\r
+ //TODO: ticker for time out\r
+ while (serial.readable()){\r
+ frame[i] = ch;\r
+ if (ch=='\n'){\r
+ break;\r
+ }\r
+ ch = serial.getc();\r
+ i++;\r
+ }\r
+ frame[++i] = '\0';\r
+ float interval=-1;\r
+ char snsType = frame[1]|0x1F;\r
+ char oper = (frame[1]|0xE0)>>SNS_BITS; // operation type\r
+ if (oper==SET_INT){\r
+ memcpy(&interval,&frame[2],4);\r
+ if (interval<0.0001||interval>10){\r
+ interval = 1;\r
+ }\r
+ }\r
+ switch(snsType){\r
+ case ACC_SNS:\r
+ {\r
+ switch(oper){\r
+ case START: accEnable = true; break;\r
+ case STOP: accEnable = false; break;\r
+ case SET_INT: accTmr = interval; break;\r
+ }\r
+ }\r
+ case MAG_SNS:\r
+ {\r
+ switch(oper){\r
+ case START: magEnable = true; break;\r
+ case STOP: magEnable = false; break;\r
+ case SET_INT: magTmr = interval; break;\r
+ }\r
+ }\r
+ case LGT_SNS:\r
+ {\r
+ switch(oper){\r
+ case START: lgtEnable = true; break;\r
+ case STOP: lgtEnable = false; break;\r
+ case SET_INT: lgtTmr = interval; break;\r
+ }\r
+ }\r
+ case TCH_SNS:\r
+ {\r
+ switch(oper){\r
+ case START: tchEnable = true; break;\r
+ case STOP: tchEnable = false; break;\r
+ case SET_INT: tchTmr = interval; break;\r
+ }\r
+ }\r
+ }\r
+ clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+}\r
+\r
+void clock1_interrupt(){\r
+ static int accCnt;\r
+ static int magCnt;\r
+ static int lgtCnt;\r
+ static int tchCnt;\r
+\r
+ accCnt++;\r
+ magCnt++;\r
+ lgtCnt++;\r
+ tchCnt++;\r
+\r
+ // TODO: send data through Serial\r
+ if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
+ sendLightInfo();\r
+ lgtCnt = 0;\r
+ }\r
+ if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){\r
+ sendMagInfo();\r
+ magCnt = 0;\r
+ }\r
+ if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){\r
+ sendTouchInfo();\r
+ tchCnt = 0;\r
+ }\r
+ if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
+ sendAccInfo();\r
+ accCnt = 0;\r
+ }\r
+}\r
+\r
+void sendLightInfo(){\r
+ char frame[MAX_FRAME_LEN];\r
+ float lightData = lightSensor.read();\r
+ packToFrame(frame,LGT_SNS,FLOAT,1,&lightData);\r
+ serial.printf("[LGT] %s\r\n", frame);\r
+ //printHex(frame);\r
+ //serial.printf("[LGT] Light Intensity=%f\r\n", lightSensor.read());\r
+}\r
+\r
+\r
+void sendTouchInfo(){\r
+ char frame[MAX_FRAME_LEN];\r
+ float touchData[2];\r
+ touchData[0] = touchSensor.readPercentage();\r
+ touchData[1] = touchSensor.readDistance();\r
+ packToFrame(frame,TCH_SNS,FLOAT,2,touchData);\r
+ serial.printf("[TCH] %s\r\n", frame);\r
+ //printHex(frame);\r
+}\r
+\r
+void sendMagInfo(){\r
+ char frame[MAX_FRAME_LEN];\r
+ int magData[3];\r
+ magSensor.getValues(magData, magData+1, magData+2);\r
+ //serial.printf("[MAG] magX=%d magY=%d magZ=%d\r\n",magData[0],magData[1],magData[2]);\r
+ int len = packToFrame(frame,MAG_SNS,INT,3,magData);\r
+ sendFrame(frame, len);\r
+ //memcpy(magData, &frame[3], 2);\r
+ //serial.printf("[MAG] %d\r\n", magData[0]);\r
+ //printHex(frame);\r
+}\r
+\r
+void sendAccInfo(){\r
+ // get acc data\r
+ char frame[MAX_FRAME_LEN];\r
+ float accData[3];\r
+ accData[0] = accSensor.getAccX();\r
+ accData[1] = accSensor.getAccY();\r
+ accData[2] = accSensor.getAccZ();\r
+ packToFrame(frame, ACC_SNS,FLOAT,3,accData);\r
+ serial.printf("[ACC] %s\n\r", frame);\r
+ //int num = int(frame[2]);\r
+ //memcpy(&accZ, &frame[3], 4);\r
+ // send acc data\r
+ //printHex(frame);\r
+}\r
+int calDataSize(char dataType){\r
+ switch(dataType){\r
+ case INT: return 2;\r
+ case LONG: return 4;\r
+ case FLOAT: return 4;\r
+ case DOUBLE: return 8;\r
+ }\r
+ return 4;\r
+}\r
+\r
+void printHex(char* frame){\r
+ int dataType = frame[1]&0xE0;\r
+ int dataSize = calDataSize(dataType);\r
+ int dataNum = frame[2];\r
+ for (int i=0; i<4+dataSize*dataNum; i++){\r
+ serial.printf("0x%x ",frame[i]);\r
+ }\r
+ serial.printf("\r\n");\r
+}\r
+\r
+int packToFrame(char* frame, char snsType,char dataType, int dataNum, void* data){\r
+ int dataSize = calDataSize(dataType);\r
+ frame[0] = HEADER;\r
+ frame[1] = (snsType|(dataType<<SNS_BITS));\r
+ frame[2] = dataNum;\r
+ memcpy(frame+3, data, dataSize*dataNum);\r
+ frame[3+dataNum*dataSize]= TAIL;\r
+ frame[4+dataNum*dataSize]= '\0';\r
+ frame[3] = 0x00;\r
+ return (4+dataNum*dataSize);\r
+}\r
+\r
+\r
+void sendFrame (char* frame, int len){\r
+ for (int i=0; i<len; i++){\r
+ serial.putc(frame[i]);\r
+ }\r
+}\r