Run jsbsim

This commit adds a hitl_run script which can be used for various simulations when running HITL simulation

Tools/hitl_run.sh

@@ -0,0 +1,122 @@
+#!/bin/bash
+# Run HITL simulation with PX4
+
+function cleanup() {
+	pkill -x px4
+	pkill gzclient
+	pkill gzserver
+}
+
+if [ "$1" == "-h" ] || [ "$1" == "--help" ]
+then
+	echo "Usage: $0 [-s <simulation>] [-m <vehicle_model>] [-w <world>] [-d <device>] [-b <baudrate>]"
+	echo "  This script allows you to run HITL simulation with PX4"
+	exit 1
+fi
+
+while getopts m:w:s:d:b:t:l: option
+do
+	case "${option}"
+	in
+		m) VEHICLE_MODEL=${OPTARG};;
+		w) WORLD=${OPTARG};;
+		s) SIMULATION=${OPTARG};;
+		d) DEVICE=${OPTARG};;
+		b) BAUDRATE=${OPTARG};;
+		l) LABEL=_${OPTARG};;
+	esac
+done
+
+world=${WORLD:=empty}
+target=${TARGET:=px4_sitl_default}
+model=${VEHICLE_MODEL:="iris"}
+simulation=${SIMULATION:="gazebo"}
+device=${DEVICE:="/dev/ttyACM0"}
+baudrate=${BAUDRATE:="921600"}
+export PX4_SIM_MODEL=${model}${LABEL}
+
+echo SIMULATION: ${simulation}
+echo MODEL: ${PX4_SIM_MODEL}
+
+echo ${SCRIPT}
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+src_path="$SCRIPT_DIR/.."
+
+sleep 1
+
+SIM_PID=0
+
+if [ "$simulation" == "jmavsim" ] && [ ! -n "$no_sim" ]; then
+	# Start Java simulator
+	"$src_path"/Tools/jmavsim_run.sh -q -s -d ${device} -b ${baudrate} -r 250 &
+	SIM_PID=$!
+elif [ "$simulation" == "gazebo" ] && [ ! -n "$no_sim" ]; then
+	if [ -x "$(command -v gazebo)" ]; then
+		# Set the plugin path so Gazebo finds our model and sim
+		source "$src_path/Tools/setup_gazebo.bash" "${src_path}" "${build_path}"
+		output_sdf=/tmp/${model}_hitl.sdf
+		python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${model}/${model}.sdf.jinja ${src_path}/Tools/sitl_gazebo --serial_enabled 1  --hil_mode 1 --serial_device ${device} --serial_baudrate ${baudrate} --mavlink_id 1 --output-file ${output_sdf}
+
+		if [ -z $PX4_SITL_WORLD ]; then
+			#Spawn predefined world
+			if [ "$world" == "none" ]; then
+				if [ -f ${src_path}/Tools/sitl_gazebo/worlds/${model}.world ]; then
+					echo "empty world, default world ${model}.world for model found"
+					world_path="${src_path}/Tools/sitl_gazebo/worlds/${model}.world"
+				else
+					echo "empty world, setting empty.world as default"
+					world_path="${src_path}/Tools/sitl_gazebo/worlds/empty.world"
+				fi
+			else
+				#Spawn empty world if world with model name doesn't exist
+				world_path="${src_path}/Tools/sitl_gazebo/worlds/${world}.world"
+			fi
+		else
+			if [ -f ${src_path}/Tools/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world ]; then
+				# Spawn world by name if exists in the worlds directory from environment variable
+				world_path="${src_path}/Tools/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world"
+			else
+				# Spawn world from environment variable with absolute path
+				world_path="$PX4_SITL_WORLD"
+			fi
+		fi
+		gzserver $verbose $world_path &
+		SIM_PID=$!
+
+		while gz model --verbose --spawn-file="${output_sdf}" --model-name=${model} -x 1.01 -y 0.98 -z 0.83 2>&1 | grep -q "An instance of Gazebo is not running."; do
+			echo "gzserver not ready yet, trying again!"
+			sleep 1
+		done
+
+		if [[ -n "$HEADLESS" ]]; then
+			echo "not running gazebo gui"
+		else
+			# gzserver needs to be running to avoid a race. Since the launch
+			# is putting it into the background we need to avoid it by backing off
+			sleep 3
+			nice -n 20 gzclient --verbose $follow_mode
+			GUI_PID=$!
+		fi
+	else
+		echo "You need to have gazebo simulator installed!"
+		exit 1
+	fi
+elif [ "$simulation" == "jsbsim" ] && [ -z "$no_sim" ]; then
+	source "$src_path/Tools/setup_jsbsim.bash" "${src_path}" "${build_path}" ${model}
+	if [[ -n "$HEADLESS" ]]; then
+		echo "not running flightgear gui"
+	else
+		fgfs --fdm=null \
+			--native-fdm=socket,in,60,,5550,udp \
+			--aircraft=$JSBSIM_AIRCRAFT_MODEL \
+			--airport=${world} \
+			--disable-hud \
+			--disable-ai-models &> /dev/null &
+		FGFS_PID=$!
+	fi
+	"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -d ${device} -b ${baudrate} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml"
+	SIM_PID=$!
+fi
+
+
+trap "cleanup" SIGINT SIGTERM EXIT

