From 12d66cc0eba9fd95f302805d463785251cd74a97 Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 27 Feb 2025 20:08:38 +0300 Subject: [PATCH 01/20] =?UTF-8?q?=D0=A0=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7?= =?UTF-8?q?=D0=BE=D0=B2=D0=B0=D0=BD=D0=B0=20=D0=B2=D0=BE=D0=B7=D0=BC=D0=BE?= =?UTF-8?q?=D0=B6=D0=BD=D0=BE=D1=81=D1=82=D1=8C=20=D1=81=D0=BE=D1=85=D1=80?= =?UTF-8?q?=D0=B0=D0=BD=D0=B5=D0=BD=D0=B8=D1=8F=20=D0=B0=D0=B4=D1=80=D0=B5?= =?UTF-8?q?=D1=81=D0=B0=20=D1=83=D1=81=D1=82=D1=80=D0=BE=D0=B9=D1=81=D1=82?= =?UTF-8?q?=D0=B2=D0=B0=20=D0=B8=20=D1=87=D1=82=D0=B5=D0=BD=D0=B8=D1=8F=20?= =?UTF-8?q?=D0=B5=D0=B3=D0=BE=20=D1=81=20Flash=20=D0=BF=D0=B0=D0=BC=D1=8F?= =?UTF-8?q?=D1=82=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/src/main.cpp | 150 +++++++++++++++++++++++++++---- 1 file changed, 135 insertions(+), 15 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index b342add..0a7074b 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -11,9 +11,24 @@ #include "wiring_analog.h" #include "wiring_constants.h" // clang-format on +#include "reg_cah.h" + + + + STM32_CAN Can(CAN2, DEF); +/* for FLASH */ +uint32_t flash_flag; +uint8_t flag_can = 0; +uint32_t flash_error; +FLASH_EraseInitTypeDef pEraseInit; +uint32_t SectorError; + + + + static CAN_message_t CAN_TX_msg; static CAN_message_t CAN_inMsg; @@ -34,6 +49,7 @@ struct MotorControlInputs { float target_angle = 0.0; float target_velocity = 0.0; bool motor_enabled = false; + bool foc_state = false; }; MotorControlInputs motor_control_inputs; @@ -109,39 +125,128 @@ void send_motor_enabled() { Can.write(CAN_TX_msg); } +void send_foc_state(){ + CAN_TX_msg.id = 1; + CAN_TX_msg.buf[0] = 'F'; + memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state, + sizeof(motor_control_inputs.foc_state)); +Can.write(CAN_TX_msg); +} + +void send_id(){ + /* data for reading of firmware */ + FLASH->KEYR = 0x45670123; // Первый ключ + FLASH->KEYR = 0xCDEF89AB; // Второй ключ + uint8_t id = *(volatile uint32_t*)ADDR_VAR_ID; + FLASH->CR |= FLASH_CR_LOCK; + CAN_TX_msg.id = id; + CAN_TX_msg.buf[0] = 0x22; + memcpy(&CAN_TX_msg.buf[1], &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; + while((FLASH->SR & FLASH_SR_BSY) != 0) + __NOP(); + FLASH->SR |= (FLASH_SR_EOP | FLASH_SR_WRPERR | FLASH_SR_PGAERR); + FLASH->CR |= FLASH_CR_PG; + *(volatile uint8_t*)ADDR_VAR_ID = my_id; + + FLASH->CR |= FLASH_CR_LOCK; + +} + + + void send_data() { send_velocity(); send_angle(); send_motor_enabled(); + // read_temperature(); digitalWrite(PC11, !digitalRead(PC11)); } -void read_can_step() { - char flag = CAN_inMsg.buf[0]; + + +/*void read_can_step() { + char flag = CAN_inMsg.buf[1]; if (flag == 'V') { motor.enable(); - memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1], + memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[2], sizeof(motor_control_inputs.target_velocity)); } else if (flag == 'A') { motor.enable(); - memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], + memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[2], sizeof(motor_control_inputs.target_angle)); } else if (flag == 'E') { - bool enable_flag = CAN_inMsg.buf[1]; + bool enable_flag = CAN_inMsg.buf[2]; if (enable_flag == 1) { - memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], + memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[2], sizeof(motor_control_inputs.motor_enabled)); motor.enable(); } else if (enable_flag == 0) { - memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], + memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[2], sizeof(motor_control_inputs.motor_enabled)); motor.disable(); } } - digitalWrite(PC10, !digitalRead(PC10)); + GPIOC->ODR ^= GPIO_ODR_OD10; //digitalWrite(PC10, !digitalRead(PC10)); +}*/ + + + + +void listen_can(){ + + uint8_t reg_id = CAN_inMsg.id; + if(CAN_inMsg.buf[0] == REG_WRITE){ + switch (reg_id) + { + case REG_ID: + /* setup new id */ + setup_id(CAN_inMsg.buf[1]); + break; + + case REG_LED_BLINK: + for(int i = 0;i < 10;i++){ + GPIOC->ODR ^= GPIO_ODR_OD10; + HAL_Delay(100); + } + break; + + default: + break; + } + } + + else if(CAN_inMsg.buf[0] == REG_READ){ + switch (reg_id) + { + case REG_ID: + send_id(); + + break; + + default: + break; + } + } + } + + + + void foc_step(BLDCMotor *motor, Commander *commander) { if (motor_control_inputs.target_velocity != 0 || motor->controller == MotionControlType::velocity) { @@ -163,7 +268,12 @@ void foc_step(BLDCMotor *motor, Commander *commander) { commander->run(); } -void setup() { + + + + + +void setup(){ Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setTx(HARDWARE_SERIAL_TX_PIN); Serial.begin(115200); @@ -180,11 +290,21 @@ void setup() { SendTimer->attachInterrupt(send_data); SendTimer->resume(); setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); + + /* Настройка параметров стирания */ + /*pEraseInit.TypeErase = TYPEERASE_SECTORS; // Стирание страниц + pEraseInit.Sector = ADDR_VAR; // Начальная страница + pEraseInit.NbSectors = 1; + pEraseInit.VoltageRange = VOLTAGE_RANGE_3;*/ +} + void loop(){ + foc_step(&motor, &command); + GPIOC->ODR ^= GPIO_ODR_OD10; + HAL_Delay(1000); + while (Can.read(CAN_inMsg)) { + listen_can(); + + } } -void loop() { - foc_step(&motor, &command); - while (Can.read(CAN_inMsg)) { - read_can_step(); - } -} + From dc445c11b74e979c09f8a8c18d44552ad808a77f Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 27 Feb 2025 20:17:48 +0300 Subject: [PATCH 02/20] git --- controller/fw/embed/include/reg_cah.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 controller/fw/embed/include/reg_cah.h diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h new file mode 100644 index 0000000..52a9102 --- /dev/null +++ b/controller/fw/embed/include/reg_cah.h @@ -0,0 +1,27 @@ +#define ADDR_VAR 0x8040000 +#define ADDR_VAR_ID ADDR_VAR +#define ADDR_VAR_P (ADDR_VAR + 1) +#define ADDR_VAR_I (ADDR_VAR + 2) +#define ADDR_VAR_D (ADDR_VAR + 3) + +#define REG_READ 0x99 +#define REG_WRITE 0xA0 + + +/* CAN REGISTER ID */ +#define REG_ID 0x01 +#define REG_BAUDRATE 0x02 + +#define REG_MOTOR_POSPID_Kp 0x30 +#define REG_MOTOR_POSPID_Ki 0x31 +#define REG_MOTOR_POSPID_Kd 0x32 + +#define REG_MOTOR_VELPID_Kp 0x40 +#define REG_MOTOR_VELPID_Ki 0x41 +#define REG_MOTOR_VELPID_Kd 0x42 + +#define REG_MOTOR_IMPPID_Kp 0x50 +#define REG_MOTOR_IMPPID_Kd 0x51 + +#define REG_RESET 0x88 +#define REG_LED_BLINK 0x8B From 119bc4f0914e9fcb3f92fc4aa4007f881f44228a Mon Sep 17 00:00:00 2001 From: lulko Date: Fri, 28 Feb 2025 10:08:55 +0300 Subject: [PATCH 03/20] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=B8?= =?UTF-8?q?=D0=BB=20=D1=82=D0=B5=D1=81=D1=82=20=D0=B4=D0=BB=D1=8F=20=D0=B7?= =?UTF-8?q?=D0=B0=D0=BF=D0=B8=D1=81=D0=B8=20=D0=B8=20=D0=BE=D0=B4=D0=BD?= =?UTF-8?q?=D0=BE=D0=B2=D1=80=D0=B5=D0=BC=D0=B5=D0=BD=D0=BD=D0=BE=20=D1=87?= =?UTF-8?q?=D1=82=D0=B5=D0=BD=D0=B8=D1=8F=20ID=20=D1=83=D1=81=D1=82=D1=80?= =?UTF-8?q?=D0=BE=D0=B9=D1=81=D1=82=D0=B2=D0=B0=20=D0=BF=D0=BE=20=20CAN?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/test/python_test_id.py | 73 ++++++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 controller/fw/embed/test/python_test_id.py diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py new file mode 100644 index 0000000..d59908c --- /dev/null +++ b/controller/fw/embed/test/python_test_id.py @@ -0,0 +1,73 @@ +import can +import time + +def send_write_read_requests(): + try: + bus = can.interface.Bus(channel='can0', bustype='socketcan') + + # Конфигурация сообщений (ЗАПОЛНИТЕ ВАШИ ЗНАЧЕНИЯ) + write_msg = { + 'arbitration_id': 0x01, # CAN ID для записи + 'data': [0xA0, 0x69, 0x00, 0x00], # Данные для записи (4 байта) + 'description': "Установка id устройства" + } + + read_msg = { + 'arbitration_id': 0x01, # CAN ID для чтения + 'data': [0x99], # Команда запроса данных + 'description': "Запрос id устройства", + 'response_id': 0x69, # Ожидаемый ID ответа + 'timeout': 1.0 # Таймаут ожидания ответа (сек) + } + + # 1. Отправка команды записи + print("Отправка команды записи...") + msg = can.Message( + arbitration_id=write_msg['arbitration_id'], + data=write_msg['data'], + is_extended_id=False + ) + bus.send(msg) + print(f"Запись: ID={hex(msg.arbitration_id)}, Данные={list(msg.data)}") + + # Ждем обработки команды устройством + time.sleep(1.5) + + # 2. Отправка запроса чтения и ожидание ответа + print("\nОтправка запроса чтения...") + msg = can.Message( + arbitration_id=read_msg['arbitration_id'], + data=read_msg['data'], + is_extended_id=False + ) + bus.send(msg) + print(f"Чтение: ID={hex(msg.arbitration_id)}, Команда={list(msg.data)}") + + # Ожидаем ответа + start_time = time.time() + response_received = False + + print("\nОжидание ответа...") + while (time.time() - start_time) < read_msg['timeout']: + response = bus.recv(timeout=0.1) + + if response and response.arbitration_id == read_msg['response_id']: + print(f"\nПолучен ответ: ID={hex(response.arbitration_id)}") + print(f"Данные: {list(response.data)}") + print(f"Длина: {response.dlc} байт") + response_received = True + break + + if not response_received: + print("\nОшибка: ответ не получен в течение заданного времени") + + except KeyboardInterrupt: + print("\nПрерывание пользователем") + except Exception as e: + print(f"Ошибка: {str(e)}") + finally: + bus.shutdown() + print("\nCAN соединение закрыто") + +if __name__ == '__main__': + send_write_read_requests() From c44edc638d1be5afdf945a50d3938528f8e846a8 Mon Sep 17 00:00:00 2001 From: lulko Date: Fri, 28 Feb 2025 11:33:01 +0300 Subject: [PATCH 04/20] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B5=20=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D1=8B?= =?UTF-8?q?=20=D1=81=20=D0=B4=D1=80=D1=83=D0=B3=D0=B8=D0=BC=D0=B8=20=D1=80?= =?UTF-8?q?=D0=B5=D0=B3=D0=B8=D1=81=D1=82=D1=80=D0=B0=D0=BC=D0=B8=20=D1=83?= =?UTF-8?q?=D1=81=D1=82=D1=80=D0=BE=D0=B9=D1=81=D1=82=D0=B2=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/src/main.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 0a7074b..c7902d7 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -223,6 +223,17 @@ void listen_can(){ } break; + case MOTOR_ENABLED: + if(CAN_inMsg.buf[1] == 1){ + motor.enable(); + motor_control_inputs.motor_enabled = 1; + } + else{ + motor.disable(); + motor_control_inputs.motor_enabled = 0; + } + + default: break; } From aa1a78117b682c1630bedec1330dd1a224853900 Mon Sep 17 00:00:00 2001 From: lulko Date: Mon, 3 Mar 2025 11:43:03 +0300 Subject: [PATCH 05/20] Fixed cpplint --- controller/fw/embed/include/reg_cah.h | 12 +++++ controller/fw/embed/src/main.cpp | 69 +++++++++++---------------- 2 files changed, 41 insertions(+), 40 deletions(-) diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index 52a9102..a8f0cf4 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.h @@ -1,3 +1,5 @@ +#ifndef REG_CAH_H_ +#define REG_CAH_H_ #define ADDR_VAR 0x8040000 #define ADDR_VAR_ID ADDR_VAR #define ADDR_VAR_P (ADDR_VAR + 1) @@ -8,6 +10,9 @@ #define REG_WRITE 0xA0 +/* Startup ID device */ +#define START_ID 0x00 + /* CAN REGISTER ID */ #define REG_ID 0x01 #define REG_BAUDRATE 0x02 @@ -25,3 +30,10 @@ #define REG_RESET 0x88 #define REG_LED_BLINK 0x8B + +#define FOC_STATE 0x60 + +#define MOTOR_VELOCITY 0x70 +#define MOTOR_ENABLED 0x71 +#define MOTOR_ANGLE 0x72 +#endif // REG_CAH_H_ diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index c7902d7..87957b9 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -125,7 +125,7 @@ void send_motor_enabled() { Can.write(CAN_TX_msg); } -void send_foc_state(){ +void send_foc_state() { CAN_TX_msg.id = 1; CAN_TX_msg.buf[0] = 'F'; memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state, @@ -133,35 +133,34 @@ void send_foc_state(){ Can.write(CAN_TX_msg); } -void send_id(){ +void send_id() { /* data for reading of firmware */ - FLASH->KEYR = 0x45670123; // Первый ключ - FLASH->KEYR = 0xCDEF89AB; // Второй ключ + FLASH->KEYR = 0x45670123; // Первый ключ + FLASH->KEYR = 0xCDEF89AB; // Второй ключ uint8_t id = *(volatile uint32_t*)ADDR_VAR_ID; FLASH->CR |= FLASH_CR_LOCK; CAN_TX_msg.id = id; CAN_TX_msg.buf[0] = 0x22; - memcpy(&CAN_TX_msg.buf[1], &id,sizeof(id)); + memcpy(&CAN_TX_msg.buf[1], &id, sizeof(id)); Can.write(CAN_TX_msg); } -void setup_id(uint8_t my_id){ +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; - while((FLASH->SR & FLASH_SR_BSY) != 0) + 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; + while ((FLASH->SR & FLASH_SR_BSY) != 0) __NOP(); FLASH->SR |= (FLASH_SR_EOP | FLASH_SR_WRPERR | FLASH_SR_PGAERR); FLASH->CR |= FLASH_CR_PG; *(volatile uint8_t*)ADDR_VAR_ID = my_id; FLASH->CR |= FLASH_CR_LOCK; - } @@ -170,7 +169,6 @@ void send_data() { send_velocity(); send_angle(); send_motor_enabled(); - // read_temperature(); digitalWrite(PC11, !digitalRead(PC11)); } @@ -205,30 +203,27 @@ void send_data() { -void listen_can(){ - +void listen_can() { uint8_t reg_id = CAN_inMsg.id; - if(CAN_inMsg.buf[0] == REG_WRITE){ - switch (reg_id) - { + if (CAN_inMsg.buf[0] == REG_WRITE) { + switch (reg_id) { case REG_ID: /* setup new id */ setup_id(CAN_inMsg.buf[1]); break; - - case REG_LED_BLINK: - for(int i = 0;i < 10;i++){ + + case REG_LED_BLINK: + for (int i = 0; i < 10; i++) { GPIOC->ODR ^= GPIO_ODR_OD10; HAL_Delay(100); } break; case MOTOR_ENABLED: - if(CAN_inMsg.buf[1] == 1){ + if (CAN_inMsg.buf[1] == 1) { motor.enable(); motor_control_inputs.motor_enabled = 1; - } - else{ + } else { motor.disable(); motor_control_inputs.motor_enabled = 0; } @@ -237,21 +232,16 @@ void listen_can(){ default: break; } - } - - else if(CAN_inMsg.buf[0] == REG_READ){ - switch (reg_id) - { +} else if (CAN_inMsg.buf[0] == REG_READ) { + switch (reg_id) { case REG_ID: - send_id(); - + send_id(); break; - + default: break; } } - } @@ -284,7 +274,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) { -void setup(){ +void setup() { Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setTx(HARDWARE_SERIAL_TX_PIN); Serial.begin(115200); @@ -308,14 +298,13 @@ void setup(){ pEraseInit.NbSectors = 1; pEraseInit.VoltageRange = VOLTAGE_RANGE_3;*/ } - void loop(){ + void loop() { foc_step(&motor, &command); GPIOC->ODR ^= GPIO_ODR_OD10; HAL_Delay(1000); while (Can.read(CAN_inMsg)) { listen_can(); - - } + } } From f02703659d2e482f4864e5d280bdf3eba274188c Mon Sep 17 00:00:00 2001 From: lulko Date: Mon, 3 Mar 2025 15:02:32 +0300 Subject: [PATCH 06/20] =?UTF-8?q?=D0=A0=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7?= =?UTF-8?q?=D0=BE=D0=B2=D0=B0=D0=BD=D0=B0=20=D0=B0=D0=B4=D1=80=D0=B5=D1=81?= =?UTF-8?q?=D0=B0=D1=86=D0=B8=D1=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/src/main.cpp | 13 +++++++------ controller/fw/embed/test/python_test_id.py | 8 ++++---- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 87957b9..1467aef 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -140,8 +140,7 @@ void send_id() { uint8_t id = *(volatile uint32_t*)ADDR_VAR_ID; FLASH->CR |= FLASH_CR_LOCK; CAN_TX_msg.id = id; - CAN_TX_msg.buf[0] = 0x22; - memcpy(&CAN_TX_msg.buf[1], &id, sizeof(id)); + memcpy(&CAN_TX_msg.buf[0], &id, sizeof(id)); Can.write(CAN_TX_msg); } @@ -205,11 +204,12 @@ void send_data() { void listen_can() { uint8_t reg_id = CAN_inMsg.id; - if (CAN_inMsg.buf[0] == REG_WRITE) { + if (CAN_inMsg.buf[0] == *(volatile uint8_t*)ADDR_VAR_ID) { + if (CAN_inMsg.buf[1] == REG_WRITE) { switch (reg_id) { case REG_ID: /* setup new id */ - setup_id(CAN_inMsg.buf[1]); + setup_id(CAN_inMsg.buf[2]); break; case REG_LED_BLINK: @@ -220,7 +220,7 @@ void listen_can() { break; case MOTOR_ENABLED: - if (CAN_inMsg.buf[1] == 1) { + if (CAN_inMsg.buf[2] == 1) { motor.enable(); motor_control_inputs.motor_enabled = 1; } else { @@ -232,7 +232,7 @@ void listen_can() { default: break; } -} else if (CAN_inMsg.buf[0] == REG_READ) { +} else if (CAN_inMsg.buf[1] == REG_READ) { switch (reg_id) { case REG_ID: send_id(); @@ -242,6 +242,7 @@ void listen_can() { break; } } + } } diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py index d59908c..8a8797d 100644 --- a/controller/fw/embed/test/python_test_id.py +++ b/controller/fw/embed/test/python_test_id.py @@ -8,15 +8,15 @@ def send_write_read_requests(): # Конфигурация сообщений (ЗАПОЛНИТЕ ВАШИ ЗНАЧЕНИЯ) write_msg = { 'arbitration_id': 0x01, # CAN ID для записи - 'data': [0xA0, 0x69, 0x00, 0x00], # Данные для записи (4 байта) + 'data': [0x27, 0xA0, 0xFF, 0x00], # Данные для записи (4 байта) 'description': "Установка id устройства" } read_msg = { 'arbitration_id': 0x01, # CAN ID для чтения - 'data': [0x99], # Команда запроса данных + 'data': [0xFF,0x99], # Адрес новый + команда запроса данных 'description': "Запрос id устройства", - 'response_id': 0x69, # Ожидаемый ID ответа + 'response_id': 0xFF, # Ожидаемый ID ответа 'timeout': 1.0 # Таймаут ожидания ответа (сек) } @@ -31,7 +31,7 @@ def send_write_read_requests(): print(f"Запись: ID={hex(msg.arbitration_id)}, Данные={list(msg.data)}") # Ждем обработки команды устройством - time.sleep(1.5) + time.sleep(2.0) # 2. Отправка запроса чтения и ожидание ответа print("\nОтправка запроса чтения...") From ca5dfc9698b3d7ed696681536ea1a930b05b9f63 Mon Sep 17 00:00:00 2001 From: lulko Date: Wed, 5 Mar 2025 16:45:21 +0300 Subject: [PATCH 07/20] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=BE=20=D1=81=D0=BC=D0=B5=D1=89=D0=B5=D0=BD=D0=B8?= =?UTF-8?q?=D0=B5=20=D0=B4=D0=BB=D1=8F=20=D0=B2=D0=B5=D0=BA=D1=82=D0=BE?= =?UTF-8?q?=D1=80=D0=BE=D0=B2=20=D0=BF=D1=80=D0=B5=D1=80=D1=8B=D0=B2=D0=B0?= =?UTF-8?q?=D0=BD=D0=B8=D0=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/include/reg_cah.h | 4 +++- controller/fw/embed/src/main.cpp | 3 +++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index a8f0cf4..61d7eac 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.h @@ -1,6 +1,8 @@ #ifndef REG_CAH_H_ #define REG_CAH_H_ -#define ADDR_VAR 0x8040000 + +#define APP_ADDR 0x08004000 // 16KB - Application +#define ADDR_VAR 0x80400000 #define ADDR_VAR_ID ADDR_VAR #define ADDR_VAR_P (ADDR_VAR + 1) #define ADDR_VAR_I (ADDR_VAR + 2) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 1467aef..28f5f32 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -276,6 +276,9 @@ void foc_step(BLDCMotor *motor, Commander *commander) { void setup() { + /* bias for vector int */ + SCB->VTOR = APP_ADDR; + Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setTx(HARDWARE_SERIAL_TX_PIN); Serial.begin(115200); From 147bad14bbe0a3b12a80d64b07fa48c4716a4f2c Mon Sep 17 00:00:00 2001 From: lulko Date: Mon, 10 Mar 2025 20:43:14 +0300 Subject: [PATCH 08/20] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B0=20=D0=B4=D0=B8=D1=80=D0=B5=D0=BA=D1=82=D0=BE?= =?UTF-8?q?=D1=80=D0=B8=D1=8F=20=D0=B1=D1=83=D1=82=D0=BB=D0=BE=D0=B0=D0=B4?= =?UTF-8?q?=D0=B5=D1=80=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/bootloader/flash.cpp | 88 ++++++++++++++++++ controller/fw/embed/bootloader/flash.h | 46 ++++++++++ controller/fw/embed/bootloader/main.cpp | 110 +++++++++++++++++++++++ 3 files changed, 244 insertions(+) create mode 100644 controller/fw/embed/bootloader/flash.cpp create mode 100644 controller/fw/embed/bootloader/flash.h create mode 100644 controller/fw/embed/bootloader/main.cpp diff --git a/controller/fw/embed/bootloader/flash.cpp b/controller/fw/embed/bootloader/flash.cpp new file mode 100644 index 0000000..186ae7b --- /dev/null +++ b/controller/fw/embed/bootloader/flash.cpp @@ -0,0 +1,88 @@ +#include "flash.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; + +} + +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 \ No newline at end of file diff --git a/controller/fw/embed/bootloader/flash.h b/controller/fw/embed/bootloader/flash.h new file mode 100644 index 0000000..c453c98 --- /dev/null +++ b/controller/fw/embed/bootloader/flash.h @@ -0,0 +1,46 @@ +#ifndef FLASH_H_ +#define FLASH_H_ +#include "stm32f446xx.h" + +// Flash sectors for STM32F407 +#define BOOT 0x08000000 // 16KB - Bootloader +#define APP_ADDR 0x08004000 // 16KB - Application +#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 +#define FLASH_KEY1 0x45670123 +#define FLASH_KEY2 0xCDEF89AB + +//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 flash_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); + + + +#endif /* FLASH_H_ */ diff --git a/controller/fw/embed/bootloader/main.cpp b/controller/fw/embed/bootloader/main.cpp new file mode 100644 index 0000000..8dbced8 --- /dev/null +++ b/controller/fw/embed/bootloader/main.cpp @@ -0,0 +1,110 @@ +#include +#include +//#include "CRC32.h" +#include "flash.h" + +//CRC + + + +// put function declarations here: +STM32_CAN Can(CAN2, DEF); +static CAN_message_t CAN_TX_msg; +static CAN_message_t CAN_inMsg; + +void buff_data_can(size_t len,size_t len_frame) { + uint8_t buff[len] = {0}; + uint32_t buff_32[len / 4] = {0}; + uint32_t bias = 0; + while(Can.read(CAN_inMsg)) { + for(size_t i = 0; i < len_frame; i++,bias++) { + buff[bias] = CAN_inMsg.buf[i]; + /* if data from len no end blink led 5 count and stop programm */ + if(bias >= len){ + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; + GPIOC->MODER |= GPIO_MODER_MODE10_0; + for(int i = 0;i < 5;i++){ + GPIOC->ODR ^= GPIO_ODR_OD10; + HAL_Delay(500); + } + return; + } + } +} + /* from 8byte to 32 byte */ + +for(size_t i = 0; i < len / 4; i++) { + buff_32[i] = (buff[i*4] << 24) | + (buff[i*4 + 1] << 16) | + (buff[i*4 + 2] << 8) | + buff[i*4 + 3]; +} + +/* CRC check */ + + + /* Work with FLASH */ + + +FLASH_UNLOCK; +uint32_t address = (uint32_t)APP_ADDR; +FLASH_32BYTE; +for(size_t i = 0; i < len; i++) { + if(i == 0) { + flash_erase_sector(1); // Erase sector 1 (APP_ADDR) + while(FLASH_BUSY); + } + + flash_program_word(address + (i * 4), buff_32[i],0); + + while(FLASH_BUSY); +} + + + +flash_lock(); + +} + + + + +void setup() { + Serial.setRx(HARDWARE_SERIAL_RX_PIN); + Serial.setTx(HARDWARE_SERIAL_TX_PIN); + Serial.begin(115200); + Can.begin(); + Can.setBaudRate(1000000); + // put your setup code here, to run once: + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; + GPIOC->MODER |= GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0;; + GPIOC->ODR |= GPIO_ODR_OD11; +} + +void loop() { + flash_program_word(FLAG_BOOT,1,1); + for(int i = 0; i < 4; i++ ) { + GPIOC->ODR ^= GPIO_ODR_OD11; + HAL_Delay(500); + } + if(FLAG_BOOT == 1){ + while (Can.read(CAN_inMsg)){ + + // buff_data_can(32000,8); + } + + + + /* Go to application */ +volatile uint32_t* appjump = (volatile uint32_t*)APP_ADDR; + +uint32_t msp_start = *(appjump); +uint32_t reset_handler = *(appjump + 1); + +pFunction start = (pFunction)reset_handler; +__set_MSP(msp_start); +start(); + } + // put your main code here, to run repeatedly: +} + From bbdf967e6d2e88431cf8387bf4bfc7a811925326 Mon Sep 17 00:00:00 2001 From: lulko Date: Tue, 11 Mar 2025 17:49:50 +0300 Subject: [PATCH 09/20] Fix CAN --- controller/fw/embed/include/reg_cah.h | 4 +-- controller/fw/embed/platformio.ini | 2 ++ controller/fw/embed/src/main.cpp | 38 ++++++++++++++++++++------- 3 files changed, 33 insertions(+), 11 deletions(-) diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index 61d7eac..2a0c51d 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.h @@ -1,8 +1,8 @@ #ifndef REG_CAH_H_ #define REG_CAH_H_ -#define APP_ADDR 0x08004000 // 16KB - Application -#define ADDR_VAR 0x80400000 +#define APP_ADDR 0x0800400 // 16KB - Application +#define ADDR_VAR 0x8040000 #define ADDR_VAR_ID ADDR_VAR #define ADDR_VAR_P (ADDR_VAR + 1) #define ADDR_VAR_I (ADDR_VAR + 2) diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini index a759816..8b6a522 100644 --- a/controller/fw/embed/platformio.ini +++ b/controller/fw/embed/platformio.ini @@ -18,10 +18,12 @@ upload_protocol = stlink debug_tool = stlink monitor_speed = 19200 monitor_parity = N + build_flags = -DSTM32F446xx -D HAL_CAN_MODULE_ENABLED -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH + lib_deps = askuric/Simple FOC@^2.3.4 pazi88/STM32_CAN@^1.1.2 diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 28f5f32..31d81ef 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -1,5 +1,6 @@ // clang-format off #include +#include "stm32f446xx.h" #include #include #include @@ -126,7 +127,8 @@ void send_motor_enabled() { } void send_foc_state() { - CAN_TX_msg.id = 1; + uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; + CAN_TX_msg.id = FOC_STATE; CAN_TX_msg.buf[0] = 'F'; memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state, sizeof(motor_control_inputs.foc_state)); @@ -135,13 +137,11 @@ Can.write(CAN_TX_msg); void send_id() { /* data for reading of firmware */ - FLASH->KEYR = 0x45670123; // Первый ключ - FLASH->KEYR = 0xCDEF89AB; // Второй ключ - uint8_t id = *(volatile uint32_t*)ADDR_VAR_ID; - FLASH->CR |= FLASH_CR_LOCK; + + uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; CAN_TX_msg.id = id; memcpy(&CAN_TX_msg.buf[0], &id, sizeof(id)); -Can.write(CAN_TX_msg); + Can.write(CAN_TX_msg); } void setup_id(uint8_t my_id) { @@ -203,8 +203,8 @@ void send_data() { void listen_can() { - uint8_t reg_id = CAN_inMsg.id; - if (CAN_inMsg.buf[0] == *(volatile uint8_t*)ADDR_VAR_ID) { + uint8_t reg_id = CAN_inMsg.buf[0]; //reg id + if (CAN_inMsg.id == *(volatile uint8_t*)ADDR_VAR_ID) { if (CAN_inMsg.buf[1] == REG_WRITE) { switch (reg_id) { case REG_ID: @@ -237,7 +237,22 @@ void listen_can() { case REG_ID: send_id(); break; + case MOTOR_VELOCITY: + send_velocity(); + break; + case MOTOR_ANGLE: + send_angle(); + break; + + case MOTOR_ENABLED: + send_motor_enabled(); + break; + + case FOC_STATE: + send_foc_state(); + break; + default: break; } @@ -277,7 +292,8 @@ void foc_step(BLDCMotor *motor, Commander *commander) { void setup() { /* bias for vector int */ - SCB->VTOR = APP_ADDR; + //SCB->VTOR = 0x08004000; + Serial.setRx(HARDWARE_SERIAL_RX_PIN); Serial.setTx(HARDWARE_SERIAL_TX_PIN); @@ -294,6 +310,10 @@ void setup() { SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz SendTimer->attachInterrupt(send_data); SendTimer->resume(); + for(int i = 0;i < 10;i++){ + GPIOC->ODR ^= GPIO_ODR_OD10; + HAL_Delay(1000); + } setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); /* Настройка параметров стирания */ From 5dbd56f200546a63b078f5c0570e3d29faf75e7b Mon Sep 17 00:00:00 2001 From: lulko Date: Wed, 12 Mar 2025 15:28:31 +0300 Subject: [PATCH 10/20] Fix algoritm FLASH --- controller/fw/embed/include/flash.h | 58 +++++++++ controller/fw/embed/include/reg_cah.h | 2 +- controller/fw/embed/src/flash.cpp | 166 ++++++++++++++++++++++++++ controller/fw/embed/src/main.cpp | 29 ++--- 4 files changed, 240 insertions(+), 15 deletions(-) create mode 100644 controller/fw/embed/include/flash.h create mode 100644 controller/fw/embed/src/flash.cpp diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h new file mode 100644 index 0000000..6551ee7 --- /dev/null +++ b/controller/fw/embed/include/flash.h @@ -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_ */ diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index 2a0c51d..ce4057a 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.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) diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp new file mode 100644 index 0000000..6c94616 --- /dev/null +++ b/controller/fw/embed/src/flash.cpp @@ -0,0 +1,166 @@ +#include "flash.h" +#include + + + +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; +} \ No newline at end of file diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 31d81ef..00e0706 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -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: From b5ff05bed6ff496626e736b827c6a9011f798c02 Mon Sep 17 00:00:00 2001 From: lulko Date: Wed, 12 Mar 2025 22:24:39 +0300 Subject: [PATCH 11/20] Algoritm for save flash(while non-work) --- controller/fw/embed/include/flash.h | 23 +++++++++++++----- controller/fw/embed/include/reg_cah.h | 2 +- controller/fw/embed/src/flash.cpp | 35 ++++++++++++++++++++------- controller/fw/embed/src/main.cpp | 22 ++++++----------- 4 files changed, 52 insertions(+), 30 deletions(-) diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index 6551ee7..e6ae270 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -7,14 +7,20 @@ /* for addr in FLASH */ typedef struct{ - uint8_t data_id; + uint8_t data_id; // data_id = id register of can uint8_t value; uint16_t crc; }FLASH_RECORD; +enum { + addr_id = 0, + foc_id = 1, +kp_id = 2, + ki_id = 3, + kd_id = 4 +}; -static uint32_t write_ptr; #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct -#define PARAM_COUNT 4 // count data in flash +#define PARAM_COUNT 3 // count data in flash // Flash sectors for STM32F407 @@ -32,7 +38,8 @@ static uint32_t write_ptr; //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 +#define FLASH_32BYTE \ + FLASH->CR = (FLASH->CR & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos) // Flash command bits #define FLASH_LOCK FLASH->CR |= FLASH_CR_LOCK @@ -52,7 +59,11 @@ 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); - - +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); +bool validate_crc(FLASH_RECORD* crc); +void flash_write(uint32_t addr, FLASH_RECORD* record); #endif /* FLASH_H_ */ diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index ce4057a..2a0c51d 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.h @@ -2,7 +2,7 @@ #define REG_CAH_H_ #define APP_ADDR 0x0800400 // 16KB - Application -#define ADDR_VAR 0x8050000 +#define ADDR_VAR 0x8040000 #define ADDR_VAR_ID ADDR_VAR #define ADDR_VAR_P (ADDR_VAR + 1) #define ADDR_VAR_I (ADDR_VAR + 2) diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp index 6c94616..ac2824d 100644 --- a/controller/fw/embed/src/flash.cpp +++ b/controller/fw/embed/src/flash.cpp @@ -1,7 +1,8 @@ #include "flash.h" #include +#include "hal_conf_extra.h" - +static uint32_t write_ptr = SECTOR_6; void flash_unlock(){ @@ -93,7 +94,7 @@ void flash_write(uint32_t addr, FLASH_RECORD* record){ FLASH->CR |= FLASH_CR_PG; for(int i = 0;i < size;i++){ - *(volatile uint32_t*)(addr + i * 4) = data[i]; + *(volatile uint32_t*)addr = data[i]; } // Clear program bit @@ -130,7 +131,7 @@ void flash_read(uint32_t addr,FLASH_RECORD* ptr){ 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;) { + for(int i = (uint32_t)SECTOR_6;i < (uint32_t)SECTOR_7;i += FLASH_RECORD_SIZE) { FLASH_RECORD rec; flash_read(i,&rec); @@ -152,15 +153,31 @@ void compact_page(){ void write_param(uint8_t param_id, uint8_t val){ - FLASH_RECORD param_flash = { - .data_id = param_id, - .value = val, - .crc = 6933 - }; + FLASH_RECORD param_flash = {param_id,val,6933}; + /* Exit data from FLASH */ if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_7) compact_page(); - flash_write((uint32_t)write_ptr,¶m_flash); + // Проверка выравнивания адреса + if (write_ptr % 4 != 0) + write_ptr += (4 - (write_ptr % 4)); // Выравнивание до 4 байт + __disable_irq(); + flash_write(write_ptr,¶m_flash); write_ptr += FLASH_RECORD_SIZE; + flash_program_word(SECTOR_7 - sizeof(uint32_t),write_ptr,0); +} + +FLASH_RECORD* load_params(){ + __disable_irq(); + static FLASH_RECORD latest[PARAM_COUNT] = {0}; + FLASH_RECORD res; + for(uint32_t addr = SECTOR_6;addr < SECTOR_7;addr +=FLASH_RECORD_SIZE) { + flash_read(addr,&res); + if (validate_crc(&res)) + latest[res.data_id] = res; + + } + __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 00e0706..24017fe 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -29,7 +29,8 @@ uint32_t SectorError; -static FLASH_RECORD flash_rec; +static FLASH_RECORD* flash_rec; +static FLASH_RECORD flash_buf[PARAM_COUNT]; static CAN_message_t CAN_TX_msg; static CAN_message_t CAN_inMsg; @@ -149,18 +150,8 @@ void send_id() { } void setup_id(uint8_t my_id) { - /* Чтобы разблокировать флэш память для записи*/ - 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); - FLASH->CR |= FLASH_CR_PG; - *(volatile uint8_t*)ADDR_VAR_ID = my_id; - - FLASH->CR |= FLASH_CR_LOCK; + write_param(addr_id,my_id); + send_id(); } @@ -204,8 +195,11 @@ void send_data() { void listen_can() { + flash_rec = load_params(); + for(int i = 0;i < PARAM_COUNT;i++) + flash_buf[i] = flash_rec[i]; uint8_t reg_id = CAN_inMsg.buf[0]; //reg id - if (CAN_inMsg.id == *(volatile uint8_t*)ADDR_VAR_ID) { + if (CAN_inMsg.id == flash_buf[addr_id].value) { if (CAN_inMsg.buf[1] == REG_WRITE) { switch (reg_id) { case REG_ID: From 023026987ccd455728dbb16176bca7084e390db4 Mon Sep 17 00:00:00 2001 From: lulko Date: Mon, 17 Mar 2025 15:43:34 +0300 Subject: [PATCH 12/20] With bootloader --- controller/fw/embed/bootloader/flash.cpp | 85 ++++++++- controller/fw/embed/bootloader/flash.h | 67 ++++--- controller/fw/embed/bootloader/main.cpp | 226 +++++++++++++---------- controller/fw/embed/bootloader/reg_cah.h | 38 ++++ controller/fw/embed/include/flash.h | 2 + controller/fw/embed/include/reg_cah.h | 5 +- controller/fw/embed/src/flash.cpp | 13 +- controller/fw/embed/src/main.cpp | 17 +- 8 files changed, 312 insertions(+), 141 deletions(-) create mode 100644 controller/fw/embed/bootloader/reg_cah.h diff --git a/controller/fw/embed/bootloader/flash.cpp b/controller/fw/embed/bootloader/flash.cpp index 186ae7b..fe988a1 100644 --- a/controller/fw/embed/bootloader/flash.cpp +++ b/controller/fw/embed/bootloader/flash.cpp @@ -1,4 +1,9 @@ #include "flash.h" +#include + + +static uint32_t write_ptr = SECTOR_6; + void flash_unlock(){ @@ -73,16 +78,78 @@ void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len){ FLASH->CR &= ~FLASH_CR_PG; } +void flash_write(uint32_t addr, FLASH_RECORD* record){ -uint8_t flash_read_word(uint32_t address){ - - // Check if address is valid - if(address < FLASH_BASE || address > FLASH_END) { - return 0; + 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(); } - // Read byte from flash memory - return *((volatile uint8_t*)address); - + // 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(); } - // Wait if flash \ No newline at end of file + + // Wait if flash + + +bool validate_crc(FLASH_RECORD* crc){ + if(crc->crc == 0x6933) + return true; + return false; +} +/* read struct from FLASH */ +void flash_read(uint32_t addr,FLASH_RECORD* ptr){ + uint32_t* flash_ptr = (uint32_t*)addr; + uint32_t* dest = (uint32_t*)ptr; + for (int i = 0; i < FLASH_RECORD_SIZE / 4; i++) { + dest[i] = flash_ptr[i]; + } +} + + +/* Поиск актуального адреса во флэш памяти */ +FLASH_RECORD load_params(){ + __disable_irq(); + FLASH_RECORD latest = {0}; + FLASH_RECORD res; + for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { + flash_read(addr,&res); + if (!validate_crc(&res)) + break; + else if(res.data_id == addr_id) { + latest = res; + } +} + + __enable_irq(); + return latest; +} +/** + * @brief Записывает страницу данных во флеш-память + * @param data: Указатель на буфер с данными + * @param len: Длина данных в байтах (должна быть кратна 4) + * @retval bool: true - успех, false - ошибка + */ + void write_flash_page(const uint8_t* data, uint16_t len) { // Добавлен const + flash_unlock(); + for (uint16_t i = 0; i < len; i += 4) { + uint32_t word; + memcpy(&word, &data[i], 4); // Безопасное копирование + HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, write_ptr + i, word); + } + flash_lock(); +} \ No newline at end of file diff --git a/controller/fw/embed/bootloader/flash.h b/controller/fw/embed/bootloader/flash.h index c453c98..a8ac795 100644 --- a/controller/fw/embed/bootloader/flash.h +++ b/controller/fw/embed/bootloader/flash.h @@ -1,31 +1,52 @@ #ifndef FLASH_H_ #define FLASH_H_ #include "stm32f446xx.h" +#include "hal_conf_extra.h" +/* for addr in FLASH */ +typedef struct{ + uint32_t write_ptr_now; + uint16_t crc; + uint8_t value; + uint8_t data_id; // data_id = id register of can + }FLASH_RECORD; +enum { + addr_id = 0 +}; + +#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct +#define PARAM_COUNT 1 // count data in flash + + + +#define FLAG_BOOT 0x08060000 // Адрес хранения флага для обновления прошивки +#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение +#define APP_ADDRESS 0x08004000 // Адрес основной прошивки +#define BOOT_CAN_ID 0x721 // CAN ID бутлоадера +#define BOOT_CAN_END 0x722 // CAN ID завершения передачи +#define DATA_CAN_ID 0x730 // CAN ID данных +#define MAX_FW_SIZE 0x3FFF // Макс. размер прошивки (256KB) + + // Flash sectors for STM32F407 -#define BOOT 0x08000000 // 16KB - Bootloader -#define APP_ADDR 0x08004000 // 16KB - Application -#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 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_6_END (SECTOR_6 + 128 * 1024) // sector 6 end + +#define SECTOR_7 0x08060000 -#define FLAG_BOOT (0x08040000 + 4) // Flash keys for unlocking flash memory -#define FLASH_KEY1 0x45670123 -#define FLASH_KEY2 0xCDEF89AB + //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 - +#define FLASH_32BYTE \ + FLASH->CR = (FLASH->CR & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos) // Flash status flags #define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY) @@ -37,10 +58,14 @@ typedef void(*pFunction)(void); // Function prototypes void flash_unlock(void); void flash_lock(void); -void flash_erase_sector(uint8_t sector); -void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len); +void erase_sector(uint8_t sector); +void flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len); +void write_flash_page(const uint8_t *data, uint16_t len); +void flash_read(uint32_t addr,FLASH_RECORD* ptr); +bool validate_crc(FLASH_RECORD* crc); uint8_t flash_read_word(uint32_t address); - - +void flash_write(uint32_t addr, FLASH_RECORD* record); +FLASH_RECORD load_params(); +//bool write_flash_page(const uint8_t* data, uint16_t len); #endif /* FLASH_H_ */ diff --git a/controller/fw/embed/bootloader/main.cpp b/controller/fw/embed/bootloader/main.cpp index 8dbced8..ee20b35 100644 --- a/controller/fw/embed/bootloader/main.cpp +++ b/controller/fw/embed/bootloader/main.cpp @@ -1,110 +1,146 @@ #include #include -//#include "CRC32.h" #include "flash.h" -//CRC - -// put function declarations here: STM32_CAN Can(CAN2, DEF); -static CAN_message_t CAN_TX_msg; -static CAN_message_t CAN_inMsg; - -void buff_data_can(size_t len,size_t len_frame) { - uint8_t buff[len] = {0}; - uint32_t buff_32[len / 4] = {0}; - uint32_t bias = 0; - while(Can.read(CAN_inMsg)) { - for(size_t i = 0; i < len_frame; i++,bias++) { - buff[bias] = CAN_inMsg.buf[i]; - /* if data from len no end blink led 5 count and stop programm */ - if(bias >= len){ - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; - GPIOC->MODER |= GPIO_MODER_MODE10_0; - for(int i = 0;i < 5;i++){ - GPIOC->ODR ^= GPIO_ODR_OD10; - HAL_Delay(500); - } - return; - } - } -} - /* from 8byte to 32 byte */ - -for(size_t i = 0; i < len / 4; i++) { - buff_32[i] = (buff[i*4] << 24) | - (buff[i*4 + 1] << 16) | - (buff[i*4 + 2] << 8) | - buff[i*4 + 3]; -} - -/* CRC check */ - - - /* Work with FLASH */ - - -FLASH_UNLOCK; -uint32_t address = (uint32_t)APP_ADDR; -FLASH_32BYTE; -for(size_t i = 0; i < len; i++) { - if(i == 0) { - flash_erase_sector(1); // Erase sector 1 (APP_ADDR) - while(FLASH_BUSY); - } - - flash_program_word(address + (i * 4), buff_32[i],0); - - while(FLASH_BUSY); -} - - - -flash_lock(); - -} - - +volatile bool fw_update = false; +volatile uint32_t fw_size = 0; +volatile uint32_t fw_crc = 0; +volatile uint32_t write_ptr = APP_ADDRESS; +static FLASH_RECORD flash_record = {0}; +// Прототипы функций +void jump_to_app(); +void process_can_message(const CAN_message_t &msg); +void erase_flash_pages(); +bool verify_firmware(); +void send_ack(uint8_t status); void setup() { - Serial.setRx(HARDWARE_SERIAL_RX_PIN); - Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); - Can.begin(); - Can.setBaudRate(1000000); - // put your setup code here, to run once: - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; - GPIOC->MODER |= GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0;; - GPIOC->ODR |= GPIO_ODR_OD11; + // Инициализация периферии + Serial.begin(115200); + Can.begin(1000000); // 1 Mbps + Can.setFilter(0,BOOT_CAN_ID,STD); + + + // Настройка GPIO + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; + GPIOC->MODER |= GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0; + GPIOC->ODR |= GPIO_ODR_OD11; + + // Проверка флага обновления + /*erase_sector(6); + flash_program_word(FLAG_BOOT,0xDEADBEEF,0); + flash_record.data_id = addr_id; + flash_record.crc = 0x6933; + flash_record.value = 0x69; + flash_record.write_ptr_now = SECTOR_6;*/ + flash_write(SECTOR_6, &flash_record); + flash_record = load_params(); + /* Добавить проверку адреса, т.е во время отправки запроса прошивки по CAN + мы сохраняем как флаг, так и аддрес устройства к которому будет обращатлься во + время прошивки */ + if(*(volatile uint32_t*)(FLAG_BOOT) == UPDATE_FLAG) { + fw_update = true; + GPIOC->ODR |= GPIO_ODR_OD10; // Индикация обновления + erase_flash_pages(); + } else { + jump_to_app(); + } + } void loop() { - flash_program_word(FLAG_BOOT,1,1); - for(int i = 0; i < 4; i++ ) { - GPIOC->ODR ^= GPIO_ODR_OD11; - HAL_Delay(500); - } - if(FLAG_BOOT == 1){ - while (Can.read(CAN_inMsg)){ - - // buff_data_can(32000,8); - } - - - - /* Go to application */ -volatile uint32_t* appjump = (volatile uint32_t*)APP_ADDR; - -uint32_t msp_start = *(appjump); -uint32_t reset_handler = *(appjump + 1); - -pFunction start = (pFunction)reset_handler; -__set_MSP(msp_start); -start(); - } - // put your main code here, to run repeatedly: + if(fw_update) { + CAN_message_t msg; + if(Can.read(msg)) { + process_can_message(msg); + } + } } +void process_can_message(const CAN_message_t &msg) { + switch(msg.id) { + case BOOT_CAN_ID: + if(msg.buf[0] == 0x01) { // Старт передачи + fw_size = *(uint32_t*)&msg.buf[1]; //размер прошивки тип 4 байта + fw_crc = *(uint32_t*)&msg.buf[5]; //crc + write_ptr = APP_ADDRESS; + send_ack(0x01); + } + break; + + case DATA_CAN_ID: // Пакет данных + if(write_ptr < (APP_ADDRESS + fw_size)) { + write_flash_page((const uint8_t*)msg.buf, msg.len); + write_ptr += msg.len; + send_ack(0x02); + } + break; + + case BOOT_CAN_END: // Завершение передачи + if(verify_firmware()) { + erase_sector(7); // Сброс флага + send_ack(0xAA); + NVIC_SystemReset(); + } else { + send_ack(0x55); + } + break; + } +} + +void jump_to_app() { + typedef void (*app_entry_t)(void); + auto app_entry = (app_entry_t)(*(volatile uint32_t*)(APP_ADDRESS + 4)); + + // SCB->VTOR = APP_ADDRESS; + __set_MSP(*(volatile uint32_t*)APP_ADDRESS); + app_entry(); +} + +void erase_flash_pages() { + FLASH_EraseInitTypeDef erase; + erase.TypeErase = FLASH_TYPEERASE_SECTORS; + erase.Sector = FLASH_SECTOR_1; + erase.NbSectors = 5; + erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; + + uint32_t error; + flash_unlock(); + HAL_FLASHEx_Erase(&erase, &error); + flash_lock(); +} + + +// CRC16 implementation for STM32 +uint16_t CalculateCRC16(const uint8_t* data, uint32_t length) { + uint16_t crc = 0xFFFF; + while (length--) { + crc ^= (uint16_t)(*data++) << 8; + for(uint8_t i = 0; i < 8; i++) { + crc = crc & 0x8000 ? (crc << 1) ^ 0x8005 : crc << 1; + } + } + return crc; +} +bool verify_firmware() { + uint32_t calculated_crc = 0; + calculated_crc = CalculateCRC16((uint8_t*)fw_crc,fw_size); + if(calculated_crc != (uint16_t)fw_crc) + return false; + // Реализация проверки CRC + // ... + // return (calculated_crc == fw_crc); + return true; +} + +void send_ack(uint8_t status) { + CAN_message_t ack; + ack.id = BOOT_CAN_ID + 2; + ack.len = 1; + ack.buf[0] = status; + Can.write(ack); +} \ No newline at end of file diff --git a/controller/fw/embed/bootloader/reg_cah.h b/controller/fw/embed/bootloader/reg_cah.h new file mode 100644 index 0000000..15efa8e --- /dev/null +++ b/controller/fw/embed/bootloader/reg_cah.h @@ -0,0 +1,38 @@ +#ifndef REG_CAH_H_ +#define REG_CAH_H_ + +#define APP_ADDR 0x0800400 // 16KB - Application +#define ADDR_VAR 0x8040000 + + +#define REG_READ 0x99 +#define REG_WRITE 0xA0 + + +/* Startup ID device */ +#define START_ID 0x00 + +/* CAN REGISTER ID */ +#define REG_ID 0x01 +#define REG_BAUDRATE 0x02 + +#define REG_MOTOR_POSPID_Kp 0x30 +#define REG_MOTOR_POSPID_Ki 0x31 +#define REG_MOTOR_POSPID_Kd 0x32 + +#define REG_MOTOR_VELPID_Kp 0x40 +#define REG_MOTOR_VELPID_Ki 0x41 +#define REG_MOTOR_VELPID_Kd 0x42 + +#define REG_MOTOR_IMPPID_Kp 0x50 +#define REG_MOTOR_IMPPID_Kd 0x51 + +#define REG_RESET 0x88 +#define REG_LED_BLINK 0x8B + +#define FOC_STATE 0x60 + +#define MOTOR_VELOCITY 0x70 +#define MOTOR_ENABLED 0x71 +#define MOTOR_ANGLE 0x72 +#endif // REG_CAH_H_ diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index e6ae270..7c17729 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -10,6 +10,7 @@ typedef struct{ uint8_t data_id; // data_id = id register of can uint8_t value; uint16_t crc; + uint32_t write_ptr_now; }FLASH_RECORD; enum { addr_id = 0, @@ -29,6 +30,7 @@ kp_id = 2, #define SECTOR_4 0x08010000 // 64KB #define SECTOR_5 0x08020000 // 128KB #define SECTOR_6 0x08040000 // 128KB +#define SECTOR_6_END (SECTOR_6 + 128 * 1024) // sector 6 end #define SECTOR_7 0x08060000 // 128KB diff --git a/controller/fw/embed/include/reg_cah.h b/controller/fw/embed/include/reg_cah.h index 2a0c51d..15efa8e 100644 --- a/controller/fw/embed/include/reg_cah.h +++ b/controller/fw/embed/include/reg_cah.h @@ -3,10 +3,7 @@ #define APP_ADDR 0x0800400 // 16KB - Application #define ADDR_VAR 0x8040000 -#define ADDR_VAR_ID ADDR_VAR -#define ADDR_VAR_P (ADDR_VAR + 1) -#define ADDR_VAR_I (ADDR_VAR + 2) -#define ADDR_VAR_D (ADDR_VAR + 3) + #define REG_READ 0x99 #define REG_WRITE 0xA0 diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp index ac2824d..bedac70 100644 --- a/controller/fw/embed/src/flash.cpp +++ b/controller/fw/embed/src/flash.cpp @@ -165,19 +165,24 @@ void write_param(uint8_t param_id, uint8_t val){ __disable_irq(); flash_write(write_ptr,¶m_flash); write_ptr += FLASH_RECORD_SIZE; - flash_program_word(SECTOR_7 - sizeof(uint32_t),write_ptr,0); + // flash_program_word(SECTOR_7 - sizeof(uint32_t),write_ptr,0); } FLASH_RECORD* load_params(){ __disable_irq(); static FLASH_RECORD latest[PARAM_COUNT] = {0}; FLASH_RECORD res; - for(uint32_t addr = SECTOR_6;addr < SECTOR_7;addr +=FLASH_RECORD_SIZE) { + for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { flash_read(addr,&res); - if (validate_crc(&res)) + if (!validate_crc(&res)) + break; + else{ latest[res.data_id] = res; - + write_ptr = addr; + } +} + __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 24017fe..41f4bc4 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -103,7 +103,7 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, void send_velocity() { float current_velocity = motor.shaftVelocity(); - uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; + uint8_t id = *(volatile uint8_t*)ADDR_VAR; CAN_TX_msg.id = id; CAN_TX_msg.buf[0] = 'V'; CAN_TX_msg.len = 5; @@ -113,7 +113,7 @@ void send_velocity() { void send_angle() { float current_angle = motor.shaftAngle(); - uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; + uint8_t id = *(volatile uint8_t*)ADDR_VAR; CAN_TX_msg.id = id; CAN_TX_msg.buf[0] = 'A'; CAN_TX_msg.len = 5; @@ -122,7 +122,7 @@ void send_angle() { } void send_motor_enabled() { - uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; + 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, @@ -131,7 +131,7 @@ void send_motor_enabled() { } void send_foc_state() { - uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; + uint8_t id = *(volatile uint8_t*)ADDR_VAR; CAN_TX_msg.id = id; CAN_TX_msg.buf[0] = 'F'; memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.foc_state, @@ -142,7 +142,7 @@ Can.write(CAN_TX_msg); void send_id() { /* data for reading of firmware */ - uint8_t id = *(volatile uint8_t*)ADDR_VAR_ID; + uint8_t id = *(volatile uint8_t*)ADDR_VAR; CAN_TX_msg.id = id; CAN_TX_msg.buf[0] = 'I'; memcpy(&CAN_TX_msg.buf[0], &id, sizeof(id)); @@ -195,9 +195,7 @@ void send_data() { void listen_can() { - flash_rec = load_params(); - for(int i = 0;i < PARAM_COUNT;i++) - flash_buf[i] = flash_rec[i]; + uint8_t reg_id = CAN_inMsg.buf[0]; //reg id if (CAN_inMsg.id == flash_buf[addr_id].value) { if (CAN_inMsg.buf[1] == REG_WRITE) { @@ -316,6 +314,9 @@ void setup() { pEraseInit.Sector = ADDR_VAR; // Начальная страница pEraseInit.NbSectors = 1; pEraseInit.VoltageRange = VOLTAGE_RANGE_3;*/ + flash_rec = load_params(); + for(int i = 0;i < PARAM_COUNT;i++) + flash_buf[i] = flash_rec[i]; } void loop() { foc_step(&motor, &command); From 1de6c1bda1fd25156ffe055e4ebd6bb0192375f9 Mon Sep 17 00:00:00 2001 From: lulko Date: Mon, 17 Mar 2025 19:08:41 +0300 Subject: [PATCH 13/20] added a description of firmware work --- controller/fw/embed/README.md | 3 + controller/fw/embed/test/python_test_boot.py | 101 +++++++++++++++++++ 2 files changed, 104 insertions(+) create mode 100644 controller/fw/embed/test/python_test_boot.py diff --git a/controller/fw/embed/README.md b/controller/fw/embed/README.md index 61796e3..65f9a3b 100644 --- a/controller/fw/embed/README.md +++ b/controller/fw/embed/README.md @@ -18,3 +18,6 @@ platformio run --target upload --environment robotroller_reborn ```bash platformio device monitor ``` +(Это на будующее тут пока что нет адресации)Для проверки работы бутлоадера нужно раскомментировать место после проверки флага обновления(строка 33 main.cpp).После чего во флэш загрузятся данные адреса и по ним будет реализована адресация. +Сначала прошиваем мк данным бутлоадером. Потом основную прошивку нужно перевести в hex формат, после чего подключаемся к мк по CAN. +1 пакет данных содержит: старт передачи = 0x01(старт передачи) + размер прошивки(32-х битное значение) + 32-х битное значение CRC. Если все норм, то мк шлет подтверждение после первого пакета данных = 0x01. После чего уже записывает прошивку msg.id выбираем для пакета данных. По окончанию записи отправляем с msg.id = BOOT_CAN_END для завершения приема бутлоадера.Тест не доделан ещё, но он хранится в test/python_test_boot.py \ No newline at end of file diff --git a/controller/fw/embed/test/python_test_boot.py b/controller/fw/embed/test/python_test_boot.py new file mode 100644 index 0000000..b8d3124 --- /dev/null +++ b/controller/fw/embed/test/python_test_boot.py @@ -0,0 +1,101 @@ +import can +import time +from intelhex import IntelHex +from crc import Calculator, Crc16 + +# Конфигурация CAN +CAN_CHANNEL = 'socketcan' +CAN_INTERFACE = 'can0' +CAN_BITRATE = 1000000 +# Параметры из заголовочного файла +BOOT_CAN_ID = 0x721 +DATA_CAN_ID = 0x730 +BOOT_CAN_END = 0x722 +ACK_CAN_ID = 0x723 + +# Конфигурация CRC16 +CRC16_POLYNOMIAL = 0x8005 # Стандартный полином CRC-16-IBM +CRC16_INIT = 0xFFFF + +def send_firmware(hex_file): + bus = can.interface.Bus(channel='can0', + bustype='socketcan') + # Чтение и преобразование HEX-файла + ih = IntelHex(hex_file) + binary_data = ih.tobinarray() + fw_size = len(binary_data) + + # Расчет CRC16 + # calculator = Calculator(Crc16.CCITT, optimize=True) + fw_crc = 0x6933 + + # Отправка команды START + start_data = bytearray([0x01]) + start_data += fw_size.to_bytes(4, 'little') + start_data += fw_crc.to_bytes(2, 'little') # 2 байта для CRC16 + + start_msg = can.Message( + arbitration_id=BOOT_CAN_ID, + data=start_data, + is_extended_id=False + ) + + bus.send(start_msg) + + # Ожидание подтверждения + ack = wait_for_ack(bus) + if not ack or ack.data[0] != 0x01: + print("Ошибка инициализации!") + bus.shutdown() + return + + # Отправка данных + packet_size = 8 + for i in range(0, len(binary_data), packet_size): + chunk = binary_data[i:i+packet_size] + chunk += b'\x00' * (8 - len(chunk)) + + data_msg = can.Message( + arbitration_id=DATA_CAN_ID, + data=chunk, + is_extended_id=False + ) + bus.send(data_msg) + + ack = wait_for_ack(bus) + if not ack or ack.data[0] != 0x02: + print("Ошибка передачи данных!") + break + + progress = (i + len(chunk)) / fw_size * 100 + print(f"\rПрогресс: {progress:.1f}%", end='') + + # Завершение передачи + finish_msg = can.Message( + arbitration_id=BOOT_CAN_END, + data=[0xAA], + is_extended_id=False + ) + bus.send(finish_msg) + + ack = wait_for_ack(bus, timeout=5) + if ack and ack.data[0] == 0xAA: + print("\nПрошивка успешно загружена!") + else: + print("\nОшибка верификации!") + + bus.shutdown() + +def wait_for_ack(bus, timeout=1.0): + start_time = time.time() + while time.time() - start_time < timeout: + msg = bus.recv(timeout=0.1) + if msg and msg.arbitration_id == ACK_CAN_ID: + return msg + return None + +if __name__ == "__main__": + import sys + if len(sys.argv) != 2: + print("Использование: python can_flasher.py firmware.hex") + sys.exit(1) From 4b543e78cef4e950e2c28339a01ac99373ef40d9 Mon Sep 17 00:00:00 2001 From: lulko Date: Wed, 19 Mar 2025 18:56:00 +0300 Subject: [PATCH 14/20] =?UTF-8?q?=D0=92=D0=BE=D0=B7=D0=BC=D0=BE=D0=B6?= =?UTF-8?q?=D0=BD=D0=B0=20=D0=B7=D0=B0=D0=B3=D1=80=D1=83=D0=B7=D0=BA=D0=B0?= =?UTF-8?q?=20=D0=BF=D1=80=D0=BE=D1=88=D0=B8=D0=B2=D0=BA=D0=B8,=20=D0=BD?= =?UTF-8?q?=D0=BE=20=D0=BD=D0=B5=20=D0=BF=D0=B5=D1=80=D0=B5=D1=85=D0=BE?= =?UTF-8?q?=D0=B4=D0=B8=D1=82=20=D0=B2=20=D0=BD=D0=B5=D1=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/bootloader/flash.h | 13 +- controller/fw/embed/bootloader/main.cpp | 80 ++++++--- controller/fw/embed/test/python_test_boot.py | 169 +++++++++++-------- 3 files changed, 160 insertions(+), 102 deletions(-) diff --git a/controller/fw/embed/bootloader/flash.h b/controller/fw/embed/bootloader/flash.h index a8ac795..fe9b413 100644 --- a/controller/fw/embed/bootloader/flash.h +++ b/controller/fw/embed/bootloader/flash.h @@ -13,6 +13,8 @@ enum { addr_id = 0 }; + + #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct #define PARAM_COUNT 1 // count data in flash @@ -20,10 +22,11 @@ enum { #define FLAG_BOOT 0x08060000 // Адрес хранения флага для обновления прошивки #define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение -#define APP_ADDRESS 0x08004000 // Адрес основной прошивки -#define BOOT_CAN_ID 0x721 // CAN ID бутлоадера -#define BOOT_CAN_END 0x722 // CAN ID завершения передачи -#define DATA_CAN_ID 0x730 // CAN ID данных +#define APP_ADDRESS 0x08008000 // Адрес основной прошивки +#define BOOT_CAN_ID 0x71 // CAN ID бутлоадера +#define BOOT_CAN_END 0x72 // CAN ID завершения передачи +#define DATA_CAN_ID 0x73 // CAN ID данных +#define ACK_CAN_ID 0x75 // CAN ID подтверждения #define MAX_FW_SIZE 0x3FFF // Макс. размер прошивки (256KB) @@ -37,7 +40,7 @@ enum { #define SECTOR_6 0x08040000 // 128KB #define SECTOR_6_END (SECTOR_6 + 128 * 1024) // sector 6 end -#define SECTOR_7 0x08060000 + // Flash keys for unlocking flash memory diff --git a/controller/fw/embed/bootloader/main.cpp b/controller/fw/embed/bootloader/main.cpp index ee20b35..ae8a7dd 100644 --- a/controller/fw/embed/bootloader/main.cpp +++ b/controller/fw/embed/bootloader/main.cpp @@ -5,11 +5,13 @@ STM32_CAN Can(CAN2, DEF); + volatile bool fw_update = false; volatile uint32_t fw_size = 0; volatile uint32_t fw_crc = 0; -volatile uint32_t write_ptr = APP_ADDRESS; + static FLASH_RECORD flash_record = {0}; +static uint32_t ptr_flash; // Прототипы функций void jump_to_app(); void process_can_message(const CAN_message_t &msg); @@ -20,10 +22,18 @@ void send_ack(uint8_t status); void setup() { // Инициализация периферии + Serial.setRx(HARDWARE_SERIAL_RX_PIN); + Serial.setTx(HARDWARE_SERIAL_TX_PIN); Serial.begin(115200); - Can.begin(1000000); // 1 Mbps - Can.setFilter(0,BOOT_CAN_ID,STD); - + Can.begin(); + Can.setBaudRate(1000000); + TIM_TypeDef *Instance = TIM2; + HardwareTimer *SendTimer = new HardwareTimer(Instance); + SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz + // SendTimer->attachInterrupt(process_can_message); + SendTimer->resume(); + // Разрешить все ID (маска 0x00000000) + Can.setFilter(0, 0, STD); // Настройка GPIO RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; @@ -31,17 +41,18 @@ void setup() { GPIOC->ODR |= GPIO_ODR_OD11; // Проверка флага обновления - /*erase_sector(6); + /* erase_sector(6); flash_program_word(FLAG_BOOT,0xDEADBEEF,0); flash_record.data_id = addr_id; flash_record.crc = 0x6933; flash_record.value = 0x69; - flash_record.write_ptr_now = SECTOR_6;*/ - flash_write(SECTOR_6, &flash_record); - flash_record = load_params(); + flash_record.write_ptr_now = SECTOR_6; + flash_write(SECTOR_6, &flash_record);*/ + flash_record = load_params(); /* Добавить проверку адреса, т.е во время отправки запроса прошивки по CAN мы сохраняем как флаг, так и аддрес устройства к которому будет обращатлься во время прошивки */ + if(*(volatile uint32_t*)(FLAG_BOOT) == UPDATE_FLAG) { fw_update = true; GPIOC->ODR |= GPIO_ODR_OD10; // Индикация обновления @@ -54,11 +65,14 @@ void setup() { void loop() { if(fw_update) { + GPIOC->ODR ^= GPIO_ODR_OD10; + HAL_Delay(100); CAN_message_t msg; - if(Can.read(msg)) { + while(Can.read(msg)) { process_can_message(msg); } } + } void process_can_message(const CAN_message_t &msg) { @@ -66,16 +80,16 @@ void process_can_message(const CAN_message_t &msg) { case BOOT_CAN_ID: if(msg.buf[0] == 0x01) { // Старт передачи fw_size = *(uint32_t*)&msg.buf[1]; //размер прошивки тип 4 байта - fw_crc = *(uint32_t*)&msg.buf[5]; //crc - write_ptr = APP_ADDRESS; + fw_crc = *(uint16_t*)&msg.buf[5]; //crc + ptr_flash = APP_ADDRESS; send_ack(0x01); } break; case DATA_CAN_ID: // Пакет данных - if(write_ptr < (APP_ADDRESS + fw_size)) { + if(ptr_flash < (APP_ADDRESS + fw_size)) { write_flash_page((const uint8_t*)msg.buf, msg.len); - write_ptr += msg.len; + ptr_flash += msg.len; send_ack(0x02); } break; @@ -86,26 +100,32 @@ void process_can_message(const CAN_message_t &msg) { send_ack(0xAA); NVIC_SystemReset(); } else { + erase_sector(7); // Сброс флага send_ack(0x55); + NVIC_SystemReset(); } break; } } void jump_to_app() { + volatile uint32_t* addr; + uint32_t appjump_addr; typedef void (*app_entry_t)(void); - auto app_entry = (app_entry_t)(*(volatile uint32_t*)(APP_ADDRESS + 4)); + addr = (__IO uint32_t*)APP_ADDRESS; + appjump_addr = (uint32_t)addr; + app_entry_t app_entry = (app_entry_t)appjump_addr; // SCB->VTOR = APP_ADDRESS; - __set_MSP(*(volatile uint32_t*)APP_ADDRESS); + __set_MSP(appjump_addr); app_entry(); } void erase_flash_pages() { FLASH_EraseInitTypeDef erase; erase.TypeErase = FLASH_TYPEERASE_SECTORS; - erase.Sector = FLASH_SECTOR_1; - erase.NbSectors = 5; + erase.Sector = FLASH_SECTOR_2; + erase.NbSectors = 4; erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; uint32_t error; @@ -117,30 +137,38 @@ void erase_flash_pages() { // CRC16 implementation for STM32 uint16_t CalculateCRC16(const uint8_t* data, uint32_t length) { - uint16_t crc = 0xFFFF; + uint16_t crc = 0xFFFF; // Начальное значение while (length--) { - crc ^= (uint16_t)(*data++) << 8; - for(uint8_t i = 0; i < 8; i++) { - crc = crc & 0x8000 ? (crc << 1) ^ 0x8005 : crc << 1; + crc ^= *data++; // Обрабатываем LSB первым + for (uint8_t i = 0; i < 8; i++) { + if (crc & 0x0001) { // Проверяем младший бит + crc = (crc >> 1) ^ 0xA001; // Полином 0x8005 (reverse) + } else { + crc >>= 1; + } } } - return crc; + return crc; // Финальный XOR = 0x0000 (не требуется) } + bool verify_firmware() { uint32_t calculated_crc = 0; - calculated_crc = CalculateCRC16((uint8_t*)fw_crc,fw_size); + calculated_crc = CalculateCRC16((uint8_t*)APP_ADDRESS,fw_size); if(calculated_crc != (uint16_t)fw_crc) return false; // Реализация проверки CRC // ... // return (calculated_crc == fw_crc); - return true; + else + return true; } void send_ack(uint8_t status) { + CAN_message_t ack; - ack.id = BOOT_CAN_ID + 2; + ack.id = ACK_CAN_ID; ack.len = 1; ack.buf[0] = status; Can.write(ack); -} \ No newline at end of file +} + diff --git a/controller/fw/embed/test/python_test_boot.py b/controller/fw/embed/test/python_test_boot.py index b8d3124..918095f 100644 --- a/controller/fw/embed/test/python_test_boot.py +++ b/controller/fw/embed/test/python_test_boot.py @@ -2,94 +2,119 @@ import can import time from intelhex import IntelHex from crc import Calculator, Crc16 - -# Конфигурация CAN +# Конфигурация CAN_CHANNEL = 'socketcan' CAN_INTERFACE = 'can0' CAN_BITRATE = 1000000 -# Параметры из заголовочного файла -BOOT_CAN_ID = 0x721 -DATA_CAN_ID = 0x730 -BOOT_CAN_END = 0x722 -ACK_CAN_ID = 0x723 +BOOT_CAN_ID = 0x71 +DATA_CAN_ID = 0x73 +BOOT_CAN_END = 0x72 +ACK_CAN_ID = 0x75 -# Конфигурация CRC16 -CRC16_POLYNOMIAL = 0x8005 # Стандартный полином CRC-16-IBM -CRC16_INIT = 0xFFFF +#конфиг для crc16 ibm + + + +def debug_print(msg): + print(f"[DEBUG] {msg}") def send_firmware(hex_file): - bus = can.interface.Bus(channel='can0', - bustype='socketcan') - # Чтение и преобразование HEX-файла - ih = IntelHex(hex_file) - binary_data = ih.tobinarray() - fw_size = len(binary_data) - - # Расчет CRC16 - # calculator = Calculator(Crc16.CCITT, optimize=True) - fw_crc = 0x6933 - - # Отправка команды START - start_data = bytearray([0x01]) - start_data += fw_size.to_bytes(4, 'little') - start_data += fw_crc.to_bytes(2, 'little') # 2 байта для CRC16 - - start_msg = can.Message( - arbitration_id=BOOT_CAN_ID, - data=start_data, - is_extended_id=False - ) - - bus.send(start_msg) - - # Ожидание подтверждения - ack = wait_for_ack(bus) - if not ack or ack.data[0] != 0x01: - print("Ошибка инициализации!") - bus.shutdown() - return - - # Отправка данных - packet_size = 8 - for i in range(0, len(binary_data), packet_size): - chunk = binary_data[i:i+packet_size] - chunk += b'\x00' * (8 - len(chunk)) + try: + debug_print("Инициализация CAN...") + bus = can.interface.Bus( + channel=CAN_INTERFACE, + bustype=CAN_CHANNEL, + bitrate=CAN_BITRATE + ) - data_msg = can.Message( - arbitration_id=DATA_CAN_ID, - data=chunk, + debug_print("Чтение HEX-файла...") + ih = IntelHex(hex_file) + binary_data = ih.tobinstr() # Исправлено на tobinstr() + fw_size = len(binary_data) + debug_print(f"Размер прошивки: {fw_size} байт") + + # Расчет CRC + debug_print("Расчёт CRC...") + calculator = Calculator(Crc16.IBM) + fw_crc = calculator.checksum(binary_data) + debug_print(f"CRC: 0x{fw_crc:04X}") + + # Отправка START + start_data = bytearray([0x01]) + start_data += fw_size.to_bytes(4, 'little') + start_data += fw_crc.to_bytes(2, 'little') + + debug_print(f"START: {list(start_data)}") + start_msg = can.Message( + arbitration_id=BOOT_CAN_ID, + data=bytes(start_data), is_extended_id=False ) - bus.send(data_msg) + try: + bus.send(start_msg) + except can.CanError as e: + debug_print(f"Ошибка отправки START: {str(e)}") + return + + # Ожидание ACK + debug_print("Ожидание ACK...") ack = wait_for_ack(bus) - if not ack or ack.data[0] != 0x02: - print("Ошибка передачи данных!") - break + if not ack: + debug_print("Таймаут ACK START") + return + debug_print(f"Получен ACK: {list(ack.data)}") + + # Отправка данных + packet_size = 8 + for i in range(0, len(binary_data), packet_size): + chunk = binary_data[i:i+packet_size] + # Дополнение до 8 байт + if len(chunk) < 8: + chunk += b'\xFF' * (8 - len(chunk)) - progress = (i + len(chunk)) / fw_size * 100 - print(f"\rПрогресс: {progress:.1f}%", end='') + debug_print(f"Пакет {i//8}: {list(chunk)}") + data_msg = can.Message( + arbitration_id=DATA_CAN_ID, + data=chunk, + is_extended_id=False + ) + + try: + bus.send(data_msg) + except can.CanError as e: + debug_print(f"Ошибка отправки данных: {str(e)}") + return - # Завершение передачи - finish_msg = can.Message( - arbitration_id=BOOT_CAN_END, - data=[0xAA], - is_extended_id=False - ) - bus.send(finish_msg) - - ack = wait_for_ack(bus, timeout=5) - if ack and ack.data[0] == 0xAA: - print("\nПрошивка успешно загружена!") - else: - print("\nОшибка верификации!") + ack = wait_for_ack(bus) + if not ack: + debug_print("Таймаут ACK DATA") + return - bus.shutdown() + # Финал + debug_print("Отправка FINISH...") + finish_msg = can.Message( + arbitration_id=BOOT_CAN_END, + data=bytes([0xAA]), + is_extended_id=False + ) + bus.send(finish_msg) + + ack = wait_for_ack(bus, timeout=5) + if ack == 0xAA: + debug_print("Прошивка подтверждена!") + else: + debug_print("Ошибка верификации!") + + except Exception as e: + debug_print(f"Критическая ошибка: {str(e)}") + finally: + bus.shutdown() def wait_for_ack(bus, timeout=1.0): start_time = time.time() while time.time() - start_time < timeout: - msg = bus.recv(timeout=0.1) + msg = bus.recv(timeout=0) # Неблокирующий режим if msg and msg.arbitration_id == ACK_CAN_ID: return msg return None @@ -97,5 +122,7 @@ def wait_for_ack(bus, timeout=1.0): if __name__ == "__main__": import sys if len(sys.argv) != 2: - print("Использование: python can_flasher.py firmware.hex") + print("Использование: sudo python3 can_flasher.py firmware.hex") sys.exit(1) + + send_firmware(sys.argv[1]) From a1bcbdb33b8b507df4a017823f196926fb79430d Mon Sep 17 00:00:00 2001 From: lulko Date: Fri, 21 Mar 2025 14:00:00 +0300 Subject: [PATCH 15/20] Bootloader CAN work --- controller/fw/embed/bootloader/main.cpp | 56 ++++++++++++++----------- 1 file changed, 31 insertions(+), 25 deletions(-) diff --git a/controller/fw/embed/bootloader/main.cpp b/controller/fw/embed/bootloader/main.cpp index ae8a7dd..3bd17fc 100644 --- a/controller/fw/embed/bootloader/main.cpp +++ b/controller/fw/embed/bootloader/main.cpp @@ -9,14 +9,13 @@ STM32_CAN Can(CAN2, DEF); volatile bool fw_update = false; volatile uint32_t fw_size = 0; volatile uint32_t fw_crc = 0; - +volatile uint32_t jump; static FLASH_RECORD flash_record = {0}; static uint32_t ptr_flash; // Прототипы функций void jump_to_app(); void process_can_message(const CAN_message_t &msg); void erase_flash_pages(); - bool verify_firmware(); void send_ack(uint8_t status); @@ -33,7 +32,7 @@ void setup() { // SendTimer->attachInterrupt(process_can_message); SendTimer->resume(); // Разрешить все ID (маска 0x00000000) - Can.setFilter(0, 0, STD); + Can.setFilter(0, 0, STD); // Настройка GPIO RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; @@ -41,13 +40,13 @@ void setup() { GPIOC->ODR |= GPIO_ODR_OD11; // Проверка флага обновления - /* erase_sector(6); - flash_program_word(FLAG_BOOT,0xDEADBEEF,0); - flash_record.data_id = addr_id; - flash_record.crc = 0x6933; - flash_record.value = 0x69; - flash_record.write_ptr_now = SECTOR_6; - flash_write(SECTOR_6, &flash_record);*/ + // erase_sector(6); + // flash_program_word(FLAG_BOOT,0xDEADBEEF,0); + // flash_record.data_id = addr_id; + // flash_record.crc = 0x6933; + // flash_record.value = 0x69; + // flash_record.write_ptr_now = SECTOR_6; + // flash_write(SECTOR_6, &flash_record); flash_record = load_params(); /* Добавить проверку адреса, т.е во время отправки запроса прошивки по CAN мы сохраняем как флаг, так и аддрес устройства к которому будет обращатлься во @@ -66,14 +65,14 @@ void setup() { void loop() { if(fw_update) { GPIOC->ODR ^= GPIO_ODR_OD10; - HAL_Delay(100); + // HAL_Delay(100); CAN_message_t msg; - while(Can.read(msg)) { + while(Can.read(msg)) process_can_message(msg); } } -} + void process_can_message(const CAN_message_t &msg) { switch(msg.id) { @@ -91,8 +90,10 @@ void process_can_message(const CAN_message_t &msg) { write_flash_page((const uint8_t*)msg.buf, msg.len); ptr_flash += msg.len; send_ack(0x02); + } break; + case BOOT_CAN_END: // Завершение передачи if(verify_firmware()) { @@ -100,24 +101,27 @@ void process_can_message(const CAN_message_t &msg) { send_ack(0xAA); NVIC_SystemReset(); } else { - erase_sector(7); // Сброс флага send_ack(0x55); - NVIC_SystemReset(); } break; } } + + void jump_to_app() { - volatile uint32_t* addr; - uint32_t appjump_addr; - typedef void (*app_entry_t)(void); - addr = (__IO uint32_t*)APP_ADDRESS; - appjump_addr = (uint32_t)addr; - app_entry_t app_entry = (app_entry_t)appjump_addr; - - // SCB->VTOR = APP_ADDRESS; - __set_MSP(appjump_addr); + __disable_irq(); + jump = *(volatile uint32_t*)(APP_ADDRESS + 4); + void (*app_entry)(void); + app_entry = (void (*)(void))jump; + + + for (uint32_t i = 0; i < 3; i++) { + NVIC->ICPR[i] = 0xFFFFFFFF; + } + + __set_MSP(*(volatile uint32_t*)APP_ADDRESS); + SCB->VTOR = APP_ADDRESS; app_entry(); } @@ -135,7 +139,8 @@ void erase_flash_pages() { } -// CRC16 implementation for STM32 + + uint16_t CalculateCRC16(const uint8_t* data, uint32_t length) { uint16_t crc = 0xFFFF; // Начальное значение while (length--) { @@ -151,6 +156,7 @@ uint16_t CalculateCRC16(const uint8_t* data, uint32_t length) { return crc; // Финальный XOR = 0x0000 (не требуется) } + bool verify_firmware() { uint32_t calculated_crc = 0; calculated_crc = CalculateCRC16((uint8_t*)APP_ADDRESS,fw_size); From cf1c6eb05c399237943299e100a69e735e8da928 Mon Sep 17 00:00:00 2001 From: lulko Date: Sat, 22 Mar 2025 13:56:28 +0300 Subject: [PATCH 16/20] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B0=20=D0=B0=D0=B4=D1=80=D0=B5=D1=81=D0=B0=D1=86?= =?UTF-8?q?=D0=B8=D1=8F=20=D0=BA=20=D0=B1=D1=83=D1=82=D0=BB=D0=BE=D0=B0?= =?UTF-8?q?=D0=B4=D0=B5=D1=80=D1=83?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/bootloader/flash.cpp | 1 + controller/fw/embed/bootloader/flash.h | 8 ++--- controller/fw/embed/bootloader/main.cpp | 32 ++++++++++++++------ controller/fw/embed/test/python_test_boot.py | 30 +++++++++++++----- 4 files changed, 50 insertions(+), 21 deletions(-) diff --git a/controller/fw/embed/bootloader/flash.cpp b/controller/fw/embed/bootloader/flash.cpp index fe988a1..d67b634 100644 --- a/controller/fw/embed/bootloader/flash.cpp +++ b/controller/fw/embed/bootloader/flash.cpp @@ -130,6 +130,7 @@ FLASH_RECORD load_params(){ flash_read(addr,&res); if (!validate_crc(&res)) break; +/* нашли адрес */ else if(res.data_id == addr_id) { latest = res; } diff --git a/controller/fw/embed/bootloader/flash.h b/controller/fw/embed/bootloader/flash.h index fe9b413..0a9d699 100644 --- a/controller/fw/embed/bootloader/flash.h +++ b/controller/fw/embed/bootloader/flash.h @@ -23,10 +23,10 @@ enum { #define FLAG_BOOT 0x08060000 // Адрес хранения флага для обновления прошивки #define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение #define APP_ADDRESS 0x08008000 // Адрес основной прошивки -#define BOOT_CAN_ID 0x71 // CAN ID бутлоадера -#define BOOT_CAN_END 0x72 // CAN ID завершения передачи -#define DATA_CAN_ID 0x73 // CAN ID данных -#define ACK_CAN_ID 0x75 // CAN ID подтверждения +#define BOOT_CAN_ID 0x01 // CAN ID бутлоадера +#define BOOT_CAN_END 0x02 // CAN ID завершения передачи +#define DATA_CAN_ID 0x03 // CAN ID данных +#define ACK_CAN_ID 0x05 // CAN ID подтверждения #define MAX_FW_SIZE 0x3FFF // Макс. размер прошивки (256KB) diff --git a/controller/fw/embed/bootloader/main.cpp b/controller/fw/embed/bootloader/main.cpp index 3bd17fc..a97062b 100644 --- a/controller/fw/embed/bootloader/main.cpp +++ b/controller/fw/embed/bootloader/main.cpp @@ -12,6 +12,11 @@ volatile uint32_t fw_crc = 0; volatile uint32_t jump; static FLASH_RECORD flash_record = {0}; static uint32_t ptr_flash; + +volatile uint32_t msg_id; +volatile uint16_t id_x; +volatile uint8_t msg_ch; + // Прототипы функций void jump_to_app(); void process_can_message(const CAN_message_t &msg); @@ -32,8 +37,8 @@ void setup() { // SendTimer->attachInterrupt(process_can_message); SendTimer->resume(); // Разрешить все ID (маска 0x00000000) - Can.setFilter(0, 0, STD); - + Can.setFilter(0, 0, STD); + // Настройка GPIO RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; GPIOC->MODER |= GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0; @@ -54,12 +59,15 @@ void setup() { if(*(volatile uint32_t*)(FLAG_BOOT) == UPDATE_FLAG) { fw_update = true; - GPIOC->ODR |= GPIO_ODR_OD10; // Индикация обновления + for(int i = 0; i < 5;i++){ + GPIOC->ODR ^= GPIO_ODR_OD10; // Индикация обновления + HAL_Delay(100); + } erase_flash_pages(); } else { jump_to_app(); } - + } void loop() { @@ -75,7 +83,15 @@ void loop() { void process_can_message(const CAN_message_t &msg) { - switch(msg.id) { + msg_id = msg.id; + /* 0x691 + 69 - адрес устройства + 1 - что делать дальше с данными */ + + id_x = (msg_id >> 4) & 0xFFFF; //получение адреса устройства страшие 2 бита + msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать + if(id_x == flash_record.value){ + switch(msg_ch) { case BOOT_CAN_ID: if(msg.buf[0] == 0x01) { // Старт передачи fw_size = *(uint32_t*)&msg.buf[1]; //размер прошивки тип 4 байта @@ -97,8 +113,8 @@ void process_can_message(const CAN_message_t &msg) { case BOOT_CAN_END: // Завершение передачи if(verify_firmware()) { - erase_sector(7); // Сброс флага send_ack(0xAA); + erase_sector(7); // Сброс флага NVIC_SystemReset(); } else { send_ack(0x55); @@ -106,8 +122,7 @@ void process_can_message(const CAN_message_t &msg) { break; } } - - +} void jump_to_app() { __disable_irq(); @@ -177,4 +192,3 @@ void send_ack(uint8_t status) { ack.buf[0] = status; Can.write(ack); } - diff --git a/controller/fw/embed/test/python_test_boot.py b/controller/fw/embed/test/python_test_boot.py index 918095f..3f7522c 100644 --- a/controller/fw/embed/test/python_test_boot.py +++ b/controller/fw/embed/test/python_test_boot.py @@ -1,4 +1,5 @@ import can +import sys import time from intelhex import IntelHex from crc import Calculator, Crc16 @@ -6,10 +7,12 @@ from crc import Calculator, Crc16 CAN_CHANNEL = 'socketcan' CAN_INTERFACE = 'can0' CAN_BITRATE = 1000000 -BOOT_CAN_ID = 0x71 -DATA_CAN_ID = 0x73 -BOOT_CAN_END = 0x72 -ACK_CAN_ID = 0x75 +#ch =int(input("Введите id устройства:")) +ch = int(sys.argv[2]) +BOOT_CAN_ID = (ch * 16) + 1 +DATA_CAN_ID = (ch * 16) + 3 +BOOT_CAN_END = (ch * 16) + 2 +ACK_CAN_ID = 0x05 #конфиг для crc16 ibm @@ -18,6 +21,17 @@ ACK_CAN_ID = 0x75 def debug_print(msg): print(f"[DEBUG] {msg}") +def calculate_crc16_modbus(data: bytes) -> int: + crc = 0xFFFF + for byte in data: + crc ^= byte + for _ in range(8): + if crc & 0x0001: + crc = (crc >> 1) ^ 0xA001 + else: + crc >>= 1 + return crc + def send_firmware(hex_file): try: debug_print("Инициализация CAN...") @@ -36,7 +50,7 @@ def send_firmware(hex_file): # Расчет CRC debug_print("Расчёт CRC...") calculator = Calculator(Crc16.IBM) - fw_crc = calculator.checksum(binary_data) + fw_crc = calculate_crc16_modbus(binary_data) debug_print(f"CRC: 0x{fw_crc:04X}") # Отправка START @@ -100,8 +114,8 @@ def send_firmware(hex_file): ) bus.send(finish_msg) - ack = wait_for_ack(bus, timeout=5) - if ack == 0xAA: + ack = wait_for_ack(bus, timeout=3) + if ack and ack.data[0] == 0xAA: debug_print("Прошивка подтверждена!") else: debug_print("Ошибка верификации!") @@ -121,7 +135,7 @@ def wait_for_ack(bus, timeout=1.0): if __name__ == "__main__": import sys - if len(sys.argv) != 2: + if len(sys.argv) != 3: print("Использование: sudo python3 can_flasher.py firmware.hex") sys.exit(1) From add1a3082765e5468e4d07dcdbefede8295f3d34 Mon Sep 17 00:00:00 2001 From: lulko Date: Sat, 22 Mar 2025 22:21:21 +0300 Subject: [PATCH 17/20] =?UTF-8?q?=D0=98=D0=BD=D1=81=D1=82=D1=80=D1=83?= =?UTF-8?q?=D0=BA=D1=86=D0=B8=D1=8F=20=D0=BA=20=D0=BF=D1=80=D0=BE=D1=88?= =?UTF-8?q?=D0=B8=D0=B2=D0=BA=D0=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/README.md | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/controller/fw/embed/README.md b/controller/fw/embed/README.md index 65f9a3b..9270225 100644 --- a/controller/fw/embed/README.md +++ b/controller/fw/embed/README.md @@ -18,6 +18,16 @@ platformio run --target upload --environment robotroller_reborn ```bash platformio device monitor ``` -(Это на будующее тут пока что нет адресации)Для проверки работы бутлоадера нужно раскомментировать место после проверки флага обновления(строка 33 main.cpp).После чего во флэш загрузятся данные адреса и по ним будет реализована адресация. -Сначала прошиваем мк данным бутлоадером. Потом основную прошивку нужно перевести в hex формат, после чего подключаемся к мк по CAN. -1 пакет данных содержит: старт передачи = 0x01(старт передачи) + размер прошивки(32-х битное значение) + 32-х битное значение CRC. Если все норм, то мк шлет подтверждение после первого пакета данных = 0x01. После чего уже записывает прошивку msg.id выбираем для пакета данных. По окончанию записи отправляем с msg.id = BOOT_CAN_END для завершения приема бутлоадера.Тест не доделан ещё, но он хранится в test/python_test_boot.py \ No newline at end of file +Чтобы отправить прошивку по can нужно: +1) сместить в линкере прошивки её адрес. Иoите файл с .ld и меняете область FLASH на 0x08008000 и соотвественно занимаемая память = 480K +пример: + MEMORY +{ + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x8008000, LENGTH = 480K +} + +меняем и в FLASH.ld и в RAM.ld +2) скомплиировать в hex формате +3) в терминале прописать: python3 boot_test.py /*адрес на hex файл*/ /*адрес устройства*/ +4) прошивка после чего микроконтроллер автоматически перезагрузится и будет уже запускаться с вашей прошивки, минуя бутлоадер \ No newline at end of file From f928f03112c8894c497478a372dd9a64f5d59a0d Mon Sep 17 00:00:00 2001 From: Igor Brylev Date: Mon, 24 Mar 2025 14:01:36 +0300 Subject: [PATCH 18/20] readme rewrite fix fix --- controller/fw/embed/README.md | 61 ++++++++++++++++++++++++++--------- 1 file changed, 45 insertions(+), 16 deletions(-) diff --git a/controller/fw/embed/README.md b/controller/fw/embed/README.md index 9270225..18e34f7 100644 --- a/controller/fw/embed/README.md +++ b/controller/fw/embed/README.md @@ -2,32 +2,61 @@ ## Для разработки -- [Установить platformio](#introduction) +- Установить platformio + ```bash pip install -U platformio ``` -- [Скомпилировать проект](#build_project) + +- Скомпилировать проект + ```bash platformio run --environment robotroller_reborn ``` -- [Загрузить прошивку](#upload_project) + +- Загрузить прошивку + ```bash platformio run --target upload --environment robotroller_reborn ``` -- [Открыть монитор UART](#monitor_port) + +- Открыть монитор UART + ```bash platformio device monitor ``` -Чтобы отправить прошивку по can нужно: -1) сместить в линкере прошивки её адрес. Иoите файл с .ld и меняете область FLASH на 0x08008000 и соотвественно занимаемая память = 480K -пример: - MEMORY -{ - RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K - FLASH (rx) : ORIGIN = 0x8008000, LENGTH = 480K -} -меняем и в FLASH.ld и в RAM.ld -2) скомплиировать в hex формате -3) в терминале прописать: python3 boot_test.py /*адрес на hex файл*/ /*адрес устройства*/ -4) прошивка после чего микроконтроллер автоматически перезагрузится и будет уже запускаться с вашей прошивки, минуя бутлоадер \ No newline at end of file +## Загрузчик (Bootloader) + +В директории `bootloader` расположен код загрузчика, который позволяет загрузить/обновить управляющую программу контроллера. + +## Инструкция по загрузке ПО в контроллер по CAN-шине + +1. Сместить в линкере прошивки её адрес. В файле с расширением .ld замените область FLASH на 0x08008000 и занимаемую память на 480K +пример (FLASH.ld и RAM.ld): + + ```cpp + MEMORY + { + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x8008000, LENGTH = 480K + } + ``` + +2. Cкомпилировать в `hex` формате +3. Запустить в терминале + + ```bash + python3 boot_test.py <адрес на hex файл> <адрес устройства>` + ``` + +4. После завершения процесса прошивки микроконтроллер автоматически перезагрузится и запустится загруженная программа, минуя загрузчик. + +## Запись CAN ID в контроллер + +TODO +Пример + +```bash +python test/write_can_id.py "1" +``` From 24c70fe33ae2a0590927173a5f1b83bc2ea8e163 Mon Sep 17 00:00:00 2001 From: lulko Date: Tue, 25 Mar 2025 15:45:38 +0300 Subject: [PATCH 19/20] =?UTF-8?q?=D0=9E=D0=BF=D0=B8=D1=81=D0=B0=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D1=83=D1=81=D1=82=D0=B0=D0=BD=D0=BE=D0=B2=D0=BA?= =?UTF-8?q?=D0=B8=20CAN=5FID=20=D0=B8=20=D1=80=D0=B0=D0=B1=D0=BE=D1=82?= =?UTF-8?q?=D1=8B=20=D1=81=20bootloader?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/fw/embed/README.md | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/controller/fw/embed/README.md b/controller/fw/embed/README.md index 18e34f7..a8efdbf 100644 --- a/controller/fw/embed/README.md +++ b/controller/fw/embed/README.md @@ -47,16 +47,24 @@ platformio device monitor 3. Запустить в терминале ```bash - python3 boot_test.py <адрес на hex файл> <адрес устройства>` + python3 boot_test.py <адрес на hex файл> <адрес устройства> + ``` + Пример: + ```bash + python3 boot_test.py my_hex 0 ``` - 4. После завершения процесса прошивки микроконтроллер автоматически перезагрузится и запустится загруженная программа, минуя загрузчик. -## Запись CAN ID в контроллер +Бтулоадер запустится только в том случае, если по адресу 0x08060000 находится ключ DEADBEEF. При первой прошивке стартовый адрес = 0 -TODO -Пример +## Запись CAN ID в контроллер -```bash -python test/write_can_id.py "1" +Запись происходит в основной прошщивке, вызывается регистр записи, после чего REG_ID c указанным адресом. +Пример: ``` +msg_id: <адрес устройства> +CAN.buff[0] : REG_WRITE +Can.buff[1] : REG_ID +Can.buff[2] : <новый адрес устройства> +``` + From 3baa5d449fc710f9dc2742b9c7767c88ac6076f6 Mon Sep 17 00:00:00 2001 From: lulko Date: Tue, 8 Apr 2025 16:09:42 +0300 Subject: [PATCH 20/20] test FOC --- controller/fw/embed/bootloader/flash.cpp | 156 ---------------- controller/fw/embed/bootloader/flash.h | 74 -------- controller/fw/embed/bootloader/main.cpp | 194 -------------------- controller/fw/embed/bootloader/reg_cah.h | 38 ---- controller/fw/embed/compile_bootloader.py | 9 + controller/fw/embed/gen_compile_commands.py | 12 ++ controller/fw/embed/include/flash.h | 7 +- controller/fw/embed/mylink.ld | 116 ++++++++++++ controller/fw/embed/platformio.ini | 7 +- controller/fw/embed/src/main.cpp | 125 ++++++------- 10 files changed, 201 insertions(+), 537 deletions(-) delete mode 100644 controller/fw/embed/bootloader/flash.cpp delete mode 100644 controller/fw/embed/bootloader/flash.h delete mode 100644 controller/fw/embed/bootloader/main.cpp delete mode 100644 controller/fw/embed/bootloader/reg_cah.h create mode 100644 controller/fw/embed/compile_bootloader.py create mode 100644 controller/fw/embed/mylink.ld diff --git a/controller/fw/embed/bootloader/flash.cpp b/controller/fw/embed/bootloader/flash.cpp deleted file mode 100644 index d67b634..0000000 --- a/controller/fw/embed/bootloader/flash.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include "flash.h" -#include - - -static uint32_t write_ptr = SECTOR_6; - - -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(); -} - - // Wait if flash - - -bool validate_crc(FLASH_RECORD* crc){ - if(crc->crc == 0x6933) - return true; - return false; -} -/* read struct from FLASH */ -void flash_read(uint32_t addr,FLASH_RECORD* ptr){ - uint32_t* flash_ptr = (uint32_t*)addr; - uint32_t* dest = (uint32_t*)ptr; - for (int i = 0; i < FLASH_RECORD_SIZE / 4; i++) { - dest[i] = flash_ptr[i]; - } -} - - -/* Поиск актуального адреса во флэш памяти */ -FLASH_RECORD load_params(){ - __disable_irq(); - FLASH_RECORD latest = {0}; - FLASH_RECORD res; - for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { - flash_read(addr,&res); - if (!validate_crc(&res)) - break; -/* нашли адрес */ - else if(res.data_id == addr_id) { - latest = res; - } -} - - __enable_irq(); - return latest; -} -/** - * @brief Записывает страницу данных во флеш-память - * @param data: Указатель на буфер с данными - * @param len: Длина данных в байтах (должна быть кратна 4) - * @retval bool: true - успех, false - ошибка - */ - void write_flash_page(const uint8_t* data, uint16_t len) { // Добавлен const - flash_unlock(); - for (uint16_t i = 0; i < len; i += 4) { - uint32_t word; - memcpy(&word, &data[i], 4); // Безопасное копирование - HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, write_ptr + i, word); - } - flash_lock(); -} \ No newline at end of file diff --git a/controller/fw/embed/bootloader/flash.h b/controller/fw/embed/bootloader/flash.h deleted file mode 100644 index 0a9d699..0000000 --- a/controller/fw/embed/bootloader/flash.h +++ /dev/null @@ -1,74 +0,0 @@ -#ifndef FLASH_H_ -#define FLASH_H_ -#include "stm32f446xx.h" -#include "hal_conf_extra.h" -/* for addr in FLASH */ -typedef struct{ - uint32_t write_ptr_now; - uint16_t crc; - uint8_t value; - uint8_t data_id; // data_id = id register of can - }FLASH_RECORD; -enum { - addr_id = 0 -}; - - - -#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct -#define PARAM_COUNT 1 // count data in flash - - - -#define FLAG_BOOT 0x08060000 // Адрес хранения флага для обновления прошивки -#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение -#define APP_ADDRESS 0x08008000 // Адрес основной прошивки -#define BOOT_CAN_ID 0x01 // CAN ID бутлоадера -#define BOOT_CAN_END 0x02 // CAN ID завершения передачи -#define DATA_CAN_ID 0x03 // CAN ID данных -#define ACK_CAN_ID 0x05 // CAN ID подтверждения -#define MAX_FW_SIZE 0x3FFF // Макс. размер прошивки (256KB) - - - -// 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_6_END (SECTOR_6 + 128 * 1024) // sector 6 end - - - - -// 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 & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos) - -// 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 flash_program_word(uint32_t address,uint32_t data,uint32_t byte_len); -void write_flash_page(const uint8_t *data, uint16_t len); -void flash_read(uint32_t addr,FLASH_RECORD* ptr); -bool validate_crc(FLASH_RECORD* crc); -uint8_t flash_read_word(uint32_t address); -void flash_write(uint32_t addr, FLASH_RECORD* record); -FLASH_RECORD load_params(); -//bool write_flash_page(const uint8_t* data, uint16_t len); - -#endif /* FLASH_H_ */ diff --git a/controller/fw/embed/bootloader/main.cpp b/controller/fw/embed/bootloader/main.cpp deleted file mode 100644 index a97062b..0000000 --- a/controller/fw/embed/bootloader/main.cpp +++ /dev/null @@ -1,194 +0,0 @@ -#include -#include -#include "flash.h" - - - -STM32_CAN Can(CAN2, DEF); - -volatile bool fw_update = false; -volatile uint32_t fw_size = 0; -volatile uint32_t fw_crc = 0; -volatile uint32_t jump; -static FLASH_RECORD flash_record = {0}; -static uint32_t ptr_flash; - -volatile uint32_t msg_id; -volatile uint16_t id_x; -volatile uint8_t msg_ch; - -// Прототипы функций -void jump_to_app(); -void process_can_message(const CAN_message_t &msg); -void erase_flash_pages(); -bool verify_firmware(); -void send_ack(uint8_t status); - -void setup() { - // Инициализация периферии - Serial.setRx(HARDWARE_SERIAL_RX_PIN); - Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); - Can.begin(); - Can.setBaudRate(1000000); - TIM_TypeDef *Instance = TIM2; - HardwareTimer *SendTimer = new HardwareTimer(Instance); - SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz - // SendTimer->attachInterrupt(process_can_message); - SendTimer->resume(); - // Разрешить все ID (маска 0x00000000) - Can.setFilter(0, 0, STD); - - // Настройка GPIO - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; - GPIOC->MODER |= GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0; - GPIOC->ODR |= GPIO_ODR_OD11; - - // Проверка флага обновления - // erase_sector(6); - // flash_program_word(FLAG_BOOT,0xDEADBEEF,0); - // flash_record.data_id = addr_id; - // flash_record.crc = 0x6933; - // flash_record.value = 0x69; - // flash_record.write_ptr_now = SECTOR_6; - // flash_write(SECTOR_6, &flash_record); - flash_record = load_params(); - /* Добавить проверку адреса, т.е во время отправки запроса прошивки по CAN - мы сохраняем как флаг, так и аддрес устройства к которому будет обращатлься во - время прошивки */ - - if(*(volatile uint32_t*)(FLAG_BOOT) == UPDATE_FLAG) { - fw_update = true; - for(int i = 0; i < 5;i++){ - GPIOC->ODR ^= GPIO_ODR_OD10; // Индикация обновления - HAL_Delay(100); - } - erase_flash_pages(); - } else { - jump_to_app(); - } - -} - -void loop() { - if(fw_update) { - GPIOC->ODR ^= GPIO_ODR_OD10; - // HAL_Delay(100); - CAN_message_t msg; - while(Can.read(msg)) - process_can_message(msg); - } - } - - - -void process_can_message(const CAN_message_t &msg) { - msg_id = msg.id; - /* 0x691 - 69 - адрес устройства - 1 - что делать дальше с данными */ - - id_x = (msg_id >> 4) & 0xFFFF; //получение адреса устройства страшие 2 бита - msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать - if(id_x == flash_record.value){ - switch(msg_ch) { - case BOOT_CAN_ID: - if(msg.buf[0] == 0x01) { // Старт передачи - fw_size = *(uint32_t*)&msg.buf[1]; //размер прошивки тип 4 байта - fw_crc = *(uint16_t*)&msg.buf[5]; //crc - ptr_flash = APP_ADDRESS; - send_ack(0x01); - } - break; - - case DATA_CAN_ID: // Пакет данных - if(ptr_flash < (APP_ADDRESS + fw_size)) { - write_flash_page((const uint8_t*)msg.buf, msg.len); - ptr_flash += msg.len; - send_ack(0x02); - - } - break; - - - case BOOT_CAN_END: // Завершение передачи - if(verify_firmware()) { - send_ack(0xAA); - erase_sector(7); // Сброс флага - NVIC_SystemReset(); - } else { - send_ack(0x55); - } - break; - } -} -} - -void jump_to_app() { - __disable_irq(); - jump = *(volatile uint32_t*)(APP_ADDRESS + 4); - void (*app_entry)(void); - app_entry = (void (*)(void))jump; - - - for (uint32_t i = 0; i < 3; i++) { - NVIC->ICPR[i] = 0xFFFFFFFF; - } - - __set_MSP(*(volatile uint32_t*)APP_ADDRESS); - SCB->VTOR = APP_ADDRESS; - app_entry(); -} - -void erase_flash_pages() { - FLASH_EraseInitTypeDef erase; - erase.TypeErase = FLASH_TYPEERASE_SECTORS; - erase.Sector = FLASH_SECTOR_2; - erase.NbSectors = 4; - erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; - - uint32_t error; - flash_unlock(); - HAL_FLASHEx_Erase(&erase, &error); - flash_lock(); -} - - - - -uint16_t CalculateCRC16(const uint8_t* data, uint32_t length) { - uint16_t crc = 0xFFFF; // Начальное значение - while (length--) { - crc ^= *data++; // Обрабатываем LSB первым - for (uint8_t i = 0; i < 8; i++) { - if (crc & 0x0001) { // Проверяем младший бит - crc = (crc >> 1) ^ 0xA001; // Полином 0x8005 (reverse) - } else { - crc >>= 1; - } - } - } - return crc; // Финальный XOR = 0x0000 (не требуется) -} - - -bool verify_firmware() { - uint32_t calculated_crc = 0; - calculated_crc = CalculateCRC16((uint8_t*)APP_ADDRESS,fw_size); - if(calculated_crc != (uint16_t)fw_crc) - return false; - // Реализация проверки CRC - // ... - // return (calculated_crc == fw_crc); - else - return true; -} - -void send_ack(uint8_t status) { - - CAN_message_t ack; - ack.id = ACK_CAN_ID; - ack.len = 1; - ack.buf[0] = status; - Can.write(ack); -} diff --git a/controller/fw/embed/bootloader/reg_cah.h b/controller/fw/embed/bootloader/reg_cah.h deleted file mode 100644 index 15efa8e..0000000 --- a/controller/fw/embed/bootloader/reg_cah.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef REG_CAH_H_ -#define REG_CAH_H_ - -#define APP_ADDR 0x0800400 // 16KB - Application -#define ADDR_VAR 0x8040000 - - -#define REG_READ 0x99 -#define REG_WRITE 0xA0 - - -/* Startup ID device */ -#define START_ID 0x00 - -/* CAN REGISTER ID */ -#define REG_ID 0x01 -#define REG_BAUDRATE 0x02 - -#define REG_MOTOR_POSPID_Kp 0x30 -#define REG_MOTOR_POSPID_Ki 0x31 -#define REG_MOTOR_POSPID_Kd 0x32 - -#define REG_MOTOR_VELPID_Kp 0x40 -#define REG_MOTOR_VELPID_Ki 0x41 -#define REG_MOTOR_VELPID_Kd 0x42 - -#define REG_MOTOR_IMPPID_Kp 0x50 -#define REG_MOTOR_IMPPID_Kd 0x51 - -#define REG_RESET 0x88 -#define REG_LED_BLINK 0x8B - -#define FOC_STATE 0x60 - -#define MOTOR_VELOCITY 0x70 -#define MOTOR_ENABLED 0x71 -#define MOTOR_ANGLE 0x72 -#endif // REG_CAH_H_ diff --git a/controller/fw/embed/compile_bootloader.py b/controller/fw/embed/compile_bootloader.py new file mode 100644 index 0000000..35d9e0d --- /dev/null +++ b/controller/fw/embed/compile_bootloader.py @@ -0,0 +1,9 @@ +import os +Import("env") + + +# include toolchain paths +env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True) + +# override compilation DB path +env.Replace(COMPILATIONDB_PATH="compile_commands.json") \ No newline at end of file diff --git a/controller/fw/embed/gen_compile_commands.py b/controller/fw/embed/gen_compile_commands.py index 0537d0b..06e5650 100644 --- a/controller/fw/embed/gen_compile_commands.py +++ b/controller/fw/embed/gen_compile_commands.py @@ -1,8 +1,20 @@ import os Import("env") + +# Custom HEX from ELF +env.AddPostAction( + "$BUILD_DIR/${PROGNAME}.elf", + env.VerboseAction(" ".join([ + "$OBJCOPY", "-O", "ihex", "-R", ".eeprom", + "$BUILD_DIR/${PROGNAME}.elf", "$BUILD_DIR/${PROGNAME}.hex" + ]), "Building $BUILD_DIR/${PROGNAME}.hex") +) + + # include toolchain paths env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True) # override compilation DB path env.Replace(COMPILATIONDB_PATH="compile_commands.json") + diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index 7c17729..a155673 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -36,8 +36,9 @@ kp_id = 2, #define FLAG_BOOT (0x08040000 + 4) // Flash keys for unlocking flash memory - - +#define BYTE32 0 +#define BYTE8 1 +#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение //FLASH SET ONE PROGRAMM WORD #define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1 #define FLASH_32BYTE \ @@ -59,7 +60,7 @@ typedef void(*pFunction)(void); 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); +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(); diff --git a/controller/fw/embed/mylink.ld b/controller/fw/embed/mylink.ld new file mode 100644 index 0000000..a00426d --- /dev/null +++ b/controller/fw/embed/mylink.ld @@ -0,0 +1,116 @@ +/* + * Линкер-скрипт для STM32F446RETx (512K FLASH, 128K RAM) + * Смещение FLASH: 0x08008000 (первые 32K под загрузчик) + */ + +/* Точка входа */ +ENTRY(Reset_Handler) + +/* Конец стека (адрес начала стека = конец RAM) */ +_estack = ORIGIN(RAM) + LENGTH(RAM); + +/* Минимальные размеры кучи и стека */ +_Min_Heap_Size = 0x1000; /* 4 КБ */ +_Min_Stack_Size = 0x2000; /* 8 КБ */ + +/* Распределение памяти */ +MEMORY { + RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 384K # Для STM32F446 (512K - 32K) +} + +/* Секции */ +SECTIONS { + /* Векторы прерываний (должны быть первыми!) */ + .isr_vector : { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Секция векторов */ + . = ALIGN(4); + } >FLASH + + /* Программный код и константы */ + .text : { + . = ALIGN(4); + *(.text) /* Код */ + *(.text*) /* Код (включая C++) */ + *(.glue_7) /* ARM/Thumb glue */ + *(.glue_7t) /* Thumb/ARM glue */ + *(.eh_frame) + + KEEP(*(.init)) /* Конструкторы */ + KEEP(*(.fini)) /* Деструкторы */ + + . = ALIGN(4); + _etext = .; /* Конец кода */ + } >FLASH + + /* Константы (только для чтения) */ + .rodata : { + . = ALIGN(4); + *(.rodata) /* Константы */ + *(.rodata*) /* Константы (включая строки) */ + . = ALIGN(4); + } >FLASH + + /* Таблицы для C++ (исключения, RTTI) */ + .ARM.extab : { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM.exidx : { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + /* Инициализированные данные (копируются в RAM при старте) */ + _sidata = LOADADDR(.data); /* Адрес данных во FLASH */ + + .data : { + . = ALIGN(4); + _sdata = .; /* Начало данных в RAM */ + *(.data) /* Инициализированные переменные */ + *(.data*) /* Инициализированные переменные (C++) */ + *(.RamFunc) /* Функции в RAM */ + *(.RamFunc*) /* Функции в RAM (C++) */ + . = ALIGN(4); + _edata = .; /* Конец данных в RAM */ + } >RAM AT> FLASH /* Физически хранятся во FLASH */ + + /* Неинициализированные данные (BSS) */ + .bss : { + . = ALIGN(4); + _sbss = .; /* Начало BSS */ + __bss_start__ = _sbss; + *(.bss) /* BSS переменные */ + *(.bss*) /* BSS переменные (C++) */ + *(COMMON) /* Общие переменные */ + . = ALIGN(4); + _ebss = .; /* Конец BSS */ + __bss_end__ = _ebss; + } >RAM + + /* Куча и стек (резервируем место) */ + ._user_heap_stack : { + . = ALIGN(8); + PROVIDE(end = .); + PROVIDE(_end = .); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + /* Удаление ненужных секций из стандартных библиотек */ + /DISCARD/ : { + libc.a (*) + libm.a (*) + libgcc.a (*) + } + + /* Атрибуты ARM */ + .ARM.attributes 0 : { *(.ARM.attributes) } +} \ No newline at end of file diff --git a/controller/fw/embed/platformio.ini b/controller/fw/embed/platformio.ini index 8b6a522..c0f3aa2 100644 --- a/controller/fw/embed/platformio.ini +++ b/controller/fw/embed/platformio.ini @@ -14,17 +14,20 @@ platform = ststm32 board = genericSTM32F446RE framework = arduino +#board_build.ldscript = mylink.ld +#board_upload.offset_address = 0x08008000 upload_protocol = stlink debug_tool = stlink monitor_speed = 19200 monitor_parity = N - build_flags = -DSTM32F446xx -D HAL_CAN_MODULE_ENABLED -D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH - lib_deps = askuric/Simple FOC@^2.3.4 pazi88/STM32_CAN@^1.1.2 + extra_scripts = pre:gen_compile_commands.py + + diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 41f4bc4..49e65e0 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -1,5 +1,5 @@ // clang-format off -#include +#include "Arduino.h" #include "stm32f446xx.h" #include #include @@ -16,10 +16,12 @@ #include "flash.h" +void SysTick_Handler(void) { + HAL_IncTick(); +} STM32_CAN Can(CAN2, DEF); - /* for FLASH */ uint32_t flash_flag; uint8_t flag_can = 0; @@ -62,6 +64,10 @@ void doMotor(char *cmd) { delayMicroseconds(2); } +void CAN2_RX0_IRQHandler() { + // Пустая функция, но прерывание не приведет к Default Handler +} + void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, DRV8313Driver *driver, LowsideCurrentSense *current_sense, Commander *commander, CommandCallback callback) { @@ -157,43 +163,13 @@ void setup_id(uint8_t my_id) { void send_data() { - send_velocity(); - send_angle(); - send_motor_enabled(); + // send_velocity(); + // send_angle(); + // send_motor_enabled(); // read_temperature(); - digitalWrite(PC11, !digitalRead(PC11)); + // GPIOC->ODR ^= GPIO_ODR_OD11; } - - -/*void read_can_step() { - char flag = CAN_inMsg.buf[1]; - if (flag == 'V') { - motor.enable(); - memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[2], - sizeof(motor_control_inputs.target_velocity)); - } else if (flag == 'A') { - motor.enable(); - memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[2], - sizeof(motor_control_inputs.target_angle)); - } else if (flag == 'E') { - bool enable_flag = CAN_inMsg.buf[2]; - if (enable_flag == 1) { - memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[2], - sizeof(motor_control_inputs.motor_enabled)); - motor.enable(); - } else if (enable_flag == 0) { - memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[2], - sizeof(motor_control_inputs.motor_enabled)); - motor.disable(); - } - } - GPIOC->ODR ^= GPIO_ODR_OD10; //digitalWrite(PC10, !digitalRead(PC10)); -}*/ - - - - void listen_can() { uint8_t reg_id = CAN_inMsg.buf[0]; //reg id @@ -202,7 +178,7 @@ void listen_can() { switch (reg_id) { case REG_ID: /* setup new id */ - setup_id(CAN_inMsg.buf[2]); + setup_id(CAN_inMsg.buf[2]); break; case REG_LED_BLINK: @@ -225,6 +201,7 @@ void listen_can() { default: break; } + } else if (CAN_inMsg.buf[1] == REG_READ) { switch (reg_id) { case REG_ID: @@ -256,6 +233,8 @@ void listen_can() { +volatile uint32_t ipsr_value = 0; + void foc_step(BLDCMotor *motor, Commander *commander) { if (motor_control_inputs.target_velocity != 0 || @@ -281,34 +260,36 @@ void foc_step(BLDCMotor *motor, Commander *commander) { - - -void setup() { +void setup(){ /* bias for vector int */ - //SCB->VTOR = 0x08004000; + // __set_MSP(*(volatile uint32_t*)0x08008000); + // SCB->VTOR = (volatile uint32_t)0x08008000; - - Serial.setRx(HARDWARE_SERIAL_RX_PIN); - Serial.setTx(HARDWARE_SERIAL_TX_PIN); - Serial.begin(115200); - // setup led pin - pinMode(PC11, OUTPUT); - pinMode(PC10, OUTPUT); - // Setup thermal sensor pin - // pinMode(TH1, INPUT_ANALOG); - Can.begin(); - Can.setBaudRate(1000000); - TIM_TypeDef *Instance = TIM2; - HardwareTimer *SendTimer = new HardwareTimer(Instance); - SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz - SendTimer->attachInterrupt(send_data); - SendTimer->resume(); - for(int i = 0;i < 10;i++){ - GPIOC->ODR ^= GPIO_ODR_OD10; - HAL_Delay(1000); - } - setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); +GPIOC->MODER &= ~(GPIO_MODER_MODE10_Msk | GPIO_MODER_MODE11_Msk); // Сброс битов режима +GPIOC->MODER |= (GPIO_MODER_MODE10_0 | GPIO_MODER_MODE11_0); // Установка режима выхода (01) +// Устанавливаем состояние выводов +GPIOC->ODR &= ~GPIO_ODR_OD10; +GPIOC->ODR |= GPIO_ODR_OD11; +Serial.setRx(HARDWARE_SERIAL_RX_PIN); +Serial.setTx(HARDWARE_SERIAL_TX_PIN); +Serial.begin(115200); +__HAL_RCC_CAN2_CLK_ENABLE(); +__HAL_RCC_GPIOB_CLK_ENABLE(); // Для CAN2 пинов PB12 (CAN2_RX), PB13 (CAN2_TX) + +// Setup thermal sensor pin +// pinMode(TH1, INPUT_ANALOG); +Can.begin(); +Can.setBaudRate(1000000); +TIM_TypeDef *Instance = TIM2; +HardwareTimer *SendTimer = new HardwareTimer(Instance); +// SendTimer->setOverflow(100, HERTZ_FORMAT); // 50 Hz +// SendTimer->attachInterrupt(send_data); +// SendTimer->resume(); +setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); + + // HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); + // HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 1, 0); // Низкий приоритет /* Настройка параметров стирания */ /*pEraseInit.TypeErase = TYPEERASE_SECTORS; // Стирание страниц pEraseInit.Sector = ADDR_VAR; // Начальная страница @@ -317,14 +298,18 @@ void setup() { flash_rec = load_params(); for(int i = 0;i < PARAM_COUNT;i++) flash_buf[i] = flash_rec[i]; -} - void loop() { - foc_step(&motor, &command); - GPIOC->ODR ^= GPIO_ODR_OD10; - HAL_Delay(1000); - while (Can.read(CAN_inMsg)) { - listen_can(); + + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 14, 0); // Низкий приоритет + HAL_NVIC_SetPriority(TIM2_IRQn, 14, 0); // Низкий приоритет } + +void loop() { + foc_step(&motor, &command); + CAN_message_t msg; + GPIOC->ODR ^= GPIO_ODR_OD11; + HAL_Delay(500); + // while (Can.read(msg)) { + // listen_can(); + // } } -