drivers/rc_input: don't accept RX_PAIR cmd when armed

src/drivers/rc_input/RCInput.cpp

@@ -259,19 +259,6 @@ void RCInput::rc_io_invert(bool invert)
 	}
 }
 
-void RCInput::answer_command(const vehicle_command_s &cmd, uint8_t result)
-{
-	/* publish ACK */
-	uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
-	vehicle_command_ack_s command_ack{};
-	command_ack.command = cmd.command;
-	command_ack.result = result;
-	command_ack.target_system = cmd.source_system;
-	command_ack.target_component = cmd.source_component;
-	command_ack.timestamp = hrt_absolute_time();
-	vehicle_command_ack_pub.publish(command_ack);
-}
-
 void RCInput::Run()
 {
 	if (should_exit()) {
@@ -301,6 +288,14 @@ void RCInput::Run()
 			updateParams();
 		}
 
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status;
+
+			if (_vehicle_status_sub.copy(&vehicle_status)) {
+				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			}
+		}
+
 		const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
 
@@ -309,12 +304,12 @@ void RCInput::Run()
 
 		if (_vehicle_cmd_sub.update(&vcmd)) {
 			// Check for a pairing command
-			if ((unsigned int)vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
+			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
 
 				uint8_t cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #if defined(SPEKTRUM_POWER)
 
-				if (!_rc_scan_locked /* !_armed.armed */) { // TODO: add armed check?
+				if (!_rc_scan_locked && !_armed) {
 					if ((int)vcmd.param1 == 0) {
 						// DSM binding command
 						int dsm_bind_mode = (int)vcmd.param2;
@@ -335,39 +330,55 @@ void RCInput::Run()
 
 						cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 					}
+
+				} else {
+					cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
-#endif /* SPEKTRUM_POWER */
-				answer_command(vcmd, cmd_ret);
+#endif // SPEKTRUM_POWER
+
+				// publish acknowledgement
+				vehicle_command_ack_s command_ack{};
+				command_ack.command = vcmd.command;
+				command_ack.result = cmd_ret;
+				command_ack.target_system = vcmd.source_system;
+				command_ack.target_component = vcmd.source_component;
+				command_ack.timestamp = hrt_absolute_time();
+				uORB::Publication<vehicle_command_ack_s> vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+				vehicle_command_ack_pub.publish(command_ack);
 			}
 		}
 
-		/* update ADC sampling */
-#ifdef ADC_RC_RSSI_CHANNEL
-		adc_report_s adc;
 
-		if (_adc_sub.update(&adc)) {
-			for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
-				if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
-					float adc_volt = adc.raw_data[i] *
-							 adc.v_ref /
-							 adc.resolution;
+#if defined(ADC_RC_RSSI_CHANNEL)
 
-					if (_analog_rc_rssi_volt < 0.0f) {
-						_analog_rc_rssi_volt = adc_volt;
-					}
+		// update ADC sampling
+		if (_adc_report_sub.updated()) {
+			adc_report_s adc;
 
-					_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
+			if (_adc_report_sub.copy(&adc)) {
+				for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) {
+					if (adc.channel_id[i] == ADC_RC_RSSI_CHANNEL) {
+						float adc_volt = adc.raw_data[i] *
+								 adc.v_ref /
+								 adc.resolution;
 
-					/* only allow this to be used if we see a high RSSI once */
-					if (_analog_rc_rssi_volt > 2.5f) {
-						_analog_rc_rssi_stable = true;
+						if (_analog_rc_rssi_volt < 0.0f) {
+							_analog_rc_rssi_volt = adc_volt;
+						}
+
+						_analog_rc_rssi_volt = _analog_rc_rssi_volt * 0.995f + adc_volt * 0.005f;
+
+						/* only allow this to be used if we see a high RSSI once */
+						if (_analog_rc_rssi_volt > 2.5f) {
+							_analog_rc_rssi_stable = true;
+						}
 					}
 				}
 			}
 		}
 
-#endif /* ADC_RC_RSSI_CHANNEL */
+#endif // ADC_RC_RSSI_CHANNEL
 
 		bool rc_updated = false;
 
@@ -375,9 +386,7 @@ void RCInput::Run()
 		// Scan for 300 msec, then switch protocol
 		constexpr hrt_abstime rc_scan_max = 300_ms;
 
-		bool sbus_failsafe, sbus_frame_drop;
 		unsigned frame_drops = 0;
-		bool dsm_11_bit;
 
 		if (_report_lock && _rc_scan_locked) {
 			_report_lock = false;
@@ -411,6 +420,9 @@ void RCInput::Run()
 
 				// parse new data
 				if (newBytes > 0) {
+					bool sbus_failsafe = false;
+					bool sbus_frame_drop = false;
+
 					rc_updated = sbus_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count, &sbus_failsafe,
 								&sbus_frame_drop, &frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS);
 
@@ -441,7 +453,8 @@ void RCInput::Run()
 				   || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
 
 				if (newBytes > 0) {
-					int8_t dsm_rssi;
+					int8_t dsm_rssi = 0;
+					bool dsm_11_bit = false;
 
 					// parse new data
 					rc_updated = dsm_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &_raw_rc_count,