From 3baa5d449fc710f9dc2742b9c7767c88ac6076f6 Mon Sep 17 00:00:00 2001 From: lulko Date: Tue, 8 Apr 2025 16:09:42 +0300 Subject: [PATCH] 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(); + // } } -