From b12dbe342c33f96ba8946da576b972fc43d04ca6 Mon Sep 17 00:00:00 2001 From: crinq Date: Thu, 16 Jan 2020 04:19:49 +0100 Subject: [PATCH] cmd clock sync, hv in rt --- conf/template/pid.txt | 2 +- conf/template/smartabs_fb0.txt | 2 +- conf/template/sserial.txt | 1 + src/comps/hv.c | 548 ++++++++++++++++----------------- src/comps/hw/io4.c | 20 +- src/comps/sserial.c | 32 +- src/setup.c | 4 +- 7 files changed, 325 insertions(+), 284 deletions(-) diff --git a/conf/template/pid.txt b/conf/template/pid.txt index 362c5fd4..b17efa92 100644 --- a/conf/template/pid.txt +++ b/conf/template/pid.txt @@ -13,7 +13,6 @@ load io load iit link conf hv0.rt_prio = 0.9 -hv0.frt_prio = 1 adc0.rt_prio = 1 reslimit0.rt_prio = 3 rev0.rt_prio = 4 @@ -22,6 +21,7 @@ vel0.rt_prio = 6 vel1.rt_prio = 6 vel2.rt_prio = 6 io0.rt_prio = 7 +io0.frt_prio = 3 pid0.rt_prio = 8 fault0.rt_prio = 10 iit0.rt_prio = 13 diff --git a/conf/template/smartabs_fb0.txt b/conf/template/smartabs_fb0.txt index 53674c07..28a37a50 100644 --- a/conf/template/smartabs_fb0.txt +++ b/conf/template/smartabs_fb0.txt @@ -1,5 +1,5 @@ load smartabs -smartabs0.rt_prio = 2 +smartabs0.rt_prio = 0.8 fb_switch0.mot_pos = smartabs0.pos fb_switch0.mot_abs_pos = smartabs0.pos fb_switch0.mot_polecount = conf0.mot_fb_polecount diff --git a/conf/template/sserial.txt b/conf/template/sserial.txt index 33ce84aa..76ea914c 100644 --- a/conf/template/sserial.txt +++ b/conf/template/sserial.txt @@ -42,3 +42,4 @@ fault0.brake_release = sserial0.out3 term0.wave2 = linrev0.cmd_d_out sserial0.current = hv0.iq_fb linrev0.scale = sserial0.scale +io0.clock_scale = sserial0.clock_scale \ No newline at end of file diff --git a/src/comps/hv.c b/src/comps/hv.c index 4474cee5..76929240 100644 --- a/src/comps/hv.c +++ b/src/comps/hv.c @@ -78,7 +78,6 @@ struct hv_ctx_t { uint32_t addr; uint16_t timeout; uint8_t conf_addr; - uint8_t frt_slot; uint8_t send_state; }; @@ -213,319 +212,312 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { } static void rt_func(float period, void *ctx_ptr, 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, void *ctx_ptr, 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.dac = PIN(dac); + 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.dac = PIN(dac); - uint32_t dma_count = MAX(sizeof(packet_from_hv_t), sizeof(packet_bootloader_t)) - DMA_GetCurrDataCounter(UART_DRV_RX_DMA); + uint32_t dma_count = MAX(sizeof(packet_from_hv_t), sizeof(packet_bootloader_t)) - DMA_GetCurrDataCounter(UART_DRV_RX_DMA); - PIN(value) = 0.0; + PIN(value) = 0.0; - if(dma_count >= sizeof(stmbl_talk_header_t)) { - // PIN(value) = 0.5; - if(dma_count >= sizeof(stmbl_talk_header_t) + ctx->from_hv.packet_from_hv.header.len * 4) { - PIN(value) = 0.75; + if(dma_count >= sizeof(stmbl_talk_header_t)) { + // PIN(value) = 0.5; + if(dma_count >= sizeof(stmbl_talk_header_t) + ctx->from_hv.packet_from_hv.header.len * 4) { + PIN(value) = 0.75; - CRC_ResetDR(); - uint32_t crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->from_hv.packet_from_hv.header.slave_addr), sizeof(stmbl_talk_header_t) / 4 + ctx->from_hv.packet_from_hv.header.len - 1); - if(ctx->from_hv.packet_from_hv.header.crc == crc) { - switch(flash_state) { - case SLAVE_IN_APP: - if(ctx->from_hv.packet_from_hv.header.slave_addr == 0 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_from_hv_t) - sizeof(stmbl_talk_header_t)) / 4) { - // from f3 app - PIN(id_fb) = ctx->from_hv.packet_from_hv.id_fb; - PIN(iq_fb) = ctx->from_hv.packet_from_hv.iq_fb; - PIN(ud_fb) = ctx->from_hv.packet_from_hv.ud_fb; - PIN(uq_fb) = ctx->from_hv.packet_from_hv.uq_fb; - PIN(fault) = ctx->from_hv.packet_from_hv.fault; - PIN(abs_cur) = sqrtf(PIN(id_fb) * PIN(id_fb) + PIN(iq_fb) * PIN(iq_fb)); - PIN(abs_volt) = sqrtf(PIN(ud_fb) * PIN(ud_fb) + PIN(uq_fb) * PIN(uq_fb)); - if(PIN(pwm_volt) > 0.0){ - PIN(duty) = PIN(abs_volt) / PIN(pwm_volt); - } + CRC_ResetDR(); + uint32_t crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->from_hv.packet_from_hv.header.slave_addr), sizeof(stmbl_talk_header_t) / 4 + ctx->from_hv.packet_from_hv.header.len - 1); + if(ctx->from_hv.packet_from_hv.header.crc == crc) { + switch(flash_state) { + case SLAVE_IN_APP: + if(ctx->from_hv.packet_from_hv.header.slave_addr == 0 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_from_hv_t) - sizeof(stmbl_talk_header_t)) / 4) { + // from f3 app + PIN(id_fb) = ctx->from_hv.packet_from_hv.id_fb; + PIN(iq_fb) = ctx->from_hv.packet_from_hv.iq_fb; + PIN(ud_fb) = ctx->from_hv.packet_from_hv.ud_fb; + PIN(uq_fb) = ctx->from_hv.packet_from_hv.uq_fb; + PIN(fault) = ctx->from_hv.packet_from_hv.fault; + PIN(abs_cur) = sqrtf(PIN(id_fb) * PIN(id_fb) + PIN(iq_fb) * PIN(iq_fb)); + PIN(abs_volt) = sqrtf(PIN(ud_fb) * PIN(ud_fb) + PIN(uq_fb) * PIN(uq_fb)); + if(PIN(pwm_volt) > 0.0){ + PIN(duty) = PIN(abs_volt) / PIN(pwm_volt); + } - uint16_t a = ctx->from_hv.packet_from_hv.header.conf_addr; - a = CLAMP(a, 0, sizeof(f3_state_data_t) / 4); - ctx->state.data[a] = ctx->from_hv.packet_from_hv.header.config.f32; + uint16_t a = ctx->from_hv.packet_from_hv.header.conf_addr; + a = CLAMP(a, 0, sizeof(f3_state_data_t) / 4); + ctx->state.data[a] = ctx->from_hv.packet_from_hv.header.config.f32; - PIN(dc_volt) = ctx->state.pins.dc_volt; - PIN(pwm_volt) = ctx->state.pins.pwm_volt; - 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(y) = ctx->state.pins.y; + PIN(dc_volt) = ctx->state.pins.dc_volt; + PIN(pwm_volt) = ctx->state.pins.pwm_volt; + 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(y) = ctx->state.pins.y; - PIN(value) = 1.0; + PIN(value) = 1.0; + ctx->timeout = 0; + + if(ctx->from_hv.packet_from_hv.buf != 0x0) { + rb_write(&hv_rx_buf, (void *)&(ctx->from_hv.packet_from_hv.buf), 1); + } + } else { + // wrong packet len or slave addr + } + break; + case SEND_TO_BOOTLOADER: + + break; + case ERASE_FLASH: + if(ctx->from_hv.packet_from_hv.header.slave_addr == 255 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4) { + // from f3 bootloader + if(ctx->from_hv.packet_from_hv_bootloader.state == BOOTLOADER_STATE_OK && ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_PAGEERASE) { ctx->timeout = 0; - - if(ctx->from_hv.packet_from_hv.buf != 0x0) { - rb_write(&hv_rx_buf, (void *)&(ctx->from_hv.packet_from_hv.buf), 1); - } - } else { - // wrong packet len or slave addr + flash_state = SEND_APP; } - break; - case SEND_TO_BOOTLOADER: - - break; - case ERASE_FLASH: - if(ctx->from_hv.packet_from_hv.header.slave_addr == 255 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4) { - // from f3 bootloader - if(ctx->from_hv.packet_from_hv_bootloader.state == BOOTLOADER_STATE_OK && ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_PAGEERASE) { - ctx->timeout = 0; - flash_state = SEND_APP; - } - } else { - // wrong packet len or slave addr - PIN(value) = 3.0; + } else { + // wrong packet len or slave addr + PIN(value) = 3.0; + } + break; + case SEND_APP: + if(ctx->from_hv.packet_from_hv.header.slave_addr == 255 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4) { + // from f3 bootloader + if(ctx->from_hv.packet_from_hv_bootloader.state == BOOTLOADER_STATE_OK && ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_WRITE && ctx->from_hv.packet_from_hv_bootloader.addr == 0x08004000 + ctx->addr * 4 && ctx->from_hv.packet_from_hv_bootloader.value == ((uint32_t *)&(_binary_obj_hvf3_hvf3_bin_start))[ctx->addr]) { + ctx->timeout = 0; + ctx->addr++; } - break; - case SEND_APP: - if(ctx->from_hv.packet_from_hv.header.slave_addr == 255 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4) { - // from f3 bootloader - if(ctx->from_hv.packet_from_hv_bootloader.state == BOOTLOADER_STATE_OK && ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_WRITE && ctx->from_hv.packet_from_hv_bootloader.addr == 0x08004000 + ctx->addr * 4 && ctx->from_hv.packet_from_hv_bootloader.value == ((uint32_t *)&(_binary_obj_hvf3_hvf3_bin_start))[ctx->addr]) { - ctx->timeout = 0; - ctx->addr++; - } - if(ctx->addr > ((uint32_t) & (_binary_obj_hvf3_hvf3_bin_size)) / 4) { - flash_state = CRC_CHECK; - // flash_state = SEND_TO_APP; - } - } else { - // wrong packet len or slave addr - PIN(value) = 3.0; + if(ctx->addr > ((uint32_t) & (_binary_obj_hvf3_hvf3_bin_size)) / 4) { + flash_state = CRC_CHECK; + // flash_state = SEND_TO_APP; } - break; - case CRC_CHECK: - if(ctx->from_hv.packet_from_hv.header.slave_addr == 255 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4) { - // from f3 bootloader - if(ctx->from_hv.packet_from_hv_bootloader.state == BOOTLOADER_STATE_OK && ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_CRCCHECK) { - ctx->timeout = 0; - flash_state = SEND_TO_APP; - } - } else { - // wrong packet len or slave addr - PIN(value) = 3.0; + } else { + // wrong packet len or slave addr + PIN(value) = 3.0; + } + break; + case CRC_CHECK: + if(ctx->from_hv.packet_from_hv.header.slave_addr == 255 && ctx->from_hv.packet_from_hv.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4) { + // from f3 bootloader + if(ctx->from_hv.packet_from_hv_bootloader.state == BOOTLOADER_STATE_OK && ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_CRCCHECK) { + ctx->timeout = 0; + flash_state = SEND_TO_APP; } - break; - case SEND_TO_APP: + } else { + // wrong packet len or slave addr + PIN(value) = 3.0; + } + break; + case SEND_TO_APP: - break; - case FLASH_FAILED: + break; + case FLASH_FAILED: - break; - } - - - } else { - // CRC fault - PIN(crc_error) - ++; - // PIN(fault) = HV_CRC_ERROR; - // PIN(value) = 4.0; + break; } + + + } else { + // CRC fault + PIN(crc_error) + ++; + // PIN(fault) = HV_CRC_ERROR; + // PIN(value) = 4.0; } } + } - if(ctx->timeout > 2) { - PIN(fault) = HV_TIMEOUT_ERROR; - } - ctx->timeout++; + if(ctx->timeout > 2) { + PIN(fault) = HV_TIMEOUT_ERROR; + } + ctx->timeout++; - float d_cmd = PIN(d_cmd); - float q_cmd = PIN(q_cmd); + 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(PIN(rev) > 0.0) { + q_cmd *= -1.0; + pos = minus(0, pos); + } - uint32_t tx_size = 0; + uint32_t tx_size = 0; - switch(flash_state) { - case SLAVE_IN_APP: - if(e > 0.0) { - ctx->to_hv.packet_to_hv.d_cmd = d_cmd; - ctx->to_hv.packet_to_hv.q_cmd = q_cmd; - ctx->to_hv.packet_to_hv.flags.enable = 1; - } else { - ctx->to_hv.packet_to_hv.d_cmd = 0.0; - ctx->to_hv.packet_to_hv.q_cmd = 0.0; - ctx->to_hv.packet_to_hv.flags.enable = 0; - } - ctx->to_hv.packet_to_hv.flags.ignore_fault_pin = PIN(ignore_fault_pin) > 0.0; - ctx->to_hv.packet_to_hv.flags.cmd_type = PIN(cmd_mode); - ctx->to_hv.packet_to_hv.flags.phase_type = PIN(phase_mode); - ctx->to_hv.packet_to_hv.pos = pos; - ctx->to_hv.packet_to_hv.vel = vel; - - ctx->to_hv.packet_to_hv.header.slave_addr = 0; - ctx->to_hv.packet_to_hv.header.flags.cmd = WRITE_CONF; - ctx->to_hv.packet_to_hv.header.flags.counter++; - ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_to_hv_t) - sizeof(stmbl_talk_header_t)) / 4; - ctx->to_hv.packet_to_hv.header.conf_addr = ctx->conf_addr; - ctx->to_hv.packet_to_hv.header.config.f32 = ctx->config.data[ctx->conf_addr++]; - - uint8_t buf[1]; - if(rb_read(&hv_tx_buf, buf, 1)) { - ctx->to_hv.packet_to_hv.flags.buf = buf[0]; - } else { - ctx->to_hv.packet_to_hv.flags.buf = 0x0; - } - - tx_size = sizeof(packet_to_hv_t); - - ctx->conf_addr %= sizeof(f3_config_data_t) / 4; - - if(send_to_bootloader) { - send_to_bootloader = 0; - flash_state = SEND_TO_BOOTLOADER; - ctx->timeout = 0; - // TODO: check f3 crc, size, ... - } - break; - - case SEND_TO_BOOTLOADER: // fix - ctx->to_hv.packet_to_hv.header.flags.cmd = BOOTLOADER; - ctx->to_hv.packet_to_hv.flags.buf = 0x0; - ctx->to_hv.packet_to_hv.header.flags.counter++; + switch(flash_state) { + case SLAVE_IN_APP: + if(e > 0.0) { + ctx->to_hv.packet_to_hv.d_cmd = d_cmd; + ctx->to_hv.packet_to_hv.q_cmd = q_cmd; + ctx->to_hv.packet_to_hv.flags.enable = 1; + } else { ctx->to_hv.packet_to_hv.d_cmd = 0.0; ctx->to_hv.packet_to_hv.q_cmd = 0.0; ctx->to_hv.packet_to_hv.flags.enable = 0; - - tx_size = sizeof(packet_to_hv_t); - - if(ctx->timeout > 10) { - ctx->timeout = 0; - flash_state = ERASE_FLASH; - } - break; - - case ERASE_FLASH: - ctx->to_hv.packet_to_hv.header.slave_addr = 255; - ctx->to_hv.packet_to_hv.header.flags.cmd = NO_CMD; - ctx->to_hv.packet_to_hv.header.flags.counter++; - ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; - ctx->to_hv.packet_to_hv.header.conf_addr = 0; - ctx->to_hv.packet_to_hv.header.config.f32 = 0; - ctx->to_hv.packet_to_hv_bootloader.addr = 0; - ctx->to_hv.packet_to_hv_bootloader.value = 0; - ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_PAGEERASE; - - tx_size = sizeof(packet_bootloader_t); - - ctx->addr = 0; - - // flash_state = SLAVE_IN_APP; - - if(ctx->timeout > 20000) { - ctx->timeout = 0; - flash_state = FLASH_FAILED; - } - break; - - case SEND_APP: - ctx->to_hv.packet_to_hv.header.flags.counter++; - ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; - ctx->to_hv.packet_to_hv_bootloader.addr = 0x08004000 + ctx->addr * 4; - ctx->to_hv.packet_to_hv_bootloader.value = ((uint32_t *)&_binary_obj_hvf3_hvf3_bin_start)[ctx->addr]; - ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_WRITE; - - tx_size = sizeof(packet_bootloader_t); - - if(ctx->timeout > 10) { - ctx->timeout = 0; - flash_state = FLASH_FAILED; - } - break; - - case CRC_CHECK: - ctx->to_hv.packet_to_hv.header.flags.counter++; - ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; - ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_CRCCHECK; - - tx_size = sizeof(packet_bootloader_t); - - if(ctx->timeout > 2000) { - ctx->timeout = 0; - flash_state = FLASH_FAILED; - } - break; - case SEND_TO_APP: - ctx->to_hv.packet_to_hv.header.flags.cmd = DO_RESET; - ctx->to_hv.packet_to_hv.header.flags.counter++; - ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; - ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_NOP; - - tx_size = sizeof(packet_bootloader_t); - - if(ctx->timeout > 2000) { - ctx->timeout = 0; - flash_state = SLAVE_IN_APP; - } - break; - case FLASH_FAILED: - if(ctx->timeout > 10) { - ctx->timeout = 0; - flash_state = SLAVE_IN_APP; - } - break; - } - - if(ctx->send_state > 1){ - if(flash_state != SLAVE_IN_APP){ - tx_size = 0; } - ctx->send_state = 0; - } - ctx->send_state++; + ctx->to_hv.packet_to_hv.flags.ignore_fault_pin = PIN(ignore_fault_pin) > 0.0; + ctx->to_hv.packet_to_hv.flags.cmd_type = PIN(cmd_mode); + ctx->to_hv.packet_to_hv.flags.phase_type = PIN(phase_mode); + ctx->to_hv.packet_to_hv.pos = pos; + ctx->to_hv.packet_to_hv.vel = vel; - if(tx_size) { - CRC_ResetDR(); - ctx->to_hv.packet_to_hv.header.crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->to_hv.packet_to_hv.header.slave_addr), tx_size / 4 - 1); + ctx->to_hv.packet_to_hv.header.slave_addr = 0; + ctx->to_hv.packet_to_hv.header.flags.cmd = WRITE_CONF; + ctx->to_hv.packet_to_hv.header.flags.counter++; + ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_to_hv_t) - sizeof(stmbl_talk_header_t)) / 4; + ctx->to_hv.packet_to_hv.header.conf_addr = ctx->conf_addr; + ctx->to_hv.packet_to_hv.header.config.f32 = ctx->config.data[ctx->conf_addr++]; - //start DMA TX transfer - DMA_Cmd(UART_DRV_TX_DMA, DISABLE); - DMA_ClearFlag(UART_DRV_TX_DMA, UART_DRV_TX_DMA_TCIF); - UART_DRV_TX_DMA->NDTR = tx_size; - DMA_Cmd(UART_DRV_TX_DMA, ENABLE); + uint8_t buf[1]; + if(rb_read(&hv_tx_buf, buf, 1)) { + ctx->to_hv.packet_to_hv.flags.buf = buf[0]; + } else { + ctx->to_hv.packet_to_hv.flags.buf = 0x0; + } - // clear uart faults - PIN(uart_sr) = UART_DRV->SR; - PIN(uart_dr) = UART_DRV->DR; + tx_size = sizeof(packet_to_hv_t); - //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); - } + ctx->conf_addr %= sizeof(f3_config_data_t) / 4; + + if(send_to_bootloader) { + send_to_bootloader = 0; + flash_state = SEND_TO_BOOTLOADER; + ctx->timeout = 0; + // TODO: check f3 crc, size, ... + } + break; + + case SEND_TO_BOOTLOADER: // fix + ctx->to_hv.packet_to_hv.header.flags.cmd = BOOTLOADER; + ctx->to_hv.packet_to_hv.flags.buf = 0x0; + ctx->to_hv.packet_to_hv.header.flags.counter++; + ctx->to_hv.packet_to_hv.d_cmd = 0.0; + ctx->to_hv.packet_to_hv.q_cmd = 0.0; + ctx->to_hv.packet_to_hv.flags.enable = 0; + + tx_size = sizeof(packet_to_hv_t); + + if(ctx->timeout > 10) { + ctx->timeout = 0; + flash_state = ERASE_FLASH; + } + break; + + case ERASE_FLASH: + ctx->to_hv.packet_to_hv.header.slave_addr = 255; + ctx->to_hv.packet_to_hv.header.flags.cmd = NO_CMD; + ctx->to_hv.packet_to_hv.header.flags.counter++; + ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; + ctx->to_hv.packet_to_hv.header.conf_addr = 0; + ctx->to_hv.packet_to_hv.header.config.f32 = 0; + ctx->to_hv.packet_to_hv_bootloader.addr = 0; + ctx->to_hv.packet_to_hv_bootloader.value = 0; + ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_PAGEERASE; + + tx_size = sizeof(packet_bootloader_t); + + ctx->addr = 0; + + // flash_state = SLAVE_IN_APP; + + if(ctx->timeout > 20000) { + ctx->timeout = 0; + flash_state = FLASH_FAILED; + } + break; + + case SEND_APP: + ctx->to_hv.packet_to_hv.header.flags.counter++; + ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; + ctx->to_hv.packet_to_hv_bootloader.addr = 0x08004000 + ctx->addr * 4; + ctx->to_hv.packet_to_hv_bootloader.value = ((uint32_t *)&_binary_obj_hvf3_hvf3_bin_start)[ctx->addr]; + ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_WRITE; + + tx_size = sizeof(packet_bootloader_t); + + if(ctx->timeout > 10) { + ctx->timeout = 0; + flash_state = FLASH_FAILED; + } + break; + + case CRC_CHECK: + ctx->to_hv.packet_to_hv.header.flags.counter++; + ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; + ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_CRCCHECK; + + tx_size = sizeof(packet_bootloader_t); + + if(ctx->timeout > 2000) { + ctx->timeout = 0; + flash_state = FLASH_FAILED; + } + break; + case SEND_TO_APP: + ctx->to_hv.packet_to_hv.header.flags.cmd = DO_RESET; + ctx->to_hv.packet_to_hv.header.flags.counter++; + ctx->to_hv.packet_to_hv.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4; + ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_NOP; + + tx_size = sizeof(packet_bootloader_t); + + if(ctx->timeout > 2000) { + ctx->timeout = 0; + flash_state = SLAVE_IN_APP; + } + break; + case FLASH_FAILED: + if(ctx->timeout > 10) { + ctx->timeout = 0; + flash_state = SLAVE_IN_APP; + } + break; } + if(ctx->send_state > 1){ + if(flash_state != SLAVE_IN_APP){ + tx_size = 0; + } + ctx->send_state = 0; + } + ctx->send_state++; + + if(tx_size) { + CRC_ResetDR(); + ctx->to_hv.packet_to_hv.header.crc = CRC_CalcBlockCRC((uint32_t *)&(ctx->to_hv.packet_to_hv.header.slave_addr), tx_size / 4 - 1); + + //start DMA TX transfer + DMA_Cmd(UART_DRV_TX_DMA, DISABLE); + DMA_ClearFlag(UART_DRV_TX_DMA, UART_DRV_TX_DMA_TCIF); + UART_DRV_TX_DMA->NDTR = tx_size; + DMA_Cmd(UART_DRV_TX_DMA, ENABLE); + + // clear uart faults + 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); + } + + PIN(state) = flash_state; } @@ -587,7 +579,7 @@ hal_comp_t hv_comp_struct = { .name = "hv", .nrt = nrt_func, .rt = rt_func, - .frt = frt_func, + .frt = 0, .nrt_init = nrt_init, .rt_start = 0, .frt_start = 0, diff --git a/src/comps/hw/io4.c b/src/comps/hw/io4.c index 1831018a..2398882a 100644 --- a/src/comps/hw/io4.c +++ b/src/comps/hw/io4.c @@ -76,6 +76,9 @@ HAL_PIN(fb1z); HAL_PIN(fbsd); //fb shutdown +HAL_PIN(master_arr); +HAL_PIN(clock_scale); + static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr; PIN(th0) = 12.0; @@ -83,6 +86,8 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { PIN(fbth0) = 2.5; PIN(fbth1) = 2.5; PIN(out_freq) = 15000; + PIN(master_arr) = TIM3->ARR; + PIN(clock_scale) = 1.0; } static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { @@ -253,6 +258,19 @@ static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { #define ARES 4096.0 #define AREF 3.3 +static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { + struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr; + if(PIN(clock_scale) > 1.01){ + TIM3->ARR = PIN(master_arr) + 1; + } + else if(PIN(clock_scale) < 0.99){ + TIM3->ARR = PIN(master_arr) - 1; + } + else{ + TIM3->ARR = PIN(master_arr); + } +} + static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { // struct io_ctx_t * ctx = (struct io_ctx_t *)ctx_ptr; struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr; @@ -502,7 +520,7 @@ hal_comp_t io_comp_struct = { .name = "io", .nrt = 0, .rt = rt_func, - .frt = 0, + .frt = frt_func, .nrt_init = nrt_init, .hw_init = hw_init, .rt_start = 0, diff --git a/src/comps/sserial.c b/src/comps/sserial.c index bd597e0a..803b1c71 100644 --- a/src/comps/sserial.c +++ b/src/comps/sserial.c @@ -45,6 +45,10 @@ HAL_PIN(vel_fb); HAL_PIN(current); HAL_PIN(scale); +HAL_PIN(clock_scale); +HAL_PIN(available); +HAL_PIN(phase); + HAL_PIN(in0); HAL_PIN(in1); HAL_PIN(in2); @@ -62,7 +66,7 @@ HAL_PIN(pos_advance); //TODO: move to ctx struct sserial_ctx_t { - uint32_t foo; + uint32_t phase; }; volatile uint8_t rxbuf[128]; //rx dma buffer @@ -327,16 +331,41 @@ static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) { block_bytes = 5; //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()); + + PIN(clock_scale) = 1.0; + PIN(phase) = 0; } +// static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { +// struct sserial_pin_ctx_t *pins = (struct sserial_pin_ctx_t *)pin_ptr; +// //struct sserial_ctx_t *mem = (struct sserial_ctx_t *)ctx_ptr; +// PIN(phase) = 0; +// } static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { struct sserial_pin_ctx_t *pins = (struct sserial_pin_ctx_t *)pin_ptr; + //struct sserial_ctx_t *mem = (struct sserial_ctx_t *)ctx_ptr; //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); + PIN(phase) += 1.0; + + uint32_t goal = 5; + PIN(clock_scale) = 1.0; + if(PIN(phase) > 3){ + PIN(phase) = 0; + if(available > goal){ + PIN(clock_scale) = 0.9; + } + else if(available < goal && available > 0){ + PIN(clock_scale) = 1.1; + } + } + + PIN(available) = available; + if(available >= 1) { lbp.byte = rxbuf[rxpos]; @@ -436,6 +465,7 @@ static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) { //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; diff --git a/src/setup.c b/src/setup.c index 79c0caa5..92ddda26 100644 --- a/src/setup.c +++ b/src/setup.c @@ -26,7 +26,7 @@ void setup() { GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; - + //fan GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_0; GPIO_Init(GPIOD, &GPIO_InitStructure); @@ -80,7 +80,7 @@ void setup_res() { 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_ARRPreloadConfig(TIM_SLAVE, ENABLE); - + TIM_SLAVE->CNT = (TIM_SLAVE->ARR + 1) / 2; TIM_Cmd(TIM_SLAVE, ENABLE); /* ADC clock enable */