Save a working version of TPM

[~andy/csm213a-hw] / yue / main.cpp

diff --git a/yue/main.cpp b/yue/main.cpp
index 19c88030b31f75966d63d439762d1495e99f7b40..3dfd736b37deeb246bdb7b3ce6e5801484a30c2c 100644 (file)
--- a/yue/main.cpp
+++ b/yue/main.cpp
@@ -15,9 +15,9 @@
          (x) > (max) ? (max) : (x))\r
 \r
 #define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address\r
-#define TIME_ACCURACY     0.01\r
+#define TIME_ACCURACY     0.0005\r
 \r
-#define ACC_SNS_DEFAULT   0.1   // default collecting interval in seconds\r
+#define ACC_SNS_DEFAULT   0.01 // default collecting interval in seconds\r
 #define MAG_SNS_DEFAULT   0.1\r
 #define LGT_SNS_DEFAULT   0.1\r
 #define TCH_SNS_DEFAULT   0.1\r
@@ -58,6 +58,7 @@ typedef enum {
 } cmd_t;\r
 \r
 // Data Frame Information\r
+#pragma pack(1)\r
 typedef struct {\r
     uint8_t header;\r
     struct {\r
@@ -66,9 +67,11 @@ typedef struct {
     } bits;\r
     uint8_t count;\r
     uint8_t data[];\r
-} __attribute__((__packed__)) state_t;\r
+} state_t;\r
+#pragma pack()\r
 \r
 // Command Frame Information\r
+#pragma pack(1)\r
 typedef struct {\r
     uint8_t header;\r
     struct {\r
@@ -76,14 +79,23 @@ typedef struct {
         uint8_t sns : 4;\r
     } bits;\r
     float   interval;\r
-} __attribute__((__packed__)) control_t;\r
+} control_t;\r
+#pragma pack()\r
 \r
 // Define Devices & Pins\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
+AnalogIn   a2dSensor0(A0);\r
+AnalogIn   a2dSensor1(A1);\r
+AnalogIn   a2dSensor2(A2);\r
+AnalogIn   a2dSensor3(A3);\r
+AnalogIn   a2dSensor4(A4);\r
+AnalogIn   a2dSensor5(A5);\r
+\r
+AnalogOut  a2dOutput(PTE30);\r
 \r
 DigitalOut led1(LED1);\r
 DigitalOut led2(LED2);\r
@@ -100,11 +112,11 @@ float lgtTmr = LGT_SNS_DEFAULT;
 float a2dTmr = A2D_SNS_DEFAULT;\r
 float ledTmr = LED_OUT_DEFAULT;\r
 \r
-bool accEnable = true;\r
+bool accEnable = false;\r
 bool magEnable = false;\r
 bool lgtEnable = false;\r
 bool tchEnable = false;\r
-bool a2dEnable = false;\r
+bool a2dEnable = true;\r
 bool ledEnable = true;\r
 \r
 bool useStr = false;\r
@@ -140,17 +152,23 @@ void printBin(uint8_t *frame, int len);
 int main(void) {\r
     // Initialization\r
     // Interruption Declarations\r
-    clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s\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
+    //serial.baud(115200);\r
+    serial.baud(230400);\r
     magSensor.begin();\r
 \r
     sdma_setup(UART0, 0);\r
 \r
     printStr("\r\n============= Start of the program ============\r\n");\r
 \r
+    int ticks = 0, tocks = 0;\r
     while(1){\r
-        wait(3);\r
+       ticks = (us_ticker_read()*1E-6) / TIME_ACCURACY;\r
+       if (tocks < ticks) {\r
+               clock1_interrupt();\r
+               tocks++;\r
+       }\r
     }\r
 }\r
 \r
@@ -206,42 +224,50 @@ void clock1_interrupt(void){
     static int tchCnt;\r
     static int a2dCnt;\r
     static int ledCnt;\r
+    static int sinCnt;\r
 \r
-    accCnt++;\r
-    magCnt++;\r
-    lgtCnt++;\r
-    tchCnt++;\r
-    a2dCnt++;\r
-    ledCnt++;\r
+    static float sinAmp  = 0.5;  // 0..1\r
+    static float sinFreq = 0.25; // Hz\r
+\r
+    // Write A2D output sine wave\r
+    a2dOutput.write(sinAmp*sin(sinCnt * TIME_ACCURACY * (2*PI) * sinFreq)*0.5 + 0.5);\r
 \r
     // Send data through Serial\r
-    if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
+    if (accEnable && accCnt >= (int)(accTmr/TIME_ACCURACY+0.5)){\r
         sendAccInfo();\r
         accCnt = 0;\r
     }\r
-    if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){\r
+    if (magEnable && magCnt >= (int)(magTmr/TIME_ACCURACY+0.5)){\r
         sendMagInfo();\r
         magCnt = 0;\r
     }\r
-    if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
+    if (lgtEnable && lgtCnt >= (int)(lgtTmr/TIME_ACCURACY+0.5)){\r
         sendLgtInfo();\r
         lgtCnt = 0;\r
     }\r
-    if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){\r
+    if (tchEnable && tchCnt >= (int)(tchTmr/TIME_ACCURACY+0.5)){\r
         sendTchInfo();\r
         tchCnt = 0;\r
     }\r
-    if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){\r
+    if (a2dEnable && a2dCnt >= (int)(a2dTmr/TIME_ACCURACY+0.5)){\r
         sendA2dInfo();\r
         a2dCnt = 0;\r
     }\r
 \r
     // Toggel LED for debugging\r
-    if (ledEnable && (ledCnt<0 || ledCnt>=ledTmr/TIME_ACCURACY)){\r
+    if (ledEnable && ledCnt>=ledTmr/TIME_ACCURACY){\r
         led1   = !led1;\r
        ledCnt = 0;\r
     }\r
 \r
+    accCnt++;\r
+    magCnt++;\r
+    lgtCnt++;\r
+    tchCnt++;\r
+    a2dCnt++;\r
+    ledCnt++;\r
+    sinCnt++;\r
+\r
     sdma_flush();\r
 }\r
 \r
@@ -358,12 +384,12 @@ void sendTchInfo(void){
 \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
+    a2dData[0] = a2dSensor0.read();\r
+    a2dData[1] = a2dSensor1.read();\r
+    a2dData[2] = a2dSensor2.read();\r
+    a2dData[3] = a2dSensor3.read();\r
+    a2dData[4] = a2dSensor4.read();\r
+    a2dData[5] = a2dSensor5.read();\r
     int len = packToFrame(txFrame, SNS_A2D, TYP_F32, 6, a2dData);\r
 \r
     printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",\r