Diff: TinyCHR6dm.h

Revision:

0:983f66650cd5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TinyCHR6dm.h	Wed Apr 13 23:32:04 2011 +0000
@@ -0,0 +1,233 @@
+#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;
+};
\ No newline at end of file