mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-18 09:29:03 +08:00
92 lines
3.4 KiB
C++
92 lines
3.4 KiB
C++
|
|
#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);
|
|
}
|
|
}
|