boards/px4/fmu-v5/default.cmake

@@ -25,7 +25,7 @@ px4_add_board(
 		distance_sensor # all available distance sensor drivers
 		dshot
 		gps
-		#heater
+		heater
 		#imu # all available imu drivers
 		imu/analog_devices/adis16448
 		imu/bosch/bmi055

msg/heater_status.msg

@@ -3,6 +3,7 @@ uint64 timestamp	# time since system start (microseconds)
 uint32 device_id
 
 bool heater_on
+bool temperature_target_met
 
 float32 temperature_sensor
 float32 temperature_target

src/drivers/heater/heater.cpp

@@ -77,7 +77,7 @@ Heater::Heater() :
 
 Heater::~Heater()
 {
-	heater_disable();
+	disable_heater();
 }
 
 int Heater::custom_command(int argc, char *argv[])
@@ -91,7 +91,7 @@ int Heater::custom_command(int argc, char *argv[])
 	return print_usage("Unrecognized command.");
 }
 
-void Heater::heater_disable()
+void Heater::disable_heater()
 {
 	// Reset heater to off state.
 #ifdef HEATER_PX4IO
@@ -106,7 +106,7 @@ void Heater::heater_disable()
 #endif
 }
 
-void Heater::heater_initialize()
+void Heater::initialize_heater_io()
 {
 	// Initialize heater to off state.
 #ifdef HEATER_PX4IO
@@ -160,13 +160,15 @@ bool Heater::initialize_topics()
 	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
 
-		if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0
-		    && PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
+		if (sensor_accel_sub.get().timestamp != 0 &&
+		    sensor_accel_sub.get().device_id != 0 &&
+		    PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
 
 			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
 			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
 				_sensor_accel_sub.ChangeInstance(i);
 				_sensor_device_id = sensor_accel_sub.get().device_id;
+				initialize_heater_io();
 				return true;
 			}
 		}
@@ -190,21 +192,10 @@ void Heater::Run()
 		return;
 	}
 
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
+	update_params();
 
 	if (_sensor_device_id == 0) {
-		if (initialize_topics()) {
-			heater_initialize();
-
-		} else {
+		if (!initialize_topics()) {
 			// if sensor still not found try again in 1 second
 			ScheduleDelayed(1_s);
 			return;
@@ -212,14 +203,15 @@ void Heater::Run()
 	}
 
 	sensor_accel_s sensor_accel;
+	float temperature_delta {0.f};
 
 	if (_heater_on) {
 		// Turn the heater off.
-		heater_off();
 		_heater_on = false;
+		heater_off();
+		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
 
 	} else if (_sensor_accel_sub.update(&sensor_accel)) {
-		float temperature_delta {0.f};
 
 		// Update the current IMU sensor temperature if valid.
 		if (PX4_ISFINITE(sensor_accel.temperature)) {
@@ -230,11 +222,6 @@ void Heater::Run()
 		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
 		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
 
-		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
-			_integrator_value = 0.f;
-		}
-
-		// Guard against integrator wind up.
 		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
 
 		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
@@ -242,36 +229,41 @@ void Heater::Run()
 
 		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
 
+		if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) {
+			_temperature_target_met = true;
+
+		} else {
+
+			_temperature_target_met = false;
+		}
+
 		_heater_on = true;
 		heater_on();
-	}
-
-	// Schedule the next cycle.
-	if (_heater_on) {
 		ScheduleDelayed(_controller_time_on_usec);
-
-	} else {
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
 	}
 
+	publish_status();
+}
 
