pixhawk commander--navigator--modules之间的联系

最新推荐文章于 2023-12-20 10:36:20 发布

虾米一代

最新推荐文章于 2023-12-20 10:36:20 发布

阅读量4.9k

点赞数 8

分类专栏：四轴飞行器

本文链接：https://blog.csdn.net/czyv587/article/details/52120876

版权

四轴飞行器专栏收录该内容

34 篇文章 31 订阅

订阅专栏

commander--navigator是决策部分，处理得到飞行模式和期望导航路线。此blog只是记下commander--navigator--modules之间的联系，从决策部分如何影响控制部分，进而完成任务，此blog不涉及详细的运行细节。吐舌头

1.commander.cpp中通过将遥控信息处理发布orb_publish(ORB_ID(vehicle_status), status_pub, &status);

处理过程参考pixhawk _control_mode如何产生的

2.navigator_main.cpp中

void
Navigator::vehicle_status_update()
{
	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
		/* in case the commander is not be running */
		_vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
	}
}

获取commander.cpp发布的ORB_ID(vehicle_status)主题

		/* Do stuff according to navigation state set by commander */
		switch (_vstatus.nav_state) {
			case vehicle_status_s::NAVIGATION_STATE_MANUAL:
			case vehicle_status_s::NAVIGATION_STATE_ACRO:
			case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
			case vehicle_status_s::NAVIGATION_STATE_POSCTL:
			case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
			case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
				if (_fw_pos_ctrl_status.abort_landing) {
					// pos controller aborted landing, requests loiter
					// above landing waypoint
					_navigation_mode = &_loiter;
					_pos_sp_triplet_published_invalid_once = false;
				} else {
					_pos_sp_triplet_published_invalid_once = false;
					_navigation_mode = &_mission;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_loiter;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_rcloss_act.get() == 1) {
					_navigation_mode = &_loiter;
				} else if (_param_rcloss_act.get() == 3) {
					_navigation_mode = &_land;
				} else if (_param_rcloss_act.get() == 4) {
					_navigation_mode = &_rcLoss;
				} else { /* if == 2 or unknown, RTL */
					_navigation_mode = &_rtl;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_rtl;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_takeoff;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_land;
				break;
			case vehicle_status_s::NAVIGATION_STATE_DESCEND:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_land;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
				/* Use complex data link loss mode only when enabled via param
				* otherwise use rtl */
				_pos_sp_triplet_published_invalid_once = false;
				if (_param_datalinkloss_act.get() == 1) {
					_navigation_mode = &_loiter;
				} else if (_param_datalinkloss_act.get() == 3) {
					_navigation_mode = &_land;
				} else if (_param_datalinkloss_act.get() == 4) {
					_navigation_mode = &_dataLinkLoss;
				} else { /* if == 2 or unknown, RTL */
					_navigation_mode = &_rtl;
				}
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_engineFailure;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_gpsFailure;
				break;
			case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
				_pos_sp_triplet_published_invalid_once = false;
				_navigation_mode = &_follow_target;
				break;
			default:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
		}

		/* iterate through navigation modes and set active/inactive for each */
		for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
		}

根据ORB_ID(vehicle_status)主题选择不同的导航模式

导航模式有10种

	/* Create a list of our possible navigation types */
	_navigation_mode_array[0] = &_mission;
	_navigation_mode_array[1] = &_loiter;
	_navigation_mode_array[2] = &_rtl;
	_navigation_mode_array[3] = &_dataLinkLoss;
	_navigation_mode_array[4] = &_engineFailure;
	_navigation_mode_array[5] = &_gpsFailure;
	_navigation_mode_array[6] = &_rcLoss;
	_navigation_mode_array[7] = &_takeoff;
	_navigation_mode_array[8] = &_land;
	_navigation_mode_array[9] = &_follow_target;

请注意

for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
	_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}

这里的run()函数里面是运行继承的函数，即on_activation();on_active();on_inactive();，因此会跳到相应的cpp文件中(因此navigator文件夹中的那么多cpp文件得以用上)

void
NavigatorMode::run(bool active)
{
	if (active) {
		if (_first_run) {
			/* first run */
			_first_run = false;
			/* Reset stay in failsafe flag */
			_navigator->get_mission_result()->stay_in_failsafe = false;
			_navigator->set_mission_result_updated();
			on_activation();
		} else {
			/* periodic updates when active */
			on_active();
		}
	} else {
		/* periodic updates when inactive */
		_first_run = true;
		on_inactive();
	}
}

那么以rtl.cpp(自动返航)为例，看看逻辑上如何完成这个自动返航这个任务的
由上程序可知，第一次运行on_activation();，以后就运行on_active();

void
RTL::on_activation()
{
	/* reset starting point so we override what the triplet contained from the previous navigation state */
	_rtl_start_lock = false;
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* check if we are pretty close to home already */
	float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

	/* decide where to enter the RTL procedure when we switch into it */
	if (_rtl_state == RTL_STATE_NONE) {
		/* for safety reasons don't go into RTL if landed */
		if (_navigator->get_land_detected()->landed) {
			_rtl_state = RTL_STATE_LANDED;
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

		/* if lower than return altitude, climb up first */
		} else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
			   + _param_return_alt.get()) {
			_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */
		} else {
			/* set altitude setpoint to current altitude */
			_rtl_state = RTL_STATE_RETURN;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_global_position()->alt;
		}

	}

	set_rtl_item();
}

void
RTL::on_active()
{
	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item();
	}
}

这两个函数都好理解，前面都是为了产生_rtl_state，以便set_rtl_item();调用

void
RTL::set_rtl_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	/* make sure we have the latest params */
	updateParams();
	if (!_rtl_start_lock) {
		set_previous_pos_setpoint();
	}
	_navigator->set_can_loiter_at_sp(false);
	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {
		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
		_mission_item.lat = _navigator->get_global_position()->lat;
		_mission_item.lon = _navigator->get_global_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = climb_alt;
		_mission_item.yaw = NAN;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;
		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
			(int)(climb_alt),
			(int)(climb_alt - _navigator->get_home_position()->alt));
		break;
	}
	case RTL_STATE_RETURN: {
		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		// don't change altitude

		// use home yaw if close to home
		/* check if we are pretty close to home already */
		float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
		if (home_dist < _param_rtl_min_dist.get()) {
			_mission_item.yaw = _navigator->get_home_position()->yaw;
		} else {
			if (pos_sp_triplet->previous.valid) {
				/* if previous setpoint is valid then use it to calculate heading to home */
				_mission_item.yaw = get_bearing_to_next_waypoint(
				        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
				        _mission_item.lat, _mission_item.lon);
			} else {
				/* else use current position */
				_mission_item.yaw = get_bearing_to_next_waypoint(
				        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
				        _mission_item.lat, _mission_item.lon);
			}
		}
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = true;
		_mission_item.origin = ORIGIN_ONBOARD;
		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
			(int)(_mission_item.altitude),
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		_rtl_start_lock = true;
		break;
	}
	case RTL_STATE_TRANSITION_TO_MC: {
		_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
		_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
		break;
	}
	case RTL_STATE_DESCEND: {
		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
		// check if we are already lower - then we will just stay there
		if (_mission_item.altitude > _navigator->get_global_position()->alt) {
			_mission_item.altitude = _navigator->get_global_position()->alt;
		}
		_mission_item.yaw = _navigator->get_home_position()->yaw;
		// except for vtol which might be still off here and should point towards this location
		float d_current = get_distance_to_next_waypoint(
			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
			_mission_item.lat, _mission_item.lon);

		if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
			_mission_item.yaw = get_bearing_to_next_waypoint(
				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
				_mission_item.lat, _mission_item.lon);
		}
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = 0.0f;
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = false;
		_mission_item.origin = ORIGIN_ONBOARD;
		/* disable previous setpoint to prevent drift */
		pos_sp_triplet->previous.valid = false;
		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
			(int)(_mission_item.altitude),
			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
		break;
	}
	case RTL_STATE_LOITER: {
		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

		_mission_item.lat = _navigator->get_home_position()->lat;
		_mission_item.lon = _navigator->get_home_position()->lon;
		// don't change altitude
		_mission_item.yaw = _navigator->get_home_position()->yaw;
		_mission_item.loiter_radius = _navigator->get_loiter_radius();
		_mission_item.loiter_direction = 1;
		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
		_mission_item.pitch_min = 0.0f;
		_mission_item.autocontinue = autoland;
		_mission_item.origin = ORIGIN_ONBOARD;
		_navigator->set_can_loiter_at_sp(true);
		if (autoland && (_mission_item.time_inside > FLT_EPSILON)) {
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
		} else {
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
		}
		break;
	}
	case RTL_STATE_LAND: {
		_mission_item.yaw = _navigator->get_home_position()->yaw;
		set_land_item(&_mission_item, false);

		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
		break;
	}
	case RTL_STATE_LANDED: {
		set_idle_item(&_mission_item);

		mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
		break;
	}
	default:
		break;
	}
	reset_mission_item_reached();
	/* execute command if set */
	if (!item_contains_position(&_mission_item)) {
		issue_command(&_mission_item);
	}
	/* convert mission item to current position setpoint and make it valid */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->next.valid = false;
	_navigator->set_position_setpoint_triplet_updated();
}

