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