diff options
Diffstat (limited to 'apps/px4io/dsm.c')
| -rw-r--r-- | apps/px4io/dsm.c | 26 |
1 files changed, 22 insertions, 4 deletions
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index 2c2e09905..744dac3d6 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -278,6 +278,7 @@ dsm_decode(hrt_abstime frame_time) last_frame_time = frame_time; if (channel_shift == 0) { dsm_guess_format(false); + system_state.dsm_input_ok = false; return; } @@ -292,6 +293,10 @@ dsm_decode(hrt_abstime frame_time) * seven channels are being transmitted. */ + const unsigned dsm_chancount = (DSM_FRAME_CHANNELS < PX4IO_INPUT_CHANNELS) ? DSM_FRAME_CHANNELS : PX4IO_INPUT_CHANNELS; + + uint16_t dsm_channels[dsm_chancount]; + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; @@ -314,10 +319,23 @@ dsm_decode(hrt_abstime frame_time) value /= 2; /* stuff the decoded channel into the PPM input buffer */ - ppm_buffer[channel] = 988 + value; + dsm_channels[channel] = 988 + value; } - /* and note that we have received data from the R/C controller */ - /* XXX failsafe will cause problems here - need a strategy for detecting it */ - ppm_last_valid_decode = frame_time; + /* DSM input is valid */ + system_state.dsm_input_ok = true; + + /* check if no S.BUS data is available */ + if (!system_state.sbus_input_ok) { + + for (unsigned i = 0; i < dsm_chancount; i++) { + system_state.rc_channel_data[i] = dsm_channels[i]; + } + + /* and note that we have received data from the R/C controller */ + /* XXX failsafe will cause problems here - need a strategy for detecting it */ + system_state.rc_channels_timestamp = frame_time; + system_state.rc_channels = dsm_chancount; + system_state.fmu_report_due = true; + } } |