- add last tick sync mechanic for driver control tasks to fix an issue with vTaskSuspend/vTaskResume

- add pump stopped mechanic to validate pump rpm
- reset pumpState when we turn off the pump

Author: vegano1

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

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <atomic>
 #include <cstdint>
 
 #include "i2c_comms.hpp"
@@ -10,8 +11,10 @@ using namespace i2c::hardware;
 
 class PressurePolicy {
   public:
-    PressurePolicy(TaskHandle hw_handle, I2C *i2c1, I2C *i2c2, I2C *i2c3)
+    PressurePolicy(TaskHandle hw_handle, std::atomic<bool> *t_sync, I2C *i2c1,
+                   I2C *i2c2, I2C *i2c3)
         : hardware_handle(hw_handle),
+          t_resync_needed(t_sync),
           i2c_comms1{i2c1},
           i2c_comms2{i2c2},
           i2c_comms3{i2c3} {}
@@ -37,6 +40,7 @@ class PressurePolicy {
 
   private:
     TaskHandle hardware_handle;
+    std::atomic<bool> *t_resync_needed;
 
     I2C *i2c_comms1;
     I2C *i2c_comms2;

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

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <atomic>
 #include <cstdint>
 
 #include "firmware/pump_hardware.h"
@@ -9,7 +10,8 @@ namespace pump_policy {
 
 class PumpPolicy {
   public:
-    PumpPolicy(TaskHandle hw_handle) : hardware_handle(hw_handle) {}
+    PumpPolicy(TaskHandle hw_handle, std::atomic<bool>* t_sync)
+        : hardware_handle(hw_handle), t_resync_needed(t_sync) {}
 
     auto start_pump_motor() -> bool;
     auto stop_pump_motor() -> bool;
@@ -23,5 +25,6 @@ class PumpPolicy {
 
   private:
     TaskHandle hardware_handle;
+    std::atomic<bool>* t_resync_needed;
 };
 }  // namespace pump_policy

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

@@ -475,9 +475,9 @@ struct GetPumpState {
         -> InputIt {
         int res = 0;
         res = snprintf(&*buf, (limit - buf),
-                       "M123 T:%.1f R:%.1f A:%d D:%d E:%d M:%d OK\n", target_rpm,
-                       current_rpm, target_pwm, current_pwm, pump_running,
-                       manual_control);
+                       "M123 T:%.1f R:%.1f A:%d D:%d E:%d M:%d OK\n",
+                       target_rpm, current_rpm, target_pwm, current_pwm,
+                       pump_running, manual_control);
         if (res <= 0) {
             return buf;
         }

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

@@ -157,8 +157,8 @@ struct GetPumpStateResponseMessage {
     double current_rpm;
     uint8_t target_pwm;
     uint8_t current_pwm;
-    bool manual_control;
     bool pump_running;
+    bool manual_control;
 };
 
 using HostCommsMessage =

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

@@ -86,7 +86,7 @@ const PressureControl pressure_control = {
     .pid = PID{.kp = 1,
                .ki = 0.5,
                .kd = 0,
-               .sampletime = CONTROL_PERIOD_MS * 1000,
+               .sampletime = CONTROL_PERIOD_MS,
                .windup_limit_high = 18000,
                .windup_limit_low = 0},
 };

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

@@ -19,15 +19,15 @@ namespace pump_task {
 
 // The frequency the pump control loop runs at.
 // static constexpr const uint32_t CONTROL_PERIOD_HZ = 100;
-static constexpr const uint32_t CONTROL_PERIOD_HZ = 1;
+static constexpr const uint32_t CONTROL_PERIOD_HZ = 20;
 static constexpr const uint32_t CONTROL_PERIOD_MS =
     (1 / CONTROL_PERIOD_HZ) * 1000;
 static constexpr const double MS_TO_SECONDS = 0.001F;
 static constexpr const uint8_t MAX_PWM = 100;
+static constexpr const double MIN_RPM = 0;
 static constexpr const double MAX_RPM = 3000;
 
 struct PumpControl {
-    // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
     double target_rpm = 0.0F;
     double current_rpm = 0.0F;
     uint8_t current_pwm = 0;
@@ -44,10 +44,10 @@ struct PumpControl {
 const PumpControl pump_control = {
     .target_rpm = 0.0f,
     .target_pwm = 0,
-    .pid = PID{.kp = 1,
-               .ki = 0.5,
-               .kd = 0,
-               .sampletime = CONTROL_PERIOD_MS * 1000,
+    .pid = PID{.kp = 2,
+               .ki = 0.8,
+               .kd = 0.1,
+               .sampletime = CONTROL_PERIOD_MS,
                .windup_limit_high = 18000,
                .windup_limit_low = 0},
 };
@@ -132,20 +132,29 @@ class PumpTask {
         static_cast<void>(m);
         static_cast<void>(policy);
 
+        if (!_pump_control.enable_pump) {
+            return;
+        }
+
         // 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 rampgen 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 rpm = policy.get_pump_rpm();
+        // reject non-sense rpm
+        if (rpm < MIN_RPM || rpm >= MAX_RPM) {
+            return;
+        }
+
+        auto difference = _pump_control.target_rpm - rpm;
         auto duty = _pump_control.pid.compute(difference, delta_s);
         duty = std::clamp<uint8_t>(duty, 0, MAX_PWM);
+        // set the motor duty cycle if different
         _pump_control.current_pwm = duty;
-
-        // set the motor duty cycle
+        _pump_control.current_rpm = rpm;
         policy.set_pump_duty_cycle(duty);
     }
 
@@ -163,15 +172,14 @@ class PumpTask {
 
         if (!m.run_pump) {
             policy.enable_pump_control(false);
+            policy.enable_pump_tach(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) {
+            _pump_control.current_rpm = 0;
+            _pump_control.current_pwm = 0;
+        } else if (!_pump_control.pump_running) {
             policy.enable_pump_tach(true);
             policy.enable_pump_control(true);
             policy.start_pump_motor();
@@ -192,8 +200,8 @@ class PumpTask {
             .current_rpm = _pump_control.current_rpm,
             .target_pwm = _pump_control.target_pwm,
             .current_pwm = _pump_control.current_pwm,
-            .manual_control = _pump_control.manual_control,
             .pump_running = _pump_control.pump_running,
+            .manual_control = _pump_control.manual_control,
         };
         static_cast<void>(
             _task_registry->send_to_address(msg, Queues::HostCommsAddress));

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

@@ -23,6 +23,8 @@ static constexpr uint32_t _hardware_stack_size = 128;
 static std::array<StackType_t, _hardware_stack_size> _hardware_stack;
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static StaticTask_t _hardware_data;
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+std::atomic<bool> t_resync_needed = true;
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static tasks::FirmwareTasks::PressureQueue
@@ -36,6 +38,12 @@ static auto _top_task = pressure_task::PressureTask(_queue, nullptr, nullptr);
 static void run_hardware_task(void* param) {
     TickType_t last_wake_time = xTaskGetTickCount();
     while (true) {
+        // Need to resync last time count after suspension
+        if (t_resync_needed.load()) {
+            last_wake_time = xTaskGetTickCount();
+            t_resync_needed.store(false);
+        }
+
         vTaskDelayUntil(
             &last_wake_time,
             // NOLINTNEXTLINE(readability-static-accessed-through-instance)
@@ -57,7 +65,8 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator,
         run_hardware_task, "PressureHardware", _hardware_stack.size(), nullptr,
         1, _hardware_stack.data(), &_hardware_data);
 
-    auto policy = PressurePolicy(hw_handle, i2c1_comms, i2c2_comms, i2c3_comms);
+    auto policy = PressurePolicy(hw_handle, &t_resync_needed, i2c1_comms,
+                                 i2c2_comms, i2c3_comms);
     policy.enable_continous_pressure(false);
     while (true) {
         _top_task.run_once(policy);

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

@@ -1,7 +1,5 @@
 #include "firmware/pressure_policy.hpp"
 
-#include <cstdint>
-
 #include "FreeRTOS.h"
 #include "firmware/pressure_sensor_hardware.h"
 #include "projdefs.h"
@@ -31,6 +29,7 @@ auto PressurePolicy::sensor_reset(PressureSensorID sensor_id) -> void {
 auto PressurePolicy::enable_continous_pressure(bool enable) -> void {
     auto handle = static_cast<TaskHandle_t>(hardware_handle);
     if (enable) {
+        t_resync_needed->store(true);
         vTaskResume(handle);
     } else {
         vTaskSuspend(handle);

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

@@ -20,6 +20,8 @@ static constexpr uint32_t _hardware_stack_size = 128;
 static std::array<StackType_t, _hardware_stack_size> _hardware_stack;
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static StaticTask_t _hardware_data;
+// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
+std::atomic<bool> t_resync_needed = true;
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
 static tasks::FirmwareTasks::PumpQueue
@@ -32,6 +34,12 @@ static auto _top_task = pump_task::PumpTask(_queue, nullptr, nullptr);
 static void run_hardware_task(void* param) {
     TickType_t last_wake_time = xTaskGetTickCount();
     while (true) {
+        // Need to resync last time count after suspension
+        if (t_resync_needed.load()) {
+            last_wake_time = xTaskGetTickCount();
+            t_resync_needed.store(false);
+        }
+
         vTaskDelayUntil(
             &last_wake_time,
             // NOLINTNEXTLINE(readability-static-accessed-through-instance)
@@ -51,7 +59,7 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
         run_hardware_task, "PumpHardware", _hardware_stack.size(), nullptr, 1,
         _hardware_stack.data(), &_hardware_data);
 
-    auto policy = PumpPolicy(hw_handle);
+    auto policy = PumpPolicy(hw_handle, &t_resync_needed);
     policy.enable_pump_control(false);
     while (true) {
         _top_task.run_once(policy);