#include "process_can.h" static CAN_message_t CAN_TX_msg; static CAN_message_t CAN_inMsg; template void send_can_with_id_crc(uint8_t id, uint8_t message_type, T* data) { // Create CAN message CAN_message_t msg_l; msg_l.id = id; // msg_l.len = 8; // Protocol-defined message length memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t)); memcpy(&msg_l.buf[1], data, sizeof(T)); // Prepare CRC calculation buffer (ID + data) uint8_t crc_data[CAN_MSG_MAX_LEN]; // Copy message ID memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(T)); // Copy all data bytes memcpy(crc_data + 1, msg_l.buf, 6); // Calculate CRC uint16_t crc_value = validate_crc16(crc_data, CAN_MSG_MAX_LEN); // Insert CRC into buffer // memcpy(&msg_l.buf[6], &crc_value, sizeof(uint16_t)); msg_l.buf[6] = crc_value & 0xFF; msg_l.buf[7] = (crc_value >> 8) & 0xFF; // Send message Can.write(msg_l); } void send_velocity() { float current_velocity = motor.shaftVelocity(); if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } float value = flash_rec[vel].value; uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'V',&value); } void send_angle() { float current_angle = motor.shaftAngle(); if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'A',¤t_angle); } void send_motor_enabled() { /* Firmware data reading */ if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t value = motor_control_inputs.motor_enabled; //copy current motor state uint8_t id = flash_rec[addr_id].value; send_can_with_id_crc(id,'M',&value); } void send_id() { /* Firmware data reading */ if (flash_rec == nullptr) { // Null check // Error handling: logging, alerts, etc. return; } uint8_t id = flash_rec[addr_id].value; 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_pid_angle(uint8_t param_pid){ if (flash_rec == nullptr) { // Null check return; } uint8_t id = flash_rec[addr_id].value; conv_float_to_int.i = flash_rec[param_pid].value; uint32_t data = conv_float_to_int.i; switch(param_pid){ case pid_p: param_pid = REG_MOTOR_POSPID_Kp; break; case pid_i: param_pid = REG_MOTOR_POSPID_Ki; break; case pid_d: param_pid = REG_MOTOR_POSPID_Kd; break; } send_can_with_id_crc(id,param_pid,&data); } void setup_id(uint8_t my_id) { write_param(addr_id,my_id); } void firmware_update(){ write_param(firmw,FIRMWARE_FLAG); NVIC_SystemReset(); } void setup_angle(float target_angle) { motor.enable(); // Enable motor if disabled motor.controller = MotionControlType::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); // } 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 /* 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 // Copy message ID (2 bytes) memcpy(data, (uint8_t*)&msg_id, sizeof(msg_id)); // Copy message data (excluding CRC bytes) memcpy(data + sizeof(msg_id), msg.buf, msg.len - 2); // Calculate CRC uint16_t calculated_crc = validate_crc16(data, sizeof(msg_id) + msg.len - 2); // Verify CRC match if (calculated_crc != received_crc) { return; // Ignore message on CRC mismatch } flash_rec = load_params(); /* Message Structure: 0x691 69 - Device address 1 - Action code */ if(id_x == flash_rec[addr_id].value){ if(msg_ch == REG_WRITE){ 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, &msg.buf[1], sizeof(motor_control_inputs.target_angle)); setup_angle(motor_control_inputs.target_angle); break; case REG_MOTOR_POSPID_Kp: memcpy(&motor.P_angle.P, &msg.buf[1], sizeof(float)); conv_float_to_int.f = motor.P_angle.P; write_param(pid_p,conv_float_to_int.i); break; case REG_MOTOR_POSPID_Ki: memcpy(&motor.P_angle.I, &msg.buf[1], sizeof(float)); conv_float_to_int.f = motor.P_angle.I; write_param(pid_i,conv_float_to_int.i); break; case REG_MOTOR_POSPID_Kd: memcpy(&motor.P_angle.D, &msg.buf[1], sizeof(float)); conv_float_to_int.f = motor.P_angle.D; write_param(pid_d,conv_float_to_int.i); break; case FIRMWARE_UPDATE: firmware_update(); 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; } } 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_angle(pid_p); break; case REG_MOTOR_POSPID_Ki: send_pid_angle(pid_i); break; case REG_MOTOR_POSPID_Kd: send_pid_angle(pid_d); break; default: break; } } } }