[Fw] Update Core-STM32 firmware.

2.Firmware/REF-STM32F4-fw/.cproject → 2.Firmware/Core-STM32F4-fw/.cproject

@@ -19,7 +19,7 @@
 						<toolChain id="fr.ac6.managedbuild.toolchain.gnu.cross.exe.debug.682049980" name="Ac6 STM32 MCU GCC" superClass="fr.ac6.managedbuild.toolchain.gnu.cross.exe.debug">
 							<option id="fr.ac6.managedbuild.option.gnu.cross.prefix.14716717" name="Prefix" superClass="fr.ac6.managedbuild.option.gnu.cross.prefix" value="arm-none-eabi-" valueType="string" />
 							<option id="fr.ac6.managedbuild.option.gnu.cross.mcu.1649063108" name="Mcu" superClass="fr.ac6.managedbuild.option.gnu.cross.mcu" value="STM32F405RGTx" valueType="string" />
-							<option id="fr.ac6.managedbuild.option.gnu.cross.board.1450084087" name="Board" superClass="fr.ac6.managedbuild.option.gnu.cross.board" value="REF-STM32F4" valueType="string" />
+							<option id="fr.ac6.managedbuild.option.gnu.cross.board.1450084087" name="Board" superClass="fr.ac6.managedbuild.option.gnu.cross.board" value="REF-STM32F4-fw" valueType="string" />
 							<option id="fr.ac6.managedbuild.option.gnu.cross.core.1083960614" name="Core" superClass="fr.ac6.managedbuild.option.gnu.cross.core" valueType="stringList">
 								<listOptionValue builtIn="false" value="ARM Cortex-M4" />
 								<listOptionValue builtIn="false" value="CM4" />
@@ -147,7 +147,7 @@
 						<toolChain id="fr.ac6.managedbuild.toolchain.gnu.cross.exe.release.1069307058" name="Ac6 STM32 MCU GCC" superClass="fr.ac6.managedbuild.toolchain.gnu.cross.exe.release">
 							<option id="fr.ac6.managedbuild.option.gnu.cross.prefix.14716717" name="Prefix" superClass="fr.ac6.managedbuild.option.gnu.cross.prefix" value="arm-none-eabi-" valueType="string" />
 							<option id="fr.ac6.managedbuild.option.gnu.cross.mcu.1649063108" name="Mcu" superClass="fr.ac6.managedbuild.option.gnu.cross.mcu" value="STM32F405RGTx" valueType="string" />
-							<option id="fr.ac6.managedbuild.option.gnu.cross.board.1450084087" name="Board" superClass="fr.ac6.managedbuild.option.gnu.cross.board" value="REF-STM32F4" valueType="string" />
+							<option id="fr.ac6.managedbuild.option.gnu.cross.board.1450084087" name="Board" superClass="fr.ac6.managedbuild.option.gnu.cross.board" value="REF-STM32F4-fw" valueType="string" />
 							<option id="fr.ac6.managedbuild.option.gnu.cross.core.1083960614" name="Core" superClass="fr.ac6.managedbuild.option.gnu.cross.core" valueType="stringList">
 								<listOptionValue builtIn="false" value="ARM Cortex-M4" />
 								<listOptionValue builtIn="false" value="CM4" />
@@ -269,7 +269,7 @@
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders" />
 	<storageModule moduleId="refreshScope" versionNumber="2">
 		<configuration artifactName="${ProjName}" configurationName="Debug">
-			<resource resourceType="PROJECT" workspacePath="REF-STM32F4" />
+			<resource resourceType="PROJECT" workspacePath="REF-STM32F4-fw" />
 		</configuration>
 	</storageModule>
 </cproject>

2.Firmware/REF-STM32F4-fw/.mxproject → 2.Firmware/Core-STM32F4-fw/.mxproject

@@ -9,48 +9,48 @@ CDefines=USE_HAL_DRIVER;STM32F405xx;USE_HAL_DRIVER;USE_HAL_DRIVER;
 [PreviousGenFiles]
 AdvancedFolderStructure=true
 HeaderFileListSize=16
