serial_query.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include "create/serial_query.h"
00004 #include "create/types.h"
00005 
00006 #define SENSORS_RESPONSE_LENGTH 20
00007 
00008 namespace create {
00009 
00010   SerialQuery::SerialQuery(boost::shared_ptr<Data> d) : Serial(d),
00011     streamRecoveryTimer(io),
00012     packetID(ID_BUMP_WHEELDROP),
00013     packetByte(0),
00014     packetData(0),
00015     maxPacketID(ID_CAPACITY) {
00016   }
00017 
00018   bool SerialQuery::startSensorStream() {
00019     if (!started) {
00020       requestSensorData();
00021       started = true;
00022     }
00023     return true;
00024   }
00025 
00026   void SerialQuery::requestSensorData() {
00027     static const uint8_t requestPacket[2] = { OC_SENSORS, ID_GROUP_0 };
00028     // Prevents previous packet from corrupting next one
00029     flushInput();
00030     send(requestPacket, 2);
00031     // Automatically resend request if no response is received
00032     streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50));
00033     streamRecoveryTimer.async_wait(boost::bind(&SerialQuery::restartSensorStream, this, _1));
00034   }
00035 
00036   void SerialQuery::restartSensorStream(const boost::system::error_code& err) {
00037     if (err != boost::asio::error::operation_aborted) {
00038       if (packetID != ID_BUMP_WHEELDROP) {
00039         ++corruptPackets;
00040       }
00041       requestSensorData();
00042     }
00043   }
00044 
00045   void SerialQuery::flushInput() {
00046     // Only works with POSIX support
00047     tcflush(port.lowest_layer().native_handle(), TCIFLUSH);
00048   }
00049 
00050   void SerialQuery::processByte(uint8_t byteRead) {
00051     packetData |= (static_cast<uint16_t>(byteRead) << (8 * packetByte));
00052 
00053     if (packetByte > 0) {
00054       --packetByte;
00055     } else if (packetID < maxPacketID) {
00056       // New packet
00057       data->getPacket(packetID)->setDataToValidate(packetData);
00058       packetData = 0;
00059       ++packetID;
00060       packetByte = data->getPacket(packetID)->nbytes - 1;
00061     } else {
00062       // Response finished
00063       packetID = ID_BUMP_WHEELDROP;
00064       packetByte = 0;
00065       packetData = 0;
00066       notifyDataReady();
00067       requestSensorData();
00068     }
00069   }
00070 } // namespace create


libcreate
Author(s): Jacob Perron
autogenerated on Thu Jun 6 2019 21:02:06