diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index d2faf8d2d9..00dc1e145b 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -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) { diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin index 0179a993e6..c12e9682d1 100755 Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin index 0179a993e6..c12e9682d1 100755 Binary files a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp index be17b084b3..3f0ddfb332 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp +++ b/boards/emlid/navio2/navio_sysfs_rc_in/NavioSysRCInput.cpp @@ -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(); diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/mro/x21-777/extras/px4_io-v2_default.bin and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin index cef251c5f5..3db45ccdad 100755 Binary files a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ diff --git a/msg/input_rc.msg b/msg/input_rc.msg index d26c31ea0f..cf194c3b49 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -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 diff --git a/msg/px4io_status.msg b/msg/px4io_status.msg index df072e71c2..5eb74deb76 100644 --- a/msg/px4io_status.msg +++ b/msg/px4io_status.msg @@ -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 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7927c623e5..d20657410a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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); } diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 071a678d0d..4bec4a26a4 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -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; } diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.cpp b/src/drivers/rpi_rc_in/rpi_rc_in.cpp index d565bac27d..18aca8c89e 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.cpp +++ b/src/drivers/rpi_rc_in/rpi_rc_in.cpp @@ -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); diff --git a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp index 228b422500..2b518b6f83 100644 --- a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp +++ b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp @@ -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[]) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d3c4a8fc47..51b605d6a0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 7d22df983c..2d02e8353e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -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); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 73c82dae9d..18314ded07 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index d8166d1dbb..aa55779c5b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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 }; diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index bb2d711d66..ca9fb0345a 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -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 diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 426ffa02ee..22d36178a1 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -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] {}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2e2ea3c69b..747bf315bf 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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; diff --git a/src/systemcmds/tests/test_rc.cpp b/src/systemcmds/tests/test_rc.cpp index 97f7065115..686cdba2e2 100644 --- a/src/systemcmds/tests/test_rc.cpp +++ b/src/systemcmds/tests/test_rc.cpp @@ -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;