Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

px4iofirmware: guard against parsing multiple RC protocols

Browse Source 
It was for example possible that DSM got parsed, and in the next iteration
SBUS got parsed, leading to multiple PX4IO_P_STATUS_FLAGS_RC_ flags set.
input_rc::input_source was then incorrectly set to DSM.

Partially guarding was already done: if SBUS got parsed, DSM and others
were skipped.

The IO will still clear all PX4IO_P_STATUS_FLAGS_RC_* flags on RC loss.
This commit is contained in:
Beat Küng
2021-02-19 14:20:31 +01:00
committed by Daniel Agar
parent 5c27c47746
commit 9d11d94e7e
1 changed files with 43 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/controls.c
+43 -35
View File
@@ -133,11 +133,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
*st24_updated = false;
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
}
if (*st24_updated && lost_count == 0) {
@@ -161,11 +163,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
*sumd_updated = false;
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) {
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
}
}
if (*sumd_updated) {
@@ -263,35 +267,39 @@ controls_tick()
perf_begin(c_gather_sbus);
#endif
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
PX4IO_RC_INPUT_CHANNELS);
bool sbus_updated = false;
if (sbus_updated) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS);
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
bool sbus_failsafe, sbus_frame_drop;
sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
PX4IO_RC_INPUT_CHANNELS);
unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
if (sbus_updated) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS);
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* set RSSI to an emulated value if ADC RSSI is off */
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
_rssi = sbus_rssi;
}
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
/* set RSSI to an emulated value if ADC RSSI is off */
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
_rssi = sbus_rssi;
}
}
#if defined(PX4IO_PERF)
@@ -321,7 +329,7 @@ controls_tick()
bool dsm_updated = false, st24_updated = false, sumd_updated = false;
if (!((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) || (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM))) {
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) {
#if defined(PX4IO_PERF)
perf_begin(c_gather_dsm);
#endif