Pileus Git - ~andy/csm213a-hw/blob - yue/main.cpp

   1 #include <stdint.h>\r
   2 #include <stdarg.h>\r
   3 \r
   4 #include "mbed.h"\r
   5 #include "TSISensor.h"          // Touch Sensor\r
   6 #include "MAG3110.h"            // Magnetic Sensor\r
   7 #include "MMA8451Q.h"           // AcceleroMeter\r
   8 \r
   9 #include "serial_dma.h"         // AcceleroMeter\r
  10 \r
  11 //#include "DataFrame.h"\r
  12 \r
  13 #define CLAMP(x, min, max)     \\r
  14         ((x) < (min) ? (min) : \\r
  15          (x) > (max) ? (max) : (x))\r
  16 \r
  17 #define MMA8451_I2C_ADDRESS (0x1d<<1)   // acc sensor address\r
  18 #define TIME_ACCURACY     0.01\r
  19 \r
  20 #define ACC_SNS_DEFAULT   0.1   // default collecting interval in seconds\r
  21 #define MAG_SNS_DEFAULT   1.0\r
  22 #define LGT_SNS_DEFAULT   1.0\r
  23 #define TCH_SNS_DEFAULT   1.0\r
  24 #define A2D_SNS_DEFAULT   1.0\r
  25 #define LED_OUT_DEFAULT   0.5\r
  26 \r
  27 #define MIN_RATE          1E-4  // 1 kHz\r
  28 #define MAX_RATE          10.0  // 0.1 Hz\r
  29 \r
  30 // Common Frame Information\r
  31 #define HEADER            0x02\r
  32 #define TAIL              0x0A  // '\n'\r
  33 \r
  34 #define MAX_FRAME_SIZE    256\r
  35 \r
  36 typedef enum {\r
  37     ACC_SNS,\r
  38     MAG_SNS,\r
  39     LGT_SNS,\r
  40     TCH_SNS,\r
  41     A2D_SNS,\r
  42 } sns_t;\r
  43 \r
  44 typedef enum {\r
  45     INT,\r
  46     LONG,\r
  47     FLOAT,\r
  48     DOUBLE,\r
  49 } type_t;\r
  50 \r
  51 typedef enum {\r
  52     START,\r
  53     STOP,\r
  54     SET_INT,\r
  55 } oper_t;\r
  56 \r
  57 // Data Frame Information\r
  58 typedef struct {\r
  59     uint8_t header;\r
  60     struct {\r
  61         uint8_t sns  : 5;\r
  62         uint8_t type : 3;\r
  63     } bits;\r
  64     uint8_t count;\r
  65     uint8_t data[];\r
  66 } state_t;\r
  67 \r
  68 // Command Frame Information\r
  69 typedef struct {\r
  70     uint8_t header;\r
  71     struct {\r
  72         uint8_t sns  : 5;\r
  73         uint8_t oper : 3;\r
  74     } bits;\r
  75     float   interval;\r
  76 } command_t;\r
  77 \r
  78 // Define Devices & Pins\r
  79 MMA8451Q   accSensor(PTE25, PTE24, MMA8451_I2C_ADDRESS);\r
  80 MAG3110    magSensor(PTE25, PTE24);\r
  81 TSISensor  tchSensor;\r
  82 AnalogIn   lgtSensor(PTE22);\r
  83 AnalogIn   a2dSensor(A0);\r
  84 \r
  85 DigitalOut led1(LED1);\r
  86 DigitalOut led2(LED2);\r
  87 \r
  88 Serial     serial(USBTX, USBRX);\r
  89 Ticker     clock1;\r
  90 \r
  91 // Global Variables\r
  92 // Initial interval: in seconds\r
  93 float accTmr = ACC_SNS_DEFAULT;\r
  94 float magTmr = MAG_SNS_DEFAULT;\r
  95 float tchTmr = TCH_SNS_DEFAULT;\r
  96 float lgtTmr = LGT_SNS_DEFAULT;\r
  97 float a2dTmr = A2D_SNS_DEFAULT;\r
  98 float ledTmr = LED_OUT_DEFAULT;\r
  99 \r
 100 bool accEnable = true;\r
 101 bool magEnable = false;\r
 102 bool lgtEnable = false;\r
 103 bool tchEnable = false;\r
 104 bool a2dEnable = false;\r
 105 bool ledEnable = true;\r
 106 \r
 107 bool useStr = true;\r
 108 bool useHex = false;\r
 109 bool useBin = false;\r
 110 \r
 111 uint8_t txFrame[MAX_FRAME_SIZE];\r
 112 uint8_t rxFrame[MAX_FRAME_SIZE];\r
 113 \r
 114 // Prototypes\r
 115 void clock1_interrupt(void);\r
 116 void serialRx_interrupt(void);\r
 117 \r
 118 void sendAccInfo(void);\r
 119 void sendMagInfo(void);\r
 120 void sendLgtInfo(void);\r
 121 void sendTchInfo(void);\r
 122 void sendA2dInfo(void);\r
 123 \r
 124 int  calDataSize(uint8_t);\r
 125 int  packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data);\r
 126 \r
 127 void printStr(const char *str, ...);\r
 128 void printHex(uint8_t *frame, int len);\r
 129 void printBin(uint8_t *frame, int len);\r
 130 \r
 131 /********\r
 132  * Main *\r
 133  ********/\r
 134 \r
 135 int main(void) {\r
 136     // Initialization\r
 137     // Interruption Declarations\r
 138     clock1.attach(&clock1_interrupt, TIME_ACCURACY);    // maximun accuracy be 0.1s\r
 139     serial.attach(&serialRx_interrupt, Serial::RxIrq);  // receive interrupt for serialS\r
 140     serial.baud(115200);\r
 141     magSensor.begin();\r
 142 \r
 143     sdma_setup(UART0, 0);\r
 144 \r
 145     printStr("\r\n============= Start of the program ============\r\n");\r
 146 \r
 147     while(1){\r
 148         wait(3);\r
 149     }\r
 150 }\r
 151 \r
 152 /**********************\r
 153  * Interrupt handlers *\r
 154  **********************/\r
 155 \r
 156 void serialRx_interrupt(void) {                     // Byte version\r
 157     clock1.detach();                                // close the interrupt temporarily\r
 158     int i = 0;\r
 159     uint8_t ch = serial.getc();\r
 160 \r
 161     while (ch!=HEADER){\r
 162         if (serial.readable()){\r
 163             ch = serial.getc();\r
 164         }\r
 165         else{\r
 166             printStr("[ERROR] broken data!\r\n");\r
 167             clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
 168             return;\r
 169         }\r
 170     }\r
 171 \r
 172     //TODO: ticker for time out\r
 173     while (serial.readable()){\r
 174         rxFrame[i] = ch;\r
 175         if (ch=='\n')\r
 176             break;\r
 177         ch = serial.getc();\r
 178         i++;\r
 179     }\r
 180     rxFrame[++i] = '\0';\r
 181 \r
 182     // Cast to command and\r
 183     command_t *cmd = (command_t *)rxFrame;\r
 184 \r
 185     // Validate interval\r
 186     sns_t  snsType  = (sns_t )cmd->bits.sns;\r
 187     oper_t operType = (oper_t)cmd->bits.oper;\r
 188     float  interval = CLAMP(cmd->interval, MIN_RATE, MAX_RATE);\r
 189 \r
 190     // Save value to global data\r
 191     switch(snsType){\r
 192         case ACC_SNS:\r
 193             switch(operType){\r
 194                 case START:     accEnable = true;  break;\r
 195                 case STOP:      accEnable = false; break;\r
 196                 case SET_INT:   accTmr = interval; break;\r
 197             }\r
 198             break;\r
 199 \r
 200         case MAG_SNS:\r
 201             switch(operType){\r
 202                 case START:     magEnable = true;  break;\r
 203                 case STOP:      magEnable = false; break;\r
 204                 case SET_INT:   magTmr = interval; break;\r
 205             }\r
 206             break;\r
 207 \r
 208         case LGT_SNS:\r
 209             switch(operType){\r
 210                 case START:     lgtEnable = true;  break;\r
 211                 case STOP:      lgtEnable = false; break;\r
 212                 case SET_INT:   lgtTmr = interval; break;\r
 213             }\r
 214             break;\r
 215 \r
 216         case TCH_SNS:\r
 217             switch(operType){\r
 218                 case START:     tchEnable = true;  break;\r
 219                 case STOP:      tchEnable = false; break;\r
 220                 case SET_INT:   tchTmr = interval; break;\r
 221             }\r
 222             break;\r
 223 \r
 224         case A2D_SNS:\r
 225             switch(operType){\r
 226                 case START:     a2dEnable = true;  break;\r
 227                 case STOP:      a2dEnable = false; break;\r
 228                 case SET_INT:   a2dTmr = interval; break;\r
 229             }\r
 230             break;\r
 231     }\r
 232 \r
 233     clock1.attach(&clock1_interrupt, TIME_ACCURACY);\r
 234 }\r
 235 \r
 236 void clock1_interrupt(void){\r
 237     static int accCnt;\r
 238     static int magCnt;\r
 239     static int lgtCnt;\r
 240     static int tchCnt;\r
 241     static int a2dCnt;\r
 242     static int ledCnt;\r
 243 \r
 244     accCnt++;\r
 245     magCnt++;\r
 246     lgtCnt++;\r
 247     tchCnt++;\r
 248     a2dCnt++;\r
 249     ledCnt++;\r
 250 \r
 251     // Send data through Serial\r
 252     if (accEnable && (accCnt<0 || accCnt>=accTmr/TIME_ACCURACY)){\r
 253         sendAccInfo();\r
 254         accCnt = 0;\r
 255     }\r
 256     if (magEnable && (magCnt<0 || magCnt>=magTmr/TIME_ACCURACY)){\r
 257         sendMagInfo();\r
 258         magCnt = 0;\r
 259     }\r
 260     if (lgtEnable && (lgtCnt<0 || lgtCnt>=lgtTmr/TIME_ACCURACY)){\r
 261         sendLgtInfo();\r
 262         lgtCnt = 0;\r
 263     }\r
 264     if (tchEnable && (tchCnt<0 || tchCnt>=tchTmr/TIME_ACCURACY)){\r
 265         sendTchInfo();\r
 266         tchCnt = 0;\r
 267     }\r
 268     if (a2dEnable && (a2dCnt<0 || a2dCnt>=a2dTmr/TIME_ACCURACY)){\r
 269         sendA2dInfo();\r
 270         a2dCnt = 0;\r
 271     }\r
 272 \r
 273     // Toggel LED for debugging\r
 274     if (ledEnable && (ledCnt<0 || ledCnt>=ledTmr/TIME_ACCURACY)){\r
 275         led1   = !led1;\r
 276         ledCnt = 0;\r
 277     }\r
 278 \r
 279     sdma_flush();\r
 280 }\r
 281 \r
 282 /*******************\r
 283  * Sensors reading *\r
 284  *******************/\r
 285 \r
 286 void sendAccInfo(void){\r
 287     float accData[3];\r
 288     accData[0] = accSensor.getAccX();\r
 289     accData[1] = accSensor.getAccY();\r
 290     accData[2] = accSensor.getAccZ();\r
 291     int len = packToFrame(txFrame, ACC_SNS, FLOAT, 3, accData);\r
 292 \r
 293     printStr("[ACC] accX=%-2.4f accY=%-2.4f accZ=%-2.4f\r\n",\r
 294                     accData[0], accData[1], accData[2]);\r
 295     printHex(txFrame, len);\r
 296     printBin(txFrame, len);\r
 297 }\r
 298 \r
 299 void sendMagInfo(void){\r
 300     int magData[3];\r
 301     magSensor.getValues(&magData[0], &magData[1], &magData[2]);\r
 302     int len = packToFrame(txFrame, MAG_SNS, INT, 3, magData);\r
 303 \r
 304     printStr("[MAG] magX=%d magY=%d magZ=%d\r\n",\r
 305                     magData[0], magData[1], magData[2]);\r
 306     printHex(txFrame, len);\r
 307     printBin(txFrame, len);\r
 308 }\r
 309 \r
 310 void sendLgtInfo(void){\r
 311     float lgtData = lgtSensor.read();\r
 312     int len = packToFrame(txFrame, LGT_SNS, FLOAT, 1, &lgtData);\r
 313 \r
 314     printStr("[LGT] intensity=%f\r\n",\r
 315                     lgtSensor.read());\r
 316     printHex(txFrame, len);\r
 317     printBin(txFrame, len);\r
 318 }\r
 319 \r
 320 void sendTchInfo(void){\r
 321     float tchData[2];\r
 322     tchData[0] = tchSensor.readPercentage();\r
 323     tchData[1] = tchSensor.readDistance();\r
 324     int len = packToFrame(txFrame, TCH_SNS, FLOAT, 2, tchData);\r
 325 \r
 326     printStr("[TCH] force=%0.4f distance=%2.2f\r\n",\r
 327                     tchData[0], tchData[1]);\r
 328     printHex(txFrame, len);\r
 329     printBin(txFrame, len);\r
 330 }\r
 331 \r
 332 void sendA2dInfo(void){\r
 333     float a2dData[6];\r
 334     a2dData[0] = 0;\r
 335     a2dData[1] = 0;\r
 336     a2dData[2] = 0;\r
 337     a2dData[3] = 0;\r
 338     a2dData[4] = 0;\r
 339     a2dData[5] = 0;\r
 340     int len = packToFrame(txFrame, A2D_SNS, FLOAT, 2, a2dData);\r
 341 \r
 342     printStr("[A2D] data=%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f\r\n",\r
 343                 a2dData[0], a2dData[1], a2dData[2],\r
 344                 a2dData[3], a2dData[4], a2dData[5]);\r
 345     printHex(txFrame, len);\r
 346     printBin(txFrame, len);\r
 347 }\r
 348 \r
 349 /********************\r
 350  * Helper functions *\r
 351  ********************/\r
 352 \r
 353 int calDataSize(uint8_t dataType){\r
 354     switch(dataType){\r
 355         case INT:    return 2;\r
 356         case LONG:   return 4;\r
 357         case FLOAT:  return 4;\r
 358         case DOUBLE: return 8;\r
 359     }\r
 360     return 4;\r
 361 }\r
 362 \r
 363 /*******************\r
 364  * Frame functions *\r
 365  *******************/\r
 366 \r
 367 int packToFrame(uint8_t *frame, sns_t snsType, type_t dataType, int dataNum, void *data){\r
 368     int      size  = dataNum + calDataSize(dataType);\r
 369     state_t *state = (state_t*)frame;\r
 370     uint8_t *tail  = &state->data[size];\r
 371 \r
 372     state->header    = HEADER;\r
 373     state->bits.sns  = snsType;\r
 374     state->bits.type = dataType;\r
 375     state->count     = dataNum;;\r
 376     memcpy(&state->data, data, size);\r
 377     tail[0]          = TAIL;\r
 378     tail[1]          = '\0';\r
 379 \r
 380     return (3 + size + 1);\r
 381 }\r
 382 \r
 383 /*******************\r
 384  * Print functions *\r
 385  *******************/\r
 386 \r
 387 void printHex(uint8_t *frame, int len){\r
 388     if (!useHex)\r
 389         return;\r
 390 \r
 391     sdma_printf("      ");\r
 392     for (int i=0; i<len; i++)\r
 393         sdma_printf("%02hx ", frame[i]);\r
 394     sdma_printf("\r\n");\r
 395 }\r
 396 \r
 397 void printStr(const char *fmt, ...){\r
 398     if (!useStr)\r
 399         return;\r
 400 \r
 401     va_list ap;\r
 402     va_start(ap, fmt);\r
 403     sdma_vprintf(fmt, ap);\r
 404     va_end(ap);\r
 405 }\r
 406 \r
 407 void printBin(uint8_t *frame, int len){\r
 408     if (!useBin)\r
 409         return;\r
 410 \r
 411     sdma_write(frame, len);\r
 412 }\r