TinyCHR6dm is a simplified interface for the CH Robotics CHR-6dm AHRS
TinyCHR6dm.cpp@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 "TinyCHR6dm.h" |
shimniok | 0:983f66650cd5 | 3 | #include "string.h" |
shimniok | 0:983f66650cd5 | 4 | |
shimniok | 0:983f66650cd5 | 5 | int chksum(uint8 pt, uint8 n, char data[]) |
shimniok | 0:983f66650cd5 | 6 | { |
shimniok | 0:983f66650cd5 | 7 | int sum = pt + n; |
shimniok | 0:983f66650cd5 | 8 | |
shimniok | 0:983f66650cd5 | 9 | for (int i=0; i < n; i++) { |
shimniok | 0:983f66650cd5 | 10 | sum += data[i]; |
shimniok | 0:983f66650cd5 | 11 | } |
shimniok | 0:983f66650cd5 | 12 | |
shimniok | 0:983f66650cd5 | 13 | return sum; |
shimniok | 0:983f66650cd5 | 14 | } |
shimniok | 0:983f66650cd5 | 15 | |
shimniok | 0:983f66650cd5 | 16 | //////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:983f66650cd5 | 17 | // PUBLIC FUNCTIONS |
shimniok | 0:983f66650cd5 | 18 | //////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:983f66650cd5 | 19 | |
shimniok | 0:983f66650cd5 | 20 | TinyCHR6dm::TinyCHR6dm(): _dataReady(false), _statusReady(false), _state(WAIT_S) |
shimniok | 0:983f66650cd5 | 21 | { |
shimniok | 0:983f66650cd5 | 22 | _state = WAIT_S; |
shimniok | 0:983f66650cd5 | 23 | } |
shimniok | 0:983f66650cd5 | 24 | |
shimniok | 0:983f66650cd5 | 25 | |
shimniok | 0:983f66650cd5 | 26 | void TinyCHR6dm::parse(char c) |
shimniok | 0:983f66650cd5 | 27 | { |
shimniok | 0:983f66650cd5 | 28 | |
shimniok | 0:983f66650cd5 | 29 | switch (_state) { |
shimniok | 0:983f66650cd5 | 30 | case WAIT_S : |
shimniok | 0:983f66650cd5 | 31 | if (debug) debug->printf("WAIT_S\n"); |
shimniok | 0:983f66650cd5 | 32 | if (c == 's') _state = WAIT_N; |
shimniok | 0:983f66650cd5 | 33 | break; |
shimniok | 0:983f66650cd5 | 34 | case WAIT_N : |
shimniok | 0:983f66650cd5 | 35 | if (debug) debug->printf("WAIT_N\n"); |
shimniok | 0:983f66650cd5 | 36 | _state = (c == 'n') ? WAIT_P : WAIT_S; |
shimniok | 0:983f66650cd5 | 37 | break; |
shimniok | 0:983f66650cd5 | 38 | case WAIT_P : |
shimniok | 0:983f66650cd5 | 39 | if (debug) debug->printf("WAIT_P\n"); |
shimniok | 0:983f66650cd5 | 40 | _state = (c == 'p') ? RX_TYPE : WAIT_S; |
shimniok | 0:983f66650cd5 | 41 | break; |
shimniok | 0:983f66650cd5 | 42 | case RX_TYPE : |
shimniok | 0:983f66650cd5 | 43 | pt = c; |
shimniok | 0:983f66650cd5 | 44 | _state = RX_N; |
shimniok | 0:983f66650cd5 | 45 | if (debug) debug->printf("PT = %02x\n", pt); |
shimniok | 0:983f66650cd5 | 46 | break; |
shimniok | 0:983f66650cd5 | 47 | case RX_N : |
shimniok | 0:983f66650cd5 | 48 | n = (uint8) c; |
shimniok | 0:983f66650cd5 | 49 | d = 0; |
shimniok | 0:983f66650cd5 | 50 | _state = (n < MAX_BYTES) ? RX_PACKET : WAIT_S; |
shimniok | 0:983f66650cd5 | 51 | if (debug) debug->printf("N = %d\n", n); |
shimniok | 0:983f66650cd5 | 52 | break; |
shimniok | 0:983f66650cd5 | 53 | case RX_PACKET : |
shimniok | 0:983f66650cd5 | 54 | if (debug) debug->printf("RX_PACKET\n"); |
shimniok | 0:983f66650cd5 | 55 | if (d >= n || d >= MAX_BYTES) { |
shimniok | 0:983f66650cd5 | 56 | _state = PROCESS_PACKET; |
shimniok | 0:983f66650cd5 | 57 | } else { |
shimniok | 0:983f66650cd5 | 58 | if (debug) debug->printf("data[%d] = %02x\n", d, c); |
shimniok | 0:983f66650cd5 | 59 | data[d++] = c; |
shimniok | 0:983f66650cd5 | 60 | } |
shimniok | 0:983f66650cd5 | 61 | break; |
shimniok | 0:983f66650cd5 | 62 | case PROCESS_PACKET : |
shimniok | 0:983f66650cd5 | 63 | if (debug) debug->printf("PROCESS_PACKET\n"); |
shimniok | 0:983f66650cd5 | 64 | process_packet(); |
shimniok | 0:983f66650cd5 | 65 | _state = WAIT_S; |
shimniok | 0:983f66650cd5 | 66 | break; |
shimniok | 0:983f66650cd5 | 67 | default : |
shimniok | 0:983f66650cd5 | 68 | _state = WAIT_S; |
shimniok | 0:983f66650cd5 | 69 | break; |
shimniok | 0:983f66650cd5 | 70 | } |
shimniok | 0:983f66650cd5 | 71 | |
shimniok | 0:983f66650cd5 | 72 | return; |
shimniok | 0:983f66650cd5 | 73 | } |
shimniok | 0:983f66650cd5 | 74 | |
shimniok | 0:983f66650cd5 | 75 | |
shimniok | 0:983f66650cd5 | 76 | float TinyCHR6dm::readYaw(void) { |
shimniok | 0:983f66650cd5 | 77 | return TinyCHR6dm::_yaw; |
shimniok | 0:983f66650cd5 | 78 | } |
shimniok | 0:983f66650cd5 | 79 | |
shimniok | 0:983f66650cd5 | 80 | bool TinyCHR6dm::dataReady(void) { |
shimniok | 0:983f66650cd5 | 81 | return TinyCHR6dm::_dataReady; |
shimniok | 0:983f66650cd5 | 82 | } |
shimniok | 0:983f66650cd5 | 83 | |
shimniok | 0:983f66650cd5 | 84 | void TinyCHR6dm::resetReady(void) { |
shimniok | 0:983f66650cd5 | 85 | TinyCHR6dm::_dataReady = false; |
shimniok | 0:983f66650cd5 | 86 | } |
shimniok | 0:983f66650cd5 | 87 | |
shimniok | 0:983f66650cd5 | 88 | |
shimniok | 0:983f66650cd5 | 89 | |
shimniok | 0:983f66650cd5 | 90 | void TinyCHR6dm::send_packet(Serial *serial, uint8 pt, uint8 n, char data[]) |
shimniok | 0:983f66650cd5 | 91 | { |
shimniok | 0:983f66650cd5 | 92 | uint checksum; |
shimniok | 0:983f66650cd5 | 93 | char p[MAX_BYTES]; |
shimniok | 0:983f66650cd5 | 94 | |
shimniok | 0:983f66650cd5 | 95 | p[0] = 's'; |
shimniok | 0:983f66650cd5 | 96 | p[1] = 'n'; |
shimniok | 0:983f66650cd5 | 97 | p[2] = 'p'; |
shimniok | 0:983f66650cd5 | 98 | p[3] = (char) pt; |
shimniok | 0:983f66650cd5 | 99 | p[4] = (char) n; |
shimniok | 0:983f66650cd5 | 100 | |
shimniok | 0:983f66650cd5 | 101 | /** Checksum |
shimniok | 0:983f66650cd5 | 102 | * Datasheet: |
shimniok | 0:983f66650cd5 | 103 | * After the CHR6-dm receives a full acket, it checks to ensure that the checksum |
shimniok | 0:983f66650cd5 | 104 | * given in the last two bytes matches the sum of all preceding bytes in the packet. |
shimniok | 0:983f66650cd5 | 105 | */ |
shimniok | 0:983f66650cd5 | 106 | checksum = 's'+'n'+'p'+pt+n; |
shimniok | 0:983f66650cd5 | 107 | for (int i=0; i < n; i++) { |
shimniok | 0:983f66650cd5 | 108 | p[i+5] = data[i]; |
shimniok | 0:983f66650cd5 | 109 | checksum += data[i]; |
shimniok | 0:983f66650cd5 | 110 | } |
shimniok | 0:983f66650cd5 | 111 | if (debug) debug->printf("Checksum: %04x\n", checksum); |
shimniok | 0:983f66650cd5 | 112 | p[5+n] = (checksum >> 8); // checksum MSB |
shimniok | 0:983f66650cd5 | 113 | p[5+n+1] = (checksum & 0x0ff); // checksum LSB |
shimniok | 0:983f66650cd5 | 114 | |
shimniok | 0:983f66650cd5 | 115 | if (debug) debug->printf("Checksum: %02x %02x\n", p[5+n], p[5+n+1]); |
shimniok | 0:983f66650cd5 | 116 | |
shimniok | 0:983f66650cd5 | 117 | if (serial) { |
shimniok | 0:983f66650cd5 | 118 | for (int i=0; i < 5+n+2; i++) { |
shimniok | 0:983f66650cd5 | 119 | serial->putc((int) p[i]); |
shimniok | 0:983f66650cd5 | 120 | if (debug) debug->printf("%02x\n", p[i]); |
shimniok | 0:983f66650cd5 | 121 | } |
shimniok | 0:983f66650cd5 | 122 | } |
shimniok | 0:983f66650cd5 | 123 | |
shimniok | 0:983f66650cd5 | 124 | } |
shimniok | 0:983f66650cd5 | 125 | |
shimniok | 0:983f66650cd5 | 126 | int TinyCHR6dm::status(void) { |
shimniok | 0:983f66650cd5 | 127 | _statusReady = false; |
shimniok | 0:983f66650cd5 | 128 | return _status; |
shimniok | 0:983f66650cd5 | 129 | } |
shimniok | 0:983f66650cd5 | 130 | |
shimniok | 0:983f66650cd5 | 131 | bool TinyCHR6dm::statusReady(void) { |
shimniok | 0:983f66650cd5 | 132 | return _statusReady; |
shimniok | 0:983f66650cd5 | 133 | } |
shimniok | 0:983f66650cd5 | 134 | |
shimniok | 0:983f66650cd5 | 135 | char *TinyCHR6dm::statusString(int status) { |
shimniok | 0:983f66650cd5 | 136 | char *s[7] = { "unknown", |
shimniok | 0:983f66650cd5 | 137 | "PT_COMMAND_COMPLETE", |
shimniok | 0:983f66650cd5 | 138 | "PT_COMMAND_FAILED", |
shimniok | 0:983f66650cd5 | 139 | "PT_BAD_CHECKSUM", |
shimniok | 0:983f66650cd5 | 140 | "PT_BAD_DATA_LENGTH", |
shimniok | 0:983f66650cd5 | 141 | "PT_UNRECOGNIZED_PACKET", |
shimniok | 0:983f66650cd5 | 142 | "PT_BUFFER_OVERFLOW" }; |
shimniok | 0:983f66650cd5 | 143 | |
shimniok | 0:983f66650cd5 | 144 | return (status >= PT_COMMAND_COMPLETE && status <= PT_BUFFER_OVERFLOW ) ? s[status - PT_COMMAND_COMPLETE + 1] : s[0]; |
shimniok | 0:983f66650cd5 | 145 | } |
shimniok | 0:983f66650cd5 | 146 | |
shimniok | 0:983f66650cd5 | 147 | //////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:983f66650cd5 | 148 | // PRIVATE FUNCTIONS |
shimniok | 0:983f66650cd5 | 149 | //////////////////////////////////////////////////////////////////////////////// |
shimniok | 0:983f66650cd5 | 150 | |
shimniok | 0:983f66650cd5 | 151 | void TinyCHR6dm::process_packet(void) |
shimniok | 0:983f66650cd5 | 152 | { |
shimniok | 0:983f66650cd5 | 153 | |
shimniok | 0:983f66650cd5 | 154 | switch (pt) { |
shimniok | 0:983f66650cd5 | 155 | /** SENSOR_DATA |
shimniok | 0:983f66650cd5 | 156 | * Datasheet: |
shimniok | 0:983f66650cd5 | 157 | * If all channels are active, then data is given in the following order: { yaw, pitch, roll, yaw_rate, |
shimniok | 0:983f66650cd5 | 158 | * pitch_rate, roll_rate, mag_z, mag_y, mag_x, gyro_z, gyro_y, gyro_x, accel_z, accel_y, accel_x }. |
shimniok | 0:983f66650cd5 | 159 | * Data bytes D3 and D4 correspond to the yaw angle, D5 and D6 to the pitch angle, etc. Data is |
shimniok | 0:983f66650cd5 | 160 | * returned as 16-bit two's complement integers. |
shimniok | 0:983f66650cd5 | 161 | * |
shimniok | 0:983f66650cd5 | 162 | * When one or more channel is inactive, then the data is returned in the same order, but skipping the |
shimniok | 0:983f66650cd5 | 163 | * inactive channels. For example, if all magnetic field and rate gyro channels are disabled, then the |
shimniok | 0:983f66650cd5 | 164 | * data is given in the following order: { yaw, pitch, roll, accel_z, accel_y, accel_x } |
shimniok | 0:983f66650cd5 | 165 | */ |
shimniok | 0:983f66650cd5 | 166 | case PT_SENSOR_DATA : |
shimniok | 0:983f66650cd5 | 167 | if (debug) debug->printf("SENSOR_DATA, YAW FLAG: %02x\n", data[0]&YAW_FLAG); |
shimniok | 0:983f66650cd5 | 168 | |
shimniok | 0:983f66650cd5 | 169 | if ((data[0] & YAW_FLAG) == YAW_FLAG) { |
shimniok | 0:983f66650cd5 | 170 | int yaw = (int) (data[2] << 8 | data[3]); |
shimniok | 0:983f66650cd5 | 171 | float yawf = 360.0 * (float) yaw / 32768.0; |
shimniok | 0:983f66650cd5 | 172 | while (yawf < 0) yawf += 360.0; |
shimniok | 0:983f66650cd5 | 173 | while (yawf >= 360.0) yawf -= 360.0; |
shimniok | 0:983f66650cd5 | 174 | TinyCHR6dm::_yaw = yawf; |
shimniok | 0:983f66650cd5 | 175 | if (debug) debug->printf("Yaw: %.2f\n", yawf); |
shimniok | 0:983f66650cd5 | 176 | } |
shimniok | 0:983f66650cd5 | 177 | _dataReady = true; |
shimniok | 0:983f66650cd5 | 178 | break; |
shimniok | 0:983f66650cd5 | 179 | case PT_COMMAND_COMPLETE : |
shimniok | 0:983f66650cd5 | 180 | case PT_COMMAND_FAILED : |
shimniok | 0:983f66650cd5 | 181 | case PT_BAD_CHECKSUM : |
shimniok | 0:983f66650cd5 | 182 | case PT_BAD_DATA_LENGTH : |
shimniok | 0:983f66650cd5 | 183 | if (debug) debug->printf("PT = %02x\n", pt); |
shimniok | 0:983f66650cd5 | 184 | _status = pt; |
shimniok | 0:983f66650cd5 | 185 | _statusReady = true; |
shimniok | 0:983f66650cd5 | 186 | break; |
shimniok | 0:983f66650cd5 | 187 | default : |
shimniok | 0:983f66650cd5 | 188 | if (debug) debug->printf("Packet type %02x was not processed\n", pt); |
shimniok | 0:983f66650cd5 | 189 | break; |
shimniok | 0:983f66650cd5 | 190 | } |
shimniok | 0:983f66650cd5 | 191 | } |