Ready for work
This commit is contained in:
parent
7b1b5831a4
commit
c02006698e
5 changed files with 675 additions and 51 deletions
72
controller/fw/embed/include/flash.h
Normal file
72
controller/fw/embed/include/flash.h
Normal file
|
|
@ -0,0 +1,72 @@
|
|||
#ifndef FLASH_H_
|
||||
#define FLASH_H_
|
||||
#include "stm32f446xx.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
/* for addr in FLASH */
|
||||
typedef struct{
|
||||
uint8_t data_id; // data_id = id register of can
|
||||
uint8_t value;
|
||||
uint16_t crc;
|
||||
// uint32_t write_ptr_now;
|
||||
}FLASH_RECORD;
|
||||
enum {
|
||||
addr_id = 0,
|
||||
foc_id = 1,
|
||||
kp_id = 2,
|
||||
ki_id = 3,
|
||||
kd_id = 4
|
||||
};
|
||||
|
||||
#define FLASH_RECORD_SIZE sizeof(FLASH_RECORD) //size flash struct
|
||||
#define PARAM_COUNT 3 // count data in flash
|
||||
|
||||
// Flash sectors for STM32F407
|
||||
|
||||
#define SECTOR_2 0x08008000 // 16KB
|
||||
#define SECTOR_3 0x0800C000 // 16KB
|
||||
#define SECTOR_4 0x08010000 // 64KB
|
||||
#define SECTOR_5 0x08020000 // 128KB
|
||||
#define SECTOR_6 0x08040000 // 128KB
|
||||
#define SECTOR_6_END (SECTOR_6 + 128 * 1024) // sector 6 end
|
||||
#define SECTOR_7 0x08060000 // 128KB
|
||||
|
||||
|
||||
#define FLAG_BOOT (0x08040000 + 4)
|
||||
// Flash keys for unlocking flash memory
|
||||
#define BYTE32 0
|
||||
#define BYTE8 1
|
||||
#define UPDATE_FLAG 0xDEADBEEF // Уникальное 32-битное значение
|
||||
//FLASH SET ONE PROGRAMM WORD
|
||||
#define FLASH_8BYTE FLASH->CR &= ~FLASH_CR_PSIZE & ~FLASH_CR_PSIZE_1
|
||||
#define FLASH_32BYTE \
|
||||
FLASH->CR = (FLASH->CR & ~FLASH_CR_PSIZE) | (0x2 << FLASH_CR_PSIZE_Pos)
|
||||
|
||||
// Flash command bits
|
||||
#define FLASH_LOCK FLASH->CR |= FLASH_CR_LOCK
|
||||
#define FLASH_UNLOCK FLASH->KEYR = FLASH_KEY1; FLASH->KEYR = FLASH_KEY2
|
||||
|
||||
|
||||
// Flash status flags
|
||||
#define FLASH_BUSY (FLASH->SR & FLASH_SR_BSY)
|
||||
#define FLASH_ERROR (FLASH->SR & (FLASH_SR_WRPERR | FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR))
|
||||
|
||||
//for bootloader
|
||||
typedef void(*pFunction)(void);
|
||||
|
||||
// Function prototypes
|
||||
void flash_unlock(void);
|
||||
void flash_lock(void);
|
||||
void erase_sector(uint8_t sector);
|
||||
void flash_program_word(uint32_t address, uint32_t data,uint32_t byte_len);
|
||||
uint8_t flash_read_word(uint32_t address);
|
||||
void write_param(uint8_t param_id, uint8_t val);
|
||||
FLASH_RECORD* load_params();
|
||||
void compact_page();
|
||||
void flash_read(uint32_t addr,FLASH_RECORD* ptr);
|
||||
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_ */
|
||||
40
controller/fw/embed/include/reg_cah.h
Normal file
40
controller/fw/embed/include/reg_cah.h
Normal file
|
|
@ -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_
|
||||
221
controller/fw/embed/src/flash.cpp
Normal file
221
controller/fw/embed/src/flash.cpp
Normal file
|
|
@ -0,0 +1,221 @@
|
|||
#include "flash.h"
|
||||
#include <stdbool.h>
|
||||
#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;
|
||||
}
|
||||
|
|
@ -1,5 +1,6 @@
|
|||
// clang-format off
|
||||
#include <Arduino.h>
|
||||
#include "Arduino.h"
|
||||
#include "stm32f446xx.h"
|
||||
#include <SimpleFOC.h>
|
||||
#include <STM32_CAN.h>
|
||||
#include <AS5045.h>
|
||||
|
|
@ -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') {
|
||||
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();
|
||||
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_control_inputs.motor_enabled = 1;
|
||||
} else {
|
||||
motor.disable();
|
||||
motor_control_inputs.motor_enabled = 0;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
digitalWrite(PC10, !digitalRead(PC10));
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
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(){
|
||||
/* 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);
|
||||
// setup led pin
|
||||
|
||||
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();
|
||||
// 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();
|
||||
CAN_message_t msg;
|
||||
GPIOC->ODR ^= GPIO_ODR_OD11;
|
||||
delay(500);
|
||||
while (Can.read(msg)) {
|
||||
listen_can(msg);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
114
controller/fw/embed/test/python_test_id.py
Normal file
114
controller/fw/embed/test/python_test_id.py
Normal file
|
|
@ -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()
|
||||
Loading…
Add table
Add a link
Reference in a new issue