implement MAV_CMD_MISSION_START

src/modules/navigator/mission.cpp

@@ -249,6 +249,27 @@ Mission::on_active()
 
 }
 
+bool Mission::set_current_offboard_mission_index(unsigned index)
+{
+	if (index < _offboard_mission.count) {
+
+		_current_offboard_mission_index = index;
+		set_current_offboard_mission_item();
+
+		// update mission items if already in active mission
+		if (_navigator->is_planned_mission()) {
+			// prevent following "previous - current" line
+			_navigator->get_position_setpoint_triplet()->previous.valid = false;
+			_navigator->get_position_setpoint_triplet()->current.valid = false;
+			set_mission_items();
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
 void
 Mission::update_onboard_mission()
 {
@@ -256,6 +277,7 @@ Mission::update_onboard_mission()
 		/* accept the current index set by the onboard mission if it is within bounds */
 		if (_onboard_mission.current_seq >= 0
 		    && _onboard_mission.current_seq < (int)_onboard_mission.count) {
+
 			_current_onboard_mission_index = _onboard_mission.current_seq;
 
 		} else {
@@ -279,6 +301,7 @@ Mission::update_onboard_mission()
 		/* reset reached info as well */
 		_navigator->get_mission_result()->reached = false;
 		_navigator->get_mission_result()->seq_reached = 0;
+		_navigator->get_mission_result()->seq_total = _onboard_mission.count;
 
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
@@ -308,9 +331,8 @@ Mission::update_offboard_mission()
 			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
 				_current_offboard_mission_index = 0;
 
-				/* if not initialized, set it to 0 */
-
 			} else if (_current_offboard_mission_index < 0) {
+				/* if not initialized, set it to 0 */
 				_current_offboard_mission_index = 0;
 			}
 
@@ -328,6 +350,7 @@ Mission::update_offboard_mission()
 			/* reset reached info as well */
 			_navigator->get_mission_result()->reached = false;
 			_navigator->get_mission_result()->seq_reached = 0;
+			_navigator->get_mission_result()->seq_total = _offboard_mission.count;
 		}
 
 	} else {
@@ -464,7 +487,6 @@ Mission::set_mission_items()
 
 			if (_navigator->get_land_detected()->landed) {
 				/* landed, refusing to take off without a mission */
-
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
 
 			} else {
@@ -472,7 +494,6 @@ Mission::set_mission_items()
 			}
 
 			user_feedback_done = true;
-
 		}
 
 		_navigator->set_position_setpoint_triplet_updated();
@@ -482,8 +503,6 @@ Mission::set_mission_items()
 	/*********************************** handle mission item *********************************************/
 
 	/* handle position mission items */
-
-
 	if (item_contains_position(&_mission_item)) {
 
 		/* force vtol land */
@@ -508,7 +527,7 @@ Mission::set_mission_items()
 
 			float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home",
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home",
 					 (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
@@ -538,7 +557,6 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 				}
 
-
 				_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 				_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
 				_mission_item.yaw = _navigator->get_global_position()->yaw;
@@ -550,7 +568,6 @@ Mission::set_mission_items()
 				_mission_item.yaw = NAN;
 				_mission_item.time_inside = 0.0f;
 			}
-
 		}
 
 		/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
@@ -632,8 +649,6 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
-
-
 		/* ignore yaw for landing items */
 		/* XXX: if specified heading for landing is desired we could add another step before the descent
 		 * that aligns the vehicle first */
@@ -641,9 +656,8 @@ Mission::set_mission_items()
 			_mission_item.yaw = NAN;
 		}
 
-		/* handle non-position mission items such as commands */
-
 	} else {
+		/* handle non-position mission items such as commands */
 
 		/* turn towards next waypoint before MC to FW transition */
 		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
@@ -661,7 +675,6 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			set_previous_pos_setpoint();
 		}
-
 	}
 
 	/*********************************** set setpoints and check next *********************************************/
@@ -849,7 +862,7 @@ Mission::heading_sp_update()
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 	/* Don't change setpoint if last and current waypoint are not valid */
-	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid) {
+	if (!pos_sp_triplet->current.valid) {
 		return;
 	}
 
@@ -880,9 +893,8 @@ Mission::heading_sp_update()
 			point_to_latlon[0] = _navigator->get_home_position()->lat;
 			point_to_latlon[1] = _navigator->get_home_position()->lon;
 
-			/* target location is next (current) waypoint */
-
 		} else {
+			/* target location is next (current) waypoint */
 			point_to_latlon[0] = pos_sp_triplet->current.lat;
 			point_to_latlon[1] = pos_sp_triplet->current.lon;
 		}
