rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

code formatting fixed

This commit is contained in:
Jakub Tomczak 2021-12-15 14:17:43 +01:00
parent 612f7a5090
commit d0acc03f86
13 changed files with 550 additions and 538 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
encoder/ABI.c
View File

@ -1,5 +1,3 @@
#include "encoder/ABI.h"
#include "ch.h"
@ -11,40 +9,48 @@
#include <math.h>
static float last_enc_angle = 0.0;
static ABI_config_t abi_config_now = {0};
static ABI_config_t abi_config_now = { 0 };
static uint32_t enc_counts = 10000;
void ABI_deinit(void){
void ABI_deinit(void) {
nvicDisableVector(HW_ENC_EXTI_CH);
TIM_DeInit(HW_ENC_TIM);
palSetPadMode(abi_config_now.incremental_config.gpio_A.port, abi_config_now.incremental_config.gpio_A.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(abi_config_now.incremental_config.gpio_B.port, abi_config_now.incremental_config.gpio_B.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(abi_config_now.incremental_config.gpio_A.port,
abi_config_now.incremental_config.gpio_A.pin,
PAL_MODE_INPUT_PULLUP);
palSetPadMode(abi_config_now.incremental_config.gpio_B.port,
abi_config_now.incremental_config.gpio_B.pin,
PAL_MODE_INPUT_PULLUP);
last_enc_angle = 0.0;
}
encoders_ret_t ABI_init(ABI_config_t *abi_config){
encoders_ret_t ABI_init(ABI_config_t *abi_config) {
EXTI_InitTypeDef EXTI_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
// Initialize variables
enc_counts = abi_config->counts;
palSetPadMode(abi_config->incremental_config.gpio_A.port, abi_config->incremental_config.gpio_A.pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
palSetPadMode(abi_config->incremental_config.gpio_B.port, abi_config->incremental_config.gpio_B.pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
palSetPadMode(abi_config->incremental_config.gpio_A.port,
abi_config->incremental_config.gpio_A.pin,
PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
palSetPadMode(abi_config->incremental_config.gpio_B.port,
abi_config->incremental_config.gpio_B.pin,
PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
// palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
// Enable timer clock
// Enable timer clock
HW_ENC_TIM_CLK_EN();
// Enable SYSCFG clock
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
TIM_EncoderInterfaceConfig (HW_ENC_TIM, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising,
TIM_ICPolarity_Rising);
TIM_EncoderInterfaceConfig(HW_ENC_TIM, TIM_EncoderMode_TI12,
TIM_ICPolarity_Rising,
TIM_ICPolarity_Rising);
TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1);
// Filter
@ -73,8 +79,8 @@ encoders_ret_t ABI_init(ABI_config_t *abi_config){
return ENCODERS_OK;
}
float ABI_read_deg(void){
last_enc_angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
float ABI_read_deg(void) {
last_enc_angle = ((float) HW_ENC_TIM->CNT * 360.0) / (float) enc_counts;
return last_enc_angle;
}

2
encoder/ABI.h
View File

@ -1,5 +1,4 @@
#ifndef ENCODER_ABI_H_
#define ENCODER_ABI_H_
@ -11,5 +10,4 @@ encoders_ret_t ABI_init(ABI_config_t *abi_config);
float ABI_read_deg(void);
#endif /* ENCODER_ABI_H_ */

169
encoder/AD2S1205.c
View File

@ -1,6 +1,4 @@
#include "encoder/AD2S1205.h"
#include "ch.h"
@ -11,7 +9,7 @@
#include "utils.h"
#include <math.h>
static AD2S1205_config_t AD2S1205_config_now = {0};
static AD2S1205_config_t AD2S1205_config_now = { 0 };
static uint16_t spi_val = 0;
static float resolver_loss_of_tracking_error_rate = 0.0;
@ -31,23 +29,25 @@ static void spi_delay(void);
static void spi_AS5047_cs_delay(void);
static bool spi_check_parity(uint16_t x);
void AD2S1205_deinit(void)
{
nvicDisableVector(HW_ENC_EXTI_CH);
nvicDisableVector(HW_ENC_TIM_ISR_CH);
void AD2S1205_deinit(void) {
nvicDisableVector(HW_ENC_EXTI_CH);
nvicDisableVector(HW_ENC_TIM_ISR_CH);
TIM_DeInit(HW_ENC_TIM);
TIM_DeInit(HW_ENC_TIM);
palSetPadMode(AD2S1205_config_now.spi_config.gpio_miso.port, AD2S1205_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AD2S1205_config_now.spi_config.gpio_sck.port, AD2S1205_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AD2S1205_config_now.spi_config.gpio_nss.port, AD2S1205_config_now.spi_config.gpio_nss.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AD2S1205_config_now.spi_config.gpio_miso.port,
AD2S1205_config_now.spi_config.gpio_miso.pin,
PAL_MODE_INPUT_PULLUP);
palSetPadMode(AD2S1205_config_now.spi_config.gpio_sck.port,
AD2S1205_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AD2S1205_config_now.spi_config.gpio_nss.port,
AD2S1205_config_now.spi_config.gpio_nss.pin, PAL_MODE_INPUT_PULLUP);
#ifdef HW_SPI_DEV
spiStop(&HW_SPI_DEV);
#endif
#ifdef HW_SPI_DEV
spiStop(&HW_SPI_DEV);
#endif
// TODO: (TO BE TESTED!!) DEINITIALIZE ALSO SAMPLE AND RDVEL
// TODO: (TO BE TESTED!!) DEINITIALIZE ALSO SAMPLE AND RDVEL
#if defined(AD2S1205_SAMPLE_GPIO)
palSetPadMode(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN, PAL_MODE_INPUT_PULLUP); // Prepare for a falling edge SAMPLE assertion
#endif
@ -57,10 +57,9 @@ void AD2S1205_deinit(void)
}
encoders_ret_t AD2S1205_init(AD2S1205_config_t* AD2S1205_config)
{
encoders_ret_t AD2S1205_init(AD2S1205_config_t *AD2S1205_config) {
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
encoders_spi_config_t AD2S1205_spi_config = AD2S1205_config->spi_config;
resolver_loss_of_tracking_error_rate = 0.0;
@ -69,9 +68,14 @@ encoders_ret_t AD2S1205_init(AD2S1205_config_t* AD2S1205_config)
resolver_loss_of_tracking_error_cnt = 0;
resolver_loss_of_signal_error_cnt = 0;
palSetPadMode(AD2S1205_spi_config.gpio_miso.port, AD2S1205_spi_config.gpio_miso.pin, PAL_MODE_INPUT);
palSetPadMode(AD2S1205_spi_config.gpio_sck.port, AD2S1205_spi_config.gpio_sck.pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(AD2S1205_spi_config.gpio_nss.port, AD2S1205_spi_config.gpio_nss.pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(AD2S1205_spi_config.gpio_miso.port,
AD2S1205_spi_config.gpio_miso.pin, PAL_MODE_INPUT);
palSetPadMode(AD2S1205_spi_config.gpio_sck.port,
AD2S1205_spi_config.gpio_sck.pin,
PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(AD2S1205_spi_config.gpio_nss.port,
AD2S1205_spi_config.gpio_nss.pin,
PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
// Set MOSI to 1
#if AD2S1205_USE_HW_SPI_PINS
@ -97,7 +101,8 @@ encoders_ret_t AD2S1205_init(AD2S1205_config_t* AD2S1205_config)
// Time Base configuration
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AD2S1205_config->refresh_rate_hz) - 1);
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2
/ AD2S1205_config->refresh_rate_hz) - 1);
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
@ -116,15 +121,15 @@ encoders_ret_t AD2S1205_init(AD2S1205_config_t* AD2S1205_config)
return ENCODERS_OK;
}
void AD2S1205_routine(void)
{
void AD2S1205_routine(void) {
uint16_t pos;
// SAMPLE signal should have been be asserted in sync with ADC sampling
#ifdef AD2S1205_RDVEL_GPIO
palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN); // Always read position
#endif
palSetPad(AD2S1205_config_now.spi_config.gpio_sck.port, AD2S1205_config_now.spi_config.gpio_sck.pin);
palSetPad(AD2S1205_config_now.spi_config.gpio_sck.port,
AD2S1205_config_now.spi_config.gpio_sck.pin);
spi_delay();
spi_begin(); // CS uses the same mcu pin as AS5047
spi_delay();
@ -136,61 +141,67 @@ void AD2S1205_routine(void)
uint16_t RDVEL = pos & 0x0008; // 1 means a position read
if((RDVEL != 0)){
if ((RDVEL != 0)) {
bool DOS = ((pos & 0x04) == 0);
bool LOT = ((pos & 0x02) == 0);
bool LOS = DOS && LOT;
bool parity_error = spi_check_parity(pos); //16 bit frame has odd parity
bool parity_error = spi_check_parity(pos);//16 bit frame has odd parity
bool angle_is_correct = true;
if(LOS) {
if (LOS) {
LOT = DOS = 0;
}
if(!parity_error) {
UTILS_LP_FAST(spi_error_rate, 0.0, 1./AD2S1205_config_now.refresh_rate_hz);
if (!parity_error) {
UTILS_LP_FAST(spi_error_rate, 0.0,
1. / AD2S1205_config_now.refresh_rate_hz);
} else {
angle_is_correct = false;
++spi_error_cnt;
UTILS_LP_FAST(spi_error_rate, 1.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(spi_error_rate, 1.0,
1. / AD2S1205_config_now.refresh_rate_hz);
}
pos &= 0xFFF0;
pos = pos >> 4;
pos &= 0x0FFF;
if(LOT) {
if (LOT) {
angle_is_correct = false;
++resolver_loss_of_tracking_error_cnt;
UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 1.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 1.0,
1. / AD2S1205_config_now.refresh_rate_hz);
} else {
UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 0.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 0.0,
1. / AD2S1205_config_now.refresh_rate_hz);
}
if(DOS) {
if (DOS) {
angle_is_correct = false;
++resolver_degradation_of_signal_error_cnt;
UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 1.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 1.0,
1. / AD2S1205_config_now.refresh_rate_hz);
} else {
UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 0.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 0.0,
1. / AD2S1205_config_now.refresh_rate_hz);
}
if(LOS) {
if (LOS) {
angle_is_correct = false;
++resolver_loss_of_signal_error_cnt;
UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 1.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 1.0,
1. / AD2S1205_config_now.refresh_rate_hz);
} else {
UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 0.0, 1./AD2S1205_config_now.refresh_rate_hz);
UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 0.0,
1. / AD2S1205_config_now.refresh_rate_hz);
}
if(angle_is_correct)
{
last_enc_angle = ((float)pos * 360.0) / 4096.0;
if (angle_is_correct) {
last_enc_angle = ((float) pos * 360.0) / 4096.0;
}
}
}
float AD2S1205_resolver_loss_of_tracking_error_rate(void) {
@ -217,13 +228,11 @@ uint32_t AD2S1205_resolver_loss_of_signal_error_cnt(void) {
return resolver_loss_of_signal_error_cnt;
}
uint32_t AD2S1205_spi_get_error_cnt(void)
{
uint32_t AD2S1205_spi_get_error_cnt(void) {
return spi_error_cnt;
}
float AD2S1205_read_deg(void)
{
float AD2S1205_read_deg(void) {
return last_enc_angle;
}
@ -233,7 +242,7 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
const encoders_gpio_t gpio_mosi = AD2S1205_config_now.spi_config.gpio_mosi;
#endif
const encoders_gpio_t gpio_sck = AD2S1205_config_now.spi_config.gpio_sck;
for (int i = 0;i < length;i++) {
for (int i = 0; i < length; i++) {
#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
@ -243,7 +252,7 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
uint16_t receive = 0;
for (int bit = 0;bit < 16;bit++) {
for (int bit = 0; bit < 16; bit++) {
#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
palWritePad(gpio_mosi.port, gpio_mosi.pin, send >> (15 - bit));
#endif
@ -278,12 +287,14 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
}
static void spi_begin(void) {
palClearPad(AD2S1205_config_now.spi_config.gpio_nss.port, AD2S1205_config_now.spi_config.gpio_nss.pin);
palClearPad(AD2S1205_config_now.spi_config.gpio_nss.port,
AD2S1205_config_now.spi_config.gpio_nss.pin);
spi_AS5047_cs_delay();
}
static void spi_end(void) {
palSetPad(AD2S1205_config_now.spi_config.gpio_nss.port, AD2S1205_config_now.spi_config.gpio_nss.pin);
palSetPad(AD2S1205_config_now.spi_config.gpio_nss.port,
AD2S1205_config_now.spi_config.gpio_nss.pin);
spi_AS5047_cs_delay();
}
@ -295,19 +306,45 @@ static void spi_delay(void) {
}
static void spi_AS5047_cs_delay(void) {
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
}

7
encoder/AD2S1205.h
View File

@ -1,14 +1,12 @@
#ifndef ENCODER_AD2S1205_H_
#define ENCODER_AD2S1205_H_
#include "datatypes.h"
#include "encoder/encoder_datatype.h"
void AD2S1205_deinit(void);
encoders_ret_t AD2S1205_init(AD2S1205_config_t* AD2S1205_config);
encoders_ret_t AD2S1205_init(AD2S1205_config_t *AD2S1205_config);
float AD2S1205_read_deg(void);
void AD2S1205_routine(void);
@ -21,7 +19,4 @@ uint32_t AD2S1205_resolver_loss_of_tracking_error_cnt(void);
uint32_t AD2S1205_resolver_degradation_of_signal_error_cnt(void);
uint32_t AD2S1205_resolver_loss_of_signal_error_cnt(void);
#endif /* ENCODER_AD2S1205_H_ */

269
encoder/AS504x.c
View File

@ -1,5 +1,4 @@
#include "encoder/AS504x.h"
#include "ch.h"
@ -18,15 +17,14 @@ static uint16_t AS504x_diag_fetch_now_count = 0;
static uint32_t AS504x_data_last_invalid_counter = 0;
#endif
static uint32_t AS504x_spi_communication_error_count = 0;
static AS504x_config_t AS504x_config_now = {0};
static AS504x_config_t AS504x_config_now = { 0 };
static uint8_t spi_data_err_raised = 0;
static AS504x_diag AS504x_sensor_diag = {0};
static AS504x_diag AS504x_sensor_diag = { 0 };
static uint16_t spi_val = 0;
static float last_enc_angle = 0.0;
static uint32_t spi_error_cnt = 0;
static float spi_error_rate = 0.0;
//Private functions
static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
static void spi_begin(void);
@ -40,45 +38,46 @@ static uint8_t AS504x_fetch_diag(void);
static uint8_t AS504x_verify_serial(void);
static void AS504x_deserialize_diag(void);
static void AS504x_fetch_clear_err_diag(void);
static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length);
static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf,
const uint16_t *out_buf, int length);
#endif
static void AS504x_determinate_if_connected(bool was_last_valid);
void AS504x_routine(void)
{
void AS504x_routine(void) {
uint16_t pos;
uint16_t pos;
// if MOSI is defined, use diagnostics
#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
spi_begin();
spi_transfer(0, 0, 1);
spi_end();
spi_begin();
spi_transfer(0, 0, 1);
spi_end();
spi_AS5047_cs_delay();
spi_AS5047_cs_delay();
spi_begin();
spi_data_err_raised = AS504x_spi_transfer_err_check(&pos, 0, 1);
spi_end();
spi_val = pos;
spi_begin();
spi_data_err_raised = AS504x_spi_transfer_err_check(&pos, 0, 1);
spi_end();
spi_val = pos;
// get diagnostic every AS504x_REFRESH_DIAG_AFTER_NSAMPLES
AS504x_diag_fetch_now_count++;
if(AS504x_diag_fetch_now_count >= AS504x_REFRESH_DIAG_AFTER_NSAMPLES || spi_data_err_raised) {
// clear error flags before getting new diagnostics data
AS504x_fetch_clear_err_diag();
// get diagnostic every AS504x_REFRESH_DIAG_AFTER_NSAMPLES
AS504x_diag_fetch_now_count++;
if (AS504x_diag_fetch_now_count >= AS504x_REFRESH_DIAG_AFTER_NSAMPLES
|| spi_data_err_raised) {
// clear error flags before getting new diagnostics data
AS504x_fetch_clear_err_diag();
if(!AS504x_fetch_diag()) {
if(!AS504x_verify_serial()) {
AS504x_deserialize_diag();
AS504x_determinate_if_connected(true);
} else {
AS504x_determinate_if_connected(false);
}
} else {
AS504x_determinate_if_connected(false);
}
AS504x_diag_fetch_now_count = 0;
}
if (!AS504x_fetch_diag()) {
if (!AS504x_verify_serial()) {
AS504x_deserialize_diag();
AS504x_determinate_if_connected(true);
} else {
AS504x_determinate_if_connected(false);
}
} else {
AS504x_determinate_if_connected(false);
}
AS504x_diag_fetch_now_count = 0;
}
#else
spi_begin();
spi_transfer(&pos, 0, 1);
@ -98,14 +97,16 @@ void AS504x_routine(void)
}
#endif
if(spi_check_parity(pos) && !spi_data_err_raised) {
pos &= 0x3FFF;
last_enc_angle = ((float)pos * 360.0) / 16384.0;
UTILS_LP_FAST(spi_error_rate, 0.0, 1./AS504x_config_now.refresh_rate_hz);
} else {
++spi_error_cnt;
UTILS_LP_FAST(spi_error_rate, 1.0, 1./AS504x_config_now.refresh_rate_hz);
}
if (spi_check_parity(pos) && !spi_data_err_raised) {
pos &= 0x3FFF;
last_enc_angle = ((float) pos * 360.0) / 16384.0;
UTILS_LP_FAST(spi_error_rate, 0.0,
1. / AS504x_config_now.refresh_rate_hz);
} else {
++spi_error_cnt;
UTILS_LP_FAST(spi_error_rate, 1.0,
1. / AS504x_config_now.refresh_rate_hz);
}
}
static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
@ -114,7 +115,7 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
const encoders_gpio_t gpio_mosi = AS504x_config_now.spi_config.gpio_mosi;
#endif
const encoders_gpio_t gpio_sck = AS504x_config_now.spi_config.gpio_sck;
for (int i = 0;i < length;i++) {
for (int i = 0; i < length; i++) {
#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
@ -124,7 +125,7 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
uint16_t receive = 0;
for (int bit = 0;bit < 16;bit++) {
for (int bit = 0; bit < 16; bit++) {
#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
palWritePad(gpio_mosi.port, gpio_mosi.pin, send >> (15 - bit));
#endif
@ -158,21 +159,26 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
}
}
encoders_ret_t AS504x_init(AS504x_config_t* AS504x_config)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
encoders_ret_t AS504x_init(AS504x_config_t *AS504x_config) {
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
encoders_spi_config_t AS504x_spi_config = AS504x_config->spi_config;
palSetPadMode(AS504x_spi_config.gpio_miso.port, AS504x_spi_config.gpio_miso.pin, PAL_MODE_INPUT);
palSetPadMode(AS504x_spi_config.gpio_sck.port, AS504x_spi_config.gpio_sck.pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(AS504x_spi_config.gpio_nss.port, AS504x_spi_config.gpio_nss.pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(AS504x_spi_config.gpio_miso.port,
AS504x_spi_config.gpio_miso.pin, PAL_MODE_INPUT);
palSetPadMode(AS504x_spi_config.gpio_sck.port,
AS504x_spi_config.gpio_sck.pin,
PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(AS504x_spi_config.gpio_nss.port,
AS504x_spi_config.gpio_nss.pin,
PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
// Set MOSI to 1
#if AS504x_USE_SW_MOSI_PIN
palSetPadMode(AS504x_spi_config.gpio_mosi.port, AS504x_spi_config.gpio_mosi.pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPad(AS504x_spi_config.gpio_mosi.port, AS504x_spi_config.gpio_mosi.pin);
palSetPadMode(AS504x_spi_config.gpio_mosi.port,
AS504x_spi_config.gpio_mosi.pin,
PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPad(AS504x_spi_config.gpio_mosi.port,
AS504x_spi_config.gpio_mosi.pin);
#endif
AS504x_config_now = *AS504x_config;
@ -182,29 +188,30 @@ encoders_ret_t AS504x_init(AS504x_config_t* AS504x_config)
// Time Base configuration
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS504x_config->refresh_rate_hz) - 1);
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2
/ AS504x_config->refresh_rate_hz) - 1);
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
// Enable overflow interrupt
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
// Enable timer
TIM_Cmd(HW_ENC_TIM, ENABLE);
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
// Enable overflow interrupt
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
// Enable timer
TIM_Cmd(HW_ENC_TIM, ENABLE);
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
spi_error_rate = 0.0;
spi_error_rate = 0.0;
AS504x_config_now.is_init = 1;
AS504x_config->is_init = 1;
return ENCODERS_OK;
AS504x_config_now.is_init = 1;
AS504x_config->is_init = 1;
return ENCODERS_OK;
}
#if (AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS)
static uint8_t AS504x_fetch_diag(void) {
uint16_t recf[2], senf[2] = {AS504x_SPI_READ_DIAG_MSG, AS504x_SPI_READ_MAGN_MSG};
uint16_t recf[2], senf[2] = { AS504x_SPI_READ_DIAG_MSG,
AS504x_SPI_READ_MAGN_MSG };
uint8_t ret = 0;
spi_begin();
spi_transfer(0, senf, 1);
spi_end();
@ -221,7 +228,7 @@ static uint8_t AS504x_fetch_diag(void) {
ret |= AS504x_spi_transfer_err_check(recf + 1, 0, 1);
spi_end();
if(!ret) {
if (!ret) {
if (spi_check_parity(recf[0]) && spi_check_parity(recf[1])) {
AS504x_sensor_diag.serial_diag_flgs = recf[0];
AS504x_sensor_diag.serial_magnitude = recf[1];
@ -238,15 +245,18 @@ static uint8_t AS504x_verify_serial() {
serial_magnitude = AS504x_get_diag().serial_magnitude;
serial_diag_flgs = AS504x_get_diag().serial_diag_flgs;
test_magnitude = serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
test_magnitude = serial_magnitude
& AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
test_AGC_value = serial_diag_flgs;
test_is_Comp_low = (serial_diag_flgs >> AS504x_SPI_DIAG_COMP_LOW_BIT_POS) & 1;
test_is_Comp_high = (serial_diag_flgs >> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS) & 1;
test_is_Comp_low = (serial_diag_flgs >> AS504x_SPI_DIAG_COMP_LOW_BIT_POS)
& 1;
test_is_Comp_high = (serial_diag_flgs >> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS)
& 1;
if (test_is_Comp_high && test_is_Comp_low) {
return 1;
}
if((uint32_t)test_magnitude + (uint32_t)test_AGC_value == 0) {
if ((uint32_t) test_magnitude + (uint32_t) test_AGC_value == 0) {
return 1;
}
@ -255,11 +265,16 @@ static uint8_t AS504x_verify_serial() {
static void AS504x_deserialize_diag() {
AS504x_sensor_diag.AGC_value = AS504x_sensor_diag.serial_diag_flgs;
AS504x_sensor_diag.is_OCF = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_OCF_BIT_POS) & 1;
AS504x_sensor_diag.is_COF = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COF_BIT_POS) & 1;
AS504x_sensor_diag.is_Comp_low = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COMP_LOW_BIT_POS) & 1;
AS504x_sensor_diag.is_Comp_high = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS) & 1;
AS504x_sensor_diag.magnitude = AS504x_sensor_diag.serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
AS504x_sensor_diag.is_OCF = (AS504x_sensor_diag.serial_diag_flgs
>> AS504x_SPI_DIAG_OCF_BIT_POS) & 1;
AS504x_sensor_diag.is_COF = (AS504x_sensor_diag.serial_diag_flgs
>> AS504x_SPI_DIAG_COF_BIT_POS) & 1;
AS504x_sensor_diag.is_Comp_low = (AS504x_sensor_diag.serial_diag_flgs
>> AS504x_SPI_DIAG_COMP_LOW_BIT_POS) & 1;
AS504x_sensor_diag.is_Comp_high = (AS504x_sensor_diag.serial_diag_flgs
>> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS) & 1;
AS504x_sensor_diag.magnitude = AS504x_sensor_diag.serial_magnitude
& AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
}
static void AS504x_fetch_clear_err_diag() {
@ -278,11 +293,12 @@ static void AS504x_fetch_clear_err_diag() {
AS504x_sensor_diag.serial_error_flags = recf;
}
static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length) {
static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf,
const uint16_t *out_buf, int length) {
spi_transfer(in_buf, out_buf, length);
for(int len_count = 0; len_count < length; len_count++) {
if(((in_buf[len_count]) >> 14) & 0b01) {
for (int len_count = 0; len_count < length; len_count++) {
if (((in_buf[len_count]) >> 14) & 0b01) {
return 1;
}
}
@ -292,15 +308,17 @@ static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf, const uint16_t *o
#endif
static void AS504x_determinate_if_connected(bool was_last_valid) {
if(!was_last_valid) {
if (!was_last_valid) {
AS504x_spi_communication_error_count++;
if(AS504x_spi_communication_error_count >= AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) {
AS504x_spi_communication_error_count = AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD;
if (AS504x_spi_communication_error_count
>= AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) {
AS504x_spi_communication_error_count =
AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD;
AS504x_sensor_diag.is_connected = 0;
}
} else {
if(AS504x_spi_communication_error_count) {
if (AS504x_spi_communication_error_count) {
AS504x_spi_communication_error_count--;
} else {
AS504x_sensor_diag.is_connected = 1;
@ -312,18 +330,15 @@ AS504x_diag AS504x_get_diag(void) {
return AS504x_sensor_diag;
}
float AS504x_read_deg(void)
{
float AS504x_read_deg(void) {
return last_enc_angle;
}
uint32_t AS504x_spi_get_val(void)
{
uint32_t AS504x_spi_get_val(void) {
return spi_val;
}
uint32_t AS504x_spi_get_error_cnt(void)
{
uint32_t AS504x_spi_get_error_cnt(void) {
return spi_error_cnt;
}
@ -337,20 +352,26 @@ void AS504x_deinit(void) {
TIM_DeInit(HW_ENC_TIM);
palSetPadMode(AS504x_config_now.spi_config.gpio_miso.port, AS504x_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_sck.port, AS504x_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_nss.port, AS504x_config_now.spi_config.gpio_nss.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_miso.port,
AS504x_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_sck.port,
AS504x_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_nss.port,
AS504x_config_now.spi_config.gpio_nss.pin, PAL_MODE_INPUT_PULLUP);
#if AS504x_USE_SW_MOSI_PIN
palSetPadMode(AS504x_config_now.spi_config.gpio_mosi.port, AS504x_config_now.spi_config.gpio_mosi.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_mosi.port,
AS504x_config_now.spi_config.gpio_mosi.pin, PAL_MODE_INPUT_PULLUP);
#endif
#ifdef HW_SPI_DEV
spiStop(&HW_SPI_DEV);
#endif
palSetPadMode(AS504x_config_now.spi_config.gpio_miso.port, AS504x_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_sck.port, AS504x_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_miso.port,
AS504x_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(AS504x_config_now.spi_config.gpio_sck.port,
AS504x_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
AS504x_config_now.is_init = 0;
last_enc_angle = 0.0;
@ -361,12 +382,14 @@ void AS504x_deinit(void) {
#pragma GCC optimize ("O0")
static void spi_begin(void) {
palClearPad(AS504x_config_now.spi_config.gpio_nss.port, AS504x_config_now.spi_config.gpio_nss.pin);
palClearPad(AS504x_config_now.spi_config.gpio_nss.port,
AS504x_config_now.spi_config.gpio_nss.pin);
spi_AS5047_cs_delay();
}
static void spi_end(void) {
palSetPad(AS504x_config_now.spi_config.gpio_nss.port, AS504x_config_now.spi_config.gpio_nss.pin);
palSetPad(AS504x_config_now.spi_config.gpio_nss.port,
AS504x_config_now.spi_config.gpio_nss.pin);
spi_AS5047_cs_delay();
}
@ -378,19 +401,45 @@ static void spi_delay(void) {
}
static void spi_AS5047_cs_delay(void) {
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
}

5
encoder/AS504x.h
View File

@ -1,5 +1,4 @@
#ifndef ENCODER_AS504X_H_
#define ENCODER_AS504X_H_
@ -9,7 +8,7 @@
void AS504x_routine(void);
float AS504x_read_deg(void);
void AS504x_deinit(void);
encoders_ret_t AS504x_init(AS504x_config_t* AS504x_config);
encoders_ret_t AS504x_init(AS504x_config_t *AS504x_config);
AS504x_diag AS504x_get_diag(void);
bool AS504x_index_found(void);
float AS504x_spi_get_error_rate(void);
@ -26,10 +25,8 @@ uint32_t AS504x_spi_get_error_cnt(void);
#define AS5047_SAMPLE_RATE_HZ 20000
#define AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK 0x3FFF
#define AS504x_SPI_DIAG_ADR 0x3FFD
#define AS504x_SPI_MAGN_ADR 0x3FFE
#define AS504x_SPI_CLEAR_ERROR_ADR 0x0001

229
encoder/MT6816.c
View File

@ -1,8 +1,6 @@
#include "encoder/MT6816.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
@ -30,10 +28,10 @@ static void spi_AS5047_cs_delay(void);
static bool spi_check_parity(uint16_t x);
#ifdef HW_SPI_DEV
static SPIConfig mt6816_spi_cfg = {0};
static SPIConfig mt6816_spi_cfg = { 0 };
#endif
static MT6816_config_t mt6816_config_now = {0};
static MT6816_config_t mt6816_config_now = { 0 };
static float spi_error_rate = 0.0;
static float encoder_no_magnet_error_rate = 0.0;
@ -42,17 +40,19 @@ static float last_enc_angle = 0.0;
static uint32_t spi_error_cnt = 0;
static uint32_t spi_val = 0;
void MT6816_deinit(void)
{
void MT6816_deinit(void) {
nvicDisableVector(HW_ENC_EXTI_CH);
nvicDisableVector(HW_ENC_TIM_ISR_CH);
TIM_DeInit(HW_ENC_TIM);
palSetPadMode(mt6816_config_now.spi_config.gpio_miso.port, mt6816_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_sck.port, mt6816_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_nss.port, mt6816_config_now.spi_config.gpio_nss.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_miso.port,
mt6816_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_sck.port,
mt6816_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_nss.port,
mt6816_config_now.spi_config.gpio_nss.pin, PAL_MODE_INPUT_PULLUP);
#if (MT6816_USE_HW_SPI_PINS)
palSetPadMode(mt6816_config_now.spi_config.gpio_mosi.port, mt6816_config_now.spi_config.gpio_mosi.pin, PAL_MODE_INPUT_PULLUP);
@ -65,23 +65,30 @@ void MT6816_deinit(void)
//palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP); //TODO: is this necessary
//palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP); //TODO: is this necessary
palSetPadMode(mt6816_config_now.spi_config.gpio_miso.port, mt6816_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_sck.port, mt6816_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_miso.port,
mt6816_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
palSetPadMode(mt6816_config_now.spi_config.gpio_sck.port,
mt6816_config_now.spi_config.gpio_sck.pin, PAL_MODE_INPUT_PULLUP);
mt6816_config_now.is_init = 0;
last_enc_angle = 0.0;
spi_error_rate = 0.0;
}
encoders_ret_t MT6816_init(MT6816_config_t* mt6816_config)
{
encoders_ret_t MT6816_init(MT6816_config_t *mt6816_config) {
#ifdef HW_SPI_DEV
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
encoders_spi_config_t mt6816_spi_config = mt6816_config->spi_config;
palSetPadMode(mt6816_spi_config.gpio_sck.port, mt6816_spi_config.gpio_sck.pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(mt6816_spi_config.gpio_miso.port, mt6816_spi_config.gpio_miso.pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(mt6816_spi_config.gpio_nss.port, mt6816_spi_config.gpio_nss.pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(mt6816_spi_config.gpio_sck.port,
mt6816_spi_config.gpio_sck.pin,
PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(mt6816_spi_config.gpio_miso.port,
mt6816_spi_config.gpio_miso.pin,
PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(mt6816_spi_config.gpio_nss.port,
mt6816_spi_config.gpio_nss.pin,
PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
#if (MT6816_USE_HW_SPI_PINS)
palSetPadMode(mt6816_spi_config.gpio_mosi.port, mt6816_spi_config.gpio_mosi.pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
@ -91,106 +98,108 @@ encoders_ret_t MT6816_init(MT6816_config_t* mt6816_config)
mt6816_spi_cfg.end_cb = NULL;
mt6816_spi_cfg.ssport = mt6816_spi_config.gpio_nss.port;
mt6816_spi_cfg.sspad =mt6816_spi_config.gpio_nss.pin;
mt6816_spi_cfg.cr1 = SPI_BaudRatePrescaler_4 | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_DATASIZE_16BIT;
mt6816_spi_cfg.sspad = mt6816_spi_config.gpio_nss.pin;
mt6816_spi_cfg.cr1 = SPI_BaudRatePrescaler_4 | SPI_CR1_CPOL | SPI_CR1_CPHA
| SPI_DATASIZE_16BIT;
//Start driver with MT6816 SPI settings
spiStart(&HW_SPI_DEV, &mt6816_spi_cfg);
spiStart(&HW_SPI_DEV, &mt6816_spi_cfg);
// Enable timer clock
HW_ENC_TIM_CLK_EN();
// Enable timer clock
HW_ENC_TIM_CLK_EN();
// 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);
// 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);
// Enable overflow interrupt
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
// Enable overflow interrupt
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
// Enable timer
TIM_Cmd(HW_ENC_TIM, ENABLE);
// Enable timer
TIM_Cmd(HW_ENC_TIM, ENABLE);
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
spi_error_rate = 0.0;
encoder_no_magnet_error_rate = 0.0;
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
spi_error_rate = 0.0;
encoder_no_magnet_error_rate = 0.0;
mt6816_config_now.is_init = 1;
mt6816_config->is_init = 1;
mt6816_config_now.is_init = 1;
mt6816_config->is_init = 1;
#endif
return ENCODERS_OK;
return ENCODERS_OK;
}
void MT6816_routine(void)
{
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;
void MT6816_routine(void) {
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;
spi_begin();
reg_data_03 = spiPolledExchange(&HW_SPI_DEV, reg_addr_03);
spi_end();
spi_delay();
spi_begin();
reg_data_04 = spiPolledExchange(&HW_SPI_DEV, reg_addr_04);
spi_end();
spi_begin();
reg_data_03 = spiPolledExchange(&HW_SPI_DEV, reg_addr_03);
spi_end();
spi_delay();
spi_begin();
reg_data_04 = spiPolledExchange(&HW_SPI_DEV, reg_addr_04);
spi_end();
pos = (reg_data_03 << 8) | reg_data_04;
spi_val = pos;
pos = (reg_data_03 << 8) | reg_data_04;
spi_val = pos;
if( spi_check_parity(pos) ) {
if (pos & MT6816_NO_MAGNET_ERROR_MASK) {
++encoder_no_magnet_error_cnt;
UTILS_LP_FAST(encoder_no_magnet_error_rate, 1.0, 1./mt6816_config_now.refresh_rate_hz);
} else {
pos = pos >> 2;
last_enc_angle = ((float)pos * 360.0) / 16384.0;
UTILS_LP_FAST(spi_error_rate, 0.0, 1./mt6816_config_now.refresh_rate_hz);
UTILS_LP_FAST(encoder_no_magnet_error_rate, 0.0, 1./mt6816_config_now.refresh_rate_hz);
}
} else {
++spi_error_cnt;
UTILS_LP_FAST(spi_error_rate, 1.0, 1./mt6816_config_now.refresh_rate_hz);
}
if (spi_check_parity(pos)) {
if (pos & MT6816_NO_MAGNET_ERROR_MASK) {
++encoder_no_magnet_error_cnt;
UTILS_LP_FAST(encoder_no_magnet_error_rate, 1.0,
1. / mt6816_config_now.refresh_rate_hz);
} else {
pos = pos >> 2;
last_enc_angle = ((float) pos * 360.0) / 16384.0;
UTILS_LP_FAST(spi_error_rate, 0.0,
1. / mt6816_config_now.refresh_rate_hz);
UTILS_LP_FAST(encoder_no_magnet_error_rate, 0.0,
1. / mt6816_config_now.refresh_rate_hz);
}
} else {
++spi_error_cnt;
UTILS_LP_FAST(spi_error_rate, 1.0,
1. / mt6816_config_now.refresh_rate_hz);
}
}
uint32_t MT6816_spi_get_val(void)
{
uint32_t MT6816_spi_get_val(void) {
return spi_val;
}
uint32_t MT6816_spi_get_error_cnt(void)
{
uint32_t MT6816_spi_get_error_cnt(void) {
return spi_error_cnt;
}
uint32_t MT6816_get_no_magnet_error_cnt(void)
{
return encoder_no_magnet_error_cnt;
uint32_t MT6816_get_no_magnet_error_cnt(void) {
return encoder_no_magnet_error_cnt;
}
uint32_t MT6816_get_no_magnet_error_rate(void)
{
return encoder_no_magnet_error_rate;
uint32_t MT6816_get_no_magnet_error_rate(void) {
return encoder_no_magnet_error_rate;
}
float MT6816_read_deg(void)
{
return last_enc_angle;
float MT6816_read_deg(void) {
return last_enc_angle;
}
static void spi_begin(void) {
palClearPad(mt6816_config_now.spi_config.gpio_nss.port, mt6816_config_now.spi_config.gpio_nss.pin);
palClearPad(mt6816_config_now.spi_config.gpio_nss.port,
mt6816_config_now.spi_config.gpio_nss.pin);
spi_AS5047_cs_delay();
}
static void spi_end(void) {
palSetPad(mt6816_config_now.spi_config.gpio_nss.port, mt6816_config_now.spi_config.gpio_nss.pin);
palSetPad(mt6816_config_now.spi_config.gpio_nss.port,
mt6816_config_now.spi_config.gpio_nss.pin);
spi_AS5047_cs_delay();
}
@ -202,19 +211,45 @@ static void spi_delay(void) {
}
static void spi_AS5047_cs_delay(void) {
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();__NOP();__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
__NOP();
}

8
encoder/MT6816.h
View File

@ -1,14 +1,12 @@
#ifndef ENCODER_MT6816_H_
#define ENCODER_MT6816_H_
#include "datatypes.h"
#include "encoder/encoder_datatype.h"
void MT6816_deinit(void);
encoders_ret_t MT6816_init(MT6816_config_t* mt6816_config);
encoders_ret_t MT6816_init(MT6816_config_t *mt6816_config);
void MT6816_routine(void);
float MT6816_read_deg(void);
@ -18,8 +16,4 @@ uint32_t MT6816_spi_get_error_cnt(void);
uint32_t MT6816_get_no_magnet_error_cnt(void);
uint32_t MT6816_get_no_magnet_error_rate(void);
#endif /* ENCODER_MT6816_H_ */

46
encoder/encoder_datatype.h
View File

@ -1,5 +1,3 @@
#ifndef ENCODER_ENCODER_DATATYPE_H_
#define ENCODER_ENCODER_DATATYPE_H_
@ -11,13 +9,11 @@
typedef uint8_t encoders_pin_t;
typedef uint32_t encoders_refresh_rate_hz_t;
typedef enum{
ENCODERS_OK = 0,
ENCODERS_NONE,
ENCODERS_ERROR
}encoders_ret_t;
typedef enum {
ENCODERS_OK = 0, ENCODERS_NONE, ENCODERS_ERROR
} encoders_ret_t;
typedef enum{
typedef enum {
ENCODERS_TYPE_NONE = 0,
ENCODERS_TYPE_AS504x,
ENCODERS_TYPE_MT6816,
@ -25,57 +21,55 @@ typedef enum{
ENCODERS_TYPE_SINCOS,
ENCODERS_TYPE_TS5700N8501,
ENCODERS_TYPE_ABI
}encoders_type_t;
} encoders_type_t;
typedef struct{
stm32_gpio_t* port;
typedef struct {
stm32_gpio_t *port;
encoders_pin_t pin;
}encoders_gpio_t;
} encoders_gpio_t;
typedef struct{
typedef struct {
encoders_gpio_t gpio_nss;
encoders_gpio_t gpio_miso;
encoders_gpio_t gpio_mosi;
encoders_gpio_t gpio_sck;
}encoders_spi_config_t;
} encoders_spi_config_t;
typedef struct{
typedef struct {
encoders_gpio_t gpio_A;
encoders_gpio_t gpio_B;
}encoders_incremental_config_t;
} encoders_incremental_config_t;
typedef struct {
bool is_init;
encoders_refresh_rate_hz_t refresh_rate_hz; //TODO: REWRITE TO POINTER OF AS504x_config_t
encoders_spi_config_t spi_config; //TODO: REWRITE TO POINTER OF AS504x_config_t
}AS504x_config_t;
encoders_spi_config_t spi_config; //TODO: REWRITE TO POINTER OF AS504x_config_t
} AS504x_config_t;
typedef struct {
bool is_init;
encoders_refresh_rate_hz_t refresh_rate_hz; //TODO: REWRITE TO POINTER OF AS504x_config_t
encoders_spi_config_t spi_config; //TODO: REWRITE TO POINTER OF AS504x_config_t
}MT6816_config_t;
encoders_spi_config_t spi_config; //TODO: REWRITE TO POINTER OF AS504x_config_t
} MT6816_config_t;
typedef struct {
bool is_init;
encoders_refresh_rate_hz_t refresh_rate_hz;
encoders_spi_config_t spi_config;
}AD2S1205_config_t;
} AD2S1205_config_t;
typedef struct {
bool is_init;
uint32_t counts;
encoders_incremental_config_t incremental_config;
}ABI_config_t;
} ABI_config_t;
typedef struct{
typedef struct {
encoders_type_t encoder_type;
encoders_refresh_rate_hz_t refresh_rate_hz;
encoders_spi_config_t spi_config;
uint32_t counts; // FOR INCREMENTAL INTERFACE
encoders_incremental_config_t incremental_config; // FOR INCREMENTAL INTERFACE
}encoders_config_t;
} encoders_config_t;
#endif /* ENCODER_ENCODER_DATATYPE_H_ */

113
encoder/encoder_hwconf.c
View File

@ -1,5 +1,3 @@
#include "encoder/encoder_hwconf.h"
#include "encoder/encoders.h"
@ -17,82 +15,49 @@
#define ENCODERS_SPI_UNUSED {{ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}, {ENCODERS_CONFIG_UNUSED}}
#define ENCODERS_ABI_COUNTER_DEFAULT_VALUE 10000ul
encoders_config_t conf_AS5047 = {
ENCODERS_TYPE_AS504x,
AS5047_SAMPLE_RATE_HZ,
{//SPI
{//NSS
HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3
},
{//MISO
HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2
},
{//MOSI
HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI
},
{//SCK
HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1
}
},
ENCODERS_CONFIG_UNUSED,
ENCODERS_INCREMENTAL_UNUSED
encoders_config_t conf_AS5047 = { ENCODERS_TYPE_AS504x,
AS5047_SAMPLE_RATE_HZ,
{/*SPI*/
{/*NSS*/HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 },
{/*MISO*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 },
{/*MOSI*/HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI },
{/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 }
},
ENCODERS_CONFIG_UNUSED,
ENCODERS_INCREMENTAL_UNUSED
};
encoders_config_t conf_MT6816 = {
ENCODERS_TYPE_MT6816,
MT6816_SAMPLE_RATE_HZ,
{//SPI
{//NSS
HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3
},
{//MISO
HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2
},
{//MOSI
HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI
},
{//SCK
HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1
}
},
ENCODERS_CONFIG_UNUSED,
ENCODERS_INCREMENTAL_UNUSED
encoders_config_t conf_MT6816 = { ENCODERS_TYPE_MT6816,
MT6816_SAMPLE_RATE_HZ,
{/*SPI*/
{/*NSS*/HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 },
{/*MISO*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 },
{/*MOSI*/HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI },
{/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 }
},
ENCODERS_CONFIG_UNUSED,
ENCODERS_INCREMENTAL_UNUSED
};
encoders_config_t conf_AD2S1205 = {
ENCODERS_TYPE_AD2S1205_SPI,
AD2S1205_SAMPLE_RATE_HZ,
{//SPI
{//NSS
HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3
},
{//MISO
HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2
},
{//MOSI
HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI
},
{//SCK
HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1
}
},
ENCODERS_CONFIG_UNUSED,
ENCODERS_INCREMENTAL_UNUSED
encoders_config_t conf_AD2S1205 = { ENCODERS_TYPE_AD2S1205_SPI,
AD2S1205_SAMPLE_RATE_HZ,
{/*SPI*/
{/*NSS*/HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 },
{/*MISO*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 },
{/*MOSI*/HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI },
{/*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 }
},
ENCODERS_CONFIG_UNUSED,
ENCODERS_INCREMENTAL_UNUSED
};
encoders_config_t conf_ABI = {
ENCODERS_TYPE_ABI,
ENCODERS_CONFIG_UNUSED,
ENCODERS_SPI_UNUSED,
ENCODERS_ABI_COUNTER_DEFAULT_VALUE,
{// INCREMENTAL PROTOCOL
{
HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1
},
{
HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2
}
}
};
encoders_config_t conf_ABI = { ENCODERS_TYPE_ABI,
ENCODERS_CONFIG_UNUSED,
ENCODERS_SPI_UNUSED,
ENCODERS_ABI_COUNTER_DEFAULT_VALUE,
{/*INCREMENTAL PROTOCOL*/
{/*A*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 },
{/*B*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 }
}
};

1
encoder/encoder_hwconf.h
View File

@ -1,5 +1,4 @@
#ifndef ENCODER_ENCODER_HWCONF_H_
#define ENCODER_ENCODER_HWCONF_H_

198
encoder/encoders.c
View File

@ -1,22 +1,18 @@
#include "encoders.h"
static encoders_type_t encoder_type_now = ENCODERS_TYPE_NONE; // TODO: CHANGE TO encoder_config_now
static uint32_t enc_counts = 10000;
static bool index_found = false;
encoders_ret_t encoders_init(encoders_config_t *encoder_config)
{
encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
//TODO: ASSERTS
if(encoder_type_now != ENCODERS_TYPE_NONE)
{
if (encoder_type_now != ENCODERS_TYPE_NONE) {
return ENCODERS_ERROR;
}
if(encoder_config->encoder_type == ENCODERS_TYPE_AS504x)
{
if (encoder_config->encoder_type == ENCODERS_TYPE_AS504x) {
AS504x_config_t as504x_config;
encoders_ret_t encoder_ret;
@ -24,8 +20,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
as504x_config.refresh_rate_hz = encoder_config->refresh_rate_hz;
encoder_ret = AS504x_init(&as504x_config);
if(ENCODERS_OK != encoder_ret || !as504x_config.is_init)
{
if (ENCODERS_OK != encoder_ret || !as504x_config.is_init) {
encoder_type_now = ENCODERS_TYPE_NONE; // TODO: maybe should be deleted
index_found = false;
return ENCODERS_ERROR;
@ -33,9 +28,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_type_now = ENCODERS_TYPE_AS504x;
index_found = true;
return ENCODERS_OK;
}
else if(encoder_config->encoder_type == ENCODERS_TYPE_MT6816)
{
} else if (encoder_config->encoder_type == ENCODERS_TYPE_MT6816) {
MT6816_config_t mt6816_config;
encoders_ret_t encoder_ret;
@ -44,8 +37,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_ret = MT6816_init(&mt6816_config);
if(ENCODERS_OK != encoder_ret || !mt6816_config.is_init)
{
if (ENCODERS_OK != encoder_ret || !mt6816_config.is_init) {
encoder_type_now = ENCODERS_TYPE_NONE;
index_found = false;
return ENCODERS_ERROR;
@ -53,9 +45,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_type_now = ENCODERS_TYPE_MT6816;
index_found = true;
return ENCODERS_OK;
}
else if(encoder_config->encoder_type == ENCODERS_TYPE_AD2S1205_SPI)
{
} else if (encoder_config->encoder_type == ENCODERS_TYPE_AD2S1205_SPI) {
AD2S1205_config_t AD2S1205_config;
encoders_ret_t encoder_ret;
@ -64,8 +54,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_ret = AD2S1205_init(&AD2S1205_config);
if(ENCODERS_OK != encoder_ret || !AD2S1205_config.is_init)
{
if (ENCODERS_OK != encoder_ret || !AD2S1205_config.is_init) {
encoder_type_now = ENCODERS_TYPE_NONE;
index_found = false;
return ENCODERS_ERROR;
@ -73,9 +62,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_type_now = ENCODERS_TYPE_AD2S1205_SPI;
index_found = true;
return ENCODERS_OK;
}
else if(encoder_config->encoder_type == ENCODERS_TYPE_ABI)
{
} else if (encoder_config->encoder_type == ENCODERS_TYPE_ABI) {
ABI_config_t abi_config;
encoders_ret_t encoder_ret;
@ -84,8 +71,7 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_ret = ABI_init(&abi_config);
if(ENCODERS_OK != encoder_ret || !abi_config.is_init)
{
if (ENCODERS_OK != encoder_ret || !abi_config.is_init) {
encoder_type_now = ENCODERS_TYPE_NONE;
index_found = false;
return ENCODERS_ERROR;
@ -93,104 +79,74 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config)
encoder_type_now = ENCODERS_TYPE_ABI;
index_found = true;
return ENCODERS_OK;
}
else
{
} else {
encoder_type_now = ENCODERS_TYPE_NONE;
index_found = false;
}
return ENCODERS_NONE;
}
float encoders_read_deg(void)
{
if(encoder_type_now == ENCODERS_TYPE_AS504x)
{
float encoders_read_deg(void) {
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
return AS504x_read_deg();
}
else if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
return MT6816_read_deg();
}
else if(encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI)
{
} else if (encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI) {
return AD2S1205_read_deg();
}
else if(encoder_type_now == ENCODERS_TYPE_ABI)
{
} else if (encoder_type_now == ENCODERS_TYPE_ABI) {
return ABI_read_deg();
}
return 0.0;
}
encoders_type_t encoders_is_configured(void)
{
encoders_type_t encoders_is_configured(void) {
return encoder_type_now;
}
bool encoders_index_found(void)
{
return index_found;
bool encoders_index_found(void) {
return index_found;
}
void encoders_deinit(void)
{
if(encoder_type_now == ENCODERS_TYPE_AS504x)
{
void encoders_deinit(void) {
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
AS504x_deinit();
}
else if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
MT6816_deinit();
}
else if(encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI)
{
} else if (encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI) {
AD2S1205_deinit();
}
else if(encoder_type_now == ENCODERS_TYPE_ABI)
{
} else if (encoder_type_now == ENCODERS_TYPE_ABI) {
ABI_deinit();
}
}
float encoders_spi_get_error_rate(void) {
if(encoder_type_now == ENCODERS_TYPE_AS504x)
{
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
return AS504x_spi_get_error_rate();
}
return 0.0;
}
uint32_t encoders_spi_get_error_cnt(void)
{
if(encoder_type_now == ENCODERS_TYPE_AS504x)
{
uint32_t encoders_spi_get_error_cnt(void) {
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
return AS504x_spi_get_error_cnt();
}
else if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
return MT6816_spi_get_error_cnt();
}
return 0;
}
uint32_t encoders_get_no_magnet_error_cnt(void)
{
if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
return MT6816_get_no_magnet_error_cnt();
}
return 0;
uint32_t encoders_get_no_magnet_error_cnt(void) {
if (encoder_type_now == ENCODERS_TYPE_MT6816) {
return MT6816_get_no_magnet_error_cnt();
}
return 0;
}
float encoders_get_no_magnet_error_rate(void)
{
if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
return MT6816_get_no_magnet_error_rate();
}
return 0.0;
float encoders_get_no_magnet_error_rate(void) {
if (encoder_type_now == ENCODERS_TYPE_MT6816) {
return MT6816_get_no_magnet_error_rate();
}
return 0.0;
}
float encoders_resolver_loss_of_tracking_error_rate(void) {
return AD2S1205_resolver_loss_of_tracking_error_rate();
@ -216,63 +172,53 @@ uint32_t encoders_resolver_loss_of_signal_error_cnt(void) {
return AD2S1205_resolver_loss_of_signal_error_cnt();
}
uint32_t encoders_spi_get_val(void)
{
if(encoder_type_now == ENCODERS_TYPE_AS504x)
{
uint32_t encoders_spi_get_val(void) {
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
return AS504x_spi_get_val();
}
else if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
return MT6816_spi_get_val();
}
return 0;
}
void encoders_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 = enc_counts / 20;
// 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 = enc_counts / 20;
if (encoders_index_found()) {
// Some plausibility filtering.
if (cnt > (enc_counts - lim) || cnt < lim) {
HW_ENC_TIM->CNT = 0;
bad_pulses = 0;
} else {
bad_pulses++;
if (encoders_index_found()) {
// Some plausibility filtering.
if (cnt > (enc_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;
}
}
if (bad_pulses > 5) {
index_found = 0;
}
}
} else {
HW_ENC_TIM->CNT = 0;
index_found = true;
bad_pulses = 0;
}
}
}
void encoders_tim_isr(void)
{
if(encoder_type_now == ENCODERS_TYPE_AS504x)
{
void encoders_tim_isr(void) {
if (encoder_type_now == ENCODERS_TYPE_AS504x) {
AS504x_routine();
}
else if(encoder_type_now == ENCODERS_TYPE_MT6816)
{
} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
return MT6816_routine();
}
else if(encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI)
{
} else if (encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI) {
return AD2S1205_routine();
}
}

3
encoder/encoders.h
View File

@ -1,5 +1,4 @@
#ifndef ENCODER_ENCODERS_H_
#define ENCODER_ENCODERS_H_
@ -31,9 +30,7 @@ uint32_t encoders_resolver_loss_of_signal_error_cnt(void);
float encoders_get_no_magnet_error_rate(void);
uint32_t encoders_spi_get_val(void);
void encoders_tim_isr(void);
void encoders_reset(void);
#endif /* ENCODER_ENCODERS_H_ */