TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS
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