mirror of https://github.com/rusefi/bldc.git
480 lines
12 KiB
C
480 lines
12 KiB
C
|
|
#include "encoder.h"
|
|
|
|
#include "encoder/AS504x.h"
|
|
#include "encoder/MT6816.h"
|
|
#include "encoder/AD2S1205.h"
|
|
#include "encoder/ABI.h"
|
|
#include "encoder/ENC_SINCOS.h"
|
|
#include "encoder/TS5700N8501.h"
|
|
#include "encoder/encoder_datatype.h"
|
|
#include <math.h>
|
|
|
|
static bool encoder_is_uart_defined(void);
|
|
static bool encoder_is_spi_defined(void);
|
|
static bool encoder_is_incremental_defined(void);
|
|
|
|
#define ENCSPI_SAMPLE_RATE_HZ 20000
|
|
|
|
#define ENCODER_CONFIG_UNUSED {0}
|
|
#define ENCODER_VAR_UNINITIALIZED 0
|
|
#define ENCODER_STRUCT_UNDEFINED {0}
|
|
#define ENCODER_ABI_COUNTER_DEFAULT_VALUE 10000ul
|
|
|
|
#define SPI_BaudRatePrescaler_2 ((uint16_t)0x0000) // 42 MHz 21 MHZ
|
|
#define SPI_BaudRatePrescaler_4 ((uint16_t)0x0008) // 21 MHz 10.5 MHz
|
|
#define SPI_BaudRatePrescaler_8 ((uint16_t)0x0010) // 10.5 MHz 5.25 MHz
|
|
#define SPI_BaudRatePrescaler_16 ((uint16_t)0x0018) // 5.25 MHz 2.626 MHz
|
|
#define SPI_BaudRatePrescaler_32 ((uint16_t)0x0020) // 2.626 MHz 1.3125 MHz
|
|
#define SPI_BaudRatePrescaler_64 ((uint16_t)0x0028) // 1.3125 MHz 656.25 KHz
|
|
#define SPI_BaudRatePrescaler_128 ((uint16_t)0x0030) // 656.25 KHz 328.125 KHz
|
|
#define SPI_BaudRatePrescaler_256 ((uint16_t)0x0038) // 328.125 KHz 164.06 KHz
|
|
#define SPI_DATASIZE_16BIT SPI_CR1_DFF
|
|
|
|
#define CH_MUTEX_INIT_ZERO {{NULL, NULL}, NULL, NULL}
|
|
|
|
ENCSPI_config_t encoder_conf_ENCSPI =
|
|
{
|
|
ENCODER_VAR_UNINITIALIZED,
|
|
ENCSPI_SAMPLE_RATE_HZ,
|
|
{/*BB_SPI*/
|
|
/*NSS*/HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 ,
|
|
/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 ,
|
|
/*MOSI*/HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI ,
|
|
/*MISO*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 ,
|
|
ENCODER_VAR_UNINITIALIZED,
|
|
ENCODER_VAR_UNINITIALIZED,
|
|
CH_MUTEX_INIT_ZERO
|
|
},
|
|
#ifdef HW_SPI_DEV
|
|
{//HARDWARE SPI CONFIG
|
|
NULL, HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, SPI_BaudRatePrescaler_4 | SPI_CR1_CPOL | SPI_CR1_CPHA
|
|
| SPI_DATASIZE_16BIT
|
|
}
|
|
#endif
|
|
};
|
|
|
|
ABI_config_t encoder_conf_ABI =
|
|
{
|
|
ENCODER_VAR_UNINITIALIZED,
|
|
ENCODER_ABI_COUNTER_DEFAULT_VALUE,
|
|
/*INCREMENTAL PROTOCOL PINOUT*/
|
|
/*A*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 ,
|
|
/*B*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2
|
|
};
|
|
|
|
ENCSINCOS_config_t encoder_conf_ENCSINCOS = ENCODER_STRUCT_UNDEFINED;
|
|
|
|
TS5700N8501_config_t encoder_conf_TS5700N8501 =
|
|
{
|
|
ENCODER_VAR_UNINITIALIZED,
|
|
/*UART PINOUT*/
|
|
HW_UART_TX_PORT, HW_UART_TX_PIN,
|
|
HW_UART_RX_PORT, HW_UART_RX_PIN,
|
|
#ifdef HW_ADC_EXT_GPIO
|
|
HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN
|
|
#else
|
|
ENCODER_VAR_UNINITIALIZED, ENCODER_VAR_UNINITIALIZED
|
|
#endif
|
|
,
|
|
{/*UART CONFIG*/
|
|
2500000,
|
|
0,
|
|
USART_CR2_LINEN,
|
|
0
|
|
}
|
|
};
|
|
|
|
static encoder_type_t encoder_type_now = ENCODER_TYPE_NONE;
|
|
static bool index_found = false;
|
|
|
|
void encoder_deinit(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
AS504x_deinit();
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
MT6816_deinit();
|
|
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
|
AD2S1205_deinit();
|
|
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
|
|
ABI_deinit();
|
|
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
ENC_SINCOS_deinit();
|
|
} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
TS5700N8501_deinit();
|
|
}
|
|
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
}
|
|
|
|
encoder_ret_t encoder_init(encoder_type_t encoder_type) {
|
|
|
|
if (encoder_type_now != ENCODER_TYPE_NONE) {
|
|
return ENCODER_ERROR;
|
|
}
|
|
|
|
if (encoder_type == ENCODER_TYPE_AS504x) {
|
|
encoder_ret_t encoder_ret;
|
|
|
|
if (encoder_is_spi_defined() == false) {
|
|
return ENCODER_ERROR;
|
|
}
|
|
|
|
encoder_conf_ENCSPI.is_init = 0;
|
|
|
|
encoder_ret = AS504x_init(&encoder_conf_ENCSPI);
|
|
|
|
if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSPI.is_init) {
|
|
index_found = false;
|
|
return ENCODER_ERROR;
|
|
}
|
|
encoder_type_now = ENCODER_TYPE_AS504x;
|
|
index_found = true;
|
|
return ENCODER_OK;
|
|
} else if (encoder_type == ENCODER_TYPE_MT6816) {
|
|
encoder_ret_t encoder_ret;
|
|
|
|
if (encoder_is_spi_defined() == false) {
|
|
return ENCODER_ERROR;
|
|
}
|
|
|
|
encoder_conf_ENCSPI.is_init = 0;
|
|
|
|
encoder_ret = MT6816_init(&encoder_conf_ENCSPI);
|
|
|
|
if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSPI.is_init) {
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
index_found = false;
|
|
return ENCODER_ERROR;
|
|
}
|
|
encoder_type_now = ENCODER_TYPE_MT6816;
|
|
index_found = true;
|
|
return ENCODER_OK;
|
|
} else if (encoder_type == ENCODER_TYPE_AD2S1205_SPI) {
|
|
encoder_ret_t encoder_ret;
|
|
|
|
if (encoder_is_spi_defined() == false) {
|
|
return ENCODER_ERROR;
|
|
}
|
|
|
|
encoder_conf_ENCSPI.is_init = 0;
|
|
|
|
encoder_ret = AD2S1205_init(&encoder_conf_ENCSPI);
|
|
|
|
if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSPI.is_init) {
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
index_found = false;
|
|
return ENCODER_ERROR;
|
|
}
|
|
encoder_type_now = ENCODER_TYPE_AD2S1205_SPI;
|
|
index_found = true;
|
|
return ENCODER_OK;
|
|
} else if (encoder_type == ENCODER_TYPE_ABI) {
|
|
encoder_ret_t encoder_ret;
|
|
|
|
if (encoder_is_incremental_defined() == false) {
|
|
return ENCODER_ERROR;
|
|
}
|
|
|
|
encoder_conf_ABI.is_init = 0;
|
|
|
|
encoder_ret = ABI_init(&encoder_conf_ABI);
|
|
|
|
if (ENCODER_OK != encoder_ret || !encoder_conf_ABI.is_init) {
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
index_found = false;
|
|
return ENCODER_ERROR;
|
|
}
|
|
encoder_type_now = ENCODER_TYPE_ABI;
|
|
index_found = true;
|
|
return ENCODER_OK;
|
|
} else if (encoder_type == ENCODER_TYPE_SINCOS) {
|
|
encoder_ret_t encoder_ret;
|
|
|
|
encoder_conf_ENCSINCOS.is_init = 0;
|
|
|
|
encoder_ret = ENC_SINCOS_init(&encoder_conf_ENCSINCOS);
|
|
|
|
if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSINCOS.is_init) {
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
index_found = false;
|
|
return ENCODER_ERROR;
|
|
}
|
|
encoder_type_now = ENCODER_TYPE_SINCOS;
|
|
index_found = true;
|
|
return ENCODER_OK;
|
|
} else if (encoder_type == ENCODER_TYPE_TS5700N8501) {
|
|
encoder_ret_t encoder_ret;
|
|
|
|
if (encoder_is_uart_defined() == false) {
|
|
return ENCODER_ERROR;
|
|
}
|
|
|
|
encoder_conf_TS5700N8501.is_init = 0;
|
|
|
|
encoder_ret = TS5700N8501_init(&encoder_conf_TS5700N8501);
|
|
|
|
if (ENCODER_OK != encoder_ret || !encoder_conf_TS5700N8501.is_init) {
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
index_found = false;
|
|
return ENCODER_ERROR;
|
|
}
|
|
encoder_type_now = ENCODER_TYPE_TS5700N8501;
|
|
index_found = true;
|
|
return ENCODER_OK;
|
|
} else {
|
|
encoder_type_now = ENCODER_TYPE_NONE;
|
|
index_found = false;
|
|
}
|
|
return ENCODER_NONE;
|
|
}
|
|
|
|
float encoder_read_deg(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
return AS504x_read_deg();
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
return MT6816_read_deg();
|
|
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
|
return AD2S1205_read_deg();
|
|
} else if (encoder_type_now == ENCODER_TYPE_ABI) {
|
|
return ABI_read_deg();
|
|
} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
return ENC_SINCOS_read_deg();
|
|
} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
return TS5700N8501_read_deg();
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
float encoder_read_deg_multiturn(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
encoder_get_abm();
|
|
float ts_mt = (float) encoder_get_abm();
|
|
if (fabsf(ts_mt) > 5000.0) {
|
|
ts_mt = 0;
|
|
encoder_reset_multiturn();
|
|
}
|
|
|
|
ts_mt += 5000;
|
|
|
|
return encoder_read_deg() / 10000.0 + (360 * ts_mt) / 10000.0;
|
|
} else {
|
|
return encoder_read_deg();
|
|
}
|
|
}
|
|
|
|
encoder_type_t encoder_is_configured(void) {
|
|
return encoder_type_now;
|
|
}
|
|
|
|
bool encoder_index_found(void) {
|
|
return index_found;
|
|
}
|
|
|
|
float encoder_spi_get_error_rate(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
return AS504x_spi_get_error_rate();
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
uint32_t encoder_spi_get_error_cnt(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
return AS504x_spi_get_error_cnt();
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
return MT6816_spi_get_error_cnt();
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
uint32_t encoder_get_no_magnet_error_cnt(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
return MT6816_get_no_magnet_error_cnt();
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
float encoder_get_no_magnet_error_rate(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
return MT6816_get_no_magnet_error_rate();
|
|
}
|
|
return 0.0;
|
|
}
|
|
float encoder_resolver_loss_of_tracking_error_rate(void) {
|
|
return AD2S1205_resolver_loss_of_tracking_error_rate();
|
|
}
|
|
|
|
float encoder_resolver_degradation_of_signal_error_rate(void) {
|
|
return AD2S1205_resolver_degradation_of_signal_error_rate();
|
|
}
|
|
|
|
float encoder_resolver_loss_of_signal_error_rate(void) {
|
|
return AD2S1205_resolver_loss_of_signal_error_rate();
|
|
}
|
|
|
|
uint32_t encoder_resolver_loss_of_tracking_error_cnt(void) {
|
|
return AD2S1205_resolver_loss_of_tracking_error_cnt();
|
|
}
|
|
|
|
uint32_t encoder_resolver_degradation_of_signal_error_cnt(void) {
|
|
return AD2S1205_resolver_degradation_of_signal_error_cnt();
|
|
}
|
|
|
|
uint32_t encoder_resolver_loss_of_signal_error_cnt(void) {
|
|
return AD2S1205_resolver_loss_of_signal_error_cnt();
|
|
}
|
|
|
|
// ABI
|
|
void encoder_set_counts(uint32_t counts) {
|
|
if (encoder_type_now == ENCODER_TYPE_ABI || encoder_type_now == ENCODER_TYPE_NONE) {
|
|
encoder_conf_ABI.counts = counts;
|
|
TIM_SetAutoreload(HW_ENC_TIM, encoder_conf_ABI.counts - 1);
|
|
index_found = false;
|
|
}
|
|
}
|
|
|
|
uint32_t encoder_spi_get_val(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
return AS504x_spi_get_val();
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
return MT6816_spi_get_val();
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void encoder_reset(void) {
|
|
// Only reset if the pin is still high to avoid too short pulses, which
|
|
// most likely are noise.
|
|
__NOP();
|
|
__NOP();
|
|
__NOP();
|
|
__NOP();
|
|
if (palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)) {
|
|
const unsigned int cnt = HW_ENC_TIM->CNT;
|
|
static int bad_pulses = 0;
|
|
const unsigned int lim = encoder_conf_ABI.counts / 20;
|
|
|
|
if (encoder_index_found()) {
|
|
// Some plausibility filtering.
|
|
if (cnt > (encoder_conf_ABI.counts - lim) || cnt < lim) {
|
|
HW_ENC_TIM->CNT = 0;
|
|
bad_pulses = 0;
|
|
} else {
|
|
bad_pulses++;
|
|
|
|
if (bad_pulses > 5) {
|
|
index_found = 0;
|
|
}
|
|
}
|
|
} else {
|
|
HW_ENC_TIM->CNT = 0;
|
|
index_found = true;
|
|
bad_pulses = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
AS504x_diag encoder_get_diag(void) {
|
|
AS504x_diag diag = { 0 };
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
diag = AS504x_get_diag();
|
|
}
|
|
return diag;
|
|
}
|
|
|
|
uint8_t* encoder_get_raw_status(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
return TS5700N8501_get_raw_status();
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
int16_t encoder_get_abm(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
return TS5700N8501_get_abm();
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void encoder_reset_errors(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
return TS5700N8501_reset_errors();
|
|
}
|
|
}
|
|
|
|
void encoder_reset_multiturn(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
|
|
return TS5700N8501_reset_multiturn();
|
|
}
|
|
}
|
|
|
|
// SINCOS TODO labels
|
|
uint32_t encoder_get_signal_below_min_error_cnt(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
return ENC_SINCOS_get_signal_below_min_error_cnt();
|
|
}
|
|
return 0;
|
|
}
|
|
uint32_t encoder_get_signal_above_max_error_cnt(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
return ENC_SINCOS_get_signal_above_max_error_cnt();
|
|
}
|
|
return 0;
|
|
}
|
|
float encoder_get_signal_below_min_error_rate(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
return ENC_SINCOS_get_signal_below_min_error_rate();
|
|
}
|
|
return 0.0;
|
|
}
|
|
float encoder_get_signal_above_max_error_rate(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_SINCOS) {
|
|
return ENC_SINCOS_get_signal_above_max_error_rate();
|
|
}
|
|
return 0.0;
|
|
}
|
|
|
|
void encoder_sincos_conf_set(float sin_gain, float sin_offset,
|
|
float cos_gain, float cos_offset, float sincos_filter_constant) {
|
|
encoder_conf_ENCSINCOS.s_gain = sin_gain;
|
|
encoder_conf_ENCSINCOS.s_offset = sin_offset;
|
|
encoder_conf_ENCSINCOS.c_gain = cos_gain;
|
|
encoder_conf_ENCSINCOS.c_offset = cos_offset;
|
|
encoder_conf_ENCSINCOS.filter_constant = sincos_filter_constant;
|
|
}
|
|
|
|
void encoder_tim_isr(void) {
|
|
if (encoder_type_now == ENCODER_TYPE_AS504x) {
|
|
AS504x_routine();
|
|
} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
|
|
MT6816_routine();
|
|
} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
|
|
AD2S1205_routine();
|
|
}
|
|
}
|
|
|
|
static bool encoder_is_uart_defined(void) {
|
|
if (!encoder_conf_TS5700N8501.RX_gpio
|
|
|| !encoder_conf_TS5700N8501.TX_gpio) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool encoder_is_spi_defined(void) {
|
|
if (!encoder_conf_ENCSPI.sw_spi.miso_gpio
|
|
|| !encoder_conf_ENCSPI.sw_spi.nss_gpio
|
|
|| !encoder_conf_ENCSPI.sw_spi.sck_gpio) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
static bool encoder_is_incremental_defined(void) {
|
|
if (!encoder_conf_ABI.A_gpio
|
|
|| !encoder_conf_ABI.B_gpio) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|