rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:vedderb/bldc

This commit is contained in:
Benjamin Vedder 2022-10-06 13:48:30 +02:00
parent e3ac778058 c41da62817
commit 9098ec6d06
10 changed files with 316 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
confgenerator.h
View File

@ -8,7 +8,7 @@
#include <stdbool.h>
// Constants
#define MCCONF_SIGNATURE 1990552441
#define MCCONF_SIGNATURE 347304876
#define APPCONF_SIGNATURE 115587144
// Functions

3
datatypes.h
View File

@ -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 {

169
encoder/enc_bissc.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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 <math.h>
#include <string.h>
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<<lenghtDataBit) - 1); //Shift out error and warning, AND with 22-bit mask to get position
uint8_t crc = 0; //CRC seed is 0b000000
crc = ((dataRx >> 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<<lenghtDataBit) - 1);
}
}
}

35
encoder/enc_bissc.h Normal file
View File

@ -0,0 +1,35 @@
/*
Copyright 2016 - 2022 Benjamin Vedder benjamin@vedder.se
Copyright 2022 Manoukian Vincent manoukianv@gmail.com
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 <http://www.gnu.org/licenses/>.
*/
#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_ */

41
encoder/encoder.c
View File

@ -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;

1
encoder/encoder.h
View File

@ -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);

3
encoder/encoder.mk
View File

@ -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

33
encoder/encoder_cfg.c
View File

@ -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
};

1
encoder/encoder_cfg.h
View File

@ -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_ */

31
encoder/encoder_datatype.h
View File

@ -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_ */