Make DO_SET_SERVO work

src/modules/mavlink/mavlink_receiver.cpp

@@ -69,6 +69,8 @@
 #include "mavlink_main.h"
 #include "mavlink_receiver.h"
 
+#include <systemlib/mavlink_log.h>
+
 #ifdef CONFIG_NET
 #define MAVLINK_RECEIVER_NET_ADDED_STACK 1360
 #else
@@ -86,6 +88,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_parameters_manager(parent),
 	_mavlink_timesync(parent)
 {
+	// Subscribing for publishing DO_SET_SERVO correctly
+	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
 }
 
 void
@@ -489,6 +493,46 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_storage_information(cmd_mavlink.param1 + 0.5f);
 		}
 
+	} else if (cmd_mavlink.command == MAV_CMD_DO_SET_SERVO) {
+		actuator_controls_s actuators = {};
+		actuators.timestamp = hrt_absolute_time();
+
+		// Copying actual AUX values to change only one output
+		// and leave others as they are
+		orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3,
+			 &actuators);
+
+		int aux_num = static_cast<int>(vehicle_command.param1);
+
+		// Aux numbers corresponding pass.aux.mix mixer
+		switch (aux_num) {
+		case 1:
+			aux_num = 5;
+			break;
+
+		case 2:
+			aux_num = 6;
+			break;
+
+		case 3:
+			aux_num = 7;
+			break;
+
+		case 4:
+			aux_num = 4;
+			break;
+
+		default:
+			break;
+		}
+
+		// aux_num - actuator number to be set
+		// param2 - new value for selected actuator in ms 1000...2000
+		actuators.control[aux_num] = (vehicle_command.param2 - static_cast<float>(PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2) /
+					     (static_cast<float>(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+
+		_actuator_controls_pubs[3].publish(actuators);
+
 	} else {
 
 		send_ack = false;

src/modules/mavlink/mavlink_receiver.h

@@ -101,6 +101,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 
+#include <drivers/drv_pwm_output.h>
+
 class Mavlink;
 
 class MavlinkReceiver : public ModuleParams
@@ -303,4 +305,6 @@ class MavlinkReceiver : public ModuleParams
 	// Disallow copy construction and move assignment.
 	MavlinkReceiver(const MavlinkReceiver &) = delete;
 	MavlinkReceiver operator=(const MavlinkReceiver &) = delete;
+
+	int _t_actuator_controls_3;
 };

src/modules/navigator/mission_block.cpp

@@ -54,6 +54,8 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
+#include <drivers/drv_pwm_output.h>
+
 using matrix::wrap_pi;
 
 MissionBlock::MissionBlock(Navigator *navigator) :
@@ -67,6 +69,9 @@ MissionBlock::MissionBlock(Navigator *navigator) :
 	_mission_item.time_inside = 0.0f;
 	_mission_item.autocontinue = true;
 	_mission_item.origin = ORIGIN_ONBOARD;
+
+	// Subscribing to publish MAV_CMD_DO_SET_SERVO
+	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
 }
 
 bool
@@ -444,9 +449,39 @@ MissionBlock::issue_command(const mission_item_s &item)
 		actuator_controls_s actuators = {};
 		actuators.timestamp = hrt_absolute_time();
 
-		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
-		// params[1] new value for selected actuator in ms 900...2000
-		actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
+		// Copying actual AUX values to change only one output
+		// and leave others as they are
+		orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3,
+			 &actuators);
+
+		int aux_num = static_cast<int>(item.params[0]);
+
+		// Aux numbers corresponding pass.aux.mix mixer
+		switch (aux_num) {
+		case 1:
+			aux_num = 5;
+			break;
+
+		case 2:
+			aux_num = 6;
+			break;
+
+		case 3:
+			aux_num = 7;
+			break;
+
+		case 4:
+			aux_num = 4;
+			break;
+
+		default:
+			break;
+		}
+
+		// aux_num - actuator number to be set
+		// param2 - new value for selected actuator in ms 1000...2000
+		actuators.control[aux_num] = (item.params[1] - static_cast<float>(PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2) /
+					     (static_cast<float>(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
 
 		_actuator_pub.publish(actuators);
 

src/modules/navigator/mission_block.h

@@ -135,5 +135,6 @@ class MissionBlock : public NavigatorMode
 	hrt_abstime _action_start{0};
 	hrt_abstime _time_wp_reached{0};
 
-	uORB::Publication<actuator_controls_s>	_actuator_pub{ORB_ID(actuator_controls_2)};
+	uORB::Publication<actuator_controls_s>	_actuator_pub{ORB_ID(actuator_controls_3)};
+	int _t_actuator_controls_3;
 };

src/modules/rc_update/rc_update.cpp

@@ -38,6 +38,7 @@
  */
 
 #include "rc_update.h"
+#include <math.h>
 
 #include "parameters.h"
 
@@ -57,6 +58,9 @@ RCUpdate::RCUpdate() :
 	rc_parameter_map_poll(true /* forced */);
 
 	parameters_updated();
+
+	// Subscribing for not to jam other publishers (MAV_CMD_DO_SET_SERVO for example)
+	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
 }
 
 RCUpdate::~RCUpdate()
@@ -473,8 +477,38 @@ RCUpdate::Run()
 			actuator_group_3.control[6] = manual.aux2;
 			actuator_group_3.control[7] = manual.aux3;
 
-			/* publish actuator_controls_3 topic */
-			_actuator_group_3_pub.publish(actuator_group_3);
+			/* Allow publishing only if controls have changed since last post.
+			It is neccessary to make other sources (such as MAV_CMD_DO_SET_SERVO) be able to publish also */
+			bool publish_allowed = false;
+
+			actuator_controls_s current_actuator_controls_from_topic{};
+			actuator_controls_s actuator_control_from_rc{};
+
+			orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &current_actuator_controls_from_topic);
+
+			for (int i = 0; i < 8; i++) {
+				// first, store the actual input values
+				actuator_control_from_rc.control[i] = actuator_group_3.control[i];
+
+				if (fabsf(actuator_group_3.control[i] - _prev_manual_controls.control[i]) > _actuator_changed_epsilon)	{
+					// If value has been changed
+					publish_allowed = true;
+
+				} else {
+					// If value has not been changed, keep the one from topic (it is needed for not to override other publishers)
+					actuator_group_3.control[i] = current_actuator_controls_from_topic.control[i];
+				}
+			}
+
+			if (publish_allowed) {
+				/* publish actuator_controls_3 topic */
+				_actuator_group_3_pub.publish(actuator_group_3);
+
+				// Store current values for calculating changes on the next step
+				for (int i = 0; i < 8; i++) {
+					_prev_manual_controls.control[i] = actuator_control_from_rc.control[i];
+				}
+			}
 
 			/* Update parameters from RC Channels (tuning with RC) if activated */
 			if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) {

src/modules/rc_update/rc_update.h

@@ -138,6 +138,8 @@ class RCUpdate : public ModuleBase<RCUpdate>, public ModuleParams, public px4::W
 
 	uORB::Publication<rc_channels_s>	_rc_pub{ORB_ID(rc_channels)};				/**< raw r/c control topic */
 	uORB::Publication<actuator_controls_s>	_actuator_group_3_pub{ORB_ID(actuator_controls_3)};	/**< manual control as actuator topic */
+	actuator_controls_s _prev_manual_controls; /**< previusly read manual controls */
+	float _actuator_changed_epsilon = 0.005;
 
 	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH};	/**< manual control signal topic */
 
