serial_stream.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include "create/serial_stream.h"
00004 #include "create/types.h"
00005 
00006 namespace create {
00007 
00008   SerialStream::SerialStream(boost::shared_ptr<Data> d, const uint8_t& header) : Serial(d), headerByte(header){
00009   }
00010 
00011   bool SerialStream::startSensorStream() {
00012     // Request from Create that we want a stream containing all packets
00013     uint8_t numPackets = data->getNumPackets();
00014     std::vector<uint8_t> packetIDs = data->getPacketIDs();
00015     uint8_t streamReq[2 + numPackets];
00016     streamReq[0] = OC_STREAM;
00017     streamReq[1] = numPackets;
00018     int i = 2;
00019     for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
00020       streamReq[i] = *it;
00021       i++;
00022     }
00023 
00024     // Start streaming data
00025     send(streamReq, 2 + numPackets);
00026 
00027     expectedNumBytes = data->getTotalDataBytes() + numPackets;
00028 
00029     return true;
00030   }
00031 
00032   void SerialStream::processByte(uint8_t byteRead) {
00033     numBytesRead++;
00034     byteSum += byteRead;
00035     switch (readState) {
00036       case READ_HEADER:
00037         if (byteRead == headerByte) {
00038           readState = READ_NBYTES;
00039           byteSum = byteRead;
00040         }
00041         break;
00042 
00043       case READ_NBYTES:
00044         if (byteRead == expectedNumBytes) {
00045           readState = READ_PACKET_ID;
00046           numBytesRead = 0;
00047         }
00048         else {
00049           //notifyDataReady();
00050           readState = READ_HEADER;
00051         }
00052         break;
00053 
00054       case READ_PACKET_ID:
00055         packetID = byteRead;
00056         if (data->isValidPacketID(packetID)) {
00057           expectedNumDataBytes = data->getPacket(packetID)->nbytes;
00058           assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
00059           numDataBytesRead = 0;
00060           packetBytes = 0;
00061           readState = READ_PACKET_BYTES;
00062         }
00063         else {
00064           //notifyDataReady();
00065           readState = READ_HEADER;
00066         }
00067         break;
00068 
00069       case READ_PACKET_BYTES:
00070         numDataBytesRead++;
00071         if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
00072           // High byte first
00073           packetBytes = (byteRead << 8) & 0xff00;
00074         }
00075         else {
00076           // Low byte
00077           packetBytes += byteRead;
00078         }
00079         if (numDataBytesRead >= expectedNumDataBytes) {
00080           data->getPacket(packetID)->setTempData(packetBytes);
00081           if (numBytesRead >= expectedNumBytes)
00082             readState = READ_CHECKSUM;
00083           else
00084             readState = READ_PACKET_ID;
00085         }
00086         break;
00087 
00088       case READ_CHECKSUM:
00089         if ((byteSum & 0xFF) == 0) {
00090           notifyDataReady();
00091         }
00092         else {
00093           // Corrupt data
00094           corruptPackets++;
00095         }
00096         totalPackets++;
00097         // Start again
00098         readState = READ_HEADER;
00099         break;
00100     } // end switch (readState)
00101   }
00102 
00103 } // namespace create


libcreate
Author(s): Jacob Perron
autogenerated on Sat Nov 26 2016 03:41:46