From 9c7b0dae2c72bd14fbb53d1cdde14492d96a0e84 Mon Sep 17 00:00:00 2001 From: Zach O'Brien Date: Mon, 18 Jul 2022 14:25:03 -0600 Subject: [PATCH 1/5] Adding support for the AS5x47U encoder. --- confgenerator.h | 2 +- datatypes.h | 4 +- encoder/enc_as5x47u.c | 375 +++++++++++++++++++++++++++++++++++++ encoder/enc_as5x47u.h | 51 +++++ encoder/enc_mt6816.c | 12 +- encoder/encoder.c | 77 +++++++- encoder/encoder.h | 1 + encoder/encoder.mk | 3 +- encoder/encoder_cfg.c | 29 +++ encoder/encoder_cfg.h | 1 + encoder/encoder_datatype.h | 50 ++++- halconf.h | 7 + hwconf/mcuconf.h | 2 +- motor/mc_interface.c | 1 + 14 files changed, 605 insertions(+), 10 deletions(-) create mode 100644 encoder/enc_as5x47u.c create mode 100644 encoder/enc_as5x47u.h diff --git a/confgenerator.h b/confgenerator.h index ce1a1ff6f..a0567c512 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 3188187738 +#define MCCONF_SIGNATURE 2217312675 #define APPCONF_SIGNATURE 2551442059 // Functions diff --git a/datatypes.h b/datatypes.h index 46bb8409e..2bbc3462e 100644 --- a/datatypes.h +++ b/datatypes.h @@ -154,6 +154,7 @@ typedef enum { FAULT_CODE_ENCODER_NO_MAGNET, FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, FAULT_CODE_PHASE_FILTER, + FAULT_CODE_ENCODER_FAULT, } mc_fault_code; typedef enum { @@ -188,7 +189,8 @@ typedef enum { SENSOR_PORT_MODE_SINCOS, SENSOR_PORT_MODE_TS5700N8501, SENSOR_PORT_MODE_TS5700N8501_MULTITURN, - SENSOR_PORT_MODE_MT6816_SPI + SENSOR_PORT_MODE_MT6816_SPI, + SENSOR_PORT_MODE_AS5x47U_SPI } sensor_port_mode; typedef struct { diff --git a/encoder/enc_as5x47u.c b/encoder/enc_as5x47u.c new file mode 100644 index 000000000..304f6a3bb --- /dev/null +++ b/encoder/enc_as5x47u.c @@ -0,0 +1,375 @@ +/* + Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se + Copyright 2022 Jakub Tomczak + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "enc_as5x47u.h" + +#include "ch.h" +#include "hal.h" +#include "stm32f4xx_conf.h" +#include "hw.h" +#include "mc_interface.h" +#include "utils_math.h" +#include "spi_bb.h" +#include "timer.h" + +#include +#include + +#define AS5x47U_SPI_READ_BIT 0x4000 +#define AS5x47U_SPI_WRITE_BIT 0x0000 + +#define AS5x47U_SPI_DIAG_FUSA_ERROR_BIT_POS 10 +#define AS5x47U_SPI_DIAG_COF_BIT_POS 2 +#define AS5x47U_SPI_DIAG_COMP_LOW_BIT_POS 3 +#define AS5x47U_SPI_DIAG_COMP_HIGH_BIT_POS 4 + +#define AS5x47U_SPI_ERRFL_WDTST_BIT_POS 7 +#define AS5x47U_SPI_ERRFL_CRC_ERROR_BIT_POS 6 +#define AS5x47U_SPI_ERRFL_MAG_HALF_BIT_POS 1 + +#define AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK 0x3FFF +#define AS5x47U_SPI_AGC_MASK 0xFF +#define AS5x47U_SPI_WARN_FLAG_MASK 0x8000 +#define AS5x47U_SPI_ERROR_FLAG_MASK 0x4000 + +#define AS5x47U_SPI_ERRFL_ADR 0x0001 +#define AS5x47U_SPI_DIAG_ADR 0x3FF5 +#define AS5x47U_SPI_MAGN_ADR 0x3FFD +#define AS5x47U_SPI_AGC_ADR 0x3FF9 +#define AS5x47U_SPI_POS_ADR 0x3FFF + +#define AS5x47U_SPI_READ_ERRFL_MSG (AS5x47U_SPI_ERRFL_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_DIAG_MSG (AS5x47U_SPI_DIAG_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_MAGN_MSG (AS5x47U_SPI_MAGN_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_AGC_MSG (AS5x47U_SPI_AGC_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_POS_MSG (AS5x47U_SPI_POS_ADR | AS5x47U_SPI_READ_BIT) + +#define AS5x47U_SPI_READ_ERRFL_CRC (0x06) +#define AS5x47U_SPI_READ_DIAG_CRC (0x6F) +#define AS5x47U_SPI_READ_MAGN_CRC (0x87) +#define AS5x47U_SPI_READ_AGC_CRC (0xF3) +#define AS5x47U_SPI_READ_POS_CRC (0xBD) + +#define AS5x47U_CONNECTION_DETERMINATOR_ERROR_THRESHOLD 5 + +enum { + SPI_SEQ_TX_MAG_RX_POS, + SPI_SEQ_TX_POS_RX_MAG, + SPI_SEQ_TX_AGC_RX_POS, + SPI_SEQ_TX_POS_RX_AGC, + SPI_SEQ_TX_DIAG_RX_POS, + SPI_SEQ_TX_POS_RX_DIAG, + SPI_SEQ_TX_ERRFL_RX_POS, + SPI_SEQ_TX_POS_RX_ERRFL, + SPI_SEQ_PREV_ERR, +}; + +// Private functions +static void AS5x47U_determinate_if_connected(AS5x47U_config_t *cfg, bool was_last_valid); +// static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data); +static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg, + uint16_t tx_data, uint8_t tx_crc); +static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData); + +static uint8_t enc_as5x47u_crc8(const uint8_t *data, size_t len, const uint8_t inital) { + // Lookup table generated using poly 0x1D + static const uint8_t crc8_lookup[] = { + 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, + 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, + 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, + 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, + 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, + 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, + 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, + 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, + 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, + 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, + 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, + 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, + 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, + 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, + 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, + 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4, + }; + uint8_t cksum = inital; + size_t i; + for (i = 0; i < len; i++) { + cksum ^= data[i]; + cksum = crc8_lookup[cksum]; + } + return cksum; +} + +void enc_as5x47u_spi_callback(SPIDriver *pspi) +{ + if (pspi != NULL && pspi->app_arg != NULL) + { + AS5x47U_config_t *cfg = (AS5x47U_config_t*)pspi->app_arg; + spiUnselectI(cfg->spi_dev); + + // Determine time step for error rate calculation + float timestep = timer_seconds_elapsed_since(cfg->state.last_update_time); + if (timestep > 1.0) { + timestep = 1.0; + } + cfg->state.last_update_time = timer_time_now(); + + uint8_t rx_crc = cfg->state.rx_buf[2]; + uint8_t calc_crc = enc_as5x47u_crc8(cfg->state.rx_buf, 2, 0xC4) ^ 0xFF; + uint16_t rx_data = cfg->state.rx_buf[0] << 8 | cfg->state.rx_buf[1]; + if (calc_crc == rx_crc) { + AS5x47U_determinate_if_connected(cfg, true); + cfg->state.sensor_diag.is_error = + (uint8_t)((rx_data & AS5x47U_SPI_ERROR_FLAG_MASK) != 0); + + if (!cfg->state.sensor_diag.is_error) + { + UTILS_LP_FAST(cfg->state.spi_error_rate, 0.0, timestep); + + switch(cfg->state.spi_seq) { + case SPI_SEQ_TX_MAG_RX_POS: { + // Receive position, then request position while receiving magnitude + AS5x47U_process_pos(cfg, rx_data); + cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_MAG; + AS5x47U_start_spi_exchange_precalc_crc( + cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); + } break; + + case SPI_SEQ_TX_POS_RX_MAG: { // Receive magnitude + cfg->state.sensor_diag.serial_magnitude = rx_data; + cfg->state.sensor_diag.magnitude = rx_data + & AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; + cfg->state.spi_seq = SPI_SEQ_TX_AGC_RX_POS; + } break; + + case SPI_SEQ_TX_AGC_RX_POS: { + // Receive position, then request position while receiving AGC + AS5x47U_process_pos(cfg, rx_data); + cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_AGC; + AS5x47U_start_spi_exchange_precalc_crc( + cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); + } break; + + case SPI_SEQ_TX_POS_RX_AGC: { // Receive AGC + cfg->state.sensor_diag.serial_AGC_value = rx_data; + cfg->state.sensor_diag.AGC_value = (uint8_t)rx_data; + cfg->state.spi_seq = SPI_SEQ_TX_DIAG_RX_POS; + } break; + case SPI_SEQ_TX_DIAG_RX_POS: { + // Receive position, then request position while requesting diagnostic flags + AS5x47U_process_pos(cfg, rx_data); + cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_DIAG; + AS5x47U_start_spi_exchange_precalc_crc( + cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); + } break; + + case SPI_SEQ_TX_POS_RX_DIAG: { // Receive diagnostic flags + cfg->state.sensor_diag.serial_diag_flgs = rx_data; + cfg->state.sensor_diag.is_broken_hall = + (rx_data >> AS5x47U_SPI_DIAG_FUSA_ERROR_BIT_POS) & 1; + cfg->state.sensor_diag.is_COF = + (rx_data >> AS5x47U_SPI_DIAG_COF_BIT_POS) & 1; + cfg->state.sensor_diag.is_Comp_low = + (rx_data >> AS5x47U_SPI_DIAG_COMP_LOW_BIT_POS) & 1; + cfg->state.sensor_diag.is_Comp_high = + (rx_data >> AS5x47U_SPI_DIAG_COMP_HIGH_BIT_POS) & 1; + cfg->state.spi_seq = SPI_SEQ_TX_ERRFL_RX_POS; + } break; + + case SPI_SEQ_TX_ERRFL_RX_POS: { + // Receive position, then request pos while receiving error flags + AS5x47U_process_pos(cfg, rx_data); + cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_ERRFL; + AS5x47U_start_spi_exchange_precalc_crc( + cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); + } break; + case SPI_SEQ_TX_POS_RX_ERRFL: { // Receive error flags + cfg->state.spi_seq = 0; + cfg->state.sensor_diag.serial_error_flgs = rx_data; + cfg->state.sensor_diag.is_wdtst = + (rx_data >> AS5x47U_SPI_ERRFL_WDTST_BIT_POS) & 1; + cfg->state.sensor_diag.is_crc_error = + (rx_data >> AS5x47U_SPI_ERRFL_CRC_ERROR_BIT_POS) & 1; + cfg->state.sensor_diag.is_mag_half = + (rx_data >> AS5x47U_SPI_ERRFL_MAG_HALF_BIT_POS) & 1; + cfg->state.spi_seq = SPI_SEQ_TX_MAG_RX_POS; + } break; + // Not sure what was just received, but just requested ERRFL, so ignore rx + // data and prepare to receive ERRFL next exchange + case SPI_SEQ_PREV_ERR: { + cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_ERRFL; + } break; + default: { + // Something went wrong, just start the sequence over. + cfg->state.spi_seq = SPI_SEQ_TX_MAG_RX_POS; + } break; + } + } else { + // Error flag is set + AS5x47U_determinate_if_connected(cfg, true); // Encoder is connected... + UTILS_LP_FAST(cfg->state.spi_error_rate, 1.0, timestep); // But there is an error + ++cfg->state.spi_error_cnt; + // Error flag is set so need to read error register next + cfg->state.spi_seq = SPI_SEQ_PREV_ERR; + } + } else { + // CRC error + AS5x47U_determinate_if_connected(cfg, false); + UTILS_LP_FAST(cfg->state.spi_error_rate, 1.0, timestep); + ++cfg->state.spi_error_cnt; + cfg->state.spi_seq = SPI_SEQ_PREV_ERR; + } + } +} + +static void as5x47u_spi_err_callback(SPIDriver *pspi) +{ + if(pspi != NULL && pspi->app_arg != NULL) + { + AS5x47U_config_t *cfg = (AS5x47U_config_t*)pspi->app_arg; + // Make sure we won't process the data + memset(cfg->state.rx_buf, 0, sizeof(cfg->state.rx_buf)); + } +} + +bool enc_as5x47u_init(AS5x47U_config_t *cfg) { + if (cfg->spi_dev == NULL) { + return false; + } + + memset(&cfg->state, 0, sizeof(AS5x47U_state)); + + palSetPadMode(cfg->sck_gpio, cfg->sck_pin, + PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->miso_gpio, cfg->miso_pin, + PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->nss_gpio, cfg->nss_pin, + PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->mosi_gpio, cfg->mosi_pin, + PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST); + + cfg->spi_dev->app_arg = (void*)cfg; + cfg->spi_dev->err_cb = as5x47u_spi_err_callback; + // cfg->spi_dev->config->end_cb = enc_as5x47u_spi_callback; // Set in encoder_cfg.c + + spiStart(cfg->spi_dev, &(cfg->hw_spi_cfg)); + + cfg->state.spi_error_rate = 0.0; + + return true; +} + +void enc_as5x47u_deinit(AS5x47U_config_t *cfg) { + if (cfg->spi_dev != NULL) { + palSetPadMode(cfg->miso_gpio, cfg->miso_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(cfg->sck_gpio, cfg->sck_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(cfg->nss_gpio, cfg->nss_pin, PAL_MODE_INPUT_PULLUP); + palSetPadMode(cfg->mosi_gpio, cfg->mosi_pin, PAL_MODE_INPUT_PULLUP); + + spiStop(cfg->spi_dev); + + cfg->state.last_enc_angle = 0.0; + cfg->state.spi_error_rate = 0.0; + } +} + +void enc_as5x47u_routine(AS5x47U_config_t *cfg) { + switch(cfg->state.spi_seq) { + case SPI_SEQ_TX_MAG_RX_POS: { // Request magnitude + AS5x47U_start_spi_exchange_precalc_crc( + cfg, AS5x47U_SPI_READ_MAGN_MSG, AS5x47U_SPI_READ_MAGN_CRC); + } break; + + case SPI_SEQ_TX_AGC_RX_POS: { // Request AGC + AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_AGC_MSG, + AS5x47U_SPI_READ_AGC_CRC); + } break; + + case SPI_SEQ_TX_DIAG_RX_POS: { // Request diagnostic flags + AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_DIAG_MSG, + AS5x47U_SPI_READ_DIAG_CRC); + } break; + + case SPI_SEQ_TX_ERRFL_RX_POS: { // Request error flags + AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG, + AS5x47U_SPI_READ_ERRFL_CRC); + } break; + + default: + case SPI_SEQ_PREV_ERR: { // There was a problem of some sort, request ERRFL + AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG, + AS5x47U_SPI_READ_ERRFL_CRC); + } break; + } +} + +static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData) +{ + cfg->state.spi_val = posData; + posData &= AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; + cfg->state.last_enc_angle = (float)(posData * 360) / (float)(1 << 14); +} + +static void AS5x47U_determinate_if_connected(AS5x47U_config_t *cfg, bool was_last_valid) { + if (!was_last_valid) { + cfg->state.spi_communication_error_count++; + + if (cfg->state.spi_communication_error_count >= + AS5x47U_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) { + cfg->state.spi_communication_error_count = + AS5x47U_CONNECTION_DETERMINATOR_ERROR_THRESHOLD; + cfg->state.sensor_diag.is_connected = 0; + } + } else { + if (cfg->state.spi_communication_error_count) { + cfg->state.spi_communication_error_count--; + } else { + cfg->state.sensor_diag.is_connected = 1; + } + } +} + +/* TODO: uncomment if needed +static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data) +{ + uint8_t tx_to_crc[] = {(tx_data >> 8) & 0xFF, (tx_data & 0xFF)}; + uint8_t tx_crc = enc_as5x47u_crc8(tx_to_crc, 2, 0xC4) ^ 0xFF; + AS5x47U_start_spi_exchange_precalc_crc(cfg, tx_data, tx_crc); +} +*/ + +static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg, + uint16_t tx_data, uint8_t tx_crc) +{ + cfg->state.tx_buf[0] = (tx_data >> 8) & 0xFF; + cfg->state.tx_buf[1] = tx_data & 0xFF; + cfg->state.tx_buf[2] = tx_crc; + memset(cfg->state.rx_buf, 0, sizeof(cfg->state.rx_buf)); + + // There is a weird corner case where the DMA may not read all of the Rx data. This + // causes the RXNE flag to be set when an exchange starts, causing the fist byte of + // data received to be from the previous exchange. This is corrected by reading the + // SPI data register, clearing the RXNE flag. + volatile uint32_t test = cfg->spi_dev->spi->DR; + (void)test; // get rid of unused warning + + spiSelectI(cfg->spi_dev); + spiStartExchangeI(cfg->spi_dev, 3, cfg->state.tx_buf, cfg->state.rx_buf); +} diff --git a/encoder/enc_as5x47u.h b/encoder/enc_as5x47u.h new file mode 100644 index 000000000..c498246e0 --- /dev/null +++ b/encoder/enc_as5x47u.h @@ -0,0 +1,51 @@ +/* + Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se + Copyright 2022 Jakub Tomczak + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef ENC_AS5x47U_H_ +#define ENC_AS5x47U_H_ + +#include "encoder/encoder_datatype.h" + +// Functions +/** + * @brief Initalized the SPI peripheral used to communicate with the AS5x47U encoder. + */ +bool enc_as5x47u_init(AS5x47U_config_t *AS504x_config); + +/** + * @brief Resets the SPI peripheral used to communicate with the AS5x47U encoder. + */ +void enc_as5x47u_deinit(AS5x47U_config_t *cfg); + +/** + * @brief Starts a new SPI transaction to get the current position from the position sensor. + * + * Every call of enc_as5x47u_routine ends up doing two SPI trnasactions. The first one + * always receives position, but will request one of MAGN, AGC, DIAG, or ERRFL. The next + * transaction receives the requested register while requesting pos for the next + * transaction. + * @param cfg A pointer to the encoder config struct. + */ +void enc_as5x47u_routine(AS5x47U_config_t *cfg); + +// Macros +#define AS5x47U_LAST_ANGLE(cfg) ((cfg)->state.last_enc_angle) + +#endif /* ENC_AS5x47U_H_ */ diff --git a/encoder/enc_mt6816.c b/encoder/enc_mt6816.c index 3079846d9..9ab6e7749 100644 --- a/encoder/enc_mt6816.c +++ b/encoder/enc_mt6816.c @@ -42,10 +42,14 @@ bool enc_mt6816_init(MT6816_config_t *cfg) { memset(&cfg->state, 0, sizeof(MT6816_state)); - palSetPadMode(cfg->sck_gpio, cfg->sck_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); - palSetPadMode(cfg->miso_gpio, cfg->miso_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); - palSetPadMode(cfg->nss_gpio, cfg->nss_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); - palSetPadMode(cfg->mosi_gpio, cfg->mosi_pin, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->sck_gpio, cfg->sck_pin, + PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->miso_gpio, cfg->miso_pin, + PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->nss_gpio, cfg->nss_pin, + PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); + palSetPadMode(cfg->mosi_gpio, cfg->mosi_pin, + PAL_MODE_ALTERNATE(cfg->spi_af) | PAL_STM32_OSPEED_HIGHEST); spiStart(cfg->spi_dev, &(cfg->hw_spi_cfg)); diff --git a/encoder/encoder.c b/encoder/encoder.c index 432b877f1..d2bef0523 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -149,6 +149,19 @@ bool encoder_init(volatile mc_configuration *conf) { res = true; } break; + case SENSOR_PORT_MODE_AS5x47U_SPI: { + SENSOR_PORT_3V3(); + + if (!enc_as5x47u_init(&encoder_cfg_as5x47u)) { + return false; + } + + encoder_type_now = ENCODER_TYPE_AS5x47U; + timer_start(10000); + + res = true; + } break; + default: SENSOR_PORT_5V(); encoder_type_now = ENCODER_TYPE_NONE; @@ -157,7 +170,7 @@ bool encoder_init(volatile mc_configuration *conf) { terminal_register_command_callback( "encoder", - "Prints the status of the AS5047, AD2S1205, or TS5700N8501 encoder.", + "Prints the status of the AS5047, AS5x47U, AD2S1205, or TS5700N8501 encoder.", 0, terminal_encoder); @@ -193,6 +206,8 @@ void encoder_deinit(void) { enc_sincos_deinit(&encoder_cfg_sincos); } else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) { enc_ts5700n8501_deinit(&encoder_cfg_TS5700N8501); + } else if (encoder_type_now == ENCODER_TYPE_AS5x47U) { + enc_as5x47u_deinit(&encoder_cfg_as5x47u); } encoder_type_now = ENCODER_TYPE_NONE; @@ -211,6 +226,8 @@ float encoder_read_deg(void) { return enc_sincos_read_deg(&encoder_cfg_sincos); } else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) { return enc_ts5700n8501_read_deg(&encoder_cfg_TS5700N8501); + } else if (encoder_type_now == ENCODER_TYPE_AS5x47U) { + return AS5x47U_LAST_ANGLE(&encoder_cfg_as5x47u); } return 0.0; } @@ -312,6 +329,26 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto } break; + case SENSOR_PORT_MODE_AS5x47U_SPI: + if (encoder_cfg_as5x47u.state.spi_error_rate > 0.05) { + mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false); + } + + AS5x47U_diag diag = encoder_cfg_as5x47u.state.sensor_diag; + if (!diag.is_connected) { + mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false); + } + + if (diag.is_Comp_high) { + mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false); + } else if(diag.is_Comp_low) { + mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false); + } else if(diag.is_broken_hall || diag.is_COF || diag.is_wdtst) { + mc_interface_fault_stop(FAULT_CODE_ENCODER_FAULT, is_second_motor, false); + } + + break; + default: break; } @@ -336,6 +373,10 @@ void encoder_tim_isr(void) { enc_ad2s1205_routine(&encoder_cfg_ad2s1205); break; + case ENCODER_TYPE_AS5x47U: + enc_as5x47u_routine(&encoder_cfg_as5x47u); + break; + default: break; } @@ -412,6 +453,40 @@ static void terminal_encoder(int argc, const char **argv) { commands_printf("Index found: %d", encoder_index_found()); break; + case SENSOR_PORT_MODE_AS5x47U_SPI: + commands_printf("SPI AS5x47U encoder value: %d, errors: %d, error rate: %.3f %%", + encoder_cfg_as5x47u.state.spi_val, encoder_cfg_as5x47u.state.spi_communication_error_count, + (double)(encoder_cfg_as5x47u.state.spi_error_rate * 100.0)); + + commands_printf("\nAS5x47U DIAGNOSTICS:\n" + "Connected : %u\n" + "AGC : %u\n" + "Magnitude : %u\n" + "COF : %u\n" + "Hall_Broken : %u\n" + "Error : %u\n" + "COMP_low : %u\n" + "COMP_high : %u\n" + "WatchdogTest: %u\n" + "CRC Error : %u\n" + "MagHalf : %u\n" + "Error Flags : %04X\n" + "Diag Flags : %04X\n", + encoder_cfg_as5x47u.state.sensor_diag.is_connected, + encoder_cfg_as5x47u.state.sensor_diag.AGC_value, + encoder_cfg_as5x47u.state.sensor_diag.magnitude, + encoder_cfg_as5x47u.state.sensor_diag.is_COF, + encoder_cfg_as5x47u.state.sensor_diag.is_broken_hall, + encoder_cfg_as5x47u.state.sensor_diag.is_error, + encoder_cfg_as5x47u.state.sensor_diag.is_Comp_low, + encoder_cfg_as5x47u.state.sensor_diag.is_Comp_high, + encoder_cfg_as5x47u.state.sensor_diag.is_wdtst, + encoder_cfg_as5x47u.state.sensor_diag.is_crc_error, + encoder_cfg_as5x47u.state.sensor_diag.is_mag_half, + encoder_cfg_as5x47u.state.sensor_diag.serial_error_flgs, + encoder_cfg_as5x47u.state.sensor_diag.serial_diag_flgs); + break; + default: commands_printf("No encoder debug info available."); break; diff --git a/encoder/encoder.h b/encoder/encoder.h index 59de2fc9d..a96f1e280 100644 --- a/encoder/encoder.h +++ b/encoder/encoder.h @@ -28,6 +28,7 @@ #include "enc_mt6816.h" #include "enc_sincos.h" #include "enc_as504x.h" +#include "enc_as5x47u.h" #include "enc_abi.h" // Functions diff --git a/encoder/encoder.mk b/encoder/encoder.mk index 930c86545..9c4c95c71 100644 --- a/encoder/encoder.mk +++ b/encoder/encoder.mk @@ -5,6 +5,7 @@ ENCSRC = encoder/encoder.c \ encoder/enc_as504x.c \ encoder/enc_sincos.c \ encoder/enc_mt6816.c \ - encoder/enc_ts5700n8501.c + encoder/enc_ts5700n8501.c \ + encoder/enc_as5x47u.c ENCINC = encoder diff --git a/encoder/encoder_cfg.c b/encoder/encoder_cfg.c index 87fb6aa0c..f93465a8a 100644 --- a/encoder/encoder_cfg.c +++ b/encoder/encoder_cfg.c @@ -33,6 +33,7 @@ static THD_WORKING_AREA(encoder_thread_wa, 512); #define SPI_BaudRatePrescaler_64 ((uint16_t)0x0028) // 1.3125 MHz 656.25 KHz #define SPI_BaudRatePrescaler_128 ((uint16_t)0x0030) // 656.25 KHz 328.125 KHz #define SPI_BaudRatePrescaler_256 ((uint16_t)0x0038) // 328.125 KHz 164.06 KHz +#define SPI_DATASIZE_8BIT 0 #define SPI_DATASIZE_16BIT SPI_CR1_DFF AS504x_config_t encoder_cfg_as504x = { @@ -78,6 +79,7 @@ MT6816_config_t encoder_cfg_mt6816 = { SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_DATASIZE_16BIT }, + HW_SPI_GPIO_AF, /*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, @@ -86,6 +88,7 @@ MT6816_config_t encoder_cfg_mt6816 = { #else 0, {0}, + 0, 0, 0, 0, 0, 0, 0, @@ -136,3 +139,29 @@ TS5700N8501_config_t encoder_cfg_TS5700N8501 = { sizeof(encoder_thread_wa), {0} }; + +void enc_as5x47u_spi_callback(SPIDriver *pspi); +AS5x47U_config_t encoder_cfg_as5x47u = { +#ifdef HW_SPI_DEV + &HW_SPI_DEV, // spi_dev + {//HARDWARE SPI CONFIG + enc_as5x47u_spi_callback, HW_SPI_PORT_NSS, HW_SPI_PIN_NSS, SPI_BaudRatePrescaler_8 | + SPI_CR1_CPHA | SPI_DATASIZE_8BIT + }, + + HW_SPI_GPIO_AF, + /*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, 0, +#endif + {0}, // State +}; diff --git a/encoder/encoder_cfg.h b/encoder/encoder_cfg.h index 5ea07e684..eb5f720e8 100644 --- a/encoder/encoder_cfg.h +++ b/encoder/encoder_cfg.h @@ -29,5 +29,6 @@ extern MT6816_config_t encoder_cfg_mt6816; extern ABI_config_t encoder_cfg_ABI; extern ENCSINCOS_config_t encoder_cfg_sincos; extern TS5700N8501_config_t encoder_cfg_TS5700N8501; +extern AS5x47U_config_t encoder_cfg_as5x47u; #endif /* ENCODER_CFG_H_ */ diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h index d35755eae..cc9157b74 100644 --- a/encoder/encoder_datatype.h +++ b/encoder/encoder_datatype.h @@ -34,7 +34,8 @@ typedef enum { ENCODER_TYPE_AD2S1205_SPI, ENCODER_TYPE_SINCOS, ENCODER_TYPE_TS5700N8501, - ENCODER_TYPE_ABI + ENCODER_TYPE_ABI, + ENCODER_TYPE_AS5x47U, } encoder_type_t; typedef struct { @@ -69,6 +70,7 @@ typedef struct { typedef struct { SPIDriver *spi_dev; SPIConfig hw_spi_cfg; + uint8_t spi_af; stm32_gpio_t *nss_gpio; int nss_pin; stm32_gpio_t *sck_gpio; @@ -181,4 +183,50 @@ typedef struct { AS504x_state state; } AS504x_config_t; +typedef struct { + uint8_t is_connected; + uint8_t is_broken_hall; + uint8_t is_error; + uint8_t is_COF; + uint8_t is_Comp_low; + uint8_t is_Comp_high; + uint8_t is_wdtst; + uint8_t is_crc_error; + uint8_t is_mag_half; + uint8_t AGC_value; + uint16_t magnitude; + uint16_t serial_AGC_value; + uint16_t serial_diag_flgs; + uint16_t serial_magnitude; + uint16_t serial_error_flgs; +} AS5x47U_diag; + +typedef struct { + uint16_t spi_seq; + uint32_t spi_communication_error_count; + AS5x47U_diag sensor_diag; + uint16_t spi_val; + float last_enc_angle; + uint32_t spi_error_cnt; + float spi_error_rate; + uint32_t last_update_time; + uint8_t rx_buf[4]; + uint8_t tx_buf[4]; +} AS5x47U_state; + +typedef struct { + SPIDriver *spi_dev; + SPIConfig hw_spi_cfg; + uint8_t spi_af; + 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; + AS5x47U_state state; +} AS5x47U_config_t; + #endif /* ENCODER_DATATYPE_H_ */ diff --git a/halconf.h b/halconf.h index abbbf1ec6..8c6c9ffcc 100644 --- a/halconf.h +++ b/halconf.h @@ -329,6 +329,13 @@ #define SPI_USE_MUTUAL_EXCLUSION TRUE #endif +/** + * @brief Allows adding custom fields to the @p SPIDriver struct. + */ +#if !defined(SPI_DRIVER_EXT_FIELDS) || defined(__DOXYGEN__) +#define SPI_DRIVER_EXT_FIELDS void* app_arg; void (*err_cb)(SPIDriver *pspi); +#endif + #endif /* _HALCONF_H_ */ /** @} */ diff --git a/hwconf/mcuconf.h b/hwconf/mcuconf.h index fbbdd165b..19169f5f9 100644 --- a/hwconf/mcuconf.h +++ b/hwconf/mcuconf.h @@ -308,7 +308,7 @@ #define STM32_SPI_SPI1_IRQ_PRIORITY 10 #define STM32_SPI_SPI2_IRQ_PRIORITY 10 #define STM32_SPI_SPI3_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) { if(spip->err_cb) { spip->err_cb(spip); } else { osalSysHalt("DMA failure"); } } /* * ST driver system settings. diff --git a/motor/mc_interface.c b/motor/mc_interface.c index af79c8056..b4c3abd12 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -487,6 +487,7 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) { 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"; case FAULT_CODE_PHASE_FILTER: return "FAULT_CODE_PHASE_FILTER"; + case FAULT_CODE_ENCODER_FAULT: return "FAULT_CODE_ENCODER_FAULT"; } return "Unknown fault"; From 86c1cdad92341b331069455a42b2d2c222eacf3b Mon Sep 17 00:00:00 2001 From: Zach O'Brien Date: Tue, 19 Jul 2022 08:46:11 -0600 Subject: [PATCH 2/5] Moving '{'s to match coding standard. Making tabbing match coding standard for switch cases. Moving comments so they are less confusing. --- encoder/enc_as5x47u.c | 129 +++++++++++++++++++++--------------------- 1 file changed, 65 insertions(+), 64 deletions(-) diff --git a/encoder/enc_as5x47u.c b/encoder/enc_as5x47u.c index 304f6a3bb..dc0bd0e18 100644 --- a/encoder/enc_as5x47u.c +++ b/encoder/enc_as5x47u.c @@ -117,10 +117,8 @@ static uint8_t enc_as5x47u_crc8(const uint8_t *data, size_t len, const uint8_t i return cksum; } -void enc_as5x47u_spi_callback(SPIDriver *pspi) -{ - if (pspi != NULL && pspi->app_arg != NULL) - { +void enc_as5x47u_spi_callback(SPIDriver *pspi) { + if (pspi != NULL && pspi->app_arg != NULL) { AS5x47U_config_t *cfg = (AS5x47U_config_t*)pspi->app_arg; spiUnselectI(cfg->spi_dev); @@ -136,51 +134,53 @@ void enc_as5x47u_spi_callback(SPIDriver *pspi) uint16_t rx_data = cfg->state.rx_buf[0] << 8 | cfg->state.rx_buf[1]; if (calc_crc == rx_crc) { AS5x47U_determinate_if_connected(cfg, true); - cfg->state.sensor_diag.is_error = + cfg->state.sensor_diag.is_error = (uint8_t)((rx_data & AS5x47U_SPI_ERROR_FLAG_MASK) != 0); - if (!cfg->state.sensor_diag.is_error) - { + if (!cfg->state.sensor_diag.is_error) { UTILS_LP_FAST(cfg->state.spi_error_rate, 0.0, timestep); switch(cfg->state.spi_seq) { - case SPI_SEQ_TX_MAG_RX_POS: { + case SPI_SEQ_TX_MAG_RX_POS: // Receive position, then request position while receiving magnitude AS5x47U_process_pos(cfg, rx_data); cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_MAG; AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); - } break; + break; - case SPI_SEQ_TX_POS_RX_MAG: { // Receive magnitude + case SPI_SEQ_TX_POS_RX_MAG: + // Receive magnitude cfg->state.sensor_diag.serial_magnitude = rx_data; cfg->state.sensor_diag.magnitude = rx_data & AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; cfg->state.spi_seq = SPI_SEQ_TX_AGC_RX_POS; - } break; + break; - case SPI_SEQ_TX_AGC_RX_POS: { + case SPI_SEQ_TX_AGC_RX_POS: // Receive position, then request position while receiving AGC AS5x47U_process_pos(cfg, rx_data); cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_AGC; AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); - } break; + break; - case SPI_SEQ_TX_POS_RX_AGC: { // Receive AGC + case SPI_SEQ_TX_POS_RX_AGC: + // Receive AGC cfg->state.sensor_diag.serial_AGC_value = rx_data; cfg->state.sensor_diag.AGC_value = (uint8_t)rx_data; cfg->state.spi_seq = SPI_SEQ_TX_DIAG_RX_POS; - } break; - case SPI_SEQ_TX_DIAG_RX_POS: { + break; + case SPI_SEQ_TX_DIAG_RX_POS: // Receive position, then request position while requesting diagnostic flags AS5x47U_process_pos(cfg, rx_data); cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_DIAG; AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); - } break; + break; - case SPI_SEQ_TX_POS_RX_DIAG: { // Receive diagnostic flags + case SPI_SEQ_TX_POS_RX_DIAG: + // Receive diagnostic flags cfg->state.sensor_diag.serial_diag_flgs = rx_data; cfg->state.sensor_diag.is_broken_hall = (rx_data >> AS5x47U_SPI_DIAG_FUSA_ERROR_BIT_POS) & 1; @@ -191,16 +191,17 @@ void enc_as5x47u_spi_callback(SPIDriver *pspi) cfg->state.sensor_diag.is_Comp_high = (rx_data >> AS5x47U_SPI_DIAG_COMP_HIGH_BIT_POS) & 1; cfg->state.spi_seq = SPI_SEQ_TX_ERRFL_RX_POS; - } break; + break; - case SPI_SEQ_TX_ERRFL_RX_POS: { + case SPI_SEQ_TX_ERRFL_RX_POS: // Receive position, then request pos while receiving error flags AS5x47U_process_pos(cfg, rx_data); cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_ERRFL; AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_POS_MSG, AS5x47U_SPI_READ_POS_CRC); - } break; - case SPI_SEQ_TX_POS_RX_ERRFL: { // Receive error flags + break; + case SPI_SEQ_TX_POS_RX_ERRFL: + // Receive error flags cfg->state.spi_seq = 0; cfg->state.sensor_diag.serial_error_flgs = rx_data; cfg->state.sensor_diag.is_wdtst = @@ -210,16 +211,16 @@ void enc_as5x47u_spi_callback(SPIDriver *pspi) cfg->state.sensor_diag.is_mag_half = (rx_data >> AS5x47U_SPI_ERRFL_MAG_HALF_BIT_POS) & 1; cfg->state.spi_seq = SPI_SEQ_TX_MAG_RX_POS; - } break; - // Not sure what was just received, but just requested ERRFL, so ignore rx - // data and prepare to receive ERRFL next exchange - case SPI_SEQ_PREV_ERR: { + break; + case SPI_SEQ_PREV_ERR: + // Not sure what was just received, but just requested ERRFL, so ignore rx + // data and prepare to receive ERRFL next exchange cfg->state.spi_seq = SPI_SEQ_TX_POS_RX_ERRFL; - } break; - default: { + break; + default: // Something went wrong, just start the sequence over. cfg->state.spi_seq = SPI_SEQ_TX_MAG_RX_POS; - } break; + break; } } else { // Error flag is set @@ -239,10 +240,8 @@ void enc_as5x47u_spi_callback(SPIDriver *pspi) } } -static void as5x47u_spi_err_callback(SPIDriver *pspi) -{ - if(pspi != NULL && pspi->app_arg != NULL) - { +static void as5x47u_spi_err_callback(SPIDriver *pspi) { + if(pspi != NULL && pspi->app_arg != NULL) { AS5x47U_config_t *cfg = (AS5x47U_config_t*)pspi->app_arg; // Make sure we won't process the data memset(cfg->state.rx_buf, 0, sizeof(cfg->state.rx_buf)); @@ -292,36 +291,40 @@ void enc_as5x47u_deinit(AS5x47U_config_t *cfg) { void enc_as5x47u_routine(AS5x47U_config_t *cfg) { switch(cfg->state.spi_seq) { - case SPI_SEQ_TX_MAG_RX_POS: { // Request magnitude - AS5x47U_start_spi_exchange_precalc_crc( - cfg, AS5x47U_SPI_READ_MAGN_MSG, AS5x47U_SPI_READ_MAGN_CRC); - } break; - - case SPI_SEQ_TX_AGC_RX_POS: { // Request AGC - AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_AGC_MSG, - AS5x47U_SPI_READ_AGC_CRC); - } break; - - case SPI_SEQ_TX_DIAG_RX_POS: { // Request diagnostic flags - AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_DIAG_MSG, - AS5x47U_SPI_READ_DIAG_CRC); - } break; - - case SPI_SEQ_TX_ERRFL_RX_POS: { // Request error flags - AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG, - AS5x47U_SPI_READ_ERRFL_CRC); - } break; - - default: - case SPI_SEQ_PREV_ERR: { // There was a problem of some sort, request ERRFL - AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG, - AS5x47U_SPI_READ_ERRFL_CRC); - } break; + case SPI_SEQ_TX_MAG_RX_POS: + // Request magnitude + AS5x47U_start_spi_exchange_precalc_crc( + cfg, AS5x47U_SPI_READ_MAGN_MSG, AS5x47U_SPI_READ_MAGN_CRC); + break; + + case SPI_SEQ_TX_AGC_RX_POS: + // Request AGC + AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_AGC_MSG, + AS5x47U_SPI_READ_AGC_CRC); + break; + + case SPI_SEQ_TX_DIAG_RX_POS: + // Request diagnostic flags + AS5x47U_start_spi_exchange_precalc_crc( cfg, AS5x47U_SPI_READ_DIAG_MSG, + AS5x47U_SPI_READ_DIAG_CRC); + break; + + case SPI_SEQ_TX_ERRFL_RX_POS: + // Request error flags + AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG, + AS5x47U_SPI_READ_ERRFL_CRC); + break; + + default: + case SPI_SEQ_PREV_ERR: + // There was a problem of some sort, request ERRFL + AS5x47U_start_spi_exchange_precalc_crc(cfg, AS5x47U_SPI_READ_ERRFL_MSG, + AS5x47U_SPI_READ_ERRFL_CRC); + break; } } -static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData) -{ +static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData) { cfg->state.spi_val = posData; posData &= AS5x47U_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK; cfg->state.last_enc_angle = (float)(posData * 360) / (float)(1 << 14); @@ -347,8 +350,7 @@ static void AS5x47U_determinate_if_connected(AS5x47U_config_t *cfg, bool was_las } /* TODO: uncomment if needed -static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data) -{ +static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data) { uint8_t tx_to_crc[] = {(tx_data >> 8) & 0xFF, (tx_data & 0xFF)}; uint8_t tx_crc = enc_as5x47u_crc8(tx_to_crc, 2, 0xC4) ^ 0xFF; AS5x47U_start_spi_exchange_precalc_crc(cfg, tx_data, tx_crc); @@ -356,8 +358,7 @@ static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data) */ static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg, - uint16_t tx_data, uint8_t tx_crc) -{ + uint16_t tx_data, uint8_t tx_crc) { cfg->state.tx_buf[0] = (tx_data >> 8) & 0xFF; cfg->state.tx_buf[1] = tx_data & 0xFF; cfg->state.tx_buf[2] = tx_crc; From 93e74e53639fad03b6d67e725f90301354341169 Mon Sep 17 00:00:00 2001 From: Zach O'Brien Date: Tue, 19 Jul 2022 09:35:38 -0600 Subject: [PATCH 3/5] Fixing spelling errors in comments. Adjusting whitespace to adhere to coding style. --- encoder/enc_as5x47u.c | 11 +++++------ encoder/enc_as5x47u.h | 2 +- encoder/encoder.c | 4 ++-- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/encoder/enc_as5x47u.c b/encoder/enc_as5x47u.c index dc0bd0e18..ba056f932 100644 --- a/encoder/enc_as5x47u.c +++ b/encoder/enc_as5x47u.c @@ -56,10 +56,10 @@ #define AS5x47U_SPI_POS_ADR 0x3FFF #define AS5x47U_SPI_READ_ERRFL_MSG (AS5x47U_SPI_ERRFL_ADR | AS5x47U_SPI_READ_BIT) -#define AS5x47U_SPI_READ_DIAG_MSG (AS5x47U_SPI_DIAG_ADR | AS5x47U_SPI_READ_BIT) -#define AS5x47U_SPI_READ_MAGN_MSG (AS5x47U_SPI_MAGN_ADR | AS5x47U_SPI_READ_BIT) -#define AS5x47U_SPI_READ_AGC_MSG (AS5x47U_SPI_AGC_ADR | AS5x47U_SPI_READ_BIT) -#define AS5x47U_SPI_READ_POS_MSG (AS5x47U_SPI_POS_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_DIAG_MSG (AS5x47U_SPI_DIAG_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_MAGN_MSG (AS5x47U_SPI_MAGN_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_AGC_MSG (AS5x47U_SPI_AGC_ADR | AS5x47U_SPI_READ_BIT) +#define AS5x47U_SPI_READ_POS_MSG (AS5x47U_SPI_POS_ADR | AS5x47U_SPI_READ_BIT) #define AS5x47U_SPI_READ_ERRFL_CRC (0x06) #define AS5x47U_SPI_READ_DIAG_CRC (0x6F) @@ -83,7 +83,6 @@ enum { // Private functions static void AS5x47U_determinate_if_connected(AS5x47U_config_t *cfg, bool was_last_valid); -// static void AS5x47U_start_spi_exchange(AS5x47U_config_t *cfg, uint16_t tx_data); static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg, uint16_t tx_data, uint8_t tx_crc); static void AS5x47U_process_pos(AS5x47U_config_t *cfg, uint16_t posData); @@ -365,7 +364,7 @@ static void AS5x47U_start_spi_exchange_precalc_crc(AS5x47U_config_t *cfg, memset(cfg->state.rx_buf, 0, sizeof(cfg->state.rx_buf)); // There is a weird corner case where the DMA may not read all of the Rx data. This - // causes the RXNE flag to be set when an exchange starts, causing the fist byte of + // causes the RXNE flag to be set when an exchange starts, causing the first byte of // data received to be from the previous exchange. This is corrected by reading the // SPI data register, clearing the RXNE flag. volatile uint32_t test = cfg->spi_dev->spi->DR; diff --git a/encoder/enc_as5x47u.h b/encoder/enc_as5x47u.h index c498246e0..4e020f295 100644 --- a/encoder/enc_as5x47u.h +++ b/encoder/enc_as5x47u.h @@ -37,7 +37,7 @@ void enc_as5x47u_deinit(AS5x47U_config_t *cfg); /** * @brief Starts a new SPI transaction to get the current position from the position sensor. * - * Every call of enc_as5x47u_routine ends up doing two SPI trnasactions. The first one + * Every call of enc_as5x47u_routine ends up doing two SPI transactions. The first one * always receives position, but will request one of MAGN, AGC, DIAG, or ERRFL. The next * transaction receives the requested register while requesting pos for the next * transaction. diff --git a/encoder/encoder.c b/encoder/encoder.c index d2bef0523..f25a55319 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -341,9 +341,9 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto if (diag.is_Comp_high) { mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false); - } else if(diag.is_Comp_low) { + } else if (diag.is_Comp_low) { mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false); - } else if(diag.is_broken_hall || diag.is_COF || diag.is_wdtst) { + } else if (diag.is_broken_hall || diag.is_COF || diag.is_wdtst) { mc_interface_fault_stop(FAULT_CODE_ENCODER_FAULT, is_second_motor, false); } From d0c61c85221f91cacf9a37a793ad54e329e5533a Mon Sep 17 00:00:00 2001 From: Zach O'Brien Date: Tue, 19 Jul 2022 09:42:13 -0600 Subject: [PATCH 4/5] Adding myself to copyright header on AS5x47U encoder files. --- encoder/enc_as5x47u.c | 2 +- encoder/enc_as5x47u.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/encoder/enc_as5x47u.c b/encoder/enc_as5x47u.c index ba056f932..b8a94f4f4 100644 --- a/encoder/enc_as5x47u.c +++ b/encoder/enc_as5x47u.c @@ -1,6 +1,6 @@ /* Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se - Copyright 2022 Jakub Tomczak + Copyright 2022 Zach O'Brien This file is part of the VESC firmware. diff --git a/encoder/enc_as5x47u.h b/encoder/enc_as5x47u.h index 4e020f295..1742e75bd 100644 --- a/encoder/enc_as5x47u.h +++ b/encoder/enc_as5x47u.h @@ -1,6 +1,6 @@ /* Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se - Copyright 2022 Jakub Tomczak + Copyright 2022 Zach O'Brien This file is part of the VESC firmware. From bb515aaf9c0d059d0042f0bdbdb75548537bd5fe Mon Sep 17 00:00:00 2001 From: Zach O'Brien Date: Wed, 20 Jul 2022 08:04:47 -0600 Subject: [PATCH 5/5] Making it clear a commented out line is included for visability. --- encoder/enc_as5x47u.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/encoder/enc_as5x47u.c b/encoder/enc_as5x47u.c index b8a94f4f4..975577657 100644 --- a/encoder/enc_as5x47u.c +++ b/encoder/enc_as5x47u.c @@ -265,7 +265,9 @@ bool enc_as5x47u_init(AS5x47U_config_t *cfg) { cfg->spi_dev->app_arg = (void*)cfg; cfg->spi_dev->err_cb = as5x47u_spi_err_callback; - // cfg->spi_dev->config->end_cb = enc_as5x47u_spi_callback; // Set in encoder_cfg.c + + // Set in encoder_cfg.c, kept here for visability + // cfg->spi_dev->config->end_cb = enc_as5x47u_spi_callback; spiStart(cfg->spi_dev, &(cfg->hw_spi_cfg));