robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

Added function for send data

Browse source
This commit is contained in:
lulko 2025-04-17 11:10:56 +03:00
parent c02006698e
commit 9c9b182705
2 changed files with 62 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

8
controller/fw/embed/include/flash.h
View file

@ -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

92
controller/fw/embed/src/main.cpp
View file

@ -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], &current_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], &current_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();
}