2022-02-19 06:37:12 -08:00
|
|
|
/*
|
|
|
|
Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se
|
|
|
|
Copyright 2022 Marcos Chaparro mchaparro@powerdesigns.ca
|
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
|
2021-12-14 07:40:40 -08:00
|
|
|
#include "encoder/MT6816.h"
|
|
|
|
|
|
|
|
#include "ch.h"
|
|
|
|
#include "hal.h"
|
|
|
|
#include "stm32f4xx_conf.h"
|
|
|
|
#include "hw.h"
|
|
|
|
#include "mc_interface.h"
|
|
|
|
#include "utils.h"
|
2021-12-26 09:44:33 -08:00
|
|
|
#include "spi_bb.h"
|
2021-12-14 07:40:40 -08:00
|
|
|
#include <math.h>
|
|
|
|
|
|
|
|
#define MT6816_NO_MAGNET_ERROR_MASK 0x0002
|
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
static MT6816_config_t MT6816_config_now = { 0 };
|
2021-12-14 07:40:40 -08:00
|
|
|
|
|
|
|
static float spi_error_rate = 0.0;
|
|
|
|
static float encoder_no_magnet_error_rate = 0.0;
|
|
|
|
static float encoder_no_magnet_error_cnt = 0.0;
|
|
|
|
static float last_enc_angle = 0.0;
|
|
|
|
static uint32_t spi_error_cnt = 0;
|
|
|
|
static uint32_t spi_val = 0;
|
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
void MT6816_deinit(void) {
|
2021-12-14 07:40:40 -08:00
|
|
|
|
|
|
|
nvicDisableVector(HW_ENC_EXTI_CH);
|
|
|
|
nvicDisableVector(HW_ENC_TIM_ISR_CH);
|
|
|
|
|
|
|
|
TIM_DeInit(HW_ENC_TIM);
|
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.miso_gpio,
|
|
|
|
MT6816_config_now.sw_spi.miso_pin, PAL_MODE_INPUT_PULLUP);
|
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.sck_gpio,
|
|
|
|
MT6816_config_now.sw_spi.sck_pin, PAL_MODE_INPUT_PULLUP);
|
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.nss_gpio,
|
|
|
|
MT6816_config_now.sw_spi.nss_pin, PAL_MODE_INPUT_PULLUP);
|
2021-12-14 07:40:40 -08:00
|
|
|
|
|
|
|
#if (MT6816_USE_HW_SPI_PINS)
|
2022-01-20 12:23:37 -08:00
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.mosi_gpio, MT6816_config_now.sw_spi.mosi_pin, PAL_MODE_INPUT_PULLUP);
|
2021-12-14 07:40:40 -08:00
|
|
|
#endif
|
|
|
|
|
|
|
|
#ifdef HW_SPI_DEV
|
|
|
|
spiStop(&HW_SPI_DEV);
|
|
|
|
#endif
|
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.miso_gpio,
|
|
|
|
MT6816_config_now.sw_spi.miso_pin, PAL_MODE_INPUT_PULLUP);
|
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.sck_gpio,
|
|
|
|
MT6816_config_now.sw_spi.sck_pin, PAL_MODE_INPUT_PULLUP);
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
MT6816_config_now.is_init = 0;
|
2021-12-14 07:40:40 -08:00
|
|
|
last_enc_angle = 0.0;
|
|
|
|
spi_error_rate = 0.0;
|
|
|
|
}
|
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
encoder_ret_t MT6816_init(MT6816_config_t *mt6816_config) {
|
2021-12-14 07:40:40 -08:00
|
|
|
#ifdef HW_SPI_DEV
|
2021-12-15 05:17:43 -08:00
|
|
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
MT6816_config_now = *mt6816_config;
|
2021-12-26 09:44:33 -08:00
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.sck_gpio,
|
|
|
|
MT6816_config_now.sw_spi.sck_pin,
|
2021-12-15 05:17:43 -08:00
|
|
|
PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
|
2022-01-20 12:23:37 -08:00
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.miso_gpio,
|
|
|
|
MT6816_config_now.sw_spi.miso_pin,
|
2021-12-15 05:17:43 -08:00
|
|
|
PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
|
2021-12-26 09:44:33 -08:00
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
spi_bb_nss_init(&(MT6816_config_now.sw_spi));
|
2021-12-14 07:40:40 -08:00
|
|
|
|
|
|
|
#if (MT6816_USE_HW_SPI_PINS)
|
2022-01-24 05:18:18 -08:00
|
|
|
palSetPadMode(MT6816_config_now.sw_spi.mosi_gpio, MT6816_config_now.sw_spi.mosi_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
|
2021-12-14 07:40:40 -08:00
|
|
|
#endif
|
|
|
|
|
|
|
|
//Start driver with MT6816 SPI settings
|
2021-12-17 07:18:07 -08:00
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
spiStart(&HW_SPI_DEV, &(MT6816_config_now.hw_spi_cfg));
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
// Enable timer clock
|
|
|
|
HW_ENC_TIM_CLK_EN();
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
// Time Base configuration
|
|
|
|
TIM_TimeBaseStructure.TIM_Prescaler = 0;
|
|
|
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
|
|
|
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2
|
|
|
|
/ mt6816_config->refresh_rate_hz) - 1);
|
|
|
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
|
|
|
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
|
|
|
TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
// Enable overflow interrupt
|
|
|
|
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
// Enable timer
|
|
|
|
TIM_Cmd(HW_ENC_TIM, ENABLE);
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
|
|
|
|
spi_error_rate = 0.0;
|
|
|
|
encoder_no_magnet_error_rate = 0.0;
|
2021-12-14 07:40:40 -08:00
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
MT6816_config_now.is_init = 1;
|
2021-12-15 05:17:43 -08:00
|
|
|
mt6816_config->is_init = 1;
|
2022-02-19 10:40:42 -08:00
|
|
|
|
2022-01-09 08:10:40 -08:00
|
|
|
return ENCODER_OK;
|
2022-02-19 10:40:42 -08:00
|
|
|
#else
|
|
|
|
(void)mt6816_config;
|
|
|
|
return ENCODER_ERROR;
|
|
|
|
#endif
|
2021-12-14 07:40:40 -08:00
|
|
|
}
|
2021-12-20 05:18:51 -08:00
|
|
|
|
|
|
|
float MT6816_read_deg(void) {
|
|
|
|
return last_enc_angle;
|
|
|
|
}
|
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
void MT6816_routine(void) {
|
2022-02-19 10:40:42 -08:00
|
|
|
#ifdef HW_SPI_DEV
|
2021-12-15 05:17:43 -08:00
|
|
|
uint16_t pos;
|
|
|
|
uint16_t reg_data_03;
|
|
|
|
uint16_t reg_data_04;
|
|
|
|
uint16_t reg_addr_03 = 0x8300;
|
|
|
|
uint16_t reg_addr_04 = 0x8400;
|
|
|
|
|
2022-01-20 12:23:37 -08:00
|
|
|
spi_bb_begin(&(MT6816_config_now.sw_spi));
|
2021-12-15 05:17:43 -08:00
|
|
|
reg_data_03 = spiPolledExchange(&HW_SPI_DEV, reg_addr_03);
|
2022-01-20 12:23:37 -08:00
|
|
|
spi_bb_end(&(MT6816_config_now.sw_spi));
|
2021-12-26 09:44:33 -08:00
|
|
|
spi_bb_delay();
|
2022-01-20 12:23:37 -08:00
|
|
|
spi_bb_begin(&(MT6816_config_now.sw_spi));
|
2021-12-15 05:17:43 -08:00
|
|
|
reg_data_04 = spiPolledExchange(&HW_SPI_DEV, reg_addr_04);
|
2022-01-20 12:23:37 -08:00
|
|
|
spi_bb_end(&(MT6816_config_now.sw_spi));
|
2021-12-15 05:17:43 -08:00
|
|
|
|
|
|
|
pos = (reg_data_03 << 8) | reg_data_04;
|
|
|
|
spi_val = pos;
|
|
|
|
|
2021-12-26 09:44:33 -08:00
|
|
|
if (spi_bb_check_parity(pos)) {
|
2021-12-15 05:17:43 -08:00
|
|
|
if (pos & MT6816_NO_MAGNET_ERROR_MASK) {
|
|
|
|
++encoder_no_magnet_error_cnt;
|
|
|
|
UTILS_LP_FAST(encoder_no_magnet_error_rate, 1.0,
|
2022-01-20 12:23:37 -08:00
|
|
|
1. / MT6816_config_now.refresh_rate_hz);
|
2021-12-15 05:17:43 -08:00
|
|
|
} else {
|
|
|
|
pos = pos >> 2;
|
|
|
|
last_enc_angle = ((float) pos * 360.0) / 16384.0;
|
|
|
|
UTILS_LP_FAST(spi_error_rate, 0.0,
|
2022-01-20 12:23:37 -08:00
|
|
|
1. / MT6816_config_now.refresh_rate_hz);
|
2021-12-15 05:17:43 -08:00
|
|
|
UTILS_LP_FAST(encoder_no_magnet_error_rate, 0.0,
|
2022-01-20 12:23:37 -08:00
|
|
|
1. / MT6816_config_now.refresh_rate_hz);
|
2021-12-15 05:17:43 -08:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
++spi_error_cnt;
|
|
|
|
UTILS_LP_FAST(spi_error_rate, 1.0,
|
2022-01-20 12:23:37 -08:00
|
|
|
1. / MT6816_config_now.refresh_rate_hz);
|
2021-12-15 05:17:43 -08:00
|
|
|
}
|
2022-02-19 10:40:42 -08:00
|
|
|
#endif
|
2021-12-14 07:40:40 -08:00
|
|
|
}
|
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
uint32_t MT6816_spi_get_val(void) {
|
2021-12-14 07:40:40 -08:00
|
|
|
return spi_val;
|
|
|
|
}
|
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
uint32_t MT6816_spi_get_error_cnt(void) {
|
2021-12-14 07:40:40 -08:00
|
|
|
return spi_error_cnt;
|
|
|
|
}
|
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
uint32_t MT6816_get_no_magnet_error_cnt(void) {
|
|
|
|
return encoder_no_magnet_error_cnt;
|
2021-12-14 07:40:40 -08:00
|
|
|
}
|
|
|
|
|
2021-12-15 05:17:43 -08:00
|
|
|
uint32_t MT6816_get_no_magnet_error_rate(void) {
|
|
|
|
return encoder_no_magnet_error_rate;
|
2021-12-14 07:40:40 -08:00
|
|
|
}
|