*

2025-05-13 01:34:53 +03:00
parent 427735e23d
commit 83f3f1c7d4
945 changed files with 633484 additions and 0 deletions
@@ -0,0 +1,89 @@
+#include "odrive_main.h"
+
+#include "low_level.h"
+
+ThermistorCurrentLimiter::ThermistorCurrentLimiter(uint16_t adc_channel,
+                                                   const float* const coefficients,
+                                                   size_t num_coeffs,
+                                                   const float& temp_limit_lower,
+                                                   const float& temp_limit_upper,
+                                                   const bool& enabled) :
+    adc_channel_(adc_channel),
+    coefficients_(coefficients),
+    num_coeffs_(num_coeffs),
+    temperature_(NAN),
+    temp_limit_lower_(temp_limit_lower),
+    temp_limit_upper_(temp_limit_upper),
+    enabled_(enabled)
+{
+}
+
+void ThermistorCurrentLimiter::update() {
+    const float normalized_voltage = get_adc_relative_voltage_ch(adc_channel_);
+    float raw_temperature_ = horner_poly_eval(normalized_voltage, coefficients_, num_coeffs_);
+
+    constexpr float tau = 0.1f; // [sec]
+    float k = current_meas_period / tau;
+    float val = raw_temperature_;
+    for (float& lpf_val : lpf_vals_) {
+        lpf_val += k * (val - lpf_val);
+        val = lpf_val;
+    }
+    if (is_nan(val)) {
+        lpf_vals_.fill(0.0f);
+    }
+    temperature_ = lpf_vals_.back();
+}
+
+bool ThermistorCurrentLimiter::do_checks() {
+    if (enabled_ && temperature_ >= temp_limit_upper_ + 5) {
+        return false;
+    }
+    return true;
+}
+
+float ThermistorCurrentLimiter::get_current_limit(float base_current_lim) const {
+    if (!enabled_) {
+        return base_current_lim;
+    }
+
+    const float temp_margin = temp_limit_upper_ - temperature_;
+    const float derating_range = temp_limit_upper_ - temp_limit_lower_;
+    float thermal_current_lim = base_current_lim * (temp_margin / derating_range);
+    if (thermal_current_lim < 0.0f || is_nan(thermal_current_lim)) {
+        thermal_current_lim = 0.0f;
+    }
+
+    return std::min(thermal_current_lim, base_current_lim);
+}
+
+OnboardThermistorCurrentLimiter::OnboardThermistorCurrentLimiter(uint16_t adc_channel, const float* const coefficients, size_t num_coeffs) :
+    ThermistorCurrentLimiter(adc_channel,
+                             coefficients,
+                             num_coeffs,
+                             config_.temp_limit_lower,
+                             config_.temp_limit_upper,
+                             config_.enabled)
+{
+}
+
+OffboardThermistorCurrentLimiter::OffboardThermistorCurrentLimiter() :
+    ThermistorCurrentLimiter(UINT16_MAX,
+                             &config_.thermistor_poly_coeffs[0],
+                             num_coeffs_,
+                             config_.temp_limit_lower,
+                             config_.temp_limit_upper,
+                             config_.enabled)
+{
+    decode_pin();
+}
+
+bool OffboardThermistorCurrentLimiter::apply_config() {
+    config_.parent = this;
+    decode_pin();
+    return true;
+}
+
+void OffboardThermistorCurrentLimiter::decode_pin() {
+    adc_channel_ = channel_from_gpio(get_gpio(config_.gpio_pin));
+}