Diff: main.cpp
- Revision:
- 0:f4a4bfc22cdc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 04 20:35:19 2011 +0000
@@ -0,0 +1,220 @@
+#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);
+
+ }
+ }
+
+}
Michael Shimniok