actuator: add support for MAV_CMD_DO_SET_ACTUATOR

Adds support for using the MAVLink command MAV_CMD_DO_SET_ACTUATOR to
update the actuator values on control group 3 aux{1, 2, 3}. A simple
deconfliction with rc_update is implemented: when a MAVLink command is
sent, RC is disabled for that channel until a major stick movement is
detected.

src/modules/mavlink/mavlink_receiver.cpp

@@ -501,8 +501,39 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_ack = true;
 		}
 
-	} else {
+	} else if (cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) {
+		// since we're only paying attention to 3 AUX outputs, the
+		// index should be 0, otherwise ignore the message
+		if (((int) vehicle_command.param7) == 0) {
+			actuator_controls_s actuator_controls{};
+			actuator_controls.timestamp = hrt_absolute_time();
 
+			// update with existing values to avoid changing unspecified controls
+			_actuator_controls_3_sub.update(&actuator_controls);
+
+			bool updated = false;
+
+			if (PX4_ISFINITE(vehicle_command.param1)) {
+				actuator_controls.control[5] = vehicle_command.param1;
+				updated = true;
+			}
+
+			if (PX4_ISFINITE(vehicle_command.param2)) {
+				actuator_controls.control[6] = vehicle_command.param2;
+				updated = true;
+			}
+
+			if (PX4_ISFINITE(vehicle_command.param3)) {
+				actuator_controls.control[7] = vehicle_command.param3;
+				updated = true;
+			}
+
+			if (updated) {
+				_actuator_controls_pubs[3].publish(actuator_controls);
+			}
+		}
+
+	} else {
 		send_ack = false;
 
 		if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {

src/modules/mavlink/mavlink_receiver.h

@@ -411,6 +411,7 @@ private:
 	uORB::Subscription	_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription 	_actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 

src/modules/rc_update/rc_update.cpp

@@ -602,16 +602,47 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime &timestamp_sample)
 	_last_manual_control_setpoint_publish = manual_control_setpoint.timestamp;
 
 
-	// populate and publish actuator_controls_3 copied from mapped manual_control_setpoint
 	actuator_controls_s actuator_group_3{};
+	// copy in previous actuator control setpoint in case aux{1, 2, 3} isn't changed
+	_actuator_controls_3_sub.update(&actuator_group_3);
+	// populate and publish actuator_controls_3 copied from mapped manual_control_setpoint
 	actuator_group_3.control[0] = manual_control_setpoint.y;
 	actuator_group_3.control[1] = manual_control_setpoint.x;
 	actuator_group_3.control[2] = manual_control_setpoint.r;
 	actuator_group_3.control[3] = manual_control_setpoint.z;
 	actuator_group_3.control[4] = manual_control_setpoint.flaps;
-	actuator_group_3.control[5] = manual_control_setpoint.aux1;
-	actuator_group_3.control[6] = manual_control_setpoint.aux2;
-	actuator_group_3.control[7] = manual_control_setpoint.aux3;
+
+	float new_aux_values[3];
+	new_aux_values[0] = manual_control_setpoint.aux1;
+	new_aux_values[1] = manual_control_setpoint.aux2;
+	new_aux_values[2] = manual_control_setpoint.aux3;
+
+	// if AUX RC was already active, we update. otherwise, we check
+	// if there is a major stick movement to re-activate RC mode
+	bool major_movement[3] = {false, false, false};
+
+	// detect a big stick movement
+	for (int i = 0; i < 3; i++) {
+		if (fabsf(_last_manual_control_setpoint[i] - new_aux_values[i]) > 0.1f) {
+			major_movement[i] = true;
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		// if someone else (DO_SET_ACTUATOR) updated the actuator control
+		// and we haven't had a major movement, switch back to automatic control
+		if ((fabsf(_last_manual_control_setpoint[i] - actuator_group_3.control[5 + i])
+		     > 0.0001f) && (!major_movement[i])) {
+			_aux_already_active[i] = false;
+		}
+
+		if (_aux_already_active[i] || major_movement[i]) {
+			_aux_already_active[i] = true;
+			_last_manual_control_setpoint[i] = new_aux_values[i];
+
+			actuator_group_3.control[5 + i] = new_aux_values[i];
+		}
+	}
 
 	actuator_group_3.timestamp = hrt_absolute_time();
 	_actuator_group_3_pub.publish(actuator_group_3);

src/modules/rc_update/rc_update.h

@@ -161,6 +161,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)};
+	uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
 
 	uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)};
 	uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
@@ -179,6 +180,8 @@ private:
 	hrt_abstime _last_timestamp_signal{0};
 
 	uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {};
+	float _last_manual_control_setpoint[3] {};
+	bool _aux_already_active[3] = {false, false, false};
 
 	uint8_t _channel_count_previous{0};
 	uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN};