compAIR/libcreate
compAIR/libcreate
4
0
Fork 1
Code Issues Pull requests Projects Releases Packages 1 Wiki Activity Actions

Disconnect from serial cleanly on SIGINT

Browse source 
Send the STOP opcode before exiting the program to ensure the robot is not left in a state that could potentially drain the battery.

Signed-off-by: Jacob Perron <jacobmperron@gmail.com>
This commit is contained in:
Jacob Perron 2019-08-27 00:11:29 -07:00 • committed by Jacob Perron
parent 04ab2ccd80
commit f5044c7ec8
3 changed files with 17 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

3
include/create/serial.h
View file

@ -51,6 +51,7 @@ namespace create {
protected: protected:
boost::asio::io_service io; boost::asio::io_service io;
boost::asio::signal_set signals;
boost::asio::serial_port port; boost::asio::serial_port port;
private: private:
@ -62,7 +63,6 @@ namespace create {
bool firstRead; bool firstRead;
uint8_t byteRead; uint8_t byteRead;
// Callback executed when data arrives from Create // Callback executed when data arrives from Create
void onData(const boost::system::error_code& e, const std::size_t& size); void onData(const boost::system::error_code& e, const std::size_t& size);
// Callback to execute once data arrives // Callback to execute once data arrives
@ -80,6 +80,7 @@ namespace create {
virtual bool startSensorStream() = 0; virtual bool startSensorStream() = 0;
virtual void processByte(uint8_t byteRead) = 0; virtual void processByte(uint8_t byteRead) = 0;
void signalHandler(const boost::system::error_code& error, int signal_number);
// Notifies main thread that data is fresh and makes the user callback // Notifies main thread that data is fresh and makes the user callback
void notifyDataReady(); void notifyDataReady();

2
src/create.cpp
View file

@ -15,8 +15,6 @@ namespace create {
namespace ublas = boost::numeric::ublas; namespace ublas = boost::numeric::ublas;
// TODO: Handle SIGINT to do clean disconnect
void Create::init() { void Create::init() {
mainMotorPower = 0; mainMotorPower = 0;
sideMotorPower = 0; sideMotorPower = 0;

15
src/serial.cpp
View file

@ -8,6 +8,7 @@ namespace create {
Serial::Serial(boost::shared_ptr<Data> d) : Serial::Serial(boost::shared_ptr<Data> d) :
data(d), data(d),
port(io), port(io),
signals(io, SIGINT, SIGTERM),
isReading(false), isReading(false),
dataReady(false), dataReady(false),
corruptPackets(0), corruptPackets(0),
@ -18,6 +19,18 @@ namespace create {
disconnect(); disconnect();
} }
void Serial::signalHandler(const boost::system::error_code& error, int signal_number) {
if (!error) {
if (connected()) {
// Ensure not in Safe/Full modes
sendOpcode(OC_START);
// Stop OI
sendOpcode(OC_STOP);
exit(signal_number);
}
}
}
bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) { bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
using namespace boost::asio; using namespace boost::asio;
port.open(portName); port.open(portName);
@ -27,6 +40,8 @@ namespace create {
port.set_option(serial_port::stop_bits(serial_port::stop_bits::one)); port.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
port.set_option(serial_port::flow_control(serial_port::flow_control::none)); port.set_option(serial_port::flow_control(serial_port::flow_control::none));
signals.async_wait(boost::bind(&Serial::signalHandler, this, _1, _2));
usleep(1000000); usleep(1000000);
if (port.is_open()) { if (port.is_open()) {