Michael Shimniok / Mbed 2 deprecated CHR-6dm_Test

Dependencies:   mbed

main.cpp@0:f4a4bfc22cdc, 2011-04-04 (annotated)

Committer:
shimniok
Date:
Mon Apr 04 20:35:19 2011 +0000
Revision:
0:f4a4bfc22cdc
Initial release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:f4a4bfc22cdc 1 #include "mbed.h"
shimniok 0:f4a4bfc22cdc 2
shimniok 0:f4a4bfc22cdc 3 /** CHR-6dm test program
shimniok 0:f4a4bfc22cdc 4 * http://www.chrobotics.com/index.php?main_page=product_info&cPath=1&products_id=2
shimniok 0:f4a4bfc22cdc 5 * The CH Robotics 6dm is an AHRS that transmits data over serial port
shimniok 0:f4a4bfc22cdc 6 *
shimniok 0:f4a4bfc22cdc 7 * Michael Shimniok - http://bot-thoughts.com/
shimniok 0:f4a4bfc22cdc 8 *
shimniok 0:f4a4bfc22cdc 9 * For now all this code does is parses broadcast output and prints out yaw data
shimniok 0:f4a4bfc22cdc 10 * Some of the framework is in place to print more. I'll be workign on a
shimniok 0:f4a4bfc22cdc 11 * library for the device in the near future.
shimniok 0:f4a4bfc22cdc 12 */
shimniok 0:f4a4bfc22cdc 13
shimniok 0:f4a4bfc22cdc 14 #define sbi(byte, bit) ((byte) |= _BV(bit))
shimniok 0:f4a4bfc22cdc 15 #define cbi(byte, bit) ((byte) &= ~_BV(bit))
shimniok 0:f4a4bfc22cdc 16 #define _BV(bit) (0x01<<(bit))
shimniok 0:f4a4bfc22cdc 17
shimniok 0:f4a4bfc22cdc 18 Serial pc(USBTX, USBRX);
shimniok 0:f4a4bfc22cdc 19 Serial ahrs(p26, p25);
shimniok 0:f4a4bfc22cdc 20
shimniok 0:f4a4bfc22cdc 21 typedef unsigned char uint8;
shimniok 0:f4a4bfc22cdc 22 typedef unsigned int uint;
shimniok 0:f4a4bfc22cdc 23
shimniok 0:f4a4bfc22cdc 24 /** Rx packet types (commands) */
shimniok 0:f4a4bfc22cdc 25
shimniok 0:f4a4bfc22cdc 26 /** Tx packet types (responses) */
shimniok 0:f4a4bfc22cdc 27 #define PT_COMMAND_COMPLETE 0xB0
shimniok 0:f4a4bfc22cdc 28 #define PT_COMMAND_FAILED 0xB1
shimniok 0:f4a4bfc22cdc 29 #define PT_BAD_CHECKSUM 0xB2
shimniok 0:f4a4bfc22cdc 30 #define PT_BAD_DATA_LENGTH 0xB3
shimniok 0:f4a4bfc22cdc 31 #define PT_UNRECOGNIZED_PACKET 0xB4
shimniok 0:f4a4bfc22cdc 32 #define PT_BUFFER_OVERFLOW 0xB5
shimniok 0:f4a4bfc22cdc 33 #define PT_STATUS_REPORT 0xB6
shimniok 0:f4a4bfc22cdc 34 #define PT_SENSOR_DATA 0xB7
shimniok 0:f4a4bfc22cdc 35 #define PT_GYRO_BIAS_REPORT 0xB8
shimniok 0:f4a4bfc22cdc 36 #define PT_GYRO_SCALE_REPORT 0xB9
shimniok 0:f4a4bfc22cdc 37 #define PT_START_CAL_REPORT 0xBA
shimniok 0:f4a4bfc22cdc 38 #define PT_ACCEL_BIAS_REPORT 0xBB
shimniok 0:f4a4bfc22cdc 39 #define PT_ACCEL_REF_VECTOR_REPORT 0xBC
shimniok 0:f4a4bfc22cdc 40 #define PT_ACTIVE_CHANNEL_REPORT 0xBD
shimniok 0:f4a4bfc22cdc 41 #define PT_ACCEL_COVARIANCE_REPORT 0xBE
shimniok 0:f4a4bfc22cdc 42 #define PT_MAG_COVARIANCE_REPORT 0xBF
shimniok 0:f4a4bfc22cdc 43 #define PT_PROCESS_COVARIANCE_REPORT 0xC0
shimniok 0:f4a4bfc22cdc 44 #define PT_STATE_COVARIANCE_REPORT 0xC1
shimniok 0:f4a4bfc22cdc 45 #define PT_EKF_CONFIG_REPORT 0xC2
shimniok 0:f4a4bfc22cdc 46 #define PT_GYRO_ALIGNMENT_REPORT 0xC3
shimniok 0:f4a4bfc22cdc 47 #define PT_ACCEL_ALIGNMENT_REPORT 0xC4
shimniok 0:f4a4bfc22cdc 48 #define PT_MAG_REF_VECTOR_REPORT 0xC5
shimniok 0:f4a4bfc22cdc 49 #define PT_MAG_CAL_REPORT 0xC6
shimniok 0:f4a4bfc22cdc 50 #define PT_MAG_BIAS_REPORT 0xC7
shimniok 0:f4a4bfc22cdc 51 #define PT_BROADCAST_MODE_REPORT 0xC8
shimniok 0:f4a4bfc22cdc 52
shimniok 0:f4a4bfc22cdc 53
shimniok 0:f4a4bfc22cdc 54 /* flags for SENSOR_DATA, ACTIVE_CHANNEL_REPORT */
shimniok 0:f4a4bfc22cdc 55 /* D1 */
shimniok 0:f4a4bfc22cdc 56 #define YAW_FLAG 0x80
shimniok 0:f4a4bfc22cdc 57 #define PITCH_FLAG 0x40
shimniok 0:f4a4bfc22cdc 58 #define ROLL_FLAG 0x20
shimniok 0:f4a4bfc22cdc 59 #define YAW_RATE_FLAG 0x10
shimniok 0:f4a4bfc22cdc 60 #define PITCH_RATE_FLAG 0x08
shimniok 0:f4a4bfc22cdc 61 #define ROLL_RATE_FLAG 0x04
shimniok 0:f4a4bfc22cdc 62 #define MX_FLAG 0x02
shimniok 0:f4a4bfc22cdc 63 #define MY_FLAG 0x01
shimniok 0:f4a4bfc22cdc 64 /* D2 */
shimniok 0:f4a4bfc22cdc 65 #define MZ_FLAG 0x80
shimniok 0:f4a4bfc22cdc 66 #define GX_FLAG 0x40
shimniok 0:f4a4bfc22cdc 67 #define GY_FLAG 0x20
shimniok 0:f4a4bfc22cdc 68 #define GZ_FLAG 0x10
shimniok 0:f4a4bfc22cdc 69 #define AX_FLAG 0x08
shimniok 0:f4a4bfc22cdc 70 #define AY_FLAG 0x04
shimniok 0:f4a4bfc22cdc 71 #define AZ_FLAG 0x02
shimniok 0:f4a4bfc22cdc 72 #define ZERO_FLAG 0x01
shimniok 0:f4a4bfc22cdc 73
shimniok 0:f4a4bfc22cdc 74
shimniok 0:f4a4bfc22cdc 75 #define MAX_BYTES 32
shimniok 0:f4a4bfc22cdc 76
shimniok 0:f4a4bfc22cdc 77 enum _states { WAIT_S, WAIT_N, WAIT_P, RX_TYPE, RX_N, RX_PACKET, PROCESS_PACKET };
shimniok 0:f4a4bfc22cdc 78
shimniok 0:f4a4bfc22cdc 79 int _state = WAIT_S;
shimniok 0:f4a4bfc22cdc 80 char c;
shimniok 0:f4a4bfc22cdc 81 int d;
shimniok 0:f4a4bfc22cdc 82 uint8 pt;
shimniok 0:f4a4bfc22cdc 83 uint n;
shimniok 0:f4a4bfc22cdc 84 char data[MAX_BYTES];
shimniok 0:f4a4bfc22cdc 85 int yaw;
shimniok 0:f4a4bfc22cdc 86
shimniok 0:f4a4bfc22cdc 87 int chksum(uint8 pt, uint8 n, char data[])
shimniok 0:f4a4bfc22cdc 88 {
shimniok 0:f4a4bfc22cdc 89 int sum = pt + n;
shimniok 0:f4a4bfc22cdc 90
shimniok 0:f4a4bfc22cdc 91 for (int i=0; i < n; i++) {
shimniok 0:f4a4bfc22cdc 92 sum += data[i];
shimniok 0:f4a4bfc22cdc 93 }
shimniok 0:f4a4bfc22cdc 94
shimniok 0:f4a4bfc22cdc 95 return sum;
shimniok 0:f4a4bfc22cdc 96 }
shimniok 0:f4a4bfc22cdc 97
shimniok 0:f4a4bfc22cdc 98 void send_packet(uint8 pt, uint8 n, char data[])
shimniok 0:f4a4bfc22cdc 99 {
shimniok 0:f4a4bfc22cdc 100 char s[MAX_BYTES];
shimniok 0:f4a4bfc22cdc 101
shimniok 0:f4a4bfc22cdc 102 ahrs.printf(s, "snp");
shimniok 0:f4a4bfc22cdc 103 ahrs.putc(pt);
shimniok 0:f4a4bfc22cdc 104 ahrs.putc(n);
shimniok 0:f4a4bfc22cdc 105
shimniok 0:f4a4bfc22cdc 106 /** Checksum
shimniok 0:f4a4bfc22cdc 107 * Datasheet:
shimniok 0:f4a4bfc22cdc 108 * After the CHR6-dm receives a full acket, it checks to ensure that the checksum
shimniok 0:f4a4bfc22cdc 109 * given in the last two bytes matches the sum of all preceding bytes in the packet.
shimniok 0:f4a4bfc22cdc 110 */
shimniok 0:f4a4bfc22cdc 111 int chksum = 's'+'n'+'p'+pt+n;
shimniok 0:f4a4bfc22cdc 112 for (int i=0; i < n; i++) {
shimniok 0:f4a4bfc22cdc 113 ahrs.putc(data[i]);
shimniok 0:f4a4bfc22cdc 114 chksum += data[i];
shimniok 0:f4a4bfc22cdc 115 }
shimniok 0:f4a4bfc22cdc 116 ahrs.putc((char) (chksum >> 8)); // MSByte
shimniok 0:f4a4bfc22cdc 117 ahrs.putc((char) (chksum & 0x0F)); // LSByte
shimniok 0:f4a4bfc22cdc 118 }
shimniok 0:f4a4bfc22cdc 119
shimniok 0:f4a4bfc22cdc 120
shimniok 0:f4a4bfc22cdc 121 void process_packet(void) {
shimniok 0:f4a4bfc22cdc 122 switch (pt) {
shimniok 0:f4a4bfc22cdc 123 /** SENSOR_DATA
shimniok 0:f4a4bfc22cdc 124 * Datasheet:
shimniok 0:f4a4bfc22cdc 125 * If all channels are active, then data is given in the following order: { yaw, pitch, roll, yaw_rate,
shimniok 0:f4a4bfc22cdc 126 * pitch_rate, roll_rate, mag_z, mag_y, mag_x, gyro_z, gyro_y, gyro_x, accel_z, accel_y, accel_x }.
shimniok 0:f4a4bfc22cdc 127 * Data bytes D3 and D4 correspond to the yaw angle, D5 and D6 to the pitch angle, etc. Data is
shimniok 0:f4a4bfc22cdc 128 * returned as 16-bit two's complement integers.
shimniok 0:f4a4bfc22cdc 129 *
shimniok 0:f4a4bfc22cdc 130 * When one or more channel is inactive, then the data is returned in the same order, but skipping the
shimniok 0:f4a4bfc22cdc 131 * inactive channels. For example, if all magnetic field and rate gyro channels are disabled, then the
shimniok 0:f4a4bfc22cdc 132 * data is given in the following order: { yaw, pitch, roll, accel_z, accel_y, accel_x }
shimniok 0:f4a4bfc22cdc 133 */
shimniok 0:f4a4bfc22cdc 134 case PT_SENSOR_DATA :
shimniok 0:f4a4bfc22cdc 135 pc.printf("SENSOR_DATA\n");
shimniok 0:f4a4bfc22cdc 136 if ((data[0] & YAW_FLAG) == YAW_FLAG) {
shimniok 0:f4a4bfc22cdc 137 yaw = data[2] << 8 | data[3];
shimniok 0:f4a4bfc22cdc 138 pc.printf("Yaw: %d\n", yaw);
shimniok 0:f4a4bfc22cdc 139 }
shimniok 0:f4a4bfc22cdc 140 break;
shimniok 0:f4a4bfc22cdc 141 case PT_COMMAND_COMPLETE :
shimniok 0:f4a4bfc22cdc 142 pc.printf("Command Complete\n");
shimniok 0:f4a4bfc22cdc 143 break;
shimniok 0:f4a4bfc22cdc 144 case PT_COMMAND_FAILED :
shimniok 0:f4a4bfc22cdc 145 pc.printf("Command Failed\n");
shimniok 0:f4a4bfc22cdc 146 break;
shimniok 0:f4a4bfc22cdc 147 case PT_BAD_CHECKSUM :
shimniok 0:f4a4bfc22cdc 148 pc.printf("Bad Checksum\n");
shimniok 0:f4a4bfc22cdc 149 break;
shimniok 0:f4a4bfc22cdc 150 case PT_BAD_DATA_LENGTH :
shimniok 0:f4a4bfc22cdc 151 pc.printf("Bad Data Length\n");
shimniok 0:f4a4bfc22cdc 152 break;
shimniok 0:f4a4bfc22cdc 153 case PT_UNRECOGNIZED_PACKET :
shimniok 0:f4a4bfc22cdc 154 pc.printf("Unrecognized Packet\n");
shimniok 0:f4a4bfc22cdc 155 break;
shimniok 0:f4a4bfc22cdc 156 case PT_BUFFER_OVERFLOW :
shimniok 0:f4a4bfc22cdc 157 pc.printf("Buffer Overflow\n");
shimniok 0:f4a4bfc22cdc 158 break;
shimniok 0:f4a4bfc22cdc 159 default :
shimniok 0:f4a4bfc22cdc 160 break;
shimniok 0:f4a4bfc22cdc 161 }
shimniok 0:f4a4bfc22cdc 162 }
shimniok 0:f4a4bfc22cdc 163
shimniok 0:f4a4bfc22cdc 164
shimniok 0:f4a4bfc22cdc 165 int main() {
shimniok 0:f4a4bfc22cdc 166
shimniok 0:f4a4bfc22cdc 167 pc.baud(115200);
shimniok 0:f4a4bfc22cdc 168 ahrs.baud(115200);
shimniok 0:f4a4bfc22cdc 169
shimniok 0:f4a4bfc22cdc 170 pc.printf("Hello!\nCHR-6dm Test Program\nMichael Shimniok - http://bot-thoughts.com/");
shimniok 0:f4a4bfc22cdc 171
shimniok 0:f4a4bfc22cdc 172 while (1) {
shimniok 0:f4a4bfc22cdc 173 if (ahrs.readable()) {
shimniok 0:f4a4bfc22cdc 174
shimniok 0:f4a4bfc22cdc 175 c = ahrs.getc();
shimniok 0:f4a4bfc22cdc 176 switch (_state) {
shimniok 0:f4a4bfc22cdc 177 case WAIT_S :
shimniok 0:f4a4bfc22cdc 178 ///pc.printf("WAIT_S\n");
shimniok 0:f4a4bfc22cdc 179 if (c == 's') _state = WAIT_N;
shimniok 0:f4a4bfc22cdc 180 break;
shimniok 0:f4a4bfc22cdc 181 case WAIT_N :
shimniok 0:f4a4bfc22cdc 182 //pc.printf("WAIT_N\n");
shimniok 0:f4a4bfc22cdc 183 _state = (c == 'n') ? WAIT_P : WAIT_S;
shimniok 0:f4a4bfc22cdc 184 break;
shimniok 0:f4a4bfc22cdc 185 case WAIT_P :
shimniok 0:f4a4bfc22cdc 186 //pc.printf("WAIT_P\n");
shimniok 0:f4a4bfc22cdc 187 _state = (c == 'p') ? RX_TYPE : WAIT_S;
shimniok 0:f4a4bfc22cdc 188 break;
shimniok 0:f4a4bfc22cdc 189 case RX_TYPE :
shimniok 0:f4a4bfc22cdc 190 pt = c;
shimniok 0:f4a4bfc22cdc 191 _state = RX_N;
shimniok 0:f4a4bfc22cdc 192 pc.printf("PT = %02x\n", pt);
shimniok 0:f4a4bfc22cdc 193 break;
shimniok 0:f4a4bfc22cdc 194 case RX_N :
shimniok 0:f4a4bfc22cdc 195 n = ((uint8) c) - 2;
shimniok 0:f4a4bfc22cdc 196 d = 0;
shimniok 0:f4a4bfc22cdc 197 _state = (n < MAX_BYTES) ? RX_PACKET : WAIT_S;
shimniok 0:f4a4bfc22cdc 198 pc.printf("N = %d\n", n);
shimniok 0:f4a4bfc22cdc 199 break;
shimniok 0:f4a4bfc22cdc 200 case RX_PACKET :
shimniok 0:f4a4bfc22cdc 201 //pc.printf("data[%d] = %02x\n", d, c);
shimniok 0:f4a4bfc22cdc 202 data[d++] = c;
shimniok 0:f4a4bfc22cdc 203 if (d >= n || d >= MAX_BYTES) _state = PROCESS_PACKET;
shimniok 0:f4a4bfc22cdc 204 break;
shimniok 0:f4a4bfc22cdc 205 case PROCESS_PACKET :
shimniok 0:f4a4bfc22cdc 206 pc.printf("PROCESS_PACKET\n");
shimniok 0:f4a4bfc22cdc 207 process_packet();
shimniok 0:f4a4bfc22cdc 208 _state = WAIT_S;
shimniok 0:f4a4bfc22cdc 209 break;
shimniok 0:f4a4bfc22cdc 210 default :
shimniok 0:f4a4bfc22cdc 211 _state = WAIT_S;
shimniok 0:f4a4bfc22cdc 212 break;
shimniok 0:f4a4bfc22cdc 213 }
shimniok 0:f4a4bfc22cdc 214
shimniok 0:f4a4bfc22cdc 215 //pc.printf("%02x\n", c);
shimniok 0:f4a4bfc22cdc 216
shimniok 0:f4a4bfc22cdc 217 }
shimniok 0:f4a4bfc22cdc 218 }
shimniok 0:f4a4bfc22cdc 219
shimniok 0:f4a4bfc22cdc 220 }