/// <summary>
/// Waits for Serial Data
/// </summary>
/// <param name="timeout">Timeout in milliseconds</param>
/// <returns></returns>
public byte[] Reponse(int timeout, iRPLidarResponse ResponseType)
{
//Receive buffer
byte[] data = new byte[ResponseType.Length];
//Wait for buffer to fill
DateTime time = DateTime.Now;
try
{
//Make sure we dont run longer than specified timeout
while ((DateTime.Now - time).TotalMilliseconds < timeout)
{
//Do we have enough bytes?
if (_serialPort.BytesToRead < ResponseType.Length)
{
Thread.Sleep(10);
}
else
{
//Read 'em
_serialPort.Read(data, 0, ResponseType.Length);
return(data);
}
}
}
catch
{
//Set connection status
this._isConnected = false;
//Then go through the motions
this.Disconnect();
}
return(data);
}
/// <summary>
/// Send Serial Command to RPLidar
/// Todo: Implement Command with Payload and Checksum.
/// </summary>
/// <param name="Command"></param>
public iRPLidarResponse SendCommand(Commands command)
{
if (_isConnected)
{
//Always combine CMD_SYNC_BYTE with the command
byte[] cmdBytes = Command.GetBytes((uint)Protocol.RPLIDAR_CMD_SYNC_BYTE, (uint)command);
//Clear input buffer of any junk
_serialPort.DiscardInBuffer();
//Write command bytes
_serialPort.Write(cmdBytes, 0, cmdBytes.Length);
//Handle Command Response, reponse format is dependent on command type
iRPLidarResponse DataResponse = null;
//We must sleep after executing some commands
bool sleep = false;
switch (command)
{
case Commands.RPLIDAR_CMD_SCAN:
DataResponse = null;
//Reponses are handled in the Scanning thread
break;
case Commands.RPLIDAR_CMD_GET_DEVICE_HEALTH:
DataResponse = new Response_Health();
break;
case Commands.RPLIDAR_CMD_GET_DEVICE_INFO:
DataResponse = new Response_Information();
break;
case Commands.RPLIDAR_CMD_RESET:
sleep = true;
DataResponse = null;
break;
case Commands.RPLIDAR_CMD_STOP:
sleep = true;
DataResponse = null;
break;
case Commands.RPLIDAR_CMD_FORCE_SCAN:
DataResponse = null;
//Reponses are handled in the Scanning thread
//Use with care, returns results without motor rotation synchronization
break;
default:
DataResponse = null;
break;
}
//We must sleep after executing some commands
if (sleep)
{
Thread.Sleep(20);
}
//Read additional data if any
if (DataResponse != null)
{
//Read Command Header Response
iRPLidarResponse hdrType = new Response_Command();
//Poll for data and parse response for the CMD
hdrType.parseData(Reponse(1000, hdrType));
//Poll for the command data and parse response
DataResponse.parseData(Reponse(1000, DataResponse));
return(DataResponse);
}
}
return(null);
}
