git status

src/modules/mavlink/mavlink_mission.cpp

@@ -52,10 +52,6 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
-#include <fcntl.h>
-#include <drivers/drv_pwm_output.h>
-
-
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -74,8 +70,6 @@ bool MavlinkMissionManager::_transfer_in_progress = false;
 						(_msg.target_component == MAV_COMP_ID_ALL)))
 
 
-int chan_id;
-
 MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
 	_time_last_recv(0),
@@ -94,8 +88,6 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
 	_mission_result_sub(-1),
 	_offboard_mission_pub(nullptr),
 	_slow_rate_limiter(_interval / 10.0f),
-	_actuators{},
-	_actuator_pub(nullptr),
 	_verbose(false)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -123,34 +115,6 @@ MavlinkMissionManager::get_size()
 	}
 }
 
-
-/**
-	 * Set the actuators
-	 */
-
-int
-MavlinkMissionManager::actuators_publish()
-{
-	_actuators.timestamp = hrt_absolute_time();
-
-	// lazily publish _actuators only once available
-	if (_actuator_pub == nullptr) {
-		_mavlink->send_statustext_critical("Actuators published 1!");
-		return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
-
-	} else {
-		_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
-		if (_actuator_pub == nullptr) {
-			_mavlink->send_statustext_critical("Actuators published 2!");
-			return OK;
-
-		} else {
-			return -1;
-		}
-	}
-}
-
-
 void
 MavlinkMissionManager::init_offboard_mission()
 {
@@ -833,29 +797,11 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		break;
 
 	case MAV_CMD_DO_SET_SERVO:
+
 		mission_item->actuator_num = mavlink_mission_item->param1;
 		mission_item->actuator_value = mavlink_mission_item->param2;
 		mission_item->autocontinue = true;
 		mission_item->time_inside=0.0f;
-
-		chan_id = mavlink_mission_item->param1;
-
-
-		_actuators.control[0] = mavlink_mission_item->param2;
-		_actuators.control[1] = mavlink_mission_item->param2;
-		_actuators.control[2] = mavlink_mission_item->param2;
-		_actuators.control[3] = mavlink_mission_item->param2;
-		_actuators.control[4] = mavlink_mission_item->param2;
-		_actuators.control[5] = mavlink_mission_item->param2;
-		_actuators.control[6] = mavlink_mission_item->param2;
-		_actuators.control[7] = mavlink_mission_item->param2;
-
-		actuators_publish();
-
-		/* for test purpose of function during mission upload */
-		//up_pwm_servo_arm(true);
-		//up_pwm_servo_set(mission_item->actuator_num-1, mission_item->actuator_value);
-
 		break;
 
 	default:

src/modules/mavlink/mavlink_mission.h

@@ -48,13 +48,6 @@
 #include "mavlink_rate_limiter.h"
 #include "mavlink_stream.h"
 
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_0.h>
-#include <uORB/topics/actuator_controls_1.h>
-#include <uORB/topics/actuator_controls_2.h>
-#include <uORB/topics/actuator_controls_3.h>
-
-
 enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_IDLE = 0,
 	MAVLINK_WPM_STATE_SENDLIST,
@@ -137,9 +130,6 @@ private:
 
 	MavlinkRateLimiter	_slow_rate_limiter;
 
-	actuator_controls_s _actuators;
-	orb_advert_t    _actuator_pub;
-
 	bool _verbose;
 
 	/* do not allow top copying this class */
@@ -205,12 +195,6 @@ private:
 	 */
 	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
 
-	/**
-	 * publish actuators state
-	 */
-	int actuators_publish();
-
-
 protected:
 	explicit MavlinkMissionManager(Mavlink *mavlink);
 

src/modules/navigator/mission_block.cpp

@@ -60,7 +60,9 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	_mission_item({0}),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0)
+	_time_first_inside_orbit(0),
+	_actuators{},
+	_actuator_pub(nullptr)
 {
 }
 
@@ -72,9 +74,11 @@ bool
 MissionBlock::is_mission_item_reached()
 {
 	if (_mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
-		//up_pwm_servo_arm(true);
-		//up_pwm_servo_set(_mission_item.actuator_num-1, _mission_item.actuator_value);
-		warnx("Set servo cmd executed");
+
+    	 _actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value;
+         _actuators.timestamp = hrt_absolute_time();
+         orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
+    	warnx("Set servo cmd executed");
 		return true;
 		}
 

src/modules/navigator/mission_block.h

@@ -46,6 +46,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/actuator_controls.h>
 
 #include "navigator_mode.h"
 
@@ -97,6 +98,10 @@ protected:
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
 	hrt_abstime _time_first_inside_orbit;
+
+	actuator_controls_s _actuators;
+	orb_advert_t    _actuator_pub;
+
 };
 
 #endif

src/modules/navigator/mission_feasibility_checker.cpp

@@ -303,9 +303,25 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 		for (unsigned i = 0; i < nMissionItems; i++) {
 			if (dm_read(dm_current, i,
 					&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
+				/* Check non navigation item */
+				if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){
 
+					//check actuator number
+			        if (mission_item.actuator_num<1 || mission_item.actuator_num>6) {
+			        	mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 1..6", (int)mission_item.actuator_num);
+			        	warning_issued = true;
+			        	return false;
+			        }
+					//check actuator value
+			        if (mission_item.actuator_value<900 || mission_item.actuator_value>2000) {
+			        	mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 900..2000", (int)mission_item.actuator_value);
+			        	warning_issued = true;
+			        	return false;
+			        }
+
+				}
 				/* check only items with valid lat/lon */
-				if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				else if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
 						mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 						mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 						mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||

src/modules/uORB/topics/mission.h

@@ -99,9 +99,8 @@ struct mission_item_s {
 	int do_jump_mission_index;	/**< index where the do jump will go to                 */
 	unsigned do_jump_repeat_count;	/**< how many times do jump needs to be done            */
 	unsigned do_jump_current_count;	/**< count how many times the jump has been done	*/
-	int actuator_num;               /**< actuator number to be set 1..6                     */
-	int actuator_value;             /**< new value for selected actuator 900...2000         */
-
+	int actuator_num;               /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6    */
+	int actuator_value;             /**< new value for selected actuator in ms 900...2000         */
 };
 
 /**