-	// publish status
+void Heater::publish_status()
+{
 	heater_status_s status{};
-	status.heater_on = _heater_on;
-	status.device_id = _sensor_device_id;
-	status.temperature_sensor = _temperature_last;
-	status.temperature_target = _param_sens_imu_temp.get();
-	status.controller_period_usec = _controller_period_usec;
+	status.device_id               = _sensor_device_id;
+	status.heater_on               = _heater_on;
+	status.temperature_sensor      = _temperature_last;
+	status.temperature_target      = _param_sens_imu_temp.get();
+	status.temperature_target_met  = _temperature_target_met;
+	status.controller_period_usec  = _controller_period_usec;
 	status.controller_time_on_usec = _controller_time_on_usec;
-	status.proportional_value = _proportional_value;
-	status.integrator_value = _integrator_value;
-	status.feed_forward_value = _param_sens_imu_temp_ff.get();
+	status.proportional_value      = _proportional_value;
+	status.integrator_value        = _integrator_value;
+	status.feed_forward_value      = _param_sens_imu_temp_ff.get();
 
 #ifdef HEATER_PX4IO
-	status.mode |= heater_status_s::MODE_PX4IO;
+	status.mode = heater_status_s::MODE_PX4IO;
 #endif
 #ifdef HEATER_GPIO
-	status.mode |= heater_status_s::MODE_GPIO;
+	status.mode = heater_status_s::MODE_GPIO;
 #endif
 
 	status.timestamp = hrt_absolute_time();
@@ -287,6 +279,7 @@ int Heater::start()
 		return PX4_ERROR;
 	}
 
+	update_params(true);
 	ScheduleNow();
 	return PX4_OK;
 }
@@ -307,6 +300,18 @@ int Heater::task_spawn(int argc, char *argv[])
 	return 0;
 }
 
