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

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中

voidNavigator::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文件得以用上)

voidNavigatorMode::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();

voidRTL::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();}

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

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

voidRTL::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 thereif (_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 locationfloat 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结构体

voidMissionBlock::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;}

voidNavigator::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 validif (!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();

voidNavigatorMode::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();}}

voidMission::on_activation(){set_mission_items();}

voidMission::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();做准备

voidMission::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 somehowif (_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