BufferedSoftSerial issue

SoftSerialTest.cpp

// SerialTestControl.h

#ifndef SERIAL_TEST_MODULE_H
#define SERIAL_TEST_MODULE_H

#include "SpindleControl.h"

class BufferedSoftSerial;

// This module implements closed loop PID control for spindle RPM.
class SerialTestControl: public SpindleControl {
    public:
        SerialTestControl() {};
        virtual ~SerialTestControl() {};
        void on_module_loaded();

        BufferedSoftSerial* serial;

        void report_speed(void);
};

#endif

//------------------------------------------------------------------------------------------
// SerialTestControl.cpp

#include "libs/Module.h"
#include "libs/Kernel.h"
#include "libs/Pin.h"
#include "SerialTestControl.h"
#include "BufferedSoftSerial.h"
#include "StreamOutputPool.h"

void SerialTestControl::on_module_loaded()
{

    serial = new BufferedSoftSerial(P2_6, P2_4);
    serial->baud(9600);
    serial->format(8, BufferedSoftSerial::None, 1);

    // register for events
    register_for_event(ON_GCODE_RECEIVED);
    register_for_event(ON_GCODE_EXECUTE);

}

void SerialTestControl::report_speed()
{

    char data_out[4] = {0x01, 0x03, 0x01, 0x01};

    serial->write(data_out, 4);

    int count = 0;
    char c;

    while(count < 4) {
        if(serial->readable()) {
            c = serial->getc();
            THEKERNEL->streams->printf("%d: %X\n", count, c);
            count++;
        }
    }

}

//------------------------------------------------------------------------------------------
// Serial Console output

picocom v1.7

port is        : /dev/smoothie0
flowcontrol    : none
baudrate is    : 115200
parity is      : none
databits are   : 8
escape is      : C-a
local echo is  : yes
noinit is      : no
noreset is     : no
nolock is      : no
send_cmd is    : sz -vv
receive_cmd is : rz -vv
imap is        : lfcrlf,
omap is        :
emap is        : crcrlf,delbs,

Terminal ready
M957
0: EF
1: 3
2: 1
3: 1
ok
M957
0: 1
1: 3
2: 1
3: 1
ok