add pressure control task

Author: vegano1

stm32-modules/include/vacuum-module/firmware/control_policy.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <cstdint>
+
+#include "i2c_comms.hpp"
+#include "systemwide.h"
+
+namespace control_policy {
+using namespace i2c::hardware;
+
+class ControlPolicy {
+  public:
+    ControlPolicy(I2C *i2c) : i2c_comms{i2c} {}
+
+    template <size_t Len>
+    auto i2c_write(uint8_t device_address, uint8_t register_address,
+                   std::array<uint8_t, Len> &data) -> bool {
+        auto ret = i2c_comms->i2c_write(device_address, register_address,
+                                        data.data(), Len);
+        return ret == 0;
+    }
+    auto static sleep_ms(uint32_t ms) -> void;
+
+  private:
+    I2C *i2c_comms;
+};
+}  // namespace control_policy

stm32-modules/include/vacuum-module/firmware/firmware_tasks.hpp

@@ -20,4 +20,7 @@ constexpr uint8_t SYSTEM_TASK_PRIORITY = 1;
 
 constexpr size_t UI_STACK_SIZE = 256;
 constexpr uint8_t UI_TASK_PRIORITY = 1;
+
+constexpr size_t CONTROL_STACK_SIZE = 256;
+constexpr uint8_t CONTROL_TASK_PRIORITY = 1;
 }  // namespace tasks

stm32-modules/include/vacuum-module/firmware/freertos_tasks.hpp

@@ -22,3 +22,9 @@ namespace system_control_task {
 // Actual function that runs in the task
 auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void;
 }  // namespace system_control_task
+
+namespace pressure_control_task {
+// Actual function that runs in the task
+auto run(tasks::FirmwareTasks::QueueAggregator* aggregator,
+         i2c::hardware::I2C* i2c_comms) -> void;
+}  // namespace pressure_control_task

stm32-modules/include/vacuum-module/vacuum-module/control_task.hpp

@@ -0,0 +1,96 @@
+#pragma once
+#include <cmath>
+#include <cstdint>
+
+#include "core/ack_cache.hpp"
+#include "core/queue_aggregator.hpp"
+#include "core/version.hpp"
+#include "firmware/control_policy.hpp"
+#include "vacuum-module/errors.hpp"
+#include "vacuum-module/messages.hpp"
+#include "vacuum-module/tasks.hpp"
+#include "hal/message_queue.hpp"
+#include "messages.hpp"
+
+namespace control_task {
+template <typename P>
+concept PressureControlPolicy = requires(P p) {
+    { p.sleep_ms(1) };
+};
+
+using ControlPolicy = control_policy::ControlPolicy;
+using Message = messages::ControlMessage;
+using Error = errors::ErrorCode;
+
+template <template <class> class QueueImpl>
+requires MessageQueue<QueueImpl<Message>, Message>
+class ControlTask {
+  private:
+    using Queue = QueueImpl<Message>;
+    using Aggregator = typename tasks::Tasks<QueueImpl>::QueueAggregator;
+    using Queues = typename tasks::Tasks<QueueImpl>;
+
+  public:
+    explicit ControlTask(Queue& q, Aggregator* aggregator, ControlPolicy* policy)
+        : _message_queue(q),
+          _task_registry(aggregator) {}
+    ControlTask(const ControlTask& other) = delete;
+    auto operator=(const ControlTask& other) -> ControlTask& = delete;
+    ControlTask(ControlTask&& other) noexcept = delete;
+    auto operator=(ControlTask&& other) noexcept -> ControlTask& = delete;
+    ~ControlTask() = default;
+
+    auto provide_aggregator(Aggregator* aggregator) {
+        _task_registry = aggregator;
+    }
+
+    template <PressureControlPolicy Policy>
+    auto run_once(Policy& policy) -> void {
+        if (!_task_registry) {
+            return;
+        }
+
+        if (!_initialized) {
+            _message_queue.set_ready();
+            _initialized = true;
+        }
+
+        auto message = Message(std::monostate());
+        _message_queue.recv(&message);
+        auto visit_helper = [this, &policy](auto& message) -> void {
+            this->visit_message(message, policy);
+        };
+        std::visit(visit_helper, message);
+    }
+
+  private:
+    auto send_error_message(Error error) -> void {
+        if (_task_registry) {
+            auto msg = messages::ErrorMessage{.code = error};
+            static_cast<void>(
+                _task_registry->send_to_address(msg, Queues::HostCommsAddress));
+        }
+    }
+
+    auto send_ack_message(uint32_t response_id, Error error = Error::NO_ERROR)
+        -> void {
+        if (_task_registry) {
+            auto msg = messages::AcknowledgePrevious{
+                .responding_to_id = response_id, .with_error = error};
+            static_cast<void>(
+                _task_registry->send_to_address(msg, Queues::HostCommsAddress));
+        }
+    }
+
+    template <PressureControlPolicy Policy>
+    auto visit_message(const std::monostate& m, Policy& policy) -> void {
+        static_cast<void>(m);
+        static_cast<void>(policy);
+    }
+
+    Queue& _message_queue;
+    Aggregator* _task_registry;
+    bool _initialized{false};
+};
+
+}  // namespace control_task

