TinyCHR6dm.h

00001 #include <mbed.h>
00002 #include "types.h"
00003 
00004 /** Rx packet types (commands) */
00005 #define SET_ACTIVE_CHANNELS             0x80
00006 #define SET_SILENT_MODE                 0x81
00007 #define SET_BROADCAST_MODE              0x82
00008 #define SET_GYRO_BIAS                   0x83
00009 #define SET_ACCEL_BIAS                  0x84
00010 #define SET_ACCEL_REF_VECTOR            0x85
00011 #define AUTO_SET_ACCEL_REF              0x86
00012 #define ZERO_RATE_GYROS                 0x87
00013 #define SELF_TEST                       0x88
00014 #define SET_START_CAL                   0x89
00015 #define SET_PROCESS_COVARIANCE          0x8A
00016 #define SET_MAG_COVARIANCE              0x8B
00017 #define SET_ACCEL_COVARIANCE            0x8C
00018 #define SET_EKF_CONFIG                  0x8D
00019 #define SET_GYRO_ALIGNMENT              0x8E
00020 #define SET_ACCEL_ALIGNMENT             0x8F
00021 #define SET_MAG_REF_VECTOR              0x90
00022 #define AUTO_SET_MAG_REF                0x91
00023 #define SET_MAG_CAL                     0x92
00024 #define SET_MAG_BIAS                    0x93
00025 #define SET_GYRO_SCALE                  0x94
00026 #define EKF_RESET                       0x95
00027 #define RESET_TO_FACTORY                0x96
00028 #define WRITE_TO_FLASH                  0xA0
00029 #define GET_DATA                        0x01
00030 #define GET_ACTIVE_CHANNELS             0x02
00031 #define GET_BROADCAST_MODE              0x03
00032 #define GET_ACCEL_BIAS                  0x04
00033 #define GET_ACCEL_REF_VECTOR            0x05
00034 #define GET_GYRO_BIAS                   0x06
00035 #define GET_GYRO_SCALE                  0x07
00036 #define GET_START_CAL                   0x08
00037 #define GET_EKF_CONFIG                  0x09
00038 #define GET_ACCEL_COVARIANCE            0x0A
00039 #define GET_MAG_COVARIANCE              0x0B
00040 #define GET_PROCESS_COVARIANCE          0x0C
00041 #define GET_STATE_COVARIANCE            0x0D
00042 #define GET_GYRO_ALIGNMENT              0x0E
00043 #define GET_ACCEL_ALIGNMENT             0x0F
00044 #define GET_MAG_REF_VECTOR              0x10
00045 #define GET_MAG_CAL                     0x11
00046 #define GET_MAG_BIAS                    0x12
00047 
00048 /** Tx packet types (responses) */
00049 #define PT_COMMAND_COMPLETE             0xB0
00050 #define PT_COMMAND_FAILED               0xB1
00051 #define PT_BAD_CHECKSUM                 0xB2
00052 #define PT_BAD_DATA_LENGTH              0xB3
00053 #define PT_UNRECOGNIZED_PACKET          0xB4  
00054 #define PT_BUFFER_OVERFLOW              0xB5
00055 #define PT_STATUS_REPORT                0xB6
00056 #define PT_SENSOR_DATA                  0xB7
00057 #define PT_GYRO_BIAS_REPORT             0xB8
00058 #define PT_GYRO_SCALE_REPORT            0xB9
00059 #define PT_START_CAL_REPORT             0xBA
00060 #define PT_ACCEL_BIAS_REPORT            0xBB
00061 #define PT_ACCEL_REF_VECTOR_REPORT      0xBC
00062 #define PT_ACTIVE_CHANNEL_REPORT        0xBD
00063 #define PT_ACCEL_COVARIANCE_REPORT      0xBE
00064 #define PT_MAG_COVARIANCE_REPORT        0xBF
00065 #define PT_PROCESS_COVARIANCE_REPORT    0xC0
00066 #define PT_STATE_COVARIANCE_REPORT      0xC1
00067 #define PT_EKF_CONFIG_REPORT            0xC2
00068 #define PT_GYRO_ALIGNMENT_REPORT        0xC3
00069 #define PT_ACCEL_ALIGNMENT_REPORT       0xC4
00070 #define PT_MAG_REF_VECTOR_REPORT        0xC5
00071 #define PT_MAG_CAL_REPORT               0xC6
00072 #define PT_MAG_BIAS_REPORT              0xC7
00073 #define PT_BROADCAST_MODE_REPORT        0xC8
00074 
00075 /** EKF sensor enable flags */
00076 #define Mag_EN                          0x01
00077 #define Accel_EN                        0x02
00078 
00079 /** flags for SENSOR_DATA, ACTIVE_CHANNEL_REPORT */
00080 /** D1 */
00081 #define YAW_FLAG                0x80
00082 #define PITCH_FLAG              0x40
00083 #define ROLL_FLAG               0x20
00084 #define YAW_RATE_FLAG           0x10
00085 #define PITCH_RATE_FLAG         0x08
00086 #define ROLL_RATE_FLAG          0x04
00087 #define MX_FLAG                 0x02
00088 #define MY_FLAG                 0x01
00089 /** D2 */
00090 #define MZ_FLAG                 0x80
00091 #define GX_FLAG                 0x40
00092 #define GY_FLAG                 0x20
00093 #define GZ_FLAG                 0x10
00094 #define AX_FLAG                 0x08
00095 #define AY_FLAG                 0x04
00096 #define AZ_FLAG                 0x02
00097 #define ZERO_FLAG               0x01
00098 
00099 #define MAX_BYTES 64
00100 
00101 /** TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS
00102  *  http://www.chrobotics.com/index.php?main_page=product_info&cPath=1&products_id=2
00103  *
00104  * Written by Michael Shimniok http://www.bot-thoughts.com/
00105  *
00106  * In the style of TinyGPS, this library parses a subset of the CHR-6dm packets to provide
00107  * a minimal set of data and status messages to the caller.  Also provides utility to send
00108  * commands to the AHRS.  This is an early release and a work in progress, so the interface
00109  * may change significantly over the next couple of versions.  Also, the current version
00110  * only supports reading yaw data.  More may be supported in the future.
00111  * @code
00112  * #include "mbed.h"
00113  * #include "TinyCHR6dm.h"
00114  *
00115  * Serial pc(USBTX, USBRX);
00116  * Serial ahrs(p26, p25);
00117  * TinyCHR6dm ahrsParser;
00118  * DigitalOut myled(LED1);
00119  * Timer t;
00120  *
00121  * void recv() {
00122  *   while (ahrs.readable())
00123  *      ahrsParser.parse(ahrs.getc());
00124  * }
00125  *
00126  * int main() {
00127  *   char data[128];
00128  *   int status;
00129  *           
00130  *   t.reset();
00131  *   t.start();
00132  *   pc.baud(115200);
00133  *   ahrs.baud(115200);
00134  *   ahrs.attach(recv, Serial::RxIrq);
00135  *
00136  *
00137  *   wait(0.5);
00138  *
00139  *   // Configure AHRS
00140  *   data[0] = Accel_EN;
00141  *   ahrsParser.send_packet(&ahrs, SET_EKF_CONFIG, 1, data);
00142  *   wait(0.5);
00143  *   status = ahrsParser.status();
00144  *   pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status));
00145  *   
00146  *   ahrsParser.send_packet(&ahrs, ZERO_RATE_GYROS, 0, 0);
00147  *   wait(0.5);
00148  *   status = ahrsParser.status();
00149  *   pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status));
00150  *
00151  *   ahrsParser.send_packet(&ahrs, EKF_RESET, 0, 0);
00152  *   wait(0.5);
00153  *   status = ahrsParser.status();
00154  *   pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status));
00155  *
00156  *   while(1) {
00157  *     int millis = t.read_ms();
00158  *     if ((millis % 500) == 0) {
00159  *       while (!ahrsParser.dataReady());
00160  *       pc.printf("yaw: %.2f\n", ahrsParser.readYaw());
00161  *     }
00162  *   }
00163  * }
00164  * @endcode
00165  */
00166 class TinyCHR6dm {
00167     public :
00168         Serial *debug;
00169     
00170         /** Creates an protocol parser for CHR-6dm device
00171          */
00172         TinyCHR6dm();
00173 
00174         /** Parse a character of data from CHR-6dm
00175          * @param c is the character to be parsed
00176          */
00177         void parse(char c);
00178         
00179         /** Get the current yaw (course)
00180          * @returns the current inertia-frame yaw aka course
00181          */
00182         float readYaw(void);
00183         
00184         /** Determine if yaw data is ready
00185          *
00186          * @param return true right after parsing SENSOR_DATA from AHRS
00187          */
00188         bool dataReady(void);
00189         
00190         /** Reset the data ready flag
00191          */        
00192         void resetReady(void);
00193 
00194         /** Create a packet using specified PT, N, and data and adding
00195          *  'snp' header and 2-byte checksum
00196          */
00197         void send_packet(Serial *serial, uint8 pt, uint8 n, char data[]);
00198         
00199         /** Return latest status packet type sent by the AHRS, resets statusReady() to false
00200          *
00201          * @param returns the status as an integer corresponding to the TX packet type
00202          */
00203         int status(void);
00204        
00205         /** Indicates whether a status message is ready to be read with status()
00206          *
00207          * @param returns true if a status message has been received since the last call to status()
00208          */
00209         bool statusReady(void);
00210        
00211         /** Converts status to readable text string
00212          *
00213          * @param returns pointer to static string containing status message text
00214          */
00215         char *statusString(int status);
00216        
00217     private :
00218 
00219         enum _states { WAIT_S, WAIT_N, WAIT_P, RX_TYPE, RX_N, RX_PACKET, PROCESS_PACKET };
00220         
00221         /** Process the packet once the data bytes are read
00222          */
00223         void process_packet(void);
00224         int _status;
00225         float _yaw;
00226         bool _dataReady;
00227         bool _statusReady;
00228         char data[MAX_BYTES];
00229         uint8 pt;
00230         uint n;
00231         int d;
00232         int _state;
00233 };