diff options
Diffstat (limited to 'src/modules/px4iofirmware/controls.c')
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 13 |
1 files changed, 10 insertions, 3 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..9561c9b1e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -95,9 +95,16 @@ controls_tick() { */ perf_begin(c_gather_dsm); - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) + uint16_t temp_count = r_raw_rc_count; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + else + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -138,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; |