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

Merge pull request #2 from AutonomyLab/fix-odom

Browse source 
Fix bug: Too many packets requested corrupting serial buffer
This commit is contained in:
jacobperron 2016-02-13 11:55:03 -08:00
commit 72e5f98969
3 changed files with 60 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

2
include/create/serial.h
View file

@ -88,6 +88,8 @@ namespace create {
// Start and stop reading data from Create // Start and stop reading data from Create
bool startReading(); bool startReading();
void stopReading(); void stopReading();
// Notifies main thread that data is fresh and makes the user callback
void notifyDataReady();
public: public:
Serial(boost::shared_ptr<Data> data, const uint8_t& header = create::util::CREATE_2_HEADER); Serial(boost::shared_ptr<Data> data, const uint8_t& header = create::util::CREATE_2_HEADER);

66
src/data.cpp
View file

@ -6,57 +6,61 @@ namespace create {
Data::Data() { Data::Data() {
// Populate data map // Populate data map
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* WARNING: Adding too many packets will flood the serial *
* buffer and corrupt the stream. *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops"); ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops");
ADD_PACKET(ID_WALL, 1, "wall"); ADD_PACKET(ID_WALL, 1, "wall");
ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left"); ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left");
ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left"); ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left");
ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right"); ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right");
ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right"); ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right");
ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall"); //ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents"); //ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents");
ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect"); ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect");
ADD_PACKET(ID_UNUSED_1, 1, "unused 1"); //ADD_PACKET(ID_UNUSED_1, 1, "unused 1");
ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode"); ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode");
ADD_PACKET(ID_BUTTONS, 1, "buttons"); ADD_PACKET(ID_BUTTONS, 1, "buttons");
ADD_PACKET(ID_DISTANCE, 2, "distance"); //ADD_PACKET(ID_DISTANCE, 2, "distance");
ADD_PACKET(ID_ANGLE, 2, "angle"); //ADD_PACKET(ID_ANGLE, 2, "angle");
ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state"); ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state");
ADD_PACKET(ID_VOLTAGE, 2, "voltage"); ADD_PACKET(ID_VOLTAGE, 2, "voltage");
ADD_PACKET(ID_CURRENT, 2, "current"); ADD_PACKET(ID_CURRENT, 2, "current");
ADD_PACKET(ID_TEMP, 1, "temperature"); ADD_PACKET(ID_TEMP, 1, "temperature");
ADD_PACKET(ID_CHARGE , 2, "battery_charge"); ADD_PACKET(ID_CHARGE , 2, "battery_charge");
ADD_PACKET(ID_CAPACITY, 2, "battery_capacity"); ADD_PACKET(ID_CAPACITY, 2, "battery_capacity");
ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_signal"); //ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_signal");
ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal"); //ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal");
ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal"); //ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal");
ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal"); //ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal");
ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal"); //ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal");
ADD_PACKET(ID_UNUSED_2, 1, "unused 2"); //ADD_PACKET(ID_UNUSED_2, 1, "unused 2");
ADD_PACKET(ID_UNUSED_3, 2, "unused 3"); //ADD_PACKET(ID_UNUSED_3, 2, "unused 3");
ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available"); //ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available");
ADD_PACKET(ID_IO_MODE, 1, "oi_mode"); //ADD_PACKET(ID_IO_MODE, 1, "oi_mode");
ADD_PACKET(ID_SONG_NUM, 1, "song_number"); //ADD_PACKET(ID_SONG_NUM, 1, "song_number");
ADD_PACKET(ID_PLAYING, 1, "song_playing"); //ADD_PACKET(ID_PLAYING, 1, "song_playing");
ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets"); //ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets");
ADD_PACKET(ID_VEL, 2, "velocity"); //ADD_PACKET(ID_VEL, 2, "velocity");
ADD_PACKET(ID_RADIUS, 2, "radius"); //ADD_PACKET(ID_RADIUS, 2, "radius");
ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right"); //ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left"); //ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left"); ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left");
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right"); ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
ADD_PACKET(ID_LIGHT, 1, "light_bumper"); ADD_PACKET(ID_LIGHT, 1, "light_bumper");
ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left"); //ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left");
ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left"); //ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left");
ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left"); //ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left");
ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right"); //ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right");
ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right"); //ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right");
ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right"); //ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right");
ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left"); ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left");
ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right"); ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right");
ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current"); //ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current");
ADD_PACKET(ID_RIGHT_MOTOR_CURRENT, 2, "right_motor_current"); //ADD_PACKET(ID_RIGHT_MOTOR_CURRENT, 2, "right_motor_current");
ADD_PACKET(ID_MAIN_BRUSH_CURRENT, 2, "main_brush_current"); //ADD_PACKET(ID_MAIN_BRUSH_CURRENT, 2, "main_brush_current");
ADD_PACKET(ID_SIDE_BRUSH_CURRENT, 2, "side_brush_current"); //ADD_PACKET(ID_SIDE_BRUSH_CURRENT, 2, "side_brush_current");
ADD_PACKET(ID_STASIS, 1, "stasis"); ADD_PACKET(ID_STASIS, 1, "stasis");
totalDataBytes = 0; totalDataBytes = 0;

38
src/serial.cpp
View file

@ -130,6 +130,24 @@ namespace create {
} }
} }
void Serial::notifyDataReady() {
// Validate all packets
data->validateAll();
// Notify first data packets ready
{
boost::lock_guard<boost::mutex> lock(dataReadyMut);
if (!dataReady) {
dataReady = true;
dataReadyCond.notify_one();
}
}
// Callback to notify data is ready
if (callback)
callback();
}
void Serial::onData(const boost::system::error_code& e, const std::size_t& size) { void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
if (e) { if (e) {
CERR("[create::Serial] ", "serial error - " << e.message()); CERR("[create::Serial] ", "serial error - " << e.message());
@ -153,8 +171,10 @@ namespace create {
readState = READ_PACKET_ID; readState = READ_PACKET_ID;
numBytesRead = 0; numBytesRead = 0;
} }
else else {
//notifyDataReady();
readState = READ_HEADER; readState = READ_HEADER;
}
break; break;
case READ_PACKET_ID: case READ_PACKET_ID:
@ -167,6 +187,7 @@ namespace create {
readState = READ_PACKET_BYTES; readState = READ_PACKET_BYTES;
} }
else { else {
//notifyDataReady();
readState = READ_HEADER; readState = READ_HEADER;
} }
break; break;
@ -192,20 +213,7 @@ namespace create {
case READ_CHECKSUM: case READ_CHECKSUM:
if ((byteSum & 0xFF) == 0) { if ((byteSum & 0xFF) == 0) {
// Validate all packets notifyDataReady();
data->validateAll();
// Notify first data packets ready
{
boost::lock_guard<boost::mutex> lock(dataReadyMut);
if (!dataReady) {
dataReady = true;
dataReadyCond.notify_one();
}
}
// Callback to notify data is ready
if (callback)
callback();
} }
else { else {
// Corrupt data // Corrupt data