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

Add clear mailbox

Browse source
This commit is contained in:
Valentin Dabstep 2025-06-16 22:56:01 +03:00
parent 4d6a5f8700
commit d0147d7c8c
3 changed files with 33 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

2
controller/fw/embed/include/process_can.h
View file

@ -33,4 +33,4 @@ void setup_angle(float target_angle);
void setup_velocity(float target_velocity);
void process_can_messages();
void listen_can(const CAN_message_t &msg);
int listen_can(const CAN_message_t &msg);

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

@ -36,7 +36,6 @@ uint32_t timeout;
volatile bool CAN_GET = false;
volatile float kt = 0.1; // Torque calculation constant
FLASH_RECORD* flash_rec;
@ -62,7 +61,7 @@ MotorControlInputs motor_control_inputs;
volatile uint8_t crc_l;
volatile bool rx_can = false;
CAN_message_t msg;
CAN_message_t msg_g;
void setup(){
SCB->VTOR = (volatile uint32_t)0x08008004;
@ -104,16 +103,33 @@ void wait_for_can_tx_complete() {
CAN2->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2;
}
bool can_mailbox_free(uint8_t mailbox) {
return (CAN2->TSR & (CAN_TSR_TME0 << mailbox)) != 0;
}
void clear_all_mailboxes() {
CAN2->TSR |= CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2;
while ((CAN2->TSR & (CAN_TSR_ABRQ0 | CAN_TSR_ABRQ1 | CAN_TSR_ABRQ2)) != 0);
}
int can_err = 0;
void loop() {
send_velocity();
HAL_Delay(1);
send_angle();
HAL_Delay(1);
// Process incoming CAN messages
if(rx_can){
Can.read(msg);
listen_can(msg);
CAN_WAIT();
clear_all_mailboxes();
can_err = listen_can(msg_g);
if(can_err == -1)
HAL_Delay(1000);
rx_can = false;
HAL_Delay(1);
}
CAN_STR();
}
if(can_mailbox_free(0))
send_velocity();
while(!is_can_busy);
if(can_mailbox_free(1))
send_angle();
while(!is_can_busy);
__enable_irq();
foc_step(&motor);
}

9
controller/fw/embed/src/process_can.cpp
View file

@ -53,6 +53,7 @@ void send_angle() {
}
uint8_t id = flash_rec[addr_id].value;
send_can_with_id_crc(id,'A',&current_angle);
delayMicroseconds(1000);
}
@ -189,7 +190,7 @@ void process_can_messages() {
* Write with data or send data
* @param msg
*/
void listen_can(const CAN_message_t &msg) {
int 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
@ -207,7 +208,7 @@ void listen_can(const CAN_message_t &msg) {
// Verify CRC match
if (calculated_crc != received_crc) {
return; // Ignore message on CRC mismatch
return -1; // Ignore message on CRC mismatch
}
flash_rec = load_params();
@ -215,7 +216,7 @@ void listen_can(const CAN_message_t &msg) {
69 - Device address
1 - Action code */
if(id_x != flash_rec[addr_id].value){
return;
return -1;
}
if(msg_ch == REG_WRITE){
switch(msg.buf[0]) {
@ -328,6 +329,6 @@ void listen_can(const CAN_message_t &msg) {
}
}
HAL_Delay(10);
return 1;
}