Mission merge offboard + onboard and simplify

2026-07-17 11:20:35 +08:00 · 2017-11-14 16:36:11 -05:00
parent 956935141e
commit 916d6a15fd
14 changed files with 181 additions and 271 deletions
@@ -60,7 +60,6 @@

 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_dist_between_wps(this, "MIS_DIST_WPS", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
@@ -82,14 +81,6 @@ Mission::on_inactive()
 	}

 	if (_inited) {
-		/* check if missions have changed so that feedback to ground station is given */
-		bool onboard_updated = false;
-		orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
-
-		if (onboard_updated) {
-			update_onboard_mission();
-		}
-
 		bool offboard_updated = false;
 		orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);

@@ -175,13 +166,6 @@ Mission::on_active()
 	check_mission_valid(false);

 	/* check if anything has changed */
-	bool onboard_updated = false;
-	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
-
-	if (onboard_updated) {
-		update_onboard_mission();
-	}
-
 	bool offboard_updated = false;
 	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);

@@ -198,7 +182,7 @@ Mission::on_active()
 	}

 	/* reset mission items if needed */
-	if (onboard_updated || offboard_updated) {
+	if (offboard_updated) {
 		set_mission_items();
 	}

@@ -282,7 +266,7 @@ Mission::find_offboard_land_start()
 	 * TODO: implement full spec and find closest landing point geographically
 	 */

-	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+	const dm_item_t dm_current = (dm_item_t)_offboard_mission.dataman_id;

 	for (size_t i = 0; i < _offboard_mission.count; i++) {
 		struct mission_item_s missionitem = {};
@@ -334,53 +318,6 @@ Mission::landing()
 	return mission_valid && on_landing_stage;
 }

-void
-Mission::update_onboard_mission()
-{
-	/* reset triplets */
-	_navigator->reset_triplets();
-
-	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
-		/* 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 {
-			/* if less WPs available, reset to first WP */
-			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
-				_current_onboard_mission_index = 0;
-				/* if not initialized, set it to 0 */
-
-			} else if (_current_onboard_mission_index < 0) {
-				_current_onboard_mission_index = 0;
-			}
-
-			/* otherwise, just leave it */
-		}
-
-		// XXX check validity here as well
-		_navigator->get_mission_result()->valid = true;
-		/* reset mission failure if we have an updated valid mission */
-		_navigator->get_mission_result()->failure = false;
-
-		/* reset sequence info as well */
-		_navigator->get_mission_result()->seq_reached = -1;
-		_navigator->get_mission_result()->seq_total = _onboard_mission.count;
-
-		/* reset work item if new mission has been accepted */
-		_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-		_navigator->increment_mission_instance_count();
-		_navigator->set_mission_result_updated();
-
-	} else {
-		_onboard_mission.count = 0;
-		_onboard_mission.current_seq = 0;
-		_current_onboard_mission_index = 0;
-	}
-}
-
 void
 Mission::update_offboard_mission()
 {
@@ -389,11 +326,7 @@ Mission::update_offboard_mission()
 	/* reset triplets */
 	_navigator->reset_triplets();

-	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
-		// The following is not really a warning, but it can be useful to have this message in the log file
-		PX4_WARN("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id,
-			 _offboard_mission.count, _offboard_mission.current_seq);
-
+	if (orb_copy(ORB_ID(mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 		/* determine current index */
 		if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
 			_current_offboard_mission_index = _offboard_mission.current_seq;
@@ -455,10 +388,6 @@ Mission::advance_mission()
 	}

 	switch (_mission_type) {
-	case MISSION_TYPE_ONBOARD:
-		_current_onboard_mission_index++;
-		break;
-
 	case MISSION_TYPE_OFFBOARD:
 		_current_offboard_mission_index++;
 		break;
@@ -486,8 +415,6 @@ Mission::set_mission_items()
 	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
 	_min_current_sp_distance_xy = FLT_MAX;

-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
 	/* the home dist check provides user feedback, so we initialize it to this */
 	bool user_feedback_done = false;

@@ -497,21 +424,7 @@ Mission::set_mission_items()

 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;

-	/* try setting onboard mission item */
-	if (_param_onboard_enabled.get()
-	    && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
-
-		/* if mission type changed, notify */
-		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission.");
-			user_feedback_done = true;
-		}
-
-		_mission_type = MISSION_TYPE_ONBOARD;
-
-		/* try setting offboard mission item */
-
-	} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
+	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission.");
@@ -544,6 +457,7 @@ Mission::set_mission_items()
 		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());

 		/* update position setpoint triplet  */
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->previous.valid = false;
 		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -588,6 +502,7 @@ Mission::set_mission_items()
 		}

 		/* we have a new position item so set previous position setpoint to current */
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->previous = pos_sp_triplet->current;

 		/* do takeoff before going to setpoint if needed and not already in takeoff */
@@ -710,8 +625,7 @@ Mission::set_mission_items()

 			float altitude = _navigator->get_global_position()->alt;

-			if (pos_sp_triplet->current.valid
-			    && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 				altitude = pos_sp_triplet->current.alt;
 			}

@@ -785,6 +699,7 @@ Mission::set_mission_items()
 		}

 	} else {
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		/* handle non-position mission items such as commands */

 		/* turn towards next waypoint before MC to FW transition */
@@ -842,6 +757,8 @@ Mission::set_mission_items()

 	/*********************************** set setpoints and check next *********************************************/

+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 	/* set current position setpoint from mission item (is protected against non-position items) */
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -987,10 +904,8 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss
 	mission_item->autocontinue = true;
 	mission_item->time_inside = 0.0f;
 	mission_item->yaw = get_bearing_to_next_waypoint(
-				    _navigator->get_global_position()->lat,
-				    _navigator->get_global_position()->lon,
-				    mission_item_next->lat,
-				    mission_item_next->lon);
+				    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				    mission_item_next->lat, mission_item_next->lon);
 	mission_item->force_heading = true;
 }

