Skip to content

Commit 11b1f71

Browse files
Manoukian Vincentmanoukianv
Manoukian Vincent
authored and
manoukianv
committed
Add the BissC encoder on HW SPI.
Update signature on 6.0
1 parent 75f9451 commit 11b1f71

7 files changed

Lines changed: 242 additions & 5 deletions

File tree

‎conf_general.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -314,6 +314,9 @@
314314
#ifndef AS504x_USE_SW_MOSI_PIN
315315
#define AS504x_USE_SW_MOSI_PIN 0
316316
#endif
317+
#ifndef BISSC_USE_HW_SPI_PINS
318+
#define BISSC_USE_HW_SPI_PINS 1
319+
#endif
317320

318321
/*
319322
* MCU

‎confgenerator.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
#include <stdbool.h>
99

1010
// Constants
11-
#define MCCONF_SIGNATURE 1018325777
11+
#define MCCONF_SIGNATURE 1412559730
1212
#define APPCONF_SIGNATURE 3733512279
1313

1414
// Functions

‎datatypes.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,8 @@ typedef enum {
180180
SENSOR_PORT_MODE_SINCOS,
181181
SENSOR_PORT_MODE_TS5700N8501,
182182
SENSOR_PORT_MODE_TS5700N8501_MULTITURN,
183-
SENSOR_PORT_MODE_MT6816_SPI
183+
SENSOR_PORT_MODE_MT6816_SPI,
184+
SENSOR_PORT_MODE_BISSC_SPI
184185
} sensor_port_mode;
185186

186187
typedef struct {

‎encoder.c‎

Lines changed: 212 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include "hw.h"
2525
#include "mc_interface.h"
2626
#include "utils.h"
27+
#include "timer.h"
2728
#include <math.h>
2829

2930
// Defines
@@ -32,11 +33,12 @@
3233
#define AD2S1205_SAMPLE_RATE_HZ 20000 //25MHz max spi clk
3334
#define MT6816_SAMPLE_RATE_HZ 20000
3435
#define MT6816_NO_MAGNET_ERROR_MASK 0x0002
36+
#define BISSC_SAMPLE_RATE_HZ 10000
3537
#define SINCOS_SAMPLE_RATE_HZ 20000
3638
#define SINCOS_MIN_AMPLITUDE 1.0 // sqrt(sin^2 + cos^2) has to be larger than this
3739
#define SINCOS_MAX_AMPLITUDE 1.65 // sqrt(sin^2 + cos^2) has to be smaller than this
3840

39-
#if (AS5047_USE_HW_SPI_PINS) || (MT6816_USE_HW_SPI_PINS) || (AD2S1205_USE_HW_SPI_PINS)
41+
#if (AS5047_USE_HW_SPI_PINS) || (MT6816_USE_HW_SPI_PINS) || (AD2S1205_USE_HW_SPI_PINS) || (BISSC_USE_HW_SPI_PINS)
4042
#ifdef HW_SPI_DEV
4143
#define SPI_SW_MISO_GPIO HW_SPI_PORT_MISO
4244
#define SPI_SW_MISO_PIN HW_SPI_PIN_MISO
@@ -75,6 +77,9 @@
7577
#define SPI_SW_CS_PIN HW_HALL_ENC_PIN3
7678
#endif
7779

80+
// Add this to move from Thread to NVIC process to schedule encoder
81+
#define BISSC_ISR_IMPL
82+
7883
// Private types
7984
typedef enum {
8085
ENCODER_MODE_NONE = 0,
@@ -83,7 +88,8 @@ typedef enum {
8388
RESOLVER_MODE_AD2S1205,
8489
ENCODER_MODE_SINCOS,
8590
ENCODER_MODE_TS5700N8501,
86-
ENCODER_MODE_MT6816_SPI
91+
ENCODER_MODE_MT6816_SPI,
92+
ENCODER_MODE_BISSC_SPI
8793
} encoder_mode;
8894

8995
// Private variables
@@ -116,6 +122,20 @@ static uint32_t sincos_signal_above_max_error_cnt = 0;
116122
static float sincos_signal_low_error_rate = 0.0;
117123
static float sincos_signal_above_max_error_rate = 0.0;
118124

125+
#ifndef BISSC_ISR_IMPL
126+
static THD_FUNCTION(bissc_thread, arg);
127+
static THD_WORKING_AREA(bissc_thread_wa, 512);
128+
static thread_t *bissc_tp;
129+
static volatile bool bissc_stop_now = true;
130+
static volatile bool bissc_thread_is_running = false;
131+
#endif
132+
static uint8_t tableCRC6n[64] = {0};
133+
static const uint8_t POLY = 0x43;
134+
static volatile uint8_t decod_buf[8] = {0};
135+
static volatile float delay_between_2biss_c;
136+
void computeBisscData(SPIDriver *spip);
137+
138+
119139
static SerialConfig TS5700N8501_uart_cfg = {
120140
2500000,
121141
0,
@@ -141,6 +161,13 @@ static const SPIConfig mt6816_spi_cfg = {
141161
SPI_SW_CS_GPIO,
142162
SPI_SW_CS_PIN,
143163
SPI_BaudRatePrescaler_4 | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_DATASIZE_16BIT};
164+
165+
static const SPIConfig bissc_spi_cfg = {
166+
&computeBisscData,
167+
SPI_SW_CS_GPIO,
168+
SPI_SW_CS_PIN,
169+
SPI_BaudRatePrescaler_32 | SPI_CR1_CPOL | SPI_CR1_CPHA};
170+
144171
#endif
145172

146173
static THD_FUNCTION(ts5700n8501_thread, arg);
@@ -250,6 +277,10 @@ void encoder_ts57n8501_reset_multiturn(void) {
250277
ts5700n8501_reset_multiturn = true;
251278
}
252279

280+
float encode_bissc_get_time_between_reads(void) {
281+
return delay_between_2biss_c;
282+
}
283+
253284
void encoder_deinit(void) {
254285
nvicDisableVector(HW_ENC_EXTI_CH);
255286
nvicDisableVector(HW_ENC_TIM_ISR_CH);
@@ -280,6 +311,16 @@ void encoder_deinit(void) {
280311
#endif
281312
}
282313

314+
#ifndef BISSC_ISR_IMPL
315+
if (mode == ENCODER_MODE_BISSC_SPI) {
316+
chEvtSignalI(bissc_tp, (eventmask_t) 1);
317+
bissc_stop_now = true;
318+
while (bissc_thread_is_running) {
319+
chThdSleepMilliseconds(1);
320+
}
321+
}
322+
#endif
323+
283324
index_found = false;
284325
mode = ENCODER_MODE_NONE;
285326
last_enc_angle = 0.0;
@@ -413,6 +454,71 @@ void encoder_init_mt6816_spi(void) {
413454
#endif
414455
}
415456

457+
void encoder_init_bissc_spi(uint32_t counts) {
458+
#ifdef HW_SPI_DEV
459+
460+
mode = ENCODER_MODE_BISSC_SPI;
461+
index_found = true;
462+
spi_error_rate = 0.0;
463+
enc_counts = counts;
464+
465+
palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST);
466+
palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST);
467+
palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST);
468+
palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
469+
470+
//Start driver with BissC SPI settings
471+
spiInit();
472+
spiStart(&HW_SPI_DEV, &bissc_spi_cfg);
473+
474+
//Init CRC table
475+
for(int i = 0; i < 64; i++){
476+
int crc = i;
477+
478+
for (int j = 0; j < 6; j++){
479+
if (crc & 0x20){
480+
crc <<= 1;
481+
crc ^= POLY;
482+
} else {
483+
crc <<= 1;
484+
}
485+
}
486+
tableCRC6n[i] = crc;
487+
}
488+
489+
#ifdef BISSC_ISR_IMPL
490+
491+
// Enable timer clock
492+
HW_ENC_TIM_CLK_EN();
493+
494+
// Time Base configuration
495+
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
496+
TIM_TimeBaseStructure.TIM_Prescaler = 0;
497+
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
498+
TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / BISSC_SAMPLE_RATE_HZ) - 1);
499+
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
500+
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
501+
TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
502+
503+
// Enable overflow interrupt
504+
TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
505+
506+
// Enable timer
507+
TIM_Cmd(HW_ENC_TIM, ENABLE);
508+
509+
nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
510+
#else
511+
bissc_thread_is_running = true;
512+
bissc_stop_now = false;
513+
514+
chThdCreateStatic(bissc_thread_wa, sizeof(bissc_thread_wa),
515+
NORMALPRIO - 10, bissc_thread, NULL);
516+
#endif
517+
518+
519+
#endif
520+
}
521+
416522
void encoder_init_ad2s1205_spi(void) {
417523
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
418524

@@ -524,6 +630,7 @@ float encoder_read_deg(void) {
524630
case ENCODER_MODE_MT6816_SPI:
525631
case RESOLVER_MODE_AD2S1205:
526632
case ENCODER_MODE_TS5700N8501:
633+
case ENCODER_MODE_BISSC_SPI:
527634
angle = last_enc_angle;
528635
break;
529636

@@ -848,6 +955,15 @@ void encoder_tim_isr(void) {
848955
UTILS_LP_FAST(spi_error_rate, 1.0, 1./MT6816_SAMPLE_RATE_HZ);
849956
}
850957
}
958+
#ifdef BISSC_ISR_IMPL
959+
if(mode == ENCODER_MODE_BISSC_SPI) {
960+
if (HW_SPI_DEV.state == SPI_READY) {
961+
spiSelectI(&HW_SPI_DEV);
962+
spiStartReceiveI(&HW_SPI_DEV, 8, (void *)decod_buf);
963+
}
964+
}
965+
#endif
966+
851967
#endif
852968

853969
if (mode == RESOLVER_MODE_AD2S1205) {
@@ -1164,3 +1280,97 @@ static THD_FUNCTION(ts5700n8501_thread, arg) {
11641280
}
11651281
}
11661282

1283+
1284+
void computeBisscData(SPIDriver *spip) {
1285+
1286+
spiUnselectI(spip);
1287+
1288+
int lenghtDataBit = enc_counts;
1289+
1290+
uint64_t rxData64;
1291+
rxData64 = (uint64_t)decod_buf[0] << 56;
1292+
rxData64 |= (uint64_t)decod_buf[1] << 48;
1293+
rxData64 |= (uint64_t)decod_buf[2] << 40;
1294+
rxData64 |= (uint64_t)decod_buf[3] << 32;
1295+
rxData64 |= (uint64_t)decod_buf[4] << 24;
1296+
rxData64 |= (uint64_t)decod_buf[5] << 16;
1297+
rxData64 |= (uint64_t)decod_buf[6] << 8;
1298+
rxData64 |= (uint64_t)decod_buf[7];
1299+
1300+
// sample of rxData64
1301+
// like this 1100000000000000100001100111010000000101110111100000000000000000
1302+
rxData64 <<= __builtin_clzll(rxData64); // slice rxData to have a value starting with 1
1303+
rxData64 &= 0x3FFFFFFFFFFFFFFF; // remove the 2 first bit
1304+
1305+
// remove the first 1, count how many digit stay in buffer after removing the 0, if there is more than 32 digits,
1306+
// keep only 32st (on the left)
1307+
// 32 because the format is : (1+1+lenghtDataBit+1+1+6) - Align bitstream to left (Startbit, CDS, 22-bit Position, Error, Warning, CRC)
1308+
int nbBit = log2(rxData64)+1;
1309+
if ( nbBit >= ( lenghtDataBit + 10 ) ) {
1310+
rxData64 >>= nbBit-( lenghtDataBit + 10 );
1311+
}
1312+
1313+
uint8_t crcRx = rxData64 & 0x3F; //extract last 6-bit digits to get CRC
1314+
uint32_t dataRx = (rxData64 >> 6) & ((1<<(lenghtDataBit + 2)) - 1); //Shift out CRC, AND with 24-bit mask to get raw data (position, error, warning)
1315+
spi_val = (dataRx >> 2) & ((1<<lenghtDataBit) - 1); //Shift out error and warning, AND with 22-bit mask to get position
1316+
1317+
uint8_t crc = 0; //CRC seed is 0b000000
1318+
crc = ((dataRx >> 30) & 0x03);
1319+
crc = tableCRC6n[((dataRx >> 24) & 0x3F) ^ crc];
1320+
crc = tableCRC6n[((dataRx >> 18) & 0x3F) ^ crc];
1321+
crc = tableCRC6n[((dataRx >> 12) & 0x3F) ^ crc];
1322+
crc = tableCRC6n[((dataRx >> 6) & 0x3F) ^ crc];
1323+
crc = tableCRC6n[((dataRx >> 0) & 0x3F) ^ crc];
1324+
crc = 0x3F & ~crc; //CRC is output inverted
1325+
1326+
if(crc != crcRx)
1327+
{
1328+
++spi_error_cnt;
1329+
UTILS_LP_FAST(spi_error_rate, 1.0, 1./BISSC_SAMPLE_RATE_HZ);
1330+
} else {
1331+
last_enc_angle = ((float)spi_val * 360.0) / ((1<<lenghtDataBit) - 1);
1332+
UTILS_LP_FAST(spi_error_rate, 0.0, 1./BISSC_SAMPLE_RATE_HZ);
1333+
}
1334+
1335+
#ifndef BISSC_ISR_IMPL
1336+
chEvtSignalI(bissc_tp, (eventmask_t) 1);
1337+
#endif
1338+
1339+
}
1340+
1341+
#ifndef BISSC_ISR_IMPL
1342+
1343+
static THD_FUNCTION(bissc_thread, arg) {
1344+
(void)arg;
1345+
1346+
chRegSetThreadName("BISSC Read");
1347+
1348+
bissc_tp = chThdGetSelfX();
1349+
int end_bissc_time = timer_time_now();
1350+
1351+
1352+
1353+
for(;;) {
1354+
// Check if it is time to stop.
1355+
if (bissc_stop_now) {
1356+
bissc_thread_is_running = false;
1357+
return;
1358+
}
1359+
1360+
delay_between_2biss_c = timer_seconds_elapsed_since(end_bissc_time);
1361+
1362+
// Call the spi incoming message in DMA, on return, the result is decoded
1363+
// by the computeBisscData callBack
1364+
spiSelectI(&HW_SPI_DEV);
1365+
spiStartReceiveI(&HW_SPI_DEV, 8, (void *)decod_buf);
1366+
1367+
chEvtWaitAny((eventmask_t) 1);
1368+
end_bissc_time = timer_time_now();
1369+
1370+
chThdSleepMicroseconds(10);
1371+
1372+
}
1373+
1374+
}
1375+
1376+
#endif

‎encoder.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ void encoder_deinit(void);
2727
void encoder_init_abi(uint32_t counts);
2828
void encoder_init_as5047p_spi(void);
2929
void encoder_init_mt6816_spi(void);
30+
void encoder_init_bissc_spi(uint32_t counts);
3031
void encoder_init_ad2s1205_spi(void);
3132
void encoder_init_sincos(float sin_gain, float sin_offset,
3233
float cos_gain, float cos_offset, float sincos_filter_constant);
@@ -59,6 +60,7 @@ int16_t encoder_ts57n8501_get_abm(void);
5960
void encoder_ts57n8501_reset_errors(void);
6061
void encoder_ts57n8501_reset_multiturn(void);
6162
AS504x_diag encoder_AS504x_get_diag(void);
63+
float encode_bissc_get_time_between_reads(void);
6264

6365
#define AS504x_SPI_READ_BIT 0x4000
6466
#define AS504x_SPI_WRITE_BIT 0x0000

‎mc_interface.c‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,21 @@ static void init_sensor_port(volatile mc_configuration *conf) {
188188
encoder_init_ts5700n8501();
189189
} break;
190190

191+
case SENSOR_PORT_MODE_BISSC_SPI:{
192+
// disable APP if it use the main extension port
193+
app_configuration *appconf = mempools_alloc_appconf();
194+
conf_general_read_app_configuration(appconf);
195+
if (appconf->app_to_use == APP_ADC ||
196+
appconf->app_to_use == APP_UART ||
197+
appconf->app_to_use == APP_PPM_UART ||
198+
appconf->app_to_use == APP_ADC_UART) {
199+
appconf->app_to_use = APP_NONE;
200+
conf_general_store_app_configuration(appconf);
201+
}
202+
mempools_free_appconf(appconf);
203+
encoder_init_bissc_spi(motor_now()->m_conf.m_encoder_counts);
204+
} break;
205+
191206
default:
192207
SENSOR_PORT_5V();
193208
break;

‎terminal.c‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -867,7 +867,8 @@ void terminal_process_string(char *str) {
867867
mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_MT6816_SPI ||
868868
mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205 ||
869869
mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501 ||
870-
mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) {
870+
mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN ||
871+
mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_BISSC_SPI) {
871872

872873
if (mcconf->m_sensor_port_mode != SENSOR_PORT_MODE_AS5047_SPI) {
873874
commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%",
@@ -897,6 +898,11 @@ void terminal_process_string(char *str) {
897898
(double)encoder_get_no_magnet_error_rate() * (double)100.0);
898899
}
899900

901+
if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_BISSC_SPI) {
902+
commands_printf("Delay between 2 bissC request: %.6f",
903+
(double)(encode_bissc_get_time_between_reads() * 1000.0));
904+
}
905+
900906
#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
901907
if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) {
902908
commands_printf("\nAS5047 DIAGNOSTICS:\n"

0 commit comments

Comments
 (0)