ManualControl: use input validity flag to check for RC calibration

PX4 · May 8, 2023 · aa728ef · aa728ef
1 parent 0e8f8e5
commit aa728ef
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 2 deletions.
diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp
@@ -83,7 +83,7 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
 					  (input.data_source == _first_valid_source
 					   || _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
 
-	return sample_from_the_past && sample_newer_than_timeout
+	return sample_from_the_past && sample_newer_than_timeout && input.valid
 	       && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
 }
 

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2055,6 +2055,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f;
 	manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id();
 	manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time();
+	manual_control_setpoint.valid = true;
 	_manual_control_input_pub.publish(manual_control_setpoint);
 }
 

diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp
@@ -127,7 +127,6 @@ void RCUpdate::parameters_updated()
 {
 	// rc values
 	for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
-
 		float min = 0.f;
 		param_get(_parameter_handles.min[i], &min);
 		_parameters.min[i] = min;
@@ -155,6 +154,12 @@ void RCUpdate::parameters_updated()
 
 	update_rc_functions();
 
+	_rc_calibrated = _param_rc_chan_cnt.get() > 0
+			 && (_param_rc_map_throttle.get() > 0
+			     || _param_rc_map_roll.get() > 0
+			     || _param_rc_map_pitch.get() > 0
+			     || _param_rc_map_yaw.get() > 0);
+
 	// deprecated parameters, will be removed post v1.12 once QGC is updated
 	{
 		int32_t rc_map_value = 0;
@@ -687,6 +692,7 @@ void RCUpdate::UpdateManualControlInput(const hrt_abstime &timestamp_sample)
 	manual_control_input.aux4  = get_rc_value(rc_channels_s::FUNCTION_AUX_4,   -1.f, 1.f);
 	manual_control_input.aux5  = get_rc_value(rc_channels_s::FUNCTION_AUX_5,   -1.f, 1.f);
 	manual_control_input.aux6  = get_rc_value(rc_channels_s::FUNCTION_AUX_6,   -1.f, 1.f);
+	manual_control_input.valid = _rc_calibrated;
 
 	// publish manual_control_input topic
 	manual_control_input.timestamp = hrt_absolute_time();

diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h
@@ -170,6 +170,7 @@ class RCUpdate : public ModuleBase<RCUpdate>, public ModuleParams, public px4::W
 	manual_control_switches_s _manual_switches_previous{};
 	manual_control_switches_s _manual_switches_last_publish{};
 	rc_channels_s _rc{};
+	bool _rc_calibrated{false};
 
 	rc_parameter_map_s _rc_parameter_map {};
 	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {};	/**< parameter values for RC control */