@@ -1048,8 +963,7 @@ Mission::heading_sp_update()
 		point_from_latlon[1] = _navigator->get_global_position()->lon;

 		/* target location is home */
-		if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
-		     || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
+		if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
 		    // need to be rotary wing for this but not in a transition
 		    // in VTOL mode this will prevent updating yaw during FW flight
 		    // (which would result in a wrong yaw setpoint spike during back transition)
@@ -1196,9 +1110,9 @@ 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 min_climbout = _navigator->get_global_position()->alt + 20.0f;
-	float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), min_climbout);
+	const float alt_landing = get_absolute_altitude_for_item(_mission_item);
+	const float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(),
+				       _navigator->get_global_position()->alt + 20.0f);

 	// turn current landing waypoint into an indefinite loiter
 	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
@@ -1209,12 +1123,11 @@ Mission::do_abort_landing()
 	_mission_item.autocontinue = false;
 	_mission_item.origin = ORIGIN_ONBOARD;

-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	mission_apply_limitation(_mission_item);
-	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+	mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
 	_navigator->set_position_setpoint_triplet_updated();

-	mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
+	mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.",
 				     (int)(alt_sp - alt_landing));

 	// reset mission index to start of landing
@@ -1240,18 +1153,18 @@ Mission::do_abort_landing()
 }

 bool
-Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
-			       struct mission_item_s *next_position_mission_item, bool *has_next_position_item)
+Mission::prepare_mission_items(mission_item_s *mission_item, mission_item_s *next_position_mission_item,
+			       bool *has_next_position_item)
 {
 	bool first_res = false;
 	int offset = 1;

-	if (read_mission_item(onboard, 0, mission_item)) {
+	if (read_mission_item(0, mission_item)) {

 		first_res = true;

 		/* trying to find next position mission item */
-		while (read_mission_item(onboard, offset, next_position_mission_item)) {
+		while (read_mission_item(offset, next_position_mission_item)) {

 			if (item_contains_position(*next_position_mission_item)) {
 				*has_next_position_item = true;
@@ -1266,32 +1179,21 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
 }

 bool
-Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item)
+Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 {
-	/* select onboard/offboard mission */
-	int *mission_index_ptr;
-	dm_item_t dm_item;
-
-	struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
-	int current_index = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
+	/* select offboard mission */
+	struct mission_s *mission = &_offboard_mission;
+	int current_index = _current_offboard_mission_index;
 	int index_to_read = current_index + offset;

+	int *mission_index_ptr = (offset == 0) ? &_current_offboard_mission_index : &index_to_read;
+	const dm_item_t dm_item = (dm_item_t)_offboard_mission.dataman_id;
+
 	/* do not work on empty missions */
 	if (mission->count == 0) {
 		return false;
 	}

-	if (onboard) {
-		/* onboard mission */
-		mission_index_ptr = (offset == 0) ? &_current_onboard_mission_index : &index_to_read;
-		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
-
-	} else {
-		/* offboard mission */
-		mission_index_ptr = (offset == 0) ? &_current_offboard_mission_index : &index_to_read;
-		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
-	}
-
 	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
 	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
 	for (int i = 0; i < 10; i++) {
@@ -1300,8 +1202,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 			/* mission item index out of bounds - if they are equal, we just reached the end */
 			if (*mission_index_ptr != (int)mission->count) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item index out of bound, index: %d, max: %d.",
-						     *mission_index_ptr,
-						     (int)mission->count);
+						     *mission_index_ptr, mission->count);
 			}

 			return false;
@@ -1331,10 +1232,8 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 					(mission_item_tmp.do_jump_current_count)++;

 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
-						     &mission_item_tmp, len) != len) {
-						/* not supposed to happen unless the datamanager can't access the
-						 * dataman */
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the dataman */
 						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.");
 						return false;
 					}
@@ -1376,7 +1275,7 @@ Mission::save_offboard_mission_state()
 	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);

 	if (dm_lock_ret != 0) {
-		PX4_ERR("lock failed");
+		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
 	}

 	/* read current state */
@@ -1387,16 +1286,19 @@ Mission::save_offboard_mission_state()
 		if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
 			/* navigator may modify only sequence, write modified state only if it changed */
 			if (mission_state.current_seq != _current_offboard_mission_index) {
+				mission_state.timestamp = hrt_absolute_time();
+
 				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state,
 					     sizeof(mission_s)) != sizeof(mission_s)) {

-					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Can't save mission state.");
+					PX4_ERR("Can't save mission state");
 				}
 			}
 		}

 	} else {
 		/* invalid data, this must not happen and indicates error in offboard_mission publisher */
+		mission_state.timestamp = hrt_absolute_time();
 		mission_state.dataman_id = _offboard_mission.dataman_id;
 		mission_state.count = _offboard_mission.count;
 		mission_state.current_seq = _current_offboard_mission_index;
@@ -1407,7 +1309,7 @@ Mission::save_offboard_mission_state()
 		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state,
 			     sizeof(mission_s)) != sizeof(mission_s)) {

-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Can't save mission state.");
+			PX4_ERR("Can't save mission state");
 		}
 	}

@@ -1424,6 +1326,7 @@ Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
 	_navigator->get_mission_result()->item_do_jump_changed = true;
 	_navigator->get_mission_result()->item_changed_index = index;
 	_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+
 	_navigator->set_mission_result_updated();
 }

@@ -1432,6 +1335,7 @@ Mission::set_mission_item_reached()
 {
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->set_mission_result_updated();
+
 	reset_mission_item_reached();
 }

@@ -1440,6 +1344,7 @@ Mission::set_current_offboard_mission_item()
 {
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
+
 	_navigator->set_mission_result_updated();

 	save_offboard_mission_state();
@@ -1474,13 +1379,13 @@ Mission::reset_offboard_mission(struct mission_s &mission)
 	dm_lock(DM_KEY_MISSION_STATE);

 	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
-		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+		if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
 			/* set current item to 0 */
 			mission.current_seq = 0;

 			/* reset jump counters */
 			if (mission.count > 0) {
-				dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
+				const dm_item_t dm_current = (dm_item_t)mission.dataman_id;

 				for (unsigned index = 0; index < mission.count; index++) {
 					struct mission_item_s item;
@@ -1506,7 +1411,8 @@ Mission::reset_offboard_mission(struct mission_s &mission)
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.");

 			/* initialize mission state in dataman */
-			mission.dataman_id = 0;
+			mission.timestamp = hrt_absolute_time();
+			mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
 			mission.count = 0;
 			mission.current_seq = 0;
 		}