diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index e5c56c8847..184506c8b4 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -222,6 +222,9 @@ public: 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 29c86d6a87..6e8a95685f 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -774,6 +774,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) diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 68af8c8813..346a67849c 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -126,6 +126,18 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state) void AP_Periph_FW::rcout_update() { + uint32_t now_ms = AP_HAL::millis(); + const uint16_t esc_timeout_ms = 100U; + const bool has_esc_rawcommand_timed_out = (now_ms - last_esc_raw_command_ms) >= esc_timeout_ms; + if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) { + // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs + int16_t esc_output[last_esc_num_channels] {}; + rcout_esc(esc_output, last_esc_num_channels); + + // register that the output has been changed + rcout_has_new_data_to_update = true; + } + if (!rcout_has_new_data_to_update) { return; } @@ -136,7 +148,6 @@ void AP_Periph_FW::rcout_update() SRV_Channels::output_ch_all(); SRV_Channels::push(); #if HAL_WITH_ESC_TELEM - uint32_t now_ms = AP_HAL::millis(); if (now_ms - last_esc_telem_update_ms >= esc_telem_update_period_ms) { last_esc_telem_update_ms = now_ms; esc_telem_update(); @@ -145,4 +156,3 @@ void AP_Periph_FW::rcout_update() } #endif // HAL_PERIPH_ENABLE_RC_OUT -