Michael Shimniok / TinyCHR6dm

TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS

Dependents:   AVC_20110423

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?

UserRevisionLine numberNew 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 };