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
CarbonixUAV · Nov 21, 2023 · eed644f · eed644f
1 parent d4aeb0e
commit eed644f
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 1 deletion.
diff --git 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<<DEBUG_SHOW_STACK)) && now - last_debug_ms > 5000) {

diff --git 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
@@ -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)