File content as of revision 0:f4a4bfc22cdc:

#include "mbed.h"

/** CHR-6dm test program 
 * http://www.chrobotics.com/index.php?main_page=product_info&cPath=1&products_id=2
 * The CH Robotics 6dm is an AHRS that transmits data over serial port
 *
 * Michael Shimniok - http://bot-thoughts.com/
 *
 * For now all this code does is parses broadcast output and prints out yaw data
 * Some of the framework is in place to print more.  I'll be workign on a
 * library for the device in the near future.
 */

#define sbi(byte, bit)      ((byte) |= _BV(bit))
#define cbi(byte, bit)      ((byte) &= ~_BV(bit))
#define _BV(bit)            (0x01<<(bit))    

Serial pc(USBTX, USBRX);
Serial ahrs(p26, p25);

typedef unsigned char uint8;
typedef unsigned int uint;

/** Rx packet types (commands) */

/** Tx packet types (responses) */
#define PT_COMMAND_COMPLETE             0xB0
#define PT_COMMAND_FAILED               0xB1
#define PT_BAD_CHECKSUM                 0xB2
#define PT_BAD_DATA_LENGTH              0xB3
#define PT_UNRECOGNIZED_PACKET          0xB4  
#define PT_BUFFER_OVERFLOW              0xB5
#define PT_STATUS_REPORT                0xB6
#define PT_SENSOR_DATA                  0xB7
#define PT_GYRO_BIAS_REPORT             0xB8
#define PT_GYRO_SCALE_REPORT            0xB9
#define PT_START_CAL_REPORT             0xBA
#define PT_ACCEL_BIAS_REPORT            0xBB
#define PT_ACCEL_REF_VECTOR_REPORT      0xBC
#define PT_ACTIVE_CHANNEL_REPORT        0xBD
#define PT_ACCEL_COVARIANCE_REPORT      0xBE
#define PT_MAG_COVARIANCE_REPORT        0xBF
#define PT_PROCESS_COVARIANCE_REPORT    0xC0
#define PT_STATE_COVARIANCE_REPORT      0xC1
#define PT_EKF_CONFIG_REPORT            0xC2
#define PT_GYRO_ALIGNMENT_REPORT        0xC3
#define PT_ACCEL_ALIGNMENT_REPORT       0xC4
#define PT_MAG_REF_VECTOR_REPORT        0xC5
#define PT_MAG_CAL_REPORT               0xC6
#define PT_MAG_BIAS_REPORT              0xC7
#define PT_BROADCAST_MODE_REPORT        0xC8


/* flags for SENSOR_DATA, ACTIVE_CHANNEL_REPORT */
/* D1 */
#define YAW_FLAG                0x80
#define PITCH_FLAG              0x40
#define ROLL_FLAG               0x20
#define YAW_RATE_FLAG           0x10
#define PITCH_RATE_FLAG         0x08
#define ROLL_RATE_FLAG          0x04
#define MX_FLAG                 0x02
#define MY_FLAG                 0x01
/* D2 */
#define MZ_FLAG                 0x80
#define GX_FLAG                 0x40
#define GY_FLAG                 0x20
#define GZ_FLAG                 0x10
#define AX_FLAG                 0x08
#define AY_FLAG                 0x04
#define AZ_FLAG                 0x02
#define ZERO_FLAG               0x01


#define MAX_BYTES 32

enum _states { WAIT_S, WAIT_N, WAIT_P, RX_TYPE, RX_N, RX_PACKET, PROCESS_PACKET };

int _state = WAIT_S;
char c;
int d;
uint8 pt;
uint n;
char data[MAX_BYTES];
int yaw;

int chksum(uint8 pt, uint8 n, char data[])
{
    int sum = pt + n;
    
    for (int i=0; i < n; i++) {
        sum += data[i];
    }
    
    return sum;
}
    
void send_packet(uint8 pt, uint8 n, char data[])
{
    char s[MAX_BYTES];
    
    ahrs.printf(s, "snp");
    ahrs.putc(pt);
    ahrs.putc(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. 
     */
    int chksum = 's'+'n'+'p'+pt+n;
    for (int i=0; i < n; i++) {
        ahrs.putc(data[i]);
        chksum += data[i];
    }
    ahrs.putc((char) (chksum >> 8));   // MSByte
    ahrs.putc((char) (chksum & 0x0F)); // LSByte
}


void 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 :
            pc.printf("SENSOR_DATA\n");
            if ((data[0] & YAW_FLAG) == YAW_FLAG) {
                yaw = data[2] << 8 | data[3];       
                pc.printf("Yaw: %d\n", yaw);
            }
            break;
        case PT_COMMAND_COMPLETE :
            pc.printf("Command Complete\n");
            break;
        case PT_COMMAND_FAILED :
            pc.printf("Command Failed\n");
            break;
        case PT_BAD_CHECKSUM :
            pc.printf("Bad Checksum\n");
            break;
        case PT_BAD_DATA_LENGTH :
            pc.printf("Bad Data Length\n");
            break;
        case PT_UNRECOGNIZED_PACKET :
            pc.printf("Unrecognized Packet\n");
            break;
        case PT_BUFFER_OVERFLOW :
            pc.printf("Buffer Overflow\n");
            break;            
        default :
            break;
    }
}


int main() {

    pc.baud(115200);
    ahrs.baud(115200);

    pc.printf("Hello!\nCHR-6dm Test Program\nMichael Shimniok - http://bot-thoughts.com/");

    while (1) { 
        if (ahrs.readable()) {

            c = ahrs.getc();
            switch (_state) {
                case WAIT_S :
                    ///pc.printf("WAIT_S\n");
                    if (c == 's') _state = WAIT_N;
                    break;
                case WAIT_N :
                    //pc.printf("WAIT_N\n");
                    _state = (c == 'n') ? WAIT_P : WAIT_S;
                    break;
                case WAIT_P :
                    //pc.printf("WAIT_P\n");
                    _state = (c == 'p') ? RX_TYPE : WAIT_S;
                    break;
                case RX_TYPE :
                    pt = c;
                    _state = RX_N;
                    pc.printf("PT = %02x\n", pt);
                    break;
                case RX_N :
                    n = ((uint8) c) - 2;
                    d = 0;
                    _state = (n < MAX_BYTES) ? RX_PACKET : WAIT_S;
                    pc.printf("N = %d\n", n);
                    break;
                case RX_PACKET :
                    //pc.printf("data[%d] = %02x\n", d, c);
                    data[d++] = c;
                    if (d >= n || d >= MAX_BYTES) _state = PROCESS_PACKET;
                    break;
                case PROCESS_PACKET :
                    pc.printf("PROCESS_PACKET\n");
                    process_packet();
                    _state = WAIT_S;
                    break;
                default :
                    _state = WAIT_S;
                    break;
            }

            //pc.printf("%02x\n", c);

        }
    }

}