simple_offboard: correctly check manual control timeout, separate it from kill switch check

clover/src/simple_offboard.cpp

@@ -73,6 +73,7 @@ ros::Duration state_timeout;
 ros::Duration velocity_timeout;
 ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
+ros::Duration manual_control_timeout;
 float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
@@ -488,16 +489,19 @@ void publishSetpoint(const ros::TimerEvent& event)
 	publish(event.current_real);
 }
 
-inline void checkKillSwitch()
+inline void checkManualControl()
 {
-	if (!TIMEOUT(manual_control, state_timeout))
+	if (!manual_control_timeout.isZero() && TIMEOUT(manual_control, manual_control_timeout)) {
-		throw std::runtime_error("Manual control timeout, can't check kill switch status");
+		throw std::runtime_error("Manual control timeout, RC is switched off?");
+	}
 
-	const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
+	if (check_kill_switch) {
-	bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT);
+		const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
+		bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT);
 
-	if (kill_switch)
+		if (kill_switch)
-		throw std::runtime_error("Kill switch is on");
+			throw std::runtime_error("Kill switch is on");
+	}
 }
 
 inline void checkState()
@@ -527,8 +531,8 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// Checks
 		checkState();
 
-		if (auto_arm && check_kill_switch) {
+		if (auto_arm) {
-			checkKillSwitch();
+			checkManualControl();
 		}
 
 		// default frame is local frame
@@ -862,6 +866,7 @@ int main(int argc, char **argv)
 	velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
 	global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
 	battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
+	manual_control_timeout = ros::Duration(nh_priv.param("manual_control_timeout", 0.0));
 
 	transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
 	telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));