TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS
TinyCHR6dm.h@0:983f66650cd5, 2011-04-13 (annotated)
- Committer:
- shimniok
- Date:
- Wed Apr 13 23:32:04 2011 +0000
- Revision:
- 0:983f66650cd5
Initial release
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:983f66650cd5 | 1 | #include <mbed.h> |
shimniok | 0:983f66650cd5 | 2 | #include "types.h" |
shimniok | 0:983f66650cd5 | 3 | |
shimniok | 0:983f66650cd5 | 4 | /** Rx packet types (commands) */ |
shimniok | 0:983f66650cd5 | 5 | #define SET_ACTIVE_CHANNELS 0x80 |
shimniok | 0:983f66650cd5 | 6 | #define SET_SILENT_MODE 0x81 |
shimniok | 0:983f66650cd5 | 7 | #define SET_BROADCAST_MODE 0x82 |
shimniok | 0:983f66650cd5 | 8 | #define SET_GYRO_BIAS 0x83 |
shimniok | 0:983f66650cd5 | 9 | #define SET_ACCEL_BIAS 0x84 |
shimniok | 0:983f66650cd5 | 10 | #define SET_ACCEL_REF_VECTOR 0x85 |
shimniok | 0:983f66650cd5 | 11 | #define AUTO_SET_ACCEL_REF 0x86 |
shimniok | 0:983f66650cd5 | 12 | #define ZERO_RATE_GYROS 0x87 |
shimniok | 0:983f66650cd5 | 13 | #define SELF_TEST 0x88 |
shimniok | 0:983f66650cd5 | 14 | #define SET_START_CAL 0x89 |
shimniok | 0:983f66650cd5 | 15 | #define SET_PROCESS_COVARIANCE 0x8A |
shimniok | 0:983f66650cd5 | 16 | #define SET_MAG_COVARIANCE 0x8B |
shimniok | 0:983f66650cd5 | 17 | #define SET_ACCEL_COVARIANCE 0x8C |
shimniok | 0:983f66650cd5 | 18 | #define SET_EKF_CONFIG 0x8D |
shimniok | 0:983f66650cd5 | 19 | #define SET_GYRO_ALIGNMENT 0x8E |
shimniok | 0:983f66650cd5 | 20 | #define SET_ACCEL_ALIGNMENT 0x8F |
shimniok | 0:983f66650cd5 | 21 | #define SET_MAG_REF_VECTOR 0x90 |
shimniok | 0:983f66650cd5 | 22 | #define AUTO_SET_MAG_REF 0x91 |
shimniok | 0:983f66650cd5 | 23 | #define SET_MAG_CAL 0x92 |
shimniok | 0:983f66650cd5 | 24 | #define SET_MAG_BIAS 0x93 |
shimniok | 0:983f66650cd5 | 25 | #define SET_GYRO_SCALE 0x94 |
shimniok | 0:983f66650cd5 | 26 | #define EKF_RESET 0x95 |
shimniok | 0:983f66650cd5 | 27 | #define RESET_TO_FACTORY 0x96 |
shimniok | 0:983f66650cd5 | 28 | #define WRITE_TO_FLASH 0xA0 |
shimniok | 0:983f66650cd5 | 29 | #define GET_DATA 0x01 |
shimniok | 0:983f66650cd5 | 30 | #define GET_ACTIVE_CHANNELS 0x02 |
shimniok | 0:983f66650cd5 | 31 | #define GET_BROADCAST_MODE 0x03 |
shimniok | 0:983f66650cd5 | 32 | #define GET_ACCEL_BIAS 0x04 |
shimniok | 0:983f66650cd5 | 33 | #define GET_ACCEL_REF_VECTOR 0x05 |
shimniok | 0:983f66650cd5 | 34 | #define GET_GYRO_BIAS 0x06 |
shimniok | 0:983f66650cd5 | 35 | #define GET_GYRO_SCALE 0x07 |
shimniok | 0:983f66650cd5 | 36 | #define GET_START_CAL 0x08 |
shimniok | 0:983f66650cd5 | 37 | #define GET_EKF_CONFIG 0x09 |
shimniok | 0:983f66650cd5 | 38 | #define GET_ACCEL_COVARIANCE 0x0A |
shimniok | 0:983f66650cd5 | 39 | #define GET_MAG_COVARIANCE 0x0B |
shimniok | 0:983f66650cd5 | 40 | #define GET_PROCESS_COVARIANCE 0x0C |
shimniok | 0:983f66650cd5 | 41 | #define GET_STATE_COVARIANCE 0x0D |
shimniok | 0:983f66650cd5 | 42 | #define GET_GYRO_ALIGNMENT 0x0E |
shimniok | 0:983f66650cd5 | 43 | #define GET_ACCEL_ALIGNMENT 0x0F |
shimniok | 0:983f66650cd5 | 44 | #define GET_MAG_REF_VECTOR 0x10 |
shimniok | 0:983f66650cd5 | 45 | #define GET_MAG_CAL 0x11 |
shimniok | 0:983f66650cd5 | 46 | #define GET_MAG_BIAS 0x12 |
shimniok | 0:983f66650cd5 | 47 | |
shimniok | 0:983f66650cd5 | 48 | /** Tx packet types (responses) */ |
shimniok | 0:983f66650cd5 | 49 | #define PT_COMMAND_COMPLETE 0xB0 |
shimniok | 0:983f66650cd5 | 50 | #define PT_COMMAND_FAILED 0xB1 |
shimniok | 0:983f66650cd5 | 51 | #define PT_BAD_CHECKSUM 0xB2 |
shimniok | 0:983f66650cd5 | 52 | #define PT_BAD_DATA_LENGTH 0xB3 |
shimniok | 0:983f66650cd5 | 53 | #define PT_UNRECOGNIZED_PACKET 0xB4 |
shimniok | 0:983f66650cd5 | 54 | #define PT_BUFFER_OVERFLOW 0xB5 |
shimniok | 0:983f66650cd5 | 55 | #define PT_STATUS_REPORT 0xB6 |
shimniok | 0:983f66650cd5 | 56 | #define PT_SENSOR_DATA 0xB7 |
shimniok | 0:983f66650cd5 | 57 | #define PT_GYRO_BIAS_REPORT 0xB8 |
shimniok | 0:983f66650cd5 | 58 | #define PT_GYRO_SCALE_REPORT 0xB9 |
shimniok | 0:983f66650cd5 | 59 | #define PT_START_CAL_REPORT 0xBA |
shimniok | 0:983f66650cd5 | 60 | #define PT_ACCEL_BIAS_REPORT 0xBB |
shimniok | 0:983f66650cd5 | 61 | #define PT_ACCEL_REF_VECTOR_REPORT 0xBC |
shimniok | 0:983f66650cd5 | 62 | #define PT_ACTIVE_CHANNEL_REPORT 0xBD |
shimniok | 0:983f66650cd5 | 63 | #define PT_ACCEL_COVARIANCE_REPORT 0xBE |
shimniok | 0:983f66650cd5 | 64 | #define PT_MAG_COVARIANCE_REPORT 0xBF |
shimniok | 0:983f66650cd5 | 65 | #define PT_PROCESS_COVARIANCE_REPORT 0xC0 |
shimniok | 0:983f66650cd5 | 66 | #define PT_STATE_COVARIANCE_REPORT 0xC1 |
shimniok | 0:983f66650cd5 | 67 | #define PT_EKF_CONFIG_REPORT 0xC2 |
shimniok | 0:983f66650cd5 | 68 | #define PT_GYRO_ALIGNMENT_REPORT 0xC3 |
shimniok | 0:983f66650cd5 | 69 | #define PT_ACCEL_ALIGNMENT_REPORT 0xC4 |
shimniok | 0:983f66650cd5 | 70 | #define PT_MAG_REF_VECTOR_REPORT 0xC5 |
shimniok | 0:983f66650cd5 | 71 | #define PT_MAG_CAL_REPORT 0xC6 |
shimniok | 0:983f66650cd5 | 72 | #define PT_MAG_BIAS_REPORT 0xC7 |
shimniok | 0:983f66650cd5 | 73 | #define PT_BROADCAST_MODE_REPORT 0xC8 |
shimniok | 0:983f66650cd5 | 74 | |
shimniok | 0:983f66650cd5 | 75 | /** EKF sensor enable flags */ |
shimniok | 0:983f66650cd5 | 76 | #define Mag_EN 0x01 |
shimniok | 0:983f66650cd5 | 77 | #define Accel_EN 0x02 |
shimniok | 0:983f66650cd5 | 78 | |
shimniok | 0:983f66650cd5 | 79 | /** flags for SENSOR_DATA, ACTIVE_CHANNEL_REPORT */ |
shimniok | 0:983f66650cd5 | 80 | /** D1 */ |
shimniok | 0:983f66650cd5 | 81 | #define YAW_FLAG 0x80 |
shimniok | 0:983f66650cd5 | 82 | #define PITCH_FLAG 0x40 |
shimniok | 0:983f66650cd5 | 83 | #define ROLL_FLAG 0x20 |
shimniok | 0:983f66650cd5 | 84 | #define YAW_RATE_FLAG 0x10 |
shimniok | 0:983f66650cd5 | 85 | #define PITCH_RATE_FLAG 0x08 |
shimniok | 0:983f66650cd5 | 86 | #define ROLL_RATE_FLAG 0x04 |
shimniok | 0:983f66650cd5 | 87 | #define MX_FLAG 0x02 |
shimniok | 0:983f66650cd5 | 88 | #define MY_FLAG 0x01 |
shimniok | 0:983f66650cd5 | 89 | /** D2 */ |
shimniok | 0:983f66650cd5 | 90 | #define MZ_FLAG 0x80 |
shimniok | 0:983f66650cd5 | 91 | #define GX_FLAG 0x40 |
shimniok | 0:983f66650cd5 | 92 | #define GY_FLAG 0x20 |
shimniok | 0:983f66650cd5 | 93 | #define GZ_FLAG 0x10 |
shimniok | 0:983f66650cd5 | 94 | #define AX_FLAG 0x08 |
shimniok | 0:983f66650cd5 | 95 | #define AY_FLAG 0x04 |
shimniok | 0:983f66650cd5 | 96 | #define AZ_FLAG 0x02 |
shimniok | 0:983f66650cd5 | 97 | #define ZERO_FLAG 0x01 |
shimniok | 0:983f66650cd5 | 98 | |
shimniok | 0:983f66650cd5 | 99 | #define MAX_BYTES 64 |
shimniok | 0:983f66650cd5 | 100 | |
shimniok | 0:983f66650cd5 | 101 | /** TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS |
shimniok | 0:983f66650cd5 | 102 | * http://www.chrobotics.com/index.php?main_page=product_info&cPath=1&products_id=2 |
shimniok | 0:983f66650cd5 | 103 | * |
shimniok | 0:983f66650cd5 | 104 | * Written by Michael Shimniok http://www.bot-thoughts.com/ |
shimniok | 0:983f66650cd5 | 105 | * |
shimniok | 0:983f66650cd5 | 106 | * In the style of TinyGPS, this library parses a subset of the CHR-6dm packets to provide |
shimniok | 0:983f66650cd5 | 107 | * a minimal set of data and status messages to the caller. Also provides utility to send |
shimniok | 0:983f66650cd5 | 108 | * commands to the AHRS. This is an early release and a work in progress, so the interface |
shimniok | 0:983f66650cd5 | 109 | * may change significantly over the next couple of versions. Also, the current version |
shimniok | 0:983f66650cd5 | 110 | * only supports reading yaw data. More may be supported in the future. |
shimniok | 0:983f66650cd5 | 111 | * @code |
shimniok | 0:983f66650cd5 | 112 | * #include "mbed.h" |
shimniok | 0:983f66650cd5 | 113 | * #include "TinyCHR6dm.h" |
shimniok | 0:983f66650cd5 | 114 | * |
shimniok | 0:983f66650cd5 | 115 | * Serial pc(USBTX, USBRX); |
shimniok | 0:983f66650cd5 | 116 | * Serial ahrs(p26, p25); |
shimniok | 0:983f66650cd5 | 117 | * TinyCHR6dm ahrsParser; |
shimniok | 0:983f66650cd5 | 118 | * DigitalOut myled(LED1); |
shimniok | 0:983f66650cd5 | 119 | * Timer t; |
shimniok | 0:983f66650cd5 | 120 | * |
shimniok | 0:983f66650cd5 | 121 | * void recv() { |
shimniok | 0:983f66650cd5 | 122 | * while (ahrs.readable()) |
shimniok | 0:983f66650cd5 | 123 | * ahrsParser.parse(ahrs.getc()); |
shimniok | 0:983f66650cd5 | 124 | * } |
shimniok | 0:983f66650cd5 | 125 | * |
shimniok | 0:983f66650cd5 | 126 | * int main() { |
shimniok | 0:983f66650cd5 | 127 | * char data[128]; |
shimniok | 0:983f66650cd5 | 128 | * int status; |
shimniok | 0:983f66650cd5 | 129 | * |
shimniok | 0:983f66650cd5 | 130 | * t.reset(); |
shimniok | 0:983f66650cd5 | 131 | * t.start(); |
shimniok | 0:983f66650cd5 | 132 | * pc.baud(115200); |
shimniok | 0:983f66650cd5 | 133 | * ahrs.baud(115200); |
shimniok | 0:983f66650cd5 | 134 | * ahrs.attach(recv, Serial::RxIrq); |
shimniok | 0:983f66650cd5 | 135 | * |
shimniok | 0:983f66650cd5 | 136 | * |
shimniok | 0:983f66650cd5 | 137 | * wait(0.5); |
shimniok | 0:983f66650cd5 | 138 | * |
shimniok | 0:983f66650cd5 | 139 | * // Configure AHRS |
shimniok | 0:983f66650cd5 | 140 | * data[0] = Accel_EN; |
shimniok | 0:983f66650cd5 | 141 | * ahrsParser.send_packet(&ahrs, SET_EKF_CONFIG, 1, data); |
shimniok | 0:983f66650cd5 | 142 | * wait(0.5); |
shimniok | 0:983f66650cd5 | 143 | * status = ahrsParser.status(); |
shimniok | 0:983f66650cd5 | 144 | * pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:983f66650cd5 | 145 | * |
shimniok | 0:983f66650cd5 | 146 | * ahrsParser.send_packet(&ahrs, ZERO_RATE_GYROS, 0, 0); |
shimniok | 0:983f66650cd5 | 147 | * wait(0.5); |
shimniok | 0:983f66650cd5 | 148 | * status = ahrsParser.status(); |
shimniok | 0:983f66650cd5 | 149 | * pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:983f66650cd5 | 150 | * |
shimniok | 0:983f66650cd5 | 151 | * ahrsParser.send_packet(&ahrs, EKF_RESET, 0, 0); |
shimniok | 0:983f66650cd5 | 152 | * wait(0.5); |
shimniok | 0:983f66650cd5 | 153 | * status = ahrsParser.status(); |
shimniok | 0:983f66650cd5 | 154 | * pc.printf("Status: %02x %s\n", status, ahrsParser.statusString(status)); |
shimniok | 0:983f66650cd5 | 155 | * |
shimniok | 0:983f66650cd5 | 156 | * while(1) { |
shimniok | 0:983f66650cd5 | 157 | * int millis = t.read_ms(); |
shimniok | 0:983f66650cd5 | 158 | * if ((millis % 500) == 0) { |
shimniok | 0:983f66650cd5 | 159 | * while (!ahrsParser.dataReady()); |
shimniok | 0:983f66650cd5 | 160 | * pc.printf("yaw: %.2f\n", ahrsParser.readYaw()); |
shimniok | 0:983f66650cd5 | 161 | * } |
shimniok | 0:983f66650cd5 | 162 | * } |
shimniok | 0:983f66650cd5 | 163 | * } |
shimniok | 0:983f66650cd5 | 164 | * @endcode |
shimniok | 0:983f66650cd5 | 165 | */ |
shimniok | 0:983f66650cd5 | 166 | class TinyCHR6dm { |
shimniok | 0:983f66650cd5 | 167 | public : |
shimniok | 0:983f66650cd5 | 168 | Serial *debug; |
shimniok | 0:983f66650cd5 | 169 | |
shimniok | 0:983f66650cd5 | 170 | /** Creates an protocol parser for CHR-6dm device |
shimniok | 0:983f66650cd5 | 171 | */ |
shimniok | 0:983f66650cd5 | 172 | TinyCHR6dm(); |
shimniok | 0:983f66650cd5 | 173 | |
shimniok | 0:983f66650cd5 | 174 | /** Parse a character of data from CHR-6dm |
shimniok | 0:983f66650cd5 | 175 | * @param c is the character to be parsed |
shimniok | 0:983f66650cd5 | 176 | */ |
shimniok | 0:983f66650cd5 | 177 | void parse(char c); |
shimniok | 0:983f66650cd5 | 178 | |
shimniok | 0:983f66650cd5 | 179 | /** Get the current yaw (course) |
shimniok | 0:983f66650cd5 | 180 | * @returns the current inertia-frame yaw aka course |
shimniok | 0:983f66650cd5 | 181 | */ |
shimniok | 0:983f66650cd5 | 182 | float readYaw(void); |
shimniok | 0:983f66650cd5 | 183 | |
shimniok | 0:983f66650cd5 | 184 | /** Determine if yaw data is ready |
shimniok | 0:983f66650cd5 | 185 | * |
shimniok | 0:983f66650cd5 | 186 | * @param return true right after parsing SENSOR_DATA from AHRS |
shimniok | 0:983f66650cd5 | 187 | */ |
shimniok | 0:983f66650cd5 | 188 | bool dataReady(void); |
shimniok | 0:983f66650cd5 | 189 | |
shimniok | 0:983f66650cd5 | 190 | /** Reset the data ready flag |
shimniok | 0:983f66650cd5 | 191 | */ |
shimniok | 0:983f66650cd5 | 192 | void resetReady(void); |
shimniok | 0:983f66650cd5 | 193 | |
shimniok | 0:983f66650cd5 | 194 | /** Create a packet using specified PT, N, and data and adding |
shimniok | 0:983f66650cd5 | 195 | * 'snp' header and 2-byte checksum |
shimniok | 0:983f66650cd5 | 196 | */ |
shimniok | 0:983f66650cd5 | 197 | void send_packet(Serial *serial, uint8 pt, uint8 n, char data[]); |
shimniok | 0:983f66650cd5 | 198 | |
shimniok | 0:983f66650cd5 | 199 | /** Return latest status packet type sent by the AHRS, resets statusReady() to false |
shimniok | 0:983f66650cd5 | 200 | * |
shimniok | 0:983f66650cd5 | 201 | * @param returns the status as an integer corresponding to the TX packet type |
shimniok | 0:983f66650cd5 | 202 | */ |
shimniok | 0:983f66650cd5 | 203 | int status(void); |
shimniok | 0:983f66650cd5 | 204 | |
shimniok | 0:983f66650cd5 | 205 | /** Indicates whether a status message is ready to be read with status() |
shimniok | 0:983f66650cd5 | 206 | * |
shimniok | 0:983f66650cd5 | 207 | * @param returns true if a status message has been received since the last call to status() |
shimniok | 0:983f66650cd5 | 208 | */ |
shimniok | 0:983f66650cd5 | 209 | bool statusReady(void); |
shimniok | 0:983f66650cd5 | 210 | |
shimniok | 0:983f66650cd5 | 211 | /** Converts status to readable text string |
shimniok | 0:983f66650cd5 | 212 | * |
shimniok | 0:983f66650cd5 | 213 | * @param returns pointer to static string containing status message text |
shimniok | 0:983f66650cd5 | 214 | */ |
shimniok | 0:983f66650cd5 | 215 | char *statusString(int status); |
shimniok | 0:983f66650cd5 | 216 | |
shimniok | 0:983f66650cd5 | 217 | private : |
shimniok | 0:983f66650cd5 | 218 | |
shimniok | 0:983f66650cd5 | 219 | enum _states { WAIT_S, WAIT_N, WAIT_P, RX_TYPE, RX_N, RX_PACKET, PROCESS_PACKET }; |
shimniok | 0:983f66650cd5 | 220 | |
shimniok | 0:983f66650cd5 | 221 | /** Process the packet once the data bytes are read |
shimniok | 0:983f66650cd5 | 222 | */ |
shimniok | 0:983f66650cd5 | 223 | void process_packet(void); |
shimniok | 0:983f66650cd5 | 224 | int _status; |
shimniok | 0:983f66650cd5 | 225 | float _yaw; |
shimniok | 0:983f66650cd5 | 226 | bool _dataReady; |
shimniok | 0:983f66650cd5 | 227 | bool _statusReady; |
shimniok | 0:983f66650cd5 | 228 | char data[MAX_BYTES]; |
shimniok | 0:983f66650cd5 | 229 | uint8 pt; |
shimniok | 0:983f66650cd5 | 230 | uint n; |
shimniok | 0:983f66650cd5 | 231 | int d; |
shimniok | 0:983f66650cd5 | 232 | int _state; |
shimniok | 0:983f66650cd5 | 233 | }; |