From 53a14e847de7f5fb6d15e9b02accc5b9f028355e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Jan 2021 15:49:45 -0500 Subject: [PATCH] drivers/rc_input: support RC_RSSI_PWM_CHAN --- src/drivers/px4io/px4io_params.c | 58 -------------------------------- src/drivers/rc_input/RCInput.cpp | 22 ++++++++++-- src/drivers/rc_input/RCInput.hpp | 14 +++++++- src/modules/rc_update/params.c | 58 ++++++++++++++++++++++++++++++++ 4 files changed, 91 insertions(+), 61 deletions(-) diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 7128093038..542be13b88 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -64,61 +64,3 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group PWM Outputs */ PARAM_DEFINE_INT32(PWM_SBUS_MODE, 0); - -/** - * PWM input channel that provides RSSI. - * - * 0: do not read RSSI from input channel - * 1-18: read RSSI from specified input channel - * - * Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters. - * - * @min 0 - * @max 18 - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - * @group Radio Calibration - * - */ -PARAM_DEFINE_INT32(RC_RSSI_PWM_CHAN, 0); - -/** - * Max input value for RSSI reading. - * - * Only used if RC_RSSI_PWM_CHAN > 0 - * - * @min 0 - * @max 2000 - * @group Radio Calibration - * - */ -PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); - -/** - * Min input value for RSSI reading. - * - * Only used if RC_RSSI_PWM_CHAN > 0 - * - * @min 0 - * @max 2000 - * @group Radio Calibration - * - */ -PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 9ed3ef830c..1e37018c21 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -41,6 +41,7 @@ using namespace time_literals; constexpr char const *RCInput::RC_SCAN_STRING[]; RCInput::RCInput(const char *device) : + ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) @@ -197,9 +198,17 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, /* fake rssi if no value was provided */ if (rssi == -1) { + if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _rc_in.channel_count)) { + const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get(); + const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get(); + const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get(); - /* set RSSI if analog RSSI input is present */ - if (_analog_rc_rssi_stable) { + // get RSSI from input channel + int rc_rssi = ((_rc_in.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min); + _rc_in.rssi = math::constrain(rc_rssi, 0, 100); + + } else if (_analog_rc_rssi_stable) { + // set RSSI if analog RSSI input is present float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f; if (rssi_analog > 100.0f) { @@ -281,6 +290,15 @@ void RCInput::Run() perf_begin(_cycle_perf); + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + const hrt_abstime cycle_timestamp = hrt_absolute_time(); diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index d2e52e2f31..9d17b2abd8 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -49,11 +49,14 @@ #include #include #include +#include #include #include #include +#include #include #include +#include #include #include "crsf_telemetry.h" @@ -62,7 +65,7 @@ # include #endif -class RCInput : public ModuleBase, public px4::ScheduledWorkItem +class RCInput : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -134,6 +137,8 @@ private: static constexpr unsigned _current_update_interval{4000}; // 250 Hz + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _adc_sub{ORB_ID(adc_report)}; @@ -142,6 +147,7 @@ private: float _analog_rc_rssi_volt{-1.0f}; bool _analog_rc_rssi_stable{false}; + uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; int _rcs_fd{-1}; @@ -157,4 +163,10 @@ private: perf_counter_t _cycle_perf; perf_counter_t _publish_interval_perf; uint32_t _bytes_rx{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_rc_rssi_pwm_chan, + (ParamInt) _param_rc_rssi_pwm_min, + (ParamInt) _param_rc_rssi_pwm_max + ) }; diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index e70c0b66fd..ee550a65a8 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -2235,3 +2235,61 @@ PARAM_DEFINE_FLOAT(RC_STAB_TH, 0.5f); * @group Radio Switches */ PARAM_DEFINE_FLOAT(RC_MAN_TH, 0.75f); + +/** + * PWM input channel that provides RSSI. + * + * 0: do not read RSSI from input channel + * 1-18: read RSSI from specified input channel + * + * Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters. + * + * @min 0 + * @max 18 + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_CHAN, 0); + +/** + * Min input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 1000); + +/** + * Max input value for RSSI reading. + * + * Only used if RC_RSSI_PWM_CHAN > 0 + * + * @min 0 + * @max 2000 + * @group Radio Calibration + * + */ +PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 2000);