File content as of revision 0:983f66650cd5:

#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;
    }
}