*

2025-05-13 01:34:53 +03:00
parent 427735e23d
commit 83f3f1c7d4
945 changed files with 633484 additions and 0 deletions
@@ -0,0 +1,91 @@
+
+#include "pwm_input.hpp"
+#include "odrive_main.h"
+
+void PwmInput::init() {
+    TIM_IC_InitTypeDef sConfigIC;
+    sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE;
+    sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
+    sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
+    sConfigIC.ICFilter = 15;
+
+    uint32_t channels[] = {TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4};
+
+    for (size_t i = 0; i < 4; ++i) {
+        if (!fibre::is_endpoint_ref_valid(odrv.config_.pwm_mappings[i].endpoint))
+            continue;
+        HAL_TIM_IC_ConfigChannel(htim_, &sConfigIC, channels[i]);
+        HAL_TIM_IC_Start_IT(htim_, channels[i]);
+    }
+}
+
+//TODO: These expressions have integer division by 1MHz, so it will be incorrect for clock speeds of not-integer MHz
+#define TIM_2_5_CLOCK_HZ        TIM_APB1_CLOCK_HZ
+#define PWM_MIN_HIGH_TIME          ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse
+#define PWM_MAX_HIGH_TIME          ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward
+#define PWM_MIN_LEGAL_HIGH_TIME    ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms
+#define PWM_MAX_LEGAL_HIGH_TIME    ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms
+#define PWM_INVERT_INPUT        false
+
+/**
+ * @param channel: A channel number in [0, 3]
+ */
+void handle_pulse(int channel, uint32_t high_time) {
+    if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME)
+        return;
+
+    if (high_time < PWM_MIN_HIGH_TIME)
+        high_time = PWM_MIN_HIGH_TIME;
+    if (high_time > PWM_MAX_HIGH_TIME)
+        high_time = PWM_MAX_HIGH_TIME;
+    float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME);
+    float value = odrv.config_.pwm_mappings[channel].min +
+                  (fraction * (odrv.config_.pwm_mappings[channel].max - odrv.config_.pwm_mappings[channel].min));
+
+    fibre::set_endpoint_from_float(odrv.config_.pwm_mappings[channel].endpoint, value);
+}
+
+/**
+ * @param channel: A channel number in [0, 3]
+ */
+void PwmInput::on_capture(int channel, uint32_t timestamp) {
+    static uint32_t last_timestamp[4] = { 0 };
+    static bool last_pin_state[4] = { false };
+    static bool last_sample_valid[4] = { false };
+
+    if (channel >= 4)
+        return;
+    Stm32Gpio gpio = get_gpio(gpios_[channel]);
+    if (!gpio)
+        return;
+    bool current_pin_state = gpio.read();
+
+    if (last_sample_valid[channel]
+        && (last_pin_state[channel] != PWM_INVERT_INPUT)
+        && (current_pin_state == PWM_INVERT_INPUT)) {
+        handle_pulse(channel, timestamp - last_timestamp[channel]);
+    }
+
+    last_timestamp[channel] = timestamp;
+    last_pin_state[channel] = current_pin_state;
+    last_sample_valid[channel] = true;
+}
+
+void PwmInput::on_capture() {
+    if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC1)) {
+        __HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC1);
+        on_capture(0, htim_->Instance->CCR1);
+    }
+    if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC2)) {
+        __HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC2);
+        on_capture(1, htim_->Instance->CCR2);
+    }
+    if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC3)) {
+        __HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC3);
+        on_capture(2, htim_->Instance->CCR3);
+    }
+    if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC4)) {
+        __HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC4);
+        on_capture(3, htim_->Instance->CCR4);
+    }
+}