aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: Add timeout to ESC driving when CAN packets are lost

Browse Source 
Fixes a periph continuing to drive an ESC to an output when the esc_rawcommand packets are lost
This commit is contained in:
James O'Shannessy 2022-11-28 15:45:59 +11:00 committed by Andrew Tridgell
parent dfb88839d3
commit 4679cb4265
3 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Tools/AP_Periph/AP_Periph.h
View File

@ -222,6 +222,9 @@ public:
SRV_Channels servo_channels; SRV_Channels servo_channels;
bool rcout_has_new_data_to_update; 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();
void rcout_init_1Hz(); void rcout_init_1Hz();
void rcout_esc(int16_t *rc, uint8_t num_channels); void rcout_esc(int16_t *rc, uint8_t num_channels);

4
Tools/AP_Periph/can.cpp
View File

@ -774,6 +774,10 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe
return; return;
} }
periph.rcout_esc(cmd.cmd.data, cmd.cmd.len); 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) static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)

14
Tools/AP_Periph/rc_out.cpp
View File

@ -126,6 +126,18 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
void AP_Periph_FW::rcout_update() 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) { if (!rcout_has_new_data_to_update) {
return; return;
} }
@ -136,7 +148,6 @@ void AP_Periph_FW::rcout_update()
SRV_Channels::output_ch_all(); SRV_Channels::output_ch_all();
SRV_Channels::push(); SRV_Channels::push();
#if HAL_WITH_ESC_TELEM #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) { if (now_ms - last_esc_telem_update_ms >= esc_telem_update_period_ms) {
last_esc_telem_update_ms = now_ms; last_esc_telem_update_ms = now_ms;
esc_telem_update(); esc_telem_update();
@ -145,4 +156,3 @@ void AP_Periph_FW::rcout_update()
} }
#endif // HAL_PERIPH_ENABLE_RC_OUT #endif // HAL_PERIPH_ENABLE_RC_OUT