From 413ccdbff1b60fc03683c2e79d01ce28f4dac7ae Mon Sep 17 00:00:00 2001 From: Vincent Manoukian <10980775+manoukianv@users.noreply.github.com> Date: Sun, 2 Oct 2022 16:33:17 +0200 Subject: [PATCH] First impl of bissc. Update the motor signature for VescTools. --- confgenerator.h | 2 +- datatypes.h | 3 +- encoder/enc_bissc.c | 169 +++++++++++++++++++++++++++++++++++++ encoder/enc_bissc.h | 35 ++++++++ encoder/encoder.c | 41 +++++++++ encoder/encoder.h | 1 + encoder/encoder.mk | 3 +- encoder/encoder_cfg.c | 33 ++++++++ encoder/encoder_cfg.h | 1 + encoder/encoder_datatype.h | 31 +++++++ 10 files changed, 316 insertions(+), 3 deletions(-) create mode 100644 encoder/enc_bissc.c create mode 100644 encoder/enc_bissc.h diff --git a/confgenerator.h b/confgenerator.h index 2f2d21c8..74963d9b 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 1990552441 +#define MCCONF_SIGNATURE 347304876 #define APPCONF_SIGNATURE 115587144 // Functions diff --git a/datatypes.h b/datatypes.h index 4ef7996b..3cac242f 100644 --- a/datatypes.h +++ b/datatypes.h @@ -191,7 +191,8 @@ typedef enum { SENSOR_PORT_MODE_TS5700N8501, SENSOR_PORT_MODE_TS5700N8501_MULTITURN, SENSOR_PORT_MODE_MT6816_SPI, - SENSOR_PORT_MODE_AS5x47U_SPI + SENSOR_PORT_MODE_AS5x47U_SPI, + SENSOR_PORT_MODE_BISSC } sensor_port_mode; typedef struct { diff --git a/encoder/enc_bissc.c b/encoder/enc_bissc.c new file mode 100644 index 00000000..41804bc2 --- /dev/null +++ b/encoder/enc_bissc.c @@ -0,0 +1,169 @@ +/* + Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se + Copyright 2022 Marcos Chaparro mchaparro@powerdesigns.ca + 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_bissc.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 + +bool enc_bissc_init(BISSC_config_t *cfg) { + if (cfg->spi_dev == NULL) { + return false; + } + + memset(&cfg->state, 0, sizeof(BISSC_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; + + //Start driver with BissC SPI settings + spiStart(cfg->spi_dev, &(cfg->hw_spi_cfg)); + + cfg->state.spi_data_error_cnt = 0; + cfg->state.spi_data_error_rate = 0.0; + cfg->state.spi_comm_error_cnt = 0; + cfg->state.spi_comm_error_rate = 0.0; + + //Init CRC table + uint8_t POLY = 0x43; + for(int i = 0; i < 64; i++){ + int crc = i; + + for (int j = 0; j < 6; j++){ + if (crc & 0x20){ + crc <<= 1; + crc ^= POLY; + } else { + crc <<= 1; + } + } + cfg->tableCRC6n[i] = crc; + } + + return true; +} + +void enc_bissc_deinit(BISSC_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_data_error_cnt = 0; + cfg->state.spi_data_error_rate = 0.0; + cfg->state.spi_comm_error_cnt = 0; + cfg->state.spi_comm_error_rate = 0.0; + } +} + +void enc_bissc_routine(BISSC_config_t *cfg) { + if (cfg->spi_dev->state == SPI_READY) { + spiSelectI(cfg->spi_dev); + spiStartReceiveI(cfg->spi_dev, 8, (void *)cfg->state.decod_buf); + } else { + ++encoder_cfg_bissc.state.spi_comm_error_cnt; + // compute rate with factor 0.0001 for 10000hz + UTILS_LP_FAST(encoder_cfg_bissc.state.spi_comm_error_rate, 1.0, 0.0001); + } +} + +void compute_bissc_callback(SPIDriver *pspi) { + if (pspi != NULL && pspi->app_arg != NULL) { + BISSC_config_t *cfg = (BISSC_config_t*)pspi->app_arg; + spiUnselectI(cfg->spi_dev); + + 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(); + + int lenghtDataBit = cfg->enc_res; + + uint64_t rxData64; + rxData64 = (uint64_t)cfg->state.decod_buf[0] << 56; + rxData64 |= (uint64_t)cfg->state.decod_buf[1] << 48; + rxData64 |= (uint64_t)cfg->state.decod_buf[2] << 40; + rxData64 |= (uint64_t)cfg->state.decod_buf[3] << 32; + rxData64 |= (uint64_t)cfg->state.decod_buf[4] << 24; + rxData64 |= (uint64_t)cfg->state.decod_buf[5] << 16; + rxData64 |= (uint64_t)cfg->state.decod_buf[6] << 8; + rxData64 |= (uint64_t)cfg->state.decod_buf[7]; + + // sample of rxData64 + // like this 1100000000000000100001100111010000000101110111100000000000000000 + rxData64 <<= __builtin_clzll(rxData64); // slice rxData to have a value starting with 1 + rxData64 &= 0x3FFFFFFFFFFFFFFF; // remove the 2 first bit + + // remove the first 1, count how many digit stay in buffer after removing the 0, if there is more than 32 digits, + // keep only 32st (on the left) + // 32 because the format is : (1+1+lenghtDataBit+1+1+6) - Align bitstream to left (Startbit, CDS, 22-bit Position, Error, Warning, CRC) + // int nbBit = log2(rxData64)+1; + int nbBit = 64-__builtin_clzll(rxData64); // Much faster than log2 + if ( nbBit >= ( lenghtDataBit + 10 ) ) { + rxData64 >>= nbBit-( lenghtDataBit + 10 ); + } + + uint8_t crcRx = rxData64 & 0x3F; //extract last 6-bit digits to get CRC + uint32_t dataRx = (rxData64 >> 6) & ((1<<(lenghtDataBit + 2)) - 1); //Shift out CRC, AND with 24-bit mask to get raw data (position, error, warning) + cfg->state.spi_val = (dataRx >> 2) & ((1<> 30) & 0x03); + crc = cfg->tableCRC6n[((dataRx >> 24) & 0x3F) ^ crc]; + crc = cfg->tableCRC6n[((dataRx >> 18) & 0x3F) ^ crc]; + crc = cfg->tableCRC6n[((dataRx >> 12) & 0x3F) ^ crc]; + crc = cfg->tableCRC6n[((dataRx >> 6) & 0x3F) ^ crc]; + crc = cfg->tableCRC6n[((dataRx >> 0) & 0x3F) ^ crc]; + crc = 0x3F & ~crc; //CRC is output inverted + + if(crc != crcRx) + { + ++cfg->state.spi_data_error_cnt; + UTILS_LP_FAST(cfg->state.spi_data_error_rate, 1.0, timestep); + } else { + cfg->state.last_enc_angle = ((float)cfg->state.spi_val * 360.0) / ((1<. + */ + +#ifndef ENC_BISSC_H_ +#define ENC_BISSC_H_ + +#include "encoder/encoder_datatype.h" + +bool enc_bissc_init(BISSC_config_t *cfg); +void enc_bissc_deinit(BISSC_config_t *cfg); +void enc_bissc_routine(BISSC_config_t *cfg); + +extern BISSC_config_t encoder_cfg_bissc; + +// Macros +#define BISSC_LAST_ANGLE(cfg) ((cfg)->state.last_enc_angle) + +#endif /* ENC_BISSC_H_ */ diff --git a/encoder/encoder.c b/encoder/encoder.c index f25a5531..0dac0171 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -162,6 +162,22 @@ bool encoder_init(volatile mc_configuration *conf) { res = true; } break; + case SENSOR_PORT_MODE_BISSC: { + SENSOR_PORT_5V(); + + encoder_cfg_bissc.enc_res = conf->m_encoder_counts; + + if (!enc_bissc_init(&encoder_cfg_bissc)) { + encoder_type_now = ENCODER_TYPE_NONE; + return false; + } + + encoder_type_now = ENCODER_TYPE_BISSC; + timer_start(10000); + + res = true; + } break; + default: SENSOR_PORT_5V(); encoder_type_now = ENCODER_TYPE_NONE; @@ -208,6 +224,8 @@ void encoder_deinit(void) { enc_ts5700n8501_deinit(&encoder_cfg_TS5700N8501); } else if (encoder_type_now == ENCODER_TYPE_AS5x47U) { enc_as5x47u_deinit(&encoder_cfg_as5x47u); + } else if (encoder_type_now == ENCODER_TYPE_BISSC) { + enc_bissc_deinit(&encoder_cfg_bissc); } encoder_type_now = ENCODER_TYPE_NONE; @@ -228,6 +246,8 @@ float encoder_read_deg(void) { return enc_ts5700n8501_read_deg(&encoder_cfg_TS5700N8501); } else if (encoder_type_now == ENCODER_TYPE_AS5x47U) { return AS5x47U_LAST_ANGLE(&encoder_cfg_as5x47u); + } else if (encoder_type_now == ENCODER_TYPE_BISSC) { + return BISSC_LAST_ANGLE(&encoder_cfg_bissc); } return 0.0; } @@ -349,6 +369,15 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto break; + case SENSOR_PORT_MODE_BISSC: + if (encoder_cfg_bissc.state.spi_comm_error_rate > 0.04) { + mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false); + } + if (encoder_cfg_bissc.state.spi_data_error_rate > 0.05) { + mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, is_second_motor, false); + } + break; + default: break; } @@ -377,6 +406,9 @@ void encoder_tim_isr(void) { enc_as5x47u_routine(&encoder_cfg_as5x47u); break; + case ENCODER_TYPE_BISSC: + enc_bissc_routine(&encoder_cfg_bissc); + default: break; } @@ -487,6 +519,15 @@ static void terminal_encoder(int argc, const char **argv) { encoder_cfg_as5x47u.state.sensor_diag.serial_diag_flgs); break; + case SENSOR_PORT_MODE_BISSC: + commands_printf("BissC Loss SPI communication (>4%c error): errors: %d, error rate: %.3f %%", 0xB0, + encoder_cfg_bissc.state.spi_comm_error_cnt, + (double)(encoder_cfg_bissc.state.spi_comm_error_rate * 100.0)); + commands_printf("BissC Degradation Of Signal (>5%c error): errors: %d, error rate: %.3f %%", 0xB0, + encoder_cfg_bissc.state.spi_data_error_cnt, + (double)(encoder_cfg_bissc.state.spi_data_error_rate * 100.0)); + break; + default: commands_printf("No encoder debug info available."); break; diff --git a/encoder/encoder.h b/encoder/encoder.h index a96f1e28..1f9d5071 100644 --- a/encoder/encoder.h +++ b/encoder/encoder.h @@ -30,6 +30,7 @@ #include "enc_as504x.h" #include "enc_as5x47u.h" #include "enc_abi.h" +#include "enc_bissc.h" // Functions bool encoder_init(volatile mc_configuration *conf); diff --git a/encoder/encoder.mk b/encoder/encoder.mk index 9c4c95c7..08147545 100644 --- a/encoder/encoder.mk +++ b/encoder/encoder.mk @@ -6,6 +6,7 @@ ENCSRC = encoder/encoder.c \ encoder/enc_sincos.c \ encoder/enc_mt6816.c \ encoder/enc_ts5700n8501.c \ - encoder/enc_as5x47u.c + encoder/enc_as5x47u.c \ + encoder/enc_bissc.c ENCINC = encoder diff --git a/encoder/encoder_cfg.c b/encoder/encoder_cfg.c index f93465a8..91ccc2a0 100644 --- a/encoder/encoder_cfg.c +++ b/encoder/encoder_cfg.c @@ -165,3 +165,36 @@ AS5x47U_config_t encoder_cfg_as5x47u = { #endif {0}, // State }; + +// Spi Handler for bissC +void compute_bissc_callback(SPIDriver *pspi); +BISSC_config_t encoder_cfg_bissc = { +#ifdef HW_SPI_DEV + &HW_SPI_DEV, // spi_dev + {//HARDWARE SPI CONFIG + //NULL, HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, + &compute_bissc_callback, HW_SPI_PORT_NSS, HW_SPI_PIN_NSS, + SPI_BaudRatePrescaler_32 | SPI_CR1_CPOL | SPI_CR1_CPHA + }, + + 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, + 22, // enc_res + {0}, // crc + {0.0, 0, 0.0, 0, 0.0, 0, 0, {0}} +#else + 0, + {0}, + 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 22, // enc_res + {0}, // crc + {0.0, 0, 0.0, 0, 0.0, 0, 0, {0}} +#endif +}; \ No newline at end of file diff --git a/encoder/encoder_cfg.h b/encoder/encoder_cfg.h index eb5f720e..4bf430eb 100644 --- a/encoder/encoder_cfg.h +++ b/encoder/encoder_cfg.h @@ -30,5 +30,6 @@ 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; +extern BISSC_config_t encoder_cfg_bissc; #endif /* ENCODER_CFG_H_ */ diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h index cc9157b7..f8bc1378 100644 --- a/encoder/encoder_datatype.h +++ b/encoder/encoder_datatype.h @@ -36,6 +36,7 @@ typedef enum { ENCODER_TYPE_TS5700N8501, ENCODER_TYPE_ABI, ENCODER_TYPE_AS5x47U, + ENCODER_TYPE_BISSC } encoder_type_t; typedef struct { @@ -229,4 +230,34 @@ typedef struct { AS5x47U_state state; } AS5x47U_config_t; +typedef struct { + float spi_data_error_rate; + uint32_t spi_data_error_cnt; + float spi_comm_error_rate; + uint32_t spi_comm_error_cnt; + float last_enc_angle; + uint32_t spi_val; + uint32_t last_update_time; + uint8_t decod_buf[8]; +} BISSC_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; + + uint32_t enc_res; + uint8_t tableCRC6n[64]; + + BISSC_state state; +} BISSC_config_t; + #endif /* ENCODER_DATATYPE_H_ */