From 5a97a6626ac201666f77fe8a7e1ca7858abdabd2 Mon Sep 17 00:00:00 2001 From: crinq Date: Wed, 6 Sep 2017 02:00:49 +0200 Subject: [PATCH] f3 volt cmd --- conf/template/acim.txt | 3 +- conf/template/pmsm.txt | 3 +- shared/common.h | 22 ++++++++++---- shared/comps/curpid.c | 11 +++++++ shared/comps/dq.c | 19 ++++++++---- shared/comps/idq.c | 15 +++++++--- src/comps/hv.c | 6 ++-- stm32f303/src/comps/ls.c | 62 ++++++++++++++++++++++------------------ stm32f303/src/main.c | 8 ++---- 9 files changed, 97 insertions(+), 52 deletions(-) diff --git a/conf/template/acim.txt b/conf/template/acim.txt index 37f285f8..fa7c29f4 100644 --- a/conf/template/acim.txt +++ b/conf/template/acim.txt @@ -20,5 +20,6 @@ acim_ttc0.vel = vel1.vel acim_ttc0.mode = 0 hv0.q_cmd = acim_ttc0.iq hv0.d_cmd = acim_ttc0.id -hv0.mode = 1 +hv0.cmd_mode = 1 +hv0.phase_mode = 2 hv0.pos = acim_ttc0.pos \ No newline at end of file diff --git a/conf/template/pmsm.txt b/conf/template/pmsm.txt index 8589bd99..567ba6e2 100644 --- a/conf/template/pmsm.txt +++ b/conf/template/pmsm.txt @@ -18,4 +18,5 @@ pid0.min_torque = pmsm_limits0.min_torque pid0.max_vel = pmsm_limits0.abs_max_vel pmsm_ttc0.torque = pid0.torque_cor_cmd hv0.q_cmd = pmsm_ttc0.cur -hv0.mode = 1 +hv0.cmd_mode = 1 +hv0.phase_mode = 2 diff --git a/shared/common.h b/shared/common.h index b249e8c7..d15a559d 100644 --- a/shared/common.h +++ b/shared/common.h @@ -34,17 +34,29 @@ typedef struct{ float vel; float value; uint16_t addr; - union { - uint16_t enable : 1; - uint16_t foo; - } flags; + union{ + struct{ + uint16_t enable : 1; + enum packet_to_hv_cmd_type_t{ + VOLT_MODE = 0, + CURRENT_MODE, + } cmd_type : 1; + enum packet_to_hv_phase_type_t{ + PHASE_90_3PH = 0, + PHASE_90_4PH, + PHASE_120_3PH, + PHASE_180_2PH, + PHASE_180_3PH, + } phase_type : 3; + } flags; + uint16_t foo; + }; uint32_t crc; } packet_to_hv_t; #pragma pack(1) typedef union { struct f3_config_data_temp{ - float mode; float r; float l; float psi; diff --git a/shared/comps/curpid.c b/shared/comps/curpid.c index fcfc8775..837615d7 100644 --- a/shared/comps/curpid.c +++ b/shared/comps/curpid.c @@ -1,4 +1,5 @@ #include "commands.h" +#include "common.h" #include "hal.h" #include "math.h" #include "defines.h" @@ -8,6 +9,7 @@ HAL_COMP(curpid); // enable HAL_PIN(en); +HAL_PIN(cmd_mode); // current command HAL_PIN(id_cmd); @@ -112,6 +114,15 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst ud += ctx->id_error_sum; uq += ctx->iq_error_sum; + if(PIN(cmd_mode) == VOLT_MODE){ + ud = LIMIT(PIN(id_cmd), max_volt); + uq = LIMIT(PIN(iq_cmd), max_volt); + ctx->id_error_sum = 0.0; + ctx->iq_error_sum = 0.0; + id_error = 0.0; + iq_error = 0.0; + } + if(PIN(en) <= 0.0){ ud = 0.0; uq = 0.0; diff --git a/shared/comps/dq.c b/shared/comps/dq.c index cecac9cc..86616d3a 100644 --- a/shared/comps/dq.c +++ b/shared/comps/dq.c @@ -1,4 +1,5 @@ #include "commands.h" +#include "common.h" #include "hal.h" #include "math.h" #include "defines.h" @@ -38,23 +39,29 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst float a, b, y; switch((int)PIN(mode)){ - case 0: // 90° + case PHASE_90_3PH: // 90° a = u - v; b = w - v; y = u / 3.0 + v / 3.0 + w / 3.0; - break; + break; - case 1: // 120° + case PHASE_120_3PH: // 120° a = u * 2.0 / 3.0 - v / 3.0 - w / 3.0; b = v / M_SQRT3 - w / M_SQRT3; y = u / 3.0 + v / 3.0 + w / 3.0; - break; + break; - case 2: // 180° + case PHASE_180_2PH: // 180° a = 0; b = (u - w) / 2.0; y = (u + w) / 2.0; - break; + break; + + case PHASE_180_3PH: // 180° + a = v; + b = (u - w) / 2.0; + y = (u + w) / 2.0; + break; default: a = 0.0; diff --git a/shared/comps/idq.c b/shared/comps/idq.c index bebf6505..8f339c7b 100644 --- a/shared/comps/idq.c +++ b/shared/comps/idq.c @@ -1,4 +1,5 @@ #include "commands.h" +#include "common.h" #include "hal.h" #include "math.h" #include "defines.h" @@ -47,23 +48,29 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst float u, v, w; switch((int)PIN(mode)){ - case 0: // 90° + case PHASE_90_3PH: // 90° u = a; v = 0.0; w = b; break; - case 1: // 120° - u = a; + case PHASE_120_3PH: // 120° + u = a; v = - a / 2.0 + b / 2.0 * M_SQRT3; w = - a / 2.0 - b / 2.0 * M_SQRT3; break; - case 2: // 180° + case PHASE_180_2PH: // 180° u = b / 2.0; v = 0.0; w = - b / 2.0; break; + + case PHASE_180_3PH: // 180° + u = b / 2.0; + v = a; + w = - b / 2.0; + break; default: u = 0.0; diff --git a/src/comps/hv.c b/src/comps/hv.c index da91139a..021e85cd 100644 --- a/src/comps/hv.c +++ b/src/comps/hv.c @@ -18,7 +18,8 @@ HAL_PIN(vel); HAL_PIN(en); // config data from LS -HAL_PIN(mode); +HAL_PIN(phase_mode); +HAL_PIN(cmd_mode); HAL_PIN(r); HAL_PIN(l); HAL_PIN(psi); @@ -170,7 +171,6 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst float pos = PIN(pos); float vel = PIN(vel); - ctx->config.pins.mode = PIN(mode); ctx->config.pins.r = PIN(r); ctx->config.pins.l = PIN(l); ctx->config.pins.psi = PIN(psi); @@ -235,6 +235,8 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst 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; diff --git a/stm32f303/src/comps/ls.c b/stm32f303/src/comps/ls.c index 66491f71..605065ad 100644 --- a/stm32f303/src/comps/ls.c +++ b/stm32f303/src/comps/ls.c @@ -18,7 +18,8 @@ HAL_PIN(vel); HAL_PIN(en); // config data from LS -HAL_PIN(mode); +HAL_PIN(cmd_mode); +HAL_PIN(phase_mode); HAL_PIN(r); HAL_PIN(l); HAL_PIN(psi); @@ -121,7 +122,6 @@ static void hw_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ DMA1->IFCR = DMA_IFCR_CTCIF3 | DMA_IFCR_CHTIF3 | DMA_IFCR_CGIF3; DMA1_Channel3->CCR |= DMA_CCR_EN; - config.pins.mode = 0.0; config.pins.r = 0.0; config.pins.l = 0.0; config.pins.psi = 0.0; @@ -152,14 +152,15 @@ static void rt_start(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 ls_ctx_t * ctx = (struct ls_ctx_t *)ctx_ptr; struct ls_pin_ctx_t * pins = (struct ls_pin_ctx_t *)pin_ptr; - + uint32_t dma_pos = sizeof(packet_to_hv_t) - DMA1_Channel3->CNDTR; - - + if(dma_pos == sizeof(packet_to_hv_t)){ uint32_t crc = HAL_CRC_Calculate(&hcrc, (uint32_t *) &(ctx->packet_to_hv), sizeof(packet_to_hv_t) / 4 - 1); if(crc == ctx->packet_to_hv.crc){ PIN(en) = ctx->packet_to_hv.flags.enable; + PIN(phase_mode) = ctx->packet_to_hv.flags.phase_type; + PIN(cmd_mode) = ctx->packet_to_hv.flags.cmd_type; PIN(d_cmd) = ctx->packet_to_hv.d_cmd; PIN(q_cmd) = ctx->packet_to_hv.q_cmd; PIN(pos) = ctx->packet_to_hv.pos; @@ -167,8 +168,7 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst uint8_t a = ctx->packet_to_hv.addr; a = CLAMP(a, 0, sizeof(config) / 4); config.data[a] = ctx->packet_to_hv.value; // TODO: first enable after complete update - - PIN(mode) = config.pins.mode; + PIN(r) = config.pins.r; PIN(l) = config.pins.l; PIN(psi) = config.pins.psi; @@ -192,12 +192,12 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst if(USART3->ISR & USART_ISR_RTOF){ // idle line USART3->ICR |= USART_ICR_RTOCF | USART_ICR_FECF | USART_ICR_ORECF; // timeout clear flag GPIOA->BSRR |= GPIO_PIN_10; - + PIN(idle)++; if(dma_pos != sizeof(packet_to_hv_t)){ PIN(dma_pos) = dma_pos; } - + // reset rx DMA DMA1_Channel3->CCR &= (uint16_t)(~DMA_CCR_EN); DMA1_Channel3->CNDTR = sizeof(packet_to_hv_t); @@ -207,9 +207,7 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst //ctx->send = 1; } - - - + if(ctx->send == 2){ ctx->send = 0; } @@ -224,7 +222,7 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst state.pins.core_temp = PIN(core_temp); state.pins.fault = PIN(fault); state.pins.y = PIN(y); - + // fill tx struct ctx->packet_from_hv.dc_volt = PIN(dc_volt); ctx->packet_from_hv.pwm_volt = PIN(pwm_volt); @@ -234,34 +232,42 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst ctx->packet_from_hv.value = state.data[ctx->tx_addr++]; ctx->tx_addr %= sizeof(state) / 4; ctx->packet_from_hv.crc = HAL_CRC_Calculate(&hcrc, (uint32_t *) &(ctx->packet_from_hv), sizeof(packet_from_hv_t) / 4 - 1); - + // start tx DMA DMA1_Channel2->CCR &= (uint16_t)(~DMA_CCR_EN); DMA1_Channel2->CNDTR = sizeof(packet_from_hv_t); DMA1_Channel2->CCR |= DMA_CCR_EN; //ctx->send = 0; } - + if(ctx->timeout > 5){//disable driver PIN(en) = 0.0; PIN(timeout)++; } ctx->timeout++; - + // TODO: sin = 0.5 - if(config.pins.mode == 0){// 90° - PIN(pwm_volt) = PIN(dc_volt) / M_SQRT2 * 0.95; - }else if(config.pins.mode == 1){// 120° - PIN(pwm_volt) = PIN(dc_volt) / M_SQRT3 * 0.95; - }else if(config.pins.mode == 2){// 180° - PIN(pwm_volt) = PIN(dc_volt) * 0.95; - }else{ - PIN(pwm_volt) = 0.0; + switch((uint16_t)PIN(phase_mode)){ + case PHASE_90_3PH: // 90° + PIN(pwm_volt) = PIN(dc_volt) / M_SQRT2 * 0.95; + break; + + case PHASE_90_4PH: // 90° + PIN(pwm_volt) = PIN(dc_volt) * 0.95; + break; + + case PHASE_120_3PH: // 120° + PIN(pwm_volt) = PIN(dc_volt) / M_SQRT3 * 0.95; + break; + + case PHASE_180_2PH: // 180° + case PHASE_180_3PH: // 180° + PIN(pwm_volt) = PIN(dc_volt) * 0.95; + break; + + default: + PIN(pwm_volt) = 0.0; } - - - - } hal_comp_t ls_comp_struct = { diff --git a/stm32f303/src/main.c b/stm32f303/src/main.c index 8d769383..3621f4e9 100644 --- a/stm32f303/src/main.c +++ b/stm32f303/src/main.c @@ -298,9 +298,9 @@ int main(void) hal_parse("curpid0.id_cmd = ls0.d_cmd"); hal_parse("curpid0.iq_cmd = ls0.q_cmd"); hal_parse("idq0.pos = ls0.pos"); - hal_parse("idq0.mode = ls0.mode"); + hal_parse("idq0.mode = ls0.phase_mode"); hal_parse("dq0.pos = ls0.pos"); - hal_parse("dq0.mode = ls0.mode"); + hal_parse("dq0.mode = ls0.phase_mode"); hal_parse("hv0.en = ls0.en"); //ADC TEST @@ -310,9 +310,6 @@ int main(void) hal_parse("dq0.v = io0.iv"); hal_parse("dq0.w = io0.iw"); - // hal_parse("sim0.vel", "idq0.pos"); - // hal_parse("sim0.vel", "dq0.pos"); - hal_parse("svm0.u = idq0.u"); hal_parse("svm0.v = idq0.v"); hal_parse("svm0.w = idq0.w"); @@ -349,6 +346,7 @@ int main(void) hal_parse("curpid0.pwm_volt = ls0.pwm_volt"); hal_parse("curpid0.vel = ls0.vel"); hal_parse("curpid0.en = ls0.en"); + hal_parse("curpid0.cmd_mode = ls0.cmd_mode"); // hal parse config // hal_init_nrt();