Michael Shimniok / TinyCHR6dm

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

Dependents:   AVC_20110423

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?

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