From 11e206231221c9db13cc07866cb2b9fd5df45240 Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Fri, 30 Jul 2021 20:53:22 +0200 Subject: [PATCH] Pull request cleanup --- conf_general.h | 3 +- datatypes.h | 6 -- encoder.c | 145 ++++++++++++++++++++++--------------------------- encoder.h | 8 +-- mc_interface.c | 8 ++- terminal.c | 56 ++++++------------- 6 files changed, 92 insertions(+), 134 deletions(-) diff --git a/conf_general.h b/conf_general.h index b01e9479..858640a5 100755 --- a/conf_general.h +++ b/conf_general.h @@ -24,7 +24,7 @@ #define FW_VERSION_MAJOR 5 #define FW_VERSION_MINOR 03 // Set to 0 for building a release and iterate during beta test builds -#define FW_TEST_VERSION_NUMBER 45 +#define FW_TEST_VERSION_NUMBER 46 #include "datatypes.h" @@ -288,7 +288,6 @@ #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 eb6406a2..e3aabf05 100644 --- a/datatypes.h +++ b/datatypes.h @@ -63,12 +63,6 @@ typedef enum { FOC_SENSOR_MODE_HFI_START } mc_foc_sensor_mode; - -typedef enum { - LAST_PACKET_NOT_OK = 0, - LAST_PACKET_OK -}AS504x_last_packet_status; - // Auxiliary output mode typedef enum { OUT_AUX_MODE_OFF = 0, diff --git a/encoder.c b/encoder.c index 8e9bddf1..d88e3465 100644 --- a/encoder.c +++ b/encoder.c @@ -61,8 +61,13 @@ #define SPI_SW_MISO_GPIO HW_HALL_ENC_GPIO2 #define SPI_SW_MISO_PIN HW_HALL_ENC_PIN2 #if AS504x_USE_SW_MOSI_PIN +#ifdef HW_SPI_SW_MOSI_GPIO #define SPI_SW_MOSI_GPIO HW_SPI_SW_MOSI_GPIO #define SPI_SW_MOSI_PIN HW_SPI_SW_MOSI_PIN +#else +#define SPI_SW_MOSI_GPIO HW_SPI_PORT_MOSI +#define SPI_SW_MOSI_PIN HW_SPI_PIN_MOSI +#endif #endif #define SPI_SW_SCK_GPIO HW_HALL_ENC_GPIO1 #define SPI_SW_SCK_PIN HW_HALL_ENC_PIN1 @@ -84,7 +89,6 @@ typedef enum { // Private variables static bool index_found = false; static uint32_t enc_counts = 10000; -static uint16_t AS504x_diag_fetch_now_count = 0; static encoder_mode mode = ENCODER_MODE_NONE; static float last_enc_angle = 0.0; static uint32_t spi_val = 0; @@ -99,8 +103,8 @@ static float resolver_loss_of_signal_error_rate = 0.0; static uint32_t resolver_loss_of_tracking_error_cnt = 0; static uint32_t resolver_degradation_of_signal_error_cnt = 0; static uint32_t resolver_loss_of_signal_error_cnt = 0; -static uint32_t AS504x_data_last_invalid_counter = 0; static uint32_t AS504x_spi_communication_error_count = 0; +static AS504x_diag AS504x_sensor_diag; static float sin_gain = 0.0; static float sin_offset = 0.0; @@ -155,14 +159,20 @@ static void spi_delay(void); static void spi_AS5047_cs_delay(void); static void TS5700N8501_send_byte(uint8_t b); -static uint8_t spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length); static void encoder_AS504x_update_connected_diag(uint8_t is_connected); +static void encoder_AS504x_determinate_if_connected(bool was_last_valid); + +#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS +static uint16_t AS504x_diag_fetch_now_count = 0; + static uint8_t encoder_AS504x_fetch_diag(void); -static void encoder_AS504x_fetch_clear_err_diag(void); static uint8_t encoder_AS504x_verify_serial(void); static void encoder_AS504x_deserialize_diag(void); -static void encoder_AS504x_determinate_if_connected(AS504x_last_packet_status was_last_valid); -static void encoder_AS504x_verify_magnet(void); +static void encoder_AS504x_fetch_clear_err_diag(void); +static uint8_t spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length); +#else +static uint32_t AS504x_data_last_invalid_counter = 0; +#endif uint32_t encoder_spi_get_error_cnt(void) { return spi_error_cnt; @@ -530,14 +540,12 @@ float encoder_read_deg(void) { ++sincos_signal_above_max_error_cnt; UTILS_LP_FAST(sincos_signal_above_max_error_rate, 1.0, 1./SINCOS_SAMPLE_RATE_HZ); angle = last_enc_angle; - } - else { + } else { if (module < SQ(SINCOS_MIN_AMPLITUDE)) { ++sincos_signal_below_min_error_cnt; UTILS_LP_FAST(sincos_signal_low_error_rate, 1.0, 1./SINCOS_SAMPLE_RATE_HZ); angle = last_enc_angle; - } - else { + } else { UTILS_LP_FAST(sincos_signal_above_max_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ); UTILS_LP_FAST(sincos_signal_low_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ); @@ -621,17 +629,12 @@ bool spi_check_parity(uint16_t x) { return (~x) & 1; } -/** - * Timer interrupt - */ - - -void encoder_AS504x_update_connected_diag(uint8_t is_connected) -{ +void encoder_AS504x_update_connected_diag(uint8_t is_connected) { AS504x_sensor_diag.is_connected = is_connected; } -uint8_t encoder_AS504x_fetch_diag(void) { +#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS +static uint8_t encoder_AS504x_fetch_diag(void) { uint16_t recf[2], senf[2] = {AS504x_SPI_READ_DIAG_MSG, AS504x_SPI_READ_MAGN_MSG}; uint8_t ret = 0; @@ -652,8 +655,6 @@ uint8_t encoder_AS504x_fetch_diag(void) { ret |= spi_transfer_err_check(recf + 1, 0, 1); spi_end(); - spi_AS5047_cs_delay(); - if(!ret) { if (spi_check_parity(recf[0]) && spi_check_parity(recf[1])) { AS504x_sensor_diag.serial_diag_flgs = recf[0]; @@ -664,11 +665,10 @@ uint8_t encoder_AS504x_fetch_diag(void) { return ret; } -#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS /* * This function fetches error flag from AS504x and afterwards clean error flag */ -void encoder_AS504x_fetch_clear_err_diag() { +static void encoder_AS504x_fetch_clear_err_diag() { uint16_t recf, senf = AS504x_SPI_READ_CLEAR_ERROR_MSG; spi_begin(); @@ -676,21 +676,19 @@ void encoder_AS504x_fetch_clear_err_diag() { spi_end(); spi_AS5047_cs_delay(); + spi_begin(); spi_transfer(&recf, 0, 1); spi_end(); - spi_AS5047_cs_delay(); - AS504x_sensor_diag.serial_error_flags = recf; } -#endif /* * Try verify if the diagnostics are not corrupt * This function can prevent deserialazing corrupted data if the MISO bus is HIGH or LOW */ -uint8_t encoder_AS504x_verify_serial() { +static uint8_t encoder_AS504x_verify_serial() { uint16_t serial_diag_flgs, serial_magnitude, test_magnitude; uint8_t test_AGC_value, test_is_Comp_high, test_is_Comp_low; @@ -712,22 +710,7 @@ uint8_t encoder_AS504x_verify_serial() { return 0; } -void encoder_AS504x_verify_magnet() { - mc_fault_code current_fault = mc_interface_get_fault(); - - if(encoder_AS504x_get_diag().is_Comp_high) { - mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, 0, 1); // COMP HIGH - } - else if(encoder_AS504x_get_diag().is_Comp_low) { - mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, 0, 1); // COMP low - } - else if(FAULT_CODE_ENCODER_NO_MAGNET == current_fault || FAULT_CODE_ENCODER_MAGNET_TOO_STRONG == current_fault) { - mc_interface_fault_stop(FAULT_CODE_NONE, 0, 1); - } - -} - -void encoder_AS504x_deserialize_diag() { +static void encoder_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; @@ -736,25 +719,35 @@ void encoder_AS504x_deserialize_diag() { AS504x_sensor_diag.magnitude = AS504x_sensor_diag.serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; } +static uint8_t 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) { + return 1; + } + } + + return 0; +} +#endif + /* * Determinate if is connected depending on last retieved data. */ -void encoder_AS504x_determinate_if_connected(AS504x_last_packet_status was_last_valid) { +void encoder_AS504x_determinate_if_connected(bool was_last_valid) { if(!was_last_valid) { AS504x_spi_communication_error_count+=AS504x_CONNECTION_DETERMINATOR_ERROR_WEIGHT; if(AS504x_spi_communication_error_count >= AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) { - AS504x_spi_communication_error_count = AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD; encoder_AS504x_update_connected_diag(0); mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, 0, 1); } - } - else { + } else { if(AS504x_spi_communication_error_count) { AS504x_spi_communication_error_count--; - } - else { + } else { encoder_AS504x_update_connected_diag(1); if(FAULT_CODE_ENCODER_SPI == mc_interface_get_fault()) { mc_interface_fault_stop(FAULT_CODE_NONE, 0, 1); @@ -770,8 +763,8 @@ AS504x_diag encoder_AS504x_get_diag(void) { void encoder_tim_isr(void) { uint16_t pos; - if(mode == ENCODER_MODE_AS5047P_SPI) { + if(mode == ENCODER_MODE_AS5047P_SPI) { // if MOSI is defined, use diagnostics #if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS spi_begin(); @@ -793,16 +786,20 @@ void encoder_tim_isr(void) { if(!encoder_AS504x_fetch_diag()) { if(!encoder_AS504x_verify_serial()) { - //encoder_AS504x_verify_magnet(); + +// if (encoder_AS504x_get_diag().is_Comp_high) { +// mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, 0, 1); // COMP HIGH +// } else if(encoder_AS504x_get_diag().is_Comp_low) { +// mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, 0, 1); // COMP low +// } + encoder_AS504x_deserialize_diag(); - encoder_AS504x_determinate_if_connected(LAST_PACKET_OK); + encoder_AS504x_determinate_if_connected(true); + } else { + encoder_AS504x_determinate_if_connected(false); } - else { - encoder_AS504x_determinate_if_connected(LAST_PACKET_NOT_OK); - } - } - else { - encoder_AS504x_determinate_if_connected(LAST_PACKET_NOT_OK); + } else { + encoder_AS504x_determinate_if_connected(false); } AS504x_diag_fetch_now_count = 0; } @@ -812,23 +809,19 @@ void encoder_tim_isr(void) { spi_end(); spi_val = pos; - spi_AS5047_cs_delay(); - if(0x0000 == pos || 0xFFFF == pos) { AS504x_data_last_invalid_counter++; - } - else { + } else { AS504x_data_last_invalid_counter = 0; - encoder_AS504x_determinate_if_connected(LAST_PACKET_OK); + encoder_AS504x_determinate_if_connected(true); } - if(AS504x_data_last_invalid_counter >= AS504x_DATA_INVALID_THRESHOLD) { - encoder_AS504x_determinate_if_connected(LAST_PACKET_NOT_OK); + if (AS504x_data_last_invalid_counter >= AS504x_DATA_INVALID_THRESHOLD) { + encoder_AS504x_determinate_if_connected(false); AS504x_data_last_invalid_counter = AS504x_DATA_INVALID_THRESHOLD; } #endif - if(spi_check_parity(pos) && !spi_data_err_raised) { pos &= 0x3FFF; last_enc_angle = ((float)pos * 360.0) / 16384.0; @@ -861,8 +854,7 @@ void encoder_tim_isr(void) { if (pos & MT6816_NO_MAGNET_ERROR_MASK) { ++encoder_no_magnet_error_cnt; UTILS_LP_FAST(encoder_no_magnet_error_rate, 1.0, 1./MT6816_SAMPLE_RATE_HZ); - } - else { + } else { pos = pos >> 2; last_enc_angle = ((float)pos * 360.0) / 16384.0; UTILS_LP_FAST(spi_error_rate, 0.0, 1./MT6816_SAMPLE_RATE_HZ); @@ -875,7 +867,7 @@ void encoder_tim_isr(void) { } #endif - if(mode == RESOLVER_MODE_AD2S1205) { + if (mode == RESOLVER_MODE_AD2S1205) { // 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 @@ -976,7 +968,13 @@ bool encoder_index_found(void) { // Software SPI static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) { 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; +#else + (void)out_buf; +#endif + uint16_t receive = 0; for (int bit = 0;bit < 16;bit++) { @@ -1013,17 +1011,6 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) } } -static uint8_t spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length) { - spi_transfer(in_buf, out_buf, length); - - for(unsigned int len_count = 0; len_count < length; len_count++) - if(((in_buf[len_count]) >> 14) & 0b01) - { - return 1; - } - return 0; -} - static void spi_begin(void) { palClearPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN); spi_AS5047_cs_delay(); diff --git a/encoder.h b/encoder.h index 9e29cad5..11cda3fa 100644 --- a/encoder.h +++ b/encoder.h @@ -60,9 +60,6 @@ void encoder_ts57n8501_reset_errors(void); void encoder_ts57n8501_reset_multiturn(void); AS504x_diag encoder_AS504x_get_diag(void); -static AS504x_diag AS504x_sensor_diag; - - #define AS504x_SPI_READ_BIT 0x4000 #define AS504x_SPI_WRITE_BIT 0x0000 @@ -86,10 +83,7 @@ static AS504x_diag AS504x_sensor_diag; #define AS504x_CONNECTION_DETERMINATOR_ERROR_WEIGHT 4 #define AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD (2 * AS504x_CONNECTION_DETERMINATOR_ERROR_WEIGHT) -#if !(AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS) -#define AS504x_DATA_INVALID_THRESHOLD 20 -#endif - +#define AS504x_DATA_INVALID_THRESHOLD 10000 #define AS504x_REFRESH_DIAG_AFTER_NSAMPLES 100 #endif /* ENCODER_H_ */ diff --git a/mc_interface.c b/mc_interface.c index 72a3a4d9..f2a78c8a 100644 --- a/mc_interface.c +++ b/mc_interface.c @@ -549,8 +549,14 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) { case FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3: return "FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3"; case FAULT_CODE_UNBALANCED_CURRENTS: return "FAULT_CODE_UNBALANCED_CURRENTS"; case FAULT_CODE_BRK: return "FAULT_CODE_BRK"; - default: return "FAULT_UNKNOWN"; break; + case FAULT_CODE_RESOLVER_LOT: return "FAULT_CODE_RESOLVER_LOT"; + case FAULT_CODE_RESOLVER_DOS: return "FAULT_CODE_RESOLVER_DOS"; + case FAULT_CODE_RESOLVER_LOS: return "FAULT_CODE_RESOLVER_LOS"; + case FAULT_CODE_ENCODER_NO_MAGNET: return "FAULT_CODE_ENCODER_NO_MAGNET"; + case FAULT_CODE_ENCODER_MAGNET_TOO_STRONG: return "FAULT_CODE_ENCODER_MAGNET_TOO_STRONG"; } + + return "Unknown fault"; } mc_state mc_interface_get_state(void) { diff --git a/terminal.c b/terminal.c index 682b5adb..89de3455 100644 --- a/terminal.c +++ b/terminal.c @@ -861,20 +861,19 @@ void terminal_process_string(char *str) { } } else if (strcmp(argv[0], "encoder") == 0) { const volatile mc_configuration *mcconf = mc_interface_get_configuration(); + if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI || mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_MT6816_SPI || mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205 || mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501 || mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) { - if(mcconf->m_sensor_port_mode != SENSOR_PORT_MODE_AS5047_SPI) - { + + if (mcconf->m_sensor_port_mode != SENSOR_PORT_MODE_AS5047_SPI) { commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%", (unsigned int)encoder_spi_get_val(), encoder_spi_get_error_cnt(), (double)encoder_spi_get_error_rate() * (double)100.0); - } - else if(argc == 1) - { + } else if (argc == 1) { commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%, Connected: %u", (unsigned int)encoder_spi_get_val(), encoder_spi_get_error_cnt(), @@ -896,41 +895,20 @@ void terminal_process_string(char *str) { encoder_get_no_magnet_error_cnt(), (double)encoder_get_no_magnet_error_rate() * (double)100.0); } + #if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS - if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) - { - if(argc == 1) - { - commands_printf("\nAS5047 DIAGNOSTICS:\nAutomatic Gain Control value: %u\nMagnitude: %u\nCOF: %u\n" - "OCF: %u\nCOMP_low: %u\nCOMP_high: %u\n\ntype \"encoder [name of a parameter]\" for help\n\r" - ,encoder_AS504x_get_diag().AGC_value, encoder_AS504x_get_diag().magnitude, - encoder_AS504x_get_diag().is_COF, encoder_AS504x_get_diag().is_OCF, - encoder_AS504x_get_diag().is_Comp_low, - encoder_AS504x_get_diag().is_Comp_high); - } - else if(argc == 2) - { - if(!strcmp(argv[1], "OCF")) - { - commands_printf("OCF (Offset Compensation Finished)\r\n", - encoder_AS504x_get_diag().is_OCF); - } - else if(!strcmp(argv[1], "COF")) - { - commands_printf("COF (CORDIC Overflow)\r\n", - encoder_AS504x_get_diag().is_COF); - } - else if(!strcmp(argv[1], "COMP_low")) - { - commands_printf("COMP low, indicates a high magnetic field.\r\n", - encoder_AS504x_get_diag().is_Comp_low); - } - else if(!strcmp(argv[1], "COMP_high")) - { - commands_printf("COMP high, indicated a weak magnetic field.\r\n", - encoder_AS504x_get_diag().is_Comp_high); - } - } + if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) { + commands_printf("\nAS5047 DIAGNOSTICS:\n" + "AGC : %u\n" + "Magnitude : %u\n" + "COF : %u\n" + "OCF : %u\n" + "COMP_low : %u\n" + "COMP_high : %u\n", + encoder_AS504x_get_diag().AGC_value, encoder_AS504x_get_diag().magnitude, + encoder_AS504x_get_diag().is_COF, encoder_AS504x_get_diag().is_OCF, + encoder_AS504x_get_diag().is_Comp_low, + encoder_AS504x_get_diag().is_Comp_high); } #endif }