Revision 0:983f66650cd5, committed 2011-04-13

Comitter:

shimniok

Date:

Wed Apr 13 23:32:04 2011 +0000

Commit message:

Initial release

Changed in this revision

diff -r 000000000000 -r 983f66650cd5 TinyCHR6dm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TinyCHR6dm.cpp	Wed Apr 13 23:32:04 2011 +0000
@@ -0,0 +1,191 @@
+#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;
+    }
+}

diff -r 000000000000 -r 983f66650cd5 TinyCHR6dm.h
--- /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

diff -r 000000000000 -r 983f66650cd5 types.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/types.h	Wed Apr 13 23:32:04 2011 +0000
@@ -0,0 +1,7 @@
+#ifndef __TinyCHR6dm_TYPES_H
+#define __TinyCHR6dm_TYPES_H
+
+typedef unsigned char uint8;
+typedef unsigned int uint;
+
+#endif
\ No newline at end of file