+void Heater::update_params(const bool force)
+{
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// update parameters from storage
+		ModuleParams::updateParams();
+	}
+}
+
 int Heater::print_usage(const char *reason)
 {
 	if (reason) {

src/drivers/heater/heater.h

@@ -57,7 +57,8 @@
 
 using namespace time_literals;
 
-#define CONTROLLER_PERIOD_DEFAULT 100000
+#define CONTROLLER_PERIOD_DEFAULT    10000
+#define TEMPERATURE_TARGET_THRESHOLD 2.5f
 
 class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
 {
@@ -101,9 +102,25 @@ public:
 
 private:
 
+	/** Disables the heater (either by GPIO or PX4IO). */
+	void disable_heater();
+
+	/** Turns the heater on (either by GPIO or PX4IO). */
+	void heater_on();
+
+	/** Turns the heater off (either by GPIO or PX4IO). */
+	void heater_off();
+
+	void initialize();
+
+	/** Enables / configures the heater (either by GPIO or PX4IO). */
+	void initialize_heater_io();
+
 	/** @brief Called once to initialize uORB topics. */
 	bool initialize_topics();
 
+	void publish_status();
+
 	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
 	void Run() override;
 
@@ -113,18 +130,6 @@ private:
 	 */
 	void update_params(const bool force = false);
 
-	/** Enables / configures the heater (either by GPIO or PX4IO). */
-	void heater_initialize();
-
-	/** Disnables the heater (either by GPIO or PX4IO). */
-	void heater_disable();
-
-	/** Turns the heater on (either by GPIO or PX4IO). */
-	void heater_on();
-
-	/** Turns the heater off (either by GPIO or PX4IO). */
-	void heater_off();
-
 	/** Work queue struct for the scheduler. */
 	static struct work_s _work;
 
@@ -133,7 +138,9 @@ private:
 	int _io_fd {-1};
 #endif
 
-	bool _heater_on = false;
+	bool _heater_initialized     = false;
+	bool _heater_on              = false;
+	bool _temperature_target_met = false;
 
 	int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
 	int _controller_time_on_usec = 0;
@@ -155,7 +162,7 @@ private:
 		(ParamFloat<px4::params::SENS_IMU_TEMP_FF>) _param_sens_imu_temp_ff,
 		(ParamFloat<px4::params::SENS_IMU_TEMP_I>)  _param_sens_imu_temp_i,
 		(ParamFloat<px4::params::SENS_IMU_TEMP_P>)  _param_sens_imu_temp_p,
-		(ParamInt<px4::params::SENS_TEMP_ID>)       _param_sens_temp_id,
-		(ParamFloat<px4::params::SENS_IMU_TEMP>)    _param_sens_imu_temp
+		(ParamFloat<px4::params::SENS_IMU_TEMP>)    _param_sens_imu_temp,
+		(ParamInt<px4::params::SENS_TEMP_ID>)       _param_sens_temp_id
 	)
 };

src/lib/rc/dsm.cpp

@@ -81,6 +81,7 @@ static unsigned dsm_partial_frame_count;	/**< Count of bytes received for curren
 static unsigned dsm_channel_shift = 0;			/**< Channel resolution, 0=unknown, 10=10 bit (1024), 11=11 bit (2048) */
 static unsigned dsm_frame_drops = 0;			/**< Count of incomplete DSM frames */
 static uint16_t dsm_chan_count = 0;         /**< DSM channel count */
+static uint16_t dsm_chan_count_prev = 0;    /**< last valid DSM channel count */
 
 /**
  * Attempt to decode a single channel raw channel datum
@@ -135,7 +136,7 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 
 		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
 		//  ±100% travel is 1102µs to 1898 µs
-		if (value < 990 || value > 2010) {
+		if (value < 903 || value > 2097) {
 			// if the value is unrealistic, fail the parsing entirely
 			PX4_DEBUG("channel %d invalid range %d", channel, value);
 			return false;
@@ -190,12 +191,14 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, uint8_t &channel, u
 
 		// Spektrum range is 903μs to 2097μs (Specification for Spektrum Remote Receiver Interfacing Rev G 9.1)
 		//  ±100% travel is 1102µs to 1898 µs
-		if (value < 990 || value > 2010) {
+		if (value < 903 || value > 2097) {
 			// if the value is unrealistic, fail the parsing entirely
 			PX4_DEBUG("channel %d invalid range %d", channel, value);
 			return false;
 		}
 
+		PX4_DEBUG(stderr, "CH%d=%d(0x%02x), ", channel, value, raw);
+
 		return true;
 	}
 
@@ -215,6 +218,7 @@ static bool dsm_guess_format(bool reset)
 
 	/* reset the 10/11 bit sniffed channel masks */
 	if (reset) {
+		PX4_DEBUG("dsm_guess_format reset");
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
@@ -283,8 +287,8 @@ static bool dsm_guess_format(bool reset)
 	printf("dsm guess format: samples: %d %s\n", samples, (reset) ? "RESET" : "");
 #endif
 
-	/* wait until we have seen plenty of frames - 5 should normally be enough */
-	if (samples < 5) {
+	/* wait until we have seen plenty of frames */
+	if (samples < 10) {
 		return false;
 	}
 
@@ -597,6 +601,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 		// abort if channel already found, no duplicate channels per DSM frame
 		if (channels_found[channel]) {
 			PX4_DEBUG("duplicate channel %d\n\n", channel);
+			dsm_guess_format(true);
 			return false;
 
 		} else {
@@ -605,6 +610,7 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 
 		/* reset bit guessing state machine if the channel index is out of bounds */
 		if (channel > DSM_MAX_CHANNEL_COUNT) {
+			PX4_DEBUG("channel %d > %d (DSM_MAX_CHANNEL_COUNT)", channel, DSM_MAX_CHANNEL_COUNT);
 			dsm_guess_format(true);
 			return false;
 		}
@@ -645,16 +651,6 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values,
 		values[channel] = value;
 	}
 
-	/*
-	 * Spektrum likes to send junk in higher channel numbers to fill
-	 * their packets. We don't know about a 13 channel model in their TX
-	 * lines, so if we get a channel count of 13, we'll return 12 (the last
-	 * data index that is stable).
-	 */
-	if (*num_values == 13) {
-		*num_values = 12;
-	}
-
 	/* Set the 11-bit data indicator */
 	*dsm_11_bit = (dsm_channel_shift == 11);
 
@@ -845,11 +841,18 @@ bool dsm_parse(const uint64_t now, const uint8_t *frame, const unsigned len, uin
 	}
 
 	if (decode_ret) {
-		*num_values = dsm_chan_count;
+		// require stable channel count (dsm_chan_count == dsm_chan_count_prev) before considering the decode valid
+		if ((dsm_chan_count > 0) && (dsm_chan_count <= DSM_MAX_CHANNEL_COUNT) && (dsm_chan_count == dsm_chan_count_prev)) {
+			*num_values = dsm_chan_count;
+			memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0]));
+
+		} else {
+			decode_ret = false;
+		}
+
+		dsm_chan_count_prev = dsm_chan_count;
 
-		memcpy(&values[0], &dsm_chan_buf[0], dsm_chan_count * sizeof(dsm_chan_buf[0]));
 #ifdef DSM_DEBUG
-
 		printf("PACKET ---------\n");
 		printf("frame drops: %u, chan #: %u\n", dsm_frame_drops, dsm_chan_count);
 

src/lib/rc/rc_tests/RCTest.cpp

@@ -231,17 +231,17 @@ bool RCTest::ghstTest()
 
 bool RCTest::dsmTest10Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 17, 1500);
 }
 
 bool RCTest::dsmTest16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 3, 1500);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_data.txt", 16, 6, 1500);
 }
 
 bool RCTest::dsmTest22msDSMX16Ch()
 {
-	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_px4_binding_data.txt", 16, 6, 1499);
+	return dsmTest(TEST_DATA_PATH "dsm_x_dx9_px4_binding_data.txt", 16, 11, 1499);
 }
 
 bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0)
