Michael Shimniok
/
CHR-6dm_Test
main.cpp
- Committer:
- shimniok
- Date:
- 2011-04-04
- Revision:
- 0:f4a4bfc22cdc
File content as of revision 0:f4a4bfc22cdc:
#include "mbed.h" /** CHR-6dm test program * http://www.chrobotics.com/index.php?main_page=product_info&cPath=1&products_id=2 * The CH Robotics 6dm is an AHRS that transmits data over serial port * * Michael Shimniok - http://bot-thoughts.com/ * * For now all this code does is parses broadcast output and prints out yaw data * Some of the framework is in place to print more. I'll be workign on a * library for the device in the near future. */ #define sbi(byte, bit) ((byte) |= _BV(bit)) #define cbi(byte, bit) ((byte) &= ~_BV(bit)) #define _BV(bit) (0x01<<(bit)) Serial pc(USBTX, USBRX); Serial ahrs(p26, p25); typedef unsigned char uint8; typedef unsigned int uint; /** Rx packet types (commands) */ /** 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 /* 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 32 enum _states { WAIT_S, WAIT_N, WAIT_P, RX_TYPE, RX_N, RX_PACKET, PROCESS_PACKET }; int _state = WAIT_S; char c; int d; uint8 pt; uint n; char data[MAX_BYTES]; int yaw; int chksum(uint8 pt, uint8 n, char data[]) { int sum = pt + n; for (int i=0; i < n; i++) { sum += data[i]; } return sum; } void send_packet(uint8 pt, uint8 n, char data[]) { char s[MAX_BYTES]; ahrs.printf(s, "snp"); ahrs.putc(pt); ahrs.putc(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. */ int chksum = 's'+'n'+'p'+pt+n; for (int i=0; i < n; i++) { ahrs.putc(data[i]); chksum += data[i]; } ahrs.putc((char) (chksum >> 8)); // MSByte ahrs.putc((char) (chksum & 0x0F)); // LSByte } void 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 : pc.printf("SENSOR_DATA\n"); if ((data[0] & YAW_FLAG) == YAW_FLAG) { yaw = data[2] << 8 | data[3]; pc.printf("Yaw: %d\n", yaw); } break; case PT_COMMAND_COMPLETE : pc.printf("Command Complete\n"); break; case PT_COMMAND_FAILED : pc.printf("Command Failed\n"); break; case PT_BAD_CHECKSUM : pc.printf("Bad Checksum\n"); break; case PT_BAD_DATA_LENGTH : pc.printf("Bad Data Length\n"); break; case PT_UNRECOGNIZED_PACKET : pc.printf("Unrecognized Packet\n"); break; case PT_BUFFER_OVERFLOW : pc.printf("Buffer Overflow\n"); break; default : break; } } int main() { pc.baud(115200); ahrs.baud(115200); pc.printf("Hello!\nCHR-6dm Test Program\nMichael Shimniok - http://bot-thoughts.com/"); while (1) { if (ahrs.readable()) { c = ahrs.getc(); switch (_state) { case WAIT_S : ///pc.printf("WAIT_S\n"); if (c == 's') _state = WAIT_N; break; case WAIT_N : //pc.printf("WAIT_N\n"); _state = (c == 'n') ? WAIT_P : WAIT_S; break; case WAIT_P : //pc.printf("WAIT_P\n"); _state = (c == 'p') ? RX_TYPE : WAIT_S; break; case RX_TYPE : pt = c; _state = RX_N; pc.printf("PT = %02x\n", pt); break; case RX_N : n = ((uint8) c) - 2; d = 0; _state = (n < MAX_BYTES) ? RX_PACKET : WAIT_S; pc.printf("N = %d\n", n); break; case RX_PACKET : //pc.printf("data[%d] = %02x\n", d, c); data[d++] = c; if (d >= n || d >= MAX_BYTES) _state = PROCESS_PACKET; break; case PROCESS_PACKET : pc.printf("PROCESS_PACKET\n"); process_packet(); _state = WAIT_S; break; default : _state = WAIT_S; break; } //pc.printf("%02x\n", c); } } }