receiver ESP32
#include <HardwareSerial.h>
HardwareSerial GPSSerial ( 1 );
// pin 2=RX, pin 15=TX
HardwareSerial LIDARSerial ( 2 );
// pin 26=RX, pin 25=TX
void setup()
{
LIDARSerial.begin ( SerialDataBits, SERIAL_8N1, 26, 25 );
GPSSerial.begin ( GPS_DataBits, SERIAL_8N1, 2, 15 ); // begin GPS hardware serial
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
void fReceiveSerial_LIDAR( void * parameters )
{
bool BeginSentence = false;
sSerial.reserve ( StringBufferSize300 );
char OneChar;
for ( ;; )
{
EventBits_t xbit = xEventGroupWaitBits (eg, evtReceiveSerial_LIDAR, pdTRUE, pdTRUE, portMAX_DELAY);
if ( LIDARSerial.available() >= 1 )
{
while ( LIDARSerial.available() )
{
OneChar = LIDARSerial.read();
if ( BeginSentence )
{
if ( OneChar == ‘>’)
{
if ( xSemaphoreTake( sema_ParseLIDAR_ReceivedSerial, xSemaphoreTicksToWait10 ) == pdTRUE )
{
xQueueOverwrite( xQ_LIDAR_Display_INFO, ( void * ) &sSerial );
xEventGroupSetBits( eg, evtParseLIDAR_ReceivedSerial );
//
}
BeginSentence = false;
break;
}
sSerial.concat ( OneChar );
}
else
{
if ( OneChar == ‘<’ )
{
sSerial = “”; // clear string buffer
BeginSentence = true; // found begining of sentence
}
}
} // while ( LIDARSerial.available() )
} //if ( LIDARSerial.available() >= 1 )
xSemaphoreGive( sema_ReceiveSerial_LIDAR );
}
vTaskDelete( NULL );
} //void fReceiveSerial_LIDAR( void * parameters )
//////////////////////////////////////////////////////////////////////////////////////////
sender esp32
#include <HardwareSerial.h>
HardwareSerial SerialTFMini( 1 );
HardwareSerial SerialBrain( 2 );
////// serial(1) = pin27=RX green, pin26=TX white
////// serial(2) = pin16=RXgreen , pin17=TX white
void setup()
{
Serial.begin( SerialDataBits );
SerialBrain.begin( SerialDataBits );
SerialTFMini.begin( SerialDataBits, SERIAL_8N1, 27, 26 );
// Initialize the TFMini LIDAR
tfmini.begin(&SerialTFMini);
}
void fSendLIDAR_InfoSerialToBrain( void *pvParameters )
{
struct stu_LIDAR_INFO pxLIDAR_INFO;
for ( ;; )
{
xEventGroupWaitBits (eg, evtGetIMU, pdTRUE, pdTRUE, portMAX_DELAY);
xSemaphoreTake( sema_LIDAR_FOR_ALARM, xSemaphoreTicksToWait );
xQueueReceive ( xQ_LIDAR_FOR_ALARM, &pxLIDAR_INFO, QueueReceiveDelayTime );
xSemaphoreGive( sema_LIDAR_FOR_ALARM );
int CellCount = 1 ;
// Serial.print ( " fSendLIDAR_InfoSerialToBrain " );
// // send LIDAR info for alarm
String sSerial = "";
sSerial.reserve ( 300 );
sSerial.concat( "<#," ); //sentence begin
sSerial.concat( String(ScanPoints) + "," );
sSerial.concat( String(pxLIDAR_INFO.ServoSweepUp) + "," ); // get direction of scan
for ( CellCount; CellCount <= ScanPoints; CellCount++ )
{
sSerial.concat( String(pxLIDAR_INFO.Range[CellCount]) + "," );
}
sSerial.concat( ">" ); //sentence end
vTaskDelay( 10 );
SerialBrain.println ( sSerial );
}
vTaskDelete( NULL );
} // void fSendLIDAAR_InfoSerialToBrain( void *pvParameters )
////
void fSendSerialToBrain( void *pvParameters )
{
struct stu_LIDAR_INFO pxLIDAR_INFO;
xSemaphoreGive ( sema_SendSerialToBrain );
String sSerial;
sSerial.reserve ( 300 );
for (;;)
{
xEventGroupWaitBits (eg, evtSendSerialToBrain, pdTRUE, pdTRUE, portMAX_DELAY);
// Serial.println ( " fSendSerialToBrain " );
if ( xSemaphoreTake( sema_SendSerialToBrain, xZeroTicksToWait ) == pdTRUE ) // grab semaphore, no wait
{
int CellCount = 1;
xSemaphoreTake( sema_LIDAR_INFO, xSemaphoreTicksToWait );
xQueueReceive ( xQ_LIDAR_INFO, &pxLIDAR_INFO, QueueReceiveDelayTime );
xSemaphoreGive( sema_LIDAR_INFO );
sSerial.concat( "<!," ); //sentence begin
sSerial.concat( String(ScanPoints) + "," );
for ( CellCount; CellCount <= ScanPoints; CellCount++ )
{
sSerial.concat( String(pxLIDAR_INFO.Range[CellCount]) + "," );
}
sSerial.concat( ">" ); //sentence end
// Serial.println ( sSerial );
SerialBrain.println ( sSerial );
// ////
// xEventGroupSetBits( eg, evtSendLIDAR_Info_For_ALARM );
// xSemaphoreGive ( sema_SendSerialToBrain );
sSerial = "";
}
}
vTaskDelete( NULL );
} // void fSendSerialToBrain( void *pvParameters )