robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

Update struct and working for FLASH

Browse source
This commit is contained in:
Valentin Dabstep 2025-05-15 17:38:59 +03:00
parent 2e88044e07
commit a0800410e0
3 changed files with 99 additions and 78 deletions
Download patch file Download diff file Expand all files Collapse all files

22
controller/fw/embed/include/flash.h
View file

@ -4,22 +4,24 @@
#include <stdio.h>
#include <stdlib.h>
/* 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_ */

65
controller/fw/embed/src/flash.cpp
View file

@ -2,7 +2,6 @@
#include <stdbool.h>
#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*)&param_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(&param_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;
}
}

90
controller/fw/embed/src/main.cpp
View file

@ -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];