From ddcd1763b20c5e2f96877166c68b34d483b84079 Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Sat, 3 Feb 2018 07:59:22 +0100 Subject: [PATCH 01/17] res freq --- conf/template/res_fb0.txt | 2 +- inc/hw/hw.h | 10 ++- src/comps/adc.c | 155 ++++++++++++++++++++++---------------- src/comps/res.c | 38 +++++++--- src/setup.c | 27 ++++--- 5 files changed, 142 insertions(+), 90 deletions(-) diff --git a/conf/template/res_fb0.txt b/conf/template/res_fb0.txt index 49111b42..39bcc660 100644 --- a/conf/template/res_fb0.txt +++ b/conf/template/res_fb0.txt @@ -2,7 +2,7 @@ load res res0.rt_prio = 2 res0.sin = adc0.sin res0.cos = adc0.cos -adc0.res_en = 1 +adc0.res_mode = res0.res_mode res0.quad = adc0.quad res0.poles = conf0.mot_fb_polecount fb_switch0.mot_pos = res0.pos diff --git a/inc/hw/hw.h b/inc/hw/hw.h index fabe1c40..c4a2d1a1 100644 --- a/inc/hw/hw.h +++ b/inc/hw/hw.h @@ -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 diff --git a/src/comps/adc.c b/src/comps/adc.c index 8daee749..883e90b3 100644 --- a/src/comps/adc.c +++ b/src/comps/adc.c @@ -8,15 +8,14 @@ #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(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(quad); //quadrant of sin/cos @@ -24,7 +23,7 @@ HAL_PIN(quad); //quadrant of sin/cos HAL_PIN(sin1); //sin output HAL_PIN(cos1); //cos output -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,20 +37,21 @@ 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 + uint32_t send_counter; //send_step counter volatile uint32_t send; //send buffer state 0=filling, 1=sending }; 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,13 +63,13 @@ 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]; + float si0[ADC_GROUPS]; + float co0[ADC_GROUPS]; uint32_t sii0, coi0; #ifdef FB1 - float co1[PID_WAVES * ADC_OVER_FB1]; - float si1[PID_WAVES * ADC_OVER_FB1]; + float co1[ADC_GROUPS]; + float si1[ADC_GROUPS]; uint32_t sii1, coi1; #endif @@ -88,52 +88,59 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ // else{ ADC_DMA_Buffer = ADC_DMA_Buffer0; // } - for(int i = 0; i < PID_WAVES; i++) { - sii0 = 0; - coi0 = 0; -#ifdef FB1 - sii1 = 0; - coi1 = 0; -#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 + int flip; + int n = PIN(res_mode); + for(int i = 0; i < ADC_GROUPS; i++) { + if(n > 0 && i % (2 * n) >= n) { + flip = -1; + } else { + flip = 1; } - 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; + si0[i] = 0; + co0[i] = 0; #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; + si1[i] = 0; + co1[i] = 0; +#endif + //ADC dual mode puts both channels in one word, right aligned. + for(int j = 0; j < ADC_OVER_FB0; j++) { + 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; + } +#ifdef FB1 + for(int j = ADC_OVER_FB0; j < ADC_OVER_FB0 + ADC_OVER_FB1; j++) { + 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; + } +#endif + + si0[i] = s_g * V_DIFF(si0[i], ADC_OVER_FB0) + s_o; + co0[i] = c_g * V_DIFF(co0[i], ADC_OVER_FB0) + c_o; +#ifdef FB1 + si1[i] = s_g * V_DIFF(si1[i], ADC_OVER_FB1) + s_o; + co1[i] = c_g * V_DIFF(co1[i], 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(sin3) = si0[ADC_GROUPS - 1]; + PIN(cos3) = co0[ADC_GROUPS - 1]; #ifdef FB1 - PIN(sin1) = si1[3]; - PIN(cos1) = co1[3]; + PIN(sin1) = si1[ADC_GROUPS - 1]; + PIN(cos1) = 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(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) @@ -150,6 +157,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ 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 +165,41 @@ 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); - 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); - ctx->txpos++; - } -#endif + 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 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++) { + 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); } @@ -190,6 +210,7 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { } } + buf[0] = 0xfe; //trigger servoterm buf[1] = 0x00; if(USB_CDC_is_connected()) { diff --git a/src/comps/res.c b/src/comps/res.c index 7b290d6b..b5f4cc3d 100644 --- a/src/comps/res.c +++ b/src/comps/res.c @@ -23,11 +23,14 @@ HAL_PIN(enable); HAL_PIN(error); HAL_PIN(state); HAL_PIN(tim_oc); +HAL_PIN(tim_arr); +HAL_PIN(res_mode); +HAL_PIN(res_freq); // TODO: in hal stop, reset adc dma struct res_ctx_t { - int lastq; // last quadrant + int lastq; // last quadrant int abspos; // multiturn position }; @@ -37,10 +40,11 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { PIN(poles) = 1.0; PIN(tim_oc) = 0.85; PIN(min_amp) = 0.15; + PIN(res_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; - // struct res_pin_ctx_t *pins = (struct res_pin_ctx_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; ctx->abspos = 0; ctx->lastq = 0; @@ -55,15 +59,29 @@ 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); TIM_SelectSlaveMode(TIM4, TIM_SlaveMode_External1); // Rising edges of the selected trigger (TRGI) clock the counter - TIM_ITRxExternalClockConfig(TIM4, TIM_TS_ITR2); // clk = TIM_MASTER(TIM2) trigger out + TIM_ITRxExternalClockConfig(TIM4, TIM_TS_ITR2); // clk = TIM_MASTER(TIM2) trigger out 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 + + PIN(tim_arr) = ADC_TRIGGER_FREQ / FRT_FREQ - 1; // resolver reference signal OC TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; @@ -102,10 +120,12 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ 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); - - float s = 0.0; - float c = 0.0; - float a = 0.0; + FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / PIN(res_freq) - 1; + PIN(tim_arr) = FB0_RES_REF_TIM->ARR; + PIN(res_mode) = ADC_GROUPS / 2 / (PIN(res_freq) / RT_FREQ); + float s = 0.0; + float c = 0.0; + float a = 0.0; s = PIN(sin); c = PIN(cos); diff --git a/src/setup.c b/src/setup.c index 4b43464d..f256d216 100644 --- a/src/setup.c +++ b/src/setup.c @@ -8,6 +8,7 @@ #include "setup.h" #include "usbd_cdc_if.h" +#include "defines.h" void setup() { //Enable clocks @@ -49,7 +50,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); @@ -68,16 +69,16 @@ 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); TIM_SelectSlaveMode(TIM_SLAVE, TIM_SlaveMode_External1); //Rising edges of the selected trigger (TRGI) clock the counter - TIM_ITRxExternalClockConfig(TIM_SLAVE, TIM_SLAVE_ITR); // clk = TIM_MASTER trigger out + TIM_ITRxExternalClockConfig(TIM_SLAVE, TIM_SLAVE_ITR); // clk = TIM_MASTER trigger out TIM_ARRPreloadConfig(TIM_SLAVE, ENABLE); TIM_Cmd(TIM_SLAVE, ENABLE); @@ -111,13 +112,13 @@ void setup_res() { ADC_DeInit(); ADC_InitTypeDef ADC_InitStructure; ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //data converted will be shifted to right - ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; //Input voltage is converted into a 12bit number giving a maximum value of 4096 - ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; //the conversion is continuous, the input data is converted more than once - ADC_InitStructure.ADC_ExternalTrigConv = TIM_MASTER_ADC; //trigger on rising edge of TIM_MASTER oc + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; //Input voltage is converted into a 12bit number giving a maximum value of 4096 + ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; //the conversion is continuous, the input data is converted more than once + ADC_InitStructure.ADC_ExternalTrigConv = TIM_MASTER_ADC; //trigger on rising edge of TIM_MASTER oc ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Rising; ADC_InitStructure.ADC_NbrOfConversion = ADC_OVER_FB0 + ADC_OVER_FB1; //I think this one is clear :p - ADC_InitStructure.ADC_ScanConvMode = ENABLE; //The scan is configured in one channel - ADC_Init(FB0_SIN_ADC, &ADC_InitStructure); //Initialize ADC with the previous configuration + ADC_InitStructure.ADC_ScanConvMode = ENABLE; //The scan is configured in one channel + ADC_Init(FB0_SIN_ADC, &ADC_InitStructure); //Initialize ADC with the previous configuration ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; ADC_Init(FB0_COS_ADC, &ADC_InitStructure); //Initialize ADC with the previous configuration @@ -140,6 +141,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 @@ -156,7 +163,7 @@ void setup_res() { DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR; 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; From 55885efef10c671d4e3afc46692abbf77657e91a Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Sat, 3 Feb 2018 08:28:40 +0100 Subject: [PATCH 02/17] fix header --- inc/setup.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/inc/setup.h b/inc/setup.h index 8ac47493..3a2bf730 100644 --- a/inc/setup.h +++ b/inc/setup.h @@ -18,7 +18,7 @@ void setup(void); void setup_res(void); -volatile uint32_t ADC_DMA_Buffer0[ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)]; -volatile uint32_t ADC_DMA_Buffer1[ADC_TR_COUNT * PID_WAVES * (ADC_OVER_FB0 + ADC_OVER_FB1)]; +volatile uint32_t ADC_DMA_Buffer0[ADC_SAMPLES_IN_RT]; //240 +volatile uint32_t ADC_DMA_Buffer1[ADC_SAMPLES_IN_RT]; RCC_ClocksTypeDef RCC_Clocks; From 3fefec1b0831b730ebce5924b0019585b94ff6be Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Thu, 8 Mar 2018 13:53:34 +0100 Subject: [PATCH 03/17] sserial: only reply to good crc --- src/comps/sserial.c | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/comps/sserial.c b/src/comps/sserial.c index c279e208..534a2fa2 100644 --- a/src/comps/sserial.c +++ b/src/comps/sserial.c @@ -444,17 +444,14 @@ 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]; } - - //send buffer - DMA_SetCurrDataCounter(DMA1_Stream4, discovery.input + 1); - DMA_Cmd(DMA1_Stream4, DISABLE); - DMA_ClearFlag(DMA1_Stream4, DMA_FLAG_TCIF4); - 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)) { + //send buffer + DMA_SetCurrDataCounter(DMA1_Stream4, discovery.input + 1); + DMA_Cmd(DMA1_Stream4, DISABLE); + DMA_ClearFlag(DMA1_Stream4, DMA_FLAG_TCIF4); + DMA_Cmd(DMA1_Stream4, ENABLE); + txbuf[discovery.input] = crc8((uint8_t *)txbuf, discovery.input); + //send(discovery.input, 1); timeout = 0; //set output pins PIN(pos_cmd) = data_out.pos_cmd; From e8bc1810fb2e22f4e1fa65cc5fb5dbe6b0d6af5b Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Fri, 9 Mar 2018 12:23:17 +0100 Subject: [PATCH 04/17] fixed adc comp, fixed double buffer --- conf/template/enc_fb0.txt | 4 +- conf/template/res_fb0.txt | 4 +- src/comps/adc.c | 97 ++++++++++++++++++++------------------- src/comps/res.c | 19 ++++---- src/setup.c | 8 ++-- 5 files changed, 65 insertions(+), 67 deletions(-) diff --git a/conf/template/enc_fb0.txt b/conf/template/enc_fb0.txt index 6f5ff077..4ef1a74a 100644 --- a/conf/template/enc_fb0.txt +++ b/conf/template/enc_fb0.txt @@ -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 diff --git a/conf/template/res_fb0.txt b/conf/template/res_fb0.txt index 39bcc660..3a467214 100644 --- a/conf/template/res_fb0.txt +++ b/conf/template/res_fb0.txt @@ -1,7 +1,7 @@ load res res0.rt_prio = 2 -res0.sin = adc0.sin -res0.cos = adc0.cos +res0.sin = adc0.sin0 +res0.cos = adc0.cos0 adc0.res_mode = res0.res_mode res0.quad = adc0.quad res0.poles = conf0.mot_fb_polecount diff --git a/src/comps/adc.c b/src/comps/adc.c index 883e90b3..27501dd5 100644 --- a/src/comps/adc.c +++ b/src/comps/adc.c @@ -14,14 +14,14 @@ 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_mode); //polarity flip mode for resolvers @@ -63,77 +63,81 @@ 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; + //scaled values for each group float si0[ADC_GROUPS]; float co0[ADC_GROUPS]; - uint32_t sii0, coi0; - + //integral per group + uint32_t sii0; + uint32_t coi0; + //scaled, all groups + float sin0all; + float cos0all; #ifdef FB1 float co1[ADC_GROUPS]; float si1[ADC_GROUPS]; - uint32_t sii1, coi1; + 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{ - ADC_DMA_Buffer = ADC_DMA_Buffer0; - // } + if(DMA_GetCurrentMemoryTarget(DMA2_Stream0)){ + ADC_DMA_Buffer = ADC_DMA_Buffer1; + } + else{ + ADC_DMA_Buffer = ADC_DMA_Buffer0; + } int flip; int n = PIN(res_mode); - for(int i = 0; i < ADC_GROUPS; i++) { + + + for(int i = 0; i < ADC_GROUPS; i++) { //each adc sampling group if(n > 0 && i % (2 * n) >= n) { flip = -1; } else { flip = 1; } - si0[i] = 0; - co0[i] = 0; -#ifdef FB1 - si1[i] = 0; - co1[i] = 0; -#endif - //ADC dual mode puts both channels in one word, right aligned. - for(int j = 0; j < ADC_OVER_FB0; j++) { + 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 - for(int j = ADC_OVER_FB0; j < ADC_OVER_FB0 + ADC_OVER_FB1; j++) { + 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 - si0[i] = s_g * V_DIFF(si0[i], ADC_OVER_FB0) + s_o; - co0[i] = c_g * V_DIFF(co0[i], ADC_OVER_FB0) + c_o; -#ifdef FB1 - si1[i] = s_g * V_DIFF(si1[i], ADC_OVER_FB1) + s_o; - co1[i] = c_g * V_DIFF(co1[i], ADC_OVER_FB1) + c_o; -#endif } - - //copy dma buffer for plotting TODO: use dual mode, for zero copy if(ctx->send == 0) { memcpy((void *)(ctx->txbuf_raw), (void *)ADC_DMA_Buffer, ADC_SAMPLES_IN_RT * 4); ctx->send = 1; } - PIN(sin3) = si0[ADC_GROUPS - 1]; - PIN(cos3) = co0[ADC_GROUPS - 1]; + 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[ADC_GROUPS - 1]; - PIN(cos1) = co1[ADC_GROUPS - 1]; + 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; @@ -142,19 +146,18 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ // c = (co0[3] + co0[2] + co0[1] + co0[0]) / 4.0; // } - if(si0[3] >= 0) { - if(co0[3] > 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; } @@ -196,8 +199,7 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { } ctx->txpos = 0; - - buf[0] = 255; + 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; @@ -210,8 +212,7 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { } } - - buf[0] = 0xfe; //trigger servoterm + buf[0] = 0xfe;//trigger servoterm buf[1] = 0x00; if(USB_CDC_is_connected()) { USB_VCP_send_string(buf); diff --git a/src/comps/res.c b/src/comps/res.c index b5f4cc3d..0a6ecd86 100644 --- a/src/comps/res.c +++ b/src/comps/res.c @@ -22,10 +22,9 @@ HAL_PIN(cos); HAL_PIN(enable); HAL_PIN(error); HAL_PIN(state); -HAL_PIN(tim_oc); -HAL_PIN(tim_arr); -HAL_PIN(res_mode); -HAL_PIN(res_freq); +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 @@ -38,9 +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(res_freq) = 10000; + 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; @@ -81,7 +80,6 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { //res_en = ADC_GROUPS/2/(res_freq/rt_freq) //arr = ADC_TRIGGER_FREQ/2/res_freq - PIN(tim_arr) = ADC_TRIGGER_FREQ / FRT_FREQ - 1; // resolver reference signal OC TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; @@ -119,10 +117,9 @@ 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); - FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / PIN(res_freq) - 1; - PIN(tim_arr) = FB0_RES_REF_TIM->ARR; - PIN(res_mode) = ADC_GROUPS / 2 / (PIN(res_freq) / RT_FREQ); + FB0_RES_REF_TIM->CCR3 = (int)CLAMP(PIN(phase) * FB0_RES_REF_TIM->ARR, 0, FB0_RES_REF_TIM->ARR - 1); + FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / PIN(freq) - 1; + PIN(res_mode) = ADC_GROUPS / 2 / (PIN(freq) / RT_FREQ); float s = 0.0; float c = 0.0; float a = 0.0; diff --git a/src/setup.c b/src/setup.c index f256d216..79c0caa5 100644 --- a/src/setup.c +++ b/src/setup.c @@ -161,7 +161,7 @@ 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 = ARRAY_SIZE(ADC_DMA_Buffer0); DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; @@ -174,9 +174,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; From a81a0bd4bf8e583ba20055b6f51e304cc395871b Mon Sep 17 00:00:00 2001 From: andypugh Date: Fri, 9 Mar 2018 19:23:08 +0000 Subject: [PATCH 05/17] Add a link to the partial documentation Rather than just say "there will be documentation" --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 19226665..d714318a 100644 --- a/README.md +++ b/README.md @@ -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 From ffa0eba628604015c7e9db3f28cedf0c21bbc1e2 Mon Sep 17 00:00:00 2001 From: crinq Date: Mon, 12 Mar 2018 23:51:17 +0100 Subject: [PATCH 06/17] weiler config --- conf/weiler_x.txt | 17 +++++++++++++++++ conf/weiler_z.txt | 17 +++++++++++++++++ 2 files changed, 34 insertions(+) create mode 100644 conf/weiler_x.txt create mode 100644 conf/weiler_z.txt diff --git a/conf/weiler_x.txt b/conf/weiler_x.txt new file mode 100644 index 00000000..b7138326 --- /dev/null +++ b/conf/weiler_x.txt @@ -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 \ No newline at end of file diff --git a/conf/weiler_z.txt b/conf/weiler_z.txt new file mode 100644 index 00000000..3f9f0ce1 --- /dev/null +++ b/conf/weiler_z.txt @@ -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 \ No newline at end of file From 320c555837aa9f366f3829dc66af5166ab4bbb15 Mon Sep 17 00:00:00 2001 From: Nico Stute Date: Thu, 15 Mar 2018 21:38:06 +0100 Subject: [PATCH 07/17] Update pinout.md --- hw/kicad/v4.0/doc/pinout.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hw/kicad/v4.0/doc/pinout.md b/hw/kicad/v4.0/doc/pinout.md index 69fb7696..3ff10afa 100644 --- a/hw/kicad/v4.0/doc/pinout.md +++ b/hw/kicad/v4.0/doc/pinout.md @@ -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, sw_o usart1_ck, tim2_ch2, (tim1_ch1, spi3_sck) | | io_swclk | pa14 | sw_clk | | io_swdio | pa13 | sw_dio | | io_rst | nrst | nrst | From 79fc9952fc0c79b202edb45fd9498fe052e89f13 Mon Sep 17 00:00:00 2001 From: Nico Stute Date: Thu, 15 Mar 2018 21:41:42 +0100 Subject: [PATCH 08/17] Update pinout.md --- hw/kicad/v4.0/doc/pinout.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hw/kicad/v4.0/doc/pinout.md b/hw/kicad/v4.0/doc/pinout.md index 3ff10afa..06187619 100644 --- a/hw/kicad/v4.0/doc/pinout.md +++ b/hw/kicad/v4.0/doc/pinout.md @@ -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, pa0 | 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 | From 08a57c75e0d041b7ec81627639b393502df7f168 Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 00:32:58 +0100 Subject: [PATCH 09/17] hv sync + sserial fix --- conf/template/pid.txt | 3 +- src/comps/hv.c | 186 ++++++++++++++++++++++-------------------- src/comps/sserial.c | 2 +- 3 files changed, 99 insertions(+), 92 deletions(-) diff --git a/conf/template/pid.txt b/conf/template/pid.txt index 2d312a3e..f5aeb209 100644 --- a/conf/template/pid.txt +++ b/conf/template/pid.txt @@ -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 diff --git a/src/comps/hv.c b/src/comps/hv.c index 9b3d54dd..564863ae 100644 --- a/src/comps/hv.c +++ b/src/comps/hv.c @@ -64,6 +64,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) { @@ -164,106 +165,111 @@ 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; + 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); - float e = PIN(en); - float pos = PIN(pos); - float vel = PIN(vel); + ctx->config.pins.r = PIN(r); + ctx->config.pins.l = PIN(l); + ctx->config.pins.psi = PIN(psi); + ctx->config.pins.cur_p = PIN(cur_p); + ctx->config.pins.cur_i = PIN(cur_i); + ctx->config.pins.cur_ff = PIN(cur_ff); + ctx->config.pins.cur_ind = PIN(cur_ind); + ctx->config.pins.max_y = PIN(max_y); + ctx->config.pins.max_cur = PIN(max_cur) * PIN(scale); - ctx->config.pins.r = PIN(r); - ctx->config.pins.l = PIN(l); - ctx->config.pins.psi = PIN(psi); - ctx->config.pins.cur_p = PIN(cur_p); - ctx->config.pins.cur_i = PIN(cur_i); - ctx->config.pins.cur_ff = PIN(cur_ff); - ctx->config.pins.cur_ind = PIN(cur_ind); - ctx->config.pins.max_y = PIN(max_y); - ctx->config.pins.max_cur = PIN(max_cur) * PIN(scale); + uint32_t dma_pos = DMA_GetCurrDataCounter(UART_DRV_RX_DMA); + if(dma_pos == 0) { + CRC_ResetDR(); + uint32_t crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->packet_from_hv), sizeof(packet_from_hv_t) / 4 - 1); - uint32_t dma_pos = DMA_GetCurrDataCounter(UART_DRV_RX_DMA); - if(dma_pos == 0) { - CRC_ResetDR(); - uint32_t crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->packet_from_hv), sizeof(packet_from_hv_t) / 4 - 1); + if(crc == ctx->packet_from_hv.crc) { + PIN(d_fb) = ctx->packet_from_hv.d_fb; + PIN(q_fb) = ctx->packet_from_hv.q_fb; + PIN(dc_volt) = ctx->packet_from_hv.dc_volt; + PIN(pwm_volt) = ctx->packet_from_hv.pwm_volt; + PIN(abs_cur) = sqrtf(PIN(d_fb) * PIN(d_fb) + PIN(q_fb) * PIN(q_fb)); - if(crc == ctx->packet_from_hv.crc) { - PIN(d_fb) = ctx->packet_from_hv.d_fb; - PIN(q_fb) = ctx->packet_from_hv.q_fb; - PIN(dc_volt) = ctx->packet_from_hv.dc_volt; - PIN(pwm_volt) = ctx->packet_from_hv.pwm_volt; - PIN(abs_cur) = sqrtf(PIN(d_fb) * PIN(d_fb) + PIN(q_fb) * PIN(q_fb)); + uint16_t a = ctx->packet_from_hv.addr; + a = CLAMP(a, 0, sizeof(f3_state_data_t) / 4); + ctx->state.data[a] = ctx->packet_from_hv.value; - uint16_t a = ctx->packet_from_hv.addr; - a = CLAMP(a, 0, sizeof(f3_state_data_t) / 4); - ctx->state.data[a] = ctx->packet_from_hv.value; - - PIN(u_fb) = ctx->state.pins.u_fb; - PIN(v_fb) = ctx->state.pins.v_fb; - PIN(w_fb) = ctx->state.pins.w_fb; - PIN(hv_temp) = ctx->state.pins.hv_temp; - PIN(mot_temp) = ctx->state.pins.mot_temp; - PIN(core_temp) = ctx->state.pins.core_temp; - PIN(fault) = ctx->state.pins.fault; - PIN(y) = ctx->state.pins.y; - if(ctx->state.pins.fault > 0.0) { - PIN(com_error) = HV_FAULT_ERROR; + PIN(u_fb) = ctx->state.pins.u_fb; + PIN(v_fb) = ctx->state.pins.v_fb; + PIN(w_fb) = ctx->state.pins.w_fb; + PIN(hv_temp) = ctx->state.pins.hv_temp; + PIN(mot_temp) = ctx->state.pins.mot_temp; + PIN(core_temp) = ctx->state.pins.core_temp; + PIN(fault) = ctx->state.pins.fault; + PIN(y) = ctx->state.pins.y; + if(ctx->state.pins.fault > 0.0) { + PIN(com_error) = HV_FAULT_ERROR; + } else { + PIN(com_error) = 0.0; + } + ctx->timeout = 0; } else { - PIN(com_error) = 0.0; + PIN(crc_error)++; + PIN(com_error) = HV_CRC_ERROR; } - ctx->timeout = 0; - } else { - PIN(crc_error)++; - PIN(com_error) = HV_CRC_ERROR; } + + if(ctx->timeout > 2) { + PIN(timeout)++; + PIN(com_error) = HV_TIMEOUT_ERROR; + } + ctx->timeout++; + + float d_cmd = PIN(d_cmd); + float q_cmd = PIN(q_cmd); + + if(PIN(rev) > 0.0) { + q_cmd *= -1.0; + pos = minus(0, pos); + } + + if(e > 0.0) { + ctx->packet_to_hv.d_cmd = d_cmd; + ctx->packet_to_hv.q_cmd = q_cmd; + ctx->packet_to_hv.flags.enable = 1; + } else { + ctx->packet_to_hv.d_cmd = 0.0; + ctx->packet_to_hv.q_cmd = 0.0; + ctx->packet_to_hv.flags.enable = 0; + } + ctx->packet_to_hv.flags.cmd_type = PIN(cmd_mode); + ctx->packet_to_hv.flags.phase_type = PIN(phase_mode); + ctx->packet_to_hv.pos = pos; + ctx->packet_to_hv.vel = vel; + ctx->packet_to_hv.addr = ctx->addr; + ctx->packet_to_hv.value = ctx->config.data[ctx->addr++]; + ctx->addr %= sizeof(f3_config_data_t) / 4; + + CRC_ResetDR(); + ctx->packet_to_hv.crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->packet_to_hv), sizeof(packet_to_hv_t) / 4 - 1); + + PIN(uart_sr) = UART_DRV->SR; + PIN(uart_dr) = UART_DRV->DR; + //start DMA RX transfer + DMA_Cmd(UART_DRV_RX_DMA, DISABLE); + DMA_ClearFlag(UART_DRV_RX_DMA, UART_DRV_RX_DMA_TCIF); + DMA_Cmd(UART_DRV_RX_DMA, ENABLE); + + //start DMA TX transfer + 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); } - - if(ctx->timeout > 2) { - PIN(timeout)++; - PIN(com_error) = HV_TIMEOUT_ERROR; - } - ctx->timeout++; - - float d_cmd = PIN(d_cmd); - float q_cmd = PIN(q_cmd); - - if(PIN(rev) > 0.0) { - q_cmd *= -1.0; - pos = minus(0, pos); - } - - if(e > 0.0) { - ctx->packet_to_hv.d_cmd = d_cmd; - ctx->packet_to_hv.q_cmd = q_cmd; - ctx->packet_to_hv.flags.enable = 1; - } else { - ctx->packet_to_hv.d_cmd = 0.0; - ctx->packet_to_hv.q_cmd = 0.0; - ctx->packet_to_hv.flags.enable = 0; - } - ctx->packet_to_hv.flags.cmd_type = PIN(cmd_mode); - ctx->packet_to_hv.flags.phase_type = PIN(phase_mode); - ctx->packet_to_hv.pos = pos; - ctx->packet_to_hv.vel = vel; - ctx->packet_to_hv.addr = ctx->addr; - ctx->packet_to_hv.value = ctx->config.data[ctx->addr++]; - ctx->addr %= sizeof(f3_config_data_t) / 4; - - CRC_ResetDR(); - ctx->packet_to_hv.crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->packet_to_hv), sizeof(packet_to_hv_t) / 4 - 1); - - PIN(uart_sr) = UART_DRV->SR; - PIN(uart_dr) = UART_DRV->DR; - //start DMA RX transfer - DMA_Cmd(UART_DRV_RX_DMA, DISABLE); - DMA_ClearFlag(UART_DRV_RX_DMA, UART_DRV_RX_DMA_TCIF); - DMA_Cmd(UART_DRV_RX_DMA, ENABLE); - - //start DMA TX transfer - 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; @@ -278,7 +284,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, diff --git a/src/comps/sserial.c b/src/comps/sserial.c index 534a2fa2..9efcd869 100644 --- a/src/comps/sserial.c +++ b/src/comps/sserial.c @@ -336,7 +336,7 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { 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()); } From ab813b9b15a58ca1135eea6bbc1e7386f4424cb0 Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Fri, 16 Mar 2018 01:59:36 +0100 Subject: [PATCH 10/17] sserial cleanup, rename IO pins --- src/comps/sserial.c | 587 ++++++++++++++++++++------------------------ tools/gentable.c | 17 +- 2 files changed, 279 insertions(+), 325 deletions(-) diff --git a/src/comps/sserial.c b/src/comps/sserial.c index 9efcd869..6cdf9efc 100644 --- a/src/comps/sserial.c +++ b/src/comps/sserial.c @@ -65,118 +65,117 @@ struct sserial_ctx_t { uint32_t foo; }; -volatile uint8_t rxbuf[128]; -volatile uint8_t txbuf[128]; +volatile uint8_t rxbuf[128];//rx dma buffer +volatile uint8_t txbuf[128];//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 - 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 - 0x73, 0x5F, 0x63, 0x6D, 0x64, 0x00, 0x00, 0x00, // 32..39 - 0x00, 0x00, 0x00, 0x00, 0xA0, 0x20, 0x10, 0x80, // 40..47 - 0x00, 0x00, 0x80, 0xFF, 0x00, 0x00, 0x80, 0x7F, // 48..55 - 0x26, 0x00, 0x72, 0x61, 0x64, 0x00, 0x76, 0x65, // 56..63 - 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 - 0x01, 0x00, 0x00, + 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 + 0x73,0x5F,0x63,0x6D,0x64,0x00,0x00,0x00,// 32..39 + 0x00,0x00,0x00,0x00,0xA0,0x20,0x10,0x80,// 40..47 + 0x00,0x00,0x80,0xFF,0x00,0x00,0x80,0x7F,// 48..55 + 0x26,0x00,0x72,0x61,0x64,0x00,0x76,0x65,// 56..63 + 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,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, +}; + +const discovery_rpc_t discovery = { + .ptocp = 0x018B, + .gtocp = 0x01A5, + .input = 11, + .output = 9, }; -uint16_t sserial_ptocp = 0x019B; -uint16_t sserial_gtocp = 0x01B5; 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; -} sserial_out_process_data_t; //size:9 bytes +} sserial_out_process_data_t; //size:9 bytes _Static_assert(sizeof(sserial_out_process_data_t) == 9, "sserial_out_process_data_t size error!"); 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 +} 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); @@ -185,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); @@ -251,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); @@ -310,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 @@ -330,11 +322,6 @@ 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 = 4; //calculate timeout in systicks for block_bytes @@ -343,241 +330,203 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { 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); - //how many packets we have the the rx buffer for processing - available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf); + //next received packet will be written to bufferpos + uint32_t bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA2_Stream5); + //how many packets we have the the rx buffer for processing + uint32_t available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf); - if(available >= 1) { - lbp.byte = rxbuf[rxpos]; + if(available >= 1) { + lbp.byte = rxbuf[rxpos]; - if(lbp.ct == CT_LOCAL && lbp.wr == 0) { //local read, cmd+crc = 2b - timeout = 0; - if(available >= 2) { - switch(lbp.byte) { - case LBPCookieCMD: - txbuf[0] = LBPCookie; - break; - case LBPStatusCMD: //TODO: return status - txbuf[0] = 0x00; - break; - case LBPCardName0Cmd ... LBPCardName3Cmd: - txbuf[0] = name[lbp.byte - LBPCardName0Cmd]; - break; - default: //TODO: handle unknown command condition - txbuf[0] = 0x00; - } - send(1, 1); - rxpos += 2; - } else { - continue; + if(lbp.ct == CT_LOCAL && lbp.wr == 0) { //local read, cmd+crc = 2b + timeout = 0; + if(available >= 2) { + switch(lbp.byte) { + case LBPCookieCMD: + txbuf[0] = LBPCookie; + break; + case LBPStatusCMD: //TODO: return status + txbuf[0] = 0x00; + break; + case LBPCardName0Cmd ... LBPCardName3Cmd: + txbuf[0] = name[lbp.byte - LBPCardName0Cmd]; + break; + default: //TODO: handle unknown command condition + txbuf[0] = 0x00; } - } else if(lbp.ct == CT_LOCAL && lbp.wr == 1) { //local write, cmd+data+crc = 3b - timeout = 0; - //0xFF and 0xFC are not followed by crc - if(rxbuf[rxpos] == 0xFF) { - // reset parser - rxpos += 1; - } else if(rxbuf[rxpos] == 0xFC) { - // todo - rxpos += 1; - } else if(available >= 3) { //writes do not expect crc in reply - 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; - if(lbp.byte == UnitNumberRPC && available >= 2) { //unit number, cmd+crc = 2b - txbuf[0] = unit.byte[0]; - txbuf[1] = unit.byte[1]; - txbuf[2] = unit.byte[2]; - txbuf[3] = unit.byte[3]; - send(4, 1); - rxpos += 2; - } else if(lbp.byte == DiscoveryRPC && available >= 2) { //discovery, cmd+crc = 2b - memcpy((void *)txbuf, ((uint8_t *)&discovery), sizeof(discovery)); - send(sizeof(discovery), 1); - rxpos += 2; - } else if(lbp.byte == ProcessDataRPC && available >= discovery.output + 2 - block_bytes) { //process data, requires cmd+output bytes+crc - uint32_t t1 = hal_get_systick_value(); - uint32_t wait_ticks = 0; - //wait with timeout until rest of process data is received - do { - uint32_t t2 = hal_get_systick_value(); - if(t1 < t2) { - t1 += hal_get_systick_reload(); - } - wait_ticks = t1 - t2; - //next received packet will be written to bufferpos - 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); - } while(available < discovery.output + 2 && wait_ticks <= max_waste_ticks); - //TODO: fault handling on timeout... - //set input pins - 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.fault = (PIN(fault) > 0) ? 1 : 0; - - //copy output pins from rx buffer - for(int i = 0; i < discovery.output; i++) { - ((uint8_t *)(&data_out))[i] = rxbuf[(rxpos + i + 1) % sizeof(rxbuf)]; - } - - //set bidirectional pins - PIN(index_out) = data_out.index_enable; - data_in.index_enable = (PIN(index_clear) > 0) ? 0 : data_out.index_enable; - - //copy input pins to tx buffer - txbuf[0] = 0x00; //fault byte - 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); - DMA_ClearFlag(DMA1_Stream4, DMA_FLAG_TCIF4); - DMA_Cmd(DMA1_Stream4, ENABLE); - txbuf[discovery.input] = crc8((uint8_t *)txbuf, discovery.input); - //send(discovery.input, 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(enable) = data_out.enable; - } else { - PIN(crc_error) - ++; - PIN(connected) = 0; - PIN(error) = 1; - PIN(pos_cmd) = 0; - PIN(pos_cmd_d) = 0; - PIN(out0) = 0; - PIN(out1) = 0; - PIN(out2) = 0; - PIN(out3) = 0; - 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 - int size = 2 * lbp.as + 2; - timeout = 0; - if(available >= 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 += 4; - } else { //address not included in command = cmd+crc - rxpos += 2; - } - //TODO: causes timeouts... - //if((address + (1 << lbp.ds)) < ARRAY_SIZE(sserial_slave)) { //check if address is valid - memcpy((void *)txbuf, &sserial_slave[address], (1 << lbp.ds)); - send((1 << lbp.ds), 1); - //} - 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; - } else { //address not included in command = cmd+crc - rxpos += 1; - } - //TODO: check size - if((address + (1 << lbp.ds)) < ARRAY_SIZE(sserial_slave)) { //check if address is valid - for(int i = 0; i < (1 << lbp.ds); i++) { - sserial_slave[address + i] = rxbuf[(rxpos + i) % sizeof(rxbuf)]; - } - } - rxpos += (1 << lbp.ds) + 1; - //update globals - float tmp; - memcpy(&tmp, &sserial_slave[scale_address], 4); - PIN(scale) = tmp; - if(lbp.ai) { //auto increment address by datasize - address += (1 << lbp.ds); - } - } else { - continue; - } - } else { - //TODO: handle unkown packet + send(1, 1); + rxpos += 2; } - } + } else if(lbp.ct == CT_LOCAL && lbp.wr == 1) { //local write, cmd+data+crc = 3b + timeout = 0; + //0xFF and 0xFC are not followed by crc + if(rxbuf[rxpos] == 0xFF) { + // reset parser + rxpos += 1; + } else if(rxbuf[rxpos] == 0xFC) { + // todo + rxpos += 1; + } else if(available >= 3) { //writes do not expect crc in reply + txbuf[0] = 0x00; + send(1, 0); + rxpos += 3; + } + } else if(lbp.ct == CT_RPC) { //RPC TODO: check for ct should not required for rpc + timeout = 0; + if(lbp.byte == UnitNumberRPC && available >= 2) { //unit number, cmd+crc = 2b + txbuf[0] = unit.byte[0]; + txbuf[1] = unit.byte[1]; + txbuf[2] = unit.byte[2]; + txbuf[3] = unit.byte[3]; + send(4, 1); + rxpos += 2; + } else if(lbp.byte == DiscoveryRPC && available >= 2) { //discovery, cmd+crc = 2b + memcpy((void *)txbuf, ((uint8_t *)&discovery), sizeof(discovery)); + send(sizeof(discovery), 1); + rxpos += 2; + } else if(lbp.byte == ProcessDataRPC && available >= discovery.output + 2 - block_bytes) { //process data, requires cmd+output bytes+crc + uint32_t t1 = hal_get_systick_value(); + uint32_t wait_ticks = 0; + //wait with timeout until rest of process data is received + do { + uint32_t t2 = hal_get_systick_value(); + if(t1 < t2) { + t1 += hal_get_systick_reload(); + } + wait_ticks = t1 - t2; + //next received packet will be written to bufferpos + 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); + } while(available < discovery.output + 2 && wait_ticks <= max_waste_ticks); + //TODO: fault handling on timeout... + //set input pins + 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.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; - if(timeout > PIN(timeout)) { //TODO: clamping - PIN(connected) = 0; - PIN(error) = 1; - PIN(pos_cmd) = 0; - PIN(pos_cmd_d) = 0; - PIN(out0) = 0; - PIN(out1) = 0; - PIN(out2) = 0; - PIN(out3) = 0; - PIN(enable) = 0; - rxpos = bufferpos; + //copy output pins from rx buffer + for(int i = 0; i < discovery.output; i++) { + ((uint8_t *)(&data_out))[i] = rxbuf[(rxpos + i + 1) % sizeof(rxbuf)]; + } + + //set bidirectional pins + PIN(index_out) = data_out.index_enable; + data_in.index_enable = (PIN(index_clear) > 0) ? 0 : data_out.index_enable; + + //copy input pins to tx buffer + txbuf[0] = 0x00; //fault byte + 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); + DMA_ClearFlag(DMA1_Stream4, DMA_FLAG_TCIF4); + DMA_Cmd(DMA1_Stream4, ENABLE); + txbuf[discovery.input] = crc8((uint8_t *)txbuf, discovery.input); + //send(discovery.input, 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.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) + ++; + PIN(connected) = 0; + PIN(error) = 1; + PIN(pos_cmd) = 0; + PIN(pos_cmd_d) = 0; + PIN(out0) = 0; + PIN(out1) = 0; + PIN(out2) = 0; + PIN(out3) = 0; + PIN(enable) = 0; + } + rxpos += discovery.output + 2; + } + } else if(lbp.ct == CT_RW && lbp.wr == 0) { //read + //size = 1 + 2*lbp.as + 1 + int size = 2 * lbp.as + 2; + timeout = 0; + if(available >= 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 += 4; + } else { //address not included in command = cmd+crc + rxpos += 2; + } + //TODO: causes timeouts... + //if((address + (1 << lbp.ds)) < ARRAY_SIZE(sserial_slave)) { //check if address is valid + memcpy((void *)txbuf, &sserial_slave[address], (1 << lbp.ds)); + send((1 << lbp.ds), 1); + //} + if(lbp.ai) { //auto increment address by datasize + address += (1 << lbp.ds); + } + } + } 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) { + 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; + } else { //address not included in command = cmd+crc + rxpos += 1; + } + //TODO: check size + if((address + (1 << lbp.ds)) < ARRAY_SIZE(sserial_slave)) { //check if address is valid + for(int i = 0; i < (1 << lbp.ds); i++) { + sserial_slave[address + i] = rxbuf[(rxpos + i) % sizeof(rxbuf)]; + } + } + rxpos += (1 << lbp.ds) + 1; + //update globals + float tmp; + memcpy(&tmp, &sserial_slave[scale_address], 4); + PIN(scale) = tmp; + if(lbp.ai) { //auto increment address by datasize + address += (1 << lbp.ds); + } + } } else { - PIN(connected) = 1; - PIN(error) = 0; + //TODO: handle unkown packet } - rxpos = rxpos % sizeof(rxbuf); } + + if(timeout > PIN(timeout)) { //TODO: clamping + PIN(connected) = 0; + PIN(error) = 1; + PIN(pos_cmd) = 0; + PIN(pos_cmd_d) = 0; + PIN(out0) = 0; + PIN(out1) = 0; + PIN(out2) = 0; + PIN(out3) = 0; + PIN(enable) = 0; + rxpos = bufferpos; + } else { + PIN(connected) = 1; + 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, diff --git a/tools/gentable.c b/tools/gentable.c index 19ca5c0a..c2b45ae0 100644 --- a/tools/gentable.c +++ b/tools/gentable.c @@ -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); From a64801a20952c079a06154decbd59f1f695f8e3e Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 02:52:52 +0100 Subject: [PATCH 11/17] call bootloader fix --- src/main.c | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/src/main.c b/src/main.c index 37d0c5be..1370bb89 100644 --- a/src/main.c +++ b/src/main.c @@ -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"); From 61795c8b731be287a4e85860c4296fe7a9880267 Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Fri, 16 Mar 2018 03:25:00 +0100 Subject: [PATCH 12/17] move config out of main.c --- Makefile | 1 + src/config.c | 90 ++++++++++++++++++++++++++++++++++++++++++++++++++++ src/main.c | 68 --------------------------------------- 3 files changed, 91 insertions(+), 68 deletions(-) create mode 100644 src/config.c diff --git a/Makefile b/Makefile index fae76c77..558ba8ff 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/src/config.c b/src/config.c new file mode 100644 index 00000000..f01fc996 --- /dev/null +++ b/src/config.c @@ -0,0 +1,90 @@ +/* +* This file is part of the stmbl project. +* +* Copyright (C) 2013-2015 Rene Hopf +* Copyright (C) 2013-2015 Nico Stute +* +* 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 . +*/ + +#include +#include +#include "hal.h" +#include "commands.h" +#include "stm32f4xx_conf.h" + +char config[15 * 1024]; +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"); diff --git a/src/main.c b/src/main.c index 1370bb89..f45d4230 100644 --- a/src/main.c +++ b/src/main.c @@ -105,74 +105,6 @@ void nv_reset(char *ptr) { } COMMAND("reset", nv_reset, "reset STMBL"); - -char config[15 * 1024]; -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; From 2ba1b83d23d85a6914100cb9545dd66c5126710a Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 04:11:30 +0100 Subject: [PATCH 13/17] res freq --- conf/festo.txt | 4 +++- src/comps/res.c | 6 ++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/conf/festo.txt b/conf/festo.txt index 9d5bb23d..1f2799cc 100644 --- a/conf/festo.txt +++ b/conf/festo.txt @@ -9,4 +9,6 @@ conf0.polecount = 4 conf0.mot_fb_rev = 1 io0.out0 = fault0.mot_brake conf0.max_ac_cur = 10 -conf0.max_force = 3 \ No newline at end of file +conf0.max_force = 3 +res0.freq = 5000 +res0.phase = 0.4 diff --git a/src/comps/res.c b/src/comps/res.c index 0a6ecd86..9080b6dc 100644 --- a/src/comps/res.c +++ b/src/comps/res.c @@ -117,9 +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! + uint32_t mult = CLAMP(PIN(freq) / RT_FREQ + 0.5, 1, 4); + PIN(freq) = RT_FREQ * mult; FB0_RES_REF_TIM->CCR3 = (int)CLAMP(PIN(phase) * FB0_RES_REF_TIM->ARR, 0, FB0_RES_REF_TIM->ARR - 1); - FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / PIN(freq) - 1; - PIN(res_mode) = ADC_GROUPS / 2 / (PIN(freq) / RT_FREQ); + FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / (RT_FREQ * mult) - 1; + PIN(res_mode) = ADC_GROUPS / 2 / mult; float s = 0.0; float c = 0.0; float a = 0.0; From 47ea302e926549ccd77766995e4049b0166eb015 Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 05:16:17 +0100 Subject: [PATCH 14/17] res freq fix --- conf/festo.txt | 2 +- src/comps/res.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/festo.txt b/conf/festo.txt index 1f2799cc..cea7605e 100644 --- a/conf/festo.txt +++ b/conf/festo.txt @@ -11,4 +11,4 @@ io0.out0 = fault0.mot_brake conf0.max_ac_cur = 10 conf0.max_force = 3 res0.freq = 5000 -res0.phase = 0.4 +res0.phase = 0.4 \ No newline at end of file diff --git a/src/comps/res.c b/src/comps/res.c index 9080b6dc..cece1fb8 100644 --- a/src/comps/res.c +++ b/src/comps/res.c @@ -119,8 +119,8 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ //TODO: arr can change! uint32_t mult = CLAMP(PIN(freq) / RT_FREQ + 0.5, 1, 4); PIN(freq) = RT_FREQ * mult; - FB0_RES_REF_TIM->CCR3 = (int)CLAMP(PIN(phase) * FB0_RES_REF_TIM->ARR, 0, FB0_RES_REF_TIM->ARR - 1); 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; From 0eb8d13a6fd5a212e5746ea2edeb37820b6e8229 Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 06:38:33 +0100 Subject: [PATCH 15/17] clear warnings --- Makefile | 2 ++ src/comps/yaskawa.c | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 558ba8ff..ea6ac0d9 100644 --- a/Makefile +++ b/Makefile @@ -181,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 diff --git a/src/comps/yaskawa.c b/src/comps/yaskawa.c index 88519375..503f0737 100644 --- a/src/comps/yaskawa.c +++ b/src/comps/yaskawa.c @@ -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++){ From ac93220859bbeae44e0058fdd20f93577ef3909e Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 06:38:53 +0100 Subject: [PATCH 16/17] clear warnings --- src/comps/res.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/comps/res.c b/src/comps/res.c index cece1fb8..919410b9 100644 --- a/src/comps/res.c +++ b/src/comps/res.c @@ -43,7 +43,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { } 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; - struct res_pin_ctx_t *pins = (struct res_pin_ctx_t *)pin_ptr; + // struct res_pin_ctx_t *pins = (struct res_pin_ctx_t *)pin_ptr; ctx->abspos = 0; ctx->lastq = 0; From 5c34297eed4d0f439b4011651f50a1e9f66810b8 Mon Sep 17 00:00:00 2001 From: crinq Date: Fri, 16 Mar 2018 06:39:20 +0100 Subject: [PATCH 17/17] adc stack fix --- src/comps/adc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/comps/adc.c b/src/comps/adc.c index 27501dd5..1355f8e2 100644 --- a/src/comps/adc.c +++ b/src/comps/adc.c @@ -70,8 +70,8 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ uint32_t sii0; uint32_t coi0; //scaled, all groups - float sin0all; - float cos0all; + float sin0all = 0.0; + float cos0all = 0.0; #ifdef FB1 float co1[ADC_GROUPS]; float si1[ADC_GROUPS];