bool Bluetooth::setBaud(uint32_t baud) {
// Standardbefehl
char command[] = "AT+BAUD4";
char p = '4';
// Gibt die höchste Stelligkeit der Baudrate an.
uint32_t h = 1000;
// Wähle entsprechende Nummer laut Datenblatt
switch (baud) {
case 1200: p = '1'; h = 1000; break;
case 2400: p = '2'; h = 1000; break;
case 4800: p = '3'; h = 1000; break;
case 9600: p = '4'; h = 1000; break;
case 19200: p = '5'; h = 10000; break;
case 38400: p = '6'; h = 10000; break;
case 57600: p = '7'; h = 10000; break;
case 115200: p = '8'; h = 100000; break;
case 230400: p = '9'; h = 100000; break;
case 460800: p = 'A'; h = 100000; break;
case 921600: p = 'B'; h = 100000; break;
case 1382400: p = 'C'; h = 1000000; break;
default:
return false;
}
command[7] = p;
write(command);
/* Überprüfe Rückgabewert, OK<r>, wobei <r> gleich die Baudrate
* in Dezimalform sein muss
*/
if (receiveChar() != 'O') {
return false;
}
if (receiveChar() != 'K') {
return false;
}
while (h > 0) {
/* Hole zunächst die erste Ziffer der Baudrate,
* dann die jeweils nächste.
*/
char expected = baud / h;
// Überprüfe, ob die Ziffer auch zurück gegeben wurde.
if (receiveChar() != expected) {
return false;
}
// Lösche die höchste Ziffer
baud -= expected * h;
// Gehe eine Dezimalstelle tiefer
h /= 10;
}
// Setze jetzt auch die Baudrate vom USART neu
setBaudrate(baud);
//USART_setBaudrate(baud);
return true;
}
bool Bluetooth::isDeviceOk() {
write("AT");
if (receiveChar() != 'O') {
return false;
}
if (receiveChar() != 'K') {
return false;
}
return true;
}
bool InterfaceAvr::receiveInt(int *value)
{
// static int receiveErrorCounter = 0;
unsigned char character = 0;
int intValue = 0;
//-----------------
// receive MS-Byte
//-----------------
if (receiveChar(&character) == false)
{
// receiveErrorCounter++;
// emit error message already in calling receiveChar!
//
// MASSIVE COMMUNICATION ERROR!
//
// if (receiveErrorCounter >= 4)
// {
// receiveErrorCounter = 0;
// emit tooMuchErrors();
// }
value = 0;
return false;
}
// reset error counter
// receiveErrorCounter = 0;
// bit shifting
intValue = (character << 8);
//-----------------
// receive LS-Byte
//-----------------
if (receiveChar(&character) == false)
{
// emit error message already in calling receiveChar!
value = 0;
return false;
}
// build the int value
// (add the LS-Byte to the MS-Byte)
*value = (intValue + character);
// emit emitMessage( QString("Received int '%1'.").arg(*value) ); // this makes the program to slow and than to crash!!
return true;
}
bool Sim900::sendATCommand(const char *command) {
commandBuffer[0] = '\0';
send(command);
send("\r\n");
do {
char received = receiveChar();
if (received) {
char str[2];
str[0] = received;
str[1] = '\0';
strcat(commandBuffer, str);
if (strncmp(commandBuffer, "\r\nOK\r\n", 6) == 0) {
return true;
} else if (strncmp(commandBuffer, "\r\nERROR\r\n", 9) == 0) {
return false;
}
} else {
return false;
}
} while(true);
}
int main(void)
{
char string[64];
unsigned char count = 0;
// run the initialization routines
initializer();
//Begin forever chatting with the PC
for(;;)
{
// Check to see if a character is waiting
if( isCharAvailable() == 1 )
{
// If a new character is received, get it
string[count++] = receiveChar();
// receive a packet up to 64 bytes long
if(string[count-1] == '\n')// Hyperterminal string ends with \r\n
{
string[count-2] = '\0'; //convert to a string
parseInput(string);
string[0] = '\0';
count = 0;
}
else if(count > 64)
{
count = 0;
string[0] = '\0';
sendString("Error - received > 64 characters");
}
}
}
return 0;
}
void Bluetooth::changeDeviceName(const char* deviceName) {
write("AT+NAME")->write(deviceName);
while (*deviceName) {
receiveChar();
deviceName++;
}
}
void Bluetooth::getData(uint8_t* buffer, uint8_t length) {
while (length) {
*buffer = receiveChar();
buffer++;
length--;
}
}
void Emulation::receiveData(const char* text, int length)
{
emit stateSet(NOTIFYACTIVITY);
bufferedUpdate();
//QString unicodeText = _decoder->toUnicode(text,length);
QString unicodeText = text;
//send characters to terminal emulator
for (int i=0;i<unicodeText.length();i++)
{
receiveChar(unicodeText[i].unicode());
}
//look for z-modem indicator
//-- someone who understands more about z-modems that I do may be able to move
//this check into the above for loop?
for (int i=0;i<length;i++)
{
if (text[i] == '\030')
{
if ((length-i-1 > 3) && (strncmp(text+i+1, "B00", 3) == 0))
emit zmodemDetected();
}
}
}
void FTPClient::retrData(std::string param)
{
char ch;
char tch;
ssize_t numBytes;
std::ofstream out;
std::string token;
std::stringstream ss(param);
// arg may be a ',' seperated list of arguments.
while ( getline(ss, token, ','/* DELIMITER */) )
{
// Establish Data Connection
dataConnect();
//########## Receive file character by character ############
out.open(token.c_str(),std::ofstream::out);
while ( numBytes = receiveChar(this->datafd) )
{
//#TODO strcpy(&ch,(char *)this->rbuffer);
ch = *(this->rbuffer);
out.put(ch);
}
//#######################################################
out.close();
// Close Data Connection (Indicates EOF )
close(this->datafd);
// Create a new data socket for FUTURE.
createDataSock();
}
}
uint8_t Bluetooth::getString(char* buffer, uint8_t length) {
uint8_t read = 0;
while (length) {
*buffer = receiveChar();
if ((*buffer == 13) || (*buffer == 10) || (*buffer == 0)) {
break;
}
buffer++;
length--;
read++;
}
if (length > 0) {
*buffer = '\0';
}
return read;
}
int SickS300::setup()
{
// see SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Get Token" (Telegram type SEND (0x41 0x44))
const unsigned char getTokenCommand[]={0x00,0x00,0x41,0x44,0x19,0x00,0x00,0x05,0xFF,0x07,0x19,0x00,0x00,0x05,0xFF,0x07,0x07,0x0F,0x9F,0xD0};
unsigned char answer = 255;
unsigned int i = 0;
emit message("Initialising Sick S300:");
// send "get token" to laser
emit message("Sending 'get token'...");
for (i=0; i<sizeof(getTokenCommand); i++)
{
if (sendChar(getTokenCommand[i]) == false)
{
emit message( QString("<font color=\"#FF0000\">ERROR sending byte no. %1 (SickS300::setup).</font>").arg(i+1) );
return -1;
}
}
// getting anser 0x00 0x00 0x00 0x00
emit message("Receiving answer...");
for (i=0; i<4; i++)
{
if (receiveChar(&answer) == true)
{
if (answer != 0)
{
emit message(QString("<font color=\"#FF0000\">ERROR: answer byte no. %1 was 0x%2 instead 0x00 (SickS300::readRequestTelegram).</font>").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR getting answer from laser.</font>");
return -1;
}
// emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
}
emit message("Sick laser S300 setup OKAY");
return 0;
}
int SickS300::closeComPort()
{
// see SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Release Token" (Telegram type SEND (0x41 0x44))
const unsigned char releaseTokenCommand[]={0x00,0x00,0x41,0x44,0x19,0x00,0x00,0x05,0xFF,0x07,0x19,0x00,0x00,0x05,0xFF,0x07,0x00,0x00,0xE7,0xB8};
unsigned int i = 0;
emit message("Shutting down Sick S300:");
// send "release token" to laser
emit message("Sending 'release token'...");
for (i=0; i<sizeof(releaseTokenCommand); i++)
{
if (sendChar(releaseTokenCommand[i]) == false)
{
emit message( QString("ERROR sending byte no. %1.").arg(i) );
return -1;
}
}
unsigned char answer = 255;
// getting anser 0x00 0x00 0x00 0x00
for (i=0; i<4; i++)
{
if (receiveChar(&answer) == true)
{
// emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
// error
if (answer == 255)
{
emit message("ERROR getting answer from laser S300.");
return -1;
}
}
}
// close serial port
serialPort->closePort();
return 0;
}
void getString ( char *string, unsigned char *flag )
{
unsigned char count = 0;
while ( 1 )
{
if ( isCharAvailable() == 1 )
{
*( string + count ) = receiveChar ();
if ( *( string + count ) == '\r' || *( string + count ) == '\n' )
break;
else if ( count > 24 )
{
sendString ( "Error - received > 24 characters \r" );
break;
}
count++;
}
}
}
//----------------------------------------------------------------------------
/* static */ UINT CSerialPort::commThread(LPVOID pParam){
//----------------------------------------------------------------------------
// Cast the void pointer passed to the thread back to
// a pointer of CSerialPort class
CSerialPort *port = (CSerialPort*)pParam;
// Set the status variable in the dialog class to
// TRUE to indicate the thread is running.
port->m_bThreadAlive = TRUE;
// Misc. variables
DWORD BytesTransfered = 0;
DWORD Event = 0;
DWORD CommEvent = 0;
DWORD dwError = 0;
BOOL bResult = TRUE;
COMSTAT comstat;
// Clear comm buffers at startup
if (port->m_hComm) // check if the port is opened
PurgeComm(port->m_hComm, PURGE_RXCLEAR | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_TXABORT);
// begin forever loop. This loop will run as long as the thread is alive.
for (;;) {
// Make a call to WaitCommEvent(). This call will return immediatly
// because our port was created as an async port (FILE_FLAG_OVERLAPPED
// and an m_OverlappedStructerlapped structure specified). This call will cause the
// m_OverlappedStructerlapped element m_OverlappedStruct.hEvent, which is part of the m_hEventArray to
// be placed in a non-signeled state if there are no bytes available to be read,
// or to a signeled state if there are bytes available. If this event handle
// is set to the non-signeled state, it will be set to signeled when a
// character arrives at the port.
// we do this for each port!
bResult = WaitCommEvent(port->m_hComm, &Event, &port->m_ov);
if (!bResult) {
// If WaitCommEvent() returns FALSE, process the last error to determin
// the reason..
switch (dwError = GetLastError()){
case ERROR_IO_PENDING:
// This is a normal return value if there are no bytes
// to read at the port.
// Do nothing and continue
break;
case 87:
// Under Windows NT, this value is returned for some reason.
// I have not investigated why, but it is also a valid reply
// Also do nothing and continue.
break;
default:
// All other error codes indicate a serious error has
// occured. Process this error.
// port->processErrorMessage("WaitCommEvent()");
break;
}
}
else{
// If WaitCommEvent() returns TRUE, check to be sure there are
// actually bytes in the buffer to read.
//
// If you are reading more than one byte at a time from the buffer
// (which this program does not do) you will have the situation occur
// where the first byte to arrive will cause the WaitForMultipleObjects()
// function to stop waiting. The WaitForMultipleObjects() function
// resets the event handle in m_OverlappedStruct.hEvent to the non-signelead state
// as it returns.
//
// If in the time between the reset of this event and the call to
// ReadFile() more bytes arrive, the m_OverlappedStruct.hEvent handle will be set again
// to the signeled state. When the call to ReadFile() occurs, it will
// read all of the bytes from the buffer, and the program will
// loop back around to WaitCommEvent().
//
// At this point you will be in the situation where m_OverlappedStruct.hEvent is set,
// but there are no bytes available to read. If you proceed and call
// ReadFile(), it will return immediatly due to the async port setup, but
// GetOverlappedResults() will not return until the next character arrives.
//
// It is not desirable for the GetOverlappedResults() function to be in
// this state. The thread shutdown event (event 0) and the WriteFile()
// event (Event2) will not DoCalculate if the thread is blocked by GetOverlappedResults().
//
// The solution to this is to check the buffer with a call to ClearCommError().
// This call will reset the event handle, and if there are no bytes to read
// we can loop back through WaitCommEvent() again, then proceed.
// If there are really bytes to read, do nothing and proceed.
bResult = ClearCommError(port->m_hComm, &dwError, &comstat);
if (comstat.cbInQue == 0)
continue;
} // end if bResult
// Main wait function. This function will normally block the thread
// until one of three events occur that require action.
Event = WaitForMultipleObjects(3, port->m_hEventArray, FALSE, INFINITE);
switch (Event){
case 0:
// Shutdown event. This is event zero so it will be
// the higest priority and be serviced first.
port->m_bThreadAlive = FALSE;
// Kill this thread. break is not needed, but makes me feel better.
AfxEndThread(100);
// never reached
break;
case 1: // read event
GetCommMask(port->m_hComm, &CommEvent);
if (CommEvent & EV_CTS){
}
if (CommEvent & EV_RXFLAG){
}
if (CommEvent & EV_BREAK){
}
if (CommEvent & EV_ERR){
}
if (CommEvent & EV_RING){
}
if (CommEvent & EV_RXCHAR){
// Receive character event from port.
receiveChar(port, comstat);
}
break;
case 2: // write event
// Write character event from port
writeChar(port);
break;
} // end switch
} // close forever loop
return 0;
}
int SickS300::readUnknownTelegram()
{
// see also SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Read Scandata (block 12)" (Telegram type FETCH (0x45 0x44))
// 00 00
// 45 0x45 means fetch telegram (request data)
// 44 0x44 means data typ: block access
// 0B destination address, block 11 (0x0B) > > > > > > > I don't know what this block means! I logged it with a serial port logger!
// 00 source address
// 00 7B size (this one was measured with a serial port analyser, using the original SICK software CDS.
// FF coordination flag, always 0xFF
// 07 device address is always 0x07, when we have only one S300
const unsigned char unknownCommand[]={0x00,0x00,0x45,0x44,0x0B,0x00,0x00,0x7B,0xFF,0x07};
unsigned char answer = 255;
unsigned int i = 0;
int result = -1;
// QTime x;
// qDebug("S300 start scan @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
// flushing serial input buffer
result = serialPort->purgeRx();
if (result != 0)
{
emit message("<font color=\"#FF0000\">ERROR flushing serial port (SickS300::readRequestTelegram).</font>");
return -1;
}
// send "read block 0B" to laser
emit message("Sending 'read block 0B'...");
for (i=0; i<sizeof(unknownCommand); i++)
{
if (sendChar(unknownCommand[i]) == false)
{
emit message( QString("ERROR sending byte no. %1.").arg(i+1) );
return -1;
}
}
// Reading answer, 4 byte (00 00 00 00)
emit message("Receiving answer...");
for (i=0; i<4; i++)
{
if (receiveChar(&answer) == true)
{
// emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
if (answer != 0)
{
emit message(QString("ERROR: answer byte no. %1 was 0x%2 instead 0x00").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')));
return -1;
}
}
else
{
// error
emit message(QString("ERROR receiving 00 00 00 00 answer at byte no. %1").arg(i+1));
return -1;
}
}
// reading repeated header, 6 byte (0B 00 00 7B FF 07)
emit message("Reading repeated header...");
for (i=0; i<6; i++)
{
if (receiveChar(&answer) == true)
{
// emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
/*
if (answer != 0)
{
emit message(QString("ERROR: answer byte no. %1 was not XXXX").arg(i+1)); // FIXME: check the repeated header
return -1;
}
*/
}
else
{
// error
emit message(QString("ERROR receiving repeated header at byte no. %1").arg(i+1));
return -1;
}
}
// Reading resulting data (don't know, what they mead!)
emit message("Now reading resulting unknown data...");
for (i=0; i<236; i++) // why 240 steps?
{
if (receiveChar(&answer) == true)
{
// emit message( QString("Received byte no. %1: 0x%2").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')) );
// store the data temporary because we get them separated by LowByte and HighByte
// unknownData[i] = answer;
}
else
{
// error
emit message(QString("ERROR receiving scan data at byte no. %1").arg(i+1));
return -1;
}
}
emit message("OKAY");
// Reading CRC, 4 bytes
emit message("Reading CRC...");
for (i=0; i<4; i++)
{
if (receiveChar(&answer) == true)
{
// emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
/// @todo check laser S300 CRC !!
/*
if (answer != 0)
{
emit message(QString("ERROR: answer byte no. %1 was not 0x00").arg(i+1));
return -1;
}
*/
}
else
{
// error
emit message(QString("ERROR receiving CRC at byte no. %1").arg(i+1));
return -1;
}
}
emit message("OKAY");
/*
// convert data from 2 x 16 Bit to one 16 Bit value
// and RETURN the data
for (i=0; i<LASERSAMPLES; i++)
{
if (angle < 270.0)
{
data[i] = (float) ( ((unknownData[2*i+1] & 0x1f)<<8) | unknownData[2*i] );
// qDebug("Copied: no. %d from %d values at %f�. Distance %f cm", i, LASERSAMPLES, angle, distances[i]);
// emit message( QString("Measured distance at angle %1: %2 cm.").arg(angle, 4, 'f', 1).arg(scanResult[i]) );
angle += 0.5;
}
}
*/
// qDebug("S300 scan end @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
return 0;
}
int SickS300::readRequestTelegram()
{
// see SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Read Scandata (block 12)" (Telegram type FETCH (0x45 0x44))
// 00 00
// 45 0x45 means fetch telegram (request data)
// 44 0x44 means data typ: block access
// 0C destination address, e.g. block 12 (0x0C) for scan data
// 00 source address
// 02 22 size (this one was measured with a serial port analyzer, using the original SICK software CDS.
// The following is the original code, regarding the original SICK documentation: 02 FE = size
// FF coordination flag, always 0xFF
// 07 device address is always 0x07, when we have only one S300
const unsigned char readScandataCommand[]={0x00,0x00,0x45,0x44,0x0C,0x00,0x02,0x22,0xFF,0x07};
unsigned char answer = 255;
unsigned int i = 0;
float angle = 0.0;
int result = -1;
// QTime x;
// qDebug("S300 start scan @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
// flushing serial input buffer
result = serialPort->purgeRx();
if (result != 0)
{
emit message(QString("<font color=\"#FF0000\">ERROR %1 flushing serial port (SickS300::readRequestTelegram).</font>").arg(result));
return -1;
}
// send "get scan data" to laser
// emit message("Sending 'get scan data'...");
for (i=0; i<sizeof(readScandataCommand); i++)
{
if (sendChar(readScandataCommand[i]) == false)
{
emit message( QString("<font color=\"#FF0000\">ERROR sending byte no. %1 (SickS300::readRequestTelegram).</font>").arg(i+1) );
return -1;
}
}
// Reading answer, 4 byte (00 00 00 00)
//emit message("Receiving answer...");
for (i=0; i<4; i++)
{
if (receiveChar(&answer) == true)
{
// check if every answer bit is 0x00
// the last byte (no 4) contains the error code if != 0.
if (answer != 0x00)
{
emit message(QString("<font color=\"#FF0000\">ERROR: answer byte no. %1 was 0x%2 instead 0x00 (SickS300::readRequestTelegram).</font>").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')));
return -1;
}
}
else
{
// error
emit message(QString("<font color=\"#FF0000\">ERROR receiving 00 00 00 00 answer at byte no. %1 (SickS300::readRequestTelegram).</font>").arg(i+1));
return -1;
}
}
//-----------------------------------------------------
// reading repeated header, 6 byte (0C 00 02 22 FF 07)
//-----------------------------------------------------
if (receiveChar(&answer) == true)
{
if (answer != 0x0C)
{
emit message(QString("ERROR: answer byte no.1 was 0x%1 instead of 0x0C.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 1 (SickS300::readRequestTelegram).</font>");
return -1;
}
if (receiveChar(&answer) == true)
{
if (answer != 0x00)
{
emit message(QString("ERROR: answer byte no.2 was 0x%1 instead of 0x00.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 2 (SickS300::readRequestTelegram).</font>");
return -1;
}
if (receiveChar(&answer) == true)
{
if (answer != 0x02)
{
emit message(QString("ERROR: answer byte no.3 was 0x%1 instead of 0x02.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 3 (SickS300::readRequestTelegram).</font>");
return -1;
}
if (receiveChar(&answer) == true)
{
if (answer != 0x22)
{
emit message(QString("ERROR: answer byte no.4 was 0x%1 instead of 0x22.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 4 (SickS300::readRequestTelegram).</font>");
return -1;
}
if (receiveChar(&answer) == true)
{
if (answer != 0xFF)
{
emit message(QString("ERROR: answer byte no.5 was 0x%1 instead of 0xFF.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 5 (SickS300::readRequestTelegram).</font>");
return -1;
}
if (receiveChar(&answer) == true)
{
if (answer != 0x07)
{
emit message(QString("ERROR: answer byte no.6 was 0x%1 instead of 0x07.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 6 (SickS300::readRequestTelegram).</font>");
return -1;
}
//-----------------------------------------------------
// reading 2 unknow bytes, 6 byte (usually 0x00 0x08 ??)
//-----------------------------------------------------
if (receiveChar(&answer) == true)
{
if (answer != 0x00)
{
emit message(QString("ERROR: 1st answer byte was 0x%1 instead of 0x00.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving 1st 'unknown byte' (SickS300::readRequestTelegram).</font>");
return -1;
}
if (receiveChar(&answer) == true)
{
if (answer != 0x08)
{
emit message(QString("ERROR: 2nd answer byte was 0x%1 instead of 0x08.").arg( answer, 2, 16, QLatin1Char('0') ));
return -1;
}
}
else
{
// error
emit message("<font color=\"#FF0000\">ERROR receiving 2nd 'unknown byte' (SickS300::readRequestTelegram).</font>");
return -1;
}
//-------------------------------------------------------------------------------
// Reading scan data (the distances!), LASERSAMPLES bytes (@sa laserSickS300.h)
//-------------------------------------------------------------------------------
for (i=0; i<LASERSAMPLES; i++)
{
if (receiveChar(&answer) == true)
{
// emit message( QString("Received byte no. %1: 0x%2").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')) );
// store the data temporary because we get them separated by LowByte and HighByte
scanData[i] = answer;
}
else
{
// error
emit message(QString("<font color=\"#FF0000\">ERROR receiving scan data at byte no. %1 from %2 (SickS300::readRequestTelegram).</font>").arg(i+1).arg(LASERSAMPLES+1));
return -1;
}
}
//----------------------
// Reading CRC, 2 bytes
//----------------------
for (i=0; i<2; i++)
{
if (receiveChar(&answer) == true)
{
// emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
/// @todo check laser S300 CRC !!
/*
if (answer != 0)
{
emit message(QString("ERROR: answer byte no. %1 was not 0x00").arg(i+1));
return -1;
}
*/
}
else
{
// error
emit message(QString("<font color=\"#FF0000\">ERROR receiving CRC at byte no. %1 (SickS300::readRequestTelegram).</font>").arg(i+1));
return -1;
}
}
//----------------------------------------------------
// convert data from 2 x 16 Bit to one 16 Bit value
// and RETURN the distances
//----------------------------------------------------
for (i=0; i<LASERSAMPLES; i++)
{
if (angle < 270.0)
{
distances[i] = (float) ( ((scanData[2*i+1] & 0x1f)<<8) | scanData[2*i] );
// qDebug("Copied: no. %d from %d values at %f�. Distance %f cm", i, LASERSAMPLES, angle, distances[i]);
// emit message( QString("Measured distance at angle %1: %2 cm.").arg(angle, 4, 'f', 1).arg(scanResult[i]) );
angle += 0.5;
/*
// If a measured laser distance is greater than LASERMAXLENGTH, it will be set to the maximum of possible "free" meters!
// (This is due to a bug when reading angle 0, which results in a lenght of 2048 cm)
//
// This value was set to 0.0 m in the past, but doesn't help if we have a 0m line in the middle of "free" lines when navigation...!!
if (distances[i] > LASERMAXLENGTH)
{
distances[i] = LASERMAXLENGTH;
}
*/
}
}
// qDebug("S300 scan end @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
return 0;
}