stm32-modules/include/vacuum-module/vacuum-module/messages.hpp

@@ -122,4 +122,6 @@ using SystemMessage =
 
 using UIMessage = ::std::variant<std::monostate, SetStatusBarStateMessage>;
 
+using ControlMessage = ::std::variant<std::monostate>;
+
 };  // namespace messages

stm32-modules/include/vacuum-module/vacuum-module/tasks.hpp

@@ -17,10 +17,12 @@ struct Tasks {
     using SystemQueue = QueueImpl<messages::SystemMessage>;
     // Message queue for UI task
     using UIQueue = QueueImpl<messages::UIMessage>;
+    // Message queue for Control task
+    using ControlQueue = QueueImpl<messages::ControlMessage>;
 
     // Central aggregator
     using QueueAggregator =
-        queue_aggregator::QueueAggregator<HostCommsQueue, SystemQueue, UIQueue>;
+        queue_aggregator::QueueAggregator<HostCommsQueue, SystemQueue, UIQueue, ControlQueue>;
 
     // Addresses
     static constexpr size_t HostCommsAddress =
@@ -29,6 +31,8 @@ struct Tasks {
         QueueAggregator::template get_queue_idx<SystemQueue>();
     static constexpr size_t UIAddress =
         QueueAggregator::template get_queue_idx<UIQueue>();
+    static constexpr size_t ControlAddress =
+        QueueAggregator::template get_queue_idx<ControlQueue>();
 };
 
 };  // namespace tasks

stm32-modules/vacuum-module/firmware/CMakeLists.txt

