diff options
author | px4dev <px4@purgatory.org> | 2012-12-01 00:50:39 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-12-01 00:50:39 -0800 |
commit | ea8872f5456dbba566fb4167ec66b4afa183b3e1 (patch) | |
tree | 40def937aa4253700cb78c07b0e70b0963537da1 /apps/px4io | |
parent | efd3b9dea689ef4244a3a2fd9f4217a544b254ee (diff) | |
parent | ef4a54666d760a18b18800163a24faf5883c1e61 (diff) | |
download | px4-firmware-ea8872f5456dbba566fb4167ec66b4afa183b3e1.tar.gz px4-firmware-ea8872f5456dbba566fb4167ec66b4afa183b3e1.tar.bz2 px4-firmware-ea8872f5456dbba566fb4167ec66b4afa183b3e1.zip |
Merge branch 'sbus' of https://github.com/PX4/Firmware into #61-px4io-spektrum-decoder
Diffstat (limited to 'apps/px4io')
-rw-r--r-- | apps/px4io/dsm.c | 26 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 25 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 2 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 5 | ||||
-rw-r--r-- | apps/px4io/sbus.c | 30 |
5 files changed, 67 insertions, 21 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; + } } diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index fb553bc6e..483e9fe4d 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -170,17 +170,26 @@ mixer_update(int mixer, uint16_t *inputs, int input_count) static void mixer_get_rc_input(void) { - /* if we haven't seen any new data in 200ms, assume we have lost input and tell FMU */ if ((hrt_absolute_time() - ppm_last_valid_decode) > 200000) { - system_state.rc_channels = 0; - system_state.fmu_report_due = true; + + /* input was ok and timed out, mark as update */ + if (system_state.ppm_input_ok) { + system_state.ppm_input_ok = false; + system_state.fmu_report_due = true; + } return; } - /* otherwise, copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; - system_state.fmu_report_due = true; + /* mark PPM as valid */ + system_state.ppm_input_ok = true; + + /* check if no DSM and S.BUS data is available */ + if (!system_state.sbus_input_ok && !system_state.dsm_input_ok) { + /* otherwise, copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + system_state.fmu_report_due = true; + } } diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index bba236322..77524797f 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -58,6 +58,8 @@ struct sys_state_s system_state; int user_start(int argc, char *argv[]) { + /* reset all to zero */ + memset(&system_state, 0, sizeof(system_state)); /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 21c1482be..483b9bcc8 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -72,11 +72,16 @@ struct sys_state_s bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ + bool ppm_input_ok; /* valid PPM input data */ + bool dsm_input_ok; /* valid Spektrum DSM data */ + bool sbus_input_ok; /* valid Futaba S.Bus data */ + /* * Data from the remote control input(s) */ int rc_channels; uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; + uint64_t rc_channels_timestamp; /* * Control signals from FMU. diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index 39d2c4939..c3949f2b0 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -50,6 +50,7 @@ #define DEBUG #include "px4io.h" #include "protocol.h" +#include "debug.h" #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 @@ -192,21 +193,22 @@ static void sbus_decode(hrt_abstime frame_time) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0xf0) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { sbus_frame_drops++; + system_state.sbus_input_ok = false; return; } /* if the failsafe bit is set, we consider that a loss of RX signal */ - if (frame[23] & (1 << 4)) + if (frame[23] & (1 << 4)) { + system_state.sbus_input_ok = false; return; + } - /* use the decoder matrix to extract channel data */ - for (unsigned channel = 0; channel < SBUS_INPUT_CHANNELS; channel++) { - - if (channel >= PX4IO_INPUT_CHANNELS) - break; + unsigned chancount = (PX4IO_INPUT_CHANNELS > 16) ? 16 : PX4IO_INPUT_CHANNELS; + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < chancount; channel++) { unsigned value = 0; for (unsigned pick = 0; pick < 3; pick++) { @@ -222,8 +224,18 @@ sbus_decode(hrt_abstime frame_time) } } /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - ppm_buffer[channel] = (value / 2) + 998; + system_state.rc_channel_data[channel] = (value / 2) + 998; } + + if (PX4IO_INPUT_CHANNELS >= 18) { + /* decode two switch channels */ + chancount = 18; + } + + system_state.rc_channels = chancount; + system_state.sbus_input_ok = true; + system_state.fmu_report_due = true; + /* and note that we have received data from the R/C controller */ - ppm_last_valid_decode = frame_time; + system_state.rc_channels_timestamp = frame_time; } |