#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}
+ 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
+ count = 0
class State: #information stored
class Device:
# Constructors
def __init__(self, config):
- print("IN")
+ print("IN")
self.config = config
self.serial = None
# Methods
def connect(self):
- print("C")
- buildingFrame = 0
+ print("C")
+ buildingFrame = 0
try:
self.inbuf = []
self.serial = Serial(self.config.device, \
return str(ex)
def disconnect(self):
- print("DC")
+ print("DC")
if self.serial and self.serial.isOpen():
self.serial.close()
return False
return True
- def control(self):#################
- print("CT")
- frame = [None]*7
+ 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]
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[2:6] = pack('f',float(rate))
frame[6] = Const.TAIL
self.serial.write(frame)
print('[SEND1] ',frame)
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 = []
- 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
+ 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): # to be changed
- #print("WA")
+ #print("WA")
if self.serial:
print('write: [' + line + ']')
self.serial.write(line + '\n')
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
+ #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 <stdint.h>\r
+#include <stdarg.h>\r
+\r
#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
+#include "serial_dma.h" // AcceleroMeter\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
+//#include "DataFrame.h"\r
\r
+#define CLAMP(x, min, max) \\r
+ ((x) < (min) ? (min) : \\r
+ (x) > (max) ? (max) : (x))\r
\r
+#define MMA8451_I2C_ADDRESS (0x1d<<1) // acc sensor address\r
+#define TIME_ACCURACY 0.01\r
+\r
+#define ACC_SNS_DEFAULT 0.1 // default collecting interval in seconds\r
+#define MAG_SNS_DEFAULT 1.0\r
+#define LGT_SNS_DEFAULT 1.0\r
+#define TCH_SNS_DEFAULT 1.0\r
+#define A2D_SNS_DEFAULT 1.0\r
+\r
+#define MIN_RATE 1E-4 // 1 kHz\r
+#define MAX_RATE 10.0 // 0.1 Hz\r
+\r
+// Common Frame Information\r
+#define HEADER 0x02\r
+#define TAIL 0x0A // '\n'\r
+\r
+#define MAX_FRAME_SIZE 256\r
+\r
+typedef enum {\r
+ ACC_SNS,\r
+ MAG_SNS,\r
+ LGT_SNS,\r
+ TCH_SNS,\r
+ A2D_SNS,\r
+} sns_t;\r
+\r
+typedef enum {\r
+ INT,\r
+ LONG,\r
+ FLOAT,\r
+ DOUBLE,\r
+} type_t;\r
+\r
+typedef enum {\r
+ START,\r
+ STOP,\r
+ SET_INT,\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
+ } bits;\r
+ uint8_t count;\r
+ uint8_t data[];\r
+} 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
+ } bits;\r
+ float interval;\r
+} command_t;\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
+MMA8451Q accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
+MAG3110 magSensor(PTE25, PTE24);\r
+TSISensor tchSensor;\r
+AnalogIn lgtSensor(PTE22);\r
+AnalogIn a2dSensor(A0);\r
+\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
+float lgtTmr = LGT_SNS_DEFAULT;\r
+float a2dTmr = A2D_SNS_DEFAULT;\r
\r
+bool accEnable = true;\r
+bool magEnable = false;\r
bool lgtEnable = false;\r
-bool accEnable = false;\r
-bool magEnable = true;\r
bool tchEnable = false;\r
+bool a2dEnable = false;\r
+\r
+bool useStr = true;\r
+bool useHex = false;\r
+bool useBin = false;\r
+\r
+uint8_t txFrame[MAX_FRAME_SIZE];\r
+uint8_t rxFrame[MAX_FRAME_SIZE];\r
+\r
+// Prototypes\r
+void clock1_interrupt(void);\r
+void serialRx_interrupt(void);\r
+\r
+void sendAccInfo(void);\r
+void sendMagInfo(void);\r
+void sendLgtInfo(void);\r
+void sendTchInfo(void);\r
+void sendA2dInfo(void);\r
+\r
+int calDataSize(uint8_t);\r
+int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data);\r
+\r
+void printStr(const char *str, ...);\r
+void printHex(uint8_t *frame, int len);\r
+void printBin(uint8_t *frame, int len);\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
+/********\r
+ * Main *\r
+ ********/\r
+\r
+int main(void) {\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
+ serial.baud(115200);\r
magSensor.begin();\r
\r
- serial.printf("\r\n============= Start of the program ============\r\n");\r
+ sdma_setup(UART0, 0);\r
+\r
+ printStr("\r\n============= Start of the program ============\r\n");\r
+\r
while(1){\r
wait(3);\r
}\r
}\r
\r
+/**********************\r
+ * Interrupt handlers *\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
+void serialRx_interrupt(void) { // 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
+ uint8_t ch = serial.getc();\r
+\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
+ printStr("[ERROR] broken data!\r\n");\r
+ clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
return;\r
}\r
}\r
+\r
//TODO: ticker for time out\r
while (serial.readable()){\r
- frame[i] = ch;\r
- if (ch=='\n'){\r
+ rxFrame[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
+ 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
- {\r
- switch(oper){\r
+ switch(operType){\r
case START: accEnable = true; break;\r
case STOP: accEnable = false; break;\r
case SET_INT: accTmr = interval; break;\r
}\r
- }\r
+ break;\r
+\r
case MAG_SNS:\r
- {\r
- switch(oper){\r
+ switch(operType){\r
case START: magEnable = true; break;\r
case STOP: magEnable = false; break;\r
case SET_INT: magTmr = interval; break;\r
}\r
- }\r
+ break;\r
+\r
case LGT_SNS:\r
- {\r
- switch(oper){\r
+ switch(operType){\r
case START: lgtEnable = true; break;\r
case STOP: lgtEnable = false; break;\r
case SET_INT: lgtTmr = interval; break;\r
}\r
- }\r
+ break;\r
+\r
case TCH_SNS:\r
- {\r
- switch(oper){\r
+ switch(operType){\r
case START: tchEnable = true; break;\r
case STOP: tchEnable = false; break;\r
case SET_INT: tchTmr = interval; break;\r
}\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
- clock1.attach(&clock1_interrupt,TIME_ACCURACY);\r
+\r
+ clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
}\r
\r
-void clock1_interrupt(){\r
+void clock1_interrupt(void){\r
static int accCnt;\r
static int magCnt;\r
static int lgtCnt;\r
static int tchCnt;\r
+ static int a2dCnt;\r
\r
accCnt++;\r
magCnt++;\r
lgtCnt++;\r
tchCnt++;\r
+ a2dCnt++;\r
\r
// TODO: send data through Serial\r
- if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
- sendLightInfo();\r
- lgtCnt = 0;\r
+ if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
+ sendAccInfo();\r
+ accCnt = 0;\r
}\r
if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){\r
sendMagInfo();\r
magCnt = 0;\r
}\r
+ if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
+ sendLgtInfo();\r
+ lgtCnt = 0;\r
+ }\r
if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){\r
- sendTouchInfo();\r
+ sendTchInfo();\r
tchCnt = 0;\r
}\r
- if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
- sendAccInfo();\r
- accCnt = 0;\r
+ if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){\r
+ sendA2dInfo();\r
+ a2dCnt = 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
+ sdma_flush();\r
}\r
\r
+/*******************\r
+ * Sensors reading *\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
+void sendAccInfo(void){\r
+ float accData[3];\r
+ accData[0] = accSensor.getAccX();\r
+ accData[1] = accSensor.getAccY();\r
+ accData[2] = accSensor.getAccZ();\r
+ int len = packToFrame(txFrame, ACC_SNS, FLOAT, 3, accData);\r
+\r
+ printStr("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",\r
+ accData[0], accData[1], accData[2]);\r
+ printHex(txFrame, len);\r
+ printBin(txFrame, len);\r
}\r
\r
-void sendMagInfo(){\r
- char frame[MAX_FRAME_LEN];\r
+void sendMagInfo(void){\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
+ magSensor.getValues(&magData[0], &magData[1], &magData[2]);\r
+ int len = packToFrame(txFrame, MAG_SNS, INT, 3, magData);\r
+\r
+ printStr("[MAG] magX=%d magY=%d magZ=%d\r\n",\r
+ magData[0], magData[1], magData[2]);\r
+ printHex(txFrame, len);\r
+ printBin(txFrame, len);\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
+void sendLgtInfo(void){\r
+ float lgtData = lgtSensor.read();\r
+ int len = packToFrame(txFrame, LGT_SNS, FLOAT, 1, &lgtData);\r
+\r
+ printStr("[LGT] intensity=%f\r\n",\r
+ lgtSensor.read());\r
+ printHex(txFrame, len);\r
+ printBin(txFrame, len);\r
}\r
-int calDataSize(char dataType){\r
+\r
+void sendTchInfo(void){\r
+ float tchData[2];\r
+ tchData[0] = tchSensor.readPercentage();\r
+ tchData[1] = tchSensor.readDistance();\r
+ int len = packToFrame(txFrame, TCH_SNS, FLOAT, 2, tchData);\r
+\r
+ printStr("[TCH] force=%0.4f distance=%2.2f\r\n",\r
+ tchData[0], tchData[1]);\r
+ printHex(txFrame, len);\r
+ printBin(txFrame, len);\r
+}\r
+\r
+void sendA2dInfo(void){\r
+ float a2dData[6];\r
+ a2dData[0] = 0;\r
+ a2dData[1] = 0;\r
+ a2dData[2] = 0;\r
+ a2dData[3] = 0;\r
+ a2dData[4] = 0;\r
+ a2dData[5] = 0;\r
+ int len = packToFrame(txFrame, A2D_SNS, FLOAT, 2, 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
+ a2dData[3], a2dData[4], a2dData[5]);\r
+ printHex(txFrame, len);\r
+ printBin(txFrame, len);\r
+}\r
+\r
+/********************\r
+ * Helper functions *\r
+ ********************/\r
+\r
+int calDataSize(uint8_t dataType){\r
switch(dataType){\r
case INT: return 2;\r
case LONG: return 4;\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
+ * Frame functions *\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
+ state_t *state = (state_t*)frame;\r
+ uint8_t *tail = &state->data[size];\r
+\r
+ state->header = HEADER;\r
+ state->bits.sns = snsType;\r
+ state->bits.type = dataType;\r
+ state->count = dataNum;;\r
+ memcpy(&state->data, data, size);\r
+ tail[0] = TAIL;\r
+ tail[1] = '\0';\r
+\r
+ return (3 + size + 1);\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
+ * Print functions *\r
+ *******************/\r
+\r
+void printHex(uint8_t *frame, int len){\r
+ if (!useHex)\r
+ return;\r
+\r
+ sdma_printf(" ");\r
+ for (int i=0; i<len; i++)\r
+ sdma_printf("%02hx ", frame[i]);\r
+ sdma_printf("\r\n");\r
}\r
\r
+void printStr(const char *fmt, ...){\r
+ if (!useStr)\r
+ return;\r
\r
-void sendFrame (char* frame, int len){\r
- for (int i=0; i<len; i++){\r
- serial.putc(frame[i]);\r
- }\r
+ va_list ap;\r
+ va_start(ap, fmt);\r
+ sdma_vprintf(fmt, ap);\r
+ va_end(ap);\r
+}\r
+\r
+void printBin(uint8_t *frame, int len){\r
+ if (!useBin)\r
+ return;\r
+\r
+ sdma_write(frame, len);\r
}\r
projects / ~andy / csm213a-hw / commitdiff