@@ -966,10 +978,10 @@ Mission::altitude_sp_foh_update()
 
 	} else {
 		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
-		* of the mission item as it is used to check if the mission item is reached
-		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
-		* radius around the current waypoint
-		**/
+		 * of the mission item as it is used to check if the mission item is reached
+		 * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
+		 * radius around the current waypoint
+		 **/
 		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
 		float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
 						   _mission_item.acceptance_radius));
@@ -1000,8 +1012,8 @@ Mission::do_abort_landing()
 	// loiter at the larger of MIS_LTRMIN_ALT above the landing point
 	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
 	float alt_landing = get_absolute_altitude_for_item(_mission_item);
-	float alt_sp =  math::max(alt_landing + _param_loiter_min_alt.get(),
-				  _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
+	float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(),
+				 _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
 
 	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 	_mission_item.altitude_is_relative = false;
@@ -1018,8 +1030,7 @@ Mission::do_abort_landing()
 
 	_navigator->set_position_setpoint_triplet_updated();
 
-	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing)",
-			 (int)(alt_sp - alt_landing));
+	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing)", (int)(alt_sp - alt_landing));
 
 	// move mission index back 1 (landing approach point) so that re-entering
 	//  the mission doesn't try to land from the loiter above land
@@ -1113,7 +1124,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) {
 
 				/* only raise the repeat count if this is for the current mission item
-				* but not for the read ahead mission item */
+				 * but not for the read ahead mission item */
 				if (offset == 0) {
 					(mission_item_tmp.do_jump_current_count)++;
 
@@ -1130,7 +1141,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 				}
 
 				/* set new mission item index and repeat
-				* we don't have to validate here, if it's invalid, we should realize this later .*/
+				 * we don't have to validate here, if it's invalid, we should realize this later .*/
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
@@ -1259,6 +1270,7 @@ Mission::check_mission_valid(bool force)
 				_navigator->get_default_acceptance_radius(),
 				_navigator->get_land_detected()->landed);
 
+		_navigator->get_mission_result()->seq_total = _offboard_mission.count;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
 		_home_inited = _navigator->home_position_valid();

src/modules/navigator/mission.h

@@ -91,6 +91,8 @@ public:
 		MISSION_YAWMODE_BACK_TO_HOME = 3,
 		MISSION_YAWMODE_MAX = 4
 	};
+	
+	bool set_current_offboard_mission_index(unsigned index);
 
 private:
 	/**
@@ -108,12 +110,6 @@ private:
 	 */
 	void advance_mission();
 
-	/**
-	 * Check distance to first waypoint (with lat/lon)
-	 * @return true only if it's not too far from home (< MIS_DIST_1WP)
-	 */
-	bool check_dist_1wp();
-
 	/**
 	 * Set new mission items
 	 */
@@ -214,7 +210,7 @@ private:
 	void set_mission_finished();
 
 	/**
-	 * Check wether a mission is ready to go
+	 * Check whether a mission is ready to go
 	 */
 	void check_mission_valid(bool force);
 
@@ -258,7 +254,6 @@ private:
 
 	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
 
-	float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */
 	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
 					    only use if current and previous are valid */
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -182,7 +182,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
 	float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
 {
-	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
+	/* Check if all waypoints are above the home altitude, only return false if bool throw_error = true */
 	for (size_t i = 0; i < nMissionItems; i++) {
 		struct mission_item_s missionitem;
 		const ssize_t len = sizeof(struct mission_item_s);

src/modules/navigator/navigator_main.cpp

@@ -492,6 +492,14 @@ Navigator::task_main()
 				rep->current.valid = true;
 				rep->next.valid = false;
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
+
+				if (get_mission_result()->valid &&
+					PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
+
+					_mission.set_current_offboard_mission_index(cmd.param1);
+				}
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
 				PX4_INFO("got pause/continue command");
 			}
@@ -502,6 +510,7 @@ Navigator::task_main()
 		if (have_geofence_position_data &&
 			(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 			(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
+
 			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid());
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
@@ -727,8 +736,6 @@ Navigator::get_altitude_acceptance_radius()
 	}
 }
 
-
-
 float
 Navigator::get_cruising_speed()
 {
@@ -863,6 +870,7 @@ int navigator_main(int argc, char *argv[])
 void
 Navigator::publish_mission_result()
 {
+	_mission_result.timestamp = hrt_absolute_time();
 	_mission_result.instance_count = _mission_instance_count;
 
 	/* lazily publish the mission result only once available */

src/modules/navigator/navigator_mode.h

@@ -74,7 +74,7 @@ public:
 	virtual void on_inactive();
 
 	/**
-	 * This function is called one time when mode become active, poos_sp_triplet must be initialized here
+	 * This function is called one time when mode become active, pos_sp_triplet must be initialized here
 	 */
 	virtual void on_activation();