From a2f1c2557a3e74e252d0bae494b0cdeb32cbe776 Mon Sep 17 00:00:00 2001 From: lulko Date: Fri, 18 Apr 2025 12:50:07 +0300 Subject: [PATCH] translate comment --- controller/fw/embed/include/flash.h | 2 +- controller/fw/embed/src/flash.cpp | 30 +-- controller/fw/embed/src/main.cpp | 326 +++++++++++----------------- 3 files changed, 146 insertions(+), 212 deletions(-) diff --git a/controller/fw/embed/include/flash.h b/controller/fw/embed/include/flash.h index f15b6ea..39be097 100644 --- a/controller/fw/embed/include/flash.h +++ b/controller/fw/embed/include/flash.h @@ -40,7 +40,7 @@ enum { // Flash keys for unlocking flash memory #define BYTE32 0 #define BYTE8 1 -#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение +#define UPDATE_FLAG 0xDEADBEEF // Unique 32bit value //FLASH SET ONE PROGRAMM WORD #define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1 #define FLASH_32BYTE \ diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp index f22bc51..1c9a265 100644 --- a/controller/fw/embed/src/flash.cpp +++ b/controller/fw/embed/src/flash.cpp @@ -120,18 +120,18 @@ bool validata_crc(FLASH_RECORD* crc){ } uint16_t validate_crc16(uint8_t *data, uint32_t length) { - uint16_t crc = 0xFFFF; // Начальное значение для MODBUS + uint16_t crc = 0xFFFF; // start value for CRC MODBUS while (length--) { - crc ^= *data++; // XOR с очередным байтом данных + crc ^= *data++; // XOR for (uint8_t i = 0; i < 8; i++) { if (crc & 0x0001) { - crc = (crc >> 1) ^ 0xA001; // Полином 0x8005 (reverse) + crc = (crc >> 1) ^ 0xA001; // polynome 0x8005 (reverse) } else { crc >>= 1; } } } - return crc; // Возвращаем вычисленный CRC + return crc; } @@ -152,11 +152,11 @@ void compact_page(){ 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 параметра валидный, сохраняем последнее значение + // if the crc does not match, we check further latest[rec.data_id] = rec; } else - //Если не совпадает продолжить читать флэш + // if continue; } @@ -164,7 +164,7 @@ void compact_page(){ write_ptr = SECTOR_6; // Сброс на начало for (int i = 0; i < PARAM_COUNT; i++) { if (latest[i].data_id != 0xFF) { - // Выравнивание перед каждой записью + // alignment if (write_ptr % 4 != 0) { write_ptr += (4 - (write_ptr % 4)); } @@ -177,27 +177,27 @@ void compact_page(){ void write_param(uint8_t param_id, uint8_t val) { FLASH_RECORD param_flash = {param_id, val}; - // __disable_irq(); // Запрещаем прерывания на время всей операции + // __disable_irq(); // Interrupt off param_flash.crc = validate_crc16((uint8_t*)¶m_flash,sizeof(param_flash) - 2);//Нахождение CRC для данных, хранящихся во флэш памяти - // Проверка выравнивания ДО проверки границ сектора кратного 4 + // check alignment if (write_ptr % 4 != 0) { write_ptr += (4 - (write_ptr % 4)); } - // Проверка переполнения с учётом выравнивания + // check buffer overflow if (write_ptr + FLASH_RECORD_SIZE >= SECTOR_6_END) { - compact_page(); // После compact_page write_ptr обновляется - // Повторно выравниваем после функции. То есть сколько не хватает для кратности + compact_page(); // after compact_page update + // alignment if (write_ptr % 4 != 0) { write_ptr += (4 - (write_ptr % 4)); } } - flash_write(write_ptr, ¶m_flash); //внутри функции итак автоматические инкрементируется указатель write_ptr на размер структуры + flash_write(write_ptr, ¶m_flash); //inside the function, the write_ptr pointer is automatically incremented by the size of the structure - // __enable_irq(); // Разрешаем прерывания + // __enable_irq(); // Interrupt on } FLASH_RECORD* load_params(){ @@ -206,7 +206,7 @@ FLASH_RECORD* load_params(){ FLASH_RECORD res; for(uint32_t addr = SECTOR_6;addr < SECTOR_6_END;addr +=FLASH_RECORD_SIZE) { flash_read(addr,&res); - /* провекра CRC */ + /* check 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; diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp index 8dd8e66..6554a89 100644 --- a/controller/fw/embed/src/main.cpp +++ b/controller/fw/embed/src/main.cpp @@ -15,14 +15,12 @@ #include "reg_cah.h" #include "flash.h" - void SysTick_Handler(void) { HAL_IncTick(); } - STM32_CAN Can(CAN2, DEF); -/* for FLASH */ +/* FLASH Related Definitions */ uint32_t flash_flag; uint8_t flag_can = 0; uint32_t flash_error; @@ -35,7 +33,7 @@ volatile uint8_t msg_ch; volatile uint8_t crc_h; volatile uint8_t crc_l; -volatile float kt = 0.1; //for torgue calculation +volatile float kt = 0.1; // Torque calculation constant static FLASH_RECORD* flash_rec; static FLASH_RECORD flash_buf[PARAM_COUNT]; @@ -70,8 +68,8 @@ void doMotor(char *cmd) { delayMicroseconds(2); } +// Empty interrupt handler to prevent default handling void CAN2_RX0_IRQHandler() { - // Пустая функция, но прерывание не приведет к Default Handler } void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, @@ -79,32 +77,35 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, Commander *commander, CommandCallback callback) { encoder->init(&spi); + // Driver configuration driver->pwm_frequency = 20000; driver->voltage_power_supply = 24; driver->voltage_limit = 24; driver->init(); + // Current sense initialization current_sense->linkDriver(driver); current_sense->init(); + // Motor configuration motor->linkSensor(encoder); motor->linkDriver(driver); motor->linkCurrentSense(current_sense); motor->useMonitoring(Serial); - motor->monitor_downsample = 5000; // default 0 + motor->monitor_downsample = 5000; // Default monitoring interval motor->controller = MotionControlType::angle; motor->torque_controller = TorqueControlType::voltage; motor->foc_modulation = FOCModulationType::SpaceVectorPWM; - // PID start + // PID Configuration motor->PID_velocity.P = 0.75; motor->PID_velocity.I = 20; motor->LPF_velocity.Tf = 0.005; motor->P_angle.P = 0.5; motor->LPF_angle.Tf = 0.001; - // PID end - motor->velocity_limit = 40; // Ограничение по скорости вращения rad/s (382 rpm) + // Motor limits + motor->velocity_limit = 40; // Speed limit in rad/s (382 rpm) motor->voltage_limit = 24; motor->current_limit = 0.5; @@ -114,41 +115,39 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor, } void send_can_with_id_crc(uint32_t id, uint8_t message_type, const void* data, size_t data_length) { - // Создаем сообщение + // Create CAN message CAN_message_t msg; msg.id = id; - msg.len = 8; // или как в протоколе + msg.len = 8; // Protocol-defined message length msg.buf[0] = message_type; memcpy(&msg.buf[1], data, data_length); - // Формируем массив для CRC, включающий ID и все данные + // Prepare CRC calculation buffer (ID + data) size_t crc_data_size = sizeof(msg.id) + data_length; uint8_t crc_data[crc_data_size]; - // Копируем ID + // Copy message ID memcpy(crc_data, &msg.id, sizeof(msg.id)); - // Копируем все байты data + // Copy all data bytes memcpy(crc_data + sizeof(msg.id), data, data_length); - // Расчет CRC + // Calculate CRC uint16_t crc_value = validate_crc16(crc_data, crc_data_size); - // Вставляем CRC в буфер + // Insert CRC into buffer msg.buf[6] = crc_value & 0xFF; msg.buf[7] = (crc_value >> 8) & 0xFF; - // Отправляем + // Send message Can.write(msg); __NOP(); } - - void send_velocity() { float current_velocity = motor.shaftVelocity(); flash_rec = load_params(); - if (flash_rec == nullptr) { // Проверка на NULL - // Обработка ошибки: запись в лог, сигнализация и т.д. + if (flash_rec == nullptr) { // Null check + // Error handling: logging, alerts, etc. return; } uint8_t value = flash_rec[vel].value; @@ -158,13 +157,11 @@ void send_velocity() { void send_angle() { float current_angle = motor.shaftAngle(); - flash_rec = load_params(); - if (flash_rec == nullptr) { // Проверка на NULL - // Обработка ошибки: запись в лог, сигнализация и т.д. + if (flash_rec == nullptr) { // Null check + // Error handling: logging, alerts, etc. 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)); } @@ -179,23 +176,22 @@ void send_motor_enabled() { } void send_foc_state() { - /* data for reading of firmware */ - flash_rec = load_params(); - if (flash_rec == nullptr) { // Проверка на NULL - // Обработка ошибки: запись в лог, сигнализация и т.д. - return; - } - + /* Firmware data reading */ + flash_rec = load_params(); + if (flash_rec == nullptr) { // Null check + // Error handling: logging, alerts, etc. + 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() { - /* data for reading of firmware */ + /* Firmware data reading */ flash_rec = load_params(); - if (flash_rec == nullptr) { // Проверка на NULL - // Обработка ошибки: запись в лог, сигнализация и т.д. + if (flash_rec == nullptr) { // Null check + // Error handling: logging, alerts, etc. return; } uint8_t id = flash_rec[addr_id].value; @@ -204,8 +200,8 @@ void send_id() { } void send_motor_torque() { - float i_q = motor.current.q; // Ток по оси q (А) - float torque = kt * i_q; // Расчет момента + float i_q = motor.current.q; // Q-axis current (A) + float torque = kt * i_q; // Torque calculation torque *= 100; flash_rec = load_params(); CAN_TX_msg.id = flash_rec->value; @@ -215,10 +211,9 @@ void send_motor_torque() { Can.write(CAN_TX_msg); } - void send_pid(uint8_t param_pid){ flash_rec = load_params(); - if (flash_rec == nullptr) { // Проверка на NULL + if (flash_rec == nullptr) { // Null check return; } uint8_t id = flash_rec[addr_id].value; @@ -229,10 +224,8 @@ void send_pid(uint8_t param_pid){ 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; @@ -243,180 +236,122 @@ void send_pid(uint8_t param_pid){ 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.enable(); // Enable motor if disabled 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; + 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); - + 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; // Extract message channel + id_x = (msg_id >> 4) & 0x7FF; // Extract device address - msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать - id_x = (msg_id >> 4) & 0x7FF; //получение адреса устройства страшие 2 бита msg_ch = msg_id & 0xF; // получения id, чтобы выбрать, что делать - - - /* Вычисление CRC */ - // Объединение старшего и младшего байтов для получения полученного CRC + /* CRC Calculation */ uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8); - uint8_t data[10] = {0}; //буфер хранения сообщения и расчета его CRC для проверки + uint8_t data[10] = {0}; // Message buffer for CRC verification - // Копируем ID сообщения в буфер данных для расчета CRC 2 байта + // Copy message ID (2 bytes) memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id)); - - // Копируем данные сообщения в буфер (без байтов CRC) + // Copy message data (excluding CRC bytes) memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2); - // Рассчитываем CRC для полученных данных + // Calculate CRC uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2); - // Проверяем совпадение CRC + // Verify CRC match if (calculated_crc != received_crc) { - // Несовпадение CRC, игнорируем сообщение - return; + return; // Ignore message on CRC mismatch } - - /* 0x691 - 69 - адрес устройства - 1 - что делать дальше с данными */ - + /* Message Structure: 0x691 + 69 - Device address + 1 - Action code */ 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; + switch(msg.buf[0]) { + case REG_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; } - - 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; + 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; + } } } } -} - - - volatile uint32_t ipsr_value = 0; - void foc_step(BLDCMotor *motor, Commander *commander) { if (motor_control_inputs.target_velocity != 0 || motor->controller == MotionControlType::velocity) { @@ -424,7 +359,6 @@ void foc_step(BLDCMotor *motor, Commander *commander) { motor->controller = MotionControlType::velocity; } motor->target = motor_control_inputs.target_velocity; - } else { if (motor->controller != MotionControlType::angle) { motor->controller = MotionControlType::angle; @@ -438,45 +372,45 @@ void foc_step(BLDCMotor *motor, Commander *commander) { commander->run(); } - - - void setup(){ - /* bias for vector int */ + // Vector table initialization (commented out) // __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); + + 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(); + pinMode(PC11, OUTPUT); + pinMode(PC10,OUTPUT); + GPIOC->ODR &= ~GPIO_ODR_OD10; + + Can.begin(); + Can.setBaudRate(1000000); + + // Load parameters from flash flash_rec = load_params(); for(int i = 0;i < PARAM_COUNT;i++) flash_buf[i] = flash_rec[i]; + + // Initialize FOC system setup_foc(&encoder, &motor, &driver, ¤t_sense, &command, doMotor); + + // Default motor configuration GPIOC->ODR |= GPIO_ODR_OD11; motor.torque_controller = TorqueControlType::foc_current; motor.controller = MotionControlType::torque; __enable_irq(); - } +} void loop() { - foc_step(&motor, &command); + foc_step(&motor, &command); CAN_message_t msg; - GPIOC->ODR ^= GPIO_ODR_OD11; + GPIOC->ODR ^= GPIO_ODR_OD11; // Toggle status LED delay(500); + + // Process incoming CAN messages while (Can.read(msg)) { listen_can(msg); } -} +} \ No newline at end of file