diff --git a/Makefile b/Makefile index a8513c87..a7280e45 100644 --- a/Makefile +++ b/Makefile @@ -28,7 +28,8 @@ HWVERSION = v4 ifeq ($(HWVERSION),v3) COMPS += src/comps/hw/io3.c - #COMPS += src/comps/hvf1.c + COMPS += src/comps/hvf1.c + SOURCES += shared/common_f1.c CFLAGS += -DV3 else COMPS += src/comps/hw/io4.c diff --git a/shared/common_f1.c b/shared/common_f1.c new file mode 100644 index 00000000..4c458426 --- /dev/null +++ b/shared/common_f1.c @@ -0,0 +1,26 @@ +#include "common_f1.h" + +void buff_packet(packet_header_t* p, uint8_t size){ + uint8_t nonkey_count = 0; + uint8_t* buf = ((uint8_t*)p) + sizeof(packet_header_t); + for(int i = size - 1; i >= 0; i--){ + if(buf[i] == p->start){ + buf[i] = nonkey_count; + nonkey_count = 0; + } + else{ + nonkey_count++; + } + } + p->key = nonkey_count; +} + +void unbuff_packet(packet_header_t* p, uint8_t size){ + uint8_t temp; + uint8_t* buf = ((uint8_t*)p) + sizeof(packet_header_t); + for(int j = p->key; j < size;){ + temp = buf[j]; + buf[j] = p->start; + j += temp + 1; + } +} \ No newline at end of file diff --git a/shared/common_f1.h b/shared/common_f1.h new file mode 100644 index 00000000..01fad31c --- /dev/null +++ b/shared/common_f1.h @@ -0,0 +1,118 @@ +#pragma once +#include + +#if __GNUC__ < 5 + #error gcc to old (< 5.0) +#endif + +//#define TROLLER + +#define DATABAUD 2250000 //baudrate used for communication + +//fixed point calculations signed bit, 9 bit predecimal, 6 bit decimal +#define TOFIXED(a) ((int16_t)((a) * 64)) +#define TOFLOAT(a) ((float)((a) / 64.0)) + +#define PWM_RES 2400 + +//TODO: CRC +typedef struct { + uint8_t start; // 255 + uint8_t key; +} packet_header_t; + +//data from f1 to f4 +#pragma pack(1) +typedef struct { + int16_t dc_cur; + int16_t dc_volt; + int16_t hv_temp; + uint8_t high_volt : 1;//hardware hi limit + uint8_t low_volt : 1;//hardware low limit + uint8_t over_cur : 1;//hardware cur limit + uint8_t over_temp : 1;//hardware temp limit + uint8_t hv_fault : 1;//iramx fault + uint8_t sys_fault : 1;//sys fault, crc error, clock error, watchdog bit, startup failure... + uint8_t padding : 2; +#ifdef TROLLER + int16_t a; + int16_t b; + int16_t c; +#endif +} from_hv_t; + +//data from f4 to f1 +#pragma pack(1) +typedef struct { + float a; + float b; + float pos; + uint8_t mode : 4;//TODO: change to enum + uint8_t enable : 1; + uint8_t padding : 3; +} to_hv_t; + +#pragma pack(1) +typedef struct { + packet_header_t head; + to_hv_t data; +} packet_to_hv_t; + +#pragma pack(1) +typedef struct { + packet_header_t head; + from_hv_t data; +} packet_from_hv_t; + +void buff_packet(packet_header_t *p, uint8_t size); +void unbuff_packet(packet_header_t *p, uint8_t size); + +//check if structs can be send at 5kHz using DATABAUD +_Static_assert(sizeof(packet_to_hv_t) <= DATABAUD / 11 / 5000 - 1 - 5, "to_hv struct to large"); +_Static_assert(sizeof(packet_from_hv_t) <= DATABAUD / 11 / 5000 - 1 - 5, "from_hv struct to large"); + + +// struct f1tof4{ +// int16 a; +// int16 b; +// int16 udc; +// int16 value; +// struct { +// bool enabled; +// bool temp_limit; +// bool current_limit; +// bool watchdog; // toggle +// int4 misc; +// }flags; +// int8 crc; +// }; +// +// struct f4tof1{ +// int16 a; // u/i +// int16 b; // u/i +// int16 value; +// int6 address; +// /* +// rw: +// r +// l +// max_i +// max_temp +// max_pwm +// phase_offset_uv +// phase_offset_vw +// svm_mode (centered, svm, bottom low) +// u/i cmd mode +// u/i fb mode +// +// r: +// temp0 +// temp1 +// temp2 +// temp3 +// hw version +// */ +// bool enable; +// bool watchdog; // toggle in hal +// int8 crc; +// }; \ No newline at end of file diff --git a/src/comps/hvf1.c b/src/comps/hvf1.c new file mode 100644 index 00000000..3aacacc7 --- /dev/null +++ b/src/comps/hvf1.c @@ -0,0 +1,215 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" +#include "stm32f4xx_conf.h" +#include "hw/hw.h" +#include "common_f1.h" + + +HAL_COMP(hv); + +HAL_PIN(a); +HAL_PIN(b); +HAL_PIN(mode); + +HAL_PIN(enable); +HAL_PIN(error); +HAL_PIN(over_cur); +HAL_PIN(over_temp); +HAL_PIN(hv_fault); + +HAL_PIN(dc_cur); +HAL_PIN(dc_volt); +HAL_PIN(pwm_volt); +HAL_PIN(hv_temp); +HAL_PIN(power); +HAL_PIN(dc_cur_sim); +HAL_PIN(ac_cur_sim); +HAL_PIN(iq); + +HAL_PIN(rev); + +struct hv_ctx_t { + volatile packet_to_hv_t packet_to_hv; + volatile packet_from_hv_t packet_from_hv; +}; + +static void nrt_init(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; + + //setup uart to f1. uses DMA to transfer to_hv struct. + GPIO_InitTypeDef GPIO_InitStruct; + USART_InitTypeDef USART_InitStruct; + UART_DRV_CLOCK_COMMAND(UART_DRV_RCC, ENABLE); + + //USART TX + GPIO_PinAFConfig(UART_DRV_TX_PORT, UART_DRV_TX_PIN_SOURCE, UART_DRV_TX_AF_SOURCE); + GPIO_InitStruct.GPIO_Pin = UART_DRV_TX_PIN; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP ; + GPIO_Init(UART_DRV_TX_PORT, &GPIO_InitStruct); + + //USART RX + GPIO_PinAFConfig(UART_DRV_RX_PORT, UART_DRV_RX_PIN_SOURCE, UART_DRV_RX_AF_SOURCE); + GPIO_InitStruct.GPIO_Pin = UART_DRV_RX_PIN; + GPIO_Init(UART_DRV_RX_PORT, &GPIO_InitStruct); + + USART_InitStruct.USART_BaudRate = DATABAUD; + USART_InitStruct.USART_WordLength = USART_WordLength_8b; + USART_InitStruct.USART_StopBits = USART_StopBits_1; + USART_InitStruct.USART_Parity = USART_Parity_No; + USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStruct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; + USART_Init(UART_DRV, &USART_InitStruct); + + /* Enable the USART */ + USART_Cmd(UART_DRV, ENABLE); + + // DMA-Disable + DMA_Cmd(UART_DRV_TX_DMA, DISABLE); + DMA_DeInit(UART_DRV_TX_DMA); + + // DMA2-Config + DMA_InitTypeDef DMA_InitStructure; + DMA_InitStructure.DMA_Channel = UART_DRV_TX_DMA_CHAN; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) &(UART_DRV->DR); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) &(ctx->packet_to_hv); + DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; + DMA_InitStructure.DMA_BufferSize = sizeof(packet_to_hv_t); + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(UART_DRV_TX_DMA, &DMA_InitStructure); + + //DMA_Cmd(UART_DRV_TX_DMA, ENABLE); + + USART_DMACmd(UART_DRV, USART_DMAReq_Tx, ENABLE); + + + // DMA-Disable + DMA_Cmd(UART_DRV_RX_DMA, DISABLE); + DMA_DeInit(UART_DRV_RX_DMA); + + // DMA2-Config + DMA_InitStructure.DMA_Channel = UART_DRV_RX_DMA_CHAN; + DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) &(UART_DRV->DR); + DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t) &(ctx->packet_from_hv); + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStructure.DMA_BufferSize = sizeof(packet_from_hv_t); + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; + DMA_InitStructure.DMA_Priority = DMA_Priority_High; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(UART_DRV_RX_DMA, &DMA_InitStructure); + + + USART_DMACmd(UART_DRV, USART_DMAReq_Rx, ENABLE); + + ctx->packet_to_hv.head.start = 255; + ctx->packet_to_hv.head.key = 0; + ctx->packet_from_hv.head.start = 0; + ctx->packet_from_hv.head.key = 0; +} + +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; + + float e = PIN(enable); + + if(ctx->packet_from_hv.head.start == 255) { + unbuff_packet((packet_header_t *) &(ctx->packet_from_hv), sizeof(from_hv_t)); + ctx->packet_from_hv.head.start = 0; + PIN(dc_cur) = TOFLOAT(ctx->packet_from_hv.data.dc_cur)*0.5 + PIN(dc_cur)*0.5; + PIN(dc_volt) = TOFLOAT(ctx->packet_from_hv.data.dc_volt); + if(PIN(mode) == 0) {//AC + PIN(pwm_volt) = PIN(dc_volt) / 2.0 * 0.95 * 1.15; + } else if(PIN(mode) == 1) {//DC + PIN(pwm_volt) = PIN(dc_volt) * 0.95; + } else if(PIN(mode) == 2) {//2phase ac + PIN(pwm_volt) = PIN(dc_volt) * 0.7 * 0.95; + } else { + PIN(pwm_volt) = 0.0; + } + PIN(hv_temp) = TOFLOAT(ctx->packet_from_hv.data.hv_temp); + PIN(over_cur) = ctx->packet_from_hv.data.over_cur;//hardware cur limit + PIN(over_temp) = ctx->packet_from_hv.data.over_temp;//hardware temp limit + PIN(hv_fault) = ctx->packet_from_hv.data.hv_fault;//iramx fault + + PIN(error) = 0.0;//TODO: link to fault + } + else { + PIN(error) = 1.0; + } + + float a = PIN(a); + float b = PIN(b); + + if(PIN(rev) > 0.0) {//TODO: rev DC + b *= -1.0; + } + + ctx->packet_to_hv.data.mode = CLAMP(PIN(mode),0,16); + if(e > 0.0) { + ctx->packet_to_hv.data.a = a; + ctx->packet_to_hv.data.b = b; + ctx->packet_to_hv.data.enable = 1; + } + else { + ctx->packet_to_hv.data.a = 0; + ctx->packet_to_hv.data.b = 0; + ctx->packet_to_hv.data.enable = 0; + } + buff_packet((packet_header_t *) &(ctx->packet_to_hv), sizeof(to_hv_t)); + + //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); + + //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); + + 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; + } + if(ABS(a*b) > 0.01){ + PIN(ac_cur_sim) = PIN(dc_cur) / sqrtf(a*a+b*b) * PIN(pwm_volt); + } +} + + +hal_comp_t hv_comp_struct = { + .name = "hv", + .nrt = 0, + .rt = rt_func, + .frt = 0, + .nrt_init = nrt_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = sizeof(struct hv_ctx_t), + .pin_count = sizeof(struct hv_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; \ No newline at end of file diff --git a/stm32f103/Makefile b/stm32f103/Makefile index 7ae03d93..aa94fab1 100644 --- a/stm32f103/Makefile +++ b/stm32f103/Makefile @@ -18,7 +18,7 @@ INCDIRS += shared SOURCES += stm32f103/src/main.c SOURCES += stm32f103/src/version.c -SOURCES += shared/common.c +SOURCES += shared/common_f1.c # Standard peripheral library CPPFLAGS += -DUSE_STDPERIPH_DRIVER diff --git a/stm32f103/src/main.c b/stm32f103/src/main.c index c39d97e2..73750c96 100644 --- a/stm32f103/src/main.c +++ b/stm32f103/src/main.c @@ -1,5 +1,5 @@ #include "stm32f10x_conf.h" -#include "common.h" +#include "common_f1.h" #include "defines.h" #include "version.h" #include