X-Git-Url: http://pileus.org/git/?a=blobdiff_plain;f=yue%2Fmain.cpp;h=150be36fd8ce0c1c93b356f99a62baa409ed72bf;hb=d79f24cc277e31d7848f9683bac599339348aa44;hp=e08c06da9e2bc652a1c10647481d5e80574e2579;hpb=6eac4234f7b20b3bc6a72f4242ad3ee99516c858;p=~andy%2Fcsm213a-hw

diff --git a/yue/main.cpp b/yue/main.cpp
index e08c06d..150be36 100644
--- a/yue/main.cpp
+++ b/yue/main.cpp
@@ -15,13 +15,14 @@
          (x) > (max) ? (max) : (x))
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address
-#define TIME_ACCURACY     0.01
+#define TIME_ACCURACY     0.0005
 
-#define ACC_SNS_DEFAULT   0.1   // default collecting interval in seconds
-#define MAG_SNS_DEFAULT   1.0
-#define LGT_SNS_DEFAULT   1.0
-#define TCH_SNS_DEFAULT   1.0
-#define A2D_SNS_DEFAULT   1.0
+#define ACC_SNS_DEFAULT   0.01 // default collecting interval in seconds
+#define MAG_SNS_DEFAULT   0.1
+#define LGT_SNS_DEFAULT   0.1
+#define TCH_SNS_DEFAULT   0.1
+#define A2D_SNS_DEFAULT   0.1
+#define LED_OUT_DEFAULT   0.5
 
 #define MIN_RATE          1E-4  // 1 kHz
 #define MAX_RATE          10.0  // 0.1 Hz
@@ -32,57 +33,75 @@
 
 #define MAX_FRAME_SIZE    256
 
+#define SNS_NUM           5
+#define TYP_NUM           8
+#define CMD_NUM           3
+
 typedef enum {
-    ACC_SNS,
-    MAG_SNS,
-    LGT_SNS,
-    TCH_SNS,
-    A2D_SNS,
+    SNS_ACC,
+    SNS_MAG,
+    SNS_LGT,
+    SNS_TCH,
+    SNS_A2D,
 } sns_t;
 
 typedef enum {
-    INT,
-    LONG,
-    FLOAT,
-    DOUBLE,
+    TYP_S8,  TYP_S16, TYP_S32,
+    TYP_U8,  TYP_U16, TYP_U32,
+    TYP_F32, TYP_F64,
 } type_t;
 
 typedef enum {
-    START,
-    STOP,
-    SET_INT,
-} oper_t;
+    CMD_STOP,
+    CMD_START,
+    CMD_RATE,
+} cmd_t;
 
 // Data Frame Information
+#pragma pack(1)
 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;
+#pragma pack()
 
 // Command Frame Information
+#pragma pack(1)
 typedef struct {
     uint8_t header;
     struct {
-        uint8_t sns  : 5;
-        uint8_t oper : 3;
+        uint8_t cmd : 4;
+        uint8_t sns : 4;
     } bits;
     float   interval;
-} command_t;
+} control_t;
+#pragma pack()
 
 // Define Devices & Pins
-MMA8451Q  accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
-MAG3110   magSensor(PTE25, PTE24);
-TSISensor tchSensor;
-AnalogIn  lgtSensor(PTE22);
-AnalogIn  a2dSensor(A0);
+MMA8451Q   accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);
+MAG3110    magSensor(PTE25, PTE24);
+TSISensor  tchSensor;
+AnalogIn   lgtSensor(PTE22);
+
+AnalogIn   a2dSensor0(A0);
+AnalogIn   a2dSensor1(A1);
+AnalogIn   a2dSensor2(A2);
+AnalogIn   a2dSensor3(A3);
+AnalogIn   a2dSensor4(A4);
+AnalogIn   a2dSensor5(A5);
+
+AnalogOut  a2dOutput(PTE30);
 
-Serial serial(USBTX, USBRX);
-Ticker clock1;
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+Serial     serial(USBTX, USBRX);
+Ticker     clock1;
 
 // Global Variables
 // Initial interval: in seconds
@@ -91,16 +110,18 @@ float magTmr = MAG_SNS_DEFAULT;
 float tchTmr = TCH_SNS_DEFAULT;
 float lgtTmr = LGT_SNS_DEFAULT;
 float a2dTmr = A2D_SNS_DEFAULT;
+float ledTmr = LED_OUT_DEFAULT;
 
