Fix algoritm FLASH
This commit is contained in:
parent
bbdf967e6d
commit
5dbd56f200
4 changed files with 240 additions and 15 deletions
58
controller/fw/embed/include/flash.h
Normal file
58
controller/fw/embed/include/flash.h
Normal file
|
|
@ -0,0 +1,58 @@
|
|||
#ifndef FLASH_H_
|
||||
#define FLASH_H_
|
||||
#include "stm32f446xx.h"
|
||||
|
||||
|
||||
|
||||
|
||||
/* for addr in FLASH */
|
||||
typedef struct{
|
||||
uint8_t data_id;
|
||||
uint8_t value;
|
||||
uint16_t crc;
|
||||
}FLASH_RECORD;
|
||||
|
||||
static uint32_t write_ptr;
|
||||
#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct
|
||||
#define PARAM_COUNT 4 // count data in flash
|
||||
|
||||
// Flash sectors for STM32F407
|
||||
|
||||
#define SECTOR_2 0x08008000 // 16KB
|
||||
#define SECTOR_3 0x0800C000 // 16KB
|
||||
#define SECTOR_4 0x08010000 // 64KB
|
||||
#define SECTOR_5 0x08020000 // 128KB
|
||||
#define SECTOR_6 0x08040000 // 128KB
|
||||
#define SECTOR_7 0x08060000 // 128KB
|
||||
|
||||
|
||||
#define FLAG_BOOT (0x08040000 + 4)
|
||||
// Flash keys for unlocking flash memory
|
||||
|
||||
|
||||
//FLASH SET ONE PROGRAMM WORD
|
||||
#define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1
|
||||
#define FLASH_32BYTE FLASH->CR |= FLASH_CR_PSIZE | FLASH_CR_PSIZE_0
|
||||
|
||||
// Flash command bits
|
||||
#define FLASH_LOCK FLASH->CR |= FLASH_CR_LOCK
|
||||
#define FLASH_UNLOCK FLASH->KEYR = FLASH_KEY1; FLASH->KEYR = FLASH_KEY2
|
||||
|
||||
|
||||
// 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))
|
||||
|
||||
//for bootloader
|
||||
typedef void(*pFunction)(void);
|
||||
|
||||
// Function prototypes
|
||||
void flash_unlock(void);
|
||||
void flash_lock(void);
|
||||
void erase_sector(uint8_t sector);
|
||||
void program_word(uint32_t address, uint32_t data,uint32_t byte_len);
|
||||
uint8_t flash_read_word(uint32_t address);
|
||||
|
||||
|
||||
|
||||
#endif /* FLASH_H_ */
|
||||
|
|
@ -2,7 +2,7 @@
|
|||
#define REG_CAH_H_
|
||||
|
||||
#define APP_ADDR 0x0800400 // 16KB - Application
|
||||
#define ADDR_VAR 0x8040000
|
||||
#define ADDR_VAR 0x8050000
|
||||
#define ADDR_VAR_ID ADDR_VAR
|
||||
#define ADDR_VAR_P (ADDR_VAR + 1)
|
||||
#define ADDR_VAR_I (ADDR_VAR + 2)
|
||||
|
|
|
|||
166
controller/fw/embed/src/flash.cpp
Normal file
166
controller/fw/embed/src/flash.cpp
Normal file
|
|
@ -0,0 +1,166 @@
|
|||
#include "flash.h"
|
||||
#include <stdbool.h>
|
||||
|
||||
|
||||
|
||||
void flash_unlock(){
|
||||
|
||||
// Check if flash is locked
|
||||
if(!(FLASH->CR & FLASH_CR_LOCK)) {
|
||||
return; // Already unlocked
|
||||
}
|
||||
|
||||
// Write flash key sequence to unlock
|
||||
FLASH->KEYR = 0x45670123; // First key
|
||||
FLASH->KEYR = 0xCDEF89AB; // Second key
|
||||
|
||||
}
|
||||
|
||||
void flash_lock() {
|
||||
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||
return; // Already locked
|
||||
}
|
||||
FLASH->CR |= FLASH_CR_LOCK; // Lock flash memory
|
||||
}
|
||||
|
||||
void erase_sector(uint8_t sector){
|
||||
|
||||
// Wait if flash is busy
|
||||
while(FLASH_BUSY);
|
||||
|
||||
// Check if flash is locked and unlock if needed
|
||||
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||
flash_unlock();
|
||||
}
|
||||
|
||||
// Set sector erase bit and sector number
|
||||
FLASH->CR |= FLASH_CR_SER;
|
||||
FLASH->CR &= ~FLASH_CR_SNB;
|
||||
FLASH->CR |= (sector << FLASH_CR_SNB_Pos) & FLASH_CR_SNB_Msk;
|
||||
|
||||
// Start erase
|
||||
FLASH->CR |= FLASH_CR_STRT;
|
||||
|
||||
// Wait for erase to complete
|
||||
while(FLASH_BUSY);
|
||||
|
||||
// Clear sector erase bit
|
||||
FLASH->CR &= ~FLASH_CR_SER;
|
||||
|
||||
}
|
||||
|
||||
void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len){
|
||||
|
||||
// Wait if flash is busy
|
||||
while(FLASH_BUSY);
|
||||
// Check if flash is locked and unlock if needed
|
||||
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||
flash_unlock();
|
||||
}
|
||||
|
||||
// Set program bit 32bit programm size and Write data to address
|
||||
if(byte_len == 1) {
|
||||
FLASH_8BYTE;
|
||||
FLASH->CR |= FLASH_CR_PG;
|
||||
*(volatile uint8_t*)address = (uint8_t)data;
|
||||
} else {
|
||||
FLASH_32BYTE;
|
||||
FLASH->CR |= FLASH_CR_PG;
|
||||
*(volatile uint32_t*)address = data;
|
||||
}
|
||||
|
||||
// Wait for programming to complete
|
||||
while(FLASH_BUSY);
|
||||
|
||||
// Clear program bit
|
||||
FLASH->CR &= ~FLASH_CR_PG;
|
||||
|
||||
}
|
||||
void flash_write(uint32_t addr, FLASH_RECORD* record){
|
||||
|
||||
uint32_t* data = (uint32_t*)record;
|
||||
uint32_t size = FLASH_RECORD_SIZE / 4; //count words in struct
|
||||
// Wait if flash is busy
|
||||
while(FLASH_BUSY);
|
||||
|
||||
// Check if flash is locked and unlock if needed
|
||||
if(FLASH->CR & FLASH_CR_LOCK) {
|
||||
flash_unlock();
|
||||
}
|
||||
|
||||
// Set program bit and write data to flash
|
||||
FLASH_32BYTE;
|
||||
FLASH->CR |= FLASH_CR_PG;
|
||||
|
||||
for(int i = 0;i < size;i++){
|
||||
*(volatile uint32_t*)(addr + i * 4) = data[i];
|
||||
}
|
||||
|
||||
// Clear program bit
|
||||
FLASH->CR &= ~FLASH_CR_PG;
|
||||
flash_lock();
|
||||
}
|
||||
|
||||
uint8_t flash_read_word(uint32_t address){
|
||||
|
||||
// Check if address is valid
|
||||
if(address < FLASH_BASE || address > FLASH_END) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Read byte from flash memory
|
||||
return *((volatile uint8_t*)address);
|
||||
|
||||
}
|
||||
// Wait if flash
|
||||
|
||||
|
||||
bool validate_crc(FLASH_RECORD* crc){
|
||||
if(crc->crc == 6933)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
/* read struct from FLASH */
|
||||
void flash_read(uint32_t addr,FLASH_RECORD* ptr){
|
||||
uint8_t* flash_ptr = (uint8_t*)addr;
|
||||
uint8_t* dest = (uint8_t*)ptr;
|
||||
for(int i = 0;i < FLASH_RECORD_SIZE;i++)
|
||||
dest[i] = flash_ptr[i];
|
||||
}
|
||||
|
||||
void compact_page(){
|
||||
FLASH_RECORD latest[PARAM_COUNT] = {0};
|
||||
for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7,i += FLASH_RECORD_SIZE;) {
|
||||
FLASH_RECORD rec;
|
||||
flash_read(i,&rec);
|
||||
|
||||
if (validate_crc(&rec)){
|
||||
latest[rec.data_id] = rec;
|
||||
}
|
||||
}
|
||||
erase_sector(6); //clean sector 6
|
||||
write_ptr = (uint32_t)SECTOR_6; //go to start sector 6
|
||||
for(int i = 0; i < PARAM_COUNT;i++) {
|
||||
if(latest[i].data_id != 0xFF){
|
||||
flash_write((uint32_t)write_ptr,&latest[i]);
|
||||
write_ptr += FLASH_RECORD_SIZE;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void write_param(uint8_t param_id, uint8_t val){
|
||||
FLASH_RECORD param_flash = {
|
||||
.data_id = param_id,
|
||||
.value = val,
|
||||
.crc = 6933
|
||||
};
|
||||
/* Exit data from FLASH */
|
||||
if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_7)
|
||||
compact_page();
|
||||
|
||||
flash_write((uint32_t)write_ptr,¶m_flash);
|
||||
write_ptr += FLASH_RECORD_SIZE;
|
||||
}
|
||||
|
|
@ -13,7 +13,7 @@
|
|||
#include "wiring_constants.h"
|
||||
// clang-format on
|
||||
#include "reg_cah.h"
|
||||
|
||||
#include "flash.h"
|
||||
|
||||
|
||||
|
||||
|
|
@ -29,7 +29,7 @@ uint32_t SectorError;
|
|||
|
||||
|
||||
|
||||
|
||||
static FLASH_RECORD flash_rec;
|
||||
static CAN_message_t CAN_TX_msg;
|
||||
static CAN_message_t CAN_inMsg;
|
||||
|
||||
|
|
@ -102,7 +102,8 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
|
|||
|
||||
void send_velocity() {
|
||||
float current_velocity = motor.shaftVelocity();
|
||||
CAN_TX_msg.id = 1;
|
||||
uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
|
||||
CAN_TX_msg.id = id;
|
||||
CAN_TX_msg.buf[0] = 'V';
|
||||
CAN_TX_msg.len = 5;
|
||||
memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity));
|
||||
|
|
@ -111,7 +112,8 @@ void send_velocity() {
|
|||
|
||||
void send_angle() {
|
||||
float current_angle = motor.shaftAngle();
|
||||
CAN_TX_msg.id = 1;
|
||||
uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
|
||||
CAN_TX_msg.id = id;
|
||||
CAN_TX_msg.buf[0] = 'A';
|
||||
CAN_TX_msg.len = 5;
|
||||
memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle));
|
||||
|
|
@ -119,7 +121,8 @@ void send_angle() {
|
|||
}
|
||||
|
||||
void send_motor_enabled() {
|
||||
CAN_TX_msg.id = 1;
|
||||
uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
|
||||
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));
|
||||
|
|
@ -128,7 +131,7 @@ void send_motor_enabled() {
|
|||
|
||||
void send_foc_state() {
|
||||
uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
|
||||
CAN_TX_msg.id = FOC_STATE;
|
||||
CAN_TX_msg.id = id;
|
||||
CAN_TX_msg.buf[0] = 'F';
|
||||
memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state,
|
||||
sizeof(motor_control_inputs.foc_state));
|
||||
|
|
@ -140,19 +143,17 @@ void send_id() {
|
|||
|
||||
uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID;
|
||||
CAN_TX_msg.id = id;
|
||||
CAN_TX_msg.buf[0] = 'I';
|
||||
memcpy(&CAN_TX_msg.buf[0], &id, sizeof(id));
|
||||
Can.write(CAN_TX_msg);
|
||||
}
|
||||
|
||||
void setup_id(uint8_t my_id) {
|
||||
/* Чтобы разблокировать флэш память для записи*/
|
||||
FLASH->KEYR = 0x45670123; // Первый ключ
|
||||
FLASH->KEYR = 0xCDEF89AB; // Второй ключ
|
||||
while (FLASH->SR & FLASH_SR_BSY) { }
|
||||
FLASH->CR &= ~FLASH_CR_PSIZE_0 & ~FLASH_CR_PSIZE_1; // Data write = 8bit
|
||||
FLASH->CR |= FLASH_CR_SNB_1 | FLASH_CR_SNB_2; // 6 SECTOR FOR ERASE
|
||||
FLASH->CR |= FLASH_CR_SER;
|
||||
FLASH->CR |= FLASH_CR_STRT;
|
||||
flash_unlock();
|
||||
while (FLASH_BUSY) { }
|
||||
|
||||
erase_sector(6); //очистка 6 сектора
|
||||
while ((FLASH->SR & FLASH_SR_BSY) != 0)
|
||||
__NOP();
|
||||
FLASH->SR |= (FLASH_SR_EOP | FLASH_SR_WRPERR | FLASH_SR_PGAERR);
|
||||
|
|
@ -238,7 +239,7 @@ void listen_can() {
|
|||
send_id();
|
||||
break;
|
||||
case MOTOR_VELOCITY:
|
||||
send_velocity();
|
||||
send_velocity();
|
||||
break;
|
||||
|
||||
case MOTOR_ANGLE:
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue