WIP: process_can #89

Draft
lulko wants to merge 3 commits from process_can into main
7 changed files with 205 additions and 42 deletions

View file

@ -22,7 +22,8 @@ enum {
firmw,
foc_id,
angl,
vel
vel,
torq
};
/* for saved in FLASH float data*/

View file

@ -19,9 +19,10 @@ void send_motor_enabled();
void send_id();
void firmware_update();
void send_pid_angle(uint8_t param_pid);
// void send_motor_torque();
void send_torque();
void send_pid(uint8_t param_pid);
void setup_id(uint8_t my_id);
void setup_angle(float target_angle);
void setup_pid_angle(uint8_t param_pid, float data);
void listen_can(const CAN_message_t &msg);
void setup_velocity(float target_velocity);
void listen_can(const CAN_message_t &msg);

View file

@ -1,5 +1,11 @@
#ifndef REG_CAH_H_
#define REG_CAH_H_
#pragma once
enum{
error_foc = 0,
error_canRX,
error_canTX
};
#define APP_ADDR 0x0800400 // 16KB - Application
#define ADDR_VAR 0x8040000
@ -16,6 +22,10 @@
#define REG_ID 0x01
#define REG_BAUDRATE 0x02
#define DATA_TYPE_ANGLE 0x03
#define DATA_TYPE_VELOCITY 0x04
#define DATA_TYPE_TORQUE 0x05
#define REG_MOTOR_POSPID_Kp 0x30
#define REG_MOTOR_POSPID_Ki 0x31
#define REG_MOTOR_POSPID_Kd 0x32
@ -43,6 +53,3 @@
#define CAN_MSG_MAX_LEN 7
#define CRC_SIZE 2
#define ID_SIZE sizeof(uint8_t)
#endif // REG_CAH_H_

View file

@ -14,10 +14,10 @@ build_flags =
-D STM32F446xx
-D HAL_CAN_MODULE_ENABLED
-D SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
lib_deps =
askuric/Simple FOC@^2.3.4
pazi88/STM32_CAN@^1.1.2
pazi88/STM32_CAN@^1.1.2
extra_scripts =
pre:gen_compile_commands.py

View file

