diff --git a/functions/ifon.h b/functions/ifon.h index 8156838cf..292d250c6 100644 --- a/functions/ifon.h +++ b/functions/ifon.h @@ -35,12 +35,10 @@ class Ifon { // RETURN VALUE: FUNCTION // 0 when off, 32768 when on, takes OUT_MILLIS to go from 0 to 32768 // takes IN_MILLIS to go from 32768 to 0. -template -class InOutFuncX { + +class InOutFuncSVFBase { public: - FunctionRunResult run(BladeBase* blade) { - out_millis_.run(blade); - in_millis_.run(blade); + FunctionRunResult run(BladeBase* blade, int out_millis, int in_millis) { uint32_t now = micros(); uint32_t delta = now - last_micros_; last_micros_ = now; @@ -50,30 +48,58 @@ class InOutFuncX { // be insanely high. extension = 0.00001; } else { - extension += delta / (out_millis_.calculate(blade) * 1000.0); + extension += delta / (out_millis * 1000.0); extension = std::min(extension, 1.0f); } } else { - extension -= delta / (in_millis_.calculate(blade) * 1000.0); + extension -= delta / (in_millis * 1000.0); extension = std::max(extension, 0.0f); } ret_ = extension * 32768.0; if (!blade->is_on() && ret_ == 0) return FunctionRunResult::ZERO_UNTIL_IGNITION; return FunctionRunResult::UNKNOWN; } + int calculate(BladeBase* blade) { return ret_; } int getInteger(int led) { return ret_; } private: - PONUA SVFWrapper out_millis_; - PONUA SVFWrapper in_millis_; float extension = 0.0; uint32_t last_micros_; int ret_; }; +template +class InOutFuncSVF : public InOutFuncSVFBase { +public: + FunctionRunResult run(BladeBase* blade) { + out_millis_.run(blade); + in_millis_.run(blade); + return InOutFuncSVFBase::run(blade, out_millis_.calculate(blade), in_millis_.calculate(blade)); + } + PONUA SVFWrapper out_millis_; + PONUA SVFWrapper in_millis_; +}; + + +// Optimized specialization +template +class SingleValueAdapter> : public InOutFuncSVF {}; + +template +using InOutFuncX = SingleValueAdapter>; + + +class InOutHelperFBase { +public: + int getInteger(int led) { + return 32768 - clampi32(thres - led * 32768, 0, 32768); + } +protected: + int thres = 0; +}; template -class InOutHelperF { +class InOutHelperF : public InOutHelperFBase { public: FunctionRunResult run(BladeBase* blade) __attribute__((warn_unused_result)) { FunctionRunResult ret = RunFunction(&extension_, blade); @@ -87,16 +113,10 @@ class InOutHelperF { } return FunctionRunResult::UNKNOWN; } - int getInteger(int led) { - return 32768 - clampi32(thres - led * 32768, 0, 32768); - } private: PONUA SVFWrapper extension_; - int thres = 0; }; - - #include "trigger.h" #include "scale.h"