-HeaderFiles#0=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/gpio.h
-HeaderFiles#1=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/FreeRTOSConfig.h
-HeaderFiles#2=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/adc.h
-HeaderFiles#3=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/can.h
-HeaderFiles#4=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/dma.h
-HeaderFiles#5=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/i2c.h
-HeaderFiles#6=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/spi.h
-HeaderFiles#7=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/tim.h
-HeaderFiles#8=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/usart.h
-HeaderFiles#9=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App/usb_device.h
-HeaderFiles#10=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/Target/usbd_conf.h
-HeaderFiles#11=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App/usbd_desc.h
-HeaderFiles#12=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App/usbd_cdc_if.h
-HeaderFiles#13=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/stm32f4xx_it.h
-HeaderFiles#14=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/stm32f4xx_hal_conf.h
-HeaderFiles#15=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc/main.h
+HeaderFiles#0=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/gpio.h
+HeaderFiles#1=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/FreeRTOSConfig.h
+HeaderFiles#2=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/adc.h
+HeaderFiles#3=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/can.h
+HeaderFiles#4=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/dma.h
+HeaderFiles#5=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/i2c.h
+HeaderFiles#6=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/spi.h
+HeaderFiles#7=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/tim.h
+HeaderFiles#8=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/usart.h
+HeaderFiles#9=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App/usb_device.h
+HeaderFiles#10=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/Target/usbd_conf.h
+HeaderFiles#11=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App/usbd_desc.h
+HeaderFiles#12=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App/usbd_cdc_if.h
+HeaderFiles#13=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/stm32f4xx_it.h
+HeaderFiles#14=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/stm32f4xx_hal_conf.h
+HeaderFiles#15=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc/main.h
 HeaderFolderListSize=3
-HeaderPath#0=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Inc
-HeaderPath#1=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App
-HeaderPath#2=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/Target
+HeaderPath#0=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Inc
+HeaderPath#1=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App
+HeaderPath#2=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/Target
 HeaderFiles=;
 SourceFileListSize=17
-SourceFiles#0=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/gpio.c
-SourceFiles#1=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/freertos.c
-SourceFiles#2=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/adc.c
-SourceFiles#3=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/can.c
-SourceFiles#4=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/dma.c
-SourceFiles#5=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/i2c.c
-SourceFiles#6=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/spi.c
-SourceFiles#7=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/tim.c
-SourceFiles#8=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/usart.c
-SourceFiles#9=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App/usb_device.c
-SourceFiles#10=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/Target/usbd_conf.c
-SourceFiles#11=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App/usbd_desc.c
-SourceFiles#12=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App/usbd_cdc_if.c
-SourceFiles#13=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/stm32f4xx_it.c
-SourceFiles#14=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/stm32f4xx_hal_msp.c
-SourceFiles#15=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/stm32f4xx_hal_timebase_tim.c
-SourceFiles#16=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src/main.c
+SourceFiles#0=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/gpio.c
+SourceFiles#1=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/freertos.c
+SourceFiles#2=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/adc.c
+SourceFiles#3=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/can.c
+SourceFiles#4=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/dma.c
+SourceFiles#5=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/i2c.c
+SourceFiles#6=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/spi.c
+SourceFiles#7=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/tim.c
+SourceFiles#8=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/usart.c
+SourceFiles#9=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App/usb_device.c
+SourceFiles#10=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/Target/usbd_conf.c
+SourceFiles#11=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App/usbd_desc.c
+SourceFiles#12=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App/usbd_cdc_if.c
+SourceFiles#13=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_it.c
+SourceFiles#14=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_hal_msp.c
+SourceFiles#15=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/stm32f4xx_hal_timebase_tim.c
+SourceFiles#16=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src/main.c
 SourceFolderListSize=3
-SourcePath#0=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/Core/Src
-SourcePath#1=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/App
-SourcePath#2=I:/onWorking/_Private/REF-STM32F4-dev/2.Firmware/REF-STM32F4-fw/USB_DEVICE/Target
+SourcePath#0=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/Core/Src
+SourcePath#1=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/App
+SourcePath#2=I:/onWorking/_Private/Dummy-Robot-dev/2.Firmware/Core-STM32F4-fw/USB_DEVICE/Target
 SourceFiles=;
 

2.Firmware/REF-STM32F4-fw/3rdParty/fibre/cpp/include/fibre/protocol.hpp → 2.Firmware/Core-STM32F4-fw/3rdParty/fibre/cpp/include/fibre/protocol.hpp

