diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index a15591c559..c21ace77d5 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -48,12 +48,6 @@ RCInput::RCInput(const char *device) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) { - // rc input, published to ORB - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - - // initialize it as RC lost - _rc_in.rc_lost = true; - // initialize raw_rc values and count for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { _raw_rc_values[i] = UINT16_MAX; @@ -116,6 +110,8 @@ RCInput::init() px4_arch_unconfiggpio(GPIO_PPM_IN); #endif // GPIO_PPM_IN + rc_io_invert(false); + return 0; } @@ -271,8 +267,6 @@ void RCInput::set_rc_scan_state(RC_SCAN newState) _rc_scan_begin = 0; _rc_scan_locked = false; - - _report_lock = true; } void RCInput::rc_io_invert(bool invert) @@ -417,8 +411,8 @@ void RCInput::Run() bool rc_updated = false; // This block scans for a supported serial RC input and locks onto the first one found - // Scan for 300 msec, then switch protocol - constexpr hrt_abstime rc_scan_max = 300_ms; + // Scan for 500 msec, then switch protocol + constexpr hrt_abstime rc_scan_max = 500_ms; unsigned frame_drops = 0; @@ -434,6 +428,8 @@ void RCInput::Run() _bytes_rx += newBytes; } + const bool rc_scan_locked = _rc_scan_locked; + switch (_rc_scan_state) { case RC_SCAN_NONE: // do nothing @@ -754,15 +750,18 @@ void RCInput::Run() _rc_scan_locked = false; } - if (_report_lock && _rc_scan_locked) { - _report_lock = false; + if (!rc_scan_locked && _rc_scan_locked) { PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); + } - if (!_armed && (_param_rc_input_proto.get() < 0)) { - // RC_INPUT_PROTO auto => locked selection - _param_rc_input_proto.set(_rc_scan_state); - _param_rc_input_proto.commit(); - } + // set RC_INPUT_PROTO if RC successfully locked for > 3 seconds + if (!_armed && rc_updated && _rc_scan_locked + && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s) + && (_param_rc_input_proto.get() < 0) + ) { + // RC_INPUT_PROTO auto => locked selection + _param_rc_input_proto.set(_rc_scan_state); + _param_rc_input_proto.commit(); } } } diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index d5b2e03ad2..efea2ba231 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -131,7 +131,6 @@ private: bool _initialized{false}; bool _rc_scan_locked{false}; - bool _report_lock{true}; static constexpr unsigned _current_update_interval{4000}; // 250 Hz