bluefruit_controller.cpp

00001 #include "bluefruit_controller.h"
00002 
00003 enum State {
00004     STATE_IDLE,         // Waiting for a message
00005     STATE_MSG,          // '!' received, read in a tag
00006     STATE_PAYLOAD,      // tag received, read in payload bytes
00007 };
00008 
00009 
00010 BluefruitController::BluefruitController(PinName tx, PinName rx, int baud)
00011     : Serial(tx, rx, baud)
00012 {
00013     buff_len = 0;
00014     buff_i = 0;
00015 }
00016 
00017 void BluefruitController::parseMessage()
00018 {
00019     while (readable()) {
00020         switch (state) {
00021             // Read until the '!'
00022             case STATE_IDLE: {
00023                 if (getc() == '!')
00024                     state = STATE_MSG;
00025                 break;
00026             }
00027 
00028             // Read in the msg tag and begin reading that many bytes
00029             case STATE_MSG: {
00030                 msg_tag = getc();
00031                 buff_i = 0;
00032                 state = STATE_PAYLOAD;
00033 
00034                 switch (msg_tag) {
00035                     case 'B':
00036                         buff_len = 3;
00037                         break;
00038                     case 'Q':
00039                         buff_len = 4*4;
00040                         break;
00041 
00042                     // For unrecognized message types, go back to waiting for
00043                     // the next '!'
00044                     default:
00045                         state = STATE_IDLE;
00046                 }
00047                 break;
00048             }
00049 
00050             // Read in payload bytes
00051             case STATE_PAYLOAD: {
00052                 // Read in bytes
00053                 if (buff_i < buff_len) {
00054                     buff[buff_i] = getc();
00055                     buff_i++;
00056                 }
00057 
00058                 // When done, parse the messages
00059                 if (buff_i == buff_len) {
00060                     state = STATE_IDLE;
00061                     switch(msg_tag) {
00062                         case 'B':
00063                             parseButton();
00064                             break;
00065                         case 'Q':
00066                             parseQuaternion();
00067                             break;
00068                     }
00069                 }
00070                 break;
00071             }
00072         }
00073     }
00074 }
00075 
00076 
00077 void BluefruitController::reset()
00078 {
00079     state = STATE_IDLE;
00080     buff_len = 0;
00081     buff_i = 0;
00082 }
00083 
00084 void BluefruitController::parseButton()
00085 {
00086     unsigned int id = buff[0] - '1';
00087     bool val = buff[1] - '0';
00088 
00089     if (id >= 8) return;
00090 
00091     button[id] = val;
00092 }
00093 
00094 void BluefruitController::parseQuaternion()
00095 {
00096     for (int i = 0; i < 4; i++) {
00097         quaternion[i] = *(((float*)buff) + i);
00098     }
00099 }