add pump control + pid for pump task

Author: vegano1

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

@@ -18,6 +18,7 @@ class PumpPolicy {
     auto enable_pump_tach(bool enable) -> bool;
     auto get_pump_rpm() -> float;
     auto sleep_ms(uint32_t ms) -> void;
+    [[nodiscard]] auto get_time_ms() const -> uint32_t;
     auto enable_pump_control(bool enable) -> void;
 
   private:

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

@@ -111,21 +111,25 @@ struct SetStatusBarStateMessage {
 };
 
 // For internal driving
-struct GetPressureMessage {};
+struct PressureControlMessage {};
 struct PumpControlMessage {};
 
 struct SetTargetPressureMessage {
     uint32_t id = 0;
     double pressure_setpoint = 0;
     double ramp_rate = 0;
     uint32_t duration_s = 0;
+    bool start_pump = false;
     bool vent_after = false;
 };
 
-struct SetTargetPWMMessage {
+struct SetPumpStateMessage {
     uint32_t id = 0;
     bool from_host = false;
-    double pwm_setpoint = 0;
+    // This is from inner pressureTask, can we use this from host?
+    // or do we need something like pwm/duty cycle?
+    double rpm_setpoint = 0;
+    bool run_pump = false;
 };
 
 using HostCommsMessage =
@@ -140,9 +144,9 @@ using SystemMessage =
 
 using UIMessage = ::std::variant<std::monostate, SetStatusBarStateMessage>;
 
-using PressureMessage = ::std::variant<std::monostate, GetPressureMessage,
+using PressureMessage = ::std::variant<std::monostate, PressureControlMessage,
                                        SetTargetPressureMessage>;
 
-using PumpMessage = ::std::variant<std::monostate, PumpControlMessage, SetTargetPWMMessage>;
+using PumpMessage = ::std::variant<std::monostate, PumpControlMessage, SetPumpStateMessage>;
 
 };  // namespace messages

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

@@ -62,12 +62,12 @@ struct PressureControl {
     double ramp_rate = 0;
     uint32_t duration_s = 0;
     bool vent_after = false;
+    bool start_pump = false;
 
     PressureRamp rampgen;
     // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
     PID pid;  // Current PID loop
     uint32_t last_tick = 0;
-    bool enable_pump = false;
 };
 
 const PressureControl pressure_control = {
@@ -178,11 +178,11 @@ class PressureTask {
     }
 
     template <PressureControlPolicy Policy>
-    auto visit_message(const messages::GetPressureMessage& m, Policy& policy)
+    auto visit_message(const messages::PressureControlMessage& m, Policy& policy)
         -> void {
         // Get delta time
         auto timestamp = policy.get_time_ms();
-        auto delta = (timestamp - _pressure_control.last_tick) * MS_TO_SECONDS;
+        auto delta_s = (timestamp - _pressure_control.last_tick) * MS_TO_SECONDS;
         _pressure_control.last_tick = timestamp;
 
         // TODO: add FIR filter for abs pressure.
@@ -201,15 +201,16 @@ class PressureTask {
             std::get<LPSDriverType>(get_sensor(ATM_PRESSURE).driver)
                 .get_pressure();
 
-        // Compute the new target pwm with ramp rate
+        // Compute the new pwm with ramp rate
         double target_setpoint =
             _pressure_control.rampgen.update_setpoint(timestamp);
         auto guage_pressure = abs_a_pressure_mbar - atm_pressure_hpa;
         auto difference = target_setpoint - guage_pressure;
-        auto pwm = _pressure_control.pid.compute(difference, delta);
+        auto rpm = _pressure_control.pid.compute(difference, delta_s);
+        // TODO: clamp the rpm here to something sensible
 
-        // Send new pwm to pump task
-        auto msg = messages::SetTargetPWMMessage{.pwm_setpoint = pwm};
+        // Send new rpm to pump task
+        auto msg = messages::SetPumpStateMessage{.rpm_setpoint = rpm, .run_pump = true};
         static_cast<void>(
             _task_registry->send_to_address(msg, Queues::PumpAddress));
     }
@@ -222,14 +223,15 @@ class PressureTask {
         _pressure_control.ramp_rate = m.ramp_rate;
         _pressure_control.duration_s = m.duration_s;
         _pressure_control.vent_after = m.vent_after;
+        _pressure_control.start_pump = m.start_pump;
 
         // Update ramp rate generator
         // TODO: Do we need to stop pump when we update ramp rate?
         auto current_pressure = _pressure_control.current_pressure;
         auto timestamp = policy.get_time_ms();
         _pressure_control.rampgen.start_ramp(
             current_pressure, m.pressure_setpoint, m.ramp_rate, timestamp);
-        // TODO: kick off pressure control here
+        // TODO: kick off pressure control here, or in sep gcode? maybe StartPump?
         // 0. set target pressure, ramp rate, etc
         // 1. start the pressure driving task (if not started)
         // 1. set the pwm

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

@@ -3,6 +3,7 @@
 #include <cstdint>
 
 #include "core/ack_cache.hpp"
+#include "core/pid.hpp"
 #include "core/queue_aggregator.hpp"
 #include "core/version.hpp"
 #include "firmware/pump_policy.hpp"
@@ -15,6 +16,29 @@
 namespace pump_task {
 
 static constexpr const uint32_t CONTROL_PERIOD_MS = 20;
+static constexpr const double MS_TO_SECONDS = 0.001F;
+
+struct PumpControl {
+    // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
+    double target_rpm = 0.0F;
+    double current_rpm = 0.0F;
+
+    // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
+    PID pid;  // Current PID loop
+    uint32_t last_tick = 0;
+    bool enable_pump = false;
+    bool pump_running = false;
+};
+
+const PumpControl pump_control = {
+    .target_rpm = 0.0f,
+    .pid = PID{.kp = 1,
+               .ki = 0.5,
+               .kd = 0,
+               .sampletime = CONTROL_PERIOD_MS * 1000,
+               .windup_limit_high = 18000,
+               .windup_limit_low = 0},
+};
 
 template <typename P>
 concept PumpControlPolicy = requires(P p) {
@@ -95,18 +119,57 @@ class PumpTask {
         -> void {
         static_cast<void>(m);
         static_cast<void>(policy);
+
+        // Get delta time
+        auto timestamp = policy.get_time_ms();
+        auto delta_s = (timestamp - pump_control.last_tick) * MS_TO_SECONDS;
+        _pump_control.last_tick = timestamp;
+
+        // TODO: Do we want ramp gen here for smooth interpolation?
+        // Compute the new duty cycle
+        auto current_rpm = policy.get_pump_rpm();
+        auto difference = _pump_control.target_rpm - current_rpm;
+        auto duty = _pump_control.pid.compute(difference, delta_s);
+
+        // set the motor duty cycle
+        policy.set_pump_duty_cycle(duty);
     }
 
     template <PumpControlPolicy Policy>
-    auto visit_message(const messages::SetTargetPWMMessage& m, Policy& policy)
+    auto visit_message(const messages::SetPumpStateMessage& m, Policy& policy)
         -> void {
         static_cast<void>(m);
         static_cast<void>(policy);
+
+        // TODO: validate incoming values
+        _pump_control.target_rpm = m.rpm_setpoint;
+        _pump_control.enable_pump = m.run_pump;
+        auto timestamp = policy.get_time_ms();
+        (void)timestamp;
+
+        if (!m.run_pump) {
+            policy.enable_pump_control(false);
+            policy.stop_pump_motor();
+            // TODO: maybe check the rpm here and verify that the pump is off
+            // Might want a way to ramp down when we turn off the pump.
+            _pump_control.pump_running = false;
+            return;
+        }
+
+        // start pump if not running
+        if (m.run_pump && !_pump_control.pump_running) {
+            policy.enable_pump_tach(true);
+            policy.enable_pump_control(true);
+            policy.start_pump_motor();
+            _pump_control.pump_running = true;
+        }
     }
 
     Queue& _message_queue;
     Aggregator* _task_registry;
     bool _initialized{false};
+
+    PumpControl _pump_control = pump_control;
 };
 
 }  // namespace pump_task

stm32-modules/vacuum-module/firmware/pressure_task/freertos_pressure_task.cpp

@@ -40,7 +40,7 @@ static void run_hardware_task(void* param) {
             &last_wake_time,
             // NOLINTNEXTLINE(readability-static-accessed-through-instance)
             pdMS_TO_TICKS(pressure_task::CONTROL_PERIOD_MS));
-        static_cast<void>(_queue.try_send(messages::GetPressureMessage{}));
+        static_cast<void>(_queue.try_send(messages::PressureControlMessage{}));
     }
 }
 

stm32-modules/vacuum-module/firmware/pump_task/pump_policy.cpp

@@ -38,10 +38,6 @@ auto pump_policy::PumpPolicy::get_pump_rpm() -> float {
     return hw_get_pump_rpm();
 }
 
-auto pump_policy::PumpPolicy::sleep_ms(uint32_t ms) -> void {
-    vTaskDelay(pdMS_TO_TICKS(ms));
-}
-
 auto pump_policy::PumpPolicy::enable_pump_control(bool enable) -> void {
     auto handle = static_cast<TaskHandle_t>(hardware_handle);
     if (enable) {
@@ -50,3 +46,13 @@ auto pump_policy::PumpPolicy::enable_pump_control(bool enable) -> void {
         vTaskSuspend(handle);
     }
 }
+
+auto pump_policy::PumpPolicy::sleep_ms(uint32_t ms) -> void {
+    vTaskDelay(pdMS_TO_TICKS(ms));
+}
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+[[nodiscard]] auto pump_policy::PumpPolicy::get_time_ms() const -> uint32_t {
+    return xTaskGetTickCount();
+}
+