RPLidar A2 M8 problemy z uruchomieniem silnika

[KosteQ95]

Witam. 

Zajmuje się obecnie uruchomieniem lidara RPLidar A2M8: https://www.slamtec.com/en/Lidar/A2 na SBC BananaPi M3: http://www.banana-pi.org/m3.html z zainstalowanym rasbianem jessie. Jako środowiska programistycznego używam Qt gdzie na podstawie SDK producenta lidaru: https://github.com/Slamtec/rplidar_sdk próbuje zaprogramować własne api.  

Problem polega na braku reakcji silnika lidaru po wysłaniu komendy na jego uruchomienie. Przerabiając fragment kody SDK otrzymałem listę komend wysyłanych w celu uruchomienia lidaru i dokładnie tą samą listę wysyła mój program napisany w Qt. 

//Fragment zmienionego kodu api 
int raw_serial::senddata(const unsigned char * data, size_t size)
{
    printf("\n\rSerialWrite: ");
    const unsigned char* tmp = data;
    char buff[100];
    for (int i = 0; i < size; i++) {
        if (*tmp < 16) printf("0");
        printf("%s ", itoa(*tmp, buff, 16));
        tmp++;
    }
  ....
 

//Lista komend wysłana przez program simplescanner z sdk producenta

SerialWrite: a5 ff 04 00 00 00 00 5e 

SerialWrite: a5 f0 02 00 00 57

SerialWrite: a5 50
RPLIDAR S/N: BCD283C1E8839EF2C0E69EF764652805
Firmware Ver: 1.20
Hardware Rev: 2

SerialWrite: a5 52
RPLidar health status : 0

SerialWrite: a5 f0 02 94 02 c1  // Po tej komendzie startuje silnik

//Lista komend wysłana przez program napisany w Qt

A5 FF 04 00 00 00 00 5E 
A5 F0 02 00 00 57 
A5 50
A5 52
A5 F0 02 94 02 C1 

//Klasa obsługi lidaru

#include "rplidar.h"
#include <QThread>


RPLidar::RPLidar(QObject *parent) :
    QObject(parent)
{

}

void RPLidar::setPort(QSerialPort *port)
{
    ptr_serialPort = port;
}

void RPLidar::sendCommand(u_int8_t cmd)
{
    u_int8_t buff[2];

    buff[SYNC_FLAG]=START_FLAG;
    buff[CMD]=cmd;

    ptr_serialPort->write((char*)buff,2);

}

void RPLidar::sendCommandPayload(u_int8_t cmd, u_int8_t* payload, u_int8_t size)
{
     u_int8_t buff[20];
    buff[0]=START_FLAG;
    buff[1]=cmd;
    buff[2]=size;
    for (int i=0; i<size; i++){
        buff[3+i]= *payload;
        payload++;
    }

    u_int8_t checksum = 0;

    for(u_int8_t i=0; i<3+size; i++){
        checksum ^= buff[i];
    }

    buff[3+size]=checksum;
    ptr_serialPort->write((char*)buff,4+size);

}

void RPLidar::setMotorSpeed(u_int16_t speed)
{
    u_int8_t buff[10];
    buff[0]=START_FLAG;
    buff[1]=0xF0;
    buff[2]=0x02;
    buff[3]=(u_int8_t)speed;
    buff[4]=(u_int8_t)(speed>>8);

    u_int8_t checksum = 0;

    for(u_int8_t i=0; i<5; i++){
        checksum ^= buff[i];
    }

    buff[5]=checksum;
    ptr_serialPort->write((char*)buff,6);
}

QString RPLidar::getDeviceInfo()
{
    QByteArray buff;
    sendCommand(GET_INFO);

    if (ptr_serialPort->waitForReadyRead(3000)){
        buff = ptr_serialPort->readAll();
    }

    QString str;
    for(int i=0; i<buff.size(); i++)
    {
       str += QString::number(buff[i],16);
       str += " ";
    }

    return str;
}

QString RPLidar::getDeviceHealth()
{
    sendCommand(GET_HEALTH);
    QByteArray buff;
    if (ptr_serialPort->waitForReadyRead(3000)){
        buff = ptr_serialPort->readAll();
    }

    QString str;
    for(int i=0; i<buff.size(); i++)
    {
       str += QString::number(buff[i],16);
       str += " ";
    }

    return str;
}


// Wywołanie komend po wciśnięciu przycisku 

    u_int8_t send[4]= {0, 0, 0, 0};
    rplidar->sendCommandPayload(0xFF,send,4);
    rplidar->setMotorSpeed(0);
    rplidar->getDeviceInfo();
    rplidar->getDeviceHealth();
    rplidar->setMotorSpeed(660);

 

Czy ktoś miał styczność z tym lidarem i jest w stanie pomóc? Korzystam z płytki akcesoryjnej która generuje PWM dla sterownika silnika lidaru. Poprzez wysłanie komend przez realterm jestem w stanie wejść w tryb scan lecz wysłanie komend startu silnika również go nie uruchamiają.