Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-19 07:08:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: Add restart acknowledgement for UAVCAN

Browse Source 
There was no acknowledgement for UAVCAN so we add it here
This commit is contained in:
Hayden Donald 2025-01-15 09:48:06 +11:00 committed by Peter Barker
parent 4086278394
commit a953cc2019
2 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/AP_Periph/AP_Periph.h
View File

@ -602,6 +602,8 @@ public:
AP_AHRS ahrs; AP_AHRS ahrs;
#endif #endif
uint32_t reboot_request_ms = 0;
HAL_Semaphore canard_broadcast_semaphore; HAL_Semaphore canard_broadcast_semaphore;
}; };

36
Tools/AP_Periph/can.cpp
View File

@ -797,15 +797,23 @@ void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance,
handle_begin_firmware_update(canard_instance, transfer); handle_begin_firmware_update(canard_instance, transfer);
break; break;
case UAVCAN_PROTOCOL_RESTARTNODE_ID: case UAVCAN_PROTOCOL_RESTARTNODE_ID: {
printf("RestartNode\n"); printf("RestartNode\n");
hal.scheduler->delay(10); uavcan_protocol_RestartNodeResponse pkt {
prepare_reboot(); ok: true,
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS };
NVIC_SystemReset(); uint8_t buffer[UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_MAX_SIZE];
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL uint16_t total_size = uavcan_protocol_RestartNodeResponse_encode(&pkt, buffer, !canfdout());
HAL_SITL::actually_reboot(); canard_respond(canard_instance,
#endif transfer,
UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE,
UAVCAN_PROTOCOL_RESTARTNODE_ID,
&buffer[0],
total_size);
// schedule a reboot to occur
reboot_request_ms = AP_HAL::millis();
}
break; break;
case UAVCAN_PROTOCOL_PARAM_GETSET_ID: case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
@ -1950,6 +1958,16 @@ void AP_Periph_FW::can_update()
} }
} }
// if there is a reboot scheduled, do it 1 second after request to allow the acknowledgement to be sent
if (reboot_request_ms != 0 && (AP_HAL::millis() - reboot_request_ms > 1000)) {
prepare_reboot();
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
NVIC_SystemReset();
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
HAL_SITL::actually_reboot();
#endif
}
} }
// printf to CAN LogMessage for debugging // printf to CAN LogMessage for debugging