diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 20bdfcb96e..3dba560206 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -209,7 +209,7 @@ private: bool _timer_rates_configured{false}; /* advertised topics */ - uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; uORB::Publication _px4io_status_pub{ORB_ID(px4io_status)}; ButtonPublisher _button_publisher; @@ -1144,7 +1144,7 @@ int PX4IO::io_publish_raw_rc() input_rc.link_quality = -1; input_rc.rssi_dbm = NAN; - _to_input_rc.publish(input_rc); + _input_rc_pub.publish(input_rc); } return ret; diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 80c7d7afd1..5b1993eaac 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -192,16 +192,16 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, unsigned frame_drops, int rssi = -1) { // fill rc_in struct for publishing - _rc_in.channel_count = raw_rc_count_local; + _input_rc.channel_count = raw_rc_count_local; - if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { - _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + if (_input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _input_rc.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } unsigned valid_chans = 0; - for (unsigned i = 0; i < _rc_in.channel_count; i++) { - _rc_in.values[i] = raw_rc_values_local[i]; + for (unsigned i = 0; i < _input_rc.channel_count; i++) { + _input_rc.values[i] = raw_rc_values_local[i]; if (raw_rc_values_local[i] != UINT16_MAX) { valid_chans++; @@ -211,20 +211,20 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, _raw_rc_values[i] = UINT16_MAX; } - _rc_in.timestamp = now; - _rc_in.timestamp_last_signal = _rc_in.timestamp; - _rc_in.rc_ppm_frame_length = 0; + _input_rc.timestamp = now; + _input_rc.timestamp_last_signal = _input_rc.timestamp; + _input_rc.rc_ppm_frame_length = 0; /* 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)) { + if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < _input_rc.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(); // 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); + int rc_rssi = ((_input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min); + _input_rc.rssi = math::constrain(rc_rssi, 0, 100); } else if (_analog_rc_rssi_stable) { // set RSSI if analog RSSI input is present @@ -238,24 +238,24 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, rssi_analog = 0.0f; } - _rc_in.rssi = rssi_analog; + _input_rc.rssi = rssi_analog; } else { - _rc_in.rssi = 255; + _input_rc.rssi = 255; } } else { - _rc_in.rssi = rssi; + _input_rc.rssi = rssi; } if (valid_chans == 0) { - _rc_in.rssi = 0; + _input_rc.rssi = 0; } - _rc_in.rc_failsafe = failsafe; - _rc_in.rc_lost = (valid_chans == 0); - _rc_in.rc_lost_frame_count = frame_drops; - _rc_in.rc_total_frame_count = 0; + _input_rc.rc_failsafe = failsafe; + _input_rc.rc_lost = (valid_chans == 0); + _input_rc.rc_lost_frame_count = frame_drops; + _input_rc.rc_total_frame_count = 0; } void RCInput::set_rc_scan_state(RC_SCAN newState) @@ -468,7 +468,7 @@ void RCInput::Run() if (rc_updated) { // we have a new SBUS frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, sbus_frame_drop, sbus_failsafe, frame_drops); _rc_scan_locked = true; @@ -506,7 +506,7 @@ void RCInput::Run() if (rc_updated) { // we have a new DSM frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, frame_drops, dsm_rssi); _rc_scan_locked = true; @@ -552,14 +552,14 @@ void RCInput::Run() if (rc_updated) { if (lost_count == 0) { // we have a new ST24 frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, frame_drops, st24_rssi); _rc_scan_locked = true; } else { // if the lost count > 0 means that there is an RC loss - _rc_in.rc_lost = true; + _input_rc.rc_lost = true; } } } @@ -600,7 +600,7 @@ void RCInput::Run() if (rc_updated) { // we have a new SUMD frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, sumd_failsafe, frame_drops, sumd_rssi); _rc_scan_locked = true; @@ -625,14 +625,14 @@ void RCInput::Run() } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { // see if we have new PPM input data - if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) { + if ((ppm_last_valid_decode != _input_rc.timestamp_last_signal) && ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. rc_updated = true; - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0); _rc_scan_locked = true; - _rc_in.rc_ppm_frame_length = ppm_frame_length; - _rc_in.timestamp_last_signal = ppm_last_valid_decode; + _input_rc.rc_ppm_frame_length = ppm_frame_length; + _input_rc.timestamp_last_signal = ppm_last_valid_decode; } } else { @@ -669,7 +669,7 @@ void RCInput::Run() if (rc_updated) { // we have a new CRSF frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0); // on Pixhawk (-related) boards we cannot write to the RC UART @@ -718,7 +718,7 @@ void RCInput::Run() if (rc_updated) { // we have a new GHST frame. Publish it. - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); // ghst telemetry works on fmu-v5 @@ -753,12 +753,12 @@ void RCInput::Run() if (rc_updated) { perf_count(_publish_interval_perf); - _rc_in.link_quality = -1; - _rc_in.rssi_dbm = NAN; + _input_rc.link_quality = -1; + _input_rc.rssi_dbm = NAN; - _to_input_rc.publish(_rc_in); + _input_rc_pub.publish(_input_rc); - } else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) { + } else if (!rc_updated && !_armed && (hrt_elapsed_time(&_input_rc.timestamp_last_signal) > 1_s)) { _rc_scan_locked = false; } @@ -907,8 +907,8 @@ int RCInput::print_status() perf_print_counter(_cycle_perf); perf_print_counter(_publish_interval_perf); - if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) { - print_message(ORB_ID(input_rc), _rc_in); + if (hrt_elapsed_time(&_input_rc.timestamp) < 1_s) { + print_message(ORB_ID(input_rc), _input_rc); } return 0; diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index efea2ba231..718fb2647c 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -127,6 +127,7 @@ private: void rc_io_invert(bool invert); + input_rc_s _input_rc{}; hrt_abstime _rc_scan_begin{0}; bool _initialized{false}; @@ -140,16 +141,13 @@ private: uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - input_rc_s _rc_in{}; + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; float _analog_rc_rssi_volt{-1.0f}; bool _analog_rc_rssi_stable{false}; bool _armed{false}; - - uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; - int _rcs_fd{-1}; char _device[20] {}; ///< device / serial port path