Implemented DiffDrive service

Author: ClemensElflein

CMakeLists.txt

@@ -53,6 +53,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
         src/services/imu_service/imu_service.cpp
         src/services/power_service/power_service.cpp
         src/services/emergency_service/emergency_service.cpp
+        src/services/diff_drive_service/diff_drive_service.cpp
         # BQ2567 driver
         src/drivers/bq_2576/bq_2576.cpp
         # VESC driver
@@ -83,6 +84,7 @@ target_link_libraries(${CMAKE_PROJECT_NAME} PUBLIC
 target_add_service(${CMAKE_PROJECT_NAME} ImuService ${CMAKE_CURRENT_SOURCE_DIR}/services/imu_service.json)
 target_add_service(${CMAKE_PROJECT_NAME} PowerService ${CMAKE_CURRENT_SOURCE_DIR}/services/power_service.json)
 target_add_service(${CMAKE_PROJECT_NAME} EmergencyService ${CMAKE_CURRENT_SOURCE_DIR}/services/emergency_service.json)
+target_add_service(${CMAKE_PROJECT_NAME} DiffDriveService ${CMAKE_CURRENT_SOURCE_DIR}/services/diff_drive_service.json)
 
 set_target_properties(${CMAKE_PROJECT_NAME}
         PROPERTIES SUFFIX ".elf")

boards/XCORE/mcuconf.h

@@ -359,16 +359,16 @@
 /*
  * SERIAL driver system settings.
  */
-#define STM32_SERIAL_USE_USART1             FALSE
-#define STM32_SERIAL_USE_USART2             FALSE
+#define STM32_SERIAL_USE_USART1             TRUE
+#define STM32_SERIAL_USE_USART2             TRUE
 #define STM32_SERIAL_USE_USART3             FALSE
-#define STM32_SERIAL_USE_UART4              FALSE
+#define STM32_SERIAL_USE_UART4              TRUE
 #define STM32_SERIAL_USE_UART5              FALSE
-#define STM32_SERIAL_USE_USART6             FALSE
-#define STM32_SERIAL_USE_UART7              FALSE
-#define STM32_SERIAL_USE_UART8              FALSE
+#define STM32_SERIAL_USE_USART6             TRUE
+#define STM32_SERIAL_USE_UART7              TRUE
+#define STM32_SERIAL_USE_UART8              TRUE
 #define STM32_SERIAL_USE_UART9              FALSE
-#define STM32_SERIAL_USE_USART10            FALSE
+#define STM32_SERIAL_USE_USART10            TRUE
 #define STM32_SERIAL_USE_LPUART1            FALSE
 
 /*
@@ -435,16 +435,16 @@
 /*
  * UART driver system settings.
  */
-#define STM32_UART_USE_USART1               TRUE
-#define STM32_UART_USE_USART2               TRUE
+#define STM32_UART_USE_USART1               FALSE
+#define STM32_UART_USE_USART2               FALSE
 #define STM32_UART_USE_USART3               FALSE
-#define STM32_UART_USE_UART4                TRUE
+#define STM32_UART_USE_UART4                FALSE
 #define STM32_UART_USE_UART5                FALSE
-#define STM32_UART_USE_USART6               TRUE
-#define STM32_UART_USE_UART7                TRUE
-#define STM32_UART_USE_UART8                TRUE
+#define STM32_UART_USE_USART6               FALSE
+#define STM32_UART_USE_UART7                FALSE
+#define STM32_UART_USE_UART8                FALSE
 #define STM32_UART_USE_UART9                FALSE
-#define STM32_UART_USE_USART10              TRUE
+#define STM32_UART_USE_USART10              FALSE
 #define STM32_UART_USART1_RX_DMA_STREAM     STM32_DMA_STREAM_ID_ANY
 #define STM32_UART_USART1_TX_DMA_STREAM     STM32_DMA_STREAM_ID_ANY
 #define STM32_UART_USART2_RX_DMA_STREAM     STM32_DMA_STREAM_ID_ANY

cfg/halconf.h

@@ -142,7 +142,7 @@
  * @brief   Enables the SERIAL subsystem.
  */
 #if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
-#define HAL_USE_SERIAL                      FALSE
+#define HAL_USE_SERIAL                      TRUE
 #endif
 
 /**
@@ -177,7 +177,7 @@
  * @brief   Enables the UART subsystem.
  */
 #if !defined(HAL_USE_UART) || defined(__DOXYGEN__)
-#define HAL_USE_UART                        TRUE
+#define HAL_USE_UART                        FALSE
 #endif
 
 /**
@@ -415,7 +415,7 @@
  *          buffers.
  */
 #if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__)
-#define SERIAL_BUFFERS_SIZE                 16
+#define SERIAL_BUFFERS_SIZE                 128
 #endif
 
 /*===========================================================================*/

main.cpp

@@ -21,28 +21,12 @@
 #include "services/emergency_service/emergency_service.hpp"
 #include "services/imu_service/imu_service.hpp"
 #include "services/power_service/power_service.hpp"
+#include "services/diff_drive_service/diff_drive_service.hpp"
 EmergencyService emergency_service{1};
+DiffDriveService diff_drive{2};
 ImuService imu_service{4};
 PowerService power_service{5};
 
-static THD_WORKING_AREA(waTestThread, 500);
-
-static void test_thread(void *p) {
-  static VescUart vesc_uart(&UARTD2);
-  while (1) {
-#ifdef USE_SEGGER_SYSTEMVIEW
-    SEGGER_SYSVIEW_Print("keepalive");
-    vesc_uart.sendKeepalive();
-    SEGGER_SYSVIEW_Print("request");
-    vesc_uart.requestVescValues();
-    SEGGER_SYSVIEW_Print("parse");
-    vesc_uart.parseVescValues();
-    SEGGER_SYSVIEW_Print("wait");
-#endif
-    chThdSleepMilliseconds(1000);
-  }
-}
-
 /*
  * Application entry point.
  */
@@ -106,9 +90,7 @@ int main(void) {
   emergency_service.start();
   imu_service.start();
   power_service.start();
-
-  chThdCreateStatic(waTestThread, sizeof(waTestThread), NORMALPRIO, test_thread,
-                    NULL);
+  diff_drive.start();
 
   // Subscribe to global events and dispatch to our services
   event_listener_t event_listener;
@@ -125,9 +107,8 @@ int main(void) {
         uint32_t status_copy = mower_status;
         chMtxUnlock(&mower_status_mutex);
         // Notify services
+        diff_drive.OnMowerStatusChanged(status_copy);
       }
     }
   }
