commander--navigator是決策部分，處理得到飛行模式和期望導(dǎo)航路線(xiàn)。此blog只是記下commander--navigator--modules之間的聯(lián)系，從決策部分如何影響控制部分，進(jìn)而完成任務(wù)，此blog不涉及詳細(xì)的運(yùn)行細(xì)節(jié)。

1.commander.cpp中通過(guò)將遙控信息處理發(fā)布o(jì)rb_publish(ORB_ID(vehicle_status), status_pub, &status);

處理過(guò)程參考pixhawk _control_mode如何產(chǎn)生的

2.navigator_main.cpp中

[cpp] view plaincopy

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發(fā)布的ORB_ID(vehicle_status)主題 [cpp] view plaincopy

/*?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]);??

}??

根據(jù)ORB_ID(vehicle_status)主題選擇不同的導(dǎo)航模式

導(dǎo)航模式有10種

[cpp] view plaincopy

/*?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;??

請(qǐng)注意 [cpp] view plaincopy

for?(unsigned?int?i?=?0;?i?<?NAVIGATOR_MODE_ARRAY_SIZE;?i++)?{??

????_navigation_mode_array[i]->run(_navigation_mode?==?_navigation_mode_array[i]);??

}??

這里的run()函數(shù)里面是運(yùn)行繼承的函數(shù)，即on_activation();on_active();on_inactive();，因此會(huì)跳到相應(yīng)的cpp文件中(因此navigator文件夾中的那么多cpp文件得以用上) [cpp] view plaincopy

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(自動(dòng)返航)為例，看看邏輯上如何完成這個(gè)自動(dòng)返航這個(gè)任務(wù)的
由上程序可知，第一次運(yùn)行on_activation();，以后就運(yùn)行on_active(); [cpp] view plaincopy

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();??

}??

[cpp] view plaincopy

void??

RTL::on_active()??

{??

????if?(_rtl_state?!=?RTL_STATE_LANDED?&&?is_mission_item_reached())?{??

????????advance_rtl();??

????????set_rtl_item();??

????}??

}??

這兩個(gè)函數(shù)都好理解，前面都是為了產(chǎn)生_rtl_state，以便set_rtl_item();調(diào)用 [cpp] view plaincopy

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結(jié)構(gòu)體賦值，mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);是將_mission_item結(jié)構(gòu)體的值賦給pos_sp_triplet結(jié)構(gòu)體
[cpp] view plaincopy

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;??

????}??

}??

[cpp] view plaincopy

if?(_pos_sp_triplet_updated)?{??

????publish_position_setpoint_triplet();??

????_pos_sp_triplet_updated?=?false;??

}??

[cpp] view plaincopy

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);??

????}??

}??

發(fā)布ORB_ID(position_setpoint_triplet)進(jìn)而用于位置控制mc_pos_control_main.cpp里面的control_auto()函數(shù)

[cpp] view plaincopy

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()函數(shù)中第一次運(yùn)行on_activation();，以后就運(yùn)行on_active();

[cpp] view plaincopy

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();??

????}??

}??

[cpp] view plaincopy

void??

Mission::on_activation()??

{??

????set_mission_items();??

}??

[cpp] view plaincopy

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();??

????}??

}??

這里都為調(diào)用set_mission_items();做準(zhǔn)備 [cpp] view plaincopy

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();??

}??

好吧，這個(gè)函數(shù)有點(diǎn)復(fù)雜，但最終目的還是為了使用

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結(jié)構(gòu)體的值賦給pos_sp_triplet結(jié)構(gòu)體
之后在navigator_main.cpp中調(diào)用

[cpp] view plaincopy

if?(_pos_sp_triplet_updated)?{??

????publish_position_setpoint_triplet();??

????_pos_sp_triplet_updated?=?false;??

}??

最后發(fā)布出去給各modules

總結(jié)

以上是生活随笔為你收集整理的pixhawk commander--navigator--modules之间的联系的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。