mirror of https://github.com/rusefi/bldc.git
120 lines
3.4 KiB
C
120 lines
3.4 KiB
C
/*
|
|
Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se
|
|
Copyright 2022 Jakub Tomczak
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
The VESC firmware is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
The VESC firmware is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "encoder/enc_abi.h"
|
|
|
|
#include "ch.h"
|
|
#include "hal.h"
|
|
#include "stm32f4xx_conf.h"
|
|
#include "hw.h"
|
|
#include "mc_interface.h"
|
|
#include "utils.h"
|
|
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
bool enc_abi_init(ABI_config_t *cfg) {
|
|
EXTI_InitTypeDef EXTI_InitStructure;
|
|
|
|
memset(&cfg->state, 0, sizeof(ABI_state));
|
|
|
|
palSetPadMode(cfg->A_gpio, cfg->A_pin, PAL_MODE_ALTERNATE(cfg->tim_af));
|
|
palSetPadMode(cfg->B_gpio, cfg->B_pin, PAL_MODE_ALTERNATE(cfg->tim_af));
|
|
palSetPadMode(cfg->I_gpio, cfg->I_pin, PAL_MODE_INPUT_PULLUP);
|
|
|
|
// Enable timer clock
|
|
HW_ENC_TIM_CLK_EN();
|
|
|
|
// Enable SYSCFG clock
|
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
|
|
|
|
TIM_EncoderInterfaceConfig(cfg->timer,
|
|
TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
|
|
TIM_SetAutoreload(cfg->timer, cfg->counts - 1);
|
|
|
|
// Filter
|
|
cfg->timer->CCMR1 |= 6 << 12 | 6 << 4;
|
|
cfg->timer->CCMR2 |= 6 << 4;
|
|
|
|
TIM_Cmd(cfg->timer, ENABLE);
|
|
|
|
// Interrupt on index pulse
|
|
|
|
// Connect EXTI Line to pin
|
|
SYSCFG_EXTILineConfig(cfg->exti_portsrc, cfg->exti_pinsrc);
|
|
|
|
// Configure EXTI Line
|
|
EXTI_InitStructure.EXTI_Line = cfg->exti_line;
|
|
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
|
|
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
|
|
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
|
|
EXTI_Init(&EXTI_InitStructure);
|
|
|
|
// Enable and set EXTI Line Interrupt to the highest priority
|
|
nvicEnableVector(cfg->exti_ch, 0);
|
|
|
|
return true;
|
|
}
|
|
|
|
void enc_abi_deinit(ABI_config_t *cfg) {
|
|
nvicDisableVector(cfg->exti_ch);
|
|
TIM_DeInit(cfg->timer);
|
|
palSetPadMode(cfg->A_gpio, cfg->A_pin, PAL_MODE_INPUT_PULLUP);
|
|
palSetPadMode(cfg->B_gpio, cfg->B_pin, PAL_MODE_INPUT_PULLUP);
|
|
palSetPadMode(cfg->I_gpio, cfg->I_pin, PAL_MODE_INPUT_PULLUP);
|
|
cfg->state.last_enc_angle = 0.0;
|
|
}
|
|
|
|
float enc_abi_read_deg(ABI_config_t *cfg) {
|
|
cfg->state.last_enc_angle = ((float) cfg->timer->CNT * 360.0) / (float) cfg->counts;
|
|
return cfg->state.last_enc_angle;
|
|
}
|
|
|
|
void enc_abi_pin_isr(ABI_config_t *cfg) {
|
|
// Only reset if the pin is still high to avoid too short pulses, which
|
|
// most likely are noise.
|
|
__NOP();
|
|
__NOP();
|
|
__NOP();
|
|
__NOP();
|
|
if (palReadPad(cfg->I_gpio, cfg->I_pin)) {
|
|
const unsigned int cnt = cfg->timer->CNT;
|
|
const unsigned int lim = cfg->counts / 20;
|
|
|
|
if (cfg->state.index_found) {
|
|
// Some plausibility filtering.
|
|
if (cnt > (cfg->counts - lim) || cnt < lim) {
|
|
cfg->timer->CNT = 0;
|
|
cfg->state.bad_pulses = 0;
|
|
} else {
|
|
cfg->state.bad_pulses++;
|
|
|
|
if (cfg->state.bad_pulses > 5) {
|
|
cfg->state.index_found = 0;
|
|
}
|
|
}
|
|
} else {
|
|
cfg->timer->CNT = 0;
|
|
cfg->state.index_found = true;
|
|
cfg->state.bad_pulses = 0;
|
|
}
|
|
}
|
|
}
|