在set_rtl_item();中，前面是给_mission_item结构体赋值，mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);是将_mission_item结构体的值赋给pos_sp_triplet结构体

void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
	/* set the correct setpoint for vtol transition */

	if(item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
			&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
		waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
										   _navigator->get_global_position()->lon,
										   item->yaw,
										   1000000.0f,
										   &sp->lat,
										   &sp->lon);
		sp->alt = _navigator->get_global_position()->alt;
	}


	/* don't change the setpoint for non-position items */
	if (!item_contains_position(item)) {
		return;
	}

	sp->valid = true;
	sp->lat = item->lat;
	sp->lon = item->lon;
	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
	sp->yaw = item->yaw;
	sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius :
				_navigator->get_loiter_radius();
	sp->loiter_direction = item->loiter_direction;
	sp->pitch_min = item->pitch_min;
	sp->acceptance_radius = item->acceptance_radius;
	sp->disable_mc_yaw_control = false;
	sp->cruising_speed = _navigator->get_cruising_speed();

	switch (item->nav_cmd) {
	case NAV_CMD_IDLE:
		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
		break;

	case NAV_CMD_TAKEOFF:
	case NAV_CMD_VTOL_TAKEOFF:
		sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
		break;

	case NAV_CMD_LAND:
	case NAV_CMD_VTOL_LAND:
		sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){
			sp->disable_mc_yaw_control = true;
		}
		break;

	case NAV_CMD_LOITER_TIME_LIMIT:
	case NAV_CMD_LOITER_TURN_COUNT:
	case NAV_CMD_LOITER_UNLIMITED:
		sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
			sp->disable_mc_yaw_control = true;
		}
		break;

	default:
		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
		break;
	}
}

if (_pos_sp_triplet_updated) {
	publish_position_setpoint_triplet();
	_pos_sp_triplet_updated = false;
}

void
Navigator::publish_position_setpoint_triplet()
{
	/* update navigation state */
	_pos_sp_triplet.nav_state = _vstatus.nav_state;

	/* do not publish an empty triplet */
	if (!_pos_sp_triplet.current.valid) {
		return;
	}

	/* lazily publish the position setpoint triplet only once available */
	if (_pos_sp_triplet_pub != nullptr) {
		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);

	} else {
		_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
	}
}

发布ORB_ID(position_setpoint_triplet)进而用于位置控制mc_pos_control_main.cpp里面的control_auto()函数

void MulticopterPositionControl::control_auto(float dt)
{
	……
	if (updated) {
		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
		//Make sure that the position setpoint is valid
		if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
				!PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
				!PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
			_pos_sp_triplet.current.valid = false;
		}
	}
	……
}

接着分析mission.cpp

run()函数中第一次运行on_activation();，以后就运行on_active();

void
NavigatorMode::run(bool active)
{
	if (active) {
		if (_first_run) {
			/* first run */
			_first_run = false;
			/* Reset stay in failsafe flag */
			_navigator->get_mission_result()->stay_in_failsafe = false;
			_navigator->set_mission_result_updated();
			on_activation();

		} else {
			/* periodic updates when active */
			on_active();
		}

	} else {
		/* periodic updates when inactive */
		_first_run = true;
		on_inactive();
	}
}