-bool accEnable = true;
+bool accEnable = false;
 bool magEnable = false;
 bool lgtEnable = false;
 bool tchEnable = false;
-bool a2dEnable = false;
+bool a2dEnable = true;
+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];
@@ -109,6 +130,8 @@ uint8_t rxFrame[MAX_FRAME_SIZE];
 void clock1_interrupt(void);
 void serialRx_interrupt(void);
 
+void runCommand(control_t *cmd);
+
 void sendAccInfo(void);
 void sendMagInfo(void);
 void sendLgtInfo(void);
@@ -129,17 +152,23 @@ void printBin(uint8_t *frame, int len);
 int main(void) {
     // Initialization
     // Interruption Declarations
-    clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s
+    //clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s
     serial.attach(&serialRx_interrupt, Serial::RxIrq);  // receive interrupt for serialS
-    serial.baud(115200);
+    //serial.baud(115200);
+    serial.baud(230400);
     magSensor.begin();
 
     sdma_setup(UART0, 0);
 
     printStr("\r\n============= Start of the program ============\r\n");
 
+    int ticks = 0, tocks = 0;
     while(1){
-        wait(3);
+    	ticks = (us_ticker_read()*1E-6) / TIME_ACCURACY;
+    	if (tocks < ticks) {
+		clock1_interrupt();
+		tocks++;
+	}
     }
 }
 
@@ -148,83 +177,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;
+
+    control_t *cmd = (control_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.cmd >= CMD_NUM) {
+                    state = index = 0;
+                } else if (cmd->bits.cmd == CMD_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){
@@ -233,38 +223,104 @@ void clock1_interrupt(void){
     static int lgtCnt;
     static int tchCnt;
     static int a2dCnt;
+    static int ledCnt;
 
-    accCnt++;
-    magCnt++;
-    lgtCnt++;
-    tchCnt++;
-    a2dCnt++;
+    // Write A2D output sine wave
+    //a2dOutput.write(sin(a2dCnt * TIME_ACCURACY * (2*PI) * 0.1));
 
-    // TODO: send data through Serial
-    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){
+    // Send data through Serial
+    if (accEnable && accCnt >= (int)(accTmr/TIME_ACCURACY+0.5)){
         sendAccInfo();
         accCnt = 0;
     }
-    if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){
+    if (magEnable && magCnt >= (int)(magTmr/TIME_ACCURACY+0.5)){
         sendMagInfo();
         magCnt = 0;
     }
-    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){
+    if (lgtEnable && lgtCnt >= (int)(lgtTmr/TIME_ACCURACY+0.5)){
         sendLgtInfo();
         lgtCnt = 0;
     }
-    if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){
+    if (tchEnable && tchCnt >= (int)(tchTmr/TIME_ACCURACY+0.5)){
         sendTchInfo();
         tchCnt = 0;
     }
-    if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){
+    if (a2dEnable && a2dCnt >= (int)(a2dTmr/TIME_ACCURACY+0.5)){
         sendA2dInfo();
         a2dCnt = 0;
     }
 
+    // Toggel LED for debugging
+    if (ledEnable && ledCnt>=ledTmr/TIME_ACCURACY){
+        led1   = !led1;
+	ledCnt = 0;
+    }
+
+    accCnt++;
+    magCnt++;
+    lgtCnt++;
+    tchCnt++;
+    a2dCnt++;
+    ledCnt++;
+
     sdma_flush();
 }
 
+/*******************
+ * Command handler *
+ *******************/
+
+void runCommand(control_t *cmd)
+{
+    // Validate interval
+    sns_t snsType = (sns_t)cmd->bits.sns;
+    cmd_t cmdType = (cmd_t)cmd->bits.cmd;
+    float interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);
+
+    // Save value to global data
+    switch(snsType){
+        case SNS_ACC:
+            switch(cmdType){
+                case CMD_STOP:   accEnable = false; break;
+                case CMD_START:  accEnable = true;  break;
+                case CMD_RATE:   accTmr = interval; break;
+            }
+            break;
+
+        case SNS_MAG:
+            switch(cmdType){
+                case CMD_STOP:   magEnable = false; break;
+                case CMD_START:  magEnable = true;  break;
+                case CMD_RATE:   magTmr = interval; break;
+            }
+            break;
+
+        case SNS_LGT:
+            switch(cmdType){
+                case CMD_STOP:   lgtEnable = false; break;
+                case CMD_START:  lgtEnable = true;  break;
+                case CMD_RATE:   lgtTmr = interval; break;
+            }
+            break;
+
+        case SNS_TCH:
+            switch(cmdType){
+                case CMD_STOP:   tchEnable = false; break;
+                case CMD_START:  tchEnable = true;  break;
+                case CMD_RATE:   tchTmr = interval; break;
+            }
+            break;
+
+        case SNS_A2D:
+            switch(cmdType){
+                case CMD_STOP:   a2dEnable = false; break;
+                case CMD_START:  a2dEnable = true;  break;
+                case CMD_RATE:   a2dTmr = interval; break;
+            }
+            break;
+    }
+}
+
 /*******************
  * Sensors reading *
  *******************/
