fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

px4io: only publish valid input_rc

Browse Source
This commit is contained in:
Daniel Agar 2022-03-23 15:54:03 -04:00
parent 3d08e031d2
commit bc26b73c07
1 changed files with 6 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/drivers/px4io/px4io.cpp
View File

@ -177,8 +177,6 @@ private:
unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO
unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO
uint64_t _rc_last_valid{0}; ///< last valid timestamp
int _class_instance{-1};
hrt_abstime _poll_last{0};
@ -1200,6 +1198,7 @@ int PX4IO::io_get_status()
int PX4IO::io_publish_raw_rc()
{
input_rc_s input_rc{};
input_rc.timestamp_last_signal = hrt_absolute_time();
/* set the RC status flag ORDER MATTERS! */
input_rc.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
@ -1259,12 +1258,6 @@ int PX4IO::io_publish_raw_rc()
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
input_rc.channel_count = channel_count;
/* rc_lost has to be set before the call to this function */
if ((channel_count > 0) && !input_rc.rc_lost && !input_rc.rc_failsafe) {
_rc_last_valid = input_rc.timestamp;
}
input_rc.timestamp_last_signal = _rc_last_valid;
/* FIELDS NOT SET HERE */
/* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */
@ -1310,18 +1303,13 @@ int PX4IO::io_publish_raw_rc()
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
} else {
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
/* only keep publishing RC input if we ever got a valid input */
if (_rc_last_valid == 0) {
/* we have never seen valid RC signals, abort */
return OK;
}
}
_to_input_rc.publish(input_rc);
if ((input_rc.channel_count > 0) && !input_rc.rc_lost && !input_rc.rc_failsafe
&& (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN)) {
_to_input_rc.publish(input_rc);
}
return ret;
}