Diff: TinyCHR6dm.cpp

Revision:

0:983f66650cd5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TinyCHR6dm.cpp	Wed Apr 13 23:32:04 2011 +0000
@@ -0,0 +1,191 @@
+#include <mbed.h>
+#include "TinyCHR6dm.h"
+#include "string.h"
+
+int chksum(uint8 pt, uint8 n, char data[])
+{
+    int sum = pt + n;
+    
+    for (int i=0; i < n; i++) {
+        sum += data[i];
+    }
+    
+    return sum;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// PUBLIC FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////
+
+TinyCHR6dm::TinyCHR6dm(): _dataReady(false), _statusReady(false), _state(WAIT_S)
+{
+    _state = WAIT_S;
+}
+
+   
+void TinyCHR6dm::parse(char c)
+{
+
+    switch (_state) {
+        case WAIT_S :
+            if (debug) debug->printf("WAIT_S\n");
+            if (c == 's') _state = WAIT_N;
+            break;
+        case WAIT_N :
+            if (debug) debug->printf("WAIT_N\n");
+            _state = (c == 'n') ? WAIT_P : WAIT_S;
+            break;
+        case WAIT_P :
+            if (debug) debug->printf("WAIT_P\n");
+            _state = (c == 'p') ? RX_TYPE : WAIT_S;
+            break;
+        case RX_TYPE :
+            pt = c;
+            _state = RX_N;
+            if (debug) debug->printf("PT = %02x\n", pt);
+            break;
+        case RX_N :
+            n = (uint8) c;
+            d = 0;
+            _state = (n < MAX_BYTES) ? RX_PACKET : WAIT_S;
+            if (debug) debug->printf("N = %d\n", n);
+            break;
+        case RX_PACKET :
+            if (debug) debug->printf("RX_PACKET\n");
+            if (d >= n || d >= MAX_BYTES) {
+                _state = PROCESS_PACKET;
+            } else {
+                if (debug) debug->printf("data[%d] = %02x\n", d, c);
+                data[d++] = c;
+            }
+            break;
+        case PROCESS_PACKET :
+            if (debug) debug->printf("PROCESS_PACKET\n");
+            process_packet();
+            _state = WAIT_S;
+            break;
+        default :
+            _state = WAIT_S;
+            break;
+    }
+
+    return;
+}
+
+
+float TinyCHR6dm::readYaw(void) {
+    return TinyCHR6dm::_yaw;
+}
+
+bool TinyCHR6dm::dataReady(void) {
+    return TinyCHR6dm::_dataReady;
+}
+
+void TinyCHR6dm::resetReady(void) {
+    TinyCHR6dm::_dataReady = false;
+}
+
+
+
+void TinyCHR6dm::send_packet(Serial *serial, uint8 pt, uint8 n, char data[])
+{
+    uint checksum;    
+    char p[MAX_BYTES];
+
+    p[0] = 's';
+    p[1] = 'n';
+    p[2] = 'p';
+    p[3] = (char) pt;
+    p[4] = (char) n;
+    
+    /** Checksum
+     * Datasheet:
+     * After the CHR6-dm receives a full  acket, it checks to ensure that the checksum
+     * given in the last two bytes matches the sum of all preceding bytes in the packet. 
+     */
+    checksum = 's'+'n'+'p'+pt+n;
+    for (int i=0; i < n; i++) {
+        p[i+5] = data[i];
+        checksum += data[i];
+    }
+    if (debug) debug->printf("Checksum: %04x\n", checksum);
+    p[5+n]   = (checksum >> 8);       // checksum MSB
+    p[5+n+1] = (checksum & 0x0ff);   // checksum LSB
+
+    if (debug) debug->printf("Checksum: %02x %02x\n", p[5+n], p[5+n+1]);
+
+    if (serial) {
+        for (int i=0; i < 5+n+2; i++) {
+            serial->putc((int) p[i]);
+            if (debug) debug->printf("%02x\n", p[i]);
+        }
+    }
+    
+}
+
+int TinyCHR6dm::status(void) {
+    _statusReady = false;
+    return _status;
+}
+
+bool TinyCHR6dm::statusReady(void) {
+    return _statusReady;
+}
+
+char *TinyCHR6dm::statusString(int status) {
+    char *s[7] = { "unknown",
+                   "PT_COMMAND_COMPLETE",
+                   "PT_COMMAND_FAILED",
+                   "PT_BAD_CHECKSUM",
+                   "PT_BAD_DATA_LENGTH",
+                   "PT_UNRECOGNIZED_PACKET",
+                   "PT_BUFFER_OVERFLOW" };
+
+    return (status >= PT_COMMAND_COMPLETE && status <= PT_BUFFER_OVERFLOW ) ? s[status - PT_COMMAND_COMPLETE + 1] : s[0];
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// PRIVATE FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////
+
+void TinyCHR6dm::process_packet(void) 
+{
+    
+    switch (pt) {
+        /** SENSOR_DATA 
+         * Datasheet:
+         * If all channels are active, then data is given in the following order: { yaw, pitch, roll, yaw_rate, 
+         * pitch_rate, roll_rate, mag_z, mag_y, mag_x, gyro_z, gyro_y, gyro_x, accel_z, accel_y, accel_x }.  
+         * Data bytes D3 and D4 correspond to the yaw angle, D5 and D6 to the pitch angle, etc.  Data is 
+         * returned as 16-bit two's complement integers. 
+         *
+         * When one or more channel is inactive, then the data is returned in the same order, but skipping the 
+         * inactive channels.  For example, if all magnetic field and rate gyro channels are disabled, then the 
+         * data is given in the following order: { yaw, pitch, roll, accel_z, accel_y, accel_x } 
+         */
+        case PT_SENSOR_DATA :
+            if (debug) debug->printf("SENSOR_DATA, YAW FLAG: %02x\n", data[0]&YAW_FLAG);
+            
+            if ((data[0] & YAW_FLAG) == YAW_FLAG) {
+                int yaw = (int) (data[2] << 8 | data[3]);
+                float yawf = 360.0 * (float) yaw / 32768.0;
+                while (yawf < 0)      yawf += 360.0;
+                while (yawf >= 360.0) yawf -= 360.0;
+                TinyCHR6dm::_yaw = yawf;
+                if (debug) debug->printf("Yaw: %.2f\n", yawf);
+            }
+            _dataReady = true;
+            break;
+        case PT_COMMAND_COMPLETE :
+        case PT_COMMAND_FAILED :
+        case PT_BAD_CHECKSUM :
+        case PT_BAD_DATA_LENGTH :
+            if (debug) debug->printf("PT = %02x\n", pt);
+            _status = pt;
+            _statusReady = true;
+            break;
+        default :
+            if (debug) debug->printf("Packet type %02x was not processed\n", pt);
+            break;
+    }
+}