diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index 4173e2ba17..4d2ed76125 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -522,13 +522,7 @@ expected EtbController::getClosedLoop(percent_t target, percent_t obs } // Normal case - use PID to compute closed loop part - float output = m_pid.getOutput(target, observation, etbPeriodSeconds); - etbDutyAverage = m_dutyAverage.average(output); - - etbDutyRateOfChange = m_dutyRocAverage.average(output - prevOutput); - prevOutput = output; - - return output; + return m_pid.getOutput(target, observation, etbPeriodSeconds); } } @@ -623,6 +617,19 @@ void EtbController::update() { ClosedLoopController::update(); } +expected EtbController::getOutput() { + // total open + closed loop parts + expected output = ClosedLoopController::getOutput(); + if (!output) { + return output; + } + etbDutyAverage = m_dutyAverage.average(output.Value); + + etbDutyRateOfChange = m_dutyRocAverage.average(output.Value - prevOutput); + prevOutput = output.Value; + return output; +} + void EtbController::autoCalibrateTps() { // Only auto calibrate throttles if (m_function == ETB_Throttle1 || m_function == ETB_Throttle2) { diff --git a/firmware/controllers/actuators/electronic_throttle.h b/firmware/controllers/actuators/electronic_throttle.h index de4818e813..96885e4af3 100644 --- a/firmware/controllers/actuators/electronic_throttle.h +++ b/firmware/controllers/actuators/electronic_throttle.h @@ -53,6 +53,7 @@ public: virtual void setIdlePosition(percent_t pos) = 0; virtual void setWastegatePosition(percent_t pos) = 0; virtual void update() = 0; + virtual expected getOutput() = 0; virtual void autoCalibrateTps() = 0; virtual const pid_state_s* getPidState() const = 0; diff --git a/firmware/controllers/actuators/electronic_throttle_impl.h b/firmware/controllers/actuators/electronic_throttle_impl.h index 67c3074d49..70c2f6a92a 100644 --- a/firmware/controllers/actuators/electronic_throttle_impl.h +++ b/firmware/controllers/actuators/electronic_throttle_impl.h @@ -34,6 +34,7 @@ public: // Update the controller's state: read sensors, send output, etc void update() override; + expected getOutput() override; // Called when the configuration may have changed. Controller will // reset if necessary. diff --git a/firmware/controllers/closed_loop_controller.h b/firmware/controllers/closed_loop_controller.h index d2d1ceb1fd..680de134d9 100644 --- a/firmware/controllers/closed_loop_controller.h +++ b/firmware/controllers/closed_loop_controller.h @@ -14,8 +14,7 @@ public: setOutput(outputValue); } -private: - expected getOutput() { + virtual expected getOutput() { expected setpoint = getSetpoint(); // If we don't know the setpoint, return failure. if (!setpoint) { @@ -42,6 +41,7 @@ private: return openLoopResult.Value + closedLoopResult.Value; } +private: // Get the setpoint: where should the controller put the plant? virtual expected getSetpoint() = 0; diff --git a/unit_tests/main.cpp b/unit_tests/main.cpp index 32a37468b1..42c20b8430 100644 --- a/unit_tests/main.cpp +++ b/unit_tests/main.cpp @@ -20,7 +20,7 @@ GTEST_API_ int main(int argc, char **argv) { * See TEST_FROM_TRIGGER_ID to limit test just for last trigger */ // setVerboseTrigger(true); - //::testing::GTEST_FLAG(filter) = "*AllTriggersFixture*"; +// ::testing::GTEST_FLAG(filter) = "*integrated*"; int result = RUN_ALL_TESTS(); // windows ERRORLEVEL in Jenkins batch file seems to want negative value to detect failure return result == 0 ? 0 : -1; diff --git a/unit_tests/mocks.h b/unit_tests/mocks.h index cd1fec8ee3..b4d45c5034 100644 --- a/unit_tests/mocks.h +++ b/unit_tests/mocks.h @@ -30,6 +30,7 @@ public: // ClosedLoopController mocks + MOCK_METHOD(expected, getOutput, (), (override)); MOCK_METHOD(expected, getSetpoint, (), (override)); MOCK_METHOD(expected, observePlant, (), (const, override)); MOCK_METHOD(expected, getOpenLoop, (percent_t setpoint), (override)); diff --git a/unit_tests/tests/actuators/test_etb_integrated.cpp b/unit_tests/tests/actuators/test_etb_integrated.cpp index 248c84ba6e..c9166410a4 100644 --- a/unit_tests/tests/actuators/test_etb_integrated.cpp +++ b/unit_tests/tests/actuators/test_etb_integrated.cpp @@ -30,18 +30,18 @@ TEST(etb, integrated) { etb->update(); ASSERT_EQ(engine->outputChannels.etbTarget, 40); - ASSERT_EQ(etb->prevOutput, 100); - ASSERT_EQ(etb->etbDutyAverage, 50); + ASSERT_NEAR(etb->prevOutput, 120.363, EPS3D); + ASSERT_NEAR(etb->etbDutyAverage, 60.1813, EPS3D); Sensor::setMockValue(SensorType::AcceleratorPedal, 10, true); etb->update(); - ASSERT_EQ(etb->etbDutyAverage, -25); - ASSERT_EQ(etb->etbDutyRateOfChange, -75); + ASSERT_NEAR(etb->etbDutyAverage, -9.89286, EPS3D); + ASSERT_NEAR(etb->etbDutyRateOfChange, -70.074, EPS3D); float destination; int offset = ELECTRONIC_THROTTLE_BASE_ADDRESS + offsetof(electronic_throttle_s, etbDutyRateOfChange); copyRange((uint8_t*)&destination, getLiveDataFragments(), offset, sizeof(destination)); - ASSERT_EQ(destination, -75); + ASSERT_NEAR(destination, -70.074, EPS3D); }