mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 01:44:06 +08:00
px4io: add RC update monotonic count and cleanup unused
This commit is contained in:
Daniel Agar
parent
889a5aa0b1
commit
b24ccc27c9
@ -525,16 +525,9 @@ Syslink::handle_raw(syslink_message_t *sys)
|
||||
|
||||
crtp_commander *cmd = (crtp_commander *) &c->data[0];
|
||||
|
||||
input_rc_s rc = {};
|
||||
|
||||
rc.timestamp = hrt_absolute_time();
|
||||
rc.timestamp_last_signal = rc.timestamp;
|
||||
input_rc_s rc{};
|
||||
rc.timestamp_sample = hrt_absolute_time();
|
||||
rc.channel_count = 5;
|
||||
rc.rc_failsafe = false;
|
||||
rc.rc_lost = false;
|
||||
rc.rc_lost_frame_count = 0;
|
||||
rc.rc_total_frame_count = 1;
|
||||
rc.rc_ppm_frame_length = 0;
|
||||
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
|
||||
rc.rssi = _rssi;
|
||||
|
||||
@ -548,6 +541,7 @@ Syslink::handle_raw(syslink_message_t *sys)
|
||||
rc.values[3] = cmd->thrust * 1000 / USHRT_MAX + 1000;
|
||||
rc.values[4] = 1000; // Dummy channel as px4 needs at least 5
|
||||
|
||||
rc.timestamp = hrt_absolute_time();
|
||||
_rc_pub.publish(rc);
|
||||
|
||||
} else if (c->port == CRTP_PORT_MAVLINK) {
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -119,8 +119,6 @@ void NavioSysRCInput::Run()
|
||||
connected_buf[sizeof(connected_buf) - 1] = '\0';
|
||||
_connected = (atoi(connected_buf) == 1);
|
||||
|
||||
data.rc_lost = !_connected;
|
||||
|
||||
uint64_t timestamp_sample = hrt_absolute_time();
|
||||
|
||||
for (int i = 0; i < CHANNELS; ++i) {
|
||||
@ -145,11 +143,11 @@ void NavioSysRCInput::Run()
|
||||
}
|
||||
}
|
||||
|
||||
if (all_zero) {
|
||||
if (all_zero || !_connected) {
|
||||
return;
|
||||
}
|
||||
|
||||
data.timestamp_last_signal = timestamp_sample;
|
||||
data.timestamp_sample = timestamp_sample;
|
||||
data.channel_count = CHANNELS;
|
||||
data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
|
||||
data.timestamp = hrt_absolute_time();
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -1,4 +1,5 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint64 timestamp_sample
|
||||
|
||||
uint8 RC_INPUT_SOURCE_UNKNOWN = 0
|
||||
uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1
|
||||
@ -16,22 +17,16 @@ uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12
|
||||
uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13
|
||||
uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14
|
||||
uint8 RC_INPUT_SOURCE_PX4FMU_GHST = 15
|
||||
uint8 input_source # Input source
|
||||
|
||||
uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
|
||||
|
||||
uint64 timestamp_last_signal # last valid reception time
|
||||
|
||||
uint8 channel_count # number of channels actually being seen
|
||||
|
||||
int8 RSSI_MAX = 100
|
||||
int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception
|
||||
|
||||
bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly.
|
||||
bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usually means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
|
||||
|
||||
uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality.
|
||||
uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality.
|
||||
uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems
|
||||
|
||||
uint8 input_source # Input source
|
||||
uint16[18] values # measured pulse widths for each of the supported channels
|
||||
|
||||
@ -13,17 +13,10 @@ bool status_fmu_ok
|
||||
bool status_init_ok
|
||||
bool status_outputs_armed
|
||||
bool status_raw_pwm
|
||||
bool status_rc_ok
|
||||
bool status_rc_dsm
|
||||
bool status_rc_ppm
|
||||
bool status_rc_sbus
|
||||
bool status_rc_st24
|
||||
bool status_rc_sumd
|
||||
bool status_safety_off
|
||||
|
||||
# PX4IO alarms (PX4IO_P_STATUS_ALARMS)
|
||||
bool alarm_pwm_error
|
||||
bool alarm_rc_lost
|
||||
|
||||
# PX4IO arming (PX4IO_P_SETUP_ARMING)
|
||||
bool arming_failsafe_custom
|
||||
@ -39,5 +32,3 @@ uint16[8] pwm_disarmed
|
||||
uint16[8] pwm_failsafe
|
||||
|
||||
uint16[8] pwm_rate_hz
|
||||
|
||||
uint16[18] raw_inputs
|
||||
|
||||
@ -202,6 +202,8 @@ private:
|
||||
|
||||
hrt_abstime _last_status_publish{0};
|
||||
|
||||
uint16_t _rc_valid_update_count{0};
|
||||
|
||||
bool _param_update_force{true}; ///< force a parameter update
|
||||
bool _timer_rates_configured{false};
|
||||
|
||||
@ -546,14 +548,14 @@ void PX4IO::Run()
|
||||
|
||||
SmartLock lock_guard(_lock);
|
||||
|
||||
if (hrt_elapsed_time(&_poll_last) >= 20_ms) {
|
||||
/* run at 50 */
|
||||
if (hrt_elapsed_time(&_poll_last) >= 10_ms) {
|
||||
// run at 100 Hz
|
||||
_poll_last = hrt_absolute_time();
|
||||
|
||||
/* pull status and alarms from IO */
|
||||
// pull status and alarms from IO
|
||||
io_get_status();
|
||||
|
||||
/* get raw R/C input from IO */
|
||||
// get raw R/C input from IO
|
||||
io_publish_raw_rc();
|
||||
}
|
||||
|
||||
@ -1119,12 +1121,6 @@ int PX4IO::io_get_status()
|
||||
|
||||
// PX4IO_P_STATUS_FLAGS
|
||||
status.status_outputs_armed = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
|
||||
status.status_rc_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_OK;
|
||||
status.status_rc_ppm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||
status.status_rc_dsm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_DSM;
|
||||
status.status_rc_sbus = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SBUS;
|
||||
status.status_rc_st24 = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24;
|
||||
status.status_rc_sumd = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD;
|
||||
status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
|
||||
status.status_fmu_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_OK;
|
||||
status.status_raw_pwm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RAW_PWM;
|
||||
@ -1134,7 +1130,6 @@ int PX4IO::io_get_status()
|
||||
status.status_safety_off = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
|
||||
|
||||
// PX4IO_P_STATUS_ALARMS
|
||||
status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
|
||||
status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR;
|
||||
|
||||
// PX4IO_P_SETUP_ARMING
|
||||
@ -1178,12 +1173,6 @@ int PX4IO::io_get_status()
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
|
||||
|
||||
for (unsigned i = 0; i < raw_inputs; i++) {
|
||||
status.raw_inputs[i] = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i);
|
||||
}
|
||||
|
||||
status.timestamp = hrt_absolute_time();
|
||||
_px4io_status_pub.publish(status);
|
||||
|
||||
@ -1198,11 +1187,18 @@ int PX4IO::io_get_status()
|
||||
|
||||
int PX4IO::io_publish_raw_rc()
|
||||
{
|
||||
input_rc_s input_rc{};
|
||||
input_rc.timestamp_last_signal = hrt_absolute_time();
|
||||
const hrt_abstime time_now_us = hrt_absolute_time();
|
||||
|
||||
/* set the RC status flag ORDER MATTERS! */
|
||||
input_rc.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
const uint16_t rc_valid_update_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_FRAME_COUNT);
|
||||
const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count);
|
||||
_rc_valid_update_count = rc_valid_update_count;
|
||||
|
||||
if (!rc_updated) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
input_rc_s input_rc{};
|
||||
input_rc.timestamp_sample = time_now_us;
|
||||
|
||||
/* we don't have the status bits, so input_source has to be set elsewhere */
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
|
||||
@ -1232,37 +1228,19 @@ int PX4IO::io_publish_raw_rc()
|
||||
channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
|
||||
}
|
||||
|
||||
input_rc.timestamp = hrt_absolute_time();
|
||||
|
||||
input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
|
||||
|
||||
if (!_analog_rc_rssi_stable) {
|
||||
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
|
||||
|
||||
} else {
|
||||
float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.0f) * 100.0f;
|
||||
float rssi_analog = ((_analog_rc_rssi_volt - 0.2f) / 3.f) * 100.f;
|
||||
|
||||
if (rssi_analog > 100.0f) {
|
||||
rssi_analog = 100.0f;
|
||||
}
|
||||
|
||||
if (rssi_analog < 0.0f) {
|
||||
rssi_analog = 0.0f;
|
||||
}
|
||||
|
||||
input_rc.rssi = rssi_analog;
|
||||
input_rc.rssi = math::constrain(rssi_analog, 0.f, 100.f);;
|
||||
}
|
||||
|
||||
input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
|
||||
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];
|
||||
input_rc.channel_count = channel_count;
|
||||
|
||||
|
||||
/* 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 */
|
||||
|
||||
if (channel_count > 9) {
|
||||
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
|
||||
|
||||
@ -1276,11 +1254,6 @@ int PX4IO::io_publish_raw_rc()
|
||||
input_rc.values[i] = regs[prolog + i];
|
||||
}
|
||||
|
||||
/* zero the remaining fields */
|
||||
for (unsigned i = channel_count; i < (sizeof(input_rc.values) / sizeof(input_rc.values[0])); i++) {
|
||||
input_rc.values[i] = 0;
|
||||
}
|
||||
|
||||
/* get RSSI from input channel */
|
||||
if (_param_rc_rssi_pwm_chan.get() > 0 && _param_rc_rssi_pwm_chan.get() <= input_rc_s::RC_INPUT_MAX_CHANNELS) {
|
||||
const auto &min = _param_rc_rssi_pwm_min.get();
|
||||
@ -1292,23 +1265,30 @@ int PX4IO::io_publish_raw_rc()
|
||||
}
|
||||
}
|
||||
|
||||
// regs[PX4IO_P_RAW_RC_NRSSI];
|
||||
|
||||
// (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
|
||||
/* sort out the source of the values */
|
||||
if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
|
||||
if (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_PPM) {
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
|
||||
|
||||
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
|
||||
} else if (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_DSM) {
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
|
||||
|
||||
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
|
||||
} else if (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_SBUS) {
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
|
||||
|
||||
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
|
||||
} else if (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_ST24) {
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
|
||||
|
||||
} else if (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_SUMD) {
|
||||
input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SUMD;
|
||||
}
|
||||
|
||||
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)) {
|
||||
if (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN) {
|
||||
|
||||
input_rc.timestamp = hrt_absolute_time();
|
||||
_to_input_rc.publish(input_rc);
|
||||
}
|
||||
|
||||
|
||||
@ -48,12 +48,6 @@ RCInput::RCInput(const char *device) :
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
|
||||
_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
|
||||
{
|
||||
// rc input, published to ORB
|
||||
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
|
||||
|
||||
// initialize it as RC lost
|
||||
_rc_in.rc_lost = true;
|
||||
|
||||
// initialize raw_rc values and count
|
||||
for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
|
||||
_raw_rc_values[i] = UINT16_MAX;
|
||||
@ -203,8 +197,7 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
|
||||
}
|
||||
|
||||
_rc_in.timestamp = now;
|
||||
_rc_in.timestamp_last_signal = _rc_in.timestamp;
|
||||
_rc_in.rc_ppm_frame_length = 0;
|
||||
_rc_in.timestamp_sample = _rc_in.timestamp;
|
||||
|
||||
/* fake rssi if no value was provided */
|
||||
if (rssi == -1) {
|
||||
@ -244,9 +237,7 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
|
||||
}
|
||||
|
||||
_rc_in.rc_failsafe = failsafe;
|
||||
_rc_in.rc_lost = (valid_chans == 0);
|
||||
_rc_in.rc_lost_frame_count = frame_drops;
|
||||
_rc_in.rc_total_frame_count = 0;
|
||||
}
|
||||
|
||||
void RCInput::set_rc_scan_state(RC_SCAN newState)
|
||||
@ -531,10 +522,6 @@ void RCInput::Run()
|
||||
fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp,
|
||||
false, false, frame_drops, st24_rssi);
|
||||
_rc_scan_locked = true;
|
||||
|
||||
} else {
|
||||
// if the lost count > 0 means that there is an RC loss
|
||||
_rc_in.rc_lost = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -600,14 +587,13 @@ void RCInput::Run()
|
||||
} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
|
||||
|
||||
// see if we have new PPM input data
|
||||
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && ppm_decoded_channels > 3) {
|
||||
if ((ppm_last_valid_decode != _rc_in.timestamp_sample) && ppm_decoded_channels > 3) {
|
||||
// we have a new PPM frame. Publish it.
|
||||
rc_updated = true;
|
||||
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
|
||||
fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0);
|
||||
_rc_scan_locked = true;
|
||||
_rc_in.rc_ppm_frame_length = ppm_frame_length;
|
||||
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
|
||||
_rc_in.timestamp_sample = ppm_last_valid_decode;
|
||||
}
|
||||
|
||||
} else {
|
||||
@ -730,7 +716,7 @@ void RCInput::Run()
|
||||
|
||||
_to_input_rc.publish(_rc_in);
|
||||
|
||||
} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
|
||||
} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_sample) > 1_s)) {
|
||||
_rc_scan_locked = false;
|
||||
}
|
||||
|
||||
|
||||
@ -115,14 +115,9 @@ void RcInput::_measure(void)
|
||||
|
||||
ts = hrt_absolute_time();
|
||||
_data.timestamp = ts;
|
||||
_data.timestamp_last_signal = ts;
|
||||
_data.timestamp_sample = ts;
|
||||
_data.channel_count = _channels;
|
||||
_data.rssi = 100;
|
||||
_data.rc_lost_frame_count = 0;
|
||||
_data.rc_total_frame_count = 1;
|
||||
_data.rc_ppm_frame_length = 100;
|
||||
_data.rc_failsafe = false;
|
||||
_data.rc_lost = false;
|
||||
_data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
|
||||
|
||||
_rcinput_pub.publish(_data);
|
||||
|
||||
@ -184,8 +184,7 @@ void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_
|
||||
}
|
||||
|
||||
input_rc.timestamp = now;
|
||||
input_rc.timestamp_last_signal = input_rc.timestamp;
|
||||
input_rc.rc_ppm_frame_length = 0;
|
||||
input_rc.timestamp_sample = input_rc.timestamp;
|
||||
|
||||
/* fake rssi if no value was provided */
|
||||
if (rssi == -1) {
|
||||
@ -201,9 +200,7 @@ void fill_input_rc(uint16_t raw_rc_count, uint16_t raw_rc_values[input_rc_s::RC_
|
||||
}
|
||||
|
||||
input_rc.rc_failsafe = failsafe;
|
||||
input_rc.rc_lost = (valid_chans == 0);
|
||||
input_rc.rc_lost_frame_count = frame_drops;
|
||||
input_rc.rc_total_frame_count = 0;
|
||||
}
|
||||
|
||||
int start(int argc, char *argv[])
|
||||
|
||||
@ -1996,12 +1996,8 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
|
||||
// fill uORB message
|
||||
input_rc_s rc{};
|
||||
// metadata
|
||||
rc.timestamp = rc.timestamp_last_signal = hrt_absolute_time();
|
||||
rc.timestamp_sample = hrt_absolute_time();
|
||||
rc.rssi = input_rc_s::RSSI_MAX;
|
||||
rc.rc_failsafe = false;
|
||||
rc.rc_lost = false;
|
||||
rc.rc_lost_frame_count = 0;
|
||||
rc.rc_total_frame_count = 1;
|
||||
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
|
||||
|
||||
// channels
|
||||
@ -2063,6 +2059,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
|
||||
}
|
||||
|
||||
// publish uORB message
|
||||
rc.timestamp = hrt_absolute_time();
|
||||
_rc_pub.publish(rc);
|
||||
}
|
||||
|
||||
|
||||
@ -56,7 +56,7 @@
|
||||
|
||||
#include "px4io.h"
|
||||
|
||||
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
|
||||
static bool ppm_input(uint16_t *values, uint16_t *num_values);
|
||||
static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated);
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
@ -72,117 +72,92 @@ int _sbus_fd = -1;
|
||||
static unsigned _rssi_adc_counts = 0;
|
||||
#endif
|
||||
|
||||
/* receive signal strenght indicator (RSSI). 0 = no connection, 100 (input_rc_s::RSSI_MAX): perfect connection */
|
||||
/* Note: this is static because RC-provided telemetry does not occur every tick */
|
||||
static uint16_t _rssi = 0;
|
||||
static unsigned _frame_drops = 0;
|
||||
|
||||
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
|
||||
{
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_begin(c_gather_dsm);
|
||||
#endif
|
||||
uint8_t n_bytes = 0;
|
||||
uint8_t *bytes;
|
||||
bool dsm_11_bit;
|
||||
int8_t spektrum_rssi;
|
||||
unsigned frame_drops;
|
||||
unsigned frame_drops = 0;
|
||||
*dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
|
||||
&spektrum_rssi, &frame_drops, PX4IO_RC_INPUT_CHANNELS);
|
||||
|
||||
if (*dsm_updated) {
|
||||
|
||||
if (dsm_11_bit) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
|
||||
}
|
||||
|
||||
if (frame_drops == _frame_drops) {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags |= (PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
}
|
||||
|
||||
_frame_drops = frame_drops;
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM;
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
|
||||
if (spektrum_rssi >= 0 && spektrum_rssi <= 100) {
|
||||
|
||||
/* ensure ADC RSSI is disabled */
|
||||
r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
|
||||
|
||||
*rssi = spektrum_rssi;
|
||||
}
|
||||
|
||||
r_page_raw_rc_input[PX4IO_P_RAW_LOST_FRAME_COUNT] = frame_drops;
|
||||
}
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_end(c_gather_dsm);
|
||||
#endif
|
||||
|
||||
/* get data from FD and attempt to parse with DSM and ST24 libs */
|
||||
uint8_t st24_rssi, lost_count;
|
||||
uint16_t st24_channel_count = 0;
|
||||
|
||||
*st24_updated = false;
|
||||
|
||||
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
|
||||
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_DSM | PX4IO_P_RAW_RC_FLAGS_RC_SUMD))) {
|
||||
uint8_t st24_rssi = 0;
|
||||
uint8_t lost_count = 0;
|
||||
uint16_t st24_channel_count = 0;
|
||||
|
||||
for (unsigned i = 0; i < n_bytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
st24_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX;
|
||||
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
|
||||
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
|
||||
}
|
||||
}
|
||||
|
||||
if (*st24_updated && lost_count == 0) {
|
||||
if (*st24_updated && lost_count == 0) {
|
||||
/* ensure ADC RSSI is disabled */
|
||||
r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
|
||||
|
||||
/* ensure ADC RSSI is disabled */
|
||||
r_setup_features &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI);
|
||||
*rssi = st24_rssi;
|
||||
r_raw_rc_count = st24_channel_count;
|
||||
|
||||
*rssi = st24_rssi;
|
||||
r_raw_rc_count = st24_channel_count;
|
||||
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_ST24;
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* get data from FD and attempt to parse with SUMD libs */
|
||||
uint8_t sumd_rssi, sumd_rx_count;
|
||||
uint16_t sumd_channel_count = 0;
|
||||
bool sumd_failsafe_state;
|
||||
|
||||
*sumd_updated = false;
|
||||
|
||||
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) {
|
||||
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_DSM | PX4IO_P_RAW_RC_FLAGS_RC_ST24))) {
|
||||
uint8_t sumd_rssi = 0;
|
||||
uint8_t sumd_rx_count = 0;
|
||||
uint16_t sumd_channel_count = 0;
|
||||
bool sumd_failsafe_state = false;
|
||||
|
||||
for (unsigned i = 0; i < n_bytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
sumd_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX;
|
||||
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
|
||||
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
|
||||
}
|
||||
}
|
||||
|
||||
if (*sumd_updated) {
|
||||
if (*sumd_updated) {
|
||||
/* not setting RSSI since SUMD does not provide one */
|
||||
r_raw_rc_count = sumd_channel_count;
|
||||
|
||||
/* not setting RSSI since SUMD does not provide one */
|
||||
r_raw_rc_count = sumd_channel_count;
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_SUMD;
|
||||
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
if (sumd_failsafe_state) {
|
||||
r_raw_rc_flags |= (PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
|
||||
if (sumd_failsafe_state) {
|
||||
r_raw_rc_flags |= (PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return (*dsm_updated | *st24_updated | *sumd_updated);
|
||||
}
|
||||
|
||||
@ -218,6 +193,8 @@ controls_tick()
|
||||
* other. Don't do that.
|
||||
*/
|
||||
|
||||
uint16_t rssi = 0;
|
||||
|
||||
#ifdef ADC_RSSI
|
||||
|
||||
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
|
||||
@ -229,45 +206,39 @@ controls_tick()
|
||||
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
|
||||
unsigned mV = _rssi_adc_counts * 3300 / 4095;
|
||||
/* scale to 0..100 (input_rc_s::RSSI_MAX == 100) */
|
||||
_rssi = (mV * INPUT_RC_RSSI_MAX / 3300);
|
||||
rssi = (mV * INPUT_RC_RSSI_MAX / 3300);
|
||||
|
||||
if (_rssi > INPUT_RC_RSSI_MAX) {
|
||||
_rssi = INPUT_RC_RSSI_MAX;
|
||||
if (rssi > INPUT_RC_RSSI_MAX) {
|
||||
rssi = INPUT_RC_RSSI_MAX;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* zero RSSI if signal is lost */
|
||||
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_OK))) {
|
||||
_rssi = 0;
|
||||
}
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_begin(c_gather_sbus);
|
||||
#endif
|
||||
|
||||
bool sbus_updated = false;
|
||||
|
||||
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
|
||||
bool sbus_failsafe, sbus_frame_drop;
|
||||
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_PPM | PX4IO_P_RAW_RC_FLAGS_RC_DSM | PX4IO_P_RAW_RC_FLAGS_RC_ST24 |
|
||||
PX4IO_P_RAW_RC_FLAGS_RC_SUMD))) {
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_begin(c_gather_sbus);
|
||||
#endif
|
||||
bool sbus_failsafe = false;
|
||||
bool sbus_frame_drop = false;
|
||||
sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
|
||||
PX4IO_RC_INPUT_CHANNELS);
|
||||
|
||||
if (sbus_updated) {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_SBUS;
|
||||
|
||||
unsigned sbus_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX
|
||||
|
||||
if (sbus_frame_drop) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
|
||||
sbus_rssi = INPUT_RC_RSSI_MAX / 2;
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
}
|
||||
|
||||
r_page_raw_rc_input[PX4IO_P_RAW_LOST_FRAME_COUNT] = sbus_dropped_frames();
|
||||
|
||||
if (sbus_failsafe) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
||||
|
||||
@ -277,56 +248,51 @@ controls_tick()
|
||||
|
||||
/* set RSSI to an emulated value if ADC RSSI is off */
|
||||
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
||||
_rssi = sbus_rssi;
|
||||
rssi = sbus_rssi;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_end(c_gather_sbus);
|
||||
perf_end(c_gather_sbus);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* XXX each S.bus frame will cause a PPM decoder interrupt
|
||||
* storm (lots of edges). It might be sensible to actually
|
||||
* disable the PPM decoder completely if we have S.bus signal.
|
||||
*/
|
||||
bool ppm_updated = false;
|
||||
|
||||
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_SBUS | PX4IO_P_RAW_RC_FLAGS_RC_DSM | PX4IO_P_RAW_RC_FLAGS_RC_ST24 |
|
||||
PX4IO_P_RAW_RC_FLAGS_RC_SUMD))) {
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_begin(c_gather_ppm);
|
||||
perf_begin(c_gather_ppm);
|
||||
#endif
|
||||
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
|
||||
ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
|
||||
|
||||
if (ppm_updated) {
|
||||
if (ppm_updated) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_PPM;
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
||||
}
|
||||
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_PPM);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_end(c_gather_ppm);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_end(c_gather_ppm);
|
||||
#endif
|
||||
|
||||
bool dsm_updated = false, st24_updated = false, sumd_updated = false;
|
||||
bool dsm_updated = false;
|
||||
bool st24_updated = false;
|
||||
bool sumd_updated = false;
|
||||
|
||||
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) {
|
||||
if (!(r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_RC_SBUS | PX4IO_P_RAW_RC_FLAGS_RC_PPM))) {
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_begin(c_gather_dsm);
|
||||
#endif
|
||||
|
||||
(void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated);
|
||||
|
||||
if (dsm_updated) {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_DSM);
|
||||
}
|
||||
|
||||
if (st24_updated) {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_ST24);
|
||||
}
|
||||
|
||||
if (sumd_updated) {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD);
|
||||
}
|
||||
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
perf_end(c_gather_dsm);
|
||||
@ -339,81 +305,40 @@ controls_tick()
|
||||
}
|
||||
|
||||
/* store RSSI */
|
||||
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = _rssi;
|
||||
|
||||
/*
|
||||
* In some cases we may have received a frame, but input has still
|
||||
* been lost.
|
||||
*/
|
||||
bool rc_input_lost = false;
|
||||
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
|
||||
|
||||
/*
|
||||
* If we received a new frame from any of the RC sources, process it.
|
||||
*/
|
||||
if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
|
||||
|
||||
/* record a bitmask of channels assigned */
|
||||
unsigned assigned_channels = 0;
|
||||
|
||||
/* update RC-received timestamp */
|
||||
system_state.rc_channels_timestamp_received = hrt_absolute_time();
|
||||
|
||||
/* set RC OK flag, as we got an update */
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;
|
||||
r_page_raw_rc_input[PX4IO_P_RAW_FRAME_COUNT]++;
|
||||
|
||||
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
|
||||
if (assigned_channels > 4) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
|
||||
} else if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
|
||||
/* clear the input-kind flags here, but only when disarmed */
|
||||
if (!(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_PPM;
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM;
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_SBUS;
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_ST24;
|
||||
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_SUMD;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* If we haven't seen any new control data in 200ms, assume we
|
||||
* have lost input.
|
||||
*/
|
||||
if (!rc_input_lost && hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
|
||||
rc_input_lost = true;
|
||||
|
||||
/* clear the input-kind flags here */
|
||||
atomic_modify_clear(&r_status_flags, (
|
||||
PX4IO_P_STATUS_FLAGS_RC_PPM |
|
||||
PX4IO_P_STATUS_FLAGS_RC_DSM |
|
||||
PX4IO_P_STATUS_FLAGS_RC_SBUS |
|
||||
PX4IO_P_STATUS_FLAGS_RC_ST24 |
|
||||
PX4IO_P_STATUS_FLAGS_RC_SUMD));
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
* Handle losing RC input
|
||||
*/
|
||||
|
||||
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
|
||||
if (rc_input_lost) {
|
||||
/* Clear the RC input status flag, clear manual override flag */
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
|
||||
/* flag raw RC as lost */
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
|
||||
|
||||
/* Set raw channel count to zero */
|
||||
r_raw_rc_count = 0;
|
||||
|
||||
/* Set the RC_LOST alarm */
|
||||
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST);
|
||||
}
|
||||
}
|
||||
|
||||
static bool
|
||||
ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
||||
ppm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
bool result = false;
|
||||
|
||||
if (!(num_values) || !(values) || !(frame_len)) {
|
||||
if (!(num_values) || !(values)) {
|
||||
return result;
|
||||
}
|
||||
|
||||
@ -441,7 +366,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
||||
ppm_last_valid_decode = 0;
|
||||
|
||||
/* store PPM frame length */
|
||||
*frame_len = ppm_frame_length;
|
||||
//*frame_len = ppm_frame_length;
|
||||
|
||||
/* good if we got any channels */
|
||||
result = (*num_values > 0);
|
||||
|
||||
@ -98,29 +98,19 @@
|
||||
#define PX4IO_PAGE_STATUS 1
|
||||
#define PX4IO_P_STATUS_FREEMEM 0
|
||||
#define PX4IO_P_STATUS_CPULOAD 1
|
||||
|
||||
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
|
||||
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
|
||||
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 1) /* controls from FMU are valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 2) /* raw PWM from FMU */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 3) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 4) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 5) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 6) /* safety is off */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 7) /* FMU was initialized and OK once */
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 0) /* timed out waiting for RC input */
|
||||
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 1) /* PWM configuration or output was bad */
|
||||
|
||||
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
|
||||
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
|
||||
#define PX4IO_P_STATUS_VSERVO 4 /* [2] servo rail voltage in mV */
|
||||
#define PX4IO_P_STATUS_VRSSI 5 /* [2] RSSI voltage */
|
||||
|
||||
/* array of PWM servo output values, microseconds */
|
||||
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
@ -129,17 +119,16 @@
|
||||
#define PX4IO_PAGE_RAW_RC_INPUT 4
|
||||
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
|
||||
#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */
|
||||
|
||||
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 0) /* receiver is in failsafe mode */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM (1 << 1) /* DSM decoding */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_PPM (1 << 2) /* PPM decoding */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_SBUS (1 << 3) /* SBUS decoding */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_ST24 (1 << 4) /* ST24 decoding */
|
||||
#define PX4IO_P_RAW_RC_FLAGS_RC_SUMD (1 << 5) /* SUMD decoding */
|
||||
#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
|
||||
#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
|
||||
#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
#define PX4IO_P_RAW_FRAME_COUNT 3 /* Number of total received frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_LOST_FRAME_COUNT 4 /* Number of total dropped frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_RC_BASE 5 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
|
||||
/* array of raw ADC values */
|
||||
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
|
||||
|
||||
@ -106,8 +106,7 @@ uint16_t r_page_raw_rc_input[] = {
|
||||
[PX4IO_P_RAW_RC_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_FLAGS] = 0,
|
||||
[PX4IO_P_RAW_RC_NRSSI] = 0,
|
||||
[PX4IO_P_RAW_RC_DATA] = 0,
|
||||
[PX4IO_P_RAW_FRAME_COUNT] = 0,
|
||||
[PX4IO_P_RAW_FRAME_COUNT] = 0,
|
||||
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
@ -372,16 +372,14 @@ void RCUpdate::Run()
|
||||
if (_input_rc_sub.update(&input_rc)) {
|
||||
|
||||
// warn if the channel count is changing (possibly indication of error)
|
||||
if (!input_rc.rc_lost) {
|
||||
if ((_channel_count_previous != input_rc.channel_count)
|
||||
&& (_channel_count_previous > 0)) {
|
||||
PX4_WARN("channel count changed %d -> %d", _channel_count_previous, input_rc.channel_count);
|
||||
}
|
||||
if ((_channel_count_previous != input_rc.channel_count)
|
||||
&& (_channel_count_previous > 0)) {
|
||||
PX4_WARN("channel count changed %d -> %d", _channel_count_previous, input_rc.channel_count);
|
||||
}
|
||||
|
||||
if ((_input_source_previous != input_rc.input_source)
|
||||
&& (_input_source_previous != input_rc_s::RC_INPUT_SOURCE_UNKNOWN)) {
|
||||
PX4_WARN("input source changed %d -> %d", _input_source_previous, input_rc.input_source);
|
||||
}
|
||||
if ((_input_source_previous != input_rc.input_source)
|
||||
&& (_input_source_previous != input_rc_s::RC_INPUT_SOURCE_UNKNOWN)) {
|
||||
PX4_WARN("input source changed %d -> %d", _input_source_previous, input_rc.input_source);
|
||||
}
|
||||
|
||||
const bool input_source_stable = (input_rc.input_source == _input_source_previous);
|
||||
@ -400,7 +398,7 @@ void RCUpdate::Run()
|
||||
bool signal_lost = true;
|
||||
|
||||
/* check flags and require at least four channels to consider the signal valid */
|
||||
if (input_rc.rc_lost || input_rc.rc_failsafe || input_rc.channel_count < 4) {
|
||||
if (input_rc.rc_failsafe || input_rc.channel_count < 4) {
|
||||
/* signal is lost or no enough channels */
|
||||
signal_lost = true;
|
||||
|
||||
@ -503,7 +501,7 @@ void RCUpdate::Run()
|
||||
_rc.channel_count = input_rc.channel_count;
|
||||
_rc.rssi = input_rc.rssi;
|
||||
_rc.signal_lost = _rc_signal_lost_hysteresis.get_state();
|
||||
_rc.timestamp = input_rc.timestamp_last_signal;
|
||||
_rc.timestamp = input_rc.timestamp;
|
||||
_rc.frame_drop_count = input_rc.rc_lost_frame_count;
|
||||
|
||||
/* publish rc_channels topic even if signal is invalid, for debug */
|
||||
@ -512,8 +510,8 @@ void RCUpdate::Run()
|
||||
// only publish manual control if the signal is present and regularly updating
|
||||
if (input_source_stable && channel_count_stable && !_rc_signal_lost_hysteresis.get_state()) {
|
||||
|
||||
if ((input_rc.timestamp_last_signal > _last_timestamp_signal)
|
||||
&& (input_rc.timestamp_last_signal < _last_timestamp_signal + VALID_DATA_MIN_INTERVAL_US)) {
|
||||
if ((input_rc.timestamp_sample > _last_timestamp_sample)
|
||||
&& (input_rc.timestamp_sample < _last_timestamp_sample + VALID_DATA_MIN_INTERVAL_US)) {
|
||||
|
||||
perf_count(_valid_data_interval_perf);
|
||||
|
||||
@ -529,10 +527,10 @@ void RCUpdate::Run()
|
||||
|
||||
// limit processing if there's no update
|
||||
if (rc_updated || (hrt_elapsed_time(&_last_manual_control_input_publish) > 300_ms)) {
|
||||
UpdateManualControlInput(input_rc.timestamp_last_signal);
|
||||
UpdateManualControlInput(input_rc.timestamp_sample);
|
||||
}
|
||||
|
||||
UpdateManualSwitches(input_rc.timestamp_last_signal);
|
||||
UpdateManualSwitches(input_rc.timestamp_sample);
|
||||
|
||||
/* Update parameters from RC Channels (tuning with RC) if activated */
|
||||
if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
|
||||
@ -541,7 +539,7 @@ void RCUpdate::Run()
|
||||
}
|
||||
}
|
||||
|
||||
_last_timestamp_signal = input_rc.timestamp_last_signal;
|
||||
_last_timestamp_sample = input_rc.timestamp_sample;
|
||||
|
||||
} else {
|
||||
// RC input unstable or lost, clear any previous manual_switches
|
||||
|
||||
@ -181,7 +181,7 @@ private:
|
||||
|
||||
hrt_abstime _last_manual_control_input_publish{0};
|
||||
hrt_abstime _last_rc_to_param_map_time{0};
|
||||
hrt_abstime _last_timestamp_signal{0};
|
||||
hrt_abstime _last_timestamp_sample{0};
|
||||
|
||||
uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {};
|
||||
float _last_manual_control_input[3] {};
|
||||
|
||||
@ -649,7 +649,7 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
|
||||
mavlink_msg_rc_channels_decode(msg, &rc_channels);
|
||||
|
||||
input_rc_s rc_input{};
|
||||
rc_input.timestamp_last_signal = hrt_absolute_time();
|
||||
rc_input.timestamp_sample = hrt_absolute_time();
|
||||
rc_input.channel_count = rc_channels.chancount;
|
||||
rc_input.rssi = rc_channels.rssi;
|
||||
rc_input.values[0] = rc_channels.chan1_raw;
|
||||
|
||||
@ -118,7 +118,7 @@ int test_rc(int argc, char *argv[])
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) {
|
||||
if (hrt_absolute_time() - rc_input.timestamp_sample > 100000) {
|
||||
PX4_ERR("TIMEOUT, less than 10 Hz updates");
|
||||
(void)orb_unsubscribe(_rc_sub);
|
||||
return ERROR;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user