mirror of https://github.com/rusefi/bldc.git
Merge branch 'master' of github.com:vedderb/bldc
This commit is contained in:
commit
9098ec6d06
|
@ -8,7 +8,7 @@
|
|||
#include <stdbool.h>
|
||||
|
||||
// Constants
|
||||
#define MCCONF_SIGNATURE 1990552441
|
||||
#define MCCONF_SIGNATURE 347304876
|
||||
#define APPCONF_SIGNATURE 115587144
|
||||
|
||||
// Functions
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
|
@ -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_ */
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
};
|
|
@ -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_ */
|
||||
|
|
|
@ -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_ */
|
||||
|
|
Loading…
Reference in New Issue