Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00029
00030 #include "APM_BinComm.h"
00031 #include "WProgram.h"
00032
00034
00035 #define DEC_WAIT_P1 0
00036 #define DEC_WAIT_P2 1
00037 #define DEC_WAIT_HEADER 2
00038 #define DEC_WAIT_MESSAGE 3
00039 #define DEC_WAIT_SUM_A 4
00040 #define DEC_WAIT_SUM_B 5
00041
00042
00043
00045 #define DEC_MESSAGE_TIMEOUT 1000
00046
00047 BinComm::BinComm(const BinComm::MessageHandler *handlerTable,
00048 Stream *interface) :
00049 _handlerTable(handlerTable),
00050 _decodePhase(DEC_WAIT_P1),
00051 _lastReceived(millis())
00052 {
00053 init(interface);
00054 };
00055
00056 void
00057 BinComm::init(Stream *interface)
00058 {
00059 _interface = interface;
00060 }
00061
00062 void
00063 BinComm::_startMessage(uint8_t messageId, uint8_t messageLength, uint8_t messageVersion)
00064 {
00065
00066 _interface->write((uint8_t)MSG_PREAMBLE_1);
00067 _interface->write((uint8_t)MSG_PREAMBLE_2);
00068
00069
00070 _encoderSumA = _encoderSumB = 0;
00071
00072
00073 _emit(messageLength);
00074 _emit(messageId);
00075 _emit(messageVersion);
00076 }
00077
00078 void
00079 BinComm::_send(const void *bytes, uint8_t count)
00080 {
00081 const uint8_t *p = (const uint8_t *)bytes;
00082 uint8_t c;
00083
00084 while (count--) {
00085 c = *p++;
00086 _encoderSumA += c;
00087 _encoderSumB += _encoderSumA;
00088 _interface->write(c);
00089 }
00090 }
00091
00092 void
00093 BinComm::_endMessage(void)
00094 {
00095 _interface->write(_encoderSumA);
00096 _interface->write(_encoderSumB);
00097 }
00098
00099 void
00100 BinComm::update(void)
00101 {
00102 uint8_t count;
00103
00104
00105
00106
00107
00108 count = _interface->available();
00109
00110 while (count--)
00111 _decode(_interface->read());
00112 }
00113
00114 void
00115 BinComm::_decode(uint8_t inByte)
00116 {
00117 uint8_t tableIndex;
00118
00119
00120
00121 if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT)
00122 _decodePhase = DEC_WAIT_P1;
00123 _lastReceived = millis();
00124
00125
00126
00127 switch (_decodePhase) {
00128
00129
00130
00131
00132
00133
00134
00135 case DEC_WAIT_P2:
00136 if (MSG_PREAMBLE_2 == inByte) {
00137 _decodePhase++;
00138
00139
00140 _bytesIn = 0;
00141 _bytesExpected = sizeof(MessageHeader);
00142
00143
00144 _decoderSumA = _decoderSumB = 0;
00145
00146 break;
00147 }
00148 _decodePhase = DEC_WAIT_P1;
00149
00150 case DEC_WAIT_P1:
00151 if (MSG_PREAMBLE_1 == inByte) {
00152 _decodePhase++;
00153 }
00154 break;
00155
00156
00157
00158 case DEC_WAIT_HEADER:
00159
00160 _decoderSumA += inByte;
00161 _decoderSumB += _decoderSumA;
00162
00163
00164 _decodeBuf.bytes[_bytesIn++] = inByte;
00165
00166
00167 if (_bytesIn == _bytesExpected) {
00168 _decodePhase++;
00169
00170
00171
00172 _bytesIn = 0;
00173 _bytesExpected = _decodeBuf.header.length;
00174 _messageID = _decodeBuf.header.messageID;
00175 _messageVersion = _decodeBuf.header.messageVersion;
00176
00177
00178 if (_bytesExpected > sizeof(_decodeBuf))
00179 _decodePhase = DEC_WAIT_P1;
00180 }
00181 break;
00182
00183
00184
00185 case DEC_WAIT_MESSAGE:
00186
00187 _decoderSumA += inByte;
00188 _decoderSumB += _decoderSumA;
00189
00190
00191 _decodeBuf.bytes[_bytesIn++] = inByte;
00192
00193
00194 if (_bytesIn == _bytesExpected) {
00195 _decodePhase++;
00196 }
00197 break;
00198
00199
00200
00201 case DEC_WAIT_SUM_A:
00202 if (inByte != _decoderSumA) {
00203 badMessagesReceived++;
00204 _decodePhase = DEC_WAIT_P1;
00205 } else {
00206 _decodePhase++;
00207 }
00208 break;
00209 case DEC_WAIT_SUM_B:
00210 if (inByte == _decoderSumB) {
00211
00212 messagesReceived++;
00213
00214
00215 for (tableIndex = 0; MSG_NULL != _handlerTable[tableIndex].messageID; tableIndex++) {
00216 if(_handlerTable[tableIndex].messageID == MSG_ANY ||
00217 _handlerTable[tableIndex].messageID == _messageID ) {
00218 _handlerTable[tableIndex].handler(_handlerTable[tableIndex].arg,
00219 _messageID, _messageVersion, &_decodeBuf);
00220
00221 if (_messageID != MSG_ACKNOWLEDGE)
00222 send_msg_acknowledge(_messageID, _decoderSumA, _decoderSumB);
00223 } else {
00224
00225 }
00226 }
00227 } else {
00228 badMessagesReceived++;
00229 }
00230
00231 default:
00232 _decodePhase = DEC_WAIT_P1;
00233 break;
00234
00235 }
00236 }