Work ID test
This commit is contained in:
parent
e80f04d857
commit
207b889fef
4 changed files with 164 additions and 89 deletions
|
|
@ -4,13 +4,19 @@
|
|||
#include "flash.h"
|
||||
#include "reg_cah.h"
|
||||
|
||||
#define CAN_TIMEOUT 50
|
||||
#define CAN_SILENCE_TIMEOUT 500
|
||||
#define MAX_CAN_READS 2
|
||||
extern FLASH_RECORD *flash_rec;
|
||||
extern volatile uint16_t msg_id;
|
||||
extern volatile uint16_t id_x;
|
||||
extern volatile uint8_t msg_ch;
|
||||
extern volatile uint8_t crc_h;
|
||||
extern volatile uint8_t crc_l;
|
||||
extern volatile bool send_blocked;
|
||||
|
||||
static volatile bool need_send_angle = false;
|
||||
static volatile bool need_send_velocity = false;
|
||||
|
||||
void send_velocity();
|
||||
void send_angle();
|
||||
|
|
@ -19,10 +25,12 @@ void send_motor_enabled();
|
|||
void send_id();
|
||||
void firmware_update();
|
||||
void send_pid_angle(uint8_t param_pid);
|
||||
void send_torque();
|
||||
void send_with_confirmation(void (*send_func)(void));
|
||||
// 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_velocity(float target_velocity);
|
||||
void process_can_messages();
|
||||
|
||||
void listen_can(const CAN_message_t &msg);
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ uint8_t flag_can = 0;
|
|||
uint32_t flash_error;
|
||||
FLASH_EraseInitTypeDef pEraseInit;
|
||||
uint32_t SectorError;
|
||||
|
||||
uint32_t timeout;
|
||||
|
||||
/* bool for test CAN */
|
||||
volatile bool CAN_GET = false;
|
||||
|
|
@ -95,26 +95,22 @@ MotorControlInputs motor_control_inputs;
|
|||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void loop() {
|
||||
__enable_irq();
|
||||
send_angle();
|
||||
send_velocity();
|
||||
foc_step(&motor);
|
||||
CAN_message_t msg;
|
||||
|
||||
|
||||
|
||||
// Process incoming CAN messages
|
||||
while (Can.read(msg)) {
|
||||
listen_can(msg);
|
||||
CAN_GET = true;
|
||||
}
|
||||
/* If receive data from CAN */
|
||||
if(CAN_GET) {
|
||||
|
||||
CAN_GET = false;
|
||||
}
|
||||
bool is_can_busy() {
|
||||
return (CAN2->TSR & (CAN_TSR_TME0 | CAN_TSR_TME1 | CAN_TSR_TME2)) != 0x07;
|
||||
}
|
||||
|
||||
void loop() {
|
||||
process_can_messages();
|
||||
|
||||
static uint32_t last_send = 0;
|
||||
if(!send_blocked && !is_can_busy() && (HAL_GetTick() - last_send >= 50)) {
|
||||
send_angle();
|
||||
send_velocity();
|
||||
last_send = HAL_GetTick();
|
||||
}
|
||||
|
||||
foc_step(&motor);
|
||||
HAL_Delay(1);
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
|
||||
static CAN_message_t CAN_TX_msg;
|
||||
static CAN_message_t CAN_inMsg;
|
||||
|
||||
|
||||
static uint8_t data_type = DATA_TYPE_ANGLE;
|
||||
volatile bool send_blocked = false;
|
||||
template <typename T>
|
||||
void send_can_with_id_crc(uint8_t id, uint8_t message_type, T* data) {
|
||||
// Create CAN message
|
||||
|
|
@ -44,7 +44,7 @@ void send_velocity() {
|
|||
uint8_t id = flash_rec[addr_id].value;
|
||||
send_can_with_id_crc(id,'V',¤t_velocity);
|
||||
}
|
||||
|
||||
|
||||
void send_angle() {
|
||||
float current_angle = motor.shaftAngle();
|
||||
if (flash_rec == nullptr) { // Null check
|
||||
|
|
@ -68,13 +68,13 @@ void send_motor_enabled() {
|
|||
}
|
||||
|
||||
|
||||
void send_torque() {
|
||||
float i_q = motor.current.q; // Q-axis current (A)
|
||||
float torque = 100 * i_q; // Torque calculation
|
||||
if (flash_rec == nullptr) return;
|
||||
uint8_t id = flash_rec[addr_id].value;
|
||||
send_can_with_id_crc(id, 'T', &torque);
|
||||
}
|
||||
// void send_torque() {
|
||||
// float i_q = motor.current.q; // Q-axis current (A)
|
||||
// float torque = 100 * i_q; // Torque calculation
|
||||
// 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 */
|
||||
|
|
@ -147,6 +147,39 @@ void send_data_type(uint8_t type_d){
|
|||
send_can_with_id_crc(id,'D',&type_d);
|
||||
}
|
||||
|
||||
// Вспомогательные функции
|
||||
void send_with_confirmation(void (*send_func)(void)) {
|
||||
uint32_t t_start = HAL_GetTick();
|
||||
send_func();
|
||||
|
||||
// Ожидание подтверждения отправки
|
||||
while (!(CAN2->TSR & (CAN_TSR_RQCP0 | CAN_TSR_RQCP1 | CAN_TSR_RQCP2))) {
|
||||
if (HAL_GetTick() - t_start > CAN_TIMEOUT) break;
|
||||
}
|
||||
}
|
||||
|
||||
void process_can_messages() {
|
||||
static uint32_t last_received = HAL_GetTick();
|
||||
CAN_message_t msg;
|
||||
uint8_t count = 0;
|
||||
|
||||
// block send while data cant
|
||||
send_blocked = true;
|
||||
|
||||
// Чтение сообщений с ограничением
|
||||
while(count < MAX_CAN_READS && Can.read(msg)) {
|
||||
listen_can(msg);
|
||||
last_received = HAL_GetTick();
|
||||
count++;
|
||||
}
|
||||
|
||||
// Проверка таймаута молчания
|
||||
if(HAL_GetTick() - last_received > CAN_SILENCE_TIMEOUT) {
|
||||
// Разблокируем отправку при отсутствии сообщений
|
||||
send_blocked = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Function for process data from CAN
|
||||
* @details Function check your ID deviceю. Compare receive and calculated CRC.
|
||||
|
|
@ -160,8 +193,7 @@ void send_data_type(uint8_t type_d){
|
|||
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
|
||||
uint16_t id_x = (msg_id >> 4) & 0x7FF; // Extract device address
|
||||
/* 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
|
||||
|
|
@ -271,7 +303,8 @@ void listen_can(const CAN_message_t &msg) {
|
|||
switch(data_type) {
|
||||
/* Read after write*/
|
||||
case DATA_TYPE_ANGLE:
|
||||
send_angle();
|
||||
send_angle();
|
||||
delay(200);
|
||||
memcpy(&motor_control_inputs.target_angle, &msg.buf[1], sizeof(float));
|
||||
setup_angle(motor_control_inputs.target_angle);
|
||||
break;
|
||||
|
|
@ -285,7 +318,7 @@ void listen_can(const CAN_message_t &msg) {
|
|||
}
|
||||
|
||||
case DATA_TYPE_TORQUE:
|
||||
send_torque();
|
||||
// send_torque();
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
|||
|
|
@ -1,12 +1,23 @@
|
|||
import can
|
||||
import time
|
||||
import sys
|
||||
|
||||
# Конфигурация
|
||||
CAN_INTERFACE = 'can0'
|
||||
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства (по умолчанию)
|
||||
OLD_DEVICE_ID = int(sys.argv[1]) # Текущий ID устройства
|
||||
REG_READ = 0x7 # Код команды чтения
|
||||
REG_ID = 0x01 # Адрес регистра с ID устройства
|
||||
|
||||
def flush_can_buffer(bus, duration=0.3):
|
||||
"""Очистка входного буфера CAN"""
|
||||
start_time = time.time()
|
||||
flushed_count = 0
|
||||
while time.time() - start_time < duration:
|
||||
msg = bus.recv(timeout=0)
|
||||
if msg:
|
||||
flushed_count += 1
|
||||
print(f"Очищено сообщений из буфера: {flushed_count}")
|
||||
|
||||
def send_can_message(bus, can_id, data):
|
||||
"""Отправка CAN-сообщения"""
|
||||
try:
|
||||
|
|
@ -23,14 +34,14 @@ def send_can_message(bus, can_id, data):
|
|||
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("[Ошибка] Таймаут")
|
||||
print("[Ошибка] Таймаут приема")
|
||||
return None
|
||||
|
||||
def validate_crc16(data):
|
||||
|
|
@ -45,64 +56,91 @@ def validate_crc16(data):
|
|||
crc >>= 1
|
||||
return crc
|
||||
|
||||
# Инициализация CAN-интерфейса
|
||||
bus = can.interface.Bus(channel=CAN_INTERFACE, bustype='socketcan')
|
||||
def main():
|
||||
# Инициализация CAN-интерфейса
|
||||
try:
|
||||
bus = can.interface.Bus(
|
||||
channel=CAN_INTERFACE,
|
||||
bustype='socketcan',
|
||||
bitrate=1000000 # Совпадает с устройством
|
||||
)
|
||||
except Exception as e:
|
||||
print(f"Ошибка инициализации CAN: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
# ======= 1. Запрос текущего ID устройства =======
|
||||
# ======= 1. Подготовка запроса =======
|
||||
can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
|
||||
data_read = [REG_ID, 0x00]
|
||||
|
||||
# Формируем полные данные для расчета CRC:
|
||||
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
|
||||
|
||||
# Рассчитываем CRC
|
||||
crc = validate_crc16(full_data_for_crc)
|
||||
crc_bytes = list(crc.to_bytes(2, 'little'))
|
||||
|
||||
# Собираем итоговый пакет
|
||||
packet_read = data_read + crc_bytes
|
||||
|
||||
# Формируем CAN ID для чтения: (OLD_DEVICE_ID << 4) | REG_READ
|
||||
can_id_read = (OLD_DEVICE_ID << 4) | REG_READ
|
||||
|
||||
# Данные для запроса: [регистр, резервный байт]
|
||||
data_read = [REG_ID, 0x00]
|
||||
|
||||
# Формируем полные данные для расчета CRC:
|
||||
# - CAN ID разбивается на 2 байта (little-endian)
|
||||
# - Добавляем данные запроса
|
||||
full_data_for_crc = list(can_id_read.to_bytes(2, 'little')) + data_read
|
||||
|
||||
# Рассчитываем CRC и разбиваем на байты (little-endian)
|
||||
crc = validate_crc16(full_data_for_crc)
|
||||
crc_bytes = list(crc.to_bytes(2, 'little'))
|
||||
|
||||
# Собираем итоговый пакет: данные + CRC
|
||||
packet_read = data_read + crc_bytes
|
||||
|
||||
print("Запрос на чтение ID:", packet_read)
|
||||
send_can_message(bus, can_id_read, packet_read)
|
||||
|
||||
# ======= 2. Получение и проверка ответа =======
|
||||
response = receive_response(bus)
|
||||
if response:
|
||||
# ======= 2. Отправка запроса с повторами =======
|
||||
max_retries = 3
|
||||
response = None
|
||||
|
||||
for attempt in range(max_retries):
|
||||
print(f"\nПопытка {attempt+1}/{max_retries}")
|
||||
|
||||
# Очистка буфера перед отправкой
|
||||
flush_can_buffer(bus, 0.3)
|
||||
|
||||
# Отправка запроса
|
||||
print(f"Отправка запроса на чтение ID: {packet_read}")
|
||||
if not send_can_message(bus, can_id_read, packet_read):
|
||||
print("Ошибка отправки, повтор...")
|
||||
time.sleep(0.2)
|
||||
continue
|
||||
|
||||
# Ожидание ответа
|
||||
response = receive_response(bus, timeout=0.5)
|
||||
if response:
|
||||
break
|
||||
|
||||
print("Ответ не получен, повтор...")
|
||||
time.sleep(0.2)
|
||||
|
||||
# ======= 3. Обработка ответа =======
|
||||
if not response:
|
||||
print("Устройство не ответило после всех попыток")
|
||||
bus.shutdown()
|
||||
sys.exit(1)
|
||||
|
||||
data = response.data
|
||||
|
||||
if len(data) < 4:
|
||||
print("Слишком короткий ответ")
|
||||
bus.shutdown()
|
||||
sys.exit(1)
|
||||
|
||||
# Проверяем минимальную длину ответа (данные + CRC)
|
||||
id_bytes = response.arbitration_id.to_bytes(1, byteorder='little')
|
||||
full_data = list(id_bytes) + list(data[:-2])
|
||||
print(f"Полные данные для CRC: {full_data}")
|
||||
|
||||
received_crc = int.from_bytes(data[-2:], byteorder='little')
|
||||
calc_crc = validate_crc16(full_data)
|
||||
|
||||
print(f"Расчитанный CRC: 0x{calc_crc:04X}, Полученный CRC: 0x{received_crc:04X}")
|
||||
|
||||
if received_crc == calc_crc:
|
||||
print(f"Текущий ID устройства: 0x{data[1]:02X}")
|
||||
else:
|
||||
id_bytes = response.arbitration_id.to_bytes(1,byteorder='little')
|
||||
#buff with id and data without CRC
|
||||
full_data = list(id_bytes) + list(data[:-2])
|
||||
print(f"Received full_data: {list(full_data)}")
|
||||
received_crc = int.from_bytes(data[-2:], byteorder='little')
|
||||
#calc CRC
|
||||
calc_crc = validate_crc16(full_data)
|
||||
|
||||
print(f"Расчитанный CRC PYTHON : 0x{calc_crc:02X}")
|
||||
if received_crc == calc_crc:
|
||||
# Если CRC совпадает, проверяем структуру ответа:
|
||||
print(f"Текущий ID устройства: 0x{data[1]:02X}")
|
||||
else:
|
||||
print("Ошибка: CRC не совпадает")
|
||||
else:
|
||||
print("Устройство не ответило")
|
||||
|
||||
# Завершаем работу с шиной
|
||||
bus.shutdown()
|
||||
print("Ошибка: CRC не совпадает")
|
||||
|
||||
# Завершаем работу с шиной
|
||||
bus.shutdown()
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
if len(sys.argv) != 2:
|
||||
print("Использование: python3 can_flasher.py address")
|
||||
print("Использование: python3 can_flasher.py <адрес_устройства>")
|
||||
print("Пример: python3 can_flasher.py 1")
|
||||
sys.exit(1)
|
||||
|
||||
main()
|
||||
Loading…
Add table
Add a link
Reference in a new issue