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