From d935263742a6dfdea0ab824c8e61561d31cbc857 Mon Sep 17 00:00:00 2001 From: Igor Brylev Date: Thu, 17 Apr 2025 17:45:23 +0300 Subject: [PATCH] ADD persistent parameters (PID, ID), request-response model via CAN bus, Position Control --- controller/fw/embed/include/flash.h | 74 ++++ controller/fw/embed/include/reg_cah.h | 40 ++ controller/fw/embed/src/flash.cpp | 221 +++++++++++ controller/fw/embed/src/main.cpp | 410 ++++++++++++++++++--- controller/fw/embed/test/python_test_id.py | 115 ++++++ 5 files changed, 801 insertions(+), 59 deletions(-) create mode 100644 controller/fw/embed/include/flash.h create mode 100644 controller/fw/embed/include/reg_cah.h create mode 100644 controller/fw/embed/src/flash.cpp create mode 100644 controller/fw/embed/test/python_test_id.py diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h new file mode 100644 index 0000000..f15b6ea --- /dev/null +++ b/controller/fw/embed/include/flash.h @@ -0,0 +1,74 @@ +#ifndef FLASH_H_ +#define FLASH_H_ +#include "stm32f446xx.h" +#include +#include + + +/* 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, + angl = 2, + vel = 3, + pid_p = 4, + pid_i, + pid_d +}; + +#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_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); +uint16_t validate_crc16(uint8_t *data,uint32_t length); +void flash_write(uint32_t addr, FLASH_RECORD* record); +bool validaate_crc(FLASH_RECORD* crc); +#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..dbbfdc7 --- /dev/null +++ b/controller/fw/embed/include/reg_cah.h @@ -0,0 +1,40 @@ +#ifndef REG_CAH_H_ +#define REG_CAH_H_ + +#define APP_ADDR 0x0800400 // 16KB - Application +#define ADDR_VAR 0x8040000 + + +#define REG_READ 0x07 +#define REG_WRITE 0x08 + + +/* 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 +#define MOTOR_TORQUE 0x73 + +#endif // REG_CAH_H_ diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp new file mode 100644 index 0000000..f22bc51 --- /dev/null +++ b/controller/fw/embed/src/flash.cpp @@ -0,0 +1,221 @@ +#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 + i) = data[i]; + write_ptr++; + } + + // 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 validata_crc(FLASH_RECORD* crc){ + return crc->crc == 0x6933? true : false; +} + +uint16_t validate_crc16(uint8_t *data, uint32_t length) { + uint16_t crc = 0xFFFF; // Начальное значение для MODBUS + while (length--) { + crc ^= *data++; // XOR с очередным байтом данных + for (uint8_t i = 0; i < 8; i++) { + if (crc & 0x0001) { + crc = (crc >> 1) ^ 0xA001; // Полином 0x8005 (reverse) + } else { + crc >>= 1; + } + } + } + return crc; // Возвращаем вычисленный CRC +} + + + +/* 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); + uint16_t calculated_crc = validate_crc16((uint8_t*)&rec, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC + + if (calculated_crc == rec.crc && rec.data_id < PARAM_COUNT) { + // Если CRC совпадает и ID параметра валидный, сохраняем последнее значение + latest[rec.data_id] = rec; + } + else + //Если не совпадает продолжить читать флэш + continue; + } + + erase_sector(6); + write_ptr = SECTOR_6; // Сброс на начало + for (int i = 0; i < PARAM_COUNT; i++) { + if (latest[i].data_id != 0xFF) { + // Выравнивание перед каждой записью + if (write_ptr % 4 != 0) { + write_ptr += (4 - (write_ptr % 4)); + } + flash_write(write_ptr, &latest[i]); + + } + } +} + + +void write_param(uint8_t param_id, uint8_t val) { + FLASH_RECORD param_flash = {param_id, val}; + // __disable_irq(); // Запрещаем прерывания на время всей операции + + param_flash.crc = validate_crc16((uint8_t*)¶m_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти + + // Проверка выравнивания ДО проверки границ сектора кратного 4 + if (write_ptr % 4 != 0) { + write_ptr += (4 - (write_ptr % 4)); + } + + // Проверка переполнения с учётом выравнивания + if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) { + compact_page(); // После compact_page write_ptr обновляется + // Повторно выравниваем после функции. То есть сколько не хватает для кратности + if (write_ptr % 4 != 0) { + write_ptr += (4 - (write_ptr % 4)); + } + } + + flash_write(write_ptr, ¶m_flash); //внутри функции итак автоматические инкрементируется указатель write_ptr на размер структуры + + // __enable_irq(); // Разрешаем прерывания +} + +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); + /* провекра CRC */ + uint16_t calculated_crc = validate_crc16((uint8_t*)&res, sizeof(FLASH_RECORD) - 2); //Вычисляем CRC без последних двух байтов.STRUCT - 2BYTE__CRC + if (calculated_crc != res.crc || res.data_id >= PARAM_COUNT) + continue; + else{ + latest[res.data_id] = res; + } + write_ptr = addr + FLASH_RECORD_SIZE; +} + + __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..8dd8e66 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,33 @@ #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; +volatile uint16_t msg_id; +volatile uint16_t id_x; +volatile uint8_t msg_ch; +volatile uint8_t crc_h; +volatile uint8_t crc_l; + +volatile float kt = 0.1; //for torgue calculation + +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 +59,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 +70,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) { @@ -83,64 +113,309 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, motor->initFOC(); } +void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) { + // Создаем сообщение + CAN_message_t msg; + msg.id = id; + msg.len = 8; // или как в протоколе + msg.buf[0] = message_type; + memcpy(&msg.buf[1], data, data_length); + + // Формируем массив для CRC, включающий ID и все данные + size_t crc_data_size = sizeof(msg.id) + data_length; + uint8_t crc_data[crc_data_size]; + + // Копируем ID + memcpy(crc_data, &msg.id, sizeof(msg.id)); + // Копируем все байты data + memcpy(crc_data + sizeof(msg.id), data, data_length); + + // Расчет CRC + uint16_t crc_value = validate_crc16(crc_data, crc_data_size); + + // Вставляем CRC в буфер + msg.buf[6] = crc_value & 0xFF; + msg.buf[7] = (crc_value >> 8) & 0xFF; + + // Отправляем + Can.write(msg); + __NOP(); +} + + + void send_velocity() { float current_velocity = motor.shaftVelocity(); - CAN_TX_msg.id = 1; - CAN_TX_msg.buf[0] = 'V'; - CAN_TX_msg.len = 5; - memcpy(&CAN_TX_msg.buf[1], ¤t_velocity, sizeof(current_velocity)); - Can.write(CAN_TX_msg); + flash_rec = load_params(); + if (flash_rec == nullptr) { // Проверка на NULL + // Обработка ошибки: запись в лог, сигнализация и т.д. + return; + } + uint8_t value = flash_rec[vel].value; + uint8_t id = flash_rec[addr_id].value; + send_can_with_id_crc(id,'V',&value,sizeof(value)); } void send_angle() { float current_angle = motor.shaftAngle(); - CAN_TX_msg.id = 1; - CAN_TX_msg.buf[0] = 'A'; - CAN_TX_msg.len = 5; - memcpy(&CAN_TX_msg.buf[1], ¤t_angle, sizeof(current_angle)); - Can.write(CAN_TX_msg); + + flash_rec = load_params(); + if (flash_rec == nullptr) { // Проверка на NULL + // Обработка ошибки: запись в лог, сигнализация и т.д. + return; + } + // uint8_t value = flash_rec[angl].value; + uint8_t id = flash_rec[addr_id].value; + send_can_with_id_crc(id,'A',¤t_angle,sizeof(current_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() { + /* data for reading of firmware */ + flash_rec = load_params(); + if (flash_rec == nullptr) { // Проверка на NULL + // Обработка ошибки: запись в лог, сигнализация и т.д. + return; + } + + uint8_t value = flash_rec[foc_id].value; + uint8_t id = flash_rec[addr_id].value; + send_can_with_id_crc(id,'F',&value,sizeof(value)); } -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 */ + flash_rec = load_params(); + if (flash_rec == nullptr) { // Проверка на NULL + // Обработка ошибки: запись в лог, сигнализация и т.д. + return; + } + uint8_t id = flash_rec[addr_id].value; + send_can_with_id_crc(id,'I',&id,sizeof(id)); + __NOP(); +} + +void send_motor_torque() { + float i_q = motor.current.q; // Ток по оси q (А) + float torque = kt * i_q; // Расчет момента + torque *= 100; + flash_rec = load_params(); + CAN_TX_msg.id = flash_rec->value; + CAN_TX_msg.buf[0] = 'T'; + CAN_TX_msg.len = 5; + memcpy(&CAN_TX_msg.buf[1], &torque, sizeof(torque)); + Can.write(CAN_TX_msg); +} + + +void send_pid(uint8_t param_pid){ + flash_rec = load_params(); + if (flash_rec == nullptr) { // Проверка на NULL + return; + } + uint8_t id = flash_rec[addr_id].value; + uint8_t d = flash_rec[param_pid].value; + uint8_t data_send = 0; + int l = 0; + while(d /= 10) + l++; + if(l >= 2) + data_send = (float)d; + + else if(l == 1) + data_send = (float)(d * 10); + + else + data_send = (float)(d * 100); + if(param_pid == pid_p)param_pid = REG_MOTOR_POSPID_Kp; + else if(param_pid == pid_i)param_pid = REG_MOTOR_POSPID_Ki; + else if(param_pid == pid_d)param_pid = REG_MOTOR_POSPID_Kd; + send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send)); +} + +void setup_id(uint8_t my_id) { + write_param(addr_id,my_id); + // send_id(); +} + +void setup_angle(float target_angle) { + // float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы + motor.enable(); // Включаем мотор если он отключен + motor.controller = MotionControlType::angle; + motor.move(target_angle); +} + +void setup_pid_angle(uint8_t param_pid, float data){ + switch (param_pid) + { + case pid_p: + motor.P_angle.P = data; + break; + + case pid_i: + motor.P_angle.I = data; + break; + + case pid_d: + motor.P_angle.D = data; + break; + + default: + break; + } + uint8_t check = uint8_t(data); + uint8_t data_save = 0; + if(check != 0) + if(check /= 10) + data_save = check; + + else + data_save = (uint8_t)(data * 10); + + else + data_save = (uint8_t)(data * 100); + + write_param(param_pid,data_save); +} + + +void listen_can(const CAN_message_t &msg) { + msg_id = msg.id; + + msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать + id_x = (msg_id >> 4) & 0x7FF; //получение адреса устройства страшие 2 бита msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать + + + /* Вычисление CRC */ + // Объединение старшего и младшего байтов для получения полученного CRC + uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8); + uint8_t data[10] = {0}; //буфер хранения сообщения и расчета его CRC для проверки + + // Копируем ID сообщения в буфер данных для расчета CRC 2 байта + memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id)); + + // Копируем данные сообщения в буфер (без байтов CRC) + memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2); + + // Рассчитываем CRC для полученных данных + uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2); + + // Проверяем совпадение CRC + if (calculated_crc != received_crc) { + // Несовпадение CRC, игнорируем сообщение + return; + } + + + /* 0x691 + 69 - адрес устройства + 1 - что делать дальше с данными */ + + if(id_x == flash_rec->value){ + if(msg_ch == REG_WRITE){ + switch(msg.buf[0]) { + case REG_ID: + /* setup new id */ + setup_id(msg.buf[1]); + break; + + case REG_LED_BLINK: + for (int i = 0; i < 10; i++) { + GPIOC->ODR ^= GPIO_ODR_OD10; + delay(100); + } + break; + + case MOTOR_ANGLE: + memcpy(&motor_control_inputs.target_angle, &CAN_inMsg.buf[1], + sizeof(motor_control_inputs.target_angle)); + setup_angle(motor_control_inputs.target_angle); + break; + + case REG_MOTOR_POSPID_Kp: + setup_pid_angle(pid_p,msg.buf[1]); + break; + + case REG_MOTOR_POSPID_Ki: + setup_pid_angle(pid_i,msg.buf[1]); + break; + + case REG_MOTOR_POSPID_Kd: + setup_pid_angle(pid_d,msg.buf[1]); + break; + + case MOTOR_ENABLED: + if (msg.buf[1] == 1) { + motor.enable(); + motor_control_inputs.motor_enabled = 1; + } else { + motor.disable(); + motor_control_inputs.motor_enabled = 0; + } + + default: + break; + } + } + else if (msg_ch == REG_READ) { + switch (msg.buf[0]) { + 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 MOTOR_TORQUE: + send_motor_torque(); + break; + + case FOC_STATE: + send_foc_state(); + break; + + case REG_MOTOR_POSPID_Kp: + send_pid(pid_p); + break; + + case REG_MOTOR_POSPID_Ki: + send_pid(pid_i); + break; + + case REG_MOTOR_POSPID_Kd: + send_pid(pid_d); + 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 || @@ -163,28 +438,45 @@ 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(); + + + +void setup(){ + /* bias for vector int */ + // __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); + +pinMode(PC11, OUTPUT); +pinMode(PC10,OUTPUT); +GPIOC->ODR &= ~GPIO_ODR_OD10; +// 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(); + flash_rec = load_params(); + for(int i = 0;i < PARAM_COUNT;i++) + flash_buf[i] = flash_rec[i]; setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); -} + GPIOC->ODR |= GPIO_ODR_OD11; + motor.torque_controller = TorqueControlType::foc_current; + motor.controller = MotionControlType::torque; + __enable_irq(); + } 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; + delay(500); + while (Can.read(msg)) { + listen_can(msg); } } 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..b7a3084 --- /dev/null +++ b/controller/fw/embed/test/python_test_id.py @@ -0,0 +1,115 @@ +import can +import time + +# Конфигурация +CAN_INTERFACE = 'can0' +OLD_DEVICE_ID = 0x00 +NEW_DEVICE_ID = 0x69 +REG_WRITE = 0x8 +REG_READ = 0x7 +REG_ID = 0x1 + +def send_can_message(bus, can_id, data): + """Отправка CAN-сообщения""" + try: + msg = can.Message( + arbitration_id=can_id, + data=data, + is_extended_id=False + ) + bus.send(msg) + print(f"[Отправка] CAN ID: 0x{can_id:03X}, Данные: {list(data)}") + return True + except can.CanError as e: + print(f"Ошибка CAN: {e}") + return False + +def receive_response(bus, timeout=2.0): + """Ожидание ответа""" + start_time = time.time() + while time.time() - start_time < timeout: + msg = bus.recv(timeout=0.1) + if msg: + print(f"[Прием] CAN ID: 0x{msg.arbitration_id:03X}, Данные: {list(msg.data)}") + return msg + print("[Ошибка] Таймаут") + return None + +def validate_crc16(data): + """Функция расчета CRC16 (MODBUS)""" + crc = 0xFFFF + for byte in data: + crc ^= byte + for _ in range(8): + if crc & 0x0001: + crc = (crc >> 1) ^ 0xA001 + else: + crc >>= 1 + return crc + +# Инициализация +bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan') + +# ======= 1. Отправляем команду изменить ID ======= + +# Весь буфер: id + команда + параметры +OLD_WITH_REG = (OLD_DEVICE_ID << 4) | REG_WRITE +id_bytes = list(OLD_WITH_REG.to_bytes(2, byteorder='little')) + +# Важные части сообщения: address (id), команда, параметры +data_write = [REG_ID, NEW_DEVICE_ID] # команда изменить ID + +# Полностью собираем массив для CRC (включая id и команду) +full_data_for_crc = id_bytes + data_write + +# Расчет CRC по всему пакету +crc = validate_crc16(full_data_for_crc) +crc_bytes = list(crc.to_bytes(2, byteorder='little')) + +# Итоговый пакет: команда + параметры + CRC +packet_write = data_write + crc_bytes + +print("Отправляем: команда изменить ID + CRC:", packet_write) +# Отправляем с `OLD_DEVICE_ID` в качестве адреса +send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write) + +time.sleep(0.5) + +# ======= 2. Запрашиваем текущий ID (используем новый адрес) ======= + +# Теперь для запроса используем **уже новый id** +NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ +current_id_bytes = list(NEW_WITH_REG.to_bytes(2, byteorder='little')) +data_read = [REG_ID, 0x00] + +full_data_for_crc = current_id_bytes + data_read +crc = validate_crc16(full_data_for_crc) +crc_bytes = list(crc.to_bytes(2, byteorder='little')) +packet_read = data_read + crc_bytes + +print("Запрос на чтение ID + CRC (после смены):", packet_read) +send_can_message(bus, (NEW_DEVICE_ID << 4) | REG_READ, packet_read) + +# ======= 3. Получение и проверка ответа ======= + +response = receive_response(bus) +if response: + data = response.data + if len(data) < 4: + print("Ответ слишком короткий") + else: + received_crc = int.from_bytes(data[-2:], byteorder='little') + print("Полученный CRC: ", received_crc) + # Расчет CRC по всему пакету без CRC + calc_crc = validate_crc16(data[:-2]) + if received_crc == calc_crc: + if data[0] == ord('I') and data[1] == NEW_DEVICE_ID: + print(f"\nУСПЕХ! ID устройства изменен на 0x{NEW_DEVICE_ID:02X}") + else: + print(f"Некорректный ответ: {list(data)}") + else: + print("CRC не совпадает, данные повреждены.") +else: + print("Нет ответа от устройства.") + +bus.shutdown()