Added Emergency Service

Author: ClemensElflein

CMakeLists.txt

@@ -1,15 +1,12 @@
 cmake_minimum_required(VERSION 3.22)
 
+
 set(CONFIG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/cfg)
 set(BOARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/boards/XCORE)
 # Set custom port path
 set(XBOT_CUSTOM_PORT_PATH ${CMAKE_SOURCE_DIR}/portable)
 
 
-# Setup compiler settings
-set(CMAKE_CXX_STANDARD 20)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS ON)
 
 
 
@@ -24,16 +21,19 @@ include("cmake/gcc-arm-none-eabi.cmake")
 set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)
 
 # Enable CMake support for ASM and C languages
-enable_language(C ASM)
+enable_language(C CXX ASM)
 
 # Core project settings
 project(${CMAKE_PROJECT_NAME})
 message("Build type: " ${CMAKE_BUILD_TYPE})
 
 # Create an executable object type
 add_executable(${CMAKE_PROJECT_NAME})
-
-
+set_target_properties(${CMAKE_PROJECT_NAME} PROPERTIES
+        CXX_STANDARD 17
+        CXX_STANDARD_REQUIRED ON
+        CXX_EXTENSIONS ON
+)
 # Add lib subdirectory
 add_subdirectory(ext)
 
@@ -45,23 +45,16 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
 # Add sources to executable
 target_sources(${CMAKE_PROJECT_NAME} PRIVATE
         main.cpp
-        src/globals.c
-        src/globals.h
+        src/globals.cpp
         src/heartbeat.c
-        src/heartbeat.h
         src/id_eeprom.c
-        src/id_eeprom.h
         src/boot_service_discovery.c
-        src/boot_service_discovery.h
         src/status_led.c
-        src/status_led.h
         src/services/imu_service/imu_service.cpp
-        src/services/imu_service/imu_service.hpp
         src/services/power_service/power_service.cpp
-        src/services/power_service/power_service.hpp
+        src/services/emergency_service/emergency_service.cpp
         # BQ2567 driver
         src/drivers/bq_2576/bq_2576.cpp
-        src/drivers/bq_2576/bq_2576.hpp
         # VESC driver
         src/drivers/vesc/buffer.cpp
         src/drivers/vesc/VescUart.cpp
@@ -84,10 +77,12 @@ target_link_libraries(${CMAKE_PROJECT_NAME} PUBLIC
         # Add user defined libraries
         ChibiOS
         LSM6DS3TR
+        etl::etl
 )
 
 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)
 
 set_target_properties(${CMAKE_PROJECT_NAME}
         PROPERTIES SUFFIX ".elf")

cfg/etl_profile.h

@@ -0,0 +1,5 @@
+#ifndef ETL_PROFILE_H
+#define ETL_PROFILE_H
+
+#define ETL_NO_STL
+#endif

cmake/gcc-arm-none-eabi.cmake

@@ -1,8 +1,6 @@
 set(CMAKE_SYSTEM_NAME               Generic)
 set(CMAKE_SYSTEM_PROCESSOR          arm)
 
-set(CMAKE_C_COMPILER_FORCED TRUE)
-set(CMAKE_CXX_COMPILER_FORCED TRUE)
 set(CMAKE_C_COMPILER_ID GNU)
 set(CMAKE_CXX_COMPILER_ID GNU)
 

ext/CMakeLists.txt

@@ -7,4 +7,5 @@ add_subdirectory(xbot_framework)
 target_link_libraries(xbot-service PUBLIC ChibiOS)
 target_compile_definitions(xbot-service PUBLIC XBOT_ENABLE_STATIC_STACK)
 
-add_subdirectory(LSM6DS3TR-C-PID)
+add_subdirectory(LSM6DS3TR-C-PID)
+add_subdirectory(etl)

ext/etl/CMakeLists.txt

@@ -0,0 +1,9 @@
+Include(FetchContent)
+
+FetchContent_Declare(
+        etl
+        GIT_REPOSITORY https://github.com/ETLCPP/etl
+        GIT_TAG        20.39.4
+)
+
+FetchContent_MakeAvailable(etl)

