TinyCHR6dm.cpp

00001 #include <mbed.h>
00002 #include "TinyCHR6dm.h"
00003 #include "string.h"
00004 
00005 int chksum(uint8 pt, uint8 n, char data[])
00006 {
00007     int sum = pt + n;
00008     
00009     for (int i=0; i < n; i++) {
00010         sum += data[i];
00011     }
00012     
00013     return sum;
00014 }
00015 
00016 ////////////////////////////////////////////////////////////////////////////////
00017 // PUBLIC FUNCTIONS
00018 ////////////////////////////////////////////////////////////////////////////////
00019 
00020 TinyCHR6dm::TinyCHR6dm(): _dataReady(false), _statusReady(false), _state(WAIT_S)
00021 {
00022     _state = WAIT_S;
00023 }
00024 
00025    
00026 void TinyCHR6dm::parse(char c)
00027 {
00028 
00029     switch (_state) {
00030         case WAIT_S :
00031             if (debug) debug->printf("WAIT_S\n");
00032             if (c == 's') _state = WAIT_N;
00033             break;
00034         case WAIT_N :
00035             if (debug) debug->printf("WAIT_N\n");
00036             _state = (c == 'n') ? WAIT_P : WAIT_S;
00037             break;
00038         case WAIT_P :
00039             if (debug) debug->printf("WAIT_P\n");
00040             _state = (c == 'p') ? RX_TYPE : WAIT_S;
00041             break;
00042         case RX_TYPE :
00043             pt = c;
00044             _state = RX_N;
00045             if (debug) debug->printf("PT = %02x\n", pt);
00046             break;
00047         case RX_N :
00048             n = (uint8) c;
00049             d = 0;
00050             _state = (n < MAX_BYTES) ? RX_PACKET : WAIT_S;
00051             if (debug) debug->printf("N = %d\n", n);
00052             break;
00053         case RX_PACKET :
00054             if (debug) debug->printf("RX_PACKET\n");
00055             if (d >= n || d >= MAX_BYTES) {
00056                 _state = PROCESS_PACKET;
00057             } else {
00058                 if (debug) debug->printf("data[%d] = %02x\n", d, c);
00059                 data[d++] = c;
00060             }
00061             break;
00062         case PROCESS_PACKET :
00063             if (debug) debug->printf("PROCESS_PACKET\n");
00064             process_packet();
00065             _state = WAIT_S;
00066             break;
00067         default :
00068             _state = WAIT_S;
00069             break;
00070     }
00071 
00072     return;
00073 }
00074 
00075 
00076 float TinyCHR6dm::readYaw(void) {
00077     return TinyCHR6dm::_yaw;
00078 }
00079 
00080 bool TinyCHR6dm::dataReady(void) {
00081     return TinyCHR6dm::_dataReady;
00082 }
00083 
00084 void TinyCHR6dm::resetReady(void) {
00085     TinyCHR6dm::_dataReady = false;
00086 }
00087 
00088 
00089 
00090 void TinyCHR6dm::send_packet(Serial *serial, uint8 pt, uint8 n, char data[])
00091 {
00092     uint checksum;    
00093     char p[MAX_BYTES];
00094 
00095     p[0] = 's';
00096     p[1] = 'n';
00097     p[2] = 'p';
00098     p[3] = (char) pt;
00099     p[4] = (char) n;
00100     
00101     /** Checksum
00102      * Datasheet:
00103      * After the CHR6-dm receives a full  acket, it checks to ensure that the checksum
00104      * given in the last two bytes matches the sum of all preceding bytes in the packet. 
00105      */
00106     checksum = 's'+'n'+'p'+pt+n;
00107     for (int i=0; i < n; i++) {
00108         p[i+5] = data[i];
00109         checksum += data[i];
00110     }
00111     if (debug) debug->printf("Checksum: %04x\n", checksum);
00112     p[5+n]   = (checksum >> 8);       // checksum MSB
00113     p[5+n+1] = (checksum & 0x0ff);   // checksum LSB
00114 
00115     if (debug) debug->printf("Checksum: %02x %02x\n", p[5+n], p[5+n+1]);
00116 
00117     if (serial) {
00118         for (int i=0; i < 5+n+2; i++) {
00119             serial->putc((int) p[i]);
00120             if (debug) debug->printf("%02x\n", p[i]);
00121         }
00122     }
00123     
00124 }
00125 
00126 int TinyCHR6dm::status(void) {
00127     _statusReady = false;
00128     return _status;
00129 }
00130 
00131 bool TinyCHR6dm::statusReady(void) {
00132     return _statusReady;
00133 }
00134 
00135 char *TinyCHR6dm::statusString(int status) {
00136     char *s[7] = { "unknown",
00137                    "PT_COMMAND_COMPLETE",
00138                    "PT_COMMAND_FAILED",
00139                    "PT_BAD_CHECKSUM",
00140                    "PT_BAD_DATA_LENGTH",
00141                    "PT_UNRECOGNIZED_PACKET",
00142                    "PT_BUFFER_OVERFLOW" };
00143 
00144     return (status >= PT_COMMAND_COMPLETE && status <= PT_BUFFER_OVERFLOW ) ? s[status - PT_COMMAND_COMPLETE + 1] : s[0];
00145 }
00146 
00147 ////////////////////////////////////////////////////////////////////////////////
00148 // PRIVATE FUNCTIONS
00149 ////////////////////////////////////////////////////////////////////////////////
00150 
00151 void TinyCHR6dm::process_packet(void) 
00152 {
00153     
00154     switch (pt) {
00155         /** SENSOR_DATA 
00156          * Datasheet:
00157          * If all channels are active, then data is given in the following order: { yaw, pitch, roll, yaw_rate, 
00158          * pitch_rate, roll_rate, mag_z, mag_y, mag_x, gyro_z, gyro_y, gyro_x, accel_z, accel_y, accel_x }.  
00159          * Data bytes D3 and D4 correspond to the yaw angle, D5 and D6 to the pitch angle, etc.  Data is 
00160          * returned as 16-bit two's complement integers. 
00161          *
00162          * When one or more channel is inactive, then the data is returned in the same order, but skipping the 
00163          * inactive channels.  For example, if all magnetic field and rate gyro channels are disabled, then the 
00164          * data is given in the following order: { yaw, pitch, roll, accel_z, accel_y, accel_x } 
00165          */
00166         case PT_SENSOR_DATA :
00167             if (debug) debug->printf("SENSOR_DATA, YAW FLAG: %02x\n", data[0]&YAW_FLAG);
00168             
00169             if ((data[0] & YAW_FLAG) == YAW_FLAG) {
00170                 int yaw = (int) (data[2] << 8 | data[3]);
00171                 float yawf = 360.0 * (float) yaw / 32768.0;
00172                 while (yawf < 0)      yawf += 360.0;
00173                 while (yawf >= 360.0) yawf -= 360.0;
00174                 TinyCHR6dm::_yaw = yawf;
00175                 if (debug) debug->printf("Yaw: %.2f\n", yawf);
00176             }
00177             _dataReady = true;
00178             break;
00179         case PT_COMMAND_COMPLETE :
00180         case PT_COMMAND_FAILED :
00181         case PT_BAD_CHECKSUM :
00182         case PT_BAD_DATA_LENGTH :
00183             if (debug) debug->printf("PT = %02x\n", pt);
00184             _status = pt;
00185             _statusReady = true;
00186             break;
00187         default :
00188             if (debug) debug->printf("Packet type %02x was not processed\n", pt);
00189             break;
00190     }
00191 }