Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-05 18:01:21 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'master' into ccm

Browse Source 
# Conflicts:
#	inc/setup.h
#	src/comps/hv.c
#	src/comps/sserial.c
#	src/main.c
This commit is contained in:
Rene Hopf
2018-03-16 06:56:18 +01:00
parent fd23c9486b 5c34297eed
commit 2886a7f485
20 changed files with 707 additions and 608 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@@ -21,6 +21,7 @@ SOURCES += src/stm32f4xx_it.c
SOURCES += src/system_stm32f4xx.c #TODO: update this, system file from cmsis
SOURCES += src/setup.c
SOURCES += src/usb_cdc.c
SOURCES += src/config.c
# SOURCES += src/hal_conf.c
SOURCES += src/hal_tbl.c
@@ -180,6 +181,8 @@ CFLAGS += -std=gnu11
CFLAGS += -ffunction-sections
CFLAGS += -fdata-sections
CFLAGS += -Wall
CFLAGS += -Wmaybe-uninitialized
CFLAGS += -Wuninitialized
CFLAGS += -fno-builtin ## from old
CFLAGS += -nostartfiles
CFLAGS += -Wfatal-errors

4
README.md
View File

@@ -20,7 +20,9 @@ This software is released under the GPLv3.
stmbl
=====
There is a wiki. https://github.com/rene-dev/stmbl/wiki
There will be documentation.
Documentation is starting to be written. See the .adoc files here: https://github.com/rene-dev/stmbl/tree/master/docs/src
**IRC: #stmbl on irc.hackint.eu**
https://webirc.hackint.org/#stmbl

2
conf/festo.txt
View File

@@ -10,3 +10,5 @@ conf0.mot_fb_rev = 1
io0.out0 = fault0.mot_brake
conf0.max_ac_cur = 10
conf0.max_force = 3
res0.freq = 5000
res0.phase = 0.4

4
conf/template/enc_fb0.txt
View File

@@ -1,8 +1,8 @@
load enc_fb
enc_fb0.rt_prio = 2
enc_fb0.res = conf0.mot_fb_res
enc_fb0.sin = adc0.sin3
enc_fb0.cos = adc0.cos3
enc_fb0.sin = adc0.sin0l
enc_fb0.cos = adc0.cos0l
enc_fb0.quad = adc0.quad
fb_switch0.mot_pos = enc_fb0.pos
fb_switch0.mot_abs_pos = enc_fb0.abs_pos

3
conf/template/pid.txt
View File

@@ -17,6 +17,8 @@ load stp
load io
load pe
link conf
hv0.rt_prio = 0.9
hv0.frt_prio = 1
adc0.rt_prio = 1
reslimit0.rt_prio = 3
rev0.rt_prio = 4
@@ -27,7 +29,6 @@ vel2.rt_prio = 6
io0.rt_prio = 7
pid0.rt_prio = 8
fault0.rt_prio = 10
hv0.rt_prio = 11
iit0.rt_prio = 12
sim0.rt_prio = 13
stp0.rt_prio = 14

6
conf/template/res_fb0.txt
View File

@@ -1,8 +1,8 @@
load res
res0.rt_prio = 2
res0.sin = adc0.sin
res0.cos = adc0.cos
adc0.res_en = 1
res0.sin = adc0.sin0
res0.cos = adc0.cos0
adc0.res_mode = res0.res_mode
res0.quad = adc0.quad
res0.poles = conf0.mot_fb_polecount
fb_switch0.mot_pos = res0.pos

17
conf/weiler_x.txt Normal file
View File

@@ -0,0 +1,17 @@
link pid
link pmsm
link enc_fb0
link misc
link sserial
linrev0.scale = -3.5483870968
conf0.r = 0.4
conf0.l = 0.01
conf0.j = 0.00015
conf0.polecount = 2
conf0.max_force = 10
conf0.max_ac_cur = 10
conf0.mot_fb_res = 16384
conf0.mot_fb_offset = -2.842850
fb_switch0.phase_cur = 7
fb_switch0.phase_time = 1
fb_switch0.mot_state = 1

17
conf/weiler_z.txt Normal file
View File

@@ -0,0 +1,17 @@
link pid
link pmsm
link enc_fb0
link misc
link misc
link sserial
linrev0.scale = -2.2
conf0.r = 0.4
conf0.l = 0.01
conf0.j = 0.0004
conf0.polecount = 2
conf0.max_force = 10
conf0.max_ac_cur = 10
conf0.mot_fb_res = 16384
conf0.mot_fb_offset = -2.842850
fb_switch0.phase_cur = 7
fb_switch0.mot_state = 1

2
hw/kicad/v4.0/doc/pinout.md
View File

@@ -38,7 +38,7 @@
| io_canrx | pd0 | can1_rx |
| io_spimosi | pb5, pa9 | spi1_mosi, usart1_tx, (tim3_ch2, tim1_ch2, spi3_mosi) |
| io_spimiso | pb4, pa10, pa15 | spi1_miso, usart1_rx, tim2_ch1, (tim3_ch1, tim1_ch3, spi3_miso) |
| io_spisck | pb3, pa8 | spi1_sck, sw_o usart1_ck, tim2_ch2, (tim1_ch1, spi3_sck) |
| io_spisck | pb3, pa8, pa0 | spi1_sck, usart1_ck, tim2_ch2, uart4_tx (tim1_ch1, spi3_sck, tim5_ch1) |
| io_swclk | pa14 | sw_clk |
| io_swdio | pa13 | sw_dio |
| io_rst | nrst | nrst |

10
inc/hw/hw.h
View File

@@ -123,11 +123,15 @@
//sample times for F4: 3,15,28,56,84,112,144,480
#define RES_SampleTime ADC_SampleTime_3Cycles
// ADC_TIMER_FREQ / RES_TIMER_FREQ / ADC_TR_COUNT \in \N
#define ADC_TR_COUNT 6 // ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) == 60
#define PID_WAVES 4
#define RT_FREQ 5000
#define FRT_FREQ 20000
#define ADC_SAMPLES_IN_RT 240
#define ADC_TRIGGER_FREQ (RT_FREQ * ADC_SAMPLES_IN_RT) // master freq
#define FRT_PRESCALER (ADC_TRIGGER_FREQ / FRT_FREQ)
#define ADC_OVER_FB0 9
#define ADC_OVER_FB1 1
#define ADC_GROUPS (ADC_SAMPLES_IN_RT / (ADC_OVER_FB0 + ADC_OVER_FB1))
#define ADC_TIMER_FREQ 84000000
#define RES_TIMER_FREQ 20000

5
inc/setup.h
View File

@@ -18,7 +18,8 @@
void setup(void);
void setup_res(void);
extern volatile uint32_t ADC_DMA_Buffer0[ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)];
extern volatile uint32_t ADC_DMA_Buffer1[ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)];
extern volatile uint32_t ADC_DMA_Buffer0[ADC_SAMPLES_IN_RT];
extern volatile uint32_t ADC_DMA_Buffer1[ADC_SAMPLES_IN_RT];
RCC_ClocksTypeDef RCC_Clocks;

182
src/comps/adc.c
View File