@@ -274,7 +330,7 @@ void sendAccInfo(void){
     accData[0] = accSensor.getAccX();
     accData[1] = accSensor.getAccY();
     accData[2] = accSensor.getAccZ();
-    int len = packToFrame(txFrame, ACC_SNS, FLOAT, 3, accData);
+    int len = packToFrame(txFrame, SNS_ACC, TYP_F32, 3, accData);
 
     printStr("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",
                     accData[0], accData[1], accData[2]);
@@ -283,19 +339,25 @@ void sendAccInfo(void){
 }
 
 void sendMagInfo(void){
-    int magData[3];
-    magSensor.getValues(&magData[0], &magData[1], &magData[2]);
-    int len = packToFrame(txFrame, MAG_SNS, INT, 3, magData);
-
-    printStr("[MAG] magX=%d magY=%d magZ=%d\r\n",
-                    magData[0], magData[1], magData[2]);
+    // magSensor uses the wrong types,
+    // so we have to convert it
+    int tmp[3];
+    magSensor.getValues(&tmp[0], &tmp[1], &tmp[2]);
+    uint16_t magData[3];
+    magData[0] = tmp[0];
+    magData[1] = tmp[1];
+    magData[2] = tmp[2];
+    int len = packToFrame(txFrame, SNS_MAG, TYP_S16, 3, magData);
+
+    printStr("[MAG] magX=%hd magY=%hd magZ=%hd %d\r\n",
+                    magData[0], magData[1], magData[2], sizeof(int));
     printHex(txFrame, len);
     printBin(txFrame, len);
 }
 
 void sendLgtInfo(void){
     float lgtData = lgtSensor.read();
-    int len = packToFrame(txFrame, LGT_SNS, FLOAT, 1, &lgtData);
+    int len = packToFrame(txFrame, SNS_LGT, TYP_F32, 1, &lgtData);
 
     printStr("[LGT] intensity=%f\r\n",
                     lgtSensor.read());
@@ -307,7 +369,7 @@ void sendTchInfo(void){
     float tchData[2];
     tchData[0] = tchSensor.readPercentage();
     tchData[1] = tchSensor.readDistance();
-    int len = packToFrame(txFrame, TCH_SNS, FLOAT, 2, tchData);
+    int len = packToFrame(txFrame, SNS_TCH, TYP_F32, 2, tchData);
 
     printStr("[TCH] force=%0.4f distance=%2.2f\r\n",
                     tchData[0], tchData[1]);
@@ -317,13 +379,13 @@ void sendTchInfo(void){
 
 void sendA2dInfo(void){
     float a2dData[6];
-    a2dData[0] = 0;
-    a2dData[1] = 0;
-    a2dData[2] = 0;
-    a2dData[3] = 0;
-    a2dData[4] = 0;
-    a2dData[5] = 0;
-    int len = packToFrame(txFrame, A2D_SNS, FLOAT, 2, a2dData);
+    a2dData[0] = a2dSensor0.read();
+    a2dData[1] = a2dSensor1.read();
+    a2dData[2] = a2dSensor2.read();
+    a2dData[3] = a2dSensor3.read();
+    a2dData[4] = a2dSensor4.read();
+    a2dData[5] = a2dSensor5.read();
+    int len = packToFrame(txFrame, SNS_A2D, TYP_F32, 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],
@@ -336,12 +398,16 @@ 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;
-        case FLOAT:  return 4;
-        case DOUBLE: return 8;
+        case TYP_S8:  return 1;
+        case TYP_S16: return 2;
+        case TYP_S32: return 4;
+        case TYP_U8:  return 1;
+        case TYP_U16: return 2;
+        case TYP_U32: return 4;
+        case TYP_F32: return 4;
+        case TYP_F64: return 8;
     }
     return 4;
 }
@@ -351,7 +417,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];