@@ -153,6 +155,7 @@ class RCUpdate : public ModuleBase<RCUpdate>, public ModuleParams, public px4::W
 	math::LowPassFilter2p _filter_yaw;
 	math::LowPassFilter2p _filter_throttle;
 
+	int _t_actuator_controls_3;
 };
 
 

src/modules/vmount/output_rc.cpp

@@ -41,16 +41,22 @@
 #include "output_rc.h"
 
 #include <uORB/topics/actuator_controls.h>
-#include <px4_platform_common/defines.h>
+#include <px4_defines.h>
+#include <systemlib/mavlink_log.h>
 
+#include <math.h>
+
+orb_advert_t		_mav_pub_rc;	///< mavlink log pub
 
 namespace vmount
 {
 
 OutputRC::OutputRC(const OutputConfig &output_config)
 	: OutputBase(output_config)
 {
+	_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
 }
+
 OutputRC::~OutputRC()
 {
 	if (_actuator_controls_pub) {
@@ -66,22 +72,62 @@ int OutputRC::update(const ControlData *control_data)
 		_set_angle_setpoints(control_data);
 	}
 
+	bool changed = false;
+	orb_check(_t_actuator_controls_2, &changed);
+
+	if (changed) {
+		orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2,
+			 &_current_actuator_controls_from_topic);
+	}
+
 	_handle_position_update();
 
 	hrt_abstime t = hrt_absolute_time();
 	_calculate_output_angles(t);
 
-	actuator_controls_s actuator_controls;
-	actuator_controls.timestamp = hrt_absolute_time();
+	actuator_controls_s actuator_controls_from_inputs;
+	actuator_controls_from_inputs.timestamp = hrt_absolute_time();
 	// _angle_outputs are in radians, actuator_controls are in [-1, 1]
-	actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset) * _config.roll_scale;
-	actuator_controls.control[1] = (_angle_outputs[1] + _config.pitch_offset) * _config.pitch_scale;
-	actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale;
-	actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value;
+	actuator_controls_from_inputs.control[0] = (_angle_outputs[0]
+			+ _config.roll_offset) * _config.roll_scale;
+	actuator_controls_from_inputs.control[1] = (_angle_outputs[1]
+			+ _config.pitch_offset) * _config.pitch_scale;
+	actuator_controls_from_inputs.control[2] = (_angle_outputs[2]
+			+ _config.yaw_offset) * _config.yaw_scale;
+	actuator_controls_from_inputs.control[3] = _retract_gimbal ?
+			_config.gimbal_retracted_mode_value
+			: _config.gimbal_normal_mode_value;
+
+	bool publish_allowed = false;
+	actuator_controls_s actuator_controls_to_publish;
+	actuator_controls_to_publish.timestamp = hrt_absolute_time();
+
+	// Publishing only if something really changed
+	for (int i = 0; i < 4; i++) {
+		if (fabsf(actuator_controls_from_inputs.control[i]
+			  - _prev_inputed_actuator_controls.control[i])
+		    > _actuator_changed_epsilon) {
+			publish_allowed = true;
+			actuator_controls_to_publish.control[i] =
+				actuator_controls_from_inputs.control[i];
+
+		} else {
+			// If something changed we allow to publish changed values
+			// and leave others as they are currently in topic.
+			// If nothing changed we publish nothing.
+			actuator_controls_to_publish.control[i]
+				= _current_actuator_controls_from_topic.control[i];
+		}
+	}
 
 	int instance;
-	orb_publish_auto(ORB_ID(actuator_controls_2), &_actuator_controls_pub, &actuator_controls,
-			 &instance, ORB_PRIO_DEFAULT);
+
+	if (publish_allowed) {
+		orb_publish_auto(ORB_ID(actuator_controls_2),
+				 &_actuator_controls_pub, &actuator_controls_to_publish,
+				 &instance, ORB_PRIO_DEFAULT);
+		_prev_inputed_actuator_controls = actuator_controls_from_inputs;
+	}
 
 	_last_update = t;
 

src/modules/vmount/output_rc.h

@@ -42,7 +42,9 @@
 #include "output.h"
 
 #include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
 
+float abs_float(float val);
 
 namespace vmount
 {
@@ -63,8 +65,12 @@ class OutputRC : public OutputBase
 	virtual void print_status();
 
 private:
+	int _t_actuator_controls_2;	///< actuator controls group 2 topic
 	orb_advert_t _actuator_controls_pub = nullptr;
 	bool _retract_gimbal = true;
+	actuator_controls_s _prev_inputed_actuator_controls;
+	actuator_controls_s _current_actuator_controls_from_topic;
+	float _actuator_changed_epsilon = 0.005;
 };