@@ -290,7 +290,7 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 					&dsm_11_bit, &dsm_frame_drops, nullptr, max_channels);
 
 		if (result) {
-			if (count > (16 * 10)) { // need to process enough data to have full channel count
+			if (count > (16 * 20)) { // need to process enough data to have full channel count
 				ut_compare("num_values == expected_chancount", num_values, expected_chancount);
 			}
 
@@ -313,6 +313,8 @@ bool RCTest::dsmTest(const char *filepath, unsigned expected_chancount, unsigned
 
 	fclose(fp);
 
+	ut_compare("num_values == expected_chancount", num_values, expected_chancount);
+
 	ut_test(ret == EOF);
 	//PX4_INFO("drop: %d", (int)last_drop);
 	ut_compare("last_drop == expected_dropcount", last_drop, expected_dropcount);

src/modules/commander/Commander.cpp

@@ -3533,7 +3533,7 @@ void Commander::data_link_check()
 			_status.data_link_lost = true;
 			_status.data_link_lost_counter++;
 
-			mavlink_log_critical(&_mavlink_log_pub, "Connection to ground station lost");
+			mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost");
 
 			_status_changed = true;
 		}

src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp

@@ -96,7 +96,17 @@ static int parse_options(int argc, char *argv[])
 
 		case 'w': _options.sleep_ms        = strtoul(myoptarg, nullptr, 10);    break;
 
-		case 'b': _options.baudrate        = strtoul(myoptarg, nullptr, 10);    break;
+		case 'b': {
+				int baudrate = 0;
+
+				if (px4_get_parameter_value(myoptarg, baudrate) != 0) {
+					PX4_ERR("baudrate parsing failed");
+				}
+
+				_options.baudrate = baudrate;
+
+				break;
+			}
 
 		case 'r': _options.recv_port       = strtoul(myoptarg, nullptr, 10);    break;
 

src/modules/rc_update/rc_update.cpp

@@ -424,35 +424,39 @@ void RCUpdate::Run()
 		/* publish rc_channels topic even if signal is invalid, for debug */
 		_rc_channels_pub.publish(_rc);
 
-		/* only publish manual control if the signal is present */
-		if (input_source_stable && channel_count_stable && !signal_lost
-		    && (input_rc.timestamp_last_signal > _last_timestamp_signal)) {
+		// only publish manual control if the signal is present and regularly updating
+		if (input_source_stable && channel_count_stable && !signal_lost) {
 
-			_last_timestamp_signal = input_rc.timestamp_last_signal;
-			perf_count(_valid_data_interval_perf);
+			if ((input_rc.timestamp_last_signal > _last_timestamp_signal)
+			    && (input_rc.timestamp_last_signal - _last_timestamp_signal < 1_s)) {
 
-			// check if channels actually updated
-			bool rc_updated = false;
+				perf_count(_valid_data_interval_perf);
 
-			for (unsigned i = 0; i < channel_count_limited; i++) {
-				if (_rc_values_previous[i] != input_rc.values[i]) {
-					rc_updated = true;
-					break;
+				// check if channels actually updated
+				bool rc_updated = false;
+
+				for (unsigned i = 0; i < channel_count_limited; i++) {
+					if (_rc_values_previous[i] != input_rc.values[i]) {
+						rc_updated = true;
+						break;
+					}
+				}
+
+				// limit processing if there's no update
+				if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
+					UpdateManualSetpoint(input_rc.timestamp_last_signal);
+				}
+
+				UpdateManualSwitches(input_rc.timestamp_last_signal);
+
+				/* Update parameters from RC Channels (tuning with RC) if activated */
+				if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
+					set_params_from_rc();
+					_last_rc_to_param_map_time = hrt_absolute_time();
 				}
 			}
 
-			// limit processing if there's no update
-			if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
-				UpdateManualSetpoint(input_rc.timestamp_last_signal);
-			}
-
-			UpdateManualSwitches(input_rc.timestamp_last_signal);
-
-			/* Update parameters from RC Channels (tuning with RC) if activated */
-			if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
-				set_params_from_rc();
-				_last_rc_to_param_map_time = hrt_absolute_time();
-			}
+			_last_timestamp_signal = input_rc.timestamp_last_signal;
 		}
 
 		memcpy(_rc_values_previous, input_rc.values, sizeof(input_rc.values[0]) * channel_count_limited);