From d0acc03f866bb7de3de9dd902a1be8c43630e362 Mon Sep 17 00:00:00 2001 From: Jakub Tomczak Date: Wed, 15 Dec 2021 14:17:43 +0100 Subject: [PATCH] code formatting fixed --- encoder/ABI.c | 38 +++--- encoder/ABI.h | 2 - encoder/AD2S1205.c | 169 ++++++++++++++--------- encoder/AD2S1205.h | 7 +- encoder/AS504x.c | 269 ++++++++++++++++++++++--------------- encoder/AS504x.h | 5 +- encoder/MT6816.c | 229 ++++++++++++++++++------------- encoder/MT6816.h | 8 +- encoder/encoder_datatype.h | 46 +++---- encoder/encoder_hwconf.c | 113 ++++++---------- encoder/encoder_hwconf.h | 1 - encoder/encoders.c | 198 ++++++++++----------------- encoder/encoders.h | 3 - 13 files changed, 550 insertions(+), 538 deletions(-) diff --git a/encoder/ABI.c b/encoder/ABI.c index 367902d8..91cd06b9 100644 --- a/encoder/ABI.c +++ b/encoder/ABI.c @@ -1,5 +1,3 @@ - - #include "encoder/ABI.h" #include "ch.h" @@ -11,40 +9,48 @@ #include 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; } diff --git a/encoder/ABI.h b/encoder/ABI.h index 5c5ca8da..2a6b8377 100644 --- a/encoder/ABI.h +++ b/encoder/ABI.h @@ -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_ */ diff --git a/encoder/AD2S1205.c b/encoder/AD2S1205.c index 0fb460f6..fa357d9f 100644 --- a/encoder/AD2S1205.c +++ b/encoder/AD2S1205.c @@ -1,6 +1,4 @@ - - #include "encoder/AD2S1205.h" #include "ch.h" @@ -11,7 +9,7 @@ #include "utils.h" #include -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(); } diff --git a/encoder/AD2S1205.h b/encoder/AD2S1205.h index 238c8f90..a9b67d71 100644 --- a/encoder/AD2S1205.h +++ b/encoder/AD2S1205.h @@ -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_ */ diff --git a/encoder/AS504x.c b/encoder/AS504x.c index 13a6ee01..6b43e158 100644 --- a/encoder/AS504x.c +++ b/encoder/AS504x.c @@ -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(); } diff --git a/encoder/AS504x.h b/encoder/AS504x.h index 3033c3f6..a5afd9ba 100644 --- a/encoder/AS504x.h +++ b/encoder/AS504x.h @@ -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 diff --git a/encoder/MT6816.c b/encoder/MT6816.c index b1b03414..cb0fdf2d 100644 --- a/encoder/MT6816.c +++ b/encoder/MT6816.c @@ -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(); } diff --git a/encoder/MT6816.h b/encoder/MT6816.h index 055f1faa..276e2ad6 100644 --- a/encoder/MT6816.h +++ b/encoder/MT6816.h @@ -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_ */ diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h index f556186f..b4ac6ad4 100644 --- a/encoder/encoder_datatype.h +++ b/encoder/encoder_datatype.h @@ -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_ */ diff --git a/encoder/encoder_hwconf.c b/encoder/encoder_hwconf.c index 35ed97e7..edb7ab01 100644 --- a/encoder/encoder_hwconf.c +++ b/encoder/encoder_hwconf.c @@ -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 } +} +}; diff --git a/encoder/encoder_hwconf.h b/encoder/encoder_hwconf.h index 5f060c74..aa71a666 100644 --- a/encoder/encoder_hwconf.h +++ b/encoder/encoder_hwconf.h @@ -1,5 +1,4 @@ - #ifndef ENCODER_ENCODER_HWCONF_H_ #define ENCODER_ENCODER_HWCONF_H_ diff --git a/encoder/encoders.c b/encoder/encoders.c index 7436c022..46f45a6c 100644 --- a/encoder/encoders.c +++ b/encoder/encoders.c @@ -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(); } } diff --git a/encoder/encoders.h b/encoder/encoders.h index b38a819f..baa49a89 100644 --- a/encoder/encoders.h +++ b/encoder/encoders.h @@ -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_ */