From a0800410e0ff4c598f3b230c48e0599e5eb52c0c Mon Sep 17 00:00:00 2001 From: Valentin Dabstep Date: Thu, 15 May 2025 17:38:59 +0300 Subject: [PATCH] Update struct and working for FLASH --- controller/fw/embed/include/flash.h | 22 ++++--- controller/fw/embed/src/flash.cpp | 65 ++++++++++++++------- controller/fw/embed/src/main.cpp | 90 ++++++++++++++--------------- 3 files changed, 99 insertions(+), 78 deletions(-) diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index 39be097..0adad30 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -4,22 +4,24 @@ #include #include - /* for addr in FLASH */ + +/* no padding for this struct, beacuse storing 8 bytes*/ typedef struct{ uint8_t data_id; // data_id = id register of can - uint8_t value; + uint8_t data_type; //float or uint8_t if 4 - float 1 - uint8_t uint16_t crc; + uint32_t value; // uint32_t write_ptr_now; }FLASH_RECORD; enum { addr_id = 0, - foc_id = 1, - angl = 2, - vel = 3, - pid_p = 4, + pid_p, pid_i, - pid_d + pid_d, + foc_id, + angl, + vel }; #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct @@ -54,7 +56,7 @@ enum { // Flash status flags #define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY) #define FLASH_ERROR (FLASH->SR & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR)) - +static uint32_t write_ptr = SECTOR_6; //for bootloader typedef void(*pFunction)(void); @@ -64,11 +66,13 @@ void flash_lock(void); void erase_sector(uint8_t sector); void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len); uint8_t flash_read_word(uint32_t address); -void write_param(uint8_t param_id, uint8_t val); FLASH_RECORD* load_params(); void compact_page(); void flash_read(uint32_t addr,FLASH_RECORD* ptr); uint16_t validate_crc16(uint8_t *data,uint32_t length); void flash_write(uint32_t addr, FLASH_RECORD* record); bool validaate_crc(FLASH_RECORD* crc); + +void write_param(uint8_t param_id,uint32_t val); + #endif /* FLASH_H_ */ diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp index b06814d..686163f 100644 --- a/controller/fw/embed/src/flash.cpp +++ b/controller/fw/embed/src/flash.cpp @@ -2,7 +2,6 @@ #include #include "hal_conf_extra.h" -static uint32_t write_ptr = SECTOR_6; void flash_unlock(){ @@ -95,11 +94,11 @@ void flash_write(uint32_t addr, FLASH_RECORD* record){ for(int i = 0;i < size;i++){ *(volatile uint32_t*)(addr + (i * 4)) = data[i]; - write_ptr++; } // Clear program bit FLASH->CR &= ~FLASH_CR_PG; + write_ptr = addr + (size * 4); //increase variable storing addr flash_lock(); } @@ -136,6 +135,24 @@ uint16_t validate_crc16(uint8_t *data, uint32_t length) { +uint16_t calc_crc_struct(FLASH_RECORD* res){ + + uint8_t arr_res[FLASH_RECORD_SIZE - 2]; + uint16_t crc_res; + /* sorting data without CRC */ + arr_res[0] = res->data_id; + arr_res[1] = res->data_type; + + /* from 32 to 8 bit */ + for(int i = 0;i < 4;i++) + arr_res[i + 2] = (uint8_t)(res->value >> i * 4); + + crc_res = validate_crc16(arr_res,FLASH_RECORD_SIZE - 2); + return crc_res; +} + + + /* read struct from FLASH */ void flash_read(uint32_t addr,FLASH_RECORD* ptr){ uint8_t* flash_ptr = (uint8_t*)addr; @@ -149,7 +166,7 @@ void compact_page(){ for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) { FLASH_RECORD rec; flash_read(i,&rec); - uint16_t calculated_crc = validate_crc16((uint8_t*)&rec, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC + uint16_t calculated_crc = calc_crc_struct(&rec); if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) { // if the crc does not match, we check further @@ -174,24 +191,25 @@ void compact_page(){ } } - -void write_param(uint8_t param_id, uint8_t val) { - FLASH_RECORD param_flash = {param_id, val}; +void write_param(uint8_t param_id, uint32_t val) { + FLASH_RECORD param_flash; // __disable_irq(); // Interrupt off - - param_flash.crc = validate_crc16((uint8_t*)¶m_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти + param_flash.data_id = param_id; + param_flash.value = val; + param_flash.data_type = sizeof(uint8_t); + param_flash.crc = calc_crc_struct(¶m_flash); - // check alignment - if (write_ptr % 4 != 0) { - write_ptr += (4 - (write_ptr % 4)); + // check alignment + if (write_ptr % 8 != 0) { + write_ptr += (8 - (write_ptr % 8)); } // check buffer overflow if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) { compact_page(); // after compact_page update // alignment - if (write_ptr % 4 != 0) { - write_ptr += (4 - (write_ptr % 4)); + if (write_ptr % 8 != 0) { + write_ptr += (8 - (write_ptr % 8)); } } @@ -200,22 +218,27 @@ void write_param(uint8_t param_id, uint8_t val) { // __enable_irq(); // Interrupt on } + + + FLASH_RECORD* load_params(){ __disable_irq(); static FLASH_RECORD latest[PARAM_COUNT] = {0}; FLASH_RECORD res; + for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { flash_read(addr,&res); - /* check CRC */ - uint16_t calculated_crc = validate_crc16((uint8_t*)&res, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC - if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) - continue; - else{ + + + uint16_t calculated_crc = calc_crc_struct(&res); + if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) continue; + + else{ latest[res.data_id] = res; + write_ptr = addr + FLASH_RECORD_SIZE; + } } - write_ptr = addr + FLASH_RECORD_SIZE; -} __enable_irq(); return latest; -} \ No newline at end of file +} diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 49732ad..56f17ec 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -151,7 +151,7 @@ void send_velocity() { // Error handling: logging, alerts, etc. return; } - uint8_t value = flash_rec[vel].value; + float value = flash_rec[vel].value; uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'V',&value); } @@ -167,12 +167,13 @@ void send_angle() { } void send_motor_enabled() { - uint8_t id = *(volatile uint8_t*)ADDR_VAR; - CAN_TX_msg.id = id; - CAN_TX_msg.buf[0] = 'E'; - memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled, - sizeof(motor_control_inputs.motor_enabled)); - Can.write(CAN_TX_msg); + int motor = 1; + if (flash_rec == nullptr) { // Null check + // Error handling: logging, alerts, etc. + return; + } + uint8_t id = flash_rec[addr_id].value; + send_can_with_id_crc(id,'M',&motor); } void send_foc_state() { @@ -198,37 +199,38 @@ void send_id() { __NOP(); } -void send_motor_torque() { - float i_q = motor.current.q; // Q-axis current (A) - float torque = kt * i_q; // Torque calculation - torque *= 100; - CAN_TX_msg.id = flash_rec->value; - CAN_TX_msg.buf[0] = 'T'; - CAN_TX_msg.len = 5; - memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque)); - Can.write(CAN_TX_msg); -} +// void send_motor_torque() { +// float i_q = motor.current.q; // Q-axis current (A) +// float torque = kt * i_q; // Torque calculation +// torque *= 100; +// CAN_TX_msg.id = flash_rec->value; +// CAN_TX_msg.buf[0] = 'T'; +// CAN_TX_msg.len = 5; +// memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque)); +// Can.write(CAN_TX_msg); +// } void send_pid(uint8_t param_pid){ if (flash_rec == nullptr) { // Null check return; } uint8_t id = flash_rec[addr_id].value; - uint8_t d = flash_rec[param_pid].value; - uint8_t data_send = 0; - int l = 0; - while(d /= 10) - l++; - if(l >= 2) - data_send = (float)d; - else if(l == 1) - data_send = (float)(d * 10); - else - data_send = (float)(d * 100); - if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp; - else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki; - else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd; - send_can_with_id_crc(id,param_pid,&data_send); + float data = flash_rec[param_pid].value; + switch(param_pid){ + case pid_p: + param_pid = REG_MOTOR_POSPID_Kp; + break; + + case pid_i: + param_pid = REG_MOTOR_POSPID_Ki; + break; + + case pid_d: + param_pid = REG_MOTOR_POSPID_Kd; + break; + } + + send_can_with_id_crc(id,param_pid,&data); } void setup_id(uint8_t my_id) { @@ -255,16 +257,8 @@ void setup_pid_angle(uint8_t param_pid, float data){ default: break; } - uint8_t check = uint8_t(data); - uint8_t data_save = 0; - if(check != 0) - if(check /= 10) - data_save = check; - else - data_save = (uint8_t)(data * 10); - else - data_save = (uint8_t)(data * 100); - write_param(param_pid,data_save); + + write_param(param_pid,data); } void listen_can(const CAN_message_t &msg) { @@ -336,7 +330,7 @@ void listen_can(const CAN_message_t &msg) { case MOTOR_VELOCITY: send_velocity(); break; case MOTOR_ANGLE: send_angle(); break; case MOTOR_ENABLED: send_motor_enabled(); break; - case MOTOR_TORQUE: send_motor_torque(); break; + // case MOTOR_TORQUE: send_motor_torque(); break; case FOC_STATE: send_foc_state(); break; case REG_MOTOR_POSPID_Kp: send_pid(pid_p); break; case REG_MOTOR_POSPID_Ki: send_pid(pid_i); break; @@ -388,12 +382,12 @@ void foc_step(BLDCMotor *motor, Commander *commander) { // Настройка прерываний CAN CAN2->IER |= CAN_IER_FMPIE0; - flash_rec = load_params(); - /* For test */ - // int value = 3; //addr - // write_param(addr_id,value); //for update address in firmware - // // Load parameters from flash // flash_rec = load_params(); + // /* For test */ + // float value = 3.46; //addr + // write_param(pid_p,value); //for update address in firmware + // // Load parameters from flash + flash_rec = load_params(); for(int i = 0;i < PARAM_COUNT;i++) flash_buf[i] = flash_rec[i];