Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-09-21 22:34:19 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

format

Browse Source
This commit is contained in:
crinq
2018-04-07 01:36:17 +02:00
parent b11ddf64f9
commit dd37e720d5
51 changed files with 797 additions and 825 deletions
Download Patch File Download Diff File Expand all files Collapse all files
bootloader/src/main.c
+1 -1
View File
@@ -61,7 +61,7 @@ int main(void) {
void (*SysMemBootJump)(void);
if((*((unsigned long *)0x2001C000) == 0xDEADBEEF) || pin || !app_ok()) { //Memory map, datasheet
*((unsigned long *)0x2001C000) = 0xCAFEFEED; //Reset bootloader trigger
*((unsigned long *)0x2001C000) = 0xCAFEFEED; //Reset bootloader trigger
__set_MSP(0x20001000);
//Point the PC to the System Memory reset vector (+4)
//AN2606
bootloader/src/version.c
+1 -1
View File
@@ -1,6 +1,6 @@
#include "version.h"
volatile const version_info_t version_info __attribute__ ((section (".version_info"))) = {
volatile const version_info_t version_info __attribute__((section(".version_info"))) = {
.product_name = "STMBL-Bootloader",
.major = 0,
.minor = 9,
f3_boot/inc/stm32f3xx_hal_conf.h
+8 -8
View File
@@ -95,7 +95,7 @@ extern "C" {
*/
#if !defined(HSE_VALUE)
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#endif /* HSE_VALUE */
/**
* @brief In the following line adjust the External High Speed oscillator (HSE) Startup
@@ -103,7 +103,7 @@ extern "C" {
*/
#if !defined(HSE_STARTUP_TIMEOUT)
#define HSE_STARTUP_TIMEOUT ((uint32_t)100) /*!< Time out for HSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */
#endif /* HSE_STARTUP_TIMEOUT */
/**
* @brief Internal High Speed oscillator (HSI) value.
@@ -112,7 +112,7 @@ extern "C" {
*/
#if !defined(HSI_VALUE)
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
#endif /* HSI_VALUE */
/**
* @brief In the following line adjust the Internal High Speed oscillator (HSI) Startup
@@ -120,7 +120,7 @@ extern "C" {
*/
#if !defined(HSI_STARTUP_TIMEOUT)
#define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */
#endif /* HSI_STARTUP_TIMEOUT */
#endif /* HSI_STARTUP_TIMEOUT */
/**
* @brief Internal Low Speed oscillator (LSI) value.
@@ -135,14 +135,14 @@ extern "C" {
*/
#if !defined(LSE_VALUE)
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */
#endif /* LSE_VALUE */
#endif /* LSE_VALUE */
/**
* @brief Time out for LSE start up value in ms.
*/
#if !defined(LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */
#endif /* LSE_STARTUP_TIMEOUT */
#endif /* LSE_STARTUP_TIMEOUT */
/**
* @brief External clock source for I2S peripheral
@@ -153,7 +153,7 @@ extern "C" {
*/
#if !defined(EXTERNAL_CLOCK_VALUE)
#define EXTERNAL_CLOCK_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz*/
#endif /* EXTERNAL_CLOCK_VALUE */
#endif /* EXTERNAL_CLOCK_VALUE */
/* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */
@@ -163,7 +163,7 @@ extern "C" {
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)0) /*!< tick interrupt priority (lowest by default) */
#define USE_RTOS 0
#define PREFETCH_ENABLE 1
f3_boot/src/main.c
+34 -39
View File
@@ -55,52 +55,49 @@ static int app_ok(void) {
}
void TIM8_UP_IRQHandler() {
static uint32_t last_dma_count = 0;
uint32_t dma_count = DMA1_Channel3->CNDTR;
// if(USART3->ISR & USART_ISR_RTOF) { // idle line
// USART3->ICR |= USART_ICR_RTOCF | USART_ICR_FECF | USART_ICR_ORECF; // timeout clear flag
if(dma_count == last_dma_count){ // framing
if(dma_count == last_dma_count) { // framing
last_dma_count = 0;
// start rx DMA
DMA1_Channel3->CCR &= (uint16_t)(~DMA_CCR_EN);
DMA1_Channel3->CNDTR = sizeof(packet_bootloader_t);
DMA1_Channel3->CCR |= DMA_CCR_EN;
}
else{
last_dma_count = dma_count;
} else {
last_dma_count = dma_count;
}
if(dma_count == 0){
if(rx_buf.header.slave_addr == 255 && rx_buf.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4 && rx_buf.header.crc == HAL_CRC_Calculate(&hcrc, (uint32_t *)&(rx_buf.header.slave_addr), sizeof(packet_bootloader_t) / 4 - 1)){
if(dma_count == 0) {
if(rx_buf.header.slave_addr == 255 && rx_buf.header.len == (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4 && rx_buf.header.crc == HAL_CRC_Calculate(&hcrc, (uint32_t *)&(rx_buf.header.slave_addr), sizeof(packet_bootloader_t) / 4 - 1)) {
//do stuff
//tx_buf.state = do_stuff();
switch(rx_buf.header.flags.cmd){
switch(rx_buf.header.flags.cmd) {
case NO_CMD:
break;
break;
case WRITE_CONF:
break;
break;
case READ_CONF:
break;
break;
case DO_RESET:
HAL_FLASH_Lock();
HAL_NVIC_SystemReset();
break;
break;
case BOOTLOADER:
break;
break;
}
HAL_StatusTypeDef status = HAL_OK;
switch(rx_buf.cmd){
switch(rx_buf.cmd) {
case BOOTLOADER_OPCODE_NOP:
break;
break;
case BOOTLOADER_OPCODE_READ:
tx_buf.value = *(uint32_t *)rx_buf.addr;
tx_buf.addr = rx_buf.addr;
tx_buf.addr = rx_buf.addr;
break;
case BOOTLOADER_OPCODE_WRITE:
@@ -111,7 +108,7 @@ void TIM8_UP_IRQHandler() {
status = HAL_ERROR;
}
tx_buf.value = *(uint32_t *)rx_buf.addr;
tx_buf.addr = rx_buf.addr;
tx_buf.addr = rx_buf.addr;
break;
case BOOTLOADER_OPCODE_PAGEERASE:
@@ -132,10 +129,9 @@ void TIM8_UP_IRQHandler() {
break;
case BOOTLOADER_OPCODE_CRCCHECK:
if(app_ok()){
if(app_ok()) {
status = HAL_OK;
}
else{
} else {
status = HAL_ERROR;
}
break;
@@ -147,15 +143,15 @@ void TIM8_UP_IRQHandler() {
// else{
// tx_buf.header.flags.error = 0;
// }
tx_buf.cmd = rx_buf.cmd;
tx_buf.cmd = rx_buf.cmd;
tx_buf.header.flags.counter = rx_buf.header.flags.counter;
tx_buf.header.slave_addr = 255;
tx_buf.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4;
tx_buf.header.conf_addr = 0;
tx_buf.header.config.u32 = 0;
tx_buf.header.flags.cmd = NO_CMD;
tx_buf.header.crc = HAL_CRC_Calculate(&hcrc, (uint32_t *)&(tx_buf.header.slave_addr), sizeof(packet_bootloader_t) / 4 - 1);
tx_buf.header.slave_addr = 255;
tx_buf.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4;
tx_buf.header.conf_addr = 0;
tx_buf.header.config.u32 = 0;
tx_buf.header.flags.cmd = NO_CMD;
tx_buf.header.crc = HAL_CRC_Calculate(&hcrc, (uint32_t *)&(tx_buf.header.slave_addr), sizeof(packet_bootloader_t) / 4 - 1);
rx_buf.header.crc = 0;
@@ -169,7 +165,7 @@ void TIM8_UP_IRQHandler() {
__HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE);
}
void uart_init(){
void uart_init() {
GPIO_InitTypeDef GPIO_InitStruct;
/* Peripheral clock enable */
@@ -205,7 +201,7 @@ void uart_init(){
//TX DMA
DMA1_Channel2->CCR &= (uint16_t)(~DMA_CCR_EN);
DMA1_Channel2->CPAR = (uint32_t) & (USART3->TDR);
DMA1_Channel2->CMAR = (uint32_t) & tx_buf;
DMA1_Channel2->CMAR = (uint32_t)&tx_buf;
DMA1_Channel2->CNDTR = sizeof(packet_bootloader_t);
DMA1_Channel2->CCR = DMA_CCR_MINC | DMA_CCR_DIR; // | DMA_CCR_PL_0 | DMA_CCR_PL_1
DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2;
@@ -213,13 +209,13 @@ void uart_init(){
//RX DMA
DMA1_Channel3->CCR &= (uint16_t)(~DMA_CCR_EN);
DMA1_Channel3->CPAR = (uint32_t) & (USART3->RDR);
DMA1_Channel3->CMAR = (uint32_t) & rx_buf;
DMA1_Channel3->CMAR = (uint32_t)&rx_buf;
DMA1_Channel3->CNDTR = sizeof(packet_bootloader_t);
DMA1_Channel3->CCR = DMA_CCR_MINC; // | DMA_CCR_PL_0 | DMA_CCR_PL_1
DMA1->IFCR = DMA_IFCR_CTCIF3 | DMA_IFCR_CHTIF3 | DMA_IFCR_CGIF3;
DMA1_Channel3->CCR |= DMA_CCR_EN;
USART3->RTOR = 16; // 16 bits timeout
USART3->RTOR = 16; // 16 bits timeout
USART3->CR2 |= USART_CR2_RTOEN; // timeout en
USART3->ICR |= USART_ICR_RTOCF; // timeout clear flag
}
@@ -303,17 +299,16 @@ int main(void) {
/* Initialize user application's Stack Pointer */
__set_MSP(*(__IO uint32_t *)APP_START);
JumpToApplication();
while(1){
while(1) {
}
}
else{
} else {
uart_init();
tx_buf.header.slave_addr = 255;
tx_buf.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4;
tx_buf.header.conf_addr = 0;
tx_buf.header.len = (sizeof(packet_bootloader_t) - sizeof(stmbl_talk_header_t)) / 4;
tx_buf.header.conf_addr = 0;
tx_buf.header.config.u32 = 0;
tx_buf.header.flags.cmd = NO_CMD;
tx_buf.header.flags.cmd = NO_CMD;
// start rx DMA
DMA1_Channel3->CCR &= (uint16_t)(~DMA_CCR_EN);
f3_boot/src/stm32f3xx_it.c
+10 -5
View File
@@ -50,7 +50,8 @@ extern PCD_HandleTypeDef hpcd_USB_FS;
*/
void NMI_Handler(void) {
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
while(1){}
while(1) {
}
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
@@ -64,7 +65,8 @@ void HardFault_Handler(void) {
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while(1){}
while(1) {
}
/* USER CODE BEGIN HardFault_IRQn 1 */
/* USER CODE END HardFault_IRQn 1 */
@@ -77,7 +79,8 @@ void MemManage_Handler(void) {
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
while(1){}
while(1) {
}
/* USER CODE BEGIN MemoryManagement_IRQn 1 */
/* USER CODE END MemoryManagement_IRQn 1 */
@@ -90,7 +93,8 @@ void BusFault_Handler(void) {
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
while(1){}
while(1) {
}
/* USER CODE BEGIN BusFault_IRQn 1 */
/* USER CODE END BusFault_IRQn 1 */
@@ -103,7 +107,8 @@ void UsageFault_Handler(void) {
/* USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 */
while(1){}
while(1) {
}
/* USER CODE BEGIN UsageFault_IRQn 1 */
/* USER CODE END UsageFault_IRQn 1 */
f3_boot/src/version.c
+1 -1
View File
@@ -1,6 +1,6 @@
#include "version.h"
volatile const version_info_t version_info __attribute__ ((section (".version_info"))) = {
volatile const version_info_t version_info __attribute__((section(".version_info"))) = {
.product_name = "STMBL-hv-f303",
.major = 0,
.minor = 9,
inc/hw/hw.h
+10 -10
View File
@@ -25,14 +25,14 @@
#define FB0_COS_PIN GPIO_Pin_5
#define FB0_COS_PORT GPIOC
#define ADC_REF 3.3 //analog reference voltage
#define ADC_REF 3.3 //analog reference voltage
#define ADC_RES 4096.0 //analog resolution, 12 bit
#define OP_R_INPUT 1000.0 //opamp input
#define OP_R_INPUT 1000.0 //opamp input
#define OP_R_FEEDBACK 3900.0 //opamp feedback
#define OP_R_OUT_LOW 180.0 //opamp out low
#define OP_R_OUT_HIGH 470.0 //opamp out high
#define OP_REF 5.0 //opamp reference voltage
#define OP_R_OUT_LOW 180.0 //opamp out low
#define OP_R_OUT_HIGH 470.0 //opamp out high
#define OP_REF 5.0 //opamp reference voltage
//FB UART
#define FB0_UART USART3
@@ -161,14 +161,14 @@
#define FB1_COS_PIN GPIO_Pin_5
#define FB1_COS_PORT GPIOA
#define ADC_REF 3.3 //analog reference voltage
#define ADC_REF 3.3 //analog reference voltage
#define ADC_RES 4096.0 //analog resolution, 12 bit
#define OP_R_INPUT 10000.0 //opamp input
#define OP_R_INPUT 10000.0 //opamp input
#define OP_R_FEEDBACK 15000.0 //opamp feedback
#define OP_R_OUT_LOW 470.0 //opamp out low
#define OP_R_OUT_HIGH 22.0 //opamp out high
#define OP_REF 1.83 //opamp reference voltage
#define OP_R_OUT_LOW 470.0 //opamp out low
#define OP_R_OUT_HIGH 22.0 //opamp out high
#define OP_REF 1.83 //opamp reference voltage
//FB UART
#define FB0_UART USART6
inc/sserial.h
+9 -9
View File
@@ -43,8 +43,8 @@
#define LBPCardName2Cmd 0xd2
#define LBPCardName3Cmd 0xd3
#define CT_RW 1 // 01b
#define CT_RPC 2 // 10b
#define CT_RW 1 // 01b
#define CT_RPC 2 // 10b
#define CT_LOCAL 3 // 11b
#define MEM_SIZE 2048
@@ -103,12 +103,12 @@
typedef union {
struct {
uint8_t ds : 2; //DataSize: Specifies data size, 00b = 1 bytes, 01b = 2 bytes, 10 b= 4 bytes, 11b = 8 bytes.
uint8_t as : 1; //AddressSize: 0 to specify current address, 1 to specify 2 byte address.
uint8_t ai : 1; //AutoInc: 0 leaves address unchanged, 1 specifies that address is post incremented by data size in bytes.
uint8_t ds : 2; //DataSize: Specifies data size, 00b = 1 bytes, 01b = 2 bytes, 10 b= 4 bytes, 11b = 8 bytes.
uint8_t as : 1; //AddressSize: 0 to specify current address, 1 to specify 2 byte address.
uint8_t ai : 1; //AutoInc: 0 leaves address unchanged, 1 specifies that address is post incremented by data size in bytes.
uint8_t rid : 1; //RPCIncludesData: 0 specifies that data is from stream, 1, that data is from RPC (RPC only, ignored for non RPC commands)
uint8_t wr : 1; //Write: 1 to specify write, 0 to specify read
uint8_t ct : 2; //CommandType: Must be 01b to specify data read/write command
uint8_t wr : 1; //Write: 1 to specify write, 0 to specify read
uint8_t ct : 2; //CommandType: Must be 01b to specify data read/write command
// 01:rw 11:local command 10:rpc
};
struct {
@@ -145,7 +145,7 @@ typedef struct {
} mode_descriptor_t;
typedef struct {
uint8_t input; //process data input bytes
uint8_t input; //process data input bytes
uint8_t output; //process data output bytes
uint16_t ptocp; //pointer to process data table start
uint16_t gtocp; //pointer to mode data table start
@@ -154,7 +154,7 @@ typedef struct {
typedef union {
struct {
discovery_rpc_t discovery; //6 bytes
uint16_t foo; //padding, so heap is word aligned
uint16_t foo; //padding, so heap is word aligned
uint8_t heap[MEM_SIZE - sizeof(discovery_rpc_t)];
};
inc/usbd_cdc_if.h
+1 -1
View File
@@ -12,7 +12,7 @@
extern uint8_t APP_Rx_Buffer[]; /* Write CDC received data in this buffer.
These data will be sent over USB IN endpoint
in the CDC core functions. */
extern uint32_t APP_Rx_ptr_in; /* Increment this pointer or roll it back to
extern uint32_t APP_Rx_ptr_in; /* Increment this pointer or roll it back to
start address when writing received data
in the buffer APP_Rx_Buffer. */
shared/common.h
+4 -4
View File
@@ -33,12 +33,12 @@ typedef struct {
stmbl_talk_header_t header;
uint32_t addr;
uint32_t value;
enum{
enum {
BOOTLOADER_OPCODE_NOP = 0,
BOOTLOADER_OPCODE_READ,
BOOTLOADER_OPCODE_WRITE, // MEM[addr] = value
BOOTLOADER_OPCODE_PAGEERASE, // clear PAGE[addr]
BOOTLOADER_OPCODE_CRCCHECK, // CRC(APP) == 0
BOOTLOADER_OPCODE_WRITE, // MEM[addr] = value
BOOTLOADER_OPCODE_PAGEERASE, // clear PAGE[addr]
BOOTLOADER_OPCODE_CRCCHECK, // CRC(APP) == 0
} cmd : 8;
enum {
BOOTLOADER_STATE_OK = 0,
shared/common_f1.h
+3 -3
View File
@@ -28,10 +28,10 @@ typedef struct {
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 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 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
shared/comps/acim_ttc.c
+1 -1
View File
@@ -42,7 +42,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
float slip = 0.0;
switch((int)PIN(mode)) {
case 0: // slip control
case 0: // slip control
id = cur_n / sqrtf(2.0); // constant flux
iq = cur_n / sqrtf(2.0) / torque_n * torque;
slip = slip_n / torque_n * torque;
shared/comps/fault.c
+28 -28
View File
@@ -130,66 +130,66 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
}
if(err_filter(&(ctx->cmd_error), 5.0, 0.001, PIN(cmd_error) > 0.0)) {
ctx->fault = CMD_ERROR;
ctx->fault = CMD_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(err_filter(&(ctx->mot_fb_error), 5.0, 0.001, PIN(mot_fb_error) > 0.0)) {
ctx->fault = MOT_FB_ERROR;
ctx->fault = MOT_FB_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(err_filter(&(ctx->com_fb_error), 5.0, 0.001, PIN(com_fb_error) > 0.0)) {
ctx->fault = COM_FB_ERROR;
ctx->fault = COM_FB_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(err_filter(&(ctx->joint_fb_error), 5.0, 0.001, PIN(joint_fb_error) > 0.0)) {
ctx->fault = JOINT_FB_ERROR;
ctx->fault = JOINT_FB_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
// if(err_filter(&(ctx->hv_error), 3.0, 0.001, PIN(hv_error) > 0.0)) {
// if(err_filter(&(ctx->hv_error), 3.0, 0.001, PIN(hv_error) > 0.0)) {
float hv_error = PIN(hv_error);
if(hv_error > 0.0) {
//TODO: pass error, move filter to hv comp
ctx->fault = hv_error;
ctx->fault = hv_error;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(ABS(PIN(pos_error)) > PIN(max_pos_error)) {
ctx->fault = POS_ERROR;
ctx->fault = POS_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(PIN(sat) > PIN(max_sat)) {
ctx->fault = SAT_ERROR;
ctx->fault = SAT_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(err_filter(&(ctx->hv_temp_error), 5.0, 0.001, PIN(hv_temp) > PIN(max_hv_temp))) {
ctx->fault = HV_TEMP_ERROR;
ctx->fault = HV_TEMP_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(err_filter(&(ctx->dc_volt_error), 5.0, 0.001, PIN(dc_volt) > PIN(max_dc_volt) || PIN(dc_volt) < PIN(min_dc_volt))) {
ctx->fault = HV_VOLT_ERROR;
ctx->fault = HV_VOLT_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
if(err_filter(&(ctx->mot_temp_error), 5.0, 0.001, PIN(mot_temp) > PIN(max_mot_temp))) {
ctx->fault = MOT_TEMP_ERROR;
ctx->fault = MOT_TEMP_ERROR;
PIN(last_fault) = ctx->fault;
ctx->state = SOFT_FAULT;
ctx->state = SOFT_FAULT;
}
float scale = 1.0;
@@ -227,11 +227,11 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
break;
case ENABLED:
PIN(mot_brake) = 1.0;
PIN(en_out) = 1.0;
PIN(en_fb) = 1.0;
PIN(en_pid) = 1.0;
ctx->fault = NO_ERROR;
PIN(mot_brake) = 1.0;
PIN(en_out) = 1.0;
PIN(en_fb) = 1.0;
PIN(en_pid) = 1.0;
ctx->fault = NO_ERROR;
PIN(last_fault) = NO_ERROR;
break;
@@ -345,7 +345,7 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
case MOT_TEMP_ERROR:
printf("Motor overtemperture\n");
break;
case HV_CURRENT_OFFSET_FAULT:
printf("Current offset fault\n");
break;
@@ -353,7 +353,7 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
case HV_OVERCURRENT_RMS:
printf("Motor overcurrent rms\n");
break;
case HV_OVERCURRENT_PEAK:
printf("Motor overcurrent peak\n");
break;
shared/comps/jog.c
+8 -9
View File
@@ -10,17 +10,17 @@ static volatile float jog;
static volatile float jog_timeout;
void jog_left(char *ptr) {
jog = -1.0;
jog = -1.0;
jog_timeout = 0.0;
}
void jog_right(char *ptr) {
jog = 1.0;
jog = 1.0;
jog_timeout = 0.0;
}
void jog_stop(char *ptr) {
jog = 0.0;
jog = 0.0;
jog_timeout = 0.0;
}
@@ -29,18 +29,17 @@ COMMAND("jogr", jog_right, "Jog right");
COMMAND("jogx", jog_stop, "Stop jog");
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
jog_timeout = 0.0;
jog = 0.0;
jog_timeout = 0.0;
jog = 0.0;
}
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct jog_pin_ctx_t *pins = (struct jog_pin_ctx_t *)pin_ptr;
if(jog_timeout < 0.75) {
jog_timeout += period;
}
else{
jog = 0.0;
jog_timeout += period;
} else {
jog = 0.0;
}
PIN(jog) = jog;
}
shared/comps/linrev.c
+1 -1
View File
@@ -37,7 +37,7 @@ HAL_PIN(rev_clear);
HAL_PIN(rev);
struct linrev_ctx_t {
int lastq; //last quadrant
int lastq; //last quadrant
int32_t rev; //current multiturn
};
shared/comps/pid.c
+23 -23
View File
@@ -27,25 +27,25 @@
HAL_COMP(pid);
HAL_PIN(pos_ext_cmd); // cmd in (rad)
HAL_PIN(pos_fb); // feedback in (rad)
HAL_PIN(pos_error); // error out (rad)
HAL_PIN(pos_fb); // feedback in (rad)
HAL_PIN(pos_error); // error out (rad)
HAL_PIN(vel_ext_cmd); // cmd in (rad/s)
HAL_PIN(vel_fb); // feedback in (rad/s)
HAL_PIN(vel_cmd); // cmd out (rad/s)
HAL_PIN(vel_error); // error out (rad/s)
HAL_PIN(vel_fb); // feedback in (rad/s)
HAL_PIN(vel_cmd); // cmd out (rad/s)
HAL_PIN(vel_error); // error out (rad/s)
HAL_PIN(acc_ext_cmd); // cmd in (rad/s^2)
HAL_PIN(acc_cmd); // cmd out (rad/s^2)
HAL_PIN(acc_cmd); // cmd out (rad/s^2)
HAL_PIN(torque_ext_cmd); // cmd in (Nm)
HAL_PIN(torque_cmd); // cmd out (Nm)
HAL_PIN(torque_cmd); // cmd out (Nm)
HAL_PIN(torque_cor_cmd); // corrected cmd out (Nm)
HAL_PIN(torque_sum);
HAL_PIN(friction); // (Nm)
HAL_PIN(damping); // (Nm/rad/s)
HAL_PIN(load); // (Nm)
HAL_PIN(damping); // (Nm/rad/s)
HAL_PIN(load); // (Nm)
HAL_PIN(enable);
@@ -68,13 +68,13 @@ HAL_PIN(j); // (kgm^2)
// system limits
HAL_PIN(max_vel); // (rad/s)
HAL_PIN(max_vel); // (rad/s)
HAL_PIN(max_torque); // (Nm)
HAL_PIN(min_torque); // (Nm)
// user limits
HAL_PIN(max_usr_vel); // (rad/s)
HAL_PIN(max_usr_acc); // (rad/s^2)
HAL_PIN(max_usr_vel); // (rad/s)
HAL_PIN(max_usr_acc); // (rad/s^2)
HAL_PIN(max_usr_torque); // (Nm)
HAL_PIN(vel_sat);
@@ -83,7 +83,7 @@ HAL_PIN(torque_sat);
HAL_PIN(saturated); // (s)
struct pid_ctx_t {
float sat; //saturated time
float sat; //saturated time
float torque_sum; //integrator
};
@@ -97,7 +97,7 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
PIN(vel_en) = 1.0;
PIN(acc_en) = 1.0;
PIN(torque_en) = 1.0;
PIN(pos_p) = 100.0; // (1/s)
PIN(pos_p) = 100.0; // (1/s)
PIN(vel_p) = 2000.0; // (1/s)
PIN(vel_i) = 10.0;
PIN(vel_g) = 1.0;
@@ -120,14 +120,14 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
float acc_ext_cmd = PIN(acc_ext_cmd);
float acc_min = -PIN(max_usr_acc); //MAX(torque_min / j, -PIN(max_usr_acc));
float acc_max = PIN(max_usr_acc); //MIN(torque_max / j, PIN(max_usr_acc));
float acc_max = PIN(max_usr_acc); //MIN(torque_max / j, PIN(max_usr_acc));
float acc_cmd;
float acc_sat;
float vel_ext_cmd = PIN(vel_ext_cmd);
float vel_fb = PIN(vel_fb);
float vel_min = MAX(-PIN(max_vel) * g, -PIN(max_usr_vel)); //MAX(MAX(velfb + accmin * period, -PIN(max_vel) * p) , -PIN(max_usr_vel));
float vel_max = MIN(PIN(max_vel) * g, PIN(max_usr_vel)); //MIN(MIN(velfb + accmax * period, PIN(max_vel) * p), PIN(max_usr_vel));
float vel_max = MIN(PIN(max_vel) * g, PIN(max_usr_vel)); //MIN(MIN(velfb + accmax * period, PIN(max_vel) * p), PIN(max_usr_vel));
float vel_cmd;
float vel_error;
float vel_sat;
@@ -177,22 +177,22 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
vel_cmd = pos_error * pos_p; // p
//vel_cmd = CLAMP(vel_cmd, vel_fb + acc_min * period, vel_fb + acc_max * period); // adaptive loop clamping
vel_cmd = CLAMP(vel_cmd, vel_min * vel_g, vel_max * vel_g); // p clamping
vel_cmd += vel_ext_cmd; // ff
vel_cmd += vel_ext_cmd; // ff
vel_sat = SAT2(vel_cmd, vel_min * 0.99, vel_max * 0.99);
vel_cmd = CLAMP(vel_cmd, vel_min, vel_max); // clamping
// vel -> acc
vel_error = vel_cmd - vel_fb;
acc_cmd = vel_error * vel_p; // p
acc_cmd = vel_error * vel_p; // p
acc_cmd = CLAMP(acc_cmd, acc_min * acc_g, acc_max * acc_g); // p clamping
acc_cmd += acc_ext_cmd; // ff
acc_cmd += acc_ext_cmd; // ff
acc_sat = SAT2(acc_cmd, acc_min * 0.99, acc_max * 0.99);
acc_cmd = CLAMP(acc_cmd, acc_min, acc_max); // clamping
// acc -> torque
torque_cmd = acc_cmd * j + torque_ext_cmd; // ff
torque_cmd = CLAMP(torque_cmd, torque_min, torque_max); // clamping
ctx->torque_sum += vel_error * period * vel_i; // i
torque_cmd = acc_cmd * j + torque_ext_cmd; // ff
torque_cmd = CLAMP(torque_cmd, torque_min, torque_max); // clamping
ctx->torque_sum += vel_error * period * vel_i; // i
ctx->torque_sum = CLAMP(ctx->torque_sum, torque_min - torque_cmd, torque_max - torque_cmd); // dynamic integral clamping
torque_cmd += ctx->torque_sum;
torque_sat = SAT2(torque_cmd, torque_min * 0.99, torque_max * 0.99);
@@ -209,7 +209,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
torque_cmd += SIGN(torque_cmd) * PIN(friction);
}
torque_cmd += vel_fb * PIN(damping); // damping ff
torque_cmd -= PIN(load); // load ff
torque_cmd -= PIN(load); // load ff
torque_sat += 2 * SAT2(torque_cmd, torque_min, torque_max);
torque_cmd = CLAMP(torque_cmd, torque_min, torque_max); // min/max clamping
shared/comps/pmsm_ttc.c
+14 -14
View File
@@ -11,20 +11,20 @@ HAL_PIN(psi);
HAL_PIN(polecount);
// cogging torque ripple, constant
HAL_PIN(ac); // amplitude
HAL_PIN(pc); // phase
HAL_PIN(nc); // frequency
HAL_PIN(ac); // amplitude
HAL_PIN(pc); // phase
HAL_PIN(nc); // frequency
// electrical torque ripple, linear with current
HAL_PIN(ae); // amplitude
HAL_PIN(pe); // phase
HAL_PIN(ne); // frequency
HAL_PIN(ae); // amplitude
HAL_PIN(pe); // phase
HAL_PIN(ne); // frequency
HAL_PIN(pos_in);
HAL_PIN(pos_out);
HAL_PIN(t); // compensation torque
HAL_PIN(g); // compensation gain
HAL_PIN(block_gain); // block commutation gain
HAL_PIN(t); // compensation torque
HAL_PIN(g); // compensation gain
HAL_PIN(block_gain); // block commutation gain
// torque cmd in
@@ -37,8 +37,8 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct sim_ctx_t * ctx = (struct sim_ctx_t *)ctx_ptr;
struct pmsm_ttc_pin_ctx_t *pins = (struct pmsm_ttc_pin_ctx_t *)pin_ptr;
PIN(nc) = 1.0;
PIN(ne) = 1.0;
PIN(nc) = 1.0;
PIN(ne) = 1.0;
PIN(block_gain) = 0.0;
}
@@ -49,14 +49,14 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
float p = MAX(PIN(polecount), 1.0);
float psi_m = MAX(PIN(psi), 0.01);
float torque = PIN(torque);
float pos = PIN(pos_in);
float pos = PIN(pos_in);
float tc = PIN(ac) * sinf(PIN(pc) + pos * PIN(nc));
float te = torque * PIN(ae) * sinf(PIN(pe) + pos * PIN(ne));
PIN(pos_out) = pos * (1.0 - PIN(block_gain)) + (((int)((pos / 2.0 / M_PI + 0.5) * 6 + 0.5)) / 6.0 * 2.0 * M_PI - M_PI) * PIN(block_gain);
PIN(t) = tc + te;
PIN(cur) = (torque + PIN(g) * (tc + te)) / 3.0 * 2.0 / p / psi_m;
PIN(t) = tc + te;
PIN(cur) = (torque + PIN(g) * (tc + te)) / 3.0 * 2.0 / p / psi_m;
}
hal_comp_t pmsm_ttc_comp_struct = {
shared/comps/spid.c
+13 -13
View File
@@ -8,18 +8,18 @@ HAL_COMP(spid);
// input
HAL_PIN(cmd); // command
HAL_PIN(fb); // feedback
HAL_PIN(en); // enalbe
HAL_PIN(fb); // feedback
HAL_PIN(en); // enalbe
// gains
HAL_PIN(kp); // proportional
HAL_PIN(ki); // integator
HAL_PIN(kd); // differential
HAL_PIN(ksd); // scaled differential
HAL_PIN(kdi); // differential integrator
HAL_PIN(ksdi); // scaled differential integrator
HAL_PIN(kff0); // feedforward 0
HAL_PIN(kff1); // feedforward 1
HAL_PIN(kp); // proportional
HAL_PIN(ki); // integator
HAL_PIN(kd); // differential
HAL_PIN(ksd); // scaled differential
HAL_PIN(kdi); // differential integrator
HAL_PIN(ksdi); // scaled differential integrator
HAL_PIN(kff0); // feedforward 0
HAL_PIN(kff1); // feedforward 1
HAL_PIN(offset); // 0 offset
HAL_PIN(min_output);
@@ -55,16 +55,16 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
float error_d = (error - ctx->last_error) / period;
float output = 0.0;
output += cmd * PIN(kff0); // feedforward 0
output += cmd * PIN(kff0); // feedforward 0
output += cmd_d * PIN(kff1); // feedforward 1
output += error * PIN(kp); // porportional
output += error * PIN(kp); // porportional
output += error_d * PIN(kd); // differential
if(PIN(ksd) != 0.0 && ABS(error) > (max - min) / PIN(ksd) * 0.001) {
ctx->error_sum += error_d / ABS(error) * PIN(ksd); // scalded differential
}
output = CLAMP(output, min, max);
ctx->error_sum += error * PIN(ki) * period; // integrator
ctx->error_sum += error * PIN(ki) * period; // integrator
ctx->error_sum += error_d * PIN(kdi) * period; // differential integrator
if(PIN(ksdi) != 0.0 && ABS(error) > (max - min) / PIN(ksdi) * 0.001) {
ctx->error_sum += error_d / ABS(error) * PIN(ksdi) * period; // scalded differential integrator
shared/comps/stp.c
+8 -8
View File
@@ -28,14 +28,14 @@ struct stp_ctx_t {
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct stp_ctx_t *ctx = (struct stp_ctx_t *)ctx_ptr;
struct stp_pin_ctx_t *pins = (struct stp_pin_ctx_t *)pin_ptr;
ctx->p0 = 0.0;
ctx->p = 0.0;
ctx->v0 = 0.0;
ctx->pold = 0.0;
ctx->vold = 0.0;
PIN(target) = 0.0;
PIN(max_vel) = 1.0 * 2.0 * M_PI;
PIN(max_acc) = 10.0 * 2.0 * M_PI;
ctx->p0 = 0.0;
ctx->p = 0.0;
ctx->v0 = 0.0;
ctx->pold = 0.0;
ctx->vold = 0.0;
PIN(target) = 0.0;
PIN(max_vel) = 1.0 * 2.0 * M_PI;
PIN(max_acc) = 10.0 * 2.0 * M_PI;
}
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
shared/comps/term.c
-1
View File
@@ -25,7 +25,6 @@ struct term_ctx_t {
};
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct term_ctx_t * ctx = (struct sim_ctx_t *)ctx_ptr;
struct term_pin_ctx_t *pins = (struct term_pin_ctx_t *)pin_ptr;
shared/comps/ypid.c
+8 -8
View File
@@ -27,22 +27,22 @@
HAL_COMP(ypid);
HAL_PIN(pos_ext_cmd); // cmd in (rad)
HAL_PIN(pos_fb); // feedback in (rad)
HAL_PIN(pos_error); // error out (rad)
HAL_PIN(pos_fb); // feedback in (rad)
HAL_PIN(pos_error); // error out (rad)
HAL_PIN(vel_ext_cmd); // cmd in (rad/s)
HAL_PIN(vel_fb); // feedback in (rad/s)
HAL_PIN(vel_cmd); // cmd out (rad/s)
HAL_PIN(vel_error); // error out (rad/s)
HAL_PIN(vel_min); // minimum velocity error 0.3
HAL_PIN(vel_fb); // feedback in (rad/s)
HAL_PIN(vel_cmd); // cmd out (rad/s)
HAL_PIN(vel_error); // error out (rad/s)
HAL_PIN(vel_min); // minimum velocity error 0.3
HAL_PIN(enable);
HAL_PIN(out);
HAL_PIN(pos_p); //10
HAL_PIN(vel_p); //0.5
HAL_PIN(vel_i); //0.005
HAL_PIN(vel_p); //0.5
HAL_PIN(vel_i); //0.005
HAL_PIN(vel_ff); //1.0
// system limits
shared/defines.h
+8 -8
View File
@@ -52,15 +52,15 @@ extern "C" {
#define STRINGIFY(x) STRINGIFY_(x)
extern volatile uint64_t systime;
#define BLINK(N) \
({ \
#define BLINK(N) \
({ \
int t = (systime / 300) % (2 * (unsigned int)(N) + 2); \
if(t < 2) { \
t = 0; \
} else { \
t = t % 2; \
} \
t; \
if(t < 2) { \
t = 0; \
} else { \
t = t % 2; \
} \
t; \
})
//TODO: change type to typeof()
shared/hal.c
+18 -19
View File
@@ -268,26 +268,26 @@ COMMAND("hal", hal_term_print_info, "print HAL stats");
uint32_t load_comp(hal_comp_t *comp) {
if(!comp) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">load_comp: not found</font>\n");
}
return (0);
}
if(hal.comp_inst_count >= HAL_MAX_COMPS - 1) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">load_comp: not enough space to load comp: %s</font>\n", comp->name);
}
return (0);
}
if(hal.pin_inst_count + comp->pin_count >= HAL_MAX_PINS - 1) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">load_comp: not enough space to load comp pins: %s</font>\n", comp->name);
}
return (0);
}
uint32_t ctx_size = ((uint32_t)ceil((comp->ctx_size / 4.0))) * 4;
if(hal.ctx_count + ctx_size >= HAL_MAX_CTX - 1) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">load_comp: not enough space to load comp ctx: %s</font>\n", comp->name);
}
return (0);
@@ -813,14 +813,14 @@ uint32_t hal_parse_(char *cmd) {
case 1: // search comps
for(int i = 0; i < hal.comp_inst_count; i++) {
if(!strncmp(hal.comp_insts[i].comp->name, sinkc, strlen(sinkc))) {
if(hal.debug_level < 1){
if(hal.debug_level < 1) {
printf("%s%lu\n", hal.comp_insts[i].comp->name, hal.comp_insts[i].instance);
}
found = 1;
}
}
if(!found) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s</font>\n", cmd);
}
}
@@ -837,7 +837,7 @@ uint32_t hal_parse_(char *cmd) {
}
}
if(!found) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s</font>\n", cmd);
}
}
@@ -849,15 +849,15 @@ uint32_t hal_parse_(char *cmd) {
source = pin_inst_by_name(sourcec, sourcei, sourcep);
if(sink && source) {
sink->source = source;
if(hal.debug_level < 1){
if(hal.debug_level < 1) {
printf("OK %s%lu.%s <= %s%lu.%s = %f\n", sinkc, sinki, sinkp, sourcec, sourcei, sourcep, source->source->value);
}
} else if(sink) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s%lu.%s</font>\n", sourcec, sourcei, sourcep);
}
} else {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s%lu.%s</font>\n", sinkc, sinki, sinkp);
}
}
@@ -875,7 +875,7 @@ uint32_t hal_parse_(char *cmd) {
}
}
if(!found) {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s</font>\n", cmd);
}
}
@@ -886,17 +886,17 @@ uint32_t hal_parse_(char *cmd) {
if(sink) {
sink->value = value;
sink->source = sink;
if(hal.debug_level < 1){
if(hal.debug_level < 1) {
printf("OK %s%lu.%s = %f\n", sinkc, sinki, sinkp, value);
}
} else {
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s%lu.%s</font>\n", sinkc, sinki, sinkp);
}
}
break;
default:
if(hal.debug_level < 2){
if(hal.debug_level < 2) {
printf("<font color=\"FireBrick\">not found: %s</font>\n", cmd);
}
}
@@ -920,18 +920,17 @@ void fault(char *ptr) {
COMMAND("fault", fault, "trigger fault");
void hal_set_debug_level(uint32_t debug_level){
void hal_set_debug_level(uint32_t debug_level) {
hal.debug_level = debug_level;
}
void debug_level(char *ptr){
void debug_level(char *ptr) {
int debug_level = 0;
if(ptr){
if(ptr) {
sscanf(ptr, "%i", &debug_level);
}
hal_set_debug_level(CLAMP(debug_level, 0, 2));
}
COMMAND("debug_level", debug_level, "set hal debug level, 0 = print all, 1 = print errors, 2 = no output");
shared/hal.h
+1 -1
View File
@@ -160,7 +160,7 @@ typedef struct {
volatile float frt_period;
hal_error_t error_info;
volatile enum {
PRINT_ALL,
PRINT_ERRORS,
shared/ringbuf.h
+2 -2
View File
@@ -7,8 +7,8 @@
*
*/
struct ringbuf {
char *buf; ///< Pointer to buffer memory
unsigned bufsize; ///< Size of buffer memory
char *buf; ///< Pointer to buffer memory
unsigned bufsize; ///< Size of buffer memory
volatile unsigned pos; ///< Current read position
volatile unsigned len; ///< Length of data in buffer
};
shared/stmbl_talk.h
+22 -22
View File
@@ -2,27 +2,27 @@
#include <stdint.h>
#pragma pack(push, 1)
typedef struct{
uint32_t crc; // can be 0 for eth comm
uint8_t slave_addr; // calc from uid, overwrite in config
uint8_t len; // #data in byte
uint8_t conf_addr;
struct {
enum stmbl_talk_cmd_t {
NO_CMD,
WRITE_CONF, // conf[rx.header.conf_addr] = rx.header.config.u32, tx.header.config.u32 = conf[tx.header.conf_addr++]
READ_CONF, // tx.header.config.u32 = conf[rx.header.conf_addr]
DO_RESET,
BOOTLOADER,
// ENABLE,
// DISABLE, // proc / conf data = default
} cmd : 4;
uint8_t counter : 4; // packet counter, mosi: ++, miso: same as request
} flags;
union{
float f32;
uint32_t u32;
int32_t i32;
} config;
typedef struct {
uint32_t crc; // can be 0 for eth comm
uint8_t slave_addr; // calc from uid, overwrite in config
uint8_t len; // #data in byte
uint8_t conf_addr;
struct {
enum stmbl_talk_cmd_t {
NO_CMD,
WRITE_CONF, // conf[rx.header.conf_addr] = rx.header.config.u32, tx.header.config.u32 = conf[tx.header.conf_addr++]
READ_CONF, // tx.header.config.u32 = conf[rx.header.conf_addr]
DO_RESET,
BOOTLOADER,
// ENABLE,
// DISABLE, // proc / conf data = default
} cmd : 4;
uint8_t counter : 4; // packet counter, mosi: ++, miso: same as request
} flags;
union {
float f32;
uint32_t u32;
int32_t i32;
} config;
} stmbl_talk_header_t;
#pragma pack(pop)
shared/version.h
+1 -1
View File
@@ -2,7 +2,7 @@
#include <stdint.h>
typedef struct{
typedef struct {
// set at link-time by add-version-info.py
uint32_t image_crc;
uint32_t image_size;
shared/yaskawa_crc16.c
+11 -14
View File
@@ -13,12 +13,11 @@
* - ReflectOut = False
* - Algorithm = table-driven
*/
#include "yaskawa_crc16.h" /* include the header file generated with pycrc */
#include "yaskawa_crc16.h" /* include the header file generated with pycrc */
#include <stdlib.h>
#include <stdint.h>
/**
* Static table used for the table_driven implementation.
*/
@@ -54,19 +53,17 @@ static const yaskawa_crc16_t crc_table[256] = {
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
};
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
yaskawa_crc16_t yaskawa_crc16_update(yaskawa_crc16_t crc, const void *data, size_t data_len)
{
const unsigned char *d = (const unsigned char *)data;
unsigned int tbl_idx;
yaskawa_crc16_t yaskawa_crc16_update(yaskawa_crc16_t crc, const void *data, size_t data_len) {
const unsigned char *d = (const unsigned char *)data;
unsigned int tbl_idx;
while (data_len--) {
tbl_idx = ((crc >> 8) ^ *d) & 0xff;
crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0xffff;
d++;
}
return crc & 0xffff;
while(data_len--) {
tbl_idx = ((crc >> 8) ^ *d) & 0xff;
crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0xffff;
d++;
}
return crc & 0xffff;
}
shared/yaskawa_crc16.h
+6 -8
View File
@@ -70,9 +70,8 @@ typedef uint_fast16_t yaskawa_crc16_t;
*
* \return The initial crc value.
*/
static inline yaskawa_crc16_t yaskawa_crc16_init(void)
{
return 0xffff;
static inline yaskawa_crc16_t yaskawa_crc16_init(void) {
return 0xffff;
}
@@ -93,14 +92,13 @@ yaskawa_crc16_t yaskawa_crc16_update(yaskawa_crc16_t crc, const void *data, size
* \param[in] crc The current crc value.
* \return The final crc value.
*/
static inline yaskawa_crc16_t yaskawa_crc16_finalize(yaskawa_crc16_t crc)
{
return crc ^ 0xffff;
static inline yaskawa_crc16_t yaskawa_crc16_finalize(yaskawa_crc16_t crc) {
return crc ^ 0xffff;
}
#ifdef __cplusplus
} /* closing brace for extern "C" */
} /* closing brace for extern "C" */
#endif
#endif /* YASKAWA_CRC16_H */
#endif /* YASKAWA_CRC16_H */
src/comps/adc.c
+11 -13
View File
@@ -14,11 +14,11 @@
HAL_COMP(adc);
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(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(sin1l); //sin output, last group only
HAL_PIN(cos1l); //cos output, last group only
@@ -86,10 +86,9 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
volatile uint32_t *ADC_DMA_Buffer;
if(DMA_GetCurrentMemoryTarget(DMA2_Stream0)){
if(DMA_GetCurrentMemoryTarget(DMA2_Stream0)) {
ADC_DMA_Buffer = ADC_DMA_Buffer1;
}
else{
} else {
ADC_DMA_Buffer = ADC_DMA_Buffer0;
}
int flip;
@@ -122,7 +121,6 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
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
}
if(ctx->send == 0) {
memcpy((void *)(ctx->txbuf_raw), (void *)ADC_DMA_Buffer, ADC_SAMPLES_IN_RT * 4);
@@ -131,8 +129,8 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
PIN(sin0l) = si0[ADC_GROUPS - 1];
PIN(cos0l) = co0[ADC_GROUPS - 1];
PIN(sin0) = sin0all / (float)ADC_GROUPS;
PIN(cos0) = cos0all / (float)ADC_GROUPS;
PIN(sin0) = sin0all / (float)ADC_GROUPS;
PIN(cos0) = cos0all / (float)ADC_GROUPS;
#ifdef FB1
PIN(sin1l) = si1[ADC_GROUPS - 1];
PIN(cos1l) = co1[ADC_GROUPS - 1];
@@ -199,7 +197,7 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
}
ctx->txpos = 0;
buf[0] = 255;//start of waves
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;
@@ -212,7 +210,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);
src/comps/enc_cmd.c
+7 -7
View File
@@ -13,7 +13,7 @@ HAL_PIN(pos);
HAL_PIN(a);
HAL_PIN(b);
HAL_PIN(fault);
HAL_PIN(mode); // 0 = quad, 1 = step/dir, 2 = dir/step, 3 = up/down
HAL_PIN(mode); // 0 = quad, 1 = step/dir, 2 = dir/step, 3 = up/down
HAL_PIN(remap); // 0 = cmd, 1 = fb0, 2 = fb1
struct enc_cmd_ctx_t {
@@ -127,16 +127,16 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
TIM_EncoderInterfaceConfig(ctx->tim, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICInitTypeDef TIM_ICInitStruct;
TIM_ICInitStruct.TIM_Channel = TIM_Channel_1;
TIM_ICInitStruct.TIM_ICFilter = 0x0f; //Digital filtering @ 1/32 fDTS
TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge; //Just trigger at the rising edge, because its the clock
TIM_ICInitStruct.TIM_ICPrescaler = 1; //no prescaler, capture is done each time an edge is detected on the capture input
TIM_ICInitStruct.TIM_ICFilter = 0x0f; //Digital filtering @ 1/32 fDTS
TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_BothEdge; //Just trigger at the rising edge, because its the clock
TIM_ICInitStruct.TIM_ICPrescaler = 1; //no prescaler, capture is done each time an edge is detected on the capture input
TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_IndirectTI; //IC1 mapped to TI1
TIM_ICInit(ctx->tim, &TIM_ICInitStruct);
TIM_ICInitStruct.TIM_Channel = TIM_Channel_2;
TIM_ICInitStruct.TIM_ICFilter = 0x0f; //Digital filtering @ 1/32 fDTS
TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Rising; //Trigger at every edge, because its the direction
TIM_ICInitStruct.TIM_ICPrescaler = 1; //no prescaler, capture is done each time an edge is detected on the capture input
TIM_ICInitStruct.TIM_ICFilter = 0x0f; //Digital filtering @ 1/32 fDTS
TIM_ICInitStruct.TIM_ICPolarity = TIM_ICPolarity_Rising; //Trigger at every edge, because its the direction
TIM_ICInitStruct.TIM_ICPrescaler = 1; //no prescaler, capture is done each time an edge is detected on the capture input
TIM_ICInitStruct.TIM_ICSelection = TIM_ICSelection_DirectTI; //IC2 mapped to TI1
TIM_ICInit(ctx->tim, &TIM_ICInitStruct);
TIM_Cmd(ctx->tim, ENABLE);
src/comps/enc_fb.c
+1 -1
View File
@@ -93,7 +93,7 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
TIM_EncoderInterfaceConfig(FB0_ENC_TIM, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
TIM_Cmd(FB0_ENC_TIM, ENABLE);
FB0_ENC_TIM->CCMR2 |= TIM_CCMR2_CC3S_0; //CC3 channel is configured as input, IC3 is mapped on CH3
FB0_ENC_TIM->CCER |= TIM_CCER_CC3E; //Capture enabled
FB0_ENC_TIM->CCER |= TIM_CCER_CC3E; //Capture enabled
}
src/comps/encf.c
+83 -84
View File
@@ -29,22 +29,22 @@ uint32_t send_counterf;
volatile uint16_t tim_data[160];
#pragma pack(push, 1)
typedef struct{
uint32_t flag0 : 4; // 0101
typedef struct {
uint32_t flag0 : 4; // 0101
uint32_t bat : 2;
uint32_t flag1 : 3; // 101
uint32_t no_index : 1;
uint32_t flag2 : 1; // 0
uint32_t flag1 : 3; // 101
uint32_t no_index : 1;
uint32_t flag2 : 1; // 0
uint32_t pos_lo : 6;
uint32_t flag3 : 2; // 10
uint32_t flag3 : 2; // 10
uint32_t pos_hi : 16;
uint32_t flag4 : 2; // 10
uint32_t flag4 : 2; // 10
uint32_t turns : 16;
uint32_t flag5 : 2; // 10
uint32_t flag5 : 2; // 10
uint32_t com_pos : 10;
uint32_t flag6 : 7; // 0000011
uint32_t flag6 : 7; // 0000011
uint32_t crc : 5;
uint32_t flag7 : 3; // 000
uint32_t flag7 : 3; // 000
} fanuc_t;
#pragma pack(pop)
/*
@@ -97,7 +97,7 @@ uint32_t state_counter;
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct encf_ctx_t *ctx = (struct encf_ctx_t *)ctx_ptr;
struct encf_pin_ctx_t * pins = (struct encf_pin_ctx_t *)pin_ptr;
struct encf_pin_ctx_t *pins = (struct encf_pin_ctx_t *)pin_ptr;
GPIO_InitTypeDef GPIO_InitStruct;
//TX enable
@@ -142,25 +142,25 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
//timer setup
RCC_APB1PeriphClockCmd(FB0_ENC_TIM_RCC, ENABLE);
FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN;
FB0_ENC_TIM->CCMR1 = TIM_CCMR1_CC1S_0; // cc1 input ti1
FB0_ENC_TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC1NP; // cc1 en, rising edge, falling edge
FB0_ENC_TIM->ARR = 65535;
FB0_ENC_TIM->DIER = TIM_DIER_CC1DE; // enable cc1 dma reeuest
FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN;
FB0_ENC_TIM->CCMR1 = TIM_CCMR1_CC1S_0; // cc1 input ti1
FB0_ENC_TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC1NP; // cc1 en, rising edge, falling edge
FB0_ENC_TIM->ARR = 65535;
FB0_ENC_TIM->DIER = TIM_DIER_CC1DE; // enable cc1 dma reeuest
//SPI is used to generate request
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3, ENABLE);
SPI_InitTypeDef SPI_InitTypeDefStruct;
SPI_InitTypeDefStruct.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
SPI_InitTypeDefStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitTypeDefStruct.SPI_Mode = SPI_Mode_Master;
SPI_InitTypeDefStruct.SPI_DataSize = SPI_DataSize_16b;
SPI_InitTypeDefStruct.SPI_NSS = SPI_NSS_Soft;
SPI_InitTypeDefStruct.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitTypeDefStruct.SPI_CPOL = SPI_CPOL_High;
SPI_InitTypeDefStruct.SPI_CPHA = SPI_CPHA_2Edge;
SPI_InitTypeDefStruct.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitTypeDefStruct.SPI_Mode = SPI_Mode_Master;
SPI_InitTypeDefStruct.SPI_DataSize = SPI_DataSize_16b;
SPI_InitTypeDefStruct.SPI_NSS = SPI_NSS_Soft;
SPI_InitTypeDefStruct.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitTypeDefStruct.SPI_CPOL = SPI_CPOL_High;
SPI_InitTypeDefStruct.SPI_CPHA = SPI_CPHA_2Edge;
SPI_Init(SPI3, &SPI_InitTypeDefStruct);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_SPI3);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
@@ -172,8 +172,8 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
GPIO_SetBits(GPIOD, GPIO_Pin_15); //tx enable
pos_offset = 0;
PIN(req_len) = 2046;
pos_offset = 0;
PIN(req_len) = 2046;
state_counter = 0;
}
@@ -182,98 +182,97 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
struct encf_pin_ctx_t *pins = (struct encf_pin_ctx_t *)pin_ptr;
uint32_t count = ARRAY_SIZE(tim_data) - DMA1_Stream0->NDTR;
PIN(dma) = count;
PIN(dma) = count;
for(int i = 0; i < 10; i++){
for(int i = 0; i < 10; i++) {
data.enc_data[i] = 0;
}
uint8_t bits_sum = 0;
for(int i = 1; i < count; i++){//each capture form dma
for(int i = 1; i < count; i++) { //each capture form dma
//1 bit = 82 ticks (1.024e-6)/(1/82e6)
//calculate time between edges
uint16_t diff = tim_data[i] - tim_data[i-1];
uint16_t diff = tim_data[i] - tim_data[i - 1];
//number of bits to set
int bits = (float)diff/(float)81;
if(i%2 == 0){//line starts high, set every even numbered captures to 1
for(int j = bits_sum; j < bits + bits_sum; j++){
data.enc_data[j/8] |= (1 << j%8);
int bits = (float)diff / (float)81;
if(i % 2 == 0) { //line starts high, set every even numbered captures to 1
for(int j = bits_sum; j < bits + bits_sum; j++) {
data.enc_data[j / 8] |= (1 << j % 8);
}
}
bits_sum += bits;
}
//set remaining bits to 1
for(int j = bits_sum; j < 77; j++){
data.enc_data[j/8] |= (1 << j%8);
for(int j = bits_sum; j < 77; j++) {
data.enc_data[j / 8] |= (1 << j % 8);
}
if(!sendf){
if(!sendf) {
memcpy((void *)print_buf, (void *)data.enc_data, 10);
sendf = 1;
}
if(bits_sum > 50){
if(bits_sum > 50) {
//check crc. TODO: use result, change to word/byte algorithm
//http://freeby.mesanet.com/fabsread.pas
uint8_t crc[5] = {0,0,0,0,0};
uint8_t oldcrc[5] = {0,0,0,0,0};
for(uint8_t i = 76; i >= 1; i--){
uint8_t bit = (data.enc_data[i/8] & (1 << i%8))?1:0;
crc[0] = oldcrc[4] ^ bit;
crc[1] = oldcrc[0];
crc[2] = oldcrc[1] ^ bit ^ oldcrc[4];
crc[3] = oldcrc[2];
crc[4] = oldcrc[3] ^ bit ^ oldcrc[4];
oldcrc[0] = crc[0];
oldcrc[1] = crc[1];
oldcrc[2] = crc[2];
oldcrc[3] = crc[3];
oldcrc[4] = crc[4];
uint8_t crc[5] = {0, 0, 0, 0, 0};
uint8_t oldcrc[5] = {0, 0, 0, 0, 0};
for(uint8_t i = 76; i >= 1; i--) {
uint8_t bit = (data.enc_data[i / 8] & (1 << i % 8)) ? 1 : 0;
crc[0] = oldcrc[4] ^ bit;
crc[1] = oldcrc[0];
crc[2] = oldcrc[1] ^ bit ^ oldcrc[4];
crc[3] = oldcrc[2];
crc[4] = oldcrc[3] ^ bit ^ oldcrc[4];
oldcrc[0] = crc[0];
oldcrc[1] = crc[1];
oldcrc[2] = crc[2];
oldcrc[3] = crc[3];
oldcrc[4] = crc[4];
}
if(crc[0] == 0 && crc[1] == 0 && crc[2] == 0 && crc[3] == 0 && crc[4] == 0){
PIN(crc_ok)++;
}else{
PIN(crc_er)++;
if(crc[0] == 0 && crc[1] == 0 && crc[2] == 0 && crc[3] == 0 && crc[4] == 0) {
PIN(crc_ok)
++;
} else {
PIN(crc_er)
++;
}
int32_t pos;
pos = data.fanuc.pos_lo + (data.fanuc.pos_hi << 6);
pos = data.fanuc.pos_lo + (data.fanuc.pos_hi << 6);
PIN(index) = data.fanuc.no_index;
PIN(abs_pos) = mod((float)pos * 2.0 * M_PI / (1<<22));
PIN(abs_pos) = mod((float)pos * 2.0 * M_PI / (1 << 22));
if(PIN(index) > 0.0){
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
PIN(state) = 1;
if(PIN(index) > 0.0) {
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
PIN(state) = 1;
state_counter = 1;
}
else if(state_counter == 1){
} else if(state_counter == 1) {
state_counter = 2;
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
}
else{
pos_offset = pos;
PIN(pos) = PIN(abs_pos);
} else {
state_counter = 3;
PIN(pos) = mod((float)(pos + pos_offset) * 2.0 * M_PI / (1<<22));
PIN(state) = 3;
PIN(pos) = mod((float)(pos + pos_offset) * 2.0 * M_PI / (1 << 22));
PIN(state) = 3;
}
PIN(turns) = data.fanuc.turns;
pos = data.fanuc.com_pos;
PIN(turns) = data.fanuc.turns;
pos = data.fanuc.com_pos;
PIN(com_pos) = mod(pos * 2.0 * M_PI / 1024);
PIN(error) = 0;
}else{
PIN(error) = 1;
PIN(state) = 1;
PIN(error) = 0;
} else {
PIN(error) = 1;
PIN(state) = 1;
state_counter = 0;
}
//reset timer
FB0_ENC_TIM->CNT = 0;
FB0_ENC_TIM->CNT = 0;
FB0_ENC_TIM->CCR1 = 0;
FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
//send request, 1/(42e6/32)*11 = 8.4uS
SPI3->DR = PIN(req_len);
@@ -285,14 +284,14 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct encf_ctx_t *ctx = (struct encf_ctx_t *)ctx_ptr;
struct encf_pin_ctx_t * pins = (struct encf_pin_ctx_t *)pin_ptr;
struct encf_pin_ctx_t *pins = (struct encf_pin_ctx_t *)pin_ptr;
if(sendf == 1 && send_counterf++ >= PIN(send_step) - 1 && PIN(send_step) >= 50) {
send_counterf = 0;
for(int i = 1; i < 77; i++){
if(print_buf[i/8] & (1 << i%8)){
for(int i = 1; i < 77; i++) {
if(print_buf[i / 8] & (1 << i % 8)) {
printf("1");
}else{
} else {
printf("0");
}
}
src/comps/encm.c
+1 -1
View File
@@ -115,7 +115,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(offset >= 0 && ctx->rxbuf[0 + offset] == 0x32 && ((ctx->rxbuf[0 + offset] ^ ctx->rxbuf[1 + offset] ^ ctx->rxbuf[2 + offset] ^ ctx->rxbuf[3 + offset] ^ ctx->rxbuf[4 + offset] ^ ctx->rxbuf[5 + offset] ^ ctx->rxbuf[6 + offset] ^ ctx->rxbuf[7 + offset] ^ ctx->rxbuf[8 + offset]) == 0)) {
uint32_t tpos = ((ctx->rxbuf[2 + offset] & 0x80) >> 7) + ctx->rxbuf[3 + offset] * 2 + ctx->rxbuf[4 + offset] * 512; // 17 bit single turn position
PIN(pos) = (tpos * M_PI * 2.0 / 131072.0) - M_PI; // convert to +-PI
PIN(pos) = (tpos * M_PI * 2.0 / 131072.0) - M_PI; // convert to +-PI
PIN(error) = 0.0;
PIN(state) = 3.0;
} else {
src/comps/encs.c
+119 -118
View File
@@ -30,12 +30,12 @@ word C:
//TODO: error,ready pin, af mapping
#define NUM_OF_SAMPLES_S 400
#define NUM_OF_SAMPLES_S 400
#define TIM_FREQ 84.0
#define TIM_DIV 5.0
#define SIG_FREQ 2.5
#define OVER (TIM_FREQ / TIM_DIV / SIG_FREQ)//one bit
#define OFFSET (OVER / 2.0)//half bit
#define OVER (TIM_FREQ / TIM_DIV / SIG_FREQ) //one bit
#define OFFSET (OVER / 2.0) //half bit
#define SIG_POS_START 16
HAL_COMP(encs);
@@ -65,16 +65,16 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(FB0_Z_TXEN_PORT, &GPIO_InitStruct);
GPIO_ResetBits(FB0_Z_TXEN_PORT,FB0_Z_TXEN_PIN);
GPIO_ResetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);
//TX
GPIO_InitStruct.GPIO_Pin = FB0_Z_PIN;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_Pin = FB0_Z_PIN;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL ;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(FB0_Z_PORT, &GPIO_InitStruct);
GPIO_ResetBits(FB0_Z_PORT,FB0_Z_PIN);
GPIO_ResetBits(FB0_Z_PORT, FB0_Z_PIN);
//RX
//v3 only
@@ -86,89 +86,89 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
//TIM8 triggers DMA to sample reply
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = 32;//14MHz
TIM_TimeBaseStructure.TIM_Prescaler = 1;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = 32; //14MHz
TIM_TimeBaseStructure.TIM_Prescaler = 1;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
TIM_ARRPreloadConfig(TIM8,ENABLE);
TIM_ARRPreloadConfig(TIM8, ENABLE);
TIM_DMACmd(TIM8, TIM_DMA_Update, ENABLE);
TIM_Cmd(TIM8, ENABLE);
//TODO: use defines...
uint32_t tx_high = GPIO_BSRR_BS_14;
uint32_t tx_low = GPIO_BSRR_BR_14;
uint32_t txen_high = GPIO_BSRR_BS_15;
uint32_t txen_low = GPIO_BSRR_BR_15;
int pos = 0;
int pos = 0;
request_buf[pos++] = tx_high | txen_high;
request_buf[pos++] = tx_high;
request_buf[pos++] = tx_high;
request_buf[pos++] = tx_high;
request_buf[pos++] = tx_low;//start bit
request_buf[pos++] = tx_low; //sync
request_buf[pos++] = tx_high;//sync
request_buf[pos++] = tx_low; //sync
request_buf[pos++] = tx_low; //frame
request_buf[pos++] = tx_low; //frame
request_buf[pos++] = tx_low; //start bit
request_buf[pos++] = tx_low; //sync
request_buf[pos++] = tx_high; //sync
request_buf[pos++] = tx_low; //sync
request_buf[pos++] = tx_low; //frame
request_buf[pos++] = tx_low; //frame
//encoder address
request_buf[pos++] = tx_low; //EA
request_buf[pos++] = tx_low; //EA
request_buf[pos++] = tx_low; //EA
request_buf[pos++] = tx_low; //EA
request_buf[pos++] = tx_low; //EA
request_buf[pos++] = tx_low; //EA
//command code = CDF1 = Absolute lower 24-bit data request
request_buf[pos++] = tx_high;//CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_high; //CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_low; //CC
request_buf[pos++] = tx_low; //CC
//crc of 10 bit, frame code to command code
request_buf[pos++] = tx_low; //crc
request_buf[pos++] = tx_low; //crc
request_buf[pos++] = tx_high;//crc
request_buf[pos++] = tx_high;//stop bit
request_buf[pos++] = tx_low; //crc
request_buf[pos++] = tx_low; //crc
request_buf[pos++] = tx_high; //crc
request_buf[pos++] = tx_high; //stop bit
request_buf[pos++] = tx_high;
request_buf[pos++] = tx_high | txen_low;
//DMA tx config
dma_tx_config.DMA_Channel = DMA_Channel_7; //TIM8_UP
dma_tx_config.DMA_PeripheralBaseAddr = (uint32_t)&FB0_Z_PORT->BSRRL;
dma_tx_config.DMA_Memory0BaseAddr = (uint32_t)&request_buf;
dma_tx_config.DMA_DIR = DMA_DIR_MemoryToPeripheral;
dma_tx_config.DMA_BufferSize = pos;
dma_tx_config.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dma_tx_config.DMA_MemoryInc = DMA_MemoryInc_Enable;
dma_tx_config.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
dma_tx_config.DMA_MemoryDataSize = DMA_PeripheralDataSize_Word;
dma_tx_config.DMA_Mode = DMA_Mode_Normal;
dma_tx_config.DMA_Priority = DMA_Priority_VeryHigh;
dma_tx_config.DMA_FIFOMode = DMA_FIFOMode_Disable;
dma_tx_config.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
dma_tx_config.DMA_MemoryBurst = DMA_MemoryBurst_Single;
dma_tx_config.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
//DMA rx config
dma_rx_config.DMA_Channel = DMA_Channel_7; //TIM8_UP
dma_rx_config.DMA_PeripheralBaseAddr = (uint32_t)&FB0_Z_PORT->IDR;
dma_rx_config.DMA_Memory0BaseAddr = (uint32_t)&reply_buf;
dma_rx_config.DMA_DIR = DMA_DIR_PeripheralToMemory;
dma_rx_config.DMA_BufferSize = NUM_OF_SAMPLES_S;
dma_rx_config.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dma_rx_config.DMA_MemoryInc = DMA_MemoryInc_Enable;
dma_rx_config.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
dma_rx_config.DMA_MemoryDataSize = DMA_PeripheralDataSize_Word;
dma_rx_config.DMA_Mode = DMA_Mode_Normal;
dma_rx_config.DMA_Priority = DMA_Priority_High;
dma_rx_config.DMA_FIFOMode = DMA_FIFOMode_Disable;
dma_rx_config.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
dma_rx_config.DMA_MemoryBurst = DMA_MemoryBurst_Single;
dma_rx_config.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
PIN(en) = 1.0;
//DMA tx config
dma_tx_config.DMA_Channel = DMA_Channel_7; //TIM8_UP
dma_tx_config.DMA_PeripheralBaseAddr = (uint32_t)&FB0_Z_PORT->BSRRL;
dma_tx_config.DMA_Memory0BaseAddr = (uint32_t)&request_buf;
dma_tx_config.DMA_DIR = DMA_DIR_MemoryToPeripheral;
dma_tx_config.DMA_BufferSize = pos;
dma_tx_config.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dma_tx_config.DMA_MemoryInc = DMA_MemoryInc_Enable;
dma_tx_config.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
dma_tx_config.DMA_MemoryDataSize = DMA_PeripheralDataSize_Word;
dma_tx_config.DMA_Mode = DMA_Mode_Normal;
dma_tx_config.DMA_Priority = DMA_Priority_VeryHigh;
dma_tx_config.DMA_FIFOMode = DMA_FIFOMode_Disable;
dma_tx_config.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
dma_tx_config.DMA_MemoryBurst = DMA_MemoryBurst_Single;
dma_tx_config.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
//DMA rx config
dma_rx_config.DMA_Channel = DMA_Channel_7; //TIM8_UP
dma_rx_config.DMA_PeripheralBaseAddr = (uint32_t)&FB0_Z_PORT->IDR;
dma_rx_config.DMA_Memory0BaseAddr = (uint32_t)&reply_buf;
dma_rx_config.DMA_DIR = DMA_DIR_PeripheralToMemory;
dma_rx_config.DMA_BufferSize = NUM_OF_SAMPLES_S;
dma_rx_config.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
dma_rx_config.DMA_MemoryInc = DMA_MemoryInc_Enable;
dma_rx_config.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
dma_rx_config.DMA_MemoryDataSize = DMA_PeripheralDataSize_Word;
dma_rx_config.DMA_Mode = DMA_Mode_Normal;
dma_rx_config.DMA_Priority = DMA_Priority_High;
dma_rx_config.DMA_FIFOMode = DMA_FIFOMode_Disable;
dma_rx_config.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
dma_rx_config.DMA_MemoryBurst = DMA_MemoryBurst_Single;
dma_rx_config.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
PIN(en) = 1.0;
PIN(offset) = 120.0;
}
@@ -176,58 +176,59 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
// struct encs_ctx_t *ctx = (struct encs_ctx_t *)ctx_ptr;
struct encs_pin_ctx_t *pins = (struct encs_pin_ctx_t *)pin_ptr;
if(PIN(en) > 0.0){
//request
TIM8->ARR = 32;//2.545 Mhz
DMA_Cmd(DMA2_Stream1, DISABLE);
DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
DMA_DeInit(DMA2_Stream1);
DMA_Init(DMA2_Stream1, &dma_tx_config);
DMA_Cmd(DMA2_Stream1, ENABLE);
//wait for DMA transfer complete
while (DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET);
//TODO: set pin to input
//reply
TIM8->ARR = 4;//16.8 Mhz
DMA_Cmd(DMA2_Stream1, DISABLE);
DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
DMA_DeInit(DMA2_Stream1);
DMA_Init(DMA2_Stream1, &dma_rx_config);
DMA_Cmd(DMA2_Stream1, ENABLE);
//wait for DMA transfer complete
while (DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET);
if(PIN(en) > 0.0) {
//request
TIM8->ARR = 32; //2.545 Mhz
DMA_Cmd(DMA2_Stream1, DISABLE);
DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
DMA_DeInit(DMA2_Stream1);
DMA_Init(DMA2_Stream1, &dma_tx_config);
DMA_Cmd(DMA2_Stream1, ENABLE);
//wait for DMA transfer complete
while(DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET)
;
//TODO: set pin to input
//reply
TIM8->ARR = 4; //16.8 Mhz
DMA_Cmd(DMA2_Stream1, DISABLE);
DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1);
DMA_DeInit(DMA2_Stream1);
DMA_Init(DMA2_Stream1, &dma_rx_config);
DMA_Cmd(DMA2_Stream1, ENABLE);
//wait for DMA transfer complete
while(DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET)
;
int i = 0;
//skip leading ones
for(; i < NUM_OF_SAMPLES_S/5; i++){
if(!(reply_buf[i] & FB0_Z_PIN)){
break;
}
int i = 0;
//skip leading ones
for(; i < NUM_OF_SAMPLES_S / 5; i++) {
if(!(reply_buf[i] & FB0_Z_PIN)) {
break;
}
//skip zeros
for(; i < NUM_OF_SAMPLES_S/5; i++){
if(reply_buf[i] & FB0_Z_PIN){
break;
}
}
//skip zeros
for(; i < NUM_OF_SAMPLES_S / 5; i++) {
if(reply_buf[i] & FB0_Z_PIN) {
break;
}
int start = i + OFFSET;//position of first bit
int d = 0;
int p = 0;
}
int start = i + OFFSET; //position of first bit
int d = 0;
int p = 0;
p = CLAMP((int)((18 + SIG_POS_START) * OVER + start + 0.5), 0, NUM_OF_SAMPLES_S - 1);
p = CLAMP((int)((18 + SIG_POS_START) * OVER + start + 0.5), 0, NUM_OF_SAMPLES_S - 1);
d += (reply_buf[p] & FB0_Z_PIN) != 0;
for(int j = 0; j < 16; j++) {
p = CLAMP((int)(((15 - j) + SIG_POS_START) * OVER + start + 0.5), 0, NUM_OF_SAMPLES_S - 1);
d = d << 1;
d += (reply_buf[p] & FB0_Z_PIN) != 0;
}
for(int j = 0; j < 16; j++){
p = CLAMP((int)(((15 - j) + SIG_POS_START) * OVER + start + 0.5), 0, NUM_OF_SAMPLES_S - 1);
d = d << 1;
d += (reply_buf[p] & FB0_Z_PIN) != 0;
}
PIN(d) = d;
PIN(start) = start;
PIN(pos) = (d * M_PI * 2.0 / 131072.0) - M_PI;
}
PIN(d) = d;
PIN(start) = start;
PIN(pos) = (d * M_PI * 2.0 / 131072.0) - M_PI;
}
}
hal_comp_t encs_comp_struct = {
src/comps/hv.c
+108 -116
View File
@@ -42,7 +42,7 @@ HAL_PIN(abs_cur);
HAL_PIN(hv_temp);
HAL_PIN(mot_temp);
HAL_PIN(core_temp);
HAL_PIN(fault);//fault from hv
HAL_PIN(fault); //fault from hv
HAL_PIN(y);
HAL_PIN(u_fb);
HAL_PIN(v_fb);
@@ -53,18 +53,18 @@ HAL_PIN(rev);
HAL_PIN(pwm_volt);
HAL_PIN(uart_sr);
HAL_PIN(uart_dr);
HAL_PIN(crc_error);//total number of crc errors, never reset
HAL_PIN(crc_error); //total number of crc errors, never reset
HAL_PIN(scale);
HAL_PIN(state);
HAL_PIN(value);
struct hv_ctx_t {
union{
union {
volatile packet_to_hv_t packet_to_hv;
volatile packet_bootloader_t packet_to_hv_bootloader;
} to_hv;
union{
union {
volatile packet_from_hv_t packet_from_hv;
volatile packet_bootloader_t packet_from_hv_bootloader;
} from_hv;
@@ -98,7 +98,7 @@ struct ringbuf hv_rx_buf = RINGBUF(128);
struct ringbuf hv_tx_buf = RINGBUF(128);
void hv_send(char *ptr) {
if(ptr){//TODO: check connection status
if(ptr) { //TODO: check connection status
rb_write(&hv_tx_buf, ptr, strlen(ptr));
rb_write(&hv_tx_buf, "\n", 1);
}
@@ -198,22 +198,22 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
DMA_Cmd(UART_DRV_RX_DMA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_CRC, ENABLE);
ctx->timeout = 0;
PIN(dac) = 1560;
ctx->timeout = 0;
PIN(dac) = 1560;
send_to_bootloader = 0;
flash_state = SLAVE_IN_APP;
flash_state = SLAVE_IN_APP;
}
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;
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){
if(ctx->frt_slot == 3) {
float e = PIN(en);
float pos = PIN(pos);
float vel = PIN(vel);
@@ -230,25 +230,25 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
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);
PIN(value) = 0.0;
if(dma_count >= sizeof(stmbl_talk_header_t)){
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){
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){
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){
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(d_fb) = ctx->from_hv.packet_from_hv.d_fb;
PIN(q_fb) = ctx->from_hv.packet_from_hv.q_fb;
PIN(fault) = ctx->from_hv.packet_from_hv.fault;
PIN(abs_cur) = sqrtf(PIN(d_fb) * PIN(d_fb) + PIN(q_fb) * PIN(q_fb));
PIN(d_fb) = ctx->from_hv.packet_from_hv.d_fb;
PIN(q_fb) = ctx->from_hv.packet_from_hv.q_fb;
PIN(fault) = ctx->from_hv.packet_from_hv.fault;
PIN(abs_cur) = sqrtf(PIN(d_fb) * PIN(d_fb) + PIN(q_fb) * PIN(q_fb));
uint16_t a = ctx->from_hv.packet_from_hv.header.conf_addr;
a = CLAMP(a, 0, sizeof(f3_state_data_t) / 4);
@@ -268,73 +268,68 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
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);
if(ctx->from_hv.packet_from_hv.buf != 0x0) {
rb_write(&hv_rx_buf, (void *)&(ctx->from_hv.packet_from_hv.buf), 1);
}
}
else{
} else {
// wrong packet len or slave addr
}
break;
break;
case SEND_TO_BOOTLOADER:
break;
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){
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.header.flags.error == 0 && */ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_PAGEERASE){
if(/*ctx->from_hv.packet_from_hv_bootloader.header.flags.error == 0 && */ ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_PAGEERASE) {
ctx->timeout = 0;
flash_state = SEND_APP;
flash_state = SEND_APP;
}
}
else{
} else {
// wrong packet len or slave addr
PIN(value) = 3.0;
}
break;
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){
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.header.flags.error == 0 && */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]){
if(/*ctx->from_hv.packet_from_hv_bootloader.header.flags.error == 0 && */ 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){
if(ctx->addr > ((uint32_t) & (_binary_obj_hvf3_hvf3_bin_size)) / 4) {
flash_state = CRC_CHECK;
}
}
else{
} else {
// wrong packet len or slave addr
PIN(value) = 3.0;
}
break;
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){
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.header.flags.error == 0 && */ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_CRCCHECK){
if(/*ctx->from_hv.packet_from_hv_bootloader.header.flags.error == 0 && */ ctx->from_hv.packet_from_hv_bootloader.cmd == BOOTLOADER_OPCODE_CRCCHECK) {
ctx->timeout = 0;
flash_state = SEND_TO_APP;
flash_state = SEND_TO_APP;
}
}
else{
} else {
// wrong packet len or slave addr
PIN(value) = 3.0;
}
break;
break;
case SEND_TO_APP:
break;
break;
case FLASH_FAILED:
break;
break;
}
}
else{
} else {
// CRC fault
PIN(crc_error)++;
PIN(crc_error)
++;
// PIN(fault) = HV_CRC_ERROR;
// PIN(value) = 4.0;
}
@@ -357,7 +352,7 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
uint32_t tx_size = 0;
switch(flash_state){
switch(flash_state) {
case SLAVE_IN_APP:
if(e > 0.0) {
ctx->to_hv.packet_to_hv.d_cmd = d_cmd;
@@ -374,16 +369,16 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
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.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.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)){
if(rb_read(&hv_tx_buf, buf, 1)) {
ctx->to_hv.packet_to_hv.flags.buf = buf[0];
}else{
} else {
ctx->to_hv.packet_to_hv.flags.buf = 0x0;
}
@@ -391,17 +386,17 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
ctx->conf_addr %= sizeof(f3_config_data_t) / 4;
if(send_to_bootloader){
if(send_to_bootloader) {
send_to_bootloader = 0;
flash_state = SEND_TO_BOOTLOADER;
ctx->timeout = 0;
flash_state = SEND_TO_BOOTLOADER;
ctx->timeout = 0;
// TODO: check f3 crc, size, ...
}
break;
break;
case SEND_TO_BOOTLOADER: // fix
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.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;
@@ -409,22 +404,22 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
tx_size = sizeof(packet_to_hv_t);
if(ctx->timeout > 10){
if(ctx->timeout > 10) {
ctx->timeout = 0;
flash_state = ERASE_FLASH;
flash_state = ERASE_FLASH;
}
break;
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.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.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;
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);
@@ -432,62 +427,61 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
// flash_state = SLAVE_IN_APP;
if(ctx->timeout > 20000){
if(ctx->timeout > 20000) {
ctx->timeout = 0;
flash_state = FLASH_FAILED;
flash_state = FLASH_FAILED;
}
break;
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.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;
ctx->to_hv.packet_to_hv_bootloader.cmd = BOOTLOADER_OPCODE_WRITE;
tx_size = sizeof(packet_bootloader_t);
if(ctx->timeout > 1000){
if(ctx->timeout > 1000) {
ctx->timeout = 0;
flash_state = FLASH_FAILED;
flash_state = FLASH_FAILED;
}
break;
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.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 > 1000){
if(ctx->timeout > 1000) {
ctx->timeout = 0;
flash_state = FLASH_FAILED;
flash_state = FLASH_FAILED;
}
break;
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.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 > 1000){
if(ctx->timeout > 1000) {
ctx->timeout = 0;
flash_state = SLAVE_IN_APP;
flash_state = SLAVE_IN_APP;
}
break;
break;
case FLASH_FAILED:
if(ctx->timeout > 10){
if(ctx->timeout > 10) {
ctx->timeout = 0;
flash_state = SLAVE_IN_APP;
flash_state = SLAVE_IN_APP;
}
break;
break;
}
if(tx_size){
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);
@@ -511,58 +505,56 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
PIN(state) = flash_state;
}
void send_boot(char *ptr){
void send_boot(char *ptr) {
send_to_bootloader = 1;
}
COMMAND("hv_update", send_boot, "try hv update");
static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
struct hv_ctx_t *ctx = (struct hv_ctx_t *)ctx_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;
char c;
while(rb_getc(&hv_rx_buf, &c)){
printf("%c",c);
while(rb_getc(&hv_rx_buf, &c)) {
printf("%c", c);
}
static flash_state_t last_flash_state = SLAVE_IN_APP;
static uint32_t last_addr = 0;
if(last_flash_state != flash_state){
switch(flash_state){
static uint32_t last_addr = 0;
if(last_flash_state != flash_state) {
switch(flash_state) {
case SLAVE_IN_APP:
printf("hv_update: SLAVE_IN_APP\n");
break;
break;
case SEND_TO_BOOTLOADER:
printf("hv_update: SEND_TO_BOOTLOADER\n");
last_addr = 0;
break;
break;
case ERASE_FLASH:
printf("hv_update: ERASE_FLASH\n");
last_addr = 0;
break;
break;
case SEND_APP:
printf("hv_update: SEND_APP\n");
break;
break;
case CRC_CHECK:
printf("hv_update: CRC_CHECK\n");
last_addr = 0;
break;
break;
case SEND_TO_APP:
printf("hv_update: SEND_TO_APP\n");
last_addr = 0;
break;
break;
case FLASH_FAILED:
printf("hv_update: FLASH_FAILED\n");
last_addr = 0;
break;
break;
}
last_flash_state = flash_state;
}
if(ctx->addr >= last_addr + 1024){
printf("hv_update: status: %i%%\n", (int)(100.0 * ctx->addr * 4. / (float)((uint32_t)&(_binary_obj_hvf3_hvf3_bin_size))));
if(ctx->addr >= last_addr + 1024) {
printf("hv_update: status: %i%%\n", (int)(100.0 * ctx->addr * 4. / (float)((uint32_t) & (_binary_obj_hvf3_hvf3_bin_size))));
last_addr = ctx->addr;
}
}
src/comps/hvf1.c
+2 -2
View File
@@ -150,9 +150,9 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
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_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(hv_fault) = ctx->packet_from_hv.data.hv_fault; //iramx fault
PIN(error) = 0.0; //TODO: link to fault
} else {
src/comps/hw/io4.c
+10 -10
View File
@@ -20,14 +20,14 @@ HAL_PIN(out0);
HAL_PIN(out1);
HAL_PIN(out2);
HAL_PIN(in0); //input 0, analog
HAL_PIN(in1); //input 1, analog
HAL_PIN(ind0); //input 0, digital
HAL_PIN(ind1); //input 1, digital
HAL_PIN(in0); //input 0, analog
HAL_PIN(in1); //input 1, analog
HAL_PIN(ind0); //input 0, digital
HAL_PIN(ind1); //input 1, digital
HAL_PIN(ind0n); //input 0 inverted
HAL_PIN(ind1n); //input 0 inverted
HAL_PIN(th0); //voltage threshold in0
HAL_PIN(th1); //voltage threshold in1
HAL_PIN(th0); //voltage threshold in0
HAL_PIN(th1); //voltage threshold in1
HAL_PIN(CTX);
HAL_PIN(CRX);
@@ -110,11 +110,11 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC3, ENABLE);
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 = 1; //ADC_ANZ;//I think this one is clear :p
ADC_InitStructure.ADC_NbrOfConversion = 1; //ADC_ANZ;//I think this one is clear :p
ADC_InitStructure.ADC_ScanConvMode = ENABLE; //The scan is configured in one channel
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_Init(ADC3, &ADC_InitStructure);
src/comps/res.c
+7 -7
View File
@@ -22,8 +22,8 @@ HAL_PIN(cos);
HAL_PIN(enable);
HAL_PIN(error);
HAL_PIN(state);
HAL_PIN(phase);//phase adjust
HAL_PIN(res_mode);//resolver mode output, calculated form frequency
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
@@ -37,12 +37,12 @@ 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(phase) = 0.85;
PIN(phase) = 0.85;
PIN(min_amp) = 0.15;
PIN(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;
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;
@@ -117,8 +117,8 @@ 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;
uint32_t mult = CLAMP(PIN(freq) / RT_FREQ + 0.5, 1, 4);
PIN(freq) = RT_FREQ * mult;
FB0_RES_REF_TIM->ARR = ADC_TRIGGER_FREQ / 2 / (RT_FREQ * mult) - 1;
FB0_RES_REF_TIM->CCR3 = (int)CLAMP(PIN(phase) * FB0_RES_REF_TIM->ARR, 0, FB0_RES_REF_TIM->ARR - 1);
PIN(res_mode) = ADC_GROUPS / 2 / mult;
src/comps/sserial.c
+15 -15
View File
@@ -36,7 +36,7 @@ HAL_COMP(sserial);
HAL_PIN(error);
HAL_PIN(crc_error); //counts crc errors, is never reset
HAL_PIN(connected); //connection status TODO: not stable during startup, needs link to pd
HAL_PIN(timeout); // 20khz / 1khz * 2 reads = 40
HAL_PIN(timeout); // 20khz / 1khz * 2 reads = 40
HAL_PIN(pos_cmd);
HAL_PIN(pos_cmd_d);
@@ -65,10 +65,10 @@ struct sserial_ctx_t {
uint32_t foo;
};
volatile uint8_t rxbuf[128];//rx dma buffer
volatile uint8_t txbuf[128];//tx dma buffer
uint16_t address; //current address pointer
int rxpos;//read pointer for rx ringbuffer
volatile uint8_t rxbuf[128]; //rx dma buffer
volatile uint8_t txbuf[128]; //tx dma buffer
uint16_t address; //current address pointer
int rxpos; //read pointer for rx ringbuffer
uint32_t timeout;
lbp_t lbp;
const char name[] = LBPCardName;
@@ -152,7 +152,7 @@ typedef struct {
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 {
@@ -166,7 +166,7 @@ typedef struct {
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:0x12c size:32 dir:0x80
#define scale_address 300
@@ -403,14 +403,14 @@ static void frt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst
} 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;
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;
//copy output pins from rx buffer
for(int i = 0; i < discovery.output; i++) {
src/comps/yaskawa.c
+68 -73
View File
@@ -48,8 +48,8 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct yaskawa_ctx_t *ctx = (struct yaskawa_ctx_t *)ctx_ptr;
struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
PIN(len) = 15;
PIN(off) = 64;
PIN(len) = 15;
PIN(off) = 64;
PIN(len3) = 57;
PIN(len4) = 58;
PIN(len5) = 59;
@@ -58,7 +58,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 yaskawa_ctx_t *ctx = (struct yaskawa_ctx_t *)ctx_ptr;
// struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
GPIO_InitTypeDef GPIO_InitStruct;
//TX enable
@@ -120,7 +120,7 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
txbuf[pos++] = tim_a;
txbuf[pos++] = tim_a;
DMA_InitStructuretx.DMA_Channel = DMA_Channel_7;
DMA_InitStructuretx.DMA_Channel = DMA_Channel_7;
DMA_InitStructuretx.DMA_PeripheralBaseAddr = (uint32_t)&FB0_Z_PORT->BSRRL; //TODO: change
DMA_InitStructuretx.DMA_Memory0BaseAddr = (uint32_t)&txbuf;
DMA_InitStructuretx.DMA_DIR = DMA_DIR_MemoryToPeripheral;
@@ -136,7 +136,7 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
DMA_InitStructuretx.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructuretx.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_InitStructurerx.DMA_Channel = DMA_Channel_2;
DMA_InitStructurerx.DMA_Channel = DMA_Channel_2;
DMA_InitStructurerx.DMA_PeripheralBaseAddr = (uint32_t)&FB0_ENC_TIM->CCR3; //TODO: change
DMA_InitStructurerx.DMA_Memory0BaseAddr = (uint32_t)&tim_data;
DMA_InitStructurerx.DMA_DIR = DMA_DIR_PeripheralToMemory;
@@ -164,11 +164,11 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
TIM_DMACmd(TIM8, TIM_DMA_Update, ENABLE);
TIM_Cmd(TIM8, ENABLE);
FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN;
FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN;
FB0_ENC_TIM->ARR = 65535;
FB0_ENC_TIM->CNT = 3300;
FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
DMA_Cmd(DMA1_Stream7, DISABLE);
DMA_DeInit(DMA1_Stream7);
DMA_Init(DMA1_Stream7, &DMA_InitStructurerx);
@@ -182,88 +182,82 @@ static void hw_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 yaskawa_ctx_t *ctx = (struct yaskawa_ctx_t *)ctx_ptr;
struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
while(FB0_ENC_TIM->CNT < 3300){
while(FB0_ENC_TIM->CNT < 3300) {
}
int count = ARRAY_SIZE(tim_data) - DMA1_Stream7->NDTR;
DMA_Cmd(DMA1_Stream7, DISABLE);
uint16_t bit_time = 15;
if(count > 80){
int pol = 0;
int read_counter = 0;
int counter = 0;
for(int i = 0;i < ARRAY_SIZE(yaskawa_reply);i++){
if(count > 80) {
int pol = 0;
int read_counter = 0;
int counter = 0;
for(int i = 0; i < ARRAY_SIZE(yaskawa_reply); i++) {
yaskawa_reply[i] = 0;
}
for(int i = 1; i < count; i++){
if(tim_data[i + 1] - tim_data[i] < bit_time){
for(int i = 1; i < count; i++) {
if(tim_data[i + 1] - tim_data[i] < bit_time) {
counter++;
}
else if(counter == 10){
} else if(counter == 10) {
read_counter = i + 1;
pol = 0;
pol = 0;
break;
}
else{
} else {
counter = 0;
}
}
int write_counter2 = 0;
for(int i = read_counter; i < count; i++){
if(tim_data[i + 1] - tim_data[i] < bit_time){
for(int i = read_counter; i < count; i++) {
if(tim_data[i + 1] - tim_data[i] < bit_time) {
i++;
if(tim_data[i + 1] - tim_data[i] < bit_time){
if(tim_data[i + 1] - tim_data[i] < bit_time) {
//data[write_counter++] = '0' + pol;
}
else{
} else {
//error
PIN(error) = 1.0;
break;
}
}
else{
} else {
pol = 1 - pol;
//data[write_counter++] = '0' + pol;
}
if(pol == 1){
if(pol == 1) {
counter++;
m_data[write_counter2] = '1';
yaskawa_reply[write_counter2/8] |= 1 << (7-(write_counter2%8));
yaskawa_reply[write_counter2 / 8] |= 1 << (7 - (write_counter2 % 8));
write_counter2++;
}
else if(counter == 5){
} else if(counter == 5) {
counter = 0;
// unstuff
}
else if(counter == 6){
} else if(counter == 6) {
// hldc
m_data[write_counter2++] = 'H';
break;
}
else{
counter = 0;
} else {
counter = 0;
m_data[write_counter2++] = '0';
}
}
yaskawa_crc16_t crc;
yaskawa_crc16_t data = (yaskawa_reply[13] & 0xff) | (yaskawa_reply[12] << 8);
crc = yaskawa_crc16_init();
crc = yaskawa_crc16_update(crc, yaskawa_reply, 12);
crc = yaskawa_crc16_finalize(crc);
if(data == crc){
PIN(crc_ok)++;
}else{
PIN(crc_error)++;
crc = yaskawa_crc16_init();
crc = yaskawa_crc16_update(crc, yaskawa_reply, 12);
crc = yaskawa_crc16_finalize(crc);
if(data == crc) {
PIN(crc_ok)
++;
} else {
PIN(crc_error)
++;
}
m_data[write_counter2] = 0;
@@ -278,26 +272,25 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
for(int i = 0; i < PIN(len2); i++) {
probe += (m_data[i + (int)PIN(off2)] == '1') << i;
}
PIN(pos) = (float)pos / (float)(1 << (int)PIN(len)) * M_PI * 2.0 - M_PI;
PIN(pos) = (float)pos / (float)(1 << (int)PIN(len)) * M_PI * 2.0 - M_PI;
PIN(probe2) = probe;
PIN(probe3) = m_data[(int)PIN(len3)] == '1';
PIN(probe4) = m_data[(int)PIN(len4)] == '1';
PIN(probe5) = m_data[(int)PIN(len5)] == '1';
PIN(error) = 0.0;
PIN(error) = 0.0;
if(dfdf < 1){
for(int i = 0; i < ARRAY_SIZE(m_data2); i++){
if(dfdf < 1) {
for(int i = 0; i < ARRAY_SIZE(m_data2); i++) {
m_data2[i] = m_data[i];
}
dfdf = 1;
}
}
else{
} else {
PIN(error) = 1.0;
// error
}
}
// PIN(send) = send;
@@ -311,36 +304,38 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF7);
DMA_Cmd(DMA2_Stream1, ENABLE); //transmit request
TIM8->CR1 &= ~TIM_CR1_CEN; // disable tim
TIM8->ARR = 20; // 168 / 2 / (9 + 1) = 8.4MHz
TIM8->DIER = TIM_DIER_UDE; // cc3 dma
TIM8->CR1 &= ~TIM_CR1_CEN; // disable tim
TIM8->ARR = 20; // 168 / 2 / (9 + 1) = 8.4MHz
TIM8->DIER = TIM_DIER_UDE; // cc3 dma
// TIM8->CCMR2 = 0; // cc3 output
// TIM8->CCR3 = 1;
TIM8->CNT = 0;
TIM8->CR1 |= TIM_CR1_CEN;
TIM8->CR1 |= TIM_CR1_CEN;
DMA_Cmd(DMA1_Stream7, DISABLE);
DMA_ClearFlag(DMA1_Stream7, DMA_FLAG_TCIF7);
DMA_Cmd(DMA1_Stream7, ENABLE);
FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN;
FB0_ENC_TIM->CCMR2 = TIM_CCMR2_CC3S_0; // cc3 input ti3
FB0_ENC_TIM->CCER = TIM_CCER_CC3E | TIM_CCER_CC3P | TIM_CCER_CC3NP; // cc3 en, rising edge, falling edge
FB0_ENC_TIM->ARR = 65535;
FB0_ENC_TIM->DIER = TIM_DIER_CC3DE; // cc3 dma
FB0_ENC_TIM->CNT = 0;
FB0_ENC_TIM->CCR3 = 0;
FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN;
FB0_ENC_TIM->CCMR2 = TIM_CCMR2_CC3S_0; // cc3 input ti3
FB0_ENC_TIM->CCER = TIM_CCER_CC3E | TIM_CCER_CC3P | TIM_CCER_CC3NP; // cc3 en, rising edge, falling edge
FB0_ENC_TIM->ARR = 65535;
FB0_ENC_TIM->DIER = TIM_DIER_CC3DE; // cc3 dma
FB0_ENC_TIM->CNT = 0;
FB0_ENC_TIM->CCR3 = 0;
while(!(DMA2->LISR & DMA_FLAG_TCIF1)); //wait for request
FB0_Z_TXEN_PORT->BSRRH = FB0_Z_TXEN_PIN; //TX disable
while(!(DMA2->LISR & DMA_FLAG_TCIF1))
; //wait for request
FB0_Z_TXEN_PORT->BSRRH = FB0_Z_TXEN_PIN; //TX disable
FB0_Z_PORT->MODER &= ~GPIO_MODER_MODER14_0; //set tx pin to af
FB0_Z_PORT->MODER |= GPIO_MODER_MODER14_1;
FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
if(PIN(error) > 0.0){
PIN(error_sum)++;
if(PIN(error) > 0.0) {
PIN(error_sum)
++;
}
}
@@ -348,8 +343,8 @@ static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// struct yaskawa_ctx_t *ctx = (struct yaskawa_ctx_t *)ctx_ptr;
struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
if(RISING_EDGE(PIN(dump))) {
for(int i = 0; i < 14; i++){
for(int j = 0; j < 8; j++){
for(int i = 0; i < 14; i++) {
for(int j = 0; j < 8; j++) {
printf("%c", m_data2[i * 8 + j]);
}
printf("|");
src/main.c
+26 -28
View File
@@ -80,8 +80,8 @@ void bootloader(char *ptr) {
RCC_DeInit();
SysTick->CTRL = 0;
SysTick->LOAD = 0;
SysTick->VAL = 0;
SysTick->VAL = 0;
RCC->AHB1RSTR = 0x22E017FF;
RCC->AHB1RSTR = 0;
RCC->AHB2RSTR = 0xF1;
@@ -94,7 +94,7 @@ void bootloader(char *ptr) {
RCC->APB2RSTR = 0;
SYSCFG->MEMRMP = 0x01;
SysMemBootJump = (void (*)(void)) (*((uint32_t *)(addr + 4)));
SysMemBootJump = (void (*)(void))(*((uint32_t *)(addr + 4)));
__set_MSP(*(uint32_t *)addr);
SysMemBootJump();
}
@@ -119,46 +119,44 @@ void about(char *ptr) {
printf("by %s on %s\n", version_info.build_user, version_info.build_host);
printf("GCC %s\n", __VERSION__);
printf("newlib %s\n", _NEWLIB_VERSION);
#ifdef __CM4_CMSIS_VERSION
printf("CMSIS %i.%i\n",__CM4_CMSIS_VERSION_MAIN,__CM4_CMSIS_VERSION_SUB);
#endif
#ifdef __STM32F4XX_STDPERIPH_VERSION
printf("StdPeriph %i.%i.%i\n",__STM32F4XX_STDPERIPH_VERSION_MAIN,__STM32F4XX_STDPERIPH_VERSION_SUB1,__STM32F4XX_STDPERIPH_VERSION_SUB2);
#endif
#ifdef __STM32F3xx_HAL_VERSION
#ifdef __CM4_CMSIS_VERSION
printf("CMSIS %i.%i\n", __CM4_CMSIS_VERSION_MAIN, __CM4_CMSIS_VERSION_SUB);
#endif
#ifdef __STM32F4XX_STDPERIPH_VERSION
printf("StdPeriph %i.%i.%i\n", __STM32F4XX_STDPERIPH_VERSION_MAIN, __STM32F4XX_STDPERIPH_VERSION_SUB1, __STM32F4XX_STDPERIPH_VERSION_SUB2);
#endif
#ifdef __STM32F3xx_HAL_VERSION
printf("HAL lib... TODO: print version\n");
#endif
#endif
// printf("CPU ID %lx %lx %lx\n",U_ID[0], U_ID[1], U_ID[2]);
printf("size: %lu crc:%lx\n", version_info.image_size, version_info.image_crc);
volatile const version_info_t* bt_version_info = (void*)0x08000188;
volatile const version_info_t *bt_version_info = (void *)0x08000188;
printf("######## Bootloader info ########\n");
printf(
"%s v%i.%i.%i %s\n",
bt_version_info->product_name,
bt_version_info->major,
bt_version_info->minor,
bt_version_info->patch,
bt_version_info->git_version
);
"%s v%i.%i.%i %s\n",
bt_version_info->product_name,
bt_version_info->major,
bt_version_info->minor,
bt_version_info->patch,
bt_version_info->git_version);
extern uint8_t _binary_obj_hvf3_hvf3_bin_start;
extern uint8_t _binary_obj_hvf3_hvf3_bin_size;
extern uint8_t _binary_obj_hvf3_hvf3_bin_end;
volatile const version_info_t* hv_version_info = (void*)(&_binary_obj_hvf3_hvf3_bin_start + 0x188);
volatile const version_info_t *hv_version_info = (void *)(&_binary_obj_hvf3_hvf3_bin_start + 0x188);
printf("######## HV info ########\n");
printf(
"%s v%i.%i.%i %s\n",
hv_version_info->product_name,
hv_version_info->major,
hv_version_info->minor,
hv_version_info->patch,
hv_version_info->git_version
);
"%s v%i.%i.%i %s\n",
hv_version_info->product_name,
hv_version_info->major,
hv_version_info->minor,
hv_version_info->patch,
hv_version_info->git_version);
// printf("Branch %s\n",bt_version_info->git_branch);
// printf("Compiled %s %s ",bt_version_info->build_date, bt_version_info->build_time);
// printf("by %s on %s\n",bt_version_info->build_user, bt_version_info->build_host);
//
printf("hv start:%p ,size:%p ,end%p \n",&_binary_obj_hvf3_hvf3_bin_start, &_binary_obj_hvf3_hvf3_bin_size, &_binary_obj_hvf3_hvf3_bin_end);
printf("hv start:%p ,size:%p ,end%p \n", &_binary_obj_hvf3_hvf3_bin_start, &_binary_obj_hvf3_hvf3_bin_size, &_binary_obj_hvf3_hvf3_bin_end);
}
COMMAND("about", about, "show system infos");
src/version.c
+1 -1
View File
@@ -1,6 +1,6 @@
#include "version.h"
volatile const version_info_t version_info __attribute__ ((section (".version_info"))) = {
volatile const version_info_t version_info __attribute__((section(".version_info"))) = {
.product_name = "STMBL",
.major = 0,
.minor = 9,
stm32f103/src/main.c
+7 -7
View File
@@ -5,10 +5,10 @@
#include <math.h>
#define ARES 4096.0 // analog resolution, 12 bit
#define AREF 3.3 // analog reference voltage
#define AREF 3.3 // analog reference voltage
//iramx v3.1-v3.3 hardware
#define RCUR 0.0181 //shunt
#define RCUR 0.0181 //shunt
#define TPULLUP 10000 //iramx temperature pullup
#define R10 10000
#define R11 180
@@ -25,8 +25,8 @@
#define VOLT(a) ((a) / ARES * AREF / VDIVDOWN * (VDIVUP + VDIVDOWN))
volatile uint16_t ADCConvertedValue[100]; //DMA buffer for ADC
volatile uint8_t rxbuf[50]; //DMA buffer for UART RX
uint32_t rxpos = 0; //UART rx buffer position
volatile uint8_t rxbuf[50]; //DMA buffer for UART RX
uint32_t rxpos = 0; //UART rx buffer position
volatile uint32_t u_cmd = 0;
volatile uint32_t v_cmd = 0;
@@ -363,7 +363,7 @@ void TIM1_UP_IRQHandler() {
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
if(timeout > 30) { //disable driver
hv_disable();
GPIO_SetBits(GPIOC, GPIO_Pin_1); //yellow led on
GPIO_SetBits(GPIOC, GPIO_Pin_1); //yellow led on
GPIO_ResetBits(GPIOC, GPIO_Pin_2); //green led off
PWM_U = 0;
@@ -377,7 +377,7 @@ void TIM1_UP_IRQHandler() {
w_cmd = TOFIXEDU32(0.0);
w_error = TOFIXEDU32(0.0);
} else {
GPIO_SetBits(GPIOC, GPIO_Pin_2); //green led on
GPIO_SetBits(GPIOC, GPIO_Pin_2); //green led on
GPIO_ResetBits(GPIOC, GPIO_Pin_1); //yellow led off
timeout++;
@@ -450,7 +450,7 @@ int main(void) {
float w = 0.0;
if(packet_to_hv.data.mode == 0) { //a,b voltages
u = ua; // inverse clarke
u = ua; // inverse clarke
v = -ua / 2.0 + ub / 2.0 * M_SQRT3;
w = -ua / 2.0 - ub / 2.0 * M_SQRT3;
} else if(packet_to_hv.data.mode == 1) { //DC, a: -dclink ... +dclink
stm32f303/inc/stm32f3xx_hal_conf.h
+8 -8
View File
@@ -95,7 +95,7 @@ extern "C" {
*/
#if !defined(HSE_VALUE)
#define HSE_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#endif /* HSE_VALUE */
/**
* @brief In the following line adjust the External High Speed oscillator (HSE) Startup
@@ -103,7 +103,7 @@ extern "C" {
*/
#if !defined(HSE_STARTUP_TIMEOUT)
#define HSE_STARTUP_TIMEOUT ((uint32_t)100) /*!< Time out for HSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */
#endif /* HSE_STARTUP_TIMEOUT */
/**
* @brief Internal High Speed oscillator (HSI) value.
@@ -112,7 +112,7 @@ extern "C" {
*/
#if !defined(HSI_VALUE)
#define HSI_VALUE ((uint32_t)8000000) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
#endif /* HSI_VALUE */
/**
* @brief In the following line adjust the Internal High Speed oscillator (HSI) Startup
@@ -120,7 +120,7 @@ extern "C" {
*/
#if !defined(HSI_STARTUP_TIMEOUT)
#define HSI_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for HSI start up */
#endif /* HSI_STARTUP_TIMEOUT */
#endif /* HSI_STARTUP_TIMEOUT */
/**
* @brief Internal Low Speed oscillator (LSI) value.
@@ -135,14 +135,14 @@ extern "C" {
*/
#if !defined(LSE_VALUE)
#define LSE_VALUE ((uint32_t)32768) /*!< Value of the External Low Speed oscillator in Hz */
#endif /* LSE_VALUE */
#endif /* LSE_VALUE */
/**
* @brief Time out for LSE start up value in ms.
*/
#if !defined(LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT ((uint32_t)5000) /*!< Time out for LSE start up, in ms */
#endif /* LSE_STARTUP_TIMEOUT */
#endif /* LSE_STARTUP_TIMEOUT */
/**
* @brief External clock source for I2S peripheral
@@ -153,7 +153,7 @@ extern "C" {
*/
#if !defined(EXTERNAL_CLOCK_VALUE)
#define EXTERNAL_CLOCK_VALUE ((uint32_t)8000000) /*!< Value of the External oscillator in Hz*/
#endif /* EXTERNAL_CLOCK_VALUE */
#endif /* EXTERNAL_CLOCK_VALUE */
/* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */
@@ -163,7 +163,7 @@ extern "C" {
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
#define VDD_VALUE ((uint32_t)3300) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)0) /*!< tick interrupt priority (lowest by default) */
#define USE_RTOS 0
#define PREFETCH_ENABLE 1
stm32f303/src/comps/io.c
+30 -33
View File
@@ -103,12 +103,12 @@ float r2temp(float r) {
return (-(r - b) / (a - b) * step + i * step + start);
}
}
return (temp[ARRAY_SIZE(temp)] + step); // TODO fix
return (temp[ARRAY_SIZE(temp)] + step); // TODO fix
}
static void nrt_init(volatile void *ctx_ptr, volatile 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;
struct io_ctx_t *ctx = (struct io_ctx_t *)ctx_ptr;
struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr;
GPIO_InitTypeDef GPIO_InitStruct;
//LED
GPIO_InitStruct.Pin = LED_PIN;
@@ -137,19 +137,19 @@ static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
// ADC34_COMMON->CCR |= ADC34_CCR_MDMA_1;
ctx->offset_count = 0;
ctx->u_offset = 0.0;
ctx->v_offset = 0.0;
ctx->w_offset = 0.0;
ctx->fault = NO_ERROR;
ctx->overtemp_error = 0;
ctx->offset_count = 0;
ctx->u_offset = 0.0;
ctx->v_offset = 0.0;
ctx->w_offset = 0.0;
ctx->fault = NO_ERROR;
ctx->overtemp_error = 0;
ctx->overvoltage_error = 0;
ctx->overcurrent_error = 0;
ctx->fault_pin_error = 0;
ctx->hv_temp = 0;
ctx->mot_temp = 0;
ctx->enabled = 0;
ctx->fault_pin_error = 0;
ctx->hv_temp = 0;
ctx->mot_temp = 0;
ctx->enabled = 0;
#ifdef HV_EN_PIN
GPIO_InitStruct.Pin = HV_EN_PIN;
@@ -185,23 +185,20 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(ctx->offset_count < 100) {
ctx->offset_count++;
}
else if(ctx->offset_count < 100 + 100){
} else if(ctx->offset_count < 100 + 100) {
ctx->w_offset += AMP((float)(a12 & 0xFFFF) / 5.0, SHUNT_GAIN) / 100.0;
ctx->u_offset += AMP((float)(a12 >> 16) / 5.0, SHUNT_GAIN) / 100.0;
ctx->v_offset += AMP((float)(a34 & 0xFFFF) / 5.0, SHUNT_GAIN) / 100.0;
ctx->offset_count++;
}
else if(ctx->offset_count < 100 + 100 + 1){
if(ABS(ctx->u_offset) > 5.0 || ABS(ctx->v_offset) > 5.0 || ABS(ctx->w_offset) > 5.0){
} else if(ctx->offset_count < 100 + 100 + 1) {
if(ABS(ctx->u_offset) > 5.0 || ABS(ctx->v_offset) > 5.0 || ABS(ctx->w_offset) > 5.0) {
ctx->fault = HV_CURRENT_OFFSET_FAULT;
}
ctx->offset_count++;
}
else{
PIN(uo) = ctx->u_offset;
PIN(vo) = ctx->v_offset;
PIN(wo) = ctx->w_offset;
} else {
PIN(uo) = ctx->u_offset;
PIN(vo) = ctx->v_offset;
PIN(wo) = ctx->w_offset;
PIN(iw) = -AMP((float)(a12 & 0xFFFF) / 5.0, SHUNT_GAIN) + ctx->w_offset; // 1u
PIN(iu) = -AMP((float)(a12 >> 16) / 5.0, SHUNT_GAIN) + ctx->u_offset;
PIN(iv) = -AMP((float)(a34 & 0xFFFF) / 5.0, SHUNT_GAIN) + ctx->v_offset;
@@ -210,9 +207,9 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
PIN(u) = VOLT(adc_34_buf[5] & 0xFFFF) * 0.05 + PIN(u) * 0.95;
PIN(udc) = VOLT(adc_34_buf[5] >> 16) * 0.05 + PIN(udc) * 0.95;
PIN(iabs) = MAX3(ABS(PIN(iu)), PIN(iv), PIN(iw));
ctx->hv_temp = adc_34_buf[0];
ctx->hv_temp = adc_34_buf[0];
ctx->mot_temp = adc_34_buf[3];
if(err_filter(&(ctx->overtemp_error), 5.0, 0.001, PIN(hv_temp) > ABS_MAX_TEMP)) {
ctx->fault = HV_TEMP_ERROR;
}
@@ -232,7 +229,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
PIN(fault) = ctx->fault;
if(PIN(hv_en) > 0.0) {
if(!ctx->enabled){//rising edge of enable
if(!ctx->enabled) { //rising edge of enable
//set timer master out enable
TIM8->BDTR |= TIM_BDTR_MOE;
#ifdef HV_EN_PIN
@@ -244,13 +241,13 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
if(ctx->fault == NO_ERROR) {
#ifdef HV_FAULT_PIN
//read fault pin from driver
if(err_filter(&(ctx->fault_pin_error), 5.0, 0.01, HAL_GPIO_ReadPin(HV_FAULT_PORT, HV_FAULT_PIN) == HV_FAULT_POLARITY)){
if(err_filter(&(ctx->fault_pin_error), 5.0, 0.01, HAL_GPIO_ReadPin(HV_FAULT_PORT, HV_FAULT_PIN) == HV_FAULT_POLARITY)) {
ctx->fault = HV_HV_FAULT;
}
#endif
//Master out enable is cleared by timer break input.
//Timer break input is connected to comperators
if(!(TIM8->BDTR & TIM_BDTR_MOE)){
if(!(TIM8->BDTR & TIM_BDTR_MOE)) {
ctx->fault = HV_OVERCURRENT_HW;
}
} else {
@@ -262,7 +259,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
}
} else {
ctx->enabled = 0;
ctx->fault = NO_ERROR;
ctx->fault = NO_ERROR;
#ifdef HV_EN_PIN
//clear driver enable pin
HAL_GPIO_WritePin(HV_EN_PORT, HV_EN_PIN, GPIO_PIN_RESET);
@@ -271,7 +268,7 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
}
//dac output for comperators
DAC1->DHR12R1 = CLAMP((uint32_t)PIN(dac),0,4095);
DAC1->DHR12R1 = CLAMP((uint32_t)PIN(dac), 0, 4095);
//comperator outputs for debugging
PIN(cu) = (COMP1->CSR & COMP_CSR_COMPxOUT) > 0;
@@ -285,14 +282,14 @@ void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
uint32_t led = (uint32_t)PIN(led);
if(hal.hal_state != HAL_OK2){
if(hal.hal_state != HAL_OK2) {
led = 2;
}
HAL_GPIO_WritePin(LED_PORT, LED_PIN, BLINK(led) > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
PIN(hv_temp) = r2temp(HV_R(ADC(ctx->hv_temp >> 16))) * 0.01 + PIN(hv_temp) * 0.99;
PIN(mot_temp) = MOT_R(MOT_REF(ADC(ctx->mot_temp >> 16)));
PIN(mot_temp) = MOT_R(MOT_REF(ADC(ctx->mot_temp >> 16)));
}
hal_comp_t io_comp_struct = {
stm32f303/src/comps/ls.c
+23 -23
View File
@@ -42,7 +42,7 @@ HAL_PIN(q_fb);
HAL_PIN(hv_temp);
HAL_PIN(mot_temp);
HAL_PIN(core_temp);
HAL_PIN(fault_in);//fault code send to f4
HAL_PIN(fault_in); //fault code send to f4
HAL_PIN(y);
HAL_PIN(u_fb);
HAL_PIN(v_fb);
@@ -55,7 +55,7 @@ HAL_PIN(crc_ok);
HAL_PIN(timeout);
HAL_PIN(dma_pos);
HAL_PIN(idle);
HAL_PIN(fault);//communication fault output
HAL_PIN(fault); //communication fault output
HAL_PIN(dma_pos2);
HAL_PIN(arr);
@@ -146,8 +146,8 @@ static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
USART3->CR2 |= USART_CR2_RTOEN; // timeout en
USART3->ICR |= USART_ICR_RTOCF; // timeout clear flag
ctx->packet_from_hv.header.len = (sizeof(packet_from_hv_t) - sizeof(stmbl_talk_header_t)) / 4;
ctx->packet_from_hv.header.flags.cmd = WRITE_CONF;
ctx->packet_from_hv.header.len = (sizeof(packet_from_hv_t) - sizeof(stmbl_talk_header_t)) / 4;
ctx->packet_from_hv.header.flags.cmd = WRITE_CONF;
ctx->packet_from_hv.header.slave_addr = 0;
}
@@ -190,25 +190,25 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
uint32_t crc = HAL_CRC_Calculate(&hcrc, (uint32_t *)&(ctx->packet_to_hv.header.slave_addr), sizeof(packet_to_hv_t) / 4 - 1);
if(ctx->packet_to_hv.header.slave_addr == 0 && ctx->packet_to_hv.header.len == (sizeof(packet_to_hv_t) - sizeof(stmbl_talk_header_t)) / 4 && crc == ctx->packet_to_hv.header.crc) {
//
uint8_t a = ctx->packet_to_hv.header.conf_addr;
a = CLAMP(a, 0, sizeof(config) / 4);
uint8_t a = ctx->packet_to_hv.header.conf_addr;
a = CLAMP(a, 0, sizeof(config) / 4);
switch(ctx->packet_to_hv.header.flags.cmd){
switch(ctx->packet_to_hv.header.flags.cmd) {
case NO_CMD:
break;
break;
case WRITE_CONF:
config.data[a] = ctx->packet_to_hv.header.config.f32; // TODO: first enable after complete update
break;
config.data[a] = ctx->packet_to_hv.header.config.f32; // TODO: first enable after complete update
break;
case READ_CONF:
ctx->tx_addr = a;
break;
break;
case DO_RESET:
NVIC_SystemReset();
break;
break;
case BOOTLOADER:
RTC->BKP0R = 0xDEADBEEF;
NVIC_SystemReset();
break;
break;
}
PIN(en) = ctx->packet_to_hv.flags.enable;
@@ -218,10 +218,10 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
PIN(q_cmd) = ctx->packet_to_hv.q_cmd;
PIN(pos) = ctx->packet_to_hv.pos;
PIN(vel) = ctx->packet_to_hv.vel;
if(ctx->packet_to_hv.flags.buf != 0x0){
if(ctx->packet_to_hv.flags.buf != 0x0) {
extern struct ringbuf rx_buf;
rb_write(&rx_buf, (void*)&(ctx->packet_to_hv.flags.buf), 1);
rb_write(&rx_buf, (void *)&(ctx->packet_to_hv.flags.buf), 1);
}
PIN(r) = config.pins.r;
@@ -285,18 +285,18 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_
state.pins.pwm_volt = PIN(pwm_volt);
// fill tx struct
ctx->packet_from_hv.fault = (uint8_t)PIN(fault_in);
ctx->packet_from_hv.d_fb = PIN(d_fb);
ctx->packet_from_hv.q_fb = PIN(q_fb);
ctx->packet_from_hv.header.conf_addr = ctx->tx_addr;
ctx->packet_from_hv.header.config.f32 = state.data[ctx->tx_addr++];
ctx->packet_from_hv.fault = (uint8_t)PIN(fault_in);
ctx->packet_from_hv.d_fb = PIN(d_fb);
ctx->packet_from_hv.q_fb = PIN(q_fb);
ctx->packet_from_hv.header.conf_addr = ctx->tx_addr;
ctx->packet_from_hv.header.config.f32 = state.data[ctx->tx_addr++];
ctx->tx_addr %= sizeof(state) / 4;
extern struct ringbuf tx_buf;
uint8_t buf[1];
if(rb_read(&tx_buf, buf, 1)){
if(rb_read(&tx_buf, buf, 1)) {
ctx->packet_from_hv.buf = buf[0];
}else{
} else {
ctx->packet_from_hv.buf = 0x0;
}
ctx->packet_from_hv.header.crc = HAL_CRC_Calculate(&hcrc, (uint32_t *)&(ctx->packet_from_hv.header.slave_addr), sizeof(packet_from_hv_t) / 4 - 1);
stm32f303/src/main.c
+8 -8
View File
@@ -96,15 +96,15 @@ void about(char *ptr) {
printf("by %s on %s\n", version_info.build_user, version_info.build_host);
printf("GCC %s\n", __VERSION__);
printf("newlib %s\n", _NEWLIB_VERSION);
#ifdef __CM4_CMSIS_VERSION
printf("CMSIS %i.%i\n",__CM4_CMSIS_VERSION_MAIN,__CM4_CMSIS_VERSION_SUB);
#endif
#ifdef __STM32F4XX_STDPERIPH_VERSION
printf("StdPeriph %i.%i.%i\n",__STM32F4XX_STDPERIPH_VERSION_MAIN,__STM32F4XX_STDPERIPH_VERSION_SUB1,__STM32F4XX_STDPERIPH_VERSION_SUB2);
#endif
#ifdef __STM32F3xx_HAL_VERSION
#ifdef __CM4_CMSIS_VERSION
printf("CMSIS %i.%i\n", __CM4_CMSIS_VERSION_MAIN, __CM4_CMSIS_VERSION_SUB);
#endif
#ifdef __STM32F4XX_STDPERIPH_VERSION
printf("StdPeriph %i.%i.%i\n", __STM32F4XX_STDPERIPH_VERSION_MAIN, __STM32F4XX_STDPERIPH_VERSION_SUB1, __STM32F4XX_STDPERIPH_VERSION_SUB2);
#endif
#ifdef __STM32F3xx_HAL_VERSION
printf("HAL lib... TODO: print version\n");
#endif
#endif
}
COMMAND("about", about, "show system infos");
stm32f303/src/stm32f3xx_it.c
+5 -5
View File
@@ -149,13 +149,13 @@ void DebugMon_Handler(void) {
* @brief This function handles USB low priority or CAN_RX0 interrupts.
*/
void USB_LP_CAN_RX0_IRQHandler(void) {
/* USER CODE BEGIN USB_LP_CAN_RX0_IRQn 0 */
//GPIOA->BSRR |= GPIO_PIN_10;
/* USER CODE BEGIN USB_LP_CAN_RX0_IRQn 0 */
//GPIOA->BSRR |= GPIO_PIN_10;
/* USER CODE END USB_LP_CAN_RX0_IRQn 0 */
#ifdef USB_TERM
/* USER CODE END USB_LP_CAN_RX0_IRQn 0 */
#ifdef USB_TERM
HAL_PCD_IRQHandler(&hpcd_USB_FS);
#endif
#endif
/* USER CODE BEGIN USB_LP_CAN_RX0_IRQn 1 */
//GPIOA->BSRR |= GPIO_PIN_10 << 16;
/* USER CODE END USB_LP_CAN_RX0_IRQn 1 */
stm32f303/src/tim.c
+1 -1
View File
@@ -90,7 +90,7 @@ void MX_TIM8_Init(void) {
sBreakDeadTimeConfig.DeadTime = PWM_DEADTIME;
sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE;
sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
sBreakDeadTimeConfig.BreakFilter = 0xa;//0.55uS
sBreakDeadTimeConfig.BreakFilter = 0xa; //0.55uS
sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
sBreakDeadTimeConfig.Break2Filter = 0;

Some files were not shown because too many files have changed in this diff Show More