aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/drivers/px4io/px4io.cpp1
-rw-r--r--apps/drivers/px4io/uploader.cpp6
-rw-r--r--apps/px4io/dsm.c26
-rw-r--r--apps/px4io/mixer.c25
-rw-r--r--apps/px4io/px4io.c2
-rw-r--r--apps/px4io/px4io.h5
-rw-r--r--apps/px4io/sbus.c30
7 files changed, 71 insertions, 24 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 07a39c881..49ad80943 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -338,7 +338,6 @@ PX4IO::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
- usleep(1000000);
continue;
}
diff --git a/apps/drivers/px4io/uploader.cpp b/apps/drivers/px4io/uploader.cpp
index 5669aeb01..8b354ff60 100644
--- a/apps/drivers/px4io/uploader.cpp
+++ b/apps/drivers/px4io/uploader.cpp
@@ -189,8 +189,10 @@ PX4IO_Uploader::drain()
int ret;
do {
- ret = recv(c, 10);
- //log("discard 0x%02x", c);
+ ret = recv(c, 250);
+ if (ret == OK) {
+ //log("discard 0x%02x", c);
+ }
} while (ret == OK);
}
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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-11 03:46:27 +0000