From 2f4c79fd1efec25bced9dbc9c0a335d418fb4a9d Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Sat, 14 Aug 2021 17:51:05 +0200 Subject: [PATCH] AP_FETtecOneWire: only check for telemetry if it has been requested at some point Only check for telemetry after sending a fast-throttle request for that particular ESC Added more debug information from message handling state machine --- .../AP_FETtecOneWire/AP_FETtecOneWire.cpp | 68 +++++++++---------- libraries/AP_FETtecOneWire/AP_FETtecOneWire.h | 4 +- 2 files changed, 37 insertions(+), 35 deletions(-) diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp index fb762c2001..0bc6b632a8 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp @@ -395,7 +395,7 @@ void AP_FETtecOneWire::handle_message(ESC &esc, const uint8_t length) // we only expect telemetry messages in this state #if HAL_WITH_ESC_TELEM if (!esc.telem_expected) { - // esc.unexpected_telem++; + esc.unexpected_telem++; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("unexpected telemetry"); #endif @@ -494,6 +494,7 @@ void AP_FETtecOneWire::read_data_from_uart() #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("bad message"); #endif + crc_rec_err_cnt++; move_frame_source_in_receive_buffer(1); continue; } @@ -501,7 +502,7 @@ void AP_FETtecOneWire::read_data_from_uart() // borrow the "OK" message to retrieve the frame_source from the buffer: const FrameSource frame_source = (FrameSource)u.packed_ok.frame_source; if (frame_source == FrameSource::MASTER) { - // this is our own message - we'd best we running in + // this is our own message - we'd best be running in // half-duplex or we're in trouble! consume_bytes(frame_length); continue; @@ -588,9 +589,7 @@ void AP_FETtecOneWire::escs_set_values(const uint16_t* motor_values) if (_esc_ofs_to_request_telem_from >= _esc_count) { _esc_ofs_to_request_telem_from = 0; } - esc_to_req_telem_from.telem_expected = true; esc_id_to_request_telem_from = esc_to_req_telem_from.id; - _fast_throttle_cmd_count++; #endif uint8_t fast_throttle_command[_fast_throttle_byte_count]; @@ -607,7 +606,13 @@ void AP_FETtecOneWire::escs_set_values(const uint16_t* motor_values) _uart->discard_input(); // send throttle commands to all configured ESCs in a single packet transfer - transmit(fast_throttle_command, sizeof(fast_throttle_command)); + if (transmit(fast_throttle_command, sizeof(fast_throttle_command))) { +#if HAL_WITH_ESC_TELEM + esc_to_req_telem_from.telem_expected = true; // used to make sure that the returned telemetry comes from this ESC and not another + esc_to_req_telem_from.telem_requested = true; // used to check if this ESC is periodically sending telemetry + _fast_throttle_cmd_count++; +#endif + } } bool AP_FETtecOneWire::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const @@ -752,7 +757,6 @@ void AP_FETtecOneWire::configure_escs() /// periodically called from SRV_Channels::push() void AP_FETtecOneWire::update() { - const uint32_t now = AP_HAL::micros(); if (!_init_done) { init(); return; // the rest of this function can only run after fully initted @@ -761,6 +765,30 @@ void AP_FETtecOneWire::update() // read all data from incoming serial: read_data_from_uart(); + const uint32_t now = AP_HAL::micros(); + +#if HAL_WITH_ESC_TELEM + if (!hal.util->get_soft_armed()) { + + _reverse_mask = _reverse_mask_parameter; // update this only when disarmed + + // if we haven't seen an ESC in a while, the user might + // have power-cycled them. Try re-initialising. + for (uint8_t i=0; i<_esc_count; i++) { + auto &esc = _escs[i]; + if (!esc.telem_requested || now - esc.last_telem_us < 1000000U ) { + // telem OK + continue; + } + _running_mask &= ~(1 << esc.servo_ofs); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "No telem from esc id=%u. Resetting it.", esc.id); + //GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "unknown %u, invalid %u, too short %u, unexpected: %u, crc_err %u", _unknown_esc_message, _message_invalid_in_state_count, _period_too_short, esc.unexpected_telem, crc_rec_err_cnt); + esc.set_state(ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE); + esc.telem_requested = false; + } + } +#endif // HAL_WITH_ESC_TELEM + if (now - _last_transmit_us < 700U) { // in case the SRV_Channels::push() is running at very high rates, limit the period // this function gets executed because FETtec needs a time gap between frames @@ -805,34 +833,6 @@ void AP_FETtecOneWire::update() // send motor setpoints to ESCs, and request for telemetry data escs_set_values(motor_pwm); - -#if HAL_WITH_ESC_TELEM - if (!hal.util->get_soft_armed()) { - - _reverse_mask = _reverse_mask_parameter; // update this only when disarmed - - // if we haven't seen an ESC in a while, the user might - // have power-cycled them. Try re-initialising. - for (uint8_t i=0; i<_esc_count; i++) { - auto &esc = _escs[i]; - if (!esc.telem_expected || now - esc.last_telem_us < 1000000) { - // telem OK - continue; - } - _running_mask &= ~(1 << esc.servo_ofs); - if (now - esc.last_reset_us < 5000000) { - // only attempt reset periodically - continue; - } - if (esc.state == ESCState::UNINITIALISED) { - continue; - } - esc.last_reset_us = now; - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "No telem from esc.id=%u; resetting it", esc.id); - esc.set_state(ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE); - } - } -#endif // HAL_WITH_ESC_TELEM } #if HAL_AP_FETTEC_ESC_BEEP diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h index 94c7381b92..823a298314 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h @@ -167,9 +167,10 @@ private: #if HAL_WITH_ESC_TELEM uint32_t last_telem_us; ///< last time we got telemetry from this ESC - uint32_t last_reset_us; + uint16_t unexpected_telem; uint16_t error_count_at_throttle_count_overflow; ///< overflow counter for error counter from the ESCs. bool telem_expected; ///< this ESC is fully configured and is now expected to send us telemetry + bool telem_requested; ///< this ESC is fully configured and at some point was requested to send us telemetry #endif uint8_t id; ///< FETtec ESC ID @@ -486,6 +487,7 @@ private: uint16_t _unknown_esc_message; uint16_t _message_invalid_in_state_count; uint16_t _period_too_short; + uint16_t crc_rec_err_cnt; uint8_t _receive_buf_used; /// shifts data to start of buffer based on magic header bytes