aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/drv_rc_input.h18
-rw-r--r--src/drivers/px4fmu/fmu.cpp12
-rw-r--r--src/drivers/px4io/px4io.cpp31
-rw-r--r--src/modules/mavlink/orb_listener.c2
-rw-r--r--src/modules/sensors/sensors.cpp9
-rw-r--r--src/systemcmds/tests/test_rc.c2
6 files changed, 59 insertions, 15 deletions
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 715df7e01..20763e265 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -63,6 +63,11 @@
#define RC_INPUT_MAX_CHANNELS 18
/**
+ * Maximum RSSI value
+ */
+#define RC_INPUT_RSSI_MAX 255
+
+/**
* Input signal type, value is a control position from zero to 100
* percent.
*/
@@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE {
* on the board involved.
*/
struct rc_input_values {
- /** decoding time */
- uint64_t timestamp;
+ /** publication time */
+ uint64_t timestamp_publication;
+
+ /** last valid reception time */
+ uint64_t timestamp_last_signal;
/** number of channels actually being seen */
uint32_t channel_count;
@@ -120,6 +128,12 @@ struct rc_input_values {
* */
uint16_t rc_total_frame_count;
+ /**
+ * Length of a single PPM frame.
+ * Zero for non-PPM systems
+ */
+ uint16_t rc_ppm_frame_length;
+
/** Input source */
enum RC_INPUT_SOURCE input_source;
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index c067d363b..0fbd84924 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -626,7 +626,7 @@ PX4FMU::task_main()
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
- if (ppm_last_valid_decode != rc_in.timestamp) {
+ if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
@@ -638,7 +638,15 @@ PX4FMU::task_main()
rc_in.values[i] = ppm_buffer[i];
}
- rc_in.timestamp = ppm_last_valid_decode;
+ rc_in.timestamp_publication = ppm_last_valid_decode;
+ rc_in.timestamp_last_signal = ppm_last_valid_decode;
+
+ rc_in.rc_ppm_frame_length = ppm_frame_length;
+ rc_in.rssi = RC_INPUT_RSSI_MAX;
+ rc_in.rc_failsafe = false;
+ rc_in.rc_lost = false;
+ rc_in.rc_lost_frame_count = 0;
+ rc_in.rc_total_frame_count = 0;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0fb7b7d24..5601230a4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -239,6 +239,7 @@ private:
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
+ uint64_t _rc_last_valid; ///< last valid timestamp
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
@@ -468,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) :
_update_interval(0),
_rc_handling_disabled(false),
_rc_chan_count(0),
+ _rc_last_valid(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
@@ -1398,7 +1400,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*
* This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
- input_rc.timestamp = hrt_absolute_time();
+ input_rc.timestamp_publication = hrt_absolute_time();
+
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
if (ret != OK)
@@ -1408,13 +1411,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
* Get the channel count any any extra channels. This is no more expensive than reading the
* channel count once.
*/
- channel_count = regs[0];
+ channel_count = regs[PX4IO_P_RAW_RC_COUNT];
if (channel_count != _rc_chan_count)
perf_count(_perf_chan_count);
_rc_chan_count = channel_count;
+ input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
+ input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
+ input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+ input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+
+ /* rc_lost has to be set before the call to this function */
+ if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+ _rc_last_valid = input_rc.timestamp_publication;
+
+ input_rc.timestamp_last_signal = _rc_last_valid;
+
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@@ -1431,13 +1446,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
int
PX4IO::io_publish_raw_rc()
{
- /* if no raw RC, just don't publish */
- if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
- return OK;
/* fetch values from IO */
rc_input_values rc_val;
- rc_val.timestamp = hrt_absolute_time();
+
+ /* set the RC status flag ORDER MATTERS! */
+ rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
int ret = io_get_raw_rc_input(rc_val);
@@ -1456,6 +1470,11 @@ PX4IO::io_publish_raw_rc()
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
+
+ /* we do not know the RC input, only publish if RC OK flag is set */
+ /* if no raw RC, just don't publish */
+ if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+ return OK;
}
/* lazily advertise on first publication */
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 92b1b45be..41c754405 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -355,7 +355,7 @@ l_input_rc(const struct listener *l)
for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
- rc_raw.timestamp / 1000,
+ rc_raw.timestamp_publication / 1000,
i,
(rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
(rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 23f20b0cb..df6cbb7b2 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1276,6 +1276,9 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
+ if (rc_input.rc_lost)
+ return;
+
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
@@ -1320,7 +1323,7 @@ Sensors::rc_poll()
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
- _rc_last_valid = rc_input.timestamp;
+ _rc_last_valid = rc_input.timestamp_last_signal;
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1369,9 +1372,9 @@ Sensors::rc_poll()
}
_rc.chan_count = rc_input.channel_count;
- _rc.timestamp = rc_input.timestamp;
+ _rc.timestamp = rc_input.timestamp_last_signal;
- manual_control.timestamp = rc_input.timestamp;
+ manual_control.timestamp = rc_input.timestamp_last_signal;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c
index 6a602ecfc..57c0e7f4c 100644
--- a/src/systemcmds/tests/test_rc.c
+++ b/src/systemcmds/tests/test_rc.c
@@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[])
return ERROR;
}
- if (hrt_absolute_time() - rc_input.timestamp > 100000) {
+ if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
warnx("TIMEOUT, less than 10 Hz updates");
(void)close(_rc_sub);
return ERROR;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 05:33:40 +0000