diff --git a/controller/fw/embed/README.md b/controller/fw/embed/README.md index 61796e3..a8efdbf 100644 --- a/controller/fw/embed/README.md +++ b/controller/fw/embed/README.md @@ -2,19 +2,69 @@ ## Для разработки -- [Установить 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 ``` + +## Загрузчик (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 файл> <адрес устройства> + ``` + Пример: + ```bash + python3 boot_test.py my_hex 0 + ``` +4. После завершения процесса прошивки микроконтроллер автоматически перезагрузится и запустится загруженная программа, минуя загрузчик. + +Бтулоадер запустится только в том случае, если по адресу 0x08060000 находится ключ DEADBEEF. При первой прошивке стартовый адрес = 0 + +## Запись CAN ID в контроллер + +Запись происходит в основной прошщивке, вызывается регистр записи, после чего REG_ID c указанным адресом. +Пример: +``` +msg_id: <адрес устройства> +CAN.buff[0] : REG_WRITE +Can.buff[1] : REG_ID +Can.buff[2] : <новый адрес устройства> +``` + 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 new file mode 100644 index 0000000..a155673 --- /dev/null +++ b/controller/fw/embed/include/flash.h @@ -0,0 +1,72 @@ +#ifndef FLASH_H_ +#define FLASH_H_ +#include "stm32f446xx.h" + + + + +/* for addr in FLASH */ +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, + foc_id = 1, +kp_id = 2, + ki_id = 3, + kd_id = 4 +}; + +#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct +#define PARAM_COUNT 3 // 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_6_END (SECTOR_6 + 128 * 1024) // sector 6 end +#define SECTOR_7 0x08060000 // 128KB + + +#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 \ + FLASH->CR = (FLASH->CR & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos) + +// 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 flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len); +uint8_t flash_read_word(uint32_t address); +void write_param(uint8_t param_id, uint8_t val); +FLASH_RECORD* load_params(); +void compact_page(); +void flash_read(uint32_t addr,FLASH_RECORD* ptr); +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 new file mode 100644 index 0000000..15efa8e --- /dev/null +++ b/controller/fw/embed/include/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/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 a759816..c0f3aa2 100644 --- a/controller/fw/embed/platformio.ini +++ b/controller/fw/embed/platformio.ini @@ -14,6 +14,8 @@ 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 @@ -25,4 +27,7 @@ build_flags = 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/flash.cpp b/controller/fw/embed/src/flash.cpp new file mode 100644 index 0000000..bedac70 --- /dev/null +++ b/controller/fw/embed/src/flash.cpp @@ -0,0 +1,188 @@ +#include "flash.h" +#include +#include "hal_conf_extra.h" + +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 = 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 = {param_id,val,6933}; + + /* Exit data from FLASH */ + if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_7) + compact_page(); + + // Проверка выравнивания адреса + 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_6_END;addr +=FLASH_RECORD_SIZE) { + flash_read(addr,&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 b342add..49e65e0 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -1,5 +1,6 @@ // clang-format off -#include +#include "Arduino.h" +#include "stm32f446xx.h" #include #include #include @@ -11,9 +12,27 @@ #include "wiring_analog.h" #include "wiring_constants.h" // clang-format on +#include "reg_cah.h" +#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; +uint32_t flash_error; +FLASH_EraseInitTypeDef pEraseInit; +uint32_t SectorError; + + +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; @@ -34,6 +53,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; @@ -44,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) { @@ -85,7 +109,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; + 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)); @@ -94,7 +119,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; + 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)); @@ -102,46 +128,114 @@ void send_angle() { } void send_motor_enabled() { - CAN_TX_msg.id = 1; + uint8_t id = *(volatile uint8_t*)ADDR_VAR; + CAN_TX_msg.id = id; CAN_TX_msg.buf[0] = 'E'; memcpy(&CAN_TX_msg.buf[1], &motor_control_inputs.motor_enabled, sizeof(motor_control_inputs.motor_enabled)); Can.write(CAN_TX_msg); } -void send_data() { - send_velocity(); - send_angle(); - send_motor_enabled(); - // read_temperature(); - digitalWrite(PC11, !digitalRead(PC11)); +void send_foc_state() { + 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, + sizeof(motor_control_inputs.foc_state)); +Can.write(CAN_TX_msg); } -void read_can_step() { - char flag = CAN_inMsg.buf[0]; - if (flag == 'V') { - motor.enable(); - memcpy(&motor_control_inputs.target_velocity, &CAN_inMsg.buf[1], - sizeof(motor_control_inputs.target_velocity)); - } else if (flag == 'A') { - motor.enable(); - memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], - sizeof(motor_control_inputs.target_angle)); - } else if (flag == 'E') { - bool enable_flag = CAN_inMsg.buf[1]; - if (enable_flag == 1) { - memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], - sizeof(motor_control_inputs.motor_enabled)); - motor.enable(); - } else if (enable_flag == 0) { - memcpy(&motor_control_inputs.motor_enabled, &CAN_inMsg.buf[1], - sizeof(motor_control_inputs.motor_enabled)); - motor.disable(); +void send_id() { + /* data for reading of firmware */ + + 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)); + Can.write(CAN_TX_msg); +} + +void setup_id(uint8_t my_id) { + write_param(addr_id,my_id); + send_id(); +} + + + +void send_data() { + // send_velocity(); + // send_angle(); + // send_motor_enabled(); + // read_temperature(); + // GPIOC->ODR ^= GPIO_ODR_OD11; +} + +void listen_can() { + + 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) { + switch (reg_id) { + case REG_ID: + /* setup new id */ + setup_id(CAN_inMsg.buf[2]); + break; + + 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[2] == 1) { + motor.enable(); + motor_control_inputs.motor_enabled = 1; + } else { + motor.disable(); + motor_control_inputs.motor_enabled = 0; + } + + + default: + break; + } + +} else if (CAN_inMsg.buf[1] == REG_READ) { + switch (reg_id) { + 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; } } - digitalWrite(PC10, !digitalRead(PC10)); + } } + + + +volatile uint32_t ipsr_value = 0; + + void foc_step(BLDCMotor *motor, Commander *commander) { if (motor_control_inputs.target_velocity != 0 || motor->controller == MotionControlType::velocity) { @@ -163,28 +257,59 @@ void foc_step(BLDCMotor *motor, Commander *commander) { commander->run(); } -void setup() { - 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(); - setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); -} + + + +void setup(){ + /* bias for vector int */ + // __set_MSP(*(volatile uint32_t*)0x08008000); + // SCB->VTOR = (volatile uint32_t)0x08008000; + +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; // Начальная страница + 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]; + + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 14, 0); // Низкий приоритет + HAL_NVIC_SetPriority(TIM2_IRQn, 14, 0); // Низкий приоритет + } void loop() { - foc_step(&motor, &command); - while (Can.read(CAN_inMsg)) { - read_can_step(); - } + foc_step(&motor, &command); + CAN_message_t msg; + GPIOC->ODR ^= GPIO_ODR_OD11; + HAL_Delay(500); + // while (Can.read(msg)) { + // listen_can(); + // } } + 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..3f7522c --- /dev/null +++ b/controller/fw/embed/test/python_test_boot.py @@ -0,0 +1,142 @@ +import can +import sys +import time +from intelhex import IntelHex +from crc import Calculator, Crc16 +# Конфигурация +CAN_CHANNEL = 'socketcan' +CAN_INTERFACE = 'can0' +CAN_BITRATE = 1000000 +#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 + + + +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...") + bus = can.interface.Bus( + channel=CAN_INTERFACE, + bustype=CAN_CHANNEL, + bitrate=CAN_BITRATE + ) + + 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 = calculate_crc16_modbus(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 + ) + + 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: + 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)) + + 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 + + ack = wait_for_ack(bus) + if not ack: + debug_print("Таймаут ACK DATA") + return + + # Финал + 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=3) + if ack and ack.data[0] == 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) # Неблокирующий режим + if msg and msg.arbitration_id == ACK_CAN_ID: + return msg + return None + +if __name__ == "__main__": + import sys + if len(sys.argv) != 3: + print("Использование: sudo python3 can_flasher.py firmware.hex") + sys.exit(1) + + send_firmware(sys.argv[1]) 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..8a8797d --- /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': [0x27, 0xA0, 0xFF, 0x00], # Данные для записи (4 байта) + 'description': "Установка id устройства" + } + + read_msg = { + 'arbitration_id': 0x01, # CAN ID для чтения + 'data': [0xFF,0x99], # Адрес новый + команда запроса данных + 'description': "Запрос id устройства", + 'response_id': 0xFF, # Ожидаемый 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(2.0) + + # 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()