@@ -8,23 +8,22 @@
#define INPUT_REF (OP_REF * OP_R_OUT_LOW / (OP_R_OUT_HIGH + OP_R_OUT_LOW))
#define INPUT_GAIN (OP_R_FEEDBACK / OP_R_INPUT * OP_R_OUT_LOW / (OP_R_OUT_HIGH + OP_R_OUT_LOW))
#define V_DIFF(ADC, OVER) ((((float)ADC) / (float)(OVER) / ADC_RES * ADC_REF - INPUT_REF) / INPUT_GAIN)
#define V_DIFF2(ADC) (((ADC) / ADC_RES * ADC_REF - INPUT_REF) / INPUT_GAIN)
#define V_DIFF(ADC, OVER) ((((float)(ADC)) / (float)(OVER) / ADC_RES * ADC_REF - INPUT_REF) / INPUT_GAIN)
#define TERM_NUM_WAVES 8
HAL_COMP(adc);
HAL_PIN(sin); //sin output
HAL_PIN(cos); //cos output
HAL_PIN(sin3); //sin output, last quater only
HAL_PIN(cos3); //cos output, last quater only
HAL_PIN(sin0); //sin output
HAL_PIN(cos0); //cos output
HAL_PIN(sin0l); //sin output, last group only
HAL_PIN(cos0l); //cos output, last group only
HAL_PIN(quad); //quadrant of sin/cos
HAL_PIN(sin1); //sin output
HAL_PIN(cos1); //cos output
HAL_PIN(sin1l); //sin output, last group only
HAL_PIN(cos1l); //cos output, last group only
HAL_PIN(res_en); //flip polarity for resolvers
HAL_PIN(res_mode); //polarity flip mode for resolvers
HAL_PIN(sin_gain);
HAL_PIN(cos_gain);
@@ -38,8 +37,8 @@ HAL_PINA(offset, 8);
HAL_PINA(gain, 8);
struct adc_ctx_t {
volatile float txbuf[8][ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)];
volatile uint32_t txbuf_raw[ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)];
volatile float txbuf[8][ADC_SAMPLES_IN_RT];
volatile uint32_t txbuf_raw[ADC_SAMPLES_IN_RT];
uint32_t txpos;
uint32_t send_counter; //send_step counter
volatile uint32_t send; //send buffer state 0=filling, 1=sending
@@ -48,10 +47,11 @@ struct adc_ctx_t {
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct adc_ctx_t *ctx = (struct adc_ctx_t *)ctx_ptr;
struct adc_pin_ctx_t *pins = (struct adc_pin_ctx_t *)pin_ptr;
PINA(gain, 0) = 200;
PINA(gain, 1) = 200;
PINA(gain, 2) = 200;
PINA(gain, 3) = 200;
PINA(gain, 0) = 150;
PINA(gain, 1) = 150;
PINA(gain, 2) = 150;
PINA(gain, 3) = 150;
PINA(gain, 4) = 80;
PIN(sin_gain) = 1.0;
PIN(cos_gain) = 1.0;
ctx->txpos = 0;
@@ -63,93 +63,104 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
struct adc_ctx_t *ctx = (struct adc_ctx_t *)ctx_ptr;
struct adc_pin_ctx_t *pins = (struct adc_pin_ctx_t *)pin_ptr;
float si0[PID_WAVES * ADC_OVER_FB0];
float co0[PID_WAVES * ADC_OVER_FB0];
uint32_t sii0, coi0;
//scaled values for each group
float si0[ADC_GROUPS];
float co0[ADC_GROUPS];
//integral per group
uint32_t sii0;
uint32_t coi0;
//scaled, all groups
float sin0all = 0.0;
float cos0all = 0.0;
#ifdef FB1
float co1[PID_WAVES * ADC_OVER_FB1];
float si1[PID_WAVES * ADC_OVER_FB1];
uint32_t sii1, coi1;
float co1[ADC_GROUPS];
float si1[ADC_GROUPS];
uint32_t sii1;
uint32_t coi1;
#endif
float s_o = PIN(sin_offset);
float c_o = PIN(cos_offset);
float s_g = PIN(sin_gain);
float c_g = PIN(cos_gain);
float s;
float c;
volatile uint32_t *ADC_DMA_Buffer;
// if(DMA_GetCurrentMemoryTarget(DMA2_Stream0) == 0){
// ADC_DMA_Buffer = ADC_DMA_Buffer1;
// }
// else{
if(DMA_GetCurrentMemoryTarget(DMA2_Stream0)){
ADC_DMA_Buffer = ADC_DMA_Buffer1;
}
else{
ADC_DMA_Buffer = ADC_DMA_Buffer0;
// }
for(int i = 0; i < PID_WAVES; i++) {
}
int flip;
int n = PIN(res_mode);
for(int i = 0; i < ADC_GROUPS; i++) { //each adc sampling group
if(n > 0 && i % (2 * n) >= n) {
flip = -1;
} else {
flip = 1;
}
sii0 = 0;
coi0 = 0;
for(int j = 0; j < ADC_OVER_FB0; j++) { //each adc sample of fb0
sii0 += ADC_DMA_Buffer[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] & 0x0000ffff;
coi0 += ADC_DMA_Buffer[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] >> 16;
}
si0[i] = flip * s_g * V_DIFF(sii0, ADC_OVER_FB0) + s_o;
co0[i] = flip * c_g * V_DIFF(coi0, ADC_OVER_FB0) + c_o;
sin0all += si0[i];
cos0all += co0[i];
#ifdef FB1
sii1 = 0;
coi1 = 0;
for(int j = ADC_OVER_FB0; j < ADC_OVER_FB0 + ADC_OVER_FB1; j++) { //each adc sample of fb1
sii1 += ADC_DMA_Buffer[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] & 0x0000ffff;
coi1 += ADC_DMA_Buffer[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] >> 16;
}
si1[i] = s_g * V_DIFF(sii1, ADC_OVER_FB1) + s_o;
co1[i] = c_g * V_DIFF(coi1, ADC_OVER_FB1) + c_o;
#endif
for(int j = 0; j < ADC_TR_COUNT; j++) {
//ADC dual mode puts both channels in one word, right aligned.
for(int k = 0; k < ADC_OVER_FB0; k++) {
sii0 += ADC_DMA_Buffer[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] & 0x0000ffff;
coi0 += ADC_DMA_Buffer[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] >> 16;
}
#ifdef FB1
for(int k = ADC_OVER_FB0; k < ADC_OVER_FB0 + ADC_OVER_FB1; k++) {
sii1 += ADC_DMA_Buffer[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] & 0x0000ffff;
coi1 += ADC_DMA_Buffer[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] >> 16;
}
#endif
}
si0[i] = s_g * V_DIFF(sii0, ADC_TR_COUNT * ADC_OVER_FB0) + s_o;
co0[i] = c_g * V_DIFF(coi0, ADC_TR_COUNT * ADC_OVER_FB0) + c_o;
#ifdef FB1
si1[i] = s_g * V_DIFF(sii1, ADC_TR_COUNT * ADC_OVER_FB1) + s_o;
co1[i] = c_g * V_DIFF(coi1, ADC_TR_COUNT * ADC_OVER_FB1) + c_o;
#endif
}
//copy dma buffer for plotting TODO: use dual mode, for zero copy
if(ctx->send == 0) {
memcpy(ctx->txbuf_raw, ADC_DMA_Buffer, ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1) * 4);
memcpy((void *)(ctx->txbuf_raw), (void *)ADC_DMA_Buffer, ADC_SAMPLES_IN_RT * 4);
ctx->send = 1;
}
PIN(sin3) = si0[3];
PIN(cos3) = co0[3];
PIN(sin0l) = si0[ADC_GROUPS - 1];
PIN(cos0l) = co0[ADC_GROUPS - 1];
PIN(sin0) = sin0all / (float)ADC_GROUPS;
PIN(cos0) = cos0all / (float)ADC_GROUPS;
#ifdef FB1
PIN(sin1) = si1[3];
PIN(cos1) = co1[3];
PIN(sin1l) = si1[ADC_GROUPS - 1];
PIN(cos1l) = co1[ADC_GROUPS - 1];
#endif
if(PIN(res_en) > 0.0) {
s = (si0[3] - si0[2] + si0[1] - si0[0]) / 4.0;
c = (co0[3] - co0[2] + co0[1] - co0[0]) / 4.0;
} else {
s = (si0[3] + si0[2] + si0[1] + si0[0]) / 4.0;
c = (co0[3] + co0[2] + co0[1] + co0[0]) / 4.0;
}
if(si0[3] >= 0) {
if(co0[3] > 0)
// if(PIN(res_en) > 0.0) {
// s = (si0[3] - si0[2] + si0[1] - si0[0]) / 4.0;
// c = (co0[3] - co0[2] + co0[1] - co0[0]) / 4.0;
// } else {
// s = (si0[3] + si0[2] + si0[1] + si0[0]) / 4.0;
// c = (co0[3] + co0[2] + co0[1] + co0[0]) / 4.0;
// }
//calculate quadrant for sin/cos interpolation
if(si0[ADC_GROUPS - 1] >= 0) {
if(co0[ADC_GROUPS - 1] > 0)
PIN(quad) = 1;
else
PIN(quad) = 2;
} else {
if(co0[3] > 0)
if(co0[ADC_GROUPS - 1] > 0)
PIN(quad) = 4;
else
PIN(quad) = 3;
}
PIN(sin) = s;
PIN(cos) = c;
}
static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct adc_ctx_t *ctx = (struct adc_ctx_t *)ctx_ptr;
struct adc_pin_ctx_t *pins = (struct adc_pin_ctx_t *)pin_ptr;
@@ -157,29 +168,40 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
int tmp = 0;
uint8_t buf[TERM_NUM_WAVES + 3];
int n = PIN(res_mode); //n gruppen pro halbwelle 1-12
int flip;
if(ctx->send == 1 && ctx->send_counter++ >= PIN(send_step) - 1 && PIN(send_step) > 0) {
ctx->send_counter = 0;
for(int i = 0; i < PID_WAVES; i++) {
for(int j = 0; j < ADC_TR_COUNT; j++) {
for(int k = 0; k < ADC_OVER_FB0; k++) {
ctx->txbuf[0][ctx->txpos] = (((i == 0 || i == 2) && (PIN(res_en) > 0.0)) ? -1.0 : 1.0) * V_DIFF2(ctx->txbuf_raw[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] & 0x0000ffff);
ctx->txbuf[1][ctx->txpos] = (((i == 0 || i == 2) && (PIN(res_en) > 0.0)) ? -1.0 : 1.0) * V_DIFF2(ctx->txbuf_raw[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] >> 16);
for(int i = 0; i < ADC_GROUPS; i++) { //each adc sampling group
if(n > 0 && i % (2 * n) >= n) {
flip = -1;
} else {
flip = 1;
}
for(int j = 0; j < ADC_OVER_FB0; j++) { //each adc sample of fb0
ctx->txbuf[0][ctx->txpos] = flip * V_DIFF(ctx->txbuf_raw[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] & 0x0000ffff, 1);
ctx->txbuf[1][ctx->txpos] = flip * V_DIFF(ctx->txbuf_raw[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] >> 16, 1);
ctx->txbuf[2][ctx->txpos] = 0;
ctx->txbuf[3][ctx->txpos] = 0;
ctx->txbuf[4][ctx->txpos] = flip;
ctx->txpos++;
}
#ifdef FB1
for(int k = ADC_OVER_FB0; k < ADC_OVER_FB0 + ADC_OVER_FB1; k++) {
ctx->txbuf[2][ctx->txpos] = V_DIFF2(ctx->txbuf_raw[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] & 0x0000ffff);
ctx->txbuf[3][ctx->txpos] = V_DIFF2(ctx->txbuf_raw[i * ADC_TR_COUNT * (ADC_OVER_FB0 + ADC_OVER_FB1) + j * (ADC_OVER_FB0 + ADC_OVER_FB1) + k] >> 16);
for(int j = ADC_OVER_FB0; j < ADC_OVER_FB0 + ADC_OVER_FB1; j++) { //each adc sample of fb1
ctx->txbuf[0][ctx->txpos] = 0;
ctx->txbuf[1][ctx->txpos] = 0;
ctx->txbuf[2][ctx->txpos] = V_DIFF(ctx->txbuf_raw[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] & 0x0000ffff, 1);
ctx->txbuf[3][ctx->txpos] = V_DIFF(ctx->txbuf_raw[i * (ADC_OVER_FB0 + ADC_OVER_FB1) + j] >> 16, 1);
ctx->txbuf[4][ctx->txpos] = flip;
ctx->txpos++;
}
#endif
}
}
ctx->txpos = 0;
buf[0] = 255;
for(int k = 0; k < ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1); k++) {
for(int i = 0; i < TERM_NUM_WAVES; i++) {
ctx->txpos = 0;
buf[0] = 255;//start of waves
for(int k = 0; k < ADC_SAMPLES_IN_RT; k++) { //each sample
for(int i = 0; i < TERM_NUM_WAVES; i++) { //each wave
tmp = (ctx->txbuf[i][k] + PINA(offset, i)) * PINA(gain, i) + 128;
buf[i + 1] = CLAMP(tmp, 1, 254);
}

14
src/comps/hv.c
View File

@@ -65,6 +65,7 @@ struct hv_ctx_t {
f3_state_data_t state;
uint16_t addr;
uint16_t timeout;
uint8_t frt_slot;
};
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
@@ -166,8 +167,14 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct hv_ctx_t *ctx = (struct hv_ctx_t *)ctx_ptr;
struct hv_pin_ctx_t *pins = (struct hv_pin_ctx_t *)pin_ptr;
ctx->frt_slot = 0;
}
static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct hv_ctx_t *ctx = (struct hv_ctx_t *)ctx_ptr;
struct hv_pin_ctx_t *pins = (struct hv_pin_ctx_t *)pin_ptr;
ctx->frt_slot++;
if(ctx->frt_slot == 3){
float e = PIN(en);
float pos = PIN(pos);
float vel = PIN(vel);
@@ -263,8 +270,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
DMA_Cmd(UART_DRV_TX_DMA, DISABLE);
DMA_ClearFlag(UART_DRV_TX_DMA, UART_DRV_TX_DMA_TCIF);
DMA_Cmd(UART_DRV_TX_DMA, ENABLE);
}
// PIN(power) = PIN(dc_cur) * PIN(dc_volt);
// if(PIN(pwm_volt) > 0.0){
// PIN(dc_cur_sim) = ABS(PIN(iq)) / PIN(pwm_volt) * sqrtf(a*a + b*b)*0.5 + PIN(dc_cur_sim)*0.5;
@@ -279,7 +285,7 @@ hal_comp_t hv_comp_struct = {
.name = "hv",
.nrt = 0,
.rt = rt_func,
.frt = 0,
.frt = frt_func,
.nrt_init = nrt_init,
.rt_start = 0,
.frt_start = 0,

29
src/comps/res.c
View File

@@ -22,7 +22,9 @@ HAL_PIN(cos);
HAL_PIN(enable);
HAL_PIN(error);
HAL_PIN(state);
HAL_PIN(tim_oc);
HAL_PIN(phase);//phase adjust
HAL_PIN(res_mode);//resolver mode output, calculated form frequency
HAL_PIN(freq);
// TODO: in hal stop, reset adc dma
@@ -35,8 +37,9 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct res_ctx_t *ctx = (struct res_ctx_t *)ctx_ptr;
struct res_pin_ctx_t *pins = (struct res_pin_ctx_t *)pin_ptr;
PIN(poles) = 1.0;
PIN(tim_oc) = 0.85;
PIN(phase) = 0.85;
PIN(min_amp) = 0.15;
PIN(freq) = 10000;
}
static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct res_ctx_t *ctx = (struct res_ctx_t *)ctx_ptr;
@@ -55,7 +58,7 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = ADC_TR_COUNT - 1; // 20kHz
TIM_TimeBaseStructure.TIM_Period = ADC_TRIGGER_FREQ / FRT_FREQ - 1; // 20kHz
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
@@ -64,6 +67,19 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
TIM_ARRPreloadConfig(TIM4, ENABLE);
TIM_Cmd(TIM4, ENABLE);
#endif
//12-1 40khz
//15-1 35khz
//20-1 30khz 2
//24-1 25khz
//30-1 20khz 3
//40-1 15khz 4
//60-1 10khz 6 default
//120-1 5khz 12
//res_en = ADC_GROUPS/2/(res_freq/rt_freq)
//arr = ADC_TRIGGER_FREQ/2/res_freq
// resolver reference signal OC
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
@@ -101,8 +117,11 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
struct res_ctx_t *ctx = (struct res_ctx_t *)ctx_ptr;
struct res_pin_ctx_t *pins = (struct res_pin_ctx_t *)pin_ptr;
//TODO: arr can change!
FB0_RES_REF_TIM->CCR3 = (int)CLAMP(PIN(tim_oc) * FB0_RES_REF_TIM->ARR, 0, FB0_RES_REF_TIM->ARR - 1);
uint32_t mult = CLAMP(PIN(freq) / RT_FREQ + 0.5, 1, 4);
PIN(freq) = RT_FREQ * mult;
FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / (RT_FREQ * mult) - 1;
FB0_RES_REF_TIM->CCR3 = (int)CLAMP(PIN(phase) * FB0_RES_REF_TIM->ARR, 0, FB0_RES_REF_TIM->ARR - 1);
PIN(res_mode) = ADC_GROUPS / 2 / mult;
float s = 0.0;
float c = 0.0;
float a = 0.0;

219
src/comps/sserial.c
View File

@@ -65,27 +65,21 @@ struct sserial_ctx_t {
uint32_t foo;
};
static volatile uint8_t rxbuf[128] __attribute__((section(".ram")));
static volatile uint8_t txbuf[128] __attribute__((section(".ram")));
volatile uint8_t rxbuf[128] __attribute__((section(".ram")));//rx dma buffer
volatile uint8_t txbuf[128] __attribute__((section(".ram")));//tx dma buffer
uint16_t address; //current address pointer
int rxpos;
discovery_rpc_t discovery;
int rxpos;//read pointer for rx ringbuffer
uint32_t timeout;
lbp_t lbp;
char name[] = LBPCardName;
int bufferpos;
int available;
const char name[] = LBPCardName;
unit_no_t unit;
uint32_t max_waste_ticks;
uint32_t block_bytes;
int printdebug;
uint8_t printbuf[16];
#pragma pack(1)
//*****************************************************************************
uint8_t sserial_slave[] = {
0x0B, 0x09, 0x9B, 0x01, 0xB5, 0x01, 0x00, 0x00, // 0..7
0x0B,0x09,0x8B,0x01,0xA5,0x01,0x00,0x00,// 0..7
0x00,0x00,0x00,0x00,0xA0,0x20,0x10,0x80,// 8..15
0x00,0x00,0x80,0xFF,0x00,0x00,0x80,0x7F,// 16..23
0x08,0x00,0x72,0x61,0x64,0x00,0x70,0x6F,// 24..31
@@ -96,62 +90,65 @@ uint8_t sserial_slave[] = {
0x6C,0x5F,0x63,0x6D,0x64,0x00,0x00,0x00,// 64..71
0xA0,0x04,0x01,0x80,0x00,0x00,0x00,0x00,// 72..79
0x00,0x00,0x80,0x3F,0x46,0x00,0x6E,0x6F,// 80..87
0x6E, 0x65, 0x00, 0x6F, 0x75, 0x74, 0x70, 0x75, // 88..95
0x74, 0x5F, 0x70, 0x69, 0x6E, 0x73, 0x00, 0x00, // 96..103
0xA0, 0x01, 0x07, 0x80, 0x00, 0x00, 0x00, 0x00, // 104..111
0x00, 0x00, 0x80, 0x3F, 0x67, 0x00, 0x6E, 0x6F, // 112..119
0x6E, 0x65, 0x00, 0x65, 0x6E, 0x61, 0x62, 0x6C, // 120..127
0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 128..135
0xA0, 0x20, 0x10, 0x00, 0x00, 0x00, 0x80, 0xFF, // 136..143
0x00, 0x00, 0x80, 0x7F, 0x82, 0x00, 0x72, 0x61, // 144..151
0x64, 0x00, 0x70, 0x6F, 0x73, 0x5F, 0x66, 0x62, // 152..159
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 160..167
0xA0, 0x20, 0x10, 0x00, 0x00, 0x00, 0x80, 0xFF, // 168..175
0x00, 0x00, 0x80, 0x7F, 0xA1, 0x00, 0x72, 0x61, // 176..183
0x64, 0x00, 0x76, 0x65, 0x6C, 0x5F, 0x66, 0x62, // 184..191
0x00, 0x00, 0x00, 0x00, 0xA0, 0x08, 0x03, 0x00, // 192..199
0x00, 0x00, 0xF0, 0xC1, 0x00, 0x00, 0xF0, 0x41, // 200..207
0xC1, 0x00, 0x41, 0x00, 0x63, 0x75, 0x72, 0x72, // 208..215
0x65, 0x6E, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, // 216..223
0xA0, 0x04, 0x01, 0x00, 0x00, 0x00, 0xC8, 0xC2, // 224..231
0x00, 0x00, 0xC8, 0x42, 0xDC, 0x00, 0x6E, 0x6F, // 232..239
0x6E, 0x65, 0x00, 0x69, 0x6E, 0x70, 0x75, 0x74, // 240..247
0x5F, 0x70, 0x69, 0x6E, 0x73, 0x00, 0x00, 0x00, // 248..255
0xA0, 0x01, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, // 256..263
0x00, 0x00, 0x80, 0x3F, 0xFE, 0x00, 0x6E, 0x6F, // 264..271
0x6E, 0x65, 0x00, 0x66, 0x61, 0x75, 0x6C, 0x74, // 272..279
0x00, 0x00, 0x00, 0x00, 0xA0, 0x01, 0x07, 0x40, // 280..287
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x3F, // 288..295
0x19, 0x01, 0x6E, 0x6F, 0x6E, 0x65, 0x00, 0x69, // 296..303
0x6E, 0x64, 0x65, 0x78, 0x5F, 0x65, 0x6E, 0x61, // 304..311
0x62, 0x6C, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, // 312..319
0xA0, 0x20, 0x10, 0x80, 0x00, 0x00, 0x80, 0xFF, // 320..327
0x00, 0x00, 0x80, 0x7F, 0x3C, 0x01, 0x6E, 0x6F, // 328..335
0x6E, 0x65, 0x00, 0x73, 0x63, 0x61, 0x6C, 0x65, // 336..343
0x00, 0xB0, 0x00, 0x01, 0x00, 0x50, 0x6F, 0x73, // 344..351
0x69, 0x74, 0x69, 0x6F, 0x6E, 0x20, 0x6D, 0x6F, // 352..359
0x64, 0x65, 0x00, 0x00, 0xA0, 0x02, 0x00, 0x00, // 360..367
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 368..375
0x6B, 0x01, 0x00, 0x70, 0x61, 0x64, 0x64, 0x69, // 376..383
0x6E, 0x67, 0x00, 0x00, 0xA0, 0x02, 0x00, 0x80, // 384..391
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // 392..399
0x83, 0x01, 0x00, 0x70, 0x61, 0x64, 0x64, 0x69, // 400..407
0x6E, 0x67, 0x00, 0x0C, 0x00, 0x2C, 0x00, 0x48, // 408..415
0x00, 0x68, 0x00, 0x88, 0x00, 0xA8, 0x00, 0xC4, // 416..423
0x00, 0xE0, 0x00, 0x00, 0x01, 0x1C, 0x01, 0x6C, // 424..431
0x01, 0x84, 0x01, 0x00, 0x00, 0x40, 0x01, 0x59, // 432..439
0x6E,0x65,0x00,0x6F,0x75,0x74,0x00,0x00,// 88..95
0xA0,0x01,0x07,0x80,0x00,0x00,0x00,0x00,// 96..103
0x00,0x00,0x80,0x3F,0x5F,0x00,0x6E,0x6F,// 104..111
0x6E,0x65,0x00,0x65,0x6E,0x61,0x62,0x6C,// 112..119
0x65,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// 120..127
0xA0,0x20,0x10,0x00,0x00,0x00,0x80,0xFF,// 128..135
0x00,0x00,0x80,0x7F,0x7A,0x00,0x72,0x61,// 136..143
0x64,0x00,0x70,0x6F,0x73,0x5F,0x66,0x62,// 144..151
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// 152..159
0xA0,0x20,0x10,0x00,0x00,0x00,0x80,0xFF,// 160..167
0x00,0x00,0x80,0x7F,0x99,0x00,0x72,0x61,// 168..175
0x64,0x00,0x76,0x65,0x6C,0x5F,0x66,0x62,// 176..183
0x00,0x00,0x00,0x00,0xA0,0x08,0x03,0x00,// 184..191
0x00,0x00,0xF0,0xC1,0x00,0x00,0xF0,0x41,// 192..199
0xB9,0x00,0x41,0x00,0x63,0x75,0x72,0x72,// 200..207
0x65,0x6E,0x74,0x00,0x00,0x00,0x00,0x00,// 208..215
0xA0,0x04,0x01,0x00,0x00,0x00,0xC8,0xC2,// 216..223
0x00,0x00,0xC8,0x42,0xD4,0x00,0x6E,0x6F,// 224..231
0x6E,0x65,0x00,0x69,0x6E,0x00,0x00,0x00,// 232..239
0xA0,0x01,0x07,0x00,0x00,0x00,0x00,0x00,// 240..247
0x00,0x00,0x80,0x3F,0xEE,0x00,0x6E,0x6F,// 248..255
0x6E,0x65,0x00,0x66,0x61,0x75,0x6C,0x74,// 256..263
0x00,0x00,0x00,0x00,0xA0,0x01,0x07,0x40,// 264..271
0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x3F,// 272..279
0x09,0x01,0x6E,0x6F,0x6E,0x65,0x00,0x69,// 280..287
0x6E,0x64,0x65,0x78,0x5F,0x65,0x6E,0x61,// 288..295
0x62,0x6C,0x65,0x00,0x00,0x00,0x00,0x00,// 296..303
0xA0,0x20,0x10,0x80,0x00,0x00,0x80,0xFF,// 304..311
0x00,0x00,0x80,0x7F,0x2C,0x01,0x6E,0x6F,// 312..319
0x6E,0x65,0x00,0x73,0x63,0x61,0x6C,0x65,// 320..327
0x00,0xB0,0x00,0x01,0x00,0x50,0x6F,0x73,// 328..335
0x69,0x74,0x69,0x6F,0x6E,0x20,0x6D,0x6F,// 336..343
0x64,0x65,0x00,0x00,0xA0,0x02,0x00,0x00,// 344..351
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// 352..359
0x5B,0x01,0x00,0x70,0x61,0x64,0x64,0x69,// 360..367
0x6E,0x67,0x00,0x00,0xA0,0x02,0x00,0x80,// 368..375
0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,// 376..383
0x73,0x01,0x00,0x70,0x61,0x64,0x64,0x69,// 384..391
0x6E,0x67,0x00,0x0C,0x00,0x2C,0x00,0x48,// 392..399
0x00,0x60,0x00,0x80,0x00,0xA0,0x00,0xBC,// 400..407
0x00,0xD8,0x00,0xF0,0x00,0x0C,0x01,0x5C,// 408..415
0x01,0x74,0x01,0x00,0x00,0x30,0x01,0x49,// 416..423
0x01,0x00,0x00,
};
uint16_t sserial_ptocp = 0x019B;
uint16_t sserial_gtocp = 0x01B5;
const discovery_rpc_t discovery = {
.ptocp = 0x018B,
.gtocp = 0x01A5,
.input = 11,
.output = 9,
};
typedef struct {
float pos_cmd;
float vel_cmd;
uint32_t output_pins_0 : 1;
uint32_t output_pins_1 : 1;
uint32_t output_pins_2 : 1;
uint32_t output_pins_3 : 1;
uint32_t out_0 : 1;
uint32_t out_1 : 1;
uint32_t out_2 : 1;
uint32_t out_3 : 1;
uint32_t enable : 1;
uint32_t index_enable : 1;
uint32_t padding : 2;
@@ -162,22 +159,23 @@ typedef struct {
float pos_fb;
float vel_fb;
int8_t current;
uint32_t input_pins_0 : 1;
uint32_t input_pins_1 : 1;
uint32_t input_pins_2 : 1;
uint32_t input_pins_3 : 1;
uint32_t in_0 : 1;
uint32_t in_1 : 1;
uint32_t in_2 : 1;
uint32_t in_3 : 1;
uint32_t fault : 1;
uint32_t index_enable : 1;
uint32_t padding : 2;
} sserial_in_process_data_t; //size:10 bytes
_Static_assert(sizeof(sserial_in_process_data_t) == 10, "sserial_in_process_data_t size error!");
//global name:scale addr:0x13c size:32 dir:0x80
#define scale_address 316
//global name:scale addr:0x12c size:32 dir:0x80
#define scale_address 300
//******************************************************************************
sserial_out_process_data_t data_out;
sserial_in_process_data_t data_in;
uint8_t crc_reuest(uint8_t len) {
static sserial_out_process_data_t data_out;
static sserial_in_process_data_t data_in;
static uint8_t crc_reuest(uint8_t len) {
uint8_t crc = crc8_init();
for(int i = rxpos; i < rxpos + len; i++) {
crc = crc8_update(crc, (void *)&(rxbuf[i % sizeof(rxbuf)]), 1);
@@ -186,13 +184,13 @@ uint8_t crc_reuest(uint8_t len) {
return crc == rxbuf[(rxpos + len) % sizeof(rxbuf)];
}
uint8_t crc8(uint8_t *addr, uint8_t len) {
static uint8_t crc8(uint8_t *addr, uint8_t len) {
uint8_t crc = crc8_init();
crc = crc8_update(crc, addr, len);
return crc8_finalize(crc);
}
void send(uint8_t len, uint8_t docrc) {
static void send(uint8_t len, uint8_t docrc) {
timeout = 0;
if(docrc) {
txbuf[len] = crc8((uint8_t *)txbuf, len);
@@ -252,12 +250,7 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
USART_Init(USART1, &USART_InitStruct);
USART_InitStruct.USART_Mode = USART_Mode_Tx;
USART_Init(UART4, &USART_InitStruct);
//USART_HalfDuplexCmd(USART1,ENABLE);
//USART_InitStruct.USART_Mode = USART_Mode_Tx;
//USART_Init(USART1, &USART_InitStruct);
//USART_Cmd(USART1, ENABLE);
USART_Cmd(USART1, ENABLE);
USART_Cmd(UART4, ENABLE);
@@ -311,8 +304,6 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA1_Stream4, &DMA_InitStructure);
//DMA_Cmd(DMA1_Stream7, ENABLE);
USART_DMACmd(UART4, USART_DMAReq_Tx, ENABLE);
//tx enable
@@ -331,26 +322,19 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
rxpos = 0;
timeout = 1000; //make sure we start in timeout
discovery.input = sizeof(sserial_in_process_data_t) + 1; //+1 for fault byte
discovery.output = sizeof(sserial_out_process_data_t);
discovery.ptocp = sserial_ptocp;
discovery.gtocp = sserial_gtocp;
//bytes to wait before expected end of transmission to prevent timeouts
block_bytes = 3;
block_bytes = 4;
//calculate timeout in systicks for block_bytes
max_waste_ticks = (1.0 / 2500000.0) * 11.0 * (float)block_bytes / (1.0f / (float)hal_get_systick_freq());
}
static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct res_ctx_t * ctx = (struct res_ctx_t *)ctx_ptr;
struct sserial_pin_ctx_t *pins = (struct sserial_pin_ctx_t *)pin_ptr;
for(int j = 0; j < 1; j++) {
//next received packet will be written to bufferpos
bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA2_Stream5);
uint32_t bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA2_Stream5);
//how many packets we have the the rx buffer for processing
available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf);
uint32_t available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf);
if(available >= 1) {
lbp.byte = rxbuf[rxpos];
@@ -373,8 +357,6 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
}
send(1, 1);
rxpos += 2;
} else {
continue;
}
} else if(lbp.ct == CT_LOCAL && lbp.wr == 1) { //local write, cmd+data+crc = 3b
timeout = 0;
@@ -389,8 +371,6 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
txbuf[0] = 0x00;
send(1, 0);
rxpos += 3;
} else {
continue;
}
} else if(lbp.ct == CT_RPC) { //RPC TODO: check for ct should not required for rpc
timeout = 0;
@@ -425,10 +405,10 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
data_in.pos_fb = PIN(pos_fb) + PIN(vel_fb) * PIN(pos_advance);
data_in.vel_fb = PIN(vel_fb);
data_in.current = CLAMP(PIN(current) / (30.0f / 128.0f), -127, 127);
data_in.input_pins_0 = (PIN(in0) > 0) ? 1 : 0;
data_in.input_pins_1 = (PIN(in1) > 0) ? 1 : 0;
data_in.input_pins_2 = (PIN(in2) > 0) ? 1 : 0;
data_in.input_pins_3 = (PIN(in3) > 0) ? 1 : 0;
data_in.in_0 = (PIN(in0) > 0) ? 1 : 0;
data_in.in_1 = (PIN(in1) > 0) ? 1 : 0;
data_in.in_2 = (PIN(in2) > 0) ? 1 : 0;
data_in.in_3 = (PIN(in3) > 0) ? 1 : 0;
data_in.fault = (PIN(fault) > 0) ? 1 : 0;
//copy output pins from rx buffer
@@ -445,7 +425,7 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
for(int i = 0; i < (discovery.input - 1); i++) {
txbuf[i + 1] = ((uint8_t *)(&data_in))[i];
}
if(crc_reuest(discovery.output + 1)) {
//send buffer
DMA_SetCurrDataCounter(DMA1_Stream4, discovery.input + 1);
DMA_Cmd(DMA1_Stream4, DISABLE);
@@ -453,17 +433,14 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
DMA_Cmd(DMA1_Stream4, ENABLE);
txbuf[discovery.input] = crc8((uint8_t *)txbuf, discovery.input);
//send(discovery.input, 1);
//we cannot send the reply based on crc, as this causes timeouts TODO: still valid?
if(crc_reuest(discovery.output + 1)) {
timeout = 0;
//set output pins
PIN(pos_cmd) = data_out.pos_cmd;
PIN(pos_cmd_d) = data_out.vel_cmd;
PIN(out0) = data_out.output_pins_0;
PIN(out1) = data_out.output_pins_1;
PIN(out2) = data_out.output_pins_2;
PIN(out3) = data_out.output_pins_3;
PIN(out0) = data_out.out_0;
PIN(out1) = data_out.out_1;
PIN(out2) = data_out.out_2;
PIN(out3) = data_out.out_3;
PIN(enable) = data_out.enable;
} else {
PIN(crc_error)
@@ -479,8 +456,6 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
PIN(enable) = 0;
}
rxpos += discovery.output + 2;
} else {
continue;
}
} else if(lbp.ct == CT_RW && lbp.wr == 0) { //read
//size = 1 + 2*lbp.as + 1
@@ -501,18 +476,12 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
if(lbp.ai) { //auto increment address by datasize
address += (1 << lbp.ds);
}
} else {
continue;
}
} else if(lbp.ct == CT_RW && lbp.wr == 1) { // lbp (addr1 addr2) data0, data1,...
//size = 1 + 2*ai +ds +crc
int size = 2 * lbp.as + (1 << lbp.ds) + 2;
timeout = 0;
if(available >= size) {
// for(int i = 0; i < size; i++) {
// printbuf[i] = rxbuf[(rxpos + i) % sizeof(rxbuf)];
// }
// printdebug = size;
if(lbp.as) { //address included in command = cmd+addr+addr+crc
address = rxbuf[(rxpos + 1) % sizeof(rxbuf)] + (rxbuf[(rxpos + 2) % sizeof(rxbuf)] << 8);
rxpos += 3;
@@ -533,8 +502,6 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
if(lbp.ai) { //auto increment address by datasize
address += (1 << lbp.ds);
}
} else {
continue;
}
} else {
//TODO: handle unkown packet
@@ -557,31 +524,9 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
PIN(error) = 0;
}
rxpos = rxpos % sizeof(rxbuf);
}
timeout++;
}
// static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct sserial_ctx_t *ctx = (struct sserial_ctx_t *)ctx_ptr;
// struct sserial_pin_ctx_t *pins = (struct sserial_pin_ctx_t *)pin_ptr;
// if(printdebug) {
// printf("write: ");
// for(int i = 0; i < printdebug; i++) {
// printf("0x%x ", printbuf[i]);
// }
// printf("\n");
// printf("sserial_slave @316: ");
// for(int i = 0; i < 4; i++) {
// printf("0x%x ", sserial_slave[i + 316]);
// }
// printf("\n");
// float foo;
// memcpy(&foo, &sserial_slave[316], 4);
// printf("value: %f\n", foo);
// printdebug = 0;
// }
// }
const hal_comp_t sserial_comp_struct = {
.name = "sserial",
.nrt = 0, //nrt_func,

1
src/comps/yaskawa.c
View File

@@ -195,7 +195,6 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(count > 80){
int pol = 0;
int read_counter = 0;
int write_counter = 0;
int counter = 0;
for(int i = 0;i < ARRAY_SIZE(yaskawa_reply);i++){

90
src/config.c Normal file
View File

@@ -0,0 +1,90 @@
/*
* This file is part of the stmbl project.
*
* Copyright (C) 2013-2015 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2013-2015 Nico Stute <crinq@crinq.de>
*
* This program 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.
*
* This program 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 <stdio.h>
#include <string.h>
#include "hal.h"
#include "commands.h"
#include "stm32f4xx_conf.h"
char config[15 * 1024] __attribute__((section(".ram")));
const char *config_ro = (char *)0x08008000;
void confcrc(char *ptr) {
uint32_t len = strnlen(config, sizeof(config) - 1);
CRC_ResetDR();
uint32_t crc = CRC_CalcBlockCRC((uint32_t *)config, len / 4);
for(int i = 0; i < len; i++) {
printf("%x ", config[i]);
}
printf("\n");
printf("size: %lu words: %lu crc:%lx\n", len, len / 4, crc);
}
COMMAND("confcrc", confcrc, "foo");
void flashloadconf(char *ptr) {
strncpy(config, config_ro, sizeof(config));
}
COMMAND("flashloadconf", flashloadconf, "load config from flash");
void flashsaveconf(char *ptr) {
printf("erasing flash page...\n");
FLASH_Unlock();
if(FLASH_EraseSector(FLASH_Sector_2, VoltageRange_3) != FLASH_COMPLETE) {
printf("error!\n");
FLASH_Lock();
return;
}
printf("saving conf\n");
int i = 0;
int ret = 0;
do {
ret = FLASH_ProgramByte((uint32_t)(config_ro + i), config[i]) != FLASH_COMPLETE;
if(ret) {
printf("error writing %i\n", ret);
break;
}
} while(config[i++] != 0);
printf("OK %i bytes written\n", i);
FLASH_Lock();
}
COMMAND("flashsaveconf", flashsaveconf, "save config to flash");
void loadconf(char *ptr) {
hal_parse(config);
}
COMMAND("loadconf", loadconf, "parse config");
void showconf(char *ptr) {
printf("%s", config_ro);
}
COMMAND("showconf", showconf, "show config");
void appendconf(char *ptr) {
printf("adding %s\n", ptr);
strncat(config, ptr, sizeof(config) - 1);
strncat(config, "\n", sizeof(config) - 1);
}
COMMAND("appendconf", appendconf, "append string to config");
void deleteconf(char *ptr) {
config[0] = '\0';
}
COMMAND("deleteconf", deleteconf, "delete config");

99
src/main.c
View File

@@ -68,8 +68,35 @@ void DMA2_Stream0_IRQHandler(void) {
}
void bootloader(char *ptr) {
*((unsigned long *)0x2001C000) = 0xDEADBEEF; //set bootloader trigger
NVIC_SystemReset();
hal_stop();
NVIC_DisableIRQ(TIM_SLAVE_IRQ);
NVIC_DisableIRQ(DMA2_Stream0_IRQn);
NVIC_DisableIRQ(SysTick_IRQn);
void (*SysMemBootJump)(void);
volatile uint32_t addr = 0x1FFF0000;
RCC_DeInit();
SysTick->CTRL = 0;
SysTick->LOAD = 0;
SysTick->VAL = 0;
RCC->AHB1RSTR = 0x22E017FF;
RCC->AHB1RSTR = 0;
RCC->AHB2RSTR = 0xF1;
RCC->AHB2RSTR = 0;
RCC->AHB3RSTR = 0x1;
RCC->AHB3RSTR = 0;
RCC->APB1RSTR = 0xF6FEC9FF;
RCC->APB1RSTR = 0;
RCC->APB2RSTR = 0x4777933;
RCC->APB2RSTR = 0;
SYSCFG->MEMRMP = 0x01;
SysMemBootJump = (void (*)(void)) (*((uint32_t *)(addr + 4)));
__set_MSP(*(uint32_t *)addr);
SysMemBootJump();
}
COMMAND("bootloader", bootloader, "enter bootloader");
@@ -78,74 +105,6 @@ void nv_reset(char *ptr) {
}
COMMAND("reset", nv_reset, "reset STMBL");
char config[15 * 1024] __attribute__((section(".ram")));
const char *config_ro = (char *)0x08008000;
void confcrc(char *ptr) {
uint32_t len = strnlen(config, sizeof(config) - 1);
CRC_ResetDR();
uint32_t crc = CRC_CalcBlockCRC((uint32_t *)config, len / 4);
for(int i = 0; i < len; i++) {
printf("%x ", config[i]);
}
printf("\n");
printf("size: %lu words: %lu crc:%lx\n", len, len / 4, crc);
}
COMMAND("confcrc", confcrc, "foo");
void flashloadconf(char *ptr) {
strncpy(config, config_ro, sizeof(config));
}
COMMAND("flashloadconf", flashloadconf, "load config from flash");
void flashsaveconf(char *ptr) {
printf("erasing flash page...\n");
FLASH_Unlock();
if(FLASH_EraseSector(FLASH_Sector_2, VoltageRange_3) != FLASH_COMPLETE) {
printf("error!\n");
FLASH_Lock();
return;
}
printf("saving conf\n");
int i = 0;
int ret = 0;
do {
ret = FLASH_ProgramByte((uint32_t)(config_ro + i), config[i]) != FLASH_COMPLETE;
if(ret) {
printf("error writing %i\n", ret);
break;
}
} while(config[i++] != 0);
printf("OK %i bytes written\n", i);
FLASH_Lock();
}
COMMAND("flashsaveconf", flashsaveconf, "save config to flash");
void loadconf(char *ptr) {
hal_parse(config);
}
COMMAND("loadconf", loadconf, "parse config");
void showconf(char *ptr) {
printf("%s", config_ro);
}
COMMAND("showconf", showconf, "show config");
void appendconf(char *ptr) {
printf("adding %s\n", ptr);
strncat(config, ptr, sizeof(config) - 1);
strncat(config, "\n", sizeof(config) - 1);
}
COMMAND("appendconf", appendconf, "append string to config");
void deleteconf(char *ptr) {
config[0] = '\0';
}
COMMAND("deleteconf", deleteconf, "delete config");
int main(void) {
// Relocate interrupt vectors
extern void *g_pfnVectors;

27
src/setup.c
View File

@@ -8,9 +8,10 @@
#include "setup.h"
#include "usbd_cdc_if.h"
#include "defines.h"
volatile uint32_t ADC_DMA_Buffer0 [ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)] __attribute__((section(".ram")));
volatile uint32_t ADC_DMA_Buffer1 [ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)] __attribute__((section(".ram")));
volatile uint32_t ADC_DMA_Buffer0 [ADC_SAMPLES_IN_RT] __attribute__((section(".ram")));
volatile uint32_t ADC_DMA_Buffer1 [ADC_SAMPLES_IN_RT] __attribute__((section(".ram")));
void setup() {
//Enable clocks
@@ -52,7 +53,7 @@ void setup_res() {
RCC_APB1PeriphClockCmd(TIM_MASTER_RCC, ENABLE);
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = ADC_TIMER_FREQ / RES_TIMER_FREQ / ADC_TR_COUNT - 1; // 240khz
TIM_TimeBaseStructure.TIM_Period = ADC_TIMER_FREQ / ADC_TRIGGER_FREQ - 1; //70 1.2MHz
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM_MASTER, &TIM_TimeBaseStructure);
@@ -71,11 +72,11 @@ void setup_res() {
TIM_MASTER_ADC_OC_PRELOAD(TIM_MASTER, TIM_OCPreload_Enable);
TIM_CtrlPWMOutputs(TIM_MASTER, ENABLE);
//slave timer
//slave timer triggers frt
RCC_APB1PeriphClockCmd(TIM_SLAVE_RCC, ENABLE);
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = ADC_TR_COUNT - 1; // 20kHz
TIM_TimeBaseStructure.TIM_Period = ADC_TRIGGER_FREQ / FRT_FREQ - 1; //60 20kHz
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM_SLAVE, &TIM_TimeBaseStructure);
@@ -143,6 +144,12 @@ void setup_res() {
}
#endif
ADC_DiscModeChannelCountConfig(ADC1, 1);
ADC_DiscModeChannelCountConfig(ADC2, 1);
ADC_DiscModeCmd(ADC1, ENABLE);
ADC_DiscModeCmd(ADC2, ENABLE);
ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
//Enable ADC conversion
@@ -157,9 +164,9 @@ void setup_res() {
DMA_InitTypeDef DMA_InitStructure;
DMA_InitStructure.DMA_Channel = DMA_Channel_0;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_DMA_Buffer0;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ADC_DMA_Buffer0;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStructure.DMA_BufferSize = ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1);
DMA_InitStructure.DMA_BufferSize = ARRAY_SIZE(ADC_DMA_Buffer0);
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
@@ -170,9 +177,9 @@ void setup_res() {
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
//TODO: use double buffer
//DMA_DoubleBufferModeConfig(DMA2_Stream0, (uint32_t)ADC_DMA_Buffer1, DMA_Memory_0);
//DMA_DoubleBufferModeCmd(DMA2_Stream0, ENABLE);
DMA_DoubleBufferModeConfig(DMA2_Stream0, (uint32_t)ADC_DMA_Buffer1, DMA_Memory_0);
DMA_DoubleBufferModeCmd(DMA2_Stream0, ENABLE);
DMA_Init(DMA2_Stream0, &DMA_InitStructure);
NVIC_InitTypeDef NVIC_InitStructure;

17
tools/gentable.c
View File

@@ -143,7 +143,7 @@ int main() {
metadata(&(pd_table.pos_cmd), last_pd);
ADD_PROCESS_VAR(("vel_cmd", "rad", 32, DATA_TYPE_FLOAT, DATA_DIRECTION_OUTPUT, -INFINITY, INFINITY));
metadata(&(pd_table.vel_cmd), last_pd);
ADD_PROCESS_VAR(("output_pins", "none", 4, DATA_TYPE_BITS, DATA_DIRECTION_OUTPUT, 0, 1));
ADD_PROCESS_VAR(("out", "none", 4, DATA_TYPE_BITS, DATA_DIRECTION_OUTPUT, 0, 1));
metadata(&(pd_table.output_pins), last_pd);
ADD_PROCESS_VAR(("enable", "none", 1, DATA_TYPE_BOOLEAN, DATA_DIRECTION_OUTPUT, 0, 1));
metadata(&(pd_table.enable), last_pd);
@@ -154,7 +154,7 @@ int main() {
metadata(&(pd_table.vel_fb), last_pd);
ADD_PROCESS_VAR(("current", "A", 8, DATA_TYPE_SIGNED, DATA_DIRECTION_INPUT, -30, 30));
metadata(&(pd_table.current), last_pd);
ADD_PROCESS_VAR(("input_pins", "none", 4, DATA_TYPE_BITS, DATA_DIRECTION_INPUT, -100, 100));
ADD_PROCESS_VAR(("in", "none", 4, DATA_TYPE_BITS, DATA_DIRECTION_INPUT, -100, 100));
metadata(&(pd_table.input_pins), last_pd);
ADD_PROCESS_VAR(("fault", "none", 1, DATA_TYPE_BOOLEAN, DATA_DIRECTION_INPUT, 0, 1));
metadata(&(pd_table.fault), last_pd);
@@ -212,10 +212,15 @@ int main() {
printf("// %i..%i\n", i - 7, i);
}
}
printf("\n};\n");
printf("uint16_t sserial_ptocp = 0x%04X;\n", memory.discovery.ptocp);
printf("uint16_t sserial_gtocp = 0x%04X;\n", memory.discovery.gtocp);
printf("\n");
printf("\n};\n\n");
printf("const discovery_rpc_t discovery = {\n");
printf(" .ptocp = 0x%04X,\n", memory.discovery.ptocp);
printf(" .gtocp = 0x%04X,\n", memory.discovery.gtocp);
printf(" .input = %u,\n", memory.discovery.input);
printf(" .output = %u,\n", memory.discovery.output);
printf("};\n\n");
printf("typedef struct {\n");
ptocp = (uint16_t *)(memory.bytes + memory.discovery.ptocp);
gtocp = (uint16_t *)(memory.bytes + memory.discovery.gtocp);