From eed644fcfac6033d8b6e6936d8080bb629467dc1 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Mon, 20 Nov 2023 17:00:17 -0800 Subject: [PATCH] AP_Periph: Add timeout to ESC driving when CAN packets are lost ARM _Monitoring: dropped to be also cleared at ESC signal received ESC_Timeout: Fixes a periph continuing to drive an ESC to an output when the esc_rawcommand packets are lost --- Tools/AP_Periph/AP_Periph.cpp | 14 +++++++++++++- Tools/AP_Periph/AP_Periph.h | 3 +++ Tools/AP_Periph/can.cpp | 4 ++++ 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 4d228cdab3..e562c2d7dc 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -386,7 +386,19 @@ void AP_Periph_FW::update() last_error_ms = now; can_printf("IERR 0x%x %u", unsigned(ierr.errors()), unsigned(ierr.last_error_line())); } - +//this will only monitor Arming signal +#if HAL_PERIPH_ARM_MONITORING_ENABLE + static uint32_t last_arm_check_ms; + if (now - last_arm_check_ms > g.disarm_delay){ + last_arm_check_ms = now; + if(periph.arm_update_status){ + periph.arm_update_status = false; + } + else{ + hal.util->set_soft_armed(UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED); + } + } +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE static uint32_t last_debug_ms; if ((g.debug&(1< 5000) { diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 85f356659d..9388e67f5f 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -216,6 +216,9 @@ class AP_Periph_FW { SRV_Channels servo_channels; bool rcout_has_new_data_to_update; + uint32_t last_esc_raw_command_ms; + uint8_t last_esc_num_channels; + void rcout_init(); void rcout_init_1Hz(); void rcout_esc(int16_t *rc, uint8_t num_channels); diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index d7759bda54..4afcc10bff 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -762,6 +762,10 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe return; } periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); + + // Update internal copy for disabling output to ESC when CAN packets are lost + periph.last_esc_num_channels = cmd.cmd.len; + periph.last_esc_raw_command_ms = AP_HAL::millis(); } static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)