void
Mission::on_activation()
{
	set_mission_items();
}

void
Mission::on_active()
{
	check_mission_valid();

	/* 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);
	if (offboard_updated) {
		update_offboard_mission();
	}
	/* reset the current offboard mission if needed */
	if (need_to_reset_mission(true)) {
		reset_offboard_mission(_offboard_mission);
		update_offboard_mission();
		offboard_updated = true;
	}
	/* reset mission items if needed */
	if (onboard_updated || offboard_updated) {
		set_mission_items();
	}
	/* lets check if we reached the current mission item */
	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
		set_mission_item_reached();
		if (_mission_item.autocontinue) {
			/* switch to next waypoint if 'autocontinue' flag set */
			advance_mission();
			set_mission_items();
		}
	} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
		altitude_sp_foh_update();
	} else {
		/* if waypoint position reached allow loiter on the setpoint */
		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
			_navigator->set_can_loiter_at_sp(true);
		}
	}
	/* see if we need to update the current yaw heading */
	if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
			&& _param_yawmode.get() < MISSION_YAWMODE_MAX
			&& _mission_type != MISSION_TYPE_NONE)
			|| _navigator->get_vstatus()->is_vtol) {
		heading_sp_update();
	}
}

这里都为调用set_mission_items();做准备

void
Mission::set_mission_items()
{
	/* make sure param is up to date */
	updateParams();

	/* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
	altitude_sp_foh_reset();

	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;

	/* mission item that comes after current if available */
	struct mission_item_s mission_item_next_position;
	bool has_next_position_item = false;

	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;

	/* copy information about the previous mission item */
	if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {
		/* Copy previous mission item altitude */
		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
	}

	/* 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_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");
			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 mission type changed, notify */
		if (_mission_type != MISSION_TYPE_OFFBOARD) {
			mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");
			user_feedback_done = true;
		}
		_mission_type = MISSION_TYPE_OFFBOARD;
	} else {
		/* no mission available or mission finished, switch to loiter */
		if (_mission_type != MISSION_TYPE_NONE) {
			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
			user_feedback_done = true;

			/* use last setpoint for loiter */
			_navigator->set_can_loiter_at_sp(true);

		}

		_mission_type = MISSION_TYPE_NONE;

		/* set loiter mission item and ensure that there is a minimum clearance from home */
		set_loiter_item(&_mission_item, _param_takeoff_alt.get());

		/* update position setpoint triplet  */
		pos_sp_triplet->previous.valid = false;
		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
		pos_sp_triplet->next.valid = false;

		/* reuse setpoint for LOITER only if it's not IDLE */
		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);

		set_mission_finished();

		if (!user_feedback_done) {
			/* only tell users that we got no mission if there has not been any
			 * better, more specific feedback yet
			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
			 */

			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 {
				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
			}

			user_feedback_done = true;

		}

		_navigator->set_position_setpoint_triplet_updated();
		return;
	}

	/*********************************** handle mission item *********************************************/

	/* handle position mission items */


	if (item_contains_position(&_mission_item)) {

		/* force vtol land */
		if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get()
				&& !_navigator->get_vstatus()->is_rotary_wing){
			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
		}

		/* we have a new position item so set previous position setpoint to current */
		set_previous_pos_setpoint();

		/* do takeoff before going to setpoint if needed and not already in takeoff */
		if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
			new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;

			/* use current mission item as next position item */
			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
			mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
			has_next_position_item = true;

			float takeoff_alt = calculate_takeoff_altitude(&_mission_item);

			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;
			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			/* ignore yaw for takeoff items */
			_mission_item.yaw = NAN;
			_mission_item.altitude = takeoff_alt;
			_mission_item.altitude_is_relative = false;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;
		}

		/* if we just did a takeoff navigate to the actual waypoint now */
		if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {

			if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
					&& _navigator->get_vstatus()->is_rotary_wing
					&& !_navigator->get_land_detected()->landed
					&& has_next_position_item) {
				/* check if the vtol_takeoff command is on top of us */
				if(do_need_move_to_takeoff()){
					new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
				} else {
					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;
			} else {
				new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
				/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
				_mission_item.yaw = NAN;
			}

		}

		/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
		if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
				&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {

			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0.0f;
		}

		/* move to land wp as fixed wing */
		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
				&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
				&& !_navigator->get_land_detected()->landed) {
			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
			/* use current mission item as next position item */
			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
			has_next_position_item = true;
			float altitude = _navigator->get_global_position()->alt;
			if (pos_sp_triplet->current.valid) {
				altitude = pos_sp_triplet->current.alt;
			}

			_mission_item.altitude = altitude;
			_mission_item.altitude_is_relative = false;
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;
		}

		/* transition to MC */
		if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
				&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
				&& !_navigator->get_vstatus()->is_rotary_wing
				&& !_navigator->get_land_detected()->landed) {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			_mission_item.autocontinue = true;
			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
		}

		/* move to landing waypoint before descent if necessary */
		if (do_need_move_to_land() &&
				(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
				 _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;

			/* use current mission item as next position item */
			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
			has_next_position_item = true;

			/*
			 * Ignoring waypoint altitude:
			 * Set altitude to the same as we have now to prevent descending too fast into
			 * the ground. Actual landing will descend anyway until it touches down.
			 * XXX: We might want to change that at some point if it is clear to the user
			 * what the altitude means on this waypoint type.
			 */
			float altitude = _navigator->get_global_position()->alt;

			_mission_item.altitude = altitude;
			_mission_item.altitude_is_relative = false;
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.autocontinue = true;
			_mission_item.time_inside = 0;
		}

		/* we just moved to the landing waypoint, now descend */
		if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
				&& _navigator->get_vstatus()->is_rotary_wing) {
			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 */
		if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND ) {
			_mission_item.yaw = NAN;
		}

	/* handle non-position mission items such as commands */
	} else {

		/* turn towards next waypoint before MC to FW transition */
		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
				&& _work_item_type != WORK_ITEM_TYPE_ALIGN
				&& _navigator->get_vstatus()->is_rotary_wing
				&& !_navigator->get_land_detected()->landed
				&& has_next_position_item) {

			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
			set_align_mission_item(&_mission_item, &mission_item_next_position);
		}

		/* yaw is aligned now */
		if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
		}

	}

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

	/* set current position setpoint from mission item (is protected agains non-position items) */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

	/* issue command if ready (will do nothing for position mission items) */
	issue_command(&_mission_item);

	/* set current work item type */
	_work_item_type = new_work_item_type;

	/* require takeoff after landing or idle */
	if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
		_need_takeoff = true;
	}

	_navigator->set_can_loiter_at_sp(false);
	reset_mission_item_reached();

	if (_mission_type == MISSION_TYPE_OFFBOARD) {
		set_current_offboard_mission_item();
	}
	// TODO: report onboard mission item somehow

	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
		/* try to process next mission item */

		if (has_next_position_item) {
			/* got next mission item, update setpoint triplet */
			mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
		} else {
			/* next mission item is not available */
			pos_sp_triplet->next.valid = false;
		}

	} else {
		/* vehicle will be paused on current waypoint, don't set next item */
		pos_sp_triplet->next.valid = false;
	}

	/* Save the distance between the current sp and the previous one */
	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
		_distance_current_previous = get_distance_to_next_waypoint(
				pos_sp_triplet->current.lat,
				pos_sp_triplet->current.lon,
				pos_sp_triplet->previous.lat,
				pos_sp_triplet->previous.lon);
	}

	_navigator->set_position_setpoint_triplet_updated();
}

好吧，这个函数有点复杂，但最终目的还是为了使用

mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);

将_mission_item结构体的值赋给pos_sp_triplet结构体
之后在navigator_main.cpp中调用

if (_pos_sp_triplet_updated) {
	publish_position_setpoint_triplet();
	_pos_sp_triplet_updated = false;
}

最后发布出去给各modules

如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!

虾米一代

关注

8
点赞
踩
17

收藏

觉得还不错? 一键收藏
1
评论
pixhawk commander--navigator--modules之间的联系

1.commander.cpp中通过将遥控信息处理发布orb_publish(ORB_ID(vehicle_status), status_pub, &status);处理过程参考pixhawk _control_mode如何产生的2.navigator_main.cpp中voidNavigator::vehicle_status_update(){ if (orb_co
复制链接

扫一扫

专栏目录