1563e2b3e1
This gives the user the option to create their own signal handler without having create::Create interfere. They can disable the sigint/sigterm handler and be responsible for disconnecting from the robot themselves. Signed-off-by: Jacob Perron <jacobmperron@gmail.com>
73 lines
2 KiB
C++
73 lines
2 KiB
C++
#include <iostream>
|
|
#include <memory>
|
|
|
|
#include "create/serial_query.h"
|
|
#include "create/types.h"
|
|
|
|
#define SENSORS_RESPONSE_LENGTH 20
|
|
|
|
namespace create {
|
|
|
|
SerialQuery::SerialQuery(std::shared_ptr<Data> d, bool install_signal_handler)
|
|
: Serial(d, install_signal_handler),
|
|
streamRecoveryTimer(io),
|
|
packetID(ID_BUMP_WHEELDROP),
|
|
packetByte(0),
|
|
packetData(0),
|
|
maxPacketID(ID_CAPACITY) {
|
|
}
|
|
|
|
bool SerialQuery::startSensorStream() {
|
|
if (!started) {
|
|
requestSensorData();
|
|
started = true;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void SerialQuery::requestSensorData() {
|
|
static const uint8_t requestPacket[2] = { OC_SENSORS, ID_GROUP_0 };
|
|
// Prevents previous packet from corrupting next one
|
|
flushInput();
|
|
send(requestPacket, 2);
|
|
// Automatically resend request if no response is received
|
|
streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50));
|
|
streamRecoveryTimer.async_wait(
|
|
std::bind(&SerialQuery::restartSensorStream, this, std::placeholders::_1));
|
|
}
|
|
|
|
void SerialQuery::restartSensorStream(const boost::system::error_code& err) {
|
|
if (err != boost::asio::error::operation_aborted) {
|
|
if (packetID != ID_BUMP_WHEELDROP) {
|
|
++corruptPackets;
|
|
}
|
|
requestSensorData();
|
|
}
|
|
}
|
|
|
|
void SerialQuery::flushInput() {
|
|
// Only works with POSIX support
|
|
tcflush(port.lowest_layer().native_handle(), TCIFLUSH);
|
|
}
|
|
|
|
void SerialQuery::processByte(uint8_t byteRead) {
|
|
packetData |= (static_cast<uint16_t>(byteRead) << (8 * packetByte));
|
|
|
|
if (packetByte > 0) {
|
|
--packetByte;
|
|
} else if (packetID < maxPacketID) {
|
|
// New packet
|
|
data->getPacket(packetID)->setDataToValidate(packetData);
|
|
packetData = 0;
|
|
++packetID;
|
|
packetByte = data->getPacket(packetID)->nbytes - 1;
|
|
} else {
|
|
// Response finished
|
|
packetID = ID_BUMP_WHEELDROP;
|
|
packetByte = 0;
|
|
packetData = 0;
|
|
notifyDataReady();
|
|
requestSensorData();
|
|
}
|
|
}
|
|
} // namespace create
|