From fc1fb4893060ca5c9829dd68d5932eee6fe469e5 Mon Sep 17 00:00:00 2001 From: jacobperron Date: Sat, 13 Feb 2016 11:50:39 -0800 Subject: [PATCH] Fix bug: Too many packets requested corrupting serial buffer --- include/create/serial.h | 2 ++ src/data.cpp | 66 ++++++++++++++++++++++------------------- src/serial.cpp | 38 ++++++++++++++---------- 3 files changed, 60 insertions(+), 46 deletions(-) diff --git a/include/create/serial.h b/include/create/serial.h index 04b7006..2f641e7 100644 --- a/include/create/serial.h +++ b/include/create/serial.h @@ -88,6 +88,8 @@ namespace create { // Start and stop reading data from Create bool startReading(); void stopReading(); + // Notifies main thread that data is fresh and makes the user callback + void notifyDataReady(); public: Serial(boost::shared_ptr data, const uint8_t& header = create::util::CREATE_2_HEADER); diff --git a/src/data.cpp b/src/data.cpp index 3cc6b59..a213ca6 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -6,57 +6,61 @@ namespace create { Data::Data() { // 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_WALL, 1, "wall"); ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_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_RIGHT, 1, "cliff_right"); - ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall"); - ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents"); + //ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall"); + //ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents"); 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_BUTTONS, 1, "buttons"); - ADD_PACKET(ID_DISTANCE, 2, "distance"); - ADD_PACKET(ID_ANGLE, 2, "angle"); + //ADD_PACKET(ID_DISTANCE, 2, "distance"); + //ADD_PACKET(ID_ANGLE, 2, "angle"); ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state"); ADD_PACKET(ID_VOLTAGE, 2, "voltage"); ADD_PACKET(ID_CURRENT, 2, "current"); ADD_PACKET(ID_TEMP, 1, "temperature"); ADD_PACKET(ID_CHARGE , 2, "battery_charge"); ADD_PACKET(ID_CAPACITY, 2, "battery_capacity"); - ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_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_RIGHT_SIGNAL, 2, "cliff_front_right_signal"); - ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal"); - ADD_PACKET(ID_UNUSED_2, 1, "unused 2"); - ADD_PACKET(ID_UNUSED_3, 2, "unused 3"); - ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available"); - ADD_PACKET(ID_IO_MODE, 1, "oi_mode"); - ADD_PACKET(ID_SONG_NUM, 1, "song_number"); - ADD_PACKET(ID_PLAYING, 1, "song_playing"); - ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets"); - ADD_PACKET(ID_VEL, 2, "velocity"); - ADD_PACKET(ID_RADIUS, 2, "radius"); - ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right"); - ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left"); + //ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_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_RIGHT_SIGNAL, 2, "cliff_front_right_signal"); + //ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal"); + //ADD_PACKET(ID_UNUSED_2, 1, "unused 2"); + //ADD_PACKET(ID_UNUSED_3, 2, "unused 3"); + //ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available"); + //ADD_PACKET(ID_IO_MODE, 1, "oi_mode"); + //ADD_PACKET(ID_SONG_NUM, 1, "song_number"); + //ADD_PACKET(ID_PLAYING, 1, "song_playing"); + //ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets"); + //ADD_PACKET(ID_VEL, 2, "velocity"); + //ADD_PACKET(ID_RADIUS, 2, "radius"); + //ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right"); + //ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left"); ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left"); ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right"); ADD_PACKET(ID_LIGHT, 1, "light_bumper"); - ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_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_RIGHT, 2, "light_bumper_center_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_LEFT, 2, "light_bumper_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_RIGHT, 2, "light_bumper_center_right"); + //ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right"); + //ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right"); ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left"); ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right"); - ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_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_SIDE_BRUSH_CURRENT, 2, "side_brush_current"); + //ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_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_SIDE_BRUSH_CURRENT, 2, "side_brush_current"); ADD_PACKET(ID_STASIS, 1, "stasis"); totalDataBytes = 0; diff --git a/src/serial.cpp b/src/serial.cpp index 89d16dd..4c98111 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -130,6 +130,24 @@ namespace create { } } + + void Serial::notifyDataReady() { + // Validate all packets + data->validateAll(); + + // Notify first data packets ready + { + boost::lock_guard 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) { if (e) { CERR("[create::Serial] ", "serial error - " << e.message()); @@ -153,8 +171,10 @@ namespace create { readState = READ_PACKET_ID; numBytesRead = 0; } - else + else { + //notifyDataReady(); readState = READ_HEADER; + } break; case READ_PACKET_ID: @@ -167,6 +187,7 @@ namespace create { readState = READ_PACKET_BYTES; } else { + //notifyDataReady(); readState = READ_HEADER; } break; @@ -192,20 +213,7 @@ namespace create { case READ_CHECKSUM: if ((byteSum & 0xFF) == 0) { - // Validate all packets - data->validateAll(); - - // Notify first data packets ready - { - boost::lock_guard lock(dataReadyMut); - if (!dataReady) { - dataReady = true; - dataReadyCond.notify_one(); - } - } - // Callback to notify data is ready - if (callback) - callback(); + notifyDataReady(); } else { // Corrupt data