2b9591f0f7
* Replace boost features with C++11 equivalents Signed-off-by: Jacob Perron <jacob@openrobotics.org> * Include <cmath> in util.h Needed for std::abs Signed-off-by: Jacob Perron <jacob@openrobotics.org>
98 lines
2.6 KiB
C++
98 lines
2.6 KiB
C++
#include <iostream>
|
|
#include <memory>
|
|
|
|
#include "create/serial_stream.h"
|
|
#include "create/types.h"
|
|
|
|
namespace create {
|
|
|
|
SerialStream::SerialStream(std::shared_ptr<Data> d, const uint8_t& header) : Serial(d), readState(READ_HEADER), headerByte(header) {
|
|
}
|
|
|
|
bool SerialStream::startSensorStream() {
|
|
// Request from Create that we want a stream containing all packets
|
|
const uint8_t numPackets = data->getNumPackets();
|
|
std::vector<uint8_t> packetIDs = data->getPacketIDs();
|
|
packetIDs.insert(packetIDs.begin(), numPackets);
|
|
packetIDs.insert(packetIDs.begin(), OC_STREAM);
|
|
|
|
// Start streaming data
|
|
send(packetIDs.data(), packetIDs.size());
|
|
|
|
expectedNumBytes = data->getTotalDataBytes() + numPackets;
|
|
|
|
return true;
|
|
}
|
|
|
|
void SerialStream::processByte(uint8_t byteRead) {
|
|
numBytesRead++;
|
|
byteSum += byteRead;
|
|
switch (readState) {
|
|
case READ_HEADER:
|
|
if (byteRead == headerByte) {
|
|
readState = READ_NBYTES;
|
|
byteSum = byteRead;
|
|
}
|
|
break;
|
|
|
|
case READ_NBYTES:
|
|
if (byteRead == expectedNumBytes) {
|
|
readState = READ_PACKET_ID;
|
|
numBytesRead = 0;
|
|
}
|
|
else {
|
|
//notifyDataReady();
|
|
readState = READ_HEADER;
|
|
}
|
|
break;
|
|
|
|
case READ_PACKET_ID:
|
|
packetID = byteRead;
|
|
if (data->isValidPacketID(packetID)) {
|
|
expectedNumDataBytes = data->getPacket(packetID)->nbytes;
|
|
assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
|
|
numDataBytesRead = 0;
|
|
packetBytes = 0;
|
|
readState = READ_PACKET_BYTES;
|
|
}
|
|
else {
|
|
//notifyDataReady();
|
|
readState = READ_HEADER;
|
|
}
|
|
break;
|
|
|
|
case READ_PACKET_BYTES:
|
|
numDataBytesRead++;
|
|
if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
|
|
// High byte first
|
|
packetBytes = (byteRead << 8) & 0xff00;
|
|
}
|
|
else {
|
|
// Low byte
|
|
packetBytes += byteRead;
|
|
}
|
|
if (numDataBytesRead >= expectedNumDataBytes) {
|
|
data->getPacket(packetID)->setDataToValidate(packetBytes);
|
|
if (numBytesRead >= expectedNumBytes)
|
|
readState = READ_CHECKSUM;
|
|
else
|
|
readState = READ_PACKET_ID;
|
|
}
|
|
break;
|
|
|
|
case READ_CHECKSUM:
|
|
if ((byteSum & 0xFF) == 0) {
|
|
notifyDataReady();
|
|
}
|
|
else {
|
|
// Corrupt data
|
|
corruptPackets++;
|
|
}
|
|
totalPackets++;
|
|
// Start again
|
|
readState = READ_HEADER;
|
|
break;
|
|
} // end switch (readState)
|
|
}
|
|
|
|
} // namespace create
|