nacelle sCurtail state update

Author: arloeitreim

include/NacelleConfig.hpp

@@ -85,7 +85,7 @@ namespace LED {
 
 namespace ENCODER {
     constexpr uint_fast16_t START_RUN_2_RPM = 200; // CONFIG
-    inline uint_fast16_t TARGET_RPM = 2200;     // CONFIG
+    inline uint_fast16_t TARGET_RPM = 3200;     // CONFIG
     constexpr uint_fast16_t MAX_RPM = 3000;        // CONFIG
     constexpr uint_fast8_t MAX_RPS = MAX_RPM / UNITS::SECS_PER_MIN;
     constexpr uint_fast16_t MIN_T_mS_PER_REV = 1000 / MAX_RPS;
@@ -160,8 +160,8 @@ namespace PITCHING {
     // todo: Ftarget rpm var
 
     /* PID Config */
-    constexpr float PITCH_Kp = 0.08f; // CONFIG - 0.005f last year // TODO
-    constexpr float PITCH_Ki = 0.000f; // CONFIG - 0.001f last year // TODO
+    constexpr float PITCH_Kp = 0.35f; // CONFIG - 0.005f last year // TODO
+    constexpr float PITCH_Ki = 0.03f; // CONFIG - 0.001f last year // TODO
     constexpr float PITCH_Kd = 0.0f;
 
     inline bool enableRpmOutput = false; // CONFIG - whether to output RPM to log

include/NacelleContainer.hpp

@@ -127,6 +127,7 @@ class NacelleContainer {
     std::atomic<int16_t> d_mVPS = 0;
     std::atomic<int16_t> current_mA = 0;
     std::atomic<int16_t> dIPS = 0;
+    std::atomic<int16_t> power_mW = 0;
 
     NacelleContainer(ActuonixL12 &pitchActuator, PID &pitchPIDController,
                      NacelleComms &nacelleComms)
@@ -146,18 +147,23 @@ class NacelleContainer {
         return true;
         // return angularAccel_RPMPS < 20;
     } // todo
+
+    inline bool isTargetPowerExceeded(float targetPower_mW) {
+        return (power_mW > targetPower_mW);
+    }
+
     inline bool isTargetRPMExceeded() const {
         return (currentRPM > ENCODER::TARGET_RPM);
     }
 
-
     // checks if the rpm has been in a steady state (within tolerance) for a certain duration
     bool isTargetRPMExceededHysteresis()  {
         const TickType_t now = xTaskGetTickCount();
         if (std::abs(static_cast<int32_t>(currentRPM) -
              static_cast<int32_t>(ENCODER::TARGET_RPM)) < steadyRpmTolerance) {
             if (initialTime != 0) {
                 if ((now - initialTime) >= pdMS_TO_TICKS(MIN_STEADY_STATE_DURATION_MS)) {
+                    ESP_LOGI(TAG, "Target RPM exceeded hysteresis condition met. Current RPM: %d, Target RPM: %d", currentRPM.load(), ENCODER::TARGET_RPM);
                     return true;
                 }
             }
@@ -291,6 +297,6 @@ class NacelleContainer {
     bool powerPositive = false;
     bool enableSafetyFlag = true;
     TickType_t initialTime = 0;
-    constexpr static uint32_t MIN_STEADY_STATE_DURATION_MS = 5000; // 5 seconds
-    constexpr static uint8_t steadyRpmTolerance = 20; // RPM tolerance for steady state check
+    constexpr static uint32_t MIN_STEADY_STATE_DURATION_MS = 1500; // minimum time for the rpm to be in relative steady state before setpoint is adjusted
+    constexpr static uint8_t steadyRpmTolerance = 80; // RPM tolerance for steady state check
 };

include/NacelleFSM.hpp

@@ -15,8 +15,8 @@
 class NacelleFSM {
   public: // MARK: Public
     static constexpr const char *TAG = "NFSM";
-    float power = 0;
-    float targetPower = 38; // W
+    uint32_t targetPower_mW = 27000; // mW
+    constexpr static int_fast16_t CURTAIL_EXIT_RPM_TOLERANCE = 400; // RPM tolerance for exiting curtailment, prevents oscillation
 
     /**
      * @brief Construct a new Nacelle FSM object
@@ -145,7 +145,7 @@ class NacelleFSM {
 
             return UPDATE_RESULT::STATE_CHANGED;
         } else if ((currentState == FSMCommon::States::sRunLoad) &&
-                   nacelle.isTargetRPMExceeded()) {
+                   nacelle.isTargetPowerExceeded(targetPower_mW)) { // todo
             // sRunLoad -> sCurtail
             currentState = FSMCommon::States::sCurtail;
 
@@ -164,17 +164,17 @@ class NacelleFSM {
         } else if ((currentState == FSMCommon::States::sCurtail) &&
                    nacelle.isTargetRPMExceededHysteresis()) {
 
-            constexpr float POWER_TOLERANCE = 0.06f;
+            constexpr float POWER_TOLERANCE = 0.05f;
             constexpr int_fast16_t RPM_STEP = 40;
 
-            float dPower = power - targetPower;
+            float powerError = nacelle.power_mW - targetPower_mW; // only checks once, noise could mess this up
 
-            if ((dPower) >  POWER_TOLERANCE * targetPower) {
+            if ((powerError) >=  POWER_TOLERANCE * targetPower_mW) {
                 nacelle.pitchPIDController.setTarget(ENCODER::TARGET_RPM - RPM_STEP); // todo adjust decrement amount
                 ENCODER::TARGET_RPM = ENCODER::TARGET_RPM - RPM_STEP; // todo adjust decrement amount
                 ESP_LOGI(TAG, "Decreasing target RPM to %u due to overpower condition", ENCODER::TARGET_RPM);
             }
-            else if (dPower < -POWER_TOLERANCE * targetPower) {
+            else if (powerError <= (-1) * POWER_TOLERANCE * targetPower_mW) {
                 nacelle.pitchPIDController.setTarget(ENCODER::TARGET_RPM + RPM_STEP); // todo adjust increment amount
                 ENCODER::TARGET_RPM = ENCODER::TARGET_RPM + RPM_STEP; // todo adjust increment amount
                 ESP_LOGI(TAG, "Increasing target RPM to %u due to underpower condition", ENCODER::TARGET_RPM);
@@ -183,6 +183,12 @@ class NacelleFSM {
                 nacelle.pitchPIDController.disable();
                 ESP_LOGI(TAG, "Power within acceptable range, PID disabled");
             }
+
+            if (nacelle.currentRPM < ENCODER::TARGET_RPM - CURTAIL_EXIT_RPM_TOLERANCE) {
+                currentState = FSMCommon::States::sRunLoad;
+                ESP_LOGI(TAG, "Current RPM below target RPM, transitioning back to sRunLoad");
+            }
+
             // sCurtail -> sRunLoad
             //currentState = FSMCommon::States::sRunLoad;
 
@@ -203,8 +209,8 @@ class NacelleFSM {
         return UPDATE_RESULT::ERROR;
     }
 
-    void setPower(float power) {
-        this->power = power;
+    void setTargetPower(float targetPower_mW) {
+        this->targetPower_mW = targetPower_mW;
     }
 
   private:                     // MARK: Private

lib/SerialInterface/SerialInterface.hpp

@@ -103,9 +103,9 @@ class SerialInterface {
                 ESP_LOGI(TAG, "Disabled RPM output to log");
 
             }
-            else if (command.equalsIgnoreCase("setPower")) {
-                nacelleFSM.setPower(parts.tokens[1].toFloat());
-                ESP_LOGI(TAG, "Updated power: %.2f", nacelleFSM.power);
+            else if (command.equalsIgnoreCase("setTargetPower")) {
+                nacelleFSM.setTargetPower(parts.tokens[1].toFloat());
+                ESP_LOGI(TAG, "Updated target power: %.2f", nacelleFSM.targetPower_mW);
             }
             else if (command.equalsIgnoreCase("help")) {
                 Serial.println("Available commands:");

src/main.cpp

@@ -440,6 +440,7 @@ vTaskRecvData([[maybe_unused]] void *pvParameters) { // NOSONAR
                 nacelle.d_mVPS = packet.d_mVPS;
                 nacelle.current_mA = packet.current_mA;
                 nacelle.dIPS = packet.dIPS;
+                nacelle.power_mW = packet.powerIfWholeNum_mW;
                 nacelle.setSafetyFlag(
                     static_cast<ESTOP_TYPE_FAST>(packet.safety));
             }