Added HighlevelService

Co-authored-by: Robert Vollmer <dev@robv.de>

Author: ClemensElflein

CMakeLists.txt

@@ -68,6 +68,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
         src/services/mower_service/mower_service.cpp
         src/services/gps_service/gps_service.cpp
         src/services/input_service/input_service.cpp
+        src/services/high_level_service/high_level_service.cpp
         # BQ2567 driver
         src/drivers/charger/bq_2576/bq_2576.cpp
         # BQ25679 driver
@@ -129,6 +130,7 @@ target_add_service(${CMAKE_PROJECT_NAME} DiffDriveService ${CMAKE_CURRENT_SOURCE
 target_add_service(${CMAKE_PROJECT_NAME} MowerService ${CMAKE_CURRENT_SOURCE_DIR}/services/mower_service.json)
 target_add_service(${CMAKE_PROJECT_NAME} GpsService ${CMAKE_CURRENT_SOURCE_DIR}/services/gps_service.json)
 target_add_service(${CMAKE_PROJECT_NAME} InputService ${CMAKE_CURRENT_SOURCE_DIR}/services/input_service.json)
+target_add_service(${CMAKE_PROJECT_NAME} HighLevelService ${CMAKE_CURRENT_SOURCE_DIR}/services/high_level_service.json)
 
 set_target_properties(${CMAKE_PROJECT_NAME}
         PROPERTIES SUFFIX ".elf")

services

@@ -15,6 +15,7 @@ ImuService imu_service{xbot::service_ids::IMU};
 PowerService power_service{xbot::service_ids::POWER};
 GpsService gps_service{xbot::service_ids::GPS};
 InputService input_service{xbot::service_ids::INPUT};
+HighLevelService high_level_service{xbot::service_ids::HIGH_LEVEL};
 
 void StartServices() {
 #define START_IF_NEEDED(service, id)                \
@@ -36,4 +37,5 @@ void StartServices() {
   START_IF_NEEDED(mower_service, MOWER)
   START_IF_NEEDED(gps_service, GPS)
   START_IF_NEEDED(input_service, INPUT)
+  START_IF_NEEDED(high_level_service, HIGH_LEVEL)
 }

src/services.cpp

@@ -4,6 +4,7 @@
 #include "services/diff_drive_service/diff_drive_service.hpp"
 #include "services/emergency_service/emergency_service.hpp"
 #include "services/gps_service/gps_service.hpp"
+#include "services/high_level_service/high_level_service.hpp"
 #include "services/imu_service/imu_service.hpp"
 #include "services/input_service/input_service.hpp"
 #include "services/mower_service/mower_service.hpp"
@@ -16,6 +17,7 @@ extern ImuService imu_service;
 extern PowerService power_service;
 extern GpsService gps_service;
 extern InputService input_service;
+extern HighLevelService high_level_service;
 
 void StartServices();
 

src/services.hpp

@@ -0,0 +1,46 @@
+#include "high_level_service.hpp"
+
+void HighLevelService::OnStateIDChanged(const HighLevelStatus &new_value) {
+  xbot::service::Lock lk{&mtx_};
+  state_id_ = new_value;
+}
+
+void HighLevelService::OnStateNameChanged(const char *new_value, uint32_t length) {
+  xbot::service::Lock lk{&mtx_};
+  if (new_value && length > 0 && length < state_name_.max_size()) {
+    state_name_ = new_value;
+  }
+}
+
+void HighLevelService::OnSubStateNameChanged(const char *new_value, uint32_t length) {
+  xbot::service::Lock lk{&mtx_};
+  if (new_value && length > 0 && length < sub_state_name_.max_size()) {
+    sub_state_name_ = new_value;
+  }
+}
+
+void HighLevelService::OnGpsQualityChanged(const float &new_value) {
+  xbot::service::Lock lk{&mtx_};
+  gps_quality_ = new_value;
+}
+
+void HighLevelService::OnCurrentAreaChanged(const int16_t &new_value) {
+  xbot::service::Lock lk{&mtx_};
+  current_area_ = new_value;
+}
+
+void HighLevelService::OnCurrentPathChanged(const int16_t &new_value) {
+  xbot::service::Lock lk{&mtx_};
+  current_path_ = new_value;
+}
+
+void HighLevelService::OnCurrentPathIndexChanged(const int16_t &new_value) {
+  xbot::service::Lock lk{&mtx_};
+  current_path_index_ = new_value;
+}
+
+void HighLevelService::OnTransactionEnd() {
+  if (state_changed_callback_) {
+    state_changed_callback_();
+  }
+}

src/services/high_level_service/high_level_service.cpp

@@ -0,0 +1,82 @@
+#ifndef HIGH_LEVEL_SERVICE_HPP
+#define HIGH_LEVEL_SERVICE_HPP
+
+#include <etl/string.h>
+
+#include <HighLevelServiceBase.hpp>
+#include <xbot-service/Lock.hpp>
+
+using namespace xbot::service;
+
+class HighLevelService : public HighLevelServiceBase {
+ private:
+  THD_WORKING_AREA(wa, 1024){};
+
+ public:
+  explicit HighLevelService(uint16_t service_id) : HighLevelServiceBase(service_id, wa, sizeof(wa)) {
+  }
+
+  HighLevelStatus GetStateId() {
+    xbot::service::Lock lk{&mtx_};
+    return state_id_;
+  }
+
+  etl::string<100> GetStateName() {
+    xbot::service::Lock lk{&mtx_};
+    return state_name_;
+  }
+
+  etl::string<100> GetSubStateName() {
+    xbot::service::Lock lk{&mtx_};
+    return sub_state_name_;
+  }
+
+  float GetGpsQuality() {
+    xbot::service::Lock lk{&mtx_};
+    return gps_quality_;
+  }
+
+  int16_t GetCurrentArea() {
+    xbot::service::Lock lk{&mtx_};
+    return current_area_;
+  }
+
+  int16_t GetCurrentPath() {
+    xbot::service::Lock lk{&mtx_};
+    return current_path_;
+  }
+
+  int16_t GetCurrentPathIndex() {
+    xbot::service::Lock lk{&mtx_};
+    return current_path_index_;
+  }
+
+  void SetCallback(const etl::delegate<void()>& callback) {
+    xbot::service::Lock lk{&mtx_};
+    state_changed_callback_ = callback;
+  }
+
+ private:
+  MUTEX_DECL(mtx_);
+
+  HighLevelStatus state_id_ = HighLevelStatus::UNKNOWN;
+  etl::string<100> state_name_{};
+  etl::string<100> sub_state_name_{};
+  float gps_quality_ = 0;
+  int16_t current_area_ = 0;
+  int16_t current_path_ = 0;
+  int16_t current_path_index_ = 0;
+
+  etl::delegate<void()> state_changed_callback_{};
+
+  void OnStateIDChanged(const HighLevelStatus& new_value) override;
+  void OnStateNameChanged(const char* new_value, uint32_t length) override;
+  void OnSubStateNameChanged(const char* new_value, uint32_t length) override;
+  void OnGpsQualityChanged(const float& new_value) override;
+  void OnCurrentAreaChanged(const int16_t& new_value) override;
+  void OnCurrentPathChanged(const int16_t& new_value) override;
+  void OnCurrentPathIndexChanged(const int16_t& new_value) override;
+  void OnTransactionEnd() override;
+};
+
+#endif  // HIGH_LEVEL_SERVICE_HPP