• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

APM_BinComm.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
00002 //
00003 // Copyright (c) 2010 Michael Smith. All rights reserved.
00004 // 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions
00007 // are met:
00008 // 1. Redistributions of source code must retain the above copyright
00009 //    notice, this list of conditions and the following disclaimer.
00010 // 2. Redistributions in binary form must reproduce the above copyright
00011 //    notice, this list of conditions and the following disclaimer in the
00012 //    documentation and/or other materials provided with the distribution.
00013 // 
00014 // THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
00015 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00016 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00017 // ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
00018 // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00019 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
00020 // OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
00021 // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00022 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
00023 // OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
00024 // SUCH DAMAGE.
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         // send the preamble first
00066         _interface->write((uint8_t)MSG_PREAMBLE_1);
00067         _interface->write((uint8_t)MSG_PREAMBLE_2);
00068 
00069         // initialise the checksum accumulators
00070         _encoderSumA = _encoderSumB = 0;
00071 
00072         // send the header
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         // Ensure that we don't spend too long here by only processing
00105         // the bytes that were available when we started.
00106         //
00107         // XXX we might want to further constrain this count
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         // handle inter-byte timeouts (resync after link loss, etc.)
00120         //
00121         if ((millis() - _lastReceived) > DEC_MESSAGE_TIMEOUT)
00122                 _decodePhase = DEC_WAIT_P1;
00123         _lastReceived = millis();
00124 
00125         // run the decode state machine
00126         //
00127         switch (_decodePhase) {
00128 
00129                 // Preamble detection
00130                 //
00131                 // Note the fallthrough from P2 to P1 deals with the case where
00132                 // we see 0x34, 0x34, 0x44 where the first 0x34 is garbage or 
00133                 // a SUM_B byte we never looked at.
00134                 //
00135         case DEC_WAIT_P2:
00136                 if (MSG_PREAMBLE_2 == inByte) {
00137                         _decodePhase++;
00138 
00139                         // prepare for the header
00140                         _bytesIn = 0;
00141                         _bytesExpected = sizeof(MessageHeader);
00142 
00143                         // intialise the checksum accumulators
00144                         _decoderSumA = _decoderSumB = 0;
00145 
00146                         break;
00147                 }
00148                 _decodePhase = DEC_WAIT_P1;
00149                 // FALLTHROUGH
00150         case DEC_WAIT_P1:
00151                 if (MSG_PREAMBLE_1 == inByte) {
00152                         _decodePhase++;
00153                 }
00154                 break;
00155 
00156                 // receiving the header
00157                 //
00158         case DEC_WAIT_HEADER:
00159                 // do checksum accumulation
00160                 _decoderSumA += inByte;
00161                 _decoderSumB += _decoderSumA;
00162 
00163                 // store the byte
00164                 _decodeBuf.bytes[_bytesIn++] = inByte;
00165 
00166                 // check for complete header received
00167                 if (_bytesIn == _bytesExpected) {
00168                         _decodePhase++;
00169 
00170                         // prepare for the payload
00171                         // variable-length data?
00172                         _bytesIn = 0;
00173                         _bytesExpected = _decodeBuf.header.length;
00174                         _messageID = _decodeBuf.header.messageID;
00175                         _messageVersion = _decodeBuf.header.messageVersion;
00176 
00177                         // sanity check to avoid buffer overflow - revert back to waiting
00178                         if (_bytesExpected > sizeof(_decodeBuf))
00179                                 _decodePhase = DEC_WAIT_P1;
00180                 }
00181                 break;
00182 
00183                 // receiving payload data
00184                 //
00185         case DEC_WAIT_MESSAGE:
00186                 // do checksum accumulation
00187                 _decoderSumA += inByte;
00188                 _decoderSumB += _decoderSumA;
00189 
00190                 // store the byte
00191                 _decodeBuf.bytes[_bytesIn++] = inByte;
00192 
00193                 // check for complete payload received
00194                 if (_bytesIn == _bytesExpected) {
00195                         _decodePhase++;
00196                 }
00197                 break;
00198 
00199                 // waiting for the checksum bytes
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                         // if we got this far, we have a message
00212                         messagesReceived++;
00213 
00214                         // call any handler interested in this message
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                                         // dont' acknowledge an acknowledgement, will echo infinetly
00221                                         if (_messageID != MSG_ACKNOWLEDGE) 
00222                                                 send_msg_acknowledge(_messageID, _decoderSumA, _decoderSumB);
00223                                 } else {
00224                                         // XXX should send a NAK of some sort here
00225                                 }
00226                         }
00227                 } else {
00228                         badMessagesReceived++;
00229                 }
00230                 // FALLTHROUGH
00231         default:
00232                 _decodePhase = DEC_WAIT_P1;
00233                 break;
00234 
00235         }
00236 }

Generated for ArduPilot Libraries by doxygen