@@ -16,6 +16,7 @@ set(VENT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/vent")
 set(PUMP_DIR "${CMAKE_CURRENT_SOURCE_DIR}/pump")
 set(ATMOSPHERE_PS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/atmosphere-pressure-sensor")
 set(VACUUM_PS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/vacuum-pressure-sensor")
+set(CONTROL_DIR "${CMAKE_CURRENT_SOURCE_DIR}/control_task")
 
 # Add source files that should be checked by clang-tidy here
 set(${TARGET_MODULE_NAME}_FW_LINTABLE_SRCS
@@ -31,6 +32,8 @@ set(${TARGET_MODULE_NAME}_FW_LINTABLE_SRCS
     ${PUMP_DIR}/pump_policy.cpp
     ${ATMOSPHERE_PS_DIR}/atmosphere_pressure_sensor_policy.cpp
     ${VACUUM_PS_DIR}/vacuum_pressure_sensor_policy.cpp
+    ${CONTROL_DIR}/freertos_control_task.cpp
+    ${CONTROL_DIR}/control_policy.cpp
     )
 
 # Add source files that should NOT be checked by clang-tidy here

stm32-modules/vacuum-module/firmware/control_task/control_policy.cpp

@@ -0,0 +1,15 @@
+#include "firmware/control_policy.hpp"
+
+#include <cstdint>
+
+#include "FreeRTOS.h"
+#include "projdefs.h"
+#include "task.h"
+
+using namespace control_policy;
+
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto ControlPolicy::sleep_ms(uint32_t ms) -> void {
+    vTaskDelay(pdMS_TO_TICKS(ms));
+}

stm32-modules/vacuum-module/firmware/control_task/freertos_control_task.cpp

@@ -0,0 +1,39 @@
+#include <cstdint>
+
+#include "FreeRTOS.h"
+#include "firmware/firmware_tasks.hpp"
+#include "firmware/freertos_tasks.hpp"
+#include "firmware/i2c_comms.hpp"
+#include "firmware/control_policy.hpp"
+#include "vacuum-module/control_task.hpp"
+#include "task.h"
+
+namespace pressure_control_task {
+using namespace control_policy;
+
+enum class Notifications : uint8_t {
+    INCOMING_MESSAGE = 1,
+};
+
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+static tasks::FirmwareTasks::ControlQueue
+    // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+    _queue(static_cast<uint8_t>(Notifications::INCOMING_MESSAGE), "Control Queue");
+
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+static auto _top_task = control_task::ControlTask(_queue, nullptr, nullptr);
+
+auto run(tasks::FirmwareTasks::QueueAggregator* aggregator,
+         i2c::hardware::I2C* i2c1_comms) -> void {
+    auto* handle = xTaskGetCurrentTaskHandle();
+    _queue.provide_handle(handle);
+    aggregator->register_queue(_queue);
+    _top_task.provide_aggregator(aggregator);
+
+    auto policy = ControlPolicy(i2c1_comms);
+    while (true) {
+        _top_task.run_once(policy);
+    }
+}
+
+}  // namespace control_task

stm32-modules/vacuum-module/firmware/main.cpp

@@ -21,13 +21,17 @@
 using EntryPoint = std::function<void(tasks::FirmwareTasks::QueueAggregator *)>;
 using EntryPointUI = std::function<void(tasks::FirmwareTasks::QueueAggregator *,
                                         i2c::hardware::I2C *)>;
+using EntryPointControl = std::function<void(tasks::FirmwareTasks::QueueAggregator *,
+                                        i2c::hardware::I2C *)>;
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto ui_task_entry = EntryPointUI(ui_control_task::run);
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto host_comms_entry = EntryPoint(host_comms_control_task::run);
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto system_task_entry = EntryPoint(system_control_task::run);
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+static auto control_task_entry = EntryPointControl(pressure_control_task::run);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto host_comms_task =
@@ -41,6 +45,10 @@ static auto ui_task =
 static auto system_task =
     ot_utils::freertos_task::FreeRTOSTask<tasks::SYSTEM_STACK_SIZE, EntryPoint>(
         system_task_entry);
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+static auto control_task =
+    ot_utils::freertos_task::FreeRTOSTask<tasks::CONTROL_STACK_SIZE, EntryPointControl>(
+        control_task_entry);
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static auto aggregator = tasks::FirmwareTasks::QueueAggregator();
@@ -65,6 +73,7 @@ auto main() -> int {
     system_task.start(tasks::SYSTEM_TASK_PRIORITY, "System", &aggregator);
     host_comms_task.start(tasks::COMMS_TASK_PRIORITY, "Comms", &aggregator);
     ui_task.start(tasks::UI_TASK_PRIORITY, "UI", &aggregator, &i2c2_comms);
+    control_task.start(tasks::CONTROL_TASK_PRIORITY, "Control", &aggregator, &i2c2_comms);
 
     vTaskStartScheduler();
     return 0;