File content as of revision 0:983f66650cd5:

#include <mbed.h>
#include "types.h"

/** Rx packet types (commands) */
#define SET_ACTIVE_CHANNELS             0x80
#define SET_SILENT_MODE                 0x81
#define SET_BROADCAST_MODE              0x82
#define SET_GYRO_BIAS                   0x83
#define SET_ACCEL_BIAS                  0x84
#define SET_ACCEL_REF_VECTOR            0x85
#define AUTO_SET_ACCEL_REF              0x86
#define ZERO_RATE_GYROS                 0x87
#define SELF_TEST                       0x88
#define SET_START_CAL                   0x89
#define SET_PROCESS_COVARIANCE          0x8A
#define SET_MAG_COVARIANCE              0x8B
#define SET_ACCEL_COVARIANCE            0x8C
#define SET_EKF_CONFIG                  0x8D
#define SET_GYRO_ALIGNMENT              0x8E
#define SET_ACCEL_ALIGNMENT             0x8F
#define SET_MAG_REF_VECTOR              0x90
#define AUTO_SET_MAG_REF                0x91
#define SET_MAG_CAL                     0x92
#define SET_MAG_BIAS                    0x93
#define SET_GYRO_SCALE                  0x94
#define EKF_RESET                       0x95
#define RESET_TO_FACTORY                0x96
#define WRITE_TO_FLASH                  0xA0
#define GET_DATA                        0x01
#define GET_ACTIVE_CHANNELS             0x02
#define GET_BROADCAST_MODE              0x03
#define GET_ACCEL_BIAS                  0x04
#define GET_ACCEL_REF_VECTOR            0x05
#define GET_GYRO_BIAS                   0x06
#define GET_GYRO_SCALE                  0x07
#define GET_START_CAL                   0x08
#define GET_EKF_CONFIG                  0x09
#define GET_ACCEL_COVARIANCE            0x0A
#define GET_MAG_COVARIANCE              0x0B
#define GET_PROCESS_COVARIANCE          0x0C
#define GET_STATE_COVARIANCE            0x0D
#define GET_GYRO_ALIGNMENT              0x0E
#define GET_ACCEL_ALIGNMENT             0x0F
#define GET_MAG_REF_VECTOR              0x10
#define GET_MAG_CAL                     0x11
#define GET_MAG_BIAS                    0x12

/** 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

/** EKF sensor enable flags */
#define Mag_EN                          0x01
#define Accel_EN                        0x02

/** 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 64

/** TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS
 *  http://www.chrobotics.com/index.php?main_page=product_info&cPath=1&products_id=2
 *
 * Written by Michael Shimniok http://www.bot-thoughts.com/
 *
 * In the style of TinyGPS, this library parses a subset of the CHR-6dm packets to provide
 * a minimal set of data and status messages to the caller.  Also provides utility to send
 * commands to the AHRS.  This is an early release and a work in progress, so the interface
 * may change significantly over the next couple of versions.  Also, the current version
 * only supports reading yaw data.  More may be supported in the future.
 * @code
 * #include "mbed.h"
 * #include "TinyCHR6dm.h"
 *
 * Serial pc(USBTX, USBRX);
 * Serial ahrs(p26, p25);
 * TinyCHR6dm ahrsParser;
 * DigitalOut myled(LED1);
 * Timer t;
 *
 * void recv() {
 *   while (ahrs.readable())
 *      ahrsParser.parse(ahrs.getc());
 * }
 *
 * int main() {
 *   char data[128];
 *   int status;
 *           
 *   t.reset();
 *   t.start();
 *   pc.baud(115200);
 *   ahrs.baud(115200);
 *   ahrs.attach(recv, Serial::RxIrq);
 *
 *
 *   wait(0.5);
 *
 *   // Configure AHRS
 *   data[0] = Accel_EN;
 *   ahrsParser.send_packet(&ahrs, SET_EKF_CONFIG, 1, data);
 *   wait(0.5);
 *   status = ahrsParser.status();
 *   pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status));
 *   
 *   ahrsParser.send_packet(&ahrs, ZERO_RATE_GYROS, 0, 0);
 *   wait(0.5);
 *   status = ahrsParser.status();
 *   pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status));
 *
 *   ahrsParser.send_packet(&ahrs, EKF_RESET, 0, 0);
 *   wait(0.5);
 *   status = ahrsParser.status();
 *   pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status));
 *
 *   while(1) {
 *     int millis = t.read_ms();
 *     if ((millis % 500) == 0) {
 *       while (!ahrsParser.dataReady());
 *       pc.printf("yaw: %.2f\n", ahrsParser.readYaw());
 *     }
 *   }
 * }
 * @endcode
 */
class TinyCHR6dm {
    public :
        Serial *debug;
    
        /** Creates an protocol parser for CHR-6dm device
         */
        TinyCHR6dm();

        /** Parse a character of data from CHR-6dm
         * @param c is the character to be parsed
         */
        void parse(char c);
        
        /** Get the current yaw (course)
         * @returns the current inertia-frame yaw aka course
         */
        float readYaw(void);
        
        /** Determine if yaw data is ready
         *
         * @param return true right after parsing SENSOR_DATA from AHRS
         */
        bool dataReady(void);
        
        /** Reset the data ready flag
         */        
        void resetReady(void);

        /** Create a packet using specified PT, N, and data and adding
         *  'snp' header and 2-byte checksum
         */
        void send_packet(Serial *serial, uint8 pt, uint8 n, char data[]);
        
        /** Return latest status packet type sent by the AHRS, resets statusReady() to false
         *
         * @param returns the status as an integer corresponding to the TX packet type
         */
        int status(void);
       
        /** Indicates whether a status message is ready to be read with status()
         *
         * @param returns true if a status message has been received since the last call to status()
         */
        bool statusReady(void);
       
        /** Converts status to readable text string
         *
         * @param returns pointer to static string containing status message text
         */
        char *statusString(int status);
       
    private :

        enum _states { WAIT_S, WAIT_N, WAIT_P, RX_TYPE, RX_N, RX_PACKET, PROCESS_PACKET };
        
        /** Process the packet once the data bytes are read
         */
        void process_packet(void);
        int _status;
        float _yaw;
        bool _dataReady;
        bool _statusReady;
        char data[MAX_BYTES];
        uint8 pt;
        uint n;
        int d;
        int _state;
};