@ -18,6 +18,7 @@
#include "config.h"
#include "process_can.h"
void SysTick_Handler(void) {
HAL_IncTick();
}
@ -98,9 +99,13 @@ MotorControlInputs motor_control_inputs;
void loop() {
__enable_irq();
lulko marked this conversation as resolved

А без этого не обойтись? Обязательно на каждом цикле включать прерывания? Это можно вынести в инициализацию? Если на каждом цикле включать прерывания, то это ведь может привести к неопределенному поведению?

А без этого не обойтись? Обязательно на каждом цикле включать прерывания? Это можно вынести в инициализацию? Если на каждом цикле включать прерывания, то это ведь может привести к неопределенному поведению?
send_angle();
send_velocity();
foc_step(&motor);
CAN_message_t msg;
// Process incoming CAN messages
while (Can.read(msg)) {

Может, чтобы не было потенциальных залипаний цикла надо ограничить while цикл? Сделать ограничение на итерации или сделать ограничения на количество обрабатываемых сообщений...

Может, чтобы не было потенциальных залипаний цикла надо ограничить while цикл? Сделать ограничение на итерации или сделать ограничения на количество обрабатываемых сообщений...
listen_can(msg);

View file

@ -40,9 +40,9 @@ void send_velocity() {
// Error handling: logging, alerts, etc.
return;
}
float value = flash_rec[vel].value;
// float value = flash_rec[vel].value;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'V',&value);
send_can_with_id_crc(id,'V',&current_velocity);
}
void send_angle() {
@ -67,6 +67,15 @@ void send_motor_enabled() {
send_can_with_id_crc(id,'M',&value);
}
void send_torque() {
float i_q = motor.current.q; // Q-axis current (A)
float torque = 100 * i_q; // Torque calculation
lulko marked this conversation as resolved

Это откуда такая формула? Если функция пока не дописана пометь ее
// TODO: something
Потом легко в коде ориентироваться
А можно пометить как
// TODO(#45): something
Типа сразу номер issue указываешь, если такой issue есть

Это откуда такая формула? Если функция пока не дописана пометь ее // TODO: something Потом легко в коде ориентироваться А можно пометить как // TODO(#45): something Типа сразу номер issue указываешь, если такой issue есть
if (flash_rec == nullptr) return;
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id, 'T', &torque);
}
void send_id() {
/* Firmware data reading */
if (flash_rec == nullptr) { // Null check
@ -78,16 +87,10 @@ void send_id() {
send_can_with_id_crc(id,'I',&id);
}
// void send_motor_torque() {
// float i_q = motor.current.q; // Q-axis current (A)
// float torque = kt * i_q; // Torque calculation
// torque *= 100;
// 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_error(uint8_t error_code){
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'P',&error_code);
}
void send_pid_angle(uint8_t param_pid){
if (flash_rec == nullptr) { // Null check
@ -129,31 +132,36 @@ void setup_angle(float target_angle) {
// motor.move(target_angle);
}
// void setup_pid_angle(uint8_t param_pid, uint32_t data){
// conv_float_to_int.f = data;
// switch (param_pid) {
// case pid_p:
// motor.P_angle.P = conv_float_to_int.f;
// break;
// case pid_i:
// motor.P_angle.I = conv_float_to_int.f;
// break;
// case pid_d:
// motor.P_angle.D = conv_float_to_int.f;
// break;
// default:
// break;
// }
// write_param(param_pid,data);
// }
/**
* @brief Set the up velocity object
*
* @param target_velocity
*/
void setup_velocity(float target_velocity) {
motor.enable();
motor_control_inputs.target_velocity = target_velocity;
}
void send_data_type(uint8_t type_d){
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'D',&type_d);
}
/**
* @brief Function for process data from CAN
* @details Function check your ID deviceю. Compare receive and calculated CRC.
* If ID && CRC == TRUE, then process message or dont send data.
* If if and crc = true:
* Check CAN_REG
* Gives your data type
* Write with data or send data
* @param msg
*/
void listen_can(const CAN_message_t &msg) {
msg_id = msg.id;
msg_ch = msg_id & 0xF; // Extract message channel
uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
uint8_t data_type = 1; //type for work with foc
/* CRC Calculation */
uint16_t received_crc = (msg.buf[msg.len - 2]) | (msg.buf[msg.len - 1] << 8);
uint8_t data[10] = {0}; // Message buffer for CRC verification
@ -171,6 +179,7 @@ void listen_can(const CAN_message_t &msg) {
return; // Ignore message on CRC mismatch
}
flash_rec = load_params();
/* Message Structure: 0x691
69 - Device address
1 - Action code */
@ -211,6 +220,18 @@ void listen_can(const CAN_message_t &msg) {
write_param(pid_d,conv_float_to_int.i);
break;
case DATA_TYPE_ANGLE:
data_type = DATA_TYPE_ANGLE;
break;
case DATA_TYPE_VELOCITY:
data_type = DATA_TYPE_VELOCITY;
break;
case DATA_TYPE_TORQUE:
data_type = DATA_TYPE_TORQUE;
break;
case FIRMWARE_UPDATE:
firmware_update();
break;
@ -233,6 +254,9 @@ void listen_can(const CAN_message_t &msg) {
case MOTOR_VELOCITY: send_velocity(); break;
case MOTOR_ANGLE: send_angle(); break;
case MOTOR_ENABLED: send_motor_enabled(); break;
case DATA_TYPE_ANGLE: send_data_type(uint8_t(DATA_TYPE_ANGLE)); break;
case DATA_TYPE_VELOCITY: send_data_type(uint8_t(DATA_TYPE_VELOCITY)); break;
case DATA_TYPE_TORQUE: send_data_type(uint8_t(DATA_TYPE_TORQUE)); break;
// case MOTOR_TORQUE: send_motor_torque(); break;
// case FOC_STATE: send_foc_state(); break;
case REG_MOTOR_POSPID_Kp: send_pid_angle(pid_p); break;
@ -241,5 +265,34 @@ void listen_can(const CAN_message_t &msg) {
default: break;
}
}
/* If msg_ch != REG_WRITE or REG_READ, then SimpleFOC*/
else{
switch(data_type) {
/* Read after write*/
case DATA_TYPE_ANGLE:
send_angle();
memcpy(&motor_control_inputs.target_angle, &msg.buf[1], sizeof(float));
setup_angle(motor_control_inputs.target_angle);
break;
case DATA_TYPE_VELOCITY:{
send_velocity();
float vel = 0.0f;
memcpy(&vel, &msg.buf[1], sizeof(float));
setup_velocity(vel);
break;
}
case DATA_TYPE_TORQUE:
send_torque();
break;
default:
send_error(error_foc);
break;
}
}
}
}

View file

@ -0,0 +1,96 @@
import can
import time
import sys
import struct
# Конфигурация
CAN_INTERFACE = 'can0'
DEVICE_ID = 0x69 # Текущий ID устройства
REG_READ = 0x7
REG_WRITE = 0x8
DATA_TYPE_ANGLE = 0x03
DATA_TYPE_VELOCITY = 0x04
DATA_TYPE_TORQUE = 0x05
# CRC функция (аналогичная устройству)
def validate_crc16(data):
crc = 0xFFFF
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x0001:
crc = (crc >> 1) ^ 0xA001
else:
crc >>= 1
return crc
def send_can_message(bus, can_id, data):
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=1.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 test_simplefoc_else_block():
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
# 1. Установка типа данных (DATA_TYPE_ANGLE)
can_id_write = (DEVICE_ID << 4) | REG_WRITE
data_set_type = [DATA_TYPE_ANGLE, 0x00]
full_data = list(can_id_write.to_bytes(2, 'little')) + data_set_type
crc = validate_crc16(full_data)
crc_bytes = list(crc.to_bytes(2, 'little'))
packet = data_set_type + crc_bytes
send_can_message(bus, can_id_write, packet)
time.sleep(0.1) # Ожидание обработки
# 2. Отправка SimpleFOC сообщения (угол)
target_angle = 45.0
angle_bytes = struct.pack('<f', target_angle)
can_id_simplefoc = (DEVICE_ID << 4) | 0x01 # Не REG_READ/REG_WRITE
payload = [0x00] + list(angle_bytes) + [0x00] # [type placeholder, angle, padding]
# Расчет CRC
full_data_sf = list(can_id_simplefoc.to_bytes(2, 'little')) + payload
crc_sf = validate_crc16(full_data_sf)
payload += list(crc_sf.to_bytes(2, 'little'))
# Отправка
print("\nТест SimpleFOC (блок else):")
send_can_message(bus, can_id_simplefoc, payload)
# 3. Проверка ответа (отправка угла + установка нового угла)
response = receive_response(bus)
if response:
# Проверка структуры ответа
if response.data[0] == ord('A'):
print("Успех: Отправлен текущий угол")
else:
print("Ошибка: Неверный тип ответа")
else:
print("Ошибка: Нет ответа от устройства")
# 4. Проверка установки нового угла (интеграционно)
# ... (может требовать дополнительной проверки на устройстве)

Можно добавить тест на изменение режима управленя и отправку скорости. 2 раза подряд угол и 2 раза подряд скорость

Можно добавить тест на изменение режима управленя и отправку скорости. 2 раза подряд угол и 2 раза подряд скорость
bus.shutdown()
if __name__ == "__main__":
test_simplefoc_else_block()