main.cpp

@@ -3,22 +3,25 @@
 #include "hal.h"
 // clang-format on
 
+#ifdef USE_SEGGER_SYSTEMVIEW
 #include <SEGGER_RTT.h>
 #include <SEGGER_RTT_streams.h>
+#endif
 #include <boot_service_discovery.h>
-#include <chprintf.h>
 #include <drivers/vesc/VescUart.h>
-#include <globals.h>
 #include <heartbeat.h>
 #include <id_eeprom.h>
 #include <lwipthread.h>
 #include <status_led.h>
 
+#include <globals.hpp>
 #include <xbot-service/Io.hpp>
 #include <xbot-service/portable/system.hpp>
 
+#include "services/emergency_service/emergency_service.hpp"
 #include "services/imu_service/imu_service.hpp"
 #include "services/power_service/power_service.hpp"
+EmergencyService emergency_service{1};
 ImuService imu_service{4};
 PowerService power_service{5};
 
@@ -27,14 +30,16 @@ 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");
-    chThdSleepMilliseconds(50);
+#endif
+    chThdSleepMilliseconds(1000);
   }
 }
 
@@ -98,11 +103,31 @@ int main(void) {
 
   xbot::service::system::initSystem();
   xbot::service::Io::start();
+  emergency_service.start();
   imu_service.start();
   power_service.start();
 
   chThdCreateStatic(waTestThread, sizeof(waTestThread), NORMALPRIO, test_thread,
                     NULL);
 
+  // Subscribe to global events and dispatch to our services
+  event_listener_t event_listener;
+  chEvtRegister(&mower_events, &event_listener, 1);
+  while (1) {
+    uint32_t event = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_INFINITE);
+    // event no 1 == "mower_events"
+    if (event == 1) {
+      // Get the flags provided by the event
+      uint32_t flags = chEvtGetAndClearFlags(&event_listener);
+      if (flags & MOWER_EVT_EMERGENCY_CHANGED) {
+        // Get the new emergency value
+        chMtxLock(&mower_status_mutex);
+        uint32_t status_copy = mower_status;
+        chMtxUnlock(&mower_status_mutex);
+        // Notify services
+      }
+    }
+  }
+
   chThdSleep(TIME_INFINITE);
 }

src/globals.c renamed to src/globals.cpp

@@ -2,10 +2,16 @@
 // Created by clemens on 01.10.24.
 //
 
-#include "globals.h"
+#include "globals.hpp"
+
 struct board_info board_info = {0};
 struct carrier_board_info carrier_board_info = {0};
 