@@ -158,7 +158,8 @@ class StreamSink
     enum ChannelType_t
     {
         CHANNEL_TYPE_USB,
-        CHANNEL_TYPE_UART
+        CHANNEL_TYPE_UART4,
+        CHANNEL_TYPE_UART5
     };
 
     ChannelType_t channelType;

2.Firmware/REF-STM32F4-fw/Bsp/communication/ascii_processor.cpp → 2.Firmware/Core-STM32F4-fw/Bsp/communication/ascii_processor.cpp

@@ -30,50 +30,19 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink &
 {
     static_assert(sizeof(char) == sizeof(uint8_t));
 
-    // scan line to find beginning of checksum and prune comment
-    uint8_t checksum = 0;
-    size_t checksum_start = SIZE_MAX;
-    for (size_t i = 0; i < len; ++i)
-    {
-        if (buffer[i] == ';')
-        { // ';' is the comment start char
-            len = i;
-            break;
-        }
-        if (checksum_start > i)
-        {
-            if (buffer[i] == '*')
-            {
-                checksum_start = i + 1;
-            } else
-            {
-                checksum ^= buffer[i];
-            }
-        }
-    }
-
     // copy everything into a local buffer so we can insert null-termination
     char cmd[MAX_LINE_LENGTH + 1];
     if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH;
     memcpy(cmd, buffer, len);
 
-    // optional checksum validation
-    bool use_checksum = (checksum_start < len);
-    if (use_checksum)
-    {
-        unsigned int received_checksum;
-        sscanf((const char*) cmd + checksum_start, "%u", &received_checksum);
-        if (received_checksum != checksum)
-            return;
-        len = checksum_start - 1; // prune checksum and asterisk
-    }
-
     cmd[len] = 0; // null-terminate
 
     if (response_channel.channelType == StreamSink::CHANNEL_TYPE_USB)
         OnUsbAsciiCmd(cmd, len, response_channel);
-    else if (response_channel.channelType == StreamSink::CHANNEL_TYPE_UART)
-        OnUartAsciiCmd(cmd, len, response_channel);
+    else if (response_channel.channelType == StreamSink::CHANNEL_TYPE_UART4)
+        OnUart4AsciiCmd(cmd, len, response_channel);
+    else if (response_channel.channelType == StreamSink::CHANNEL_TYPE_UART5)
+        OnUart5AsciiCmd(cmd, len, response_channel);
 }
 
 void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink &response_channel)
@@ -93,7 +62,7 @@ void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink &
 
         // Fetch the next char
         uint8_t c = *(buffer++);
-        bool is_end_of_line = (c == '\r' || c == '\n' || c == '!');
+        bool is_end_of_line = (c == '\r' || c == '\n');
         if (is_end_of_line)
         {
             if (read_active)

2.Firmware/REF-STM32F4-fw/Bsp/communication/ascii_processor.hpp → 2.Firmware/Core-STM32F4-fw/Bsp/communication/ascii_processor.hpp

@@ -18,24 +18,17 @@
 /* Exported functions --------------------------------------------------------*/
 void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel);
 void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink& _responseChannel);
-void OnUartAsciiCmd(const char* _cmd, size_t _len, StreamSink& _responseChannel);
+void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink& _responseChannel);
+void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink& _responseChannel);
 
 // Function to send messages back through specific channel (UART or USB-VCP).
 // Use this function instead of printf because printf will send messages over ALL CHANNEL.
 template<typename ... TArgs>
-void Respond(StreamSink &output, bool include_checksum, const char *fmt, TArgs &&... args)
+void Respond(StreamSink &output , const char *fmt, TArgs &&... args)
 {
     char response[64];
     size_t len = snprintf(response, sizeof(response), fmt, std::forward<TArgs>(args)...);
     output.process_bytes((uint8_t *) response, len, nullptr);
-    if (include_checksum)
-    {
-        uint8_t checksum = 0;
-        for (size_t i = 0; i < len; ++i)
-            checksum ^= response[i];
-        len = snprintf(response, sizeof(response), "*%u", checksum);
-        output.process_bytes((uint8_t *) response, len, nullptr);
-    }
     output.process_bytes((const uint8_t *) "\r\n", 2, nullptr);
 }
 

2.Firmware/REF-STM32F4-fw/Bsp/communication/communication.cpp → 2.Firmware/Core-STM32F4-fw/Bsp/communication/communication.cpp

@@ -37,7 +37,7 @@ void InitCommunication(void)
 extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
 osThreadId_t usbIrqTaskHandle;
 
-void UsbDeferredInterruptTask(void *ctx)
+void UsbDeferredInterruptTask(void* ctx)
 {
     (void) ctx; // unused parameter
 
@@ -57,7 +57,7 @@ void UsbDeferredInterruptTask(void *ctx)
 
 // Thread to handle deffered processing of USB interrupt, and
 // read commands out of the UART DMA circular buffer
-void CommunicationTask(void *ctx)
+void CommunicationTask(void* ctx)
 {
     (void) ctx; // unused parameter
 
@@ -78,13 +78,18 @@ void CommunicationTask(void *ctx)
 }
 
 extern "C" {
-int _write(int file, const char *data, int len);
+int _write(int file, const char* data, int len);
 }
 
 // @brief This is what printf calls internally
-int _write(int file, const char *data, int len)
+int _write(int file, const char* data, int len)
 {
-    usb_stream_output_ptr->process_bytes((const uint8_t *) data, len, nullptr);
-    uart4_stream_output_ptr->process_bytes((const uint8_t *) data, len, nullptr);
+#ifdef DEBUG_VIA_USB_SERIAL
+    usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
+    uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
+#endif
+
+    uart5StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
+
     return len;
 }

2.Firmware/REF-STM32F4-fw/Bsp/communication/interface_can.cpp → 2.Firmware/Core-STM32F4-fw/Bsp/communication/interface_can.cpp

@@ -43,16 +43,9 @@ CAN_context can1Ctx;
 CAN_context can2Ctx;
 static CAN_context* ctxs = nullptr;
 static CAN_RxHeaderTypeDef headerRx;
-static CAN_TxHeaderTypeDef headerTx = {
-    .StdId = boardConfig.canNodeId,
-    .ExtId = 0,
-    .IDE = CAN_ID_STD,
-    .RTR = CAN_RTR_DATA,
-    .DLC = 8,
-    .TransmitGlobalTime = DISABLE
-};;
 static uint8_t data[8];
 
+
 struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
 {
     if (hcan->Instance == CAN1)
@@ -78,7 +71,7 @@ bool StartCanServer(CAN_TypeDef* hcan)
 
     HAL_StatusTypeDef status;
 
-    ctxs->node_id = boardConfig.canNodeId;
+    ctxs->node_id = 0;
     ctxs->serial_number = serialNumber;
     osSemaphoreDef(sem_send_heartbeat);
     ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
@@ -117,23 +110,19 @@ bool StartCanServer(CAN_TypeDef* hcan)
     if (status != HAL_OK)
         return false;
 
-    headerTx = {
-        .StdId = ctxs->node_id,
-        .ExtId = 0,
-        .IDE = CAN_ID_STD,
-        .RTR = CAN_RTR_DATA,
-        .DLC = sizeof(data),
-        .TransmitGlobalTime = DISABLE
-    };
-
     return true;
 }
 
 void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
 {
-    CAN_context* ctx = get_can_ctx(hcan);
-    if (!ctx) return;
-    ctx->tx_msg_cnt++;
+//    CAN_context* ctx = get_can_ctx(hcan);
+//    if (!ctx) return;
+//    ctx->tx_msg_cnt++;
+
+    if (hcan->Instance == CAN1)
+        osSemaphoreRelease(sem_can1_tx);
+    else if (hcan->Instance == CAN2)
+        osSemaphoreRelease(sem_can2_tx);
 }
 
 void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
@@ -245,11 +234,19 @@ void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
         ctx->unexpected_errors++;
 }
 
-void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader = &headerTx)
+void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
 {
-    HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
+    osStatus semaphore_status;
+    if (canCtx->handle->Instance == CAN1)
+        semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
+    else if (canCtx->handle->Instance == CAN2)
+        semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
+    else
+        return;
 
-    // ToDo: make this asynchronous By PZH.
-    delayMicroseconds(300);
+    if (semaphore_status == osOK)
+    {
+        HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
+    }
 }