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