+EVENTSOURCE_DECL(mower_events);
+MUTEX_DECL(mower_status_mutex);
+// Start with emergency engaged
+uint32_t mower_status = MOWER_FLAG_EMERGENCY_LATCH;
+
 void InitGlobals() {
   ID_EEPROM_GetBoardInfo(&board_info);
   ID_EEPROM_GetCarrierBoardInfo(&carrier_board_info);

src/globals.h

@@ -0,0 +1,29 @@
+//
+// Created by clemens on 01.10.24.
+//
+
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+#include <id_eeprom.h>
+
+#include "ch.h"
+// Event to notify threads whenever the emergency flags have changed
+static constexpr uint32_t MOWER_EVT_EMERGENCY_CHANGED = 1;
+
+// Flag to track if there was an emergency which has not been reset
+static constexpr uint32_t MOWER_FLAG_EMERGENCY_LATCH = 1;
+// Flag to track if there is currently an emergency (this is updated by the
+// high-prio emergency task)
+static constexpr uint32_t MOWER_FLAG_EMERGENCY_ACTIVE = 2;
+
+extern struct board_info board_info;
+extern struct carrier_board_info carrier_board_info;
+
+// event source for mower events (e.g. emergency)
+extern event_source_t mower_events;
+extern mutex_t mower_status_mutex;
+extern uint32_t mower_status;
+
+void InitGlobals();
+#endif  // GLOBALS_H

src/globals.hpp

@@ -0,0 +1,89 @@
+//
+// Created by clemens on 26.07.24.
+//
+
+#include "emergency_service.hpp"
+
+#include "../../globals.hpp"
+
+bool EmergencyService::Configure() {
+  // No config needed
+  return true;
+}
+void EmergencyService::OnStart() {
+  emergency_reason = "Boot";
+  // set the emergency and notify services
+  chMtxLock(&mower_status_mutex);
+  mower_status |= MOWER_FLAG_EMERGENCY_LATCH;
+  chMtxUnlock(&mower_status_mutex);
+  chEvtBroadcastFlags(&mower_events, MOWER_EVT_EMERGENCY_CHANGED);
+}
+
+void EmergencyService::OnStop() {
+  emergency_reason = "Stopped";
+  // set the emergency and notify services
+  chMtxLock(&mower_status_mutex);
+  mower_status |= MOWER_FLAG_EMERGENCY_LATCH;
+  chMtxUnlock(&mower_status_mutex);
+  chEvtBroadcastFlags(&mower_events, MOWER_EVT_EMERGENCY_CHANGED);
+}
+void EmergencyService::OnCreate() {}
+
+void EmergencyService::tick() {
+  // Get the current emergency state
+  chMtxLock(&mower_status_mutex);
+  uint32_t status_copy = mower_status;
+  chMtxUnlock(&mower_status_mutex);
+  bool emergency_latch = (status_copy & MOWER_FLAG_EMERGENCY_LATCH) != 0;
+  bool emergency_active = (status_copy & MOWER_FLAG_EMERGENCY_ACTIVE) != 0;
+  // Check timeout, but only overwrite if no emergency is currently active
+  // reasoning is that we want to keep the original reason and not overwrite
+  // with "timeout"
+  if (!emergency_latch &&
+      chVTTimeElapsedSinceX(last_clear_emergency_message_) > TIME_S2I(1)) {
+    emergency_reason = "Timeout";
+    // set the emergency and notify services
+    chMtxLock(&mower_status_mutex);
+    mower_status |= MOWER_FLAG_EMERGENCY_LATCH;
+    chMtxUnlock(&mower_status_mutex);
+    chEvtBroadcastFlags(&mower_events, MOWER_EVT_EMERGENCY_CHANGED);
+    // The last flags did not have emergency yet, so need to set it here as well
+    emergency_latch = true;
+  }
+
+  StartTransaction();
+  SendEmergencyActive(emergency_active);
+
+  SendEmergencyLatch(emergency_latch);
+  SendEmergencyReason(emergency_reason.c_str(), emergency_reason.length());
+  CommitTransaction();
+}
+
+bool EmergencyService::OnSetEmergencyChanged(const uint8_t& new_value) {
+  if (new_value) {
+    emergency_reason = "High Level Emergency";
+    // set the emergency and notify services
+    chMtxLock(&mower_status_mutex);
+    mower_status |= MOWER_FLAG_EMERGENCY_LATCH;
+    chMtxUnlock(&mower_status_mutex);
+    chEvtBroadcastFlags(&mower_events, MOWER_EVT_EMERGENCY_CHANGED);
+  } else {
+    // Get the current emergency state
+    chMtxLock(&mower_status_mutex);
+    uint32_t status_copy = mower_status;
+    chMtxUnlock(&mower_status_mutex);
+    bool emergency_active = (status_copy & MOWER_FLAG_EMERGENCY_ACTIVE) != 0;
+
+    // want to reset emergency, but only do it, if no emergency exists right now
+    if (!emergency_active) {
+      // clear the emergency and notify services
+      chMtxLock(&mower_status_mutex);
+      mower_status &= ~MOWER_FLAG_EMERGENCY_LATCH;
+      chMtxUnlock(&mower_status_mutex);
+      chEvtBroadcastFlags(&mower_events, MOWER_EVT_EMERGENCY_CHANGED);
+      emergency_reason = "None";
+      last_clear_emergency_message_ = chVTGetSystemTime();
+    }
+  }
+  return true;
+}