-
-  chThdSleep(TIME_INFINITE);
 }

services/diff_drive_service.json

@@ -14,26 +14,36 @@
     },
     {
       "id": 1,
+      "name": "Left ESC Status",
+      "type": "uint8_t"
+    },
+    {
+      "id": 2,
       "name": "Left ESC Temperature",
       "type": "float"
     },
     {
-      "id": 2,
+      "id": 3,
       "name": "Left ESC Current",
       "type": "float"
     },
     {
-      "id": 3,
+      "id": 4,
+      "name": "Right ESC Status",
+      "type": "uint8_t"
+    },
+    {
+      "id": 5,
       "name": "Right ESC Temperature",
       "type": "float"
     },
     {
-      "id": 4,
+      "id": 6,
       "name": "Right ESC Current",
       "type": "float"
     },
     {
-      "id": 5,
+      "id": 7,
       "name": "Wheel Ticks",
       "type": "uint32_t[2]"
     }

src/drivers/vesc/VescUart.cpp

@@ -17,57 +17,60 @@ int VescUart::receiveUartMessage(uint8_t* payloadReceived) {
   bool messageRead = false;
   uint8_t messageReceived[256];
   uint16_t lenPayload = 0;
-  size_t bytes_read = 1;
-  const auto start = chVTGetSystemTimeX();
-  while (chVTTimeElapsedSinceX(start) < TIME_US2I(_TIMEOUT) &&
-         messageRead == false) {
-    while (uartReceiveTimeout(uart_, &bytes_read, messageReceived + counter,
-                              _TIMEOUT) == MSG_OK) {
-      counter += bytes_read;
-      // read one more byte
-      bytes_read = 1;
-      if (counter == 2) {
-        switch (messageReceived[0]) {
-          case 2:
-            endMessage = messageReceived[1] +
-                         5;  // Payload size + 2 for sice + 3 for SRC and End.
-            lenPayload = messageReceived[1];
-
-            if (endMessage >= sizeof(messageReceived)) {
-              // invalid message length, try again
-              counter = 0;
-              bytes_read = 1;
-            } else {
-              // we know exactly how many more bytes to read
-              bytes_read = endMessage - counter;
-            }
-            break;
-
-          case 3:
-            // ToDo: Add Message Handling > 255 (starting with 3)
-
-            break;
-
-          default:
-
-            break;
-        }
+  size_t bytes_read = 0;
+  size_t bytes_to_read = 1;
+  while (bytes_to_read > 0 &&
+         (bytes_read = sdReadTimeout(uart_, messageReceived + counter,
+                                     bytes_to_read, TIME_MS2I(_TIMEOUT))) > 0) {
+    counter += bytes_read;
+    // keep on reading, if a larger chunk was requested
+    bytes_to_read -= bytes_read;
+    // if only one byte was requested, read another one
+    if (bytes_to_read == 0) {
+      bytes_to_read = 1;
+    }
+    if (counter == 2) {
+      switch (messageReceived[0]) {
+        case 2:
+          endMessage = messageReceived[1] +
+                       5;  // Payload size + 2 for sice + 3 for SRC and End.
+          lenPayload = messageReceived[1];
+
+          if (endMessage >= sizeof(messageReceived)) {
+            // invalid message length, try again
+            counter = 0;
+            bytes_to_read = 1;
+          } else {
+            // we know exactly how many more bytes to read
+            bytes_to_read = endMessage - counter;
+          }
+          break;
+
+        case 3:
+          // ToDo: Add Message Handling > 255 (starting with 3)
+
+          break;
+
+        default:
+
+          break;
       }
+    }
 
-      if (counter >= sizeof(messageReceived)) {
-        counter = 0;
-        break;
-      }
+    if (counter >= sizeof(messageReceived)) {
+      counter = 0;
+      break;
+    }
 
-      if (counter == endMessage && messageReceived[endMessage - 1] == 3) {
-        messageReceived[endMessage] = 0;
+    if (counter == endMessage && messageReceived[endMessage - 1] == 3) {
+      messageReceived[endMessage] = 0;
 
-        messageRead = true;
-        break;  // Exit if end of message is reached, even if there is still
-                // more data in the buffer.
-      }
+      messageRead = true;
+      break;  // Exit if end of message is reached, even if there is still
+              // more data in the buffer.
     }
   }
+
   // if(messageRead == false && debugPort != nullptr ) {
   //		debugPort->println("Timeout");
   //	}
@@ -214,17 +217,17 @@ bool VescUart::processReadPacket(uint8_t* message) {
   }
 }
 
-VescUart::VescUart(UARTDriver* uart_handle, uint32_t timeout_ms)
+VescUart::VescUart(SerialDriver* uart_handle, uint32_t timeout_ms)
     : _TIMEOUT(timeout_ms), uart_(uart_handle) {
   nunchuck.valueX = 127;
   nunchuck.valueY = 127;
   nunchuck.lowerButton = false;
   nunchuck.upperButton = false;
+}
+bool VescUart::startDriver() {
   // acquire the UART and never let go
-  uartAcquireBus(uart_);
-  uart_config_.speed = 115200;
-  uart_config_.cr2 = USART_CR2_LINEN | USART_CR2_STOP1_BITS;
-  uartStart(uart_, &uart_config_);
+  serial_config_.speed = 115200;
+  return sdStart(uart_, &serial_config_) == MSG_OK;
 }
 bool VescUart::getFWversion(void) { return getFWversion(0); }
 
@@ -401,5 +404,5 @@ void VescUart::sendKeepalive(uint8_t canId) {
   packSendPayload(payload, payloadSize);
 }
 void VescUart::sendRaw(uint8_t* data, size_t size) {
-  uartSendFullTimeout(uart_, &size, data, TIME_INFINITE);
+  sdWrite(uart_, data, size);
 }

src/drivers/vesc/VescUart.h

@@ -47,7 +47,9 @@ class VescUart {
   /**
    * @brief      Class constructor
    */
-  explicit VescUart(UARTDriver* uart, uint32_t timeout_ms = 100);
+  explicit VescUart(SerialDriver* uart, uint32_t timeout_ms = 100);
+
+  bool startDriver();
 
   /** Variabel to hold measurements returned from VESC */
   dataPackage data;
@@ -185,8 +187,8 @@ class VescUart {
   void printVescValues(void);
 
  private:
-  UARTDriver* uart_;
-  UARTConfig uart_config_{};
+  SerialDriver* uart_;
+  SerialConfig serial_config_{};
   void sendRaw(uint8_t* data, size_t size);
 
   /**

src/drivers/vesc/esc_status.h

@@ -0,0 +1,14 @@
+//
+// Created by clemens on 05.11.24.
+//
+
+#ifndef ESC_STATUS_H
+#define ESC_STATUS_H
+enum {
+  ESC_STATUS_DISCONNECTED = 99u,
+  ESC_STATUS_ERROR = 100u,
+  ESC_STATUS_STALLED = 150u,
+  ESC_STATUS_OK = 200u,
+  ESC_STATUS_RUNNING = 201u,
+};
+#endif  // ESC_STATUS_H