From c02006698ee500062f9ba3a59f96dec43585a45b Mon Sep 17 00:00:00 2001 From: lulko Date: Wed, 16 Apr 2025 22:51:39 +0300 Subject: [PATCH 1/6] Ready for work --- controller/fw/embed/include/flash.h | 72 ++++++ controller/fw/embed/include/reg_cah.h | 40 +++ controller/fw/embed/src/flash.cpp | 221 ++++++++++++++++ controller/fw/embed/src/main.cpp | 279 +++++++++++++++++---- controller/fw/embed/test/python_test_id.py | 114 +++++++++ 5 files changed, 675 insertions(+), 51 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..ab2fe80 --- /dev/null +++ b/controller/fw/embed/include/flash.h @@ -0,0 +1,72 @@ +#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, +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); +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..ba5ef5d 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) { @@ -85,7 +115,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 +125,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,45 +134,173 @@ 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 */ + flash_rec = load_params(); + if (flash_rec == nullptr) { // Проверка на NULL + // Обработка ошибки: запись в лог, сигнализация и т.д. + return; + } + CAN_TX_msg.id = flash_rec->value; + CAN_TX_msg.len = 8; + CAN_TX_msg.buf[0] = 'I'; + memcpy(&CAN_TX_msg.buf[1], &(flash_rec->value), sizeof(uint8_t)); + + uint8_t crc_data[sizeof(CAN_TX_msg.id) + 2] = {0}; // Размер: размер ID + 2 байта данных + memcpy(crc_data, &CAN_TX_msg.id, sizeof(CAN_TX_msg.id)); // Копируем ID (11/29 бит) + memcpy(crc_data + sizeof(CAN_TX_msg.id), &CAN_TX_msg.buf[0], 2); // Копируем 'I' и value + // Расчет CRC + uint16_t crc_value = validate_crc16(crc_data, sizeof(crc_data)); + + // Добавление CRC к сообщению + CAN_TX_msg.buf[6] = crc_value & 0xFF; // Добавляем старший байт CRC + CAN_TX_msg.buf[7] = (crc_value >> 8) & 0xFF; // Добавляем младший байт CRC + + Can.write(CAN_TX_msg); + __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 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(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_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; + + 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 +323,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..7429060 --- /dev/null +++ b/controller/fw/embed/test/python_test_id.py @@ -0,0 +1,114 @@ +import can +import time + +# Конфигурация +CAN_INTERFACE = 'can0' +OLD_DEVICE_ID = 0x69 +NEW_DEVICE_ID = 0x00 +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') + # Расчет 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() -- 2.49.0 From 9c9b1827055ade2cea0ee2eda6483f4305fa9346 Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 17 Apr 2025 11:10:56 +0300 Subject: [PATCH 2/6] Added function for send data --- controller/fw/embed/include/flash.h | 8 +-- controller/fw/embed/src/main.cpp | 92 ++++++++++++++++++----------- 2 files changed, 62 insertions(+), 38 deletions(-) diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index ab2fe80..70de7e8 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -15,13 +15,13 @@ typedef struct{ enum { addr_id = 0, foc_id = 1, -kp_id = 2, - ki_id = 3, - kd_id = 4 + angl = 2, + vel = 3, + kp_id = 4 }; #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct -#define PARAM_COUNT 3 // count data in flash +#define PARAM_COUNT 4 // count data in flash // Flash sectors for STM32F407 diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index ba5ef5d..d99e2f6 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -113,24 +113,58 @@ 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 + 2 байта данных (например, 'I' + value) + size_t crc_data_size = sizeof(msg.id) + 2; + uint8_t crc_data[crc_data_size]; + + // Копируем ID + memcpy(crc_data, &msg.id, sizeof(msg.id)); + // Копируем 2 байта данных (например, msg.buf[0..1], где msg.buf[0] — message_type, а дальше значение) + memcpy(crc_data + sizeof(msg.id), &msg.buf[0], 2); + + // Расчет 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); +} + + void send_velocity() { float current_velocity = motor.shaftVelocity(); - 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)); - 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(); - 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)); - 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',&value,sizeof(value)); } void send_motor_enabled() { @@ -143,12 +177,16 @@ void send_motor_enabled() { } 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); + /* 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 send_id() { @@ -158,22 +196,8 @@ void send_id() { // Обработка ошибки: запись в лог, сигнализация и т.д. return; } - CAN_TX_msg.id = flash_rec->value; - CAN_TX_msg.len = 8; - CAN_TX_msg.buf[0] = 'I'; - memcpy(&CAN_TX_msg.buf[1], &(flash_rec->value), sizeof(uint8_t)); - - uint8_t crc_data[sizeof(CAN_TX_msg.id) + 2] = {0}; // Размер: размер ID + 2 байта данных - memcpy(crc_data, &CAN_TX_msg.id, sizeof(CAN_TX_msg.id)); // Копируем ID (11/29 бит) - memcpy(crc_data + sizeof(CAN_TX_msg.id), &CAN_TX_msg.buf[0], 2); // Копируем 'I' и value - // Расчет CRC - uint16_t crc_value = validate_crc16(crc_data, sizeof(crc_data)); - - // Добавление CRC к сообщению - CAN_TX_msg.buf[6] = crc_value & 0xFF; // Добавляем старший байт CRC - CAN_TX_msg.buf[7] = (crc_value >> 8) & 0xFF; // Добавляем младший байт CRC - - Can.write(CAN_TX_msg); + uint8_t id = flash_rec[addr_id].value; + send_can_with_id_crc(id,'I',&id,sizeof(id)); __NOP(); } -- 2.49.0 From 317a4c48ea81636a1634362bbc7b27330e1f43b7 Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 17 Apr 2025 14:30:22 +0300 Subject: [PATCH 3/6] Add setup for angle using SimpleFOC --- controller/fw/embed/src/main.cpp | 19 ++++++++++--------- controller/fw/embed/test/python_test_id.py | 2 +- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index d99e2f6..c22f247 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -139,6 +139,7 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s // Отправка Can.write(msg); + __NOP(); } @@ -215,17 +216,13 @@ void send_motor_torque() { void setup_id(uint8_t my_id) { write_param(addr_id,my_id); - send_id(); + // send_id(); } - - -void send_data() { - // send_velocity(); - // send_angle(); - // send_motor_enabled(); - // read_temperature(); - // GPIOC->ODR ^= GPIO_ODR_OD11; +void setup_angle(int target_angle_rad) { + float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы + motor.controller = MotionControlType::angle; + motor.move(target_angle); } void listen_can(const CAN_message_t &msg) { @@ -275,6 +272,10 @@ void listen_can(const CAN_message_t &msg) { } break; + case MOTOR_ANGLE: + setup_angle(msg.buf[1]); + break; + case MOTOR_ENABLED: if (msg.buf[1] == 1) { motor.enable(); diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py index 7429060..433cd88 100644 --- a/controller/fw/embed/test/python_test_id.py +++ b/controller/fw/embed/test/python_test_id.py @@ -75,7 +75,7 @@ send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write) time.sleep(0.5) -# ======= 2. Запрашиваем текущий ID (используем новй адрес) ======= +# ======= 2. Запрашиваем текущий ID (используем новы й адрес) ======= # Теперь для запроса используем **уже новый id** NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ -- 2.49.0 From 06aae3981e1f40dcc22bd0389c558adb8bf33c2f Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 17 Apr 2025 15:53:03 +0300 Subject: [PATCH 4/6] fix work with float type of angle --- controller/fw/embed/src/main.cpp | 32 ++++++++++++---------- controller/fw/embed/test/python_test_id.py | 4 +-- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index c22f247..572cc52 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -117,32 +117,33 @@ void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, s // Создаем сообщение CAN_message_t msg; msg.id = id; - msg.len = 8; // или как у вас в протоколе + msg.len = 8; // или как в протоколе msg.buf[0] = message_type; memcpy(&msg.buf[1], data, data_length); - // Формируем массив для CRC: ID + 2 байта данных (например, 'I' + value) - size_t crc_data_size = sizeof(msg.id) + 2; + // Формируем массив для 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)); - // Копируем 2 байта данных (например, msg.buf[0..1], где msg.buf[0] — message_type, а дальше значение) - memcpy(crc_data + sizeof(msg.id), &msg.buf[0], 2); + // Копируем все байты 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; // старший байт + // Вставляем 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(); flash_rec = load_params(); @@ -163,9 +164,9 @@ void send_angle() { // Обработка ошибки: запись в лог, сигнализация и т.д. return; } - uint8_t value = flash_rec[angl].value; + // uint8_t value = flash_rec[angl].value; uint8_t id = flash_rec[addr_id].value; - send_can_with_id_crc(id,'A',&value,sizeof(value)); + send_can_with_id_crc(id,'A',¤t_angle,sizeof(current_angle)); } void send_motor_enabled() { @@ -219,8 +220,9 @@ void setup_id(uint8_t my_id) { // send_id(); } -void setup_angle(int target_angle_rad) { - float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы +void setup_angle(float target_angle) { + // float target_angle = target_angle_rad / 100.0f; // Предполагаем, что передается в значениях сотых градуса или сотые радианы + motor.enable(); // Включаем мотор если он отключен motor.controller = MotionControlType::angle; motor.move(target_angle); } @@ -273,7 +275,9 @@ void listen_can(const CAN_message_t &msg) { break; case MOTOR_ANGLE: - setup_angle(msg.buf[1]); + 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 MOTOR_ENABLED: diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py index 433cd88..4788dc1 100644 --- a/controller/fw/embed/test/python_test_id.py +++ b/controller/fw/embed/test/python_test_id.py @@ -3,8 +3,8 @@ import time # Конфигурация CAN_INTERFACE = 'can0' -OLD_DEVICE_ID = 0x69 -NEW_DEVICE_ID = 0x00 +OLD_DEVICE_ID = 0x00 +NEW_DEVICE_ID = 0x69 REG_WRITE = 0x8 REG_READ = 0x7 REG_ID = 0x1 -- 2.49.0 From e65857ca6fd55178a5d812b02dcd9f2a0e23c4b6 Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 17 Apr 2025 16:49:39 +0300 Subject: [PATCH 5/6] Added PID --- controller/fw/embed/include/flash.h | 4 +- controller/fw/embed/src/main.cpp | 60 ++++++++++++++++++++++ controller/fw/embed/test/python_test_id.py | 3 +- 3 files changed, 65 insertions(+), 2 deletions(-) diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index 70de7e8..f15b6ea 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -17,7 +17,9 @@ enum { foc_id = 1, angl = 2, vel = 3, - kp_id = 4 + pid_p = 4, + pid_i, + pid_d }; #define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 572cc52..4b0d974 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -215,6 +215,32 @@ void send_motor_torque() { Can.write(CAN_TX_msg); } + +void send_pid(uint8_t param_pid, float data){ + 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,sizeof(data)); +} + void setup_id(uint8_t my_id) { write_param(addr_id,my_id); // send_id(); @@ -227,6 +253,40 @@ void setup_angle(float target_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; diff --git a/controller/fw/embed/test/python_test_id.py b/controller/fw/embed/test/python_test_id.py index 4788dc1..b7a3084 100644 --- a/controller/fw/embed/test/python_test_id.py +++ b/controller/fw/embed/test/python_test_id.py @@ -75,7 +75,7 @@ send_can_message(bus, (OLD_DEVICE_ID << 4) | REG_WRITE, packet_write) time.sleep(0.5) -# ======= 2. Запрашиваем текущий ID (используем новы й адрес) ======= +# ======= 2. Запрашиваем текущий ID (используем новый адрес) ======= # Теперь для запроса используем **уже новый id** NEW_WITH_REG = (NEW_DEVICE_ID << 4) | REG_READ @@ -99,6 +99,7 @@ if response: 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: -- 2.49.0 From 7ef7228b31f6639c106e30d41317ac25234da51f Mon Sep 17 00:00:00 2001 From: lulko Date: Thu, 17 Apr 2025 16:55:48 +0300 Subject: [PATCH 6/6] add pid --- controller/fw/embed/src/main.cpp | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 4b0d974..8dd8e66 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -216,7 +216,7 @@ void send_motor_torque() { } -void send_pid(uint8_t param_pid, float data){ +void send_pid(uint8_t param_pid){ flash_rec = load_params(); if (flash_rec == nullptr) { // Проверка на NULL return; @@ -238,7 +238,7 @@ void send_pid(uint8_t param_pid, float data){ 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,sizeof(data)); + send_can_with_id_crc(id,param_pid,&data_send,sizeof(data_send)); } void setup_id(uint8_t my_id) { @@ -340,6 +340,18 @@ void listen_can(const CAN_message_t &msg) { 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(); @@ -378,6 +390,20 @@ void listen_can(const CAN_message_t &msg) { 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; } -- 2.49.0