From 95183ce724ca4aa5643559dd02880d0ccdbe06cf Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Sat, 19 Feb 2022 23:28:58 +0100 Subject: [PATCH] More refactoring, moved state out of AS504x so that multiple instances can be used --- conf_general.h | 9 -- datatypes.h | 13 -- encoder/enc_abi.c | 16 +- encoder/enc_ad2s1205.c | 9 +- encoder/enc_as504x.c | 302 +++++++++++++++---------------------- encoder/enc_as504x.h | 13 +- encoder/enc_mt6816.c | 101 +++++-------- encoder/enc_mt6816.h | 2 +- encoder/enc_sincos.c | 5 - encoder/enc_ts5700n8501.c | 58 +++---- encoder/enc_ts5700n8501.h | 2 +- encoder/encoder.c | 217 +++++++++++--------------- encoder/encoder_datatype.h | 61 ++++++-- spi_bb.c | 33 ++-- spi_bb.h | 3 +- 15 files changed, 348 insertions(+), 496 deletions(-) diff --git a/conf_general.h b/conf_general.h index 591c6975..f3276b33 100755 --- a/conf_general.h +++ b/conf_general.h @@ -302,15 +302,6 @@ //#define V_REG 3.3 // Use the pins for the hardware SPI port instead of the hall/encoder pins for the AS5047 -#ifndef AS5047_USE_HW_SPI_PINS -#define AS5047_USE_HW_SPI_PINS 0 -#endif -#ifndef AD2S1205_USE_HW_SPI_PINS -#define AD2S1205_USE_HW_SPI_PINS 0 -#endif -#ifndef MT6816_USE_HW_SPI_PINS -#define MT6816_USE_HW_SPI_PINS 0 -#endif #ifndef AS504x_USE_SW_MOSI_PIN #define AS504x_USE_SW_MOSI_PIN 0 #endif diff --git a/datatypes.h b/datatypes.h index 5d913f34..5780d50e 100644 --- a/datatypes.h +++ b/datatypes.h @@ -838,19 +838,6 @@ typedef struct { float gyro_offsets[3]; } imu_config; -typedef struct { - uint8_t is_connected; - uint8_t AGC_value; - uint16_t magnitude; - uint8_t is_OCF; - uint8_t is_COF; - uint8_t is_Comp_low; - uint8_t is_Comp_high; - uint16_t serial_diag_flgs; - uint16_t serial_magnitude; - uint16_t serial_error_flags; -}AS504x_diag; - typedef enum { CAN_MODE_VESC = 0, CAN_MODE_UAVCAN, diff --git a/encoder/enc_abi.c b/encoder/enc_abi.c index 8047c32d..7d6b294c 100644 --- a/encoder/enc_abi.c +++ b/encoder/enc_abi.c @@ -44,26 +44,19 @@ void enc_abi_deinit(void) { PAL_MODE_INPUT_PULLUP); last_enc_angle = 0.0; - - ABI_config_now.is_init = 0; } encoder_ret_t enc_abi_init(ABI_config_t *abi_config) { - EXTI_InitTypeDef EXTI_InitStructure; // Initialize variables ABI_config_now = *abi_config; - palSetPadMode(ABI_config_now.A_gpio, - ABI_config_now.A_pin, - PAL_MODE_ALTERNATE(HW_ENC_TIM_AF)); - palSetPadMode(ABI_config_now.B_gpio, - ABI_config_now.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)); + palSetPadMode(ABI_config_now.A_gpio, ABI_config_now.A_pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF)); + palSetPadMode(ABI_config_now.B_gpio, ABI_config_now.B_pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF)); + palSetPadMode(ABI_config_now.I_gpio, ABI_config_now.I_pin, PAL_MODE_INPUT_PULLUP); -// Enable timer clock + // Enable timer clock HW_ENC_TIM_CLK_EN(); // Enable SYSCFG clock @@ -92,7 +85,6 @@ encoder_ret_t enc_abi_init(ABI_config_t *abi_config) { EXTI_InitStructure.EXTI_LineCmd = ENABLE; EXTI_Init(&EXTI_InitStructure); - abi_config->is_init = 1; ABI_config_now = *abi_config; // Enable and set EXTI Line Interrupt to the highest priority diff --git a/encoder/enc_ad2s1205.c b/encoder/enc_ad2s1205.c index 5ecc2653..bfa10d4c 100644 --- a/encoder/enc_ad2s1205.c +++ b/encoder/enc_ad2s1205.c @@ -57,8 +57,6 @@ void enc_ad2s1205_deinit(void) { #if defined(AD2S1205_RDVEL_GPIO) palSetPadMode(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN, PAL_MODE_INPUT_PULLUP); // Will always read position #endif - - AD2S1205_config_now.is_init = 0; } encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config) { @@ -77,12 +75,12 @@ encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config) { palSetPadMode(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); palSetPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN); // Prepare for a falling edge SAMPLE assertion #endif + #if defined(AD2S1205_RDVEL_GPIO) palSetPadMode(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN); // Will always read position #endif - AD2S1205_config->is_init = 1; AD2S1205_config_now = *AD2S1205_config; nvicEnableVector(HW_ENC_TIM_ISR_CH, 6); @@ -101,8 +99,8 @@ void enc_ad2s1205_routine(float rate) { palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN); // Always read position #endif - palSetPad(AD2S1205_config_now.sw_spi.sck_gpio, - AD2S1205_config_now.sw_spi.sck_pin); + palSetPad(AD2S1205_config_now.sw_spi.sck_gpio, AD2S1205_config_now.sw_spi.sck_pin); + spi_bb_delay(); spi_bb_begin(&(AD2S1205_config_now.sw_spi)); // CS uses the same mcu pin as AS5047 spi_bb_delay(); @@ -166,7 +164,6 @@ void enc_ad2s1205_routine(float rate) { last_enc_angle = ((float) pos * 360.0) / 4096.0; } } - } float enc_ad2s1205_resolver_loss_of_tracking_error_rate(void) { diff --git a/encoder/enc_as504x.c b/encoder/enc_as504x.c index 6826f1e5..ffe0d9a1 100644 --- a/encoder/enc_as504x.c +++ b/encoder/enc_as504x.c @@ -27,6 +27,8 @@ #include "mc_interface.h" #include "utils.h" #include "spi_bb.h" + +#include #include #define AS504x_SPI_READ_BIT 0x4000 @@ -52,141 +54,104 @@ #define AS504x_DATA_INVALID_THRESHOLD 20000 #define AS504x_REFRESH_DIAG_AFTER_NSAMPLES 100 -static AS504x_config_t AS504x_config_now = { 0 }; - -//Private variables -#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS -static uint16_t AS504x_diag_fetch_now_count = 0; -#endif -#if !(AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS) -static uint32_t AS504x_data_last_invalid_counter = 0; -#endif -static uint32_t AS504x_spi_communication_error_count = 0; -static uint8_t spi_data_err_raised = 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 -#if (AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS) static void long_delay(void); -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); -#endif -static void AS504x_determinate_if_connected(bool was_last_valid); +static uint8_t AS504x_fetch_diag(AS504x_config_t *cfg); +static uint8_t AS504x_verify_serial(AS504x_config_t *cfg); +static void AS504x_deserialize_diag(AS504x_config_t *cfg); +static void AS504x_fetch_clear_err_diag(AS504x_config_t *cfg); +static uint8_t AS504x_spi_transfer_err_check(spi_bb_state *sw_spi, + uint16_t *in_buf, const uint16_t *out_buf, int length); +static void AS504x_determinate_if_connected(AS504x_config_t *cfg, bool was_last_valid); -void enc_as504x_routine(float rate) { - uint16_t pos; -// if MOSI is defined, use diagnostics -#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS - spi_bb_begin(&(AS504x_config_now.sw_spi)); - spi_bb_transfer_16(&(AS504x_config_now.sw_spi), 0, 0, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); - - long_delay(); - - spi_bb_begin(&(AS504x_config_now.sw_spi)); - spi_data_err_raised = AS504x_spi_transfer_err_check(&pos, 0, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); - 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(); - - 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_bb_begin(&(AS504x_config_now.sw_spi)); - spi_bb_transfer_16(&(AS504x_config_now.sw_spi), &pos, 0, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); - spi_val = pos; - - if(0x0000 == pos || 0xFFFF == pos) { - AS504x_data_last_invalid_counter++; - } else { - AS504x_data_last_invalid_counter = 0; - AS504x_determinate_if_connected(true); - } - - if (AS504x_data_last_invalid_counter >= AS504x_DATA_INVALID_THRESHOLD) { - AS504x_determinate_if_connected(false); - AS504x_data_last_invalid_counter = AS504x_DATA_INVALID_THRESHOLD; - } -#endif - - if (spi_bb_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.0 / rate); - } else { - ++spi_error_cnt; - UTILS_LP_FAST(spi_error_rate, 1.0, 1. / rate); - } -} - -void enc_as504x_deinit(void) { - nvicDisableVector(HW_ENC_EXTI_CH); - nvicDisableVector(HW_ENC_TIM_ISR_CH); - - TIM_DeInit(HW_ENC_TIM); - - spi_bb_deinit(&(AS504x_config_now.sw_spi)); - -#ifdef HW_SPI_DEV - spiStop(&HW_SPI_DEV); -#endif - - AS504x_config_now.is_init = 0; - last_enc_angle = 0.0; - spi_error_rate = 0.0; -} - -encoder_ret_t enc_as504x_init(AS504x_config_t *AS504x_config) { - AS504x_config_now = *AS504x_config; - spi_bb_init(&(AS504x_config_now.sw_spi)); - - AS504x_config->is_init = 1; - AS504x_config_now = *AS504x_config; - - nvicEnableVector(HW_ENC_TIM_ISR_CH, 6); - - spi_error_rate = 0.0; +encoder_ret_t enc_as504x_init(AS504x_config_t *cfg) { + memset(&cfg->state, 0, sizeof(AS504x_state)); + spi_bb_init(&(cfg->sw_spi)); return ENCODER_OK; } -#if (AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS) +void enc_as504x_deinit(AS504x_config_t *cfg) { + spi_bb_deinit(&(cfg->sw_spi)); + cfg->state.last_enc_angle = 0.0; + cfg->state.spi_error_rate = 0.0; +} + +void enc_as504x_routine(AS504x_config_t *cfg, float rate) { + uint16_t pos; + + // if MOSI is defined, use diagnostics + if (cfg->sw_spi.mosi_gpio != 0) { + spi_bb_begin(&(cfg->sw_spi)); + spi_bb_transfer_16(&(cfg->sw_spi), 0, 0, 1); + spi_bb_end(&(cfg->sw_spi)); + + long_delay(); + + spi_bb_begin(&(cfg->sw_spi)); + cfg->state.spi_data_err_raised = AS504x_spi_transfer_err_check(&cfg->sw_spi, &pos, 0, 1); + spi_bb_end(&(cfg->sw_spi)); + cfg->state.spi_val = pos; + + // get diagnostic every AS504x_REFRESH_DIAG_AFTER_NSAMPLES + cfg->state.diag_fetch_now_count++; + if (cfg->state.diag_fetch_now_count >= AS504x_REFRESH_DIAG_AFTER_NSAMPLES || + cfg->state.spi_data_err_raised) { + // clear error flags before getting new diagnostics data + AS504x_fetch_clear_err_diag(cfg); + + if (!AS504x_fetch_diag(cfg)) { + if (!AS504x_verify_serial(cfg)) { + AS504x_deserialize_diag(cfg); + AS504x_determinate_if_connected(cfg, true); + } else { + AS504x_determinate_if_connected(cfg, false); + } + } else { + AS504x_determinate_if_connected(cfg, false); + } + cfg->state.diag_fetch_now_count = 0; + } + } else { + spi_bb_begin(&(cfg->sw_spi)); + spi_bb_transfer_16(&(cfg->sw_spi), &pos, 0, 1); + spi_bb_end(&(cfg->sw_spi)); + cfg->state.spi_val = pos; + + if(0x0000 == pos || 0xFFFF == pos) { + cfg->state.data_last_invalid_counter++; + } else { + cfg->state.data_last_invalid_counter = 0; + AS504x_determinate_if_connected(cfg, true); + } + + if (cfg->state.data_last_invalid_counter >= AS504x_DATA_INVALID_THRESHOLD) { + AS504x_determinate_if_connected(cfg, false); + cfg->state.data_last_invalid_counter = AS504x_DATA_INVALID_THRESHOLD; + } + } + + if (spi_bb_check_parity(pos) && !cfg->state.spi_data_err_raised) { + pos &= 0x3FFF; + cfg->state.last_enc_angle = ((float) pos * 360.0) / 16384.0; + UTILS_LP_FAST(cfg->state.spi_error_rate, 0.0, 1.0 / rate); + } else { + ++cfg->state.spi_error_cnt; + UTILS_LP_FAST(cfg->state.spi_error_rate, 1.0, 1. / rate); + } +} + static void long_delay(void) { for (volatile int i = 0; i < 40; i++) { __NOP(); } } -static uint8_t AS504x_verify_serial() { +static uint8_t AS504x_verify_serial(AS504x_config_t *cfg) { uint16_t serial_diag_flgs, serial_magnitude, test_magnitude; uint8_t test_AGC_value, test_is_Comp_high, test_is_Comp_low; - serial_magnitude = enc_as504x_get_diag().serial_magnitude; - serial_diag_flgs = enc_as504x_get_diag().serial_diag_flgs; + serial_magnitude = cfg->state.sensor_diag.serial_magnitude; + serial_diag_flgs = cfg->state.sensor_diag.serial_diag_flgs; test_magnitude = serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; @@ -206,70 +171,70 @@ static uint8_t AS504x_verify_serial() { return 0; } -static uint8_t AS504x_fetch_diag(void) { +static uint8_t AS504x_fetch_diag(AS504x_config_t *cfg) { uint16_t recf[2], senf[2] = { AS504x_SPI_READ_DIAG_MSG, AS504x_SPI_READ_MAGN_MSG }; uint8_t ret = 0; - spi_bb_begin(&(AS504x_config_now.sw_spi)); - spi_bb_transfer_16(&(AS504x_config_now.sw_spi), 0, senf, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); + spi_bb_begin(&(cfg->sw_spi)); + spi_bb_transfer_16(&(cfg->sw_spi), 0, senf, 1); + spi_bb_end(&(cfg->sw_spi)); long_delay(); - spi_bb_begin(&(AS504x_config_now.sw_spi)); - ret |= AS504x_spi_transfer_err_check(recf, senf + 1, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); + spi_bb_begin(&(cfg->sw_spi)); + ret |= AS504x_spi_transfer_err_check(&(cfg->sw_spi), recf, senf + 1, 1); + spi_bb_end(&(cfg->sw_spi)); long_delay(); - spi_bb_begin(&(AS504x_config_now.sw_spi)); - ret |= AS504x_spi_transfer_err_check(recf + 1, 0, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); + spi_bb_begin(&(cfg->sw_spi)); + ret |= AS504x_spi_transfer_err_check(&(cfg->sw_spi), recf + 1, 0, 1); + spi_bb_end(&(cfg->sw_spi)); if (!ret) { if (spi_bb_check_parity(recf[0]) && spi_bb_check_parity(recf[1])) { - AS504x_sensor_diag.serial_diag_flgs = recf[0]; - AS504x_sensor_diag.serial_magnitude = recf[1]; + cfg->state.sensor_diag.serial_diag_flgs = recf[0]; + cfg->state.sensor_diag.serial_magnitude = recf[1]; } } return ret; } -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 +static void AS504x_deserialize_diag(AS504x_config_t *cfg) { + cfg->state.sensor_diag.AGC_value = cfg->state.sensor_diag.serial_diag_flgs; + cfg->state.sensor_diag.is_OCF = (cfg->state.sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_OCF_BIT_POS) & 1; - AS504x_sensor_diag.is_COF = (AS504x_sensor_diag.serial_diag_flgs + cfg->state.sensor_diag.is_COF = (cfg->state.sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COF_BIT_POS) & 1; - AS504x_sensor_diag.is_Comp_low = (AS504x_sensor_diag.serial_diag_flgs + cfg->state.sensor_diag.is_Comp_low = (cfg->state.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 + cfg->state.sensor_diag.is_Comp_high = (cfg->state.sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS) & 1; - AS504x_sensor_diag.magnitude = AS504x_sensor_diag.serial_magnitude + cfg->state.sensor_diag.magnitude = cfg->state.sensor_diag.serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; } -static void AS504x_fetch_clear_err_diag() { +static void AS504x_fetch_clear_err_diag(AS504x_config_t *cfg) { uint16_t recf, senf = AS504x_SPI_READ_CLEAR_ERROR_MSG; - spi_bb_begin(&(AS504x_config_now.sw_spi)); - spi_bb_transfer_16(&(AS504x_config_now.sw_spi), 0, &senf, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); + spi_bb_begin(&(cfg->sw_spi)); + spi_bb_transfer_16(&(cfg->sw_spi), 0, &senf, 1); + spi_bb_end(&(cfg->sw_spi)); long_delay(); - spi_bb_begin(&(AS504x_config_now.sw_spi)); - spi_bb_transfer_16(&(AS504x_config_now.sw_spi), &recf, 0, 1); - spi_bb_end(&(AS504x_config_now.sw_spi)); + spi_bb_begin(&(cfg->sw_spi)); + spi_bb_transfer_16(&(cfg->sw_spi), &recf, 0, 1); + spi_bb_end(&(cfg->sw_spi)); - AS504x_sensor_diag.serial_error_flags = recf; + cfg->state.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) { - spi_bb_transfer_16(&(AS504x_config_now.sw_spi) ,in_buf, out_buf, length); +static uint8_t AS504x_spi_transfer_err_check(spi_bb_state *sw_spi, + uint16_t *in_buf, const uint16_t *out_buf, int length) { + spi_bb_transfer_16(sw_spi, in_buf, out_buf, length); for (int len_count = 0; len_count < length; len_count++) { if (((in_buf[len_count]) >> 14) & 0b01) { @@ -279,43 +244,22 @@ static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf, return 0; } -#endif -static void AS504x_determinate_if_connected(bool was_last_valid) { +static void AS504x_determinate_if_connected(AS504x_config_t *cfg, bool was_last_valid) { if (!was_last_valid) { - AS504x_spi_communication_error_count++; + cfg->state.spi_communication_error_count++; - if (AS504x_spi_communication_error_count + if (cfg->state.spi_communication_error_count >= AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) { - AS504x_spi_communication_error_count = + cfg->state.spi_communication_error_count = AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD; - AS504x_sensor_diag.is_connected = 0; + cfg->state.sensor_diag.is_connected = 0; } } else { - if (AS504x_spi_communication_error_count) { - AS504x_spi_communication_error_count--; + if (cfg->state.spi_communication_error_count) { + cfg->state.spi_communication_error_count--; } else { - AS504x_sensor_diag.is_connected = 1; + cfg->state.sensor_diag.is_connected = 1; } } } - -AS504x_diag enc_as504x_get_diag(void) { - return AS504x_sensor_diag; -} - -float enc_as504x_read_deg(void) { - return last_enc_angle; -} - -uint32_t enc_as504x_spi_get_val(void) { - return spi_val; -} - -uint32_t enc_as504x_spi_get_error_cnt(void) { - return spi_error_cnt; -} - -float enc_as504x_spi_get_error_rate(void) { - return spi_error_rate; -} diff --git a/encoder/enc_as504x.h b/encoder/enc_as504x.h index dd6ff178..dc9ca3d3 100644 --- a/encoder/enc_as504x.h +++ b/encoder/enc_as504x.h @@ -21,18 +21,11 @@ #ifndef ENC_AS504X_H_ #define ENC_AS504X_H_ -#include "datatypes.h" #include "encoder/encoder_datatype.h" -void enc_as504x_deinit(void); +// Functions encoder_ret_t enc_as504x_init(AS504x_config_t *AS504x_config); - -float enc_as504x_read_deg(void); -void enc_as504x_routine(float rate); - -AS504x_diag enc_as504x_get_diag(void); -float enc_as504x_spi_get_error_rate(void); -uint32_t enc_as504x_spi_get_val(void); -uint32_t enc_as504x_spi_get_error_cnt(void); +void enc_as504x_deinit(AS504x_config_t *cfg); +void enc_as504x_routine(AS504x_config_t *cfg, float rate); #endif /* ENC_AS504X_H_ */ diff --git a/encoder/enc_mt6816.c b/encoder/enc_mt6816.c index 1eba1f16..3c50294e 100644 --- a/encoder/enc_mt6816.c +++ b/encoder/enc_mt6816.c @@ -41,71 +41,51 @@ static float last_enc_angle = 0.0; static uint32_t spi_error_cnt = 0; static uint32_t spi_val = 0; -void enc_mt6816_deinit(void) { - - nvicDisableVector(HW_ENC_EXTI_CH); - nvicDisableVector(HW_ENC_TIM_ISR_CH); - - TIM_DeInit(HW_ENC_TIM); - - palSetPadMode(MT6816_config_now.sw_spi.miso_gpio, - MT6816_config_now.sw_spi.miso_pin, PAL_MODE_INPUT_PULLUP); - palSetPadMode(MT6816_config_now.sw_spi.sck_gpio, - MT6816_config_now.sw_spi.sck_pin, PAL_MODE_INPUT_PULLUP); - palSetPadMode(MT6816_config_now.sw_spi.nss_gpio, - MT6816_config_now.sw_spi.nss_pin, PAL_MODE_INPUT_PULLUP); - -#if (MT6816_USE_HW_SPI_PINS) - palSetPadMode(MT6816_config_now.sw_spi.mosi_gpio, MT6816_config_now.sw_spi.mosi_pin, PAL_MODE_INPUT_PULLUP); -#endif - -#ifdef HW_SPI_DEV - spiStop(&HW_SPI_DEV); -#endif - - palSetPadMode(MT6816_config_now.sw_spi.miso_gpio, - MT6816_config_now.sw_spi.miso_pin, PAL_MODE_INPUT_PULLUP); - palSetPadMode(MT6816_config_now.sw_spi.sck_gpio, - MT6816_config_now.sw_spi.sck_pin, PAL_MODE_INPUT_PULLUP); - - MT6816_config_now.is_init = 0; - last_enc_angle = 0.0; - spi_error_rate = 0.0; -} - encoder_ret_t enc_mt6816_init(MT6816_config_t *mt6816_config) { -#ifdef HW_SPI_DEV + if (mt6816_config->spi_dev == NULL) { + return ENCODER_ERROR; + } + MT6816_config_now = *mt6816_config; - palSetPadMode(MT6816_config_now.sw_spi.sck_gpio, - MT6816_config_now.sw_spi.sck_pin, + palSetPadMode(MT6816_config_now.sck_gpio, MT6816_config_now.sck_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); - palSetPadMode(MT6816_config_now.sw_spi.miso_gpio, - MT6816_config_now.sw_spi.miso_pin, + palSetPadMode(MT6816_config_now.miso_gpio, MT6816_config_now.miso_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); - spi_bb_nss_init(&(MT6816_config_now.sw_spi)); + palSetPadMode(MT6816_config_now.nss_gpio, MT6816_config_now.nss_pin, + PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); -#if (MT6816_USE_HW_SPI_PINS) - palSetPadMode(MT6816_config_now.sw_spi.mosi_gpio, MT6816_config_now.sw_spi.mosi_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); -#endif + palSetPadMode(MT6816_config_now.mosi_gpio, MT6816_config_now.mosi_pin, + PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); - //Start driver with MT6816 SPI settings - - spiStart(&HW_SPI_DEV, &(MT6816_config_now.hw_spi_cfg)); + spiStart(MT6816_config_now.spi_dev, &(MT6816_config_now.hw_spi_cfg)); 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; - return ENCODER_OK; -#else - (void)mt6816_config; - return ENCODER_ERROR; -#endif +} + +void enc_mt6816_deinit(void) { + if (MT6816_config_now.spi_dev == NULL) { + return; + } + + palSetPadMode(MT6816_config_now.miso_gpio, + MT6816_config_now.miso_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(MT6816_config_now.sck_gpio, + MT6816_config_now.sck_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(MT6816_config_now.nss_gpio, + MT6816_config_now.nss_pin, PAL_MODE_INPUT_PULLUP); + + palSetPadMode(MT6816_config_now.mosi_gpio, MT6816_config_now.mosi_pin, PAL_MODE_INPUT_PULLUP); + + spiStop(MT6816_config_now.spi_dev); + + last_enc_angle = 0.0; + spi_error_rate = 0.0; } float enc_mt6816_read_deg(void) { @@ -113,20 +93,22 @@ float enc_mt6816_read_deg(void) { } void enc_mt6816_routine(float rate) { -#ifdef HW_SPI_DEV 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_bb_begin(&(MT6816_config_now.sw_spi)); - reg_data_03 = spiPolledExchange(&HW_SPI_DEV, reg_addr_03); - spi_bb_end(&(MT6816_config_now.sw_spi)); +#define SPI_BEGIN() spi_bb_delay(); palClearPad(MT6816_config_now.nss_gpio, MT6816_config_now.nss_pin); spi_bb_delay(); +#define SPI_END() spi_bb_delay(); palSetPad(MT6816_config_now.nss_gpio, MT6816_config_now.nss_pin); spi_bb_delay(); + + SPI_BEGIN(); + reg_data_03 = spiPolledExchange(MT6816_config_now.spi_dev, reg_addr_03); + SPI_END(); spi_bb_delay(); - spi_bb_begin(&(MT6816_config_now.sw_spi)); - reg_data_04 = spiPolledExchange(&HW_SPI_DEV, reg_addr_04); - spi_bb_end(&(MT6816_config_now.sw_spi)); + SPI_BEGIN(); + reg_data_04 = spiPolledExchange(MT6816_config_now.spi_dev, reg_addr_04); + SPI_END(); pos = (reg_data_03 << 8) | reg_data_04; spi_val = pos; @@ -145,9 +127,6 @@ void enc_mt6816_routine(float rate) { ++spi_error_cnt; UTILS_LP_FAST(spi_error_rate, 1.0, 1.0 / rate); } -#else - (void)rate; -#endif } uint32_t enc_mt6816_spi_get_val(void) { diff --git a/encoder/enc_mt6816.h b/encoder/enc_mt6816.h index e4f226b1..848767ac 100644 --- a/encoder/enc_mt6816.h +++ b/encoder/enc_mt6816.h @@ -25,8 +25,8 @@ #include "datatypes.h" #include "encoder/encoder_datatype.h" -void enc_mt6816_deinit(void); encoder_ret_t enc_mt6816_init(MT6816_config_t *mt6816_config); +void enc_mt6816_deinit(void); float enc_mt6816_read_deg(void); void enc_mt6816_routine(float rate); diff --git a/encoder/enc_sincos.c b/encoder/enc_sincos.c index da2ee9b4..37f92fce 100644 --- a/encoder/enc_sincos.c +++ b/encoder/enc_sincos.c @@ -29,7 +29,6 @@ #include #include "hw.h" -//TODO move defines to encoder_hwconf.h #define SINCOS_SAMPLE_RATE_HZ 20000 #define SINCOS_MIN_AMPLITUDE 1.0 // sqrt(sin^2 + cos^2) has to be larger than this #define SINCOS_MAX_AMPLITUDE 1.65 // sqrt(sin^2 + cos^2) has to be smaller than this @@ -45,10 +44,8 @@ static float last_enc_angle = 0.0; void enc_sincos_deinit(void) { last_enc_angle = 0.0; - sincos_signal_low_error_rate = 0.0; sincos_signal_above_max_error_rate = 0.0; - enc_sincos_config_now.is_init = 0; } encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *enc_sincos_config) { @@ -59,8 +56,6 @@ encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *enc_sincos_config) { sincos_signal_low_error_rate = 0.0; sincos_signal_above_max_error_rate = 0.0; last_enc_angle = 0.0; - - enc_sincos_config->is_init = 1; enc_sincos_config_now = *enc_sincos_config; return ENCODER_OK; } diff --git a/encoder/enc_ts5700n8501.c b/encoder/enc_ts5700n8501.c index 8a7e174a..1a5d27dc 100644 --- a/encoder/enc_ts5700n8501.c +++ b/encoder/enc_ts5700n8501.c @@ -44,7 +44,28 @@ static uint32_t spi_val = 0; static float last_enc_angle = 0.0; +encoder_ret_t enc_ts5700n8501_init(TS5700N8501_config_t *ts5700n8501_config) { + if (ts5700n8501_config->sd == NULL) { + return ENCODER_ERROR; + } + + spi_error_rate = 0.0; + spi_error_cnt = 0; + ts5700n8501_is_running = true; + ts5700n8501_stop_now = false; + + ts5700n8501_config_now = *ts5700n8501_config; + + chThdCreateStatic(ts5700n8501_thread_wa, sizeof(ts5700n8501_thread_wa), + NORMALPRIO - 10, ts5700n8501_thread, NULL); + + return ENCODER_OK; +} + void enc_ts5700n8501_deinit(void) { + if (ts5700n8501_config_now.sd == NULL) { + return; + } ts5700n8501_stop_now = true; while (ts5700n8501_is_running) { @@ -57,37 +78,12 @@ void enc_ts5700n8501_deinit(void) { palSetPadMode(ts5700n8501_config_now.RX_gpio, ts5700n8501_config_now.RX_pin, PAL_MODE_INPUT_PULLUP); -#ifdef HW_ADC_EXT_GPIO palSetPadMode(ts5700n8501_config_now.EXT_gpio, ts5700n8501_config_now.EXT_pin, PAL_MODE_INPUT_ANALOG); -#endif - - ts5700n8501_config_now.is_init = 0; last_enc_angle = 0.0; spi_error_rate = 0.0; } -encoder_ret_t enc_ts5700n8501_init(TS5700N8501_config_t *ts5700n8501_config) { -#ifdef HW_UART_DEV - spi_error_rate = 0.0; - spi_error_cnt = 0; - ts5700n8501_is_running = true; - ts5700n8501_stop_now = false; - - ts5700n8501_config_now = *ts5700n8501_config; - - chThdCreateStatic(ts5700n8501_thread_wa, sizeof(ts5700n8501_thread_wa), - NORMALPRIO - 10, ts5700n8501_thread, NULL); - - ts5700n8501_config_now.is_init = 1; - ts5700n8501_config->is_init = 1; - return ENCODER_OK; -#else - ts5700n8501_config->is_init = 0; - return ENCODER_ERROR; -#endif -} - float enc_ts5700n8501_read_deg(void) { return last_enc_angle; } @@ -138,9 +134,7 @@ static void TS5700N8501_delay_uart(void) { */ static void TS5700N8501_send_byte(uint8_t b) { utils_sys_lock_cnt(); -#ifdef HW_ADC_EXT_GPIO palSetPad(ts5700n8501_config_now.EXT_gpio, ts5700n8501_config_now.EXT_pin); -#endif TS5700N8501_delay_uart(); palWritePad(ts5700n8501_config_now.TX_gpio, ts5700n8501_config_now.TX_pin, 0); @@ -186,9 +180,7 @@ static void TS5700N8501_send_byte(uint8_t b) { palWritePad(ts5700n8501_config_now.TX_gpio, ts5700n8501_config_now.TX_pin, 1); TS5700N8501_delay_uart(); -#ifdef HW_ADC_EXT_GPIO palClearPad(ts5700n8501_config_now.EXT_gpio, ts5700n8501_config_now.EXT_pin); -#endif utils_sys_unlock_cnt(); } @@ -199,18 +191,16 @@ static THD_FUNCTION(ts5700n8501_thread, arg) { SerialConfig sd_init = ts5700n8501_config_now.uart_param; - sdStart(&HW_UART_DEV, &sd_init); + sdStart(ts5700n8501_config_now.sd, &sd_init); palSetPadMode(ts5700n8501_config_now.TX_gpio, ts5700n8501_config_now.TX_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP); palSetPadMode(ts5700n8501_config_now.RX_gpio, ts5700n8501_config_now.RX_pin, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP); -#ifdef HW_ADC_EXT_GPIO palSetPadMode(ts5700n8501_config_now.EXT_gpio, ts5700n8501_config_now.EXT_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP); -#endif for (;;) { // Check if it is time to stop. @@ -245,12 +235,12 @@ static THD_FUNCTION(ts5700n8501_thread, arg) { uint8_t reply[11]; int reply_ind = 0; - msg_t res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE); + msg_t res = sdGetTimeout(ts5700n8501_config_now.sd, TIME_IMMEDIATE); while (res != MSG_TIMEOUT ) { if (reply_ind < (int) sizeof(reply)) { reply[reply_ind++] = res; } - res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE); + res = sdGetTimeout(ts5700n8501_config_now.sd, TIME_IMMEDIATE); } uint8_t crc = 0; diff --git a/encoder/enc_ts5700n8501.h b/encoder/enc_ts5700n8501.h index 84b50b47..9d9a3b43 100644 --- a/encoder/enc_ts5700n8501.h +++ b/encoder/enc_ts5700n8501.h @@ -24,8 +24,8 @@ #include "datatypes.h" #include "encoder/encoder_datatype.h" -void enc_ts5700n8501_deinit(void); encoder_ret_t enc_ts5700n8501_init(TS5700N8501_config_t *ts5700n8501_config); +void enc_ts5700n8501_deinit(void); float enc_ts5700n8501_read_deg(void); diff --git a/encoder/encoder.c b/encoder/encoder.c index fb0dfc40..9f2424a6 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -30,11 +30,6 @@ #include -#define ENCODER_CONFIG_UNUSED {0} -#define ENCODER_VAR_UNINITIALIZED 0 -#define ENCODER_STRUCT_UNDEFINED {0} -#define ENCODER_ABI_COUNTER_DEFAULT_VALUE 10000ul - #define SPI_BaudRatePrescaler_2 ((uint16_t)0x0000) // 42 MHz 21 MHZ #define SPI_BaudRatePrescaler_4 ((uint16_t)0x0008) // 21 MHz 10.5 MHz #define SPI_BaudRatePrescaler_8 ((uint16_t)0x0010) // 10.5 MHz 5.25 MHz @@ -47,59 +42,92 @@ #define CH_MUTEX_INIT_ZERO {{NULL, NULL}, NULL, NULL} -ENCSPI_config_t encoder_conf_ENCSPI = -{ - ENCODER_VAR_UNINITIALIZED, - {/*BB_SPI*/ - /*NSS*/HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, - /*SCK*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, -#ifdef HW_SPI_PORT_MOSI - /*MOSI*/HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, +AS504x_config_t as504x_cfg = { + { + HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, + HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, +#if defined(HW_SPI_PORT_MOSI) && AS504x_USE_SW_MOSI_PIN + HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, #else - /*MOSI*/0, 0, + 0, 0, #endif - /*MISO*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, - ENCODER_VAR_UNINITIALIZED, - ENCODER_VAR_UNINITIALIZED, + HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, + 0, // has_started + 0, // has_error CH_MUTEX_INIT_ZERO }, + + {0} // State +}; + +AD2S1205_config_t ad2s1205_cfg = { + { // BB_SPI + HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, + HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, +#if defined(HW_SPI_PORT_MOSI) && AS504x_USE_SW_MOSI_PIN + HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, +#else + 0, 0, +#endif + HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, + 0, // has_started + 0, // has_error + CH_MUTEX_INIT_ZERO + }, +}; + +MT6816_config_t mt6816_cfg = { #ifdef HW_SPI_DEV + &HW_SPI_DEV, // spi_dev {//HARDWARE SPI CONFIG - NULL, HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, SPI_BaudRatePrescaler_4 | SPI_CR1_CPOL | SPI_CR1_CPHA - | SPI_DATASIZE_16BIT - } + NULL, HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, SPI_BaudRatePrescaler_4 | + SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_DATASIZE_16BIT + }, + + /*NSS*/HW_SPI_PORT_NSS, HW_SPI_PIN_NSS, + /*SCK*/HW_SPI_PORT_SCK, HW_SPI_PIN_SCK, + /*MOSI*/HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, + /*MISO*/HW_SPI_PORT_MISO, HW_SPI_PIN_MISO, +#else + 0, + {0}, + 0, 0, + 0, 0, + 0, 0, + 0, 0, #endif }; ABI_config_t encoder_conf_ABI = { - ENCODER_VAR_UNINITIALIZED, - ENCODER_ABI_COUNTER_DEFAULT_VALUE, - /*INCREMENTAL PROTOCOL PINOUT*/ - /*A*/HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1 , - /*B*/HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2 + 10000, // counts + HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, + HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, + HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3 }; -ENCSINCOS_config_t encoder_conf_ENCSINCOS = ENCODER_STRUCT_UNDEFINED; +ENCSINCOS_config_t encoder_conf_ENCSINCOS = {0}; -TS5700N8501_config_t encoder_conf_TS5700N8501 = -{ - ENCODER_VAR_UNINITIALIZED, +TS5700N8501_config_t encoder_conf_TS5700N8501 = { +#if defined(HW_ADC_EXT_GPIO) && defined(HW_ADC_EXT_GPIO) + &HW_UART_DEV, /*UART PINOUT*/ HW_UART_TX_PORT, HW_UART_TX_PIN, HW_UART_RX_PORT, HW_UART_RX_PIN, -#ifdef HW_ADC_EXT_GPIO - HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN -#else - ENCODER_VAR_UNINITIALIZED, ENCODER_VAR_UNINITIALIZED -#endif - , - {/*UART CONFIG*/ + HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN, + {// UART CONFIG 2500000, 0, USART_CR2_LINEN, 0 } +#else + 0, + 0, 0, + 0, 0, + 0, 0, + {0} +#endif }; static encoder_type_t encoder_type_now = ENCODER_TYPE_NONE; @@ -110,10 +138,6 @@ static float timer_rate_now = 1.0; static void terminal_encoder(int argc, const char **argv); static void terminal_encoder_clear_errors(int argc, const char **argv); static void terminal_encoder_clear_multiturn(int argc, const char **argv); - -static bool is_uart_defined(void); -static bool is_spi_defined(void); -static bool is_incremental_defined(void); static void timer_start(float rate); void encoder_deinit(void) { @@ -122,7 +146,7 @@ void encoder_deinit(void) { TIM_DeInit(HW_ENC_TIM); if (encoder_type_now == ENCODER_TYPE_AS504x) { - enc_as504x_deinit(); + enc_as504x_deinit(&as504x_cfg); } else if (encoder_type_now == ENCODER_TYPE_MT6816) { enc_mt6816_deinit(); } else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) { @@ -150,17 +174,9 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { SENSOR_PORT_5V(); encoder_conf_ABI.counts = conf->m_encoder_counts; - encoder_ret_t encoder_ret; + encoder_ret_t encoder_ret = enc_abi_init(&encoder_conf_ABI); - if (is_incremental_defined() == false) { - return ENCODER_ERROR; - } - - encoder_conf_ABI.is_init = 0; - - encoder_ret = enc_abi_init(&encoder_conf_ABI); - - if (ENCODER_OK != encoder_ret || !encoder_conf_ABI.is_init) { + if (ENCODER_OK != encoder_ret) { encoder_type_now = ENCODER_TYPE_NONE; index_found = false; return ENCODER_ERROR; @@ -173,20 +189,13 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { case SENSOR_PORT_MODE_AS5047_SPI: { SENSOR_PORT_3V3(); - encoder_ret_t encoder_ret; + encoder_ret_t encoder_ret = enc_as504x_init(&as504x_cfg); - if (is_spi_defined() == false) { - return ENCODER_ERROR; - } - - encoder_conf_ENCSPI.is_init = 0; - - encoder_ret = enc_as504x_init(&encoder_conf_ENCSPI); - - if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSPI.is_init) { + if (ENCODER_OK != encoder_ret) { index_found = false; return ENCODER_ERROR; } + encoder_type_now = ENCODER_TYPE_AS504x; index_found = true; @@ -197,21 +206,15 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { case SENSOR_PORT_MODE_MT6816_SPI: { SENSOR_PORT_5V(); - encoder_ret_t encoder_ret; - if (is_spi_defined() == false) { - return ENCODER_ERROR; - } + encoder_ret_t encoder_ret = enc_mt6816_init(&mt6816_cfg); - encoder_conf_ENCSPI.is_init = 0; - - encoder_ret = enc_mt6816_init(&encoder_conf_ENCSPI); - - if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSPI.is_init) { + if (ENCODER_OK != encoder_ret) { encoder_type_now = ENCODER_TYPE_NONE; index_found = false; return ENCODER_ERROR; } + encoder_type_now = ENCODER_TYPE_MT6816; index_found = true; @@ -222,17 +225,10 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { case SENSOR_PORT_MODE_AD2S1205: { SENSOR_PORT_5V(); - encoder_ret_t encoder_ret; - if (is_spi_defined() == false) { - return ENCODER_ERROR; - } + encoder_ret_t encoder_ret = enc_ad2s1205_init(&ad2s1205_cfg); - encoder_conf_ENCSPI.is_init = 0; - - encoder_ret = enc_ad2s1205_init(&encoder_conf_ENCSPI); - - if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSPI.is_init) { + if (ENCODER_OK != encoder_ret) { encoder_type_now = ENCODER_TYPE_NONE; index_found = false; return ENCODER_ERROR; @@ -254,13 +250,9 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { encoder_conf_ENCSINCOS.c_offset = conf->foc_encoder_cos_offset; encoder_conf_ENCSINCOS.filter_constant = conf->foc_encoder_sincos_filter_constant; - encoder_ret_t encoder_ret; + encoder_ret_t encoder_ret = enc_sincos_init(&encoder_conf_ENCSINCOS); - encoder_conf_ENCSINCOS.is_init = 0; - - encoder_ret = enc_sincos_init(&encoder_conf_ENCSINCOS); - - if (ENCODER_OK != encoder_ret || !encoder_conf_ENCSINCOS.is_init) { + if (ENCODER_OK != encoder_ret) { encoder_type_now = ENCODER_TYPE_NONE; index_found = false; return ENCODER_ERROR; @@ -284,17 +276,9 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { } mempools_free_appconf(appconf); - encoder_ret_t encoder_ret; + encoder_ret_t encoder_ret = enc_ts5700n8501_init(&encoder_conf_TS5700N8501); - if (is_uart_defined() == false) { - return ENCODER_ERROR; - } - - encoder_conf_TS5700N8501.is_init = 0; - - encoder_ret = enc_ts5700n8501_init(&encoder_conf_TS5700N8501); - - if (ENCODER_OK != encoder_ret || !encoder_conf_TS5700N8501.is_init) { + if (ENCODER_OK != encoder_ret) { encoder_type_now = ENCODER_TYPE_NONE; index_found = false; return ENCODER_ERROR; @@ -333,7 +317,7 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) { float encoder_read_deg(void) { if (encoder_type_now == ENCODER_TYPE_AS504x) { - return enc_as504x_read_deg(); + return as504x_cfg.state.last_enc_angle; } else if (encoder_type_now == ENCODER_TYPE_MT6816) { return enc_mt6816_read_deg(); } else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) { @@ -391,7 +375,7 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI && mcpwm_foc_is_using_encoder() && - enc_as504x_spi_get_error_rate() > 0.05) { + as504x_cfg.state.spi_error_rate > 0.05) { mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false); } @@ -416,7 +400,7 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto if (m_conf->motor_type == MOTOR_TYPE_FOC && m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) { - AS504x_diag diag = enc_as504x_get_diag(); + AS504x_diag diag = as504x_cfg.state.sensor_diag; if (!diag.is_connected) { mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false); @@ -475,7 +459,7 @@ void encoder_pin_isr(void) { void encoder_tim_isr(void) { if (encoder_type_now == ENCODER_TYPE_AS504x) { - enc_as504x_routine(timer_rate_now); + enc_as504x_routine(&as504x_cfg, timer_rate_now); } else if (encoder_type_now == ENCODER_TYPE_MT6816) { enc_mt6816_routine(timer_rate_now); } else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) { @@ -500,10 +484,10 @@ static void terminal_encoder(int argc, const char **argv) { AS504x_diag diag = { 0 }; if (encoder_is_configured() == ENCODER_TYPE_AS504x) { - spi_val = enc_as504x_spi_get_val(); - error_rate = enc_as504x_spi_get_error_rate(); - error_cnt = enc_as504x_spi_get_error_cnt(); - diag = enc_as504x_get_diag(); + spi_val = as504x_cfg.state.spi_val; + error_rate = as504x_cfg.state.spi_error_rate; + error_cnt = as504x_cfg.state.spi_communication_error_count; + diag = as504x_cfg.state.sensor_diag; } else if (encoder_is_configured() == ENCODER_TYPE_MT6816) { spi_val = enc_mt6816_spi_get_val(); error_cnt = enc_mt6816_spi_get_error_cnt(); @@ -537,7 +521,7 @@ static void terminal_encoder(int argc, const char **argv) { (double)(enc_mt6816_get_no_magnet_error_rate() * 100.0)); } -#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS +#if AS504x_USE_SW_MOSI_PIN if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) { commands_printf("\nAS5047 DIAGNOSTICS:\n" "AGC : %u\n" @@ -593,31 +577,6 @@ static void terminal_encoder_clear_multiturn(int argc, const char **argv) { commands_printf("Done!\n"); } -static bool is_uart_defined(void) { - if (!encoder_conf_TS5700N8501.RX_gpio - || !encoder_conf_TS5700N8501.TX_gpio) { - return false; - } - return true; -} - -static bool is_spi_defined(void) { - if (!encoder_conf_ENCSPI.sw_spi.miso_gpio - || !encoder_conf_ENCSPI.sw_spi.nss_gpio - || !encoder_conf_ENCSPI.sw_spi.sck_gpio) { - return false; - } - return true; -} - -static bool is_incremental_defined(void) { - if (!encoder_conf_ABI.A_gpio - || !encoder_conf_ABI.B_gpio) { - return false; - } - return true; -} - static void timer_start(float rate) { timer_rate_now = rate; diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h index ed24dfbc..4c1a5001 100644 --- a/encoder/encoder_datatype.h +++ b/encoder/encoder_datatype.h @@ -1,5 +1,6 @@ /* Copyright 2022 Jakub Tomczak + Copyright 2022 Benjamin Vedder benjamin@vedder.se This file is part of the VESC firmware. @@ -41,28 +42,33 @@ typedef enum { } encoder_type_t; typedef struct { - bool is_init; spi_bb_state sw_spi; -#ifdef HW_SPI_DEV - SPIConfig hw_spi_cfg; -#endif -} ENCSPI_config_t; - -typedef ENCSPI_config_t MT6816_config_t; -typedef ENCSPI_config_t AD2S1205_config_t; -typedef ENCSPI_config_t AS504x_config_t; +} AD2S1205_config_t; + +typedef struct { + SPIDriver *spi_dev; + SPIConfig hw_spi_cfg; + stm32_gpio_t *nss_gpio; + int nss_pin; + stm32_gpio_t *sck_gpio; + int sck_pin; + stm32_gpio_t *mosi_gpio; + int mosi_pin; + stm32_gpio_t *miso_gpio; + int miso_pin; +} MT6816_config_t; typedef struct { - bool is_init; uint32_t counts; stm32_gpio_t *A_gpio; uint8_t A_pin; stm32_gpio_t *B_gpio; uint8_t B_pin; + stm32_gpio_t *I_gpio; + uint8_t I_pin; } ABI_config_t; typedef struct { - bool is_init; uint32_t refresh_rate_hz; float s_gain; float s_offset; @@ -72,7 +78,7 @@ typedef struct { } ENCSINCOS_config_t; typedef struct { - bool is_init; + SerialDriver *sd; stm32_gpio_t *TX_gpio; uint8_t TX_pin; stm32_gpio_t *RX_gpio; @@ -81,4 +87,35 @@ typedef struct { uint8_t EXT_pin; SerialConfig uart_param; } TS5700N8501_config_t; + +typedef struct { + uint8_t is_connected; + uint8_t AGC_value; + uint16_t magnitude; + uint8_t is_OCF; + uint8_t is_COF; + uint8_t is_Comp_low; + uint8_t is_Comp_high; + uint16_t serial_diag_flgs; + uint16_t serial_magnitude; + uint16_t serial_error_flags; +} AS504x_diag; + +typedef struct { + uint16_t diag_fetch_now_count; + uint32_t data_last_invalid_counter; + uint32_t spi_communication_error_count; + uint8_t spi_data_err_raised; + AS504x_diag sensor_diag; + uint16_t spi_val; + float last_enc_angle; + uint32_t spi_error_cnt; + float spi_error_rate; +} AS504x_state; + +typedef struct { + spi_bb_state sw_spi; + AS504x_state state; +} AS504x_config_t; + #endif /* ENCODER_ENCODER_DATATYPE_H_ */ diff --git a/spi_bb.c b/spi_bb.c index eabe8f81..a63a935e 100644 --- a/spi_bb.c +++ b/spi_bb.c @@ -22,21 +22,6 @@ // Software SPI -void spi_bb_deinit(spi_bb_state *s) { - chMtxObjectInit(&s->mutex); - - palSetPadMode(s->miso_gpio, s->miso_pin, PAL_MODE_INPUT_PULLUP); - palSetPadMode(s->sck_gpio, s->sck_pin, PAL_MODE_INPUT_PULLUP); - palSetPadMode(s->nss_gpio, s->nss_pin, PAL_MODE_INPUT_PULLUP); - - if (s->mosi_gpio) { // TODO: assure that the gpio is 0x0 when unused - palSetPadMode(s->mosi_gpio, s->mosi_pin, PAL_MODE_INPUT_PULLUP); - } - - s->has_started = false; - s->has_error = false; -} - void spi_bb_init(spi_bb_state *s) { chMtxObjectInit(&s->mutex); @@ -59,13 +44,19 @@ void spi_bb_init(spi_bb_state *s) { s->has_error = false; } -void spi_bb_nss_init(spi_bb_state *s) { +void spi_bb_deinit(spi_bb_state *s) { chMtxObjectInit(&s->mutex); - if (s->nss_gpio) { - palSetPadMode(s->nss_gpio, s->nss_pin, - PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(s->miso_gpio, s->miso_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(s->sck_gpio, s->sck_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(s->nss_gpio, s->nss_pin, PAL_MODE_INPUT_PULLUP); + + if (s->mosi_gpio) { // TODO: assure that the gpio is 0x0 when unused + palSetPadMode(s->mosi_gpio, s->mosi_pin, PAL_MODE_INPUT_PULLUP); } + + s->has_started = false; + s->has_error = false; } uint8_t spi_bb_exchange_8(spi_bb_state *s, uint8_t x) { @@ -117,10 +108,8 @@ void spi_bb_transfer_8(spi_bb_state *s, uint8_t *in_buf, const uint8_t *out_buf, } } -void spi_bb_transfer_16(spi_bb_state *s, uint16_t *in_buf, - const uint16_t *out_buf, int length) { +void spi_bb_transfer_16(spi_bb_state *s, uint16_t *in_buf, const uint16_t *out_buf, int length) { for (int i = 0; i < length; i++) { - uint16_t send = out_buf ? out_buf[i] : 0xFFFF; uint16_t receive = 0; diff --git a/spi_bb.h b/spi_bb.h index 7bedfce1..355eb480 100644 --- a/spi_bb.h +++ b/spi_bb.h @@ -39,9 +39,8 @@ typedef struct { mutex_t mutex; } spi_bb_state; -void spi_bb_deinit(spi_bb_state *s); void spi_bb_init(spi_bb_state *s); -void spi_bb_nss_init(spi_bb_state *s); +void spi_bb_deinit(spi_bb_state *s); uint8_t spi_bb_exchange_8(spi_bb_state *s, uint8_t x); void spi_bb_transfer_8(spi_bb_state *s, uint8_t *in_buf, const uint8_t *out_buf, int length); void spi_bb_transfer_16(spi_bb_state *s, uint16_t *in_buf, const uint16_t *out_buf, int length);