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