Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix uncalibrated RC input being used #21543

Merged
merged 3 commits into from
May 23, 2023
Merged

Fix uncalibrated RC input being used #21543

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
23931f9
RCInput: follow topic name convention
MaEtUgR Apr 30, 2023
2f5846a
ManualControl: use input validity flag to check for RC calibration
MaEtUgR May 2, 2023
f573b6a
ManualControlSelectorTest: adapt to using input validity flag
MaEtUgR May 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/drivers/px4io/px4io.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,7 @@ class PX4IO : public cdev::CDev, public ModuleBase<PX4IO>, public OutputModuleIn
bool _timer_rates_configured{false};

/* advertised topics */
uORB::PublicationMulti<input_rc_s> _to_input_rc{ORB_ID(input_rc)};
uORB::PublicationMulti<input_rc_s> _input_rc_pub{ORB_ID(input_rc)};
uORB::Publication<px4io_status_s> _px4io_status_pub{ORB_ID(px4io_status)};

ButtonPublisher _button_publisher;
Expand Down Expand Up @@ -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;
Expand Down
72 changes: 36 additions & 36 deletions src/drivers/rc_input/RCInput.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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++;
Expand All @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
}
}
Expand Down Expand Up @@ -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;
Expand All @@ -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 {
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down
6 changes: 2 additions & 4 deletions src/drivers/rc_input/RCInput.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,7 @@ class RCInput : public ModuleBase<RCInput>, public ModuleParams, public px4::Sch

void rc_io_invert(bool invert);

input_rc_s _input_rc{};
hrt_abstime _rc_scan_begin{0};

bool _initialized{false};
Expand All @@ -140,16 +141,13 @@ class RCInput : public ModuleBase<RCInput>, public ModuleParams, public px4::Sch
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_s> _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<input_rc_s> _to_input_rc{ORB_ID(input_rc)};

int _rcs_fd{-1};
char _device[20] {}; ///< device / serial port path

Expand Down
2 changes: 1 addition & 1 deletion src/modules/manual_control/ManualControlSelector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
(input.data_source == _first_valid_source
|| _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);

return sample_from_the_past && sample_newer_than_timeout
return sample_from_the_past && sample_newer_than_timeout && input.valid
&& (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
}

Expand Down
Loading