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中
[cpp] view plaincopy
void??Navigator::vehicle_status_update()??{??????if?(orb_copy(ORB_ID(vehicle_status),?_vstatus_sub,?&_vstatus)?!=?OK)?{????????????????????_vstatus.arming_state?=?vehicle_status_s::ARMING_STATE_STANDBY;??????}??}??
獲取commander.cpp發布的ORB_ID(vehicle_status)主題
[cpp] view plaincopy
??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)?{??????????????????????????????????????????_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?{???????????????_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:?????????????????????_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?{???????????????_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;??}??????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種
[cpp] view plaincopy
??_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;??
請注意
[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()函數里面是運行繼承的函數,即on_activation();on_active();on_inactive();,因此會跳到相應的cpp文件中(因此navigator文件夾中的那么多cpp文件得以用上)
[cpp] view plaincopy
void??NavigatorMode::run(bool?active)??{??????if?(active)?{??????????if?(_first_run)?{????????????????????????????_first_run?=?false;????????????????????????????_navigator->get_mission_result()->stay_in_failsafe?=?false;??????????????_navigator->set_mission_result_updated();??????????????on_activation();??????????}?else?{????????????????????????????on_active();??????????}??????}?else?{????????????????????_first_run?=?true;??????????on_inactive();??????}??}??
那么以rtl.cpp(自動返航)為例,看看邏輯上如何完成這個自動返航這個任務的
由上程序可知,第一次運行on_activation();,以后就運行on_active();
[cpp] view plaincopy
void??RTL::on_activation()??{????????????_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);??????????????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?(_rtl_state?==?RTL_STATE_NONE)?{????????????????????if?(_navigator->get_land_detected()->landed)?{??????????????_rtl_state?=?RTL_STATE_LANDED;??????????????mavlink_log_critical(_navigator->get_mavlink_log_pub(),?"Already?landed,?not?executing?RTL");??????????????????????}?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;??????????????????????}?else?{????????????????????????????_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();??????}??}??
這兩個函數都好理解,前面都是為了產生_rtl_state,以便set_rtl_item();調用
[cpp] view plaincopy
void??RTL::set_rtl_item()??{??????struct?position_setpoint_triplet_s?*pos_sp_triplet?=?_navigator->get_position_setpoint_triplet();????????????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;??????????????????????????????????????????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)?{????????????????????????????????????_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?{????????????????????????????????????_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();????????????????????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;????????????????????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;????????????????????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;????????????????????_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();????????????if?(!item_contains_position(&_mission_item))?{??????????issue_command(&_mission_item);??????}????????????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結構體
[cpp] view plaincopy
void??MissionBlock::mission_item_to_position_setpoint(const?struct?mission_item_s?*item,?struct?position_setpoint_s?*sp)??{??????????????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;??????}????????????????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()??{????????????_pos_sp_triplet.nav_state?=?_vstatus.nav_state;??????????????if?(!_pos_sp_triplet.current.valid)?{??????????return;??????}??????????????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()函數
[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);????????????????????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();
[cpp] view plaincopy
void??NavigatorMode::run(bool?active)??{??????if?(active)?{??????????if?(_first_run)?{????????????????????????????_first_run?=?false;????????????????????????????_navigator->get_mission_result()->stay_in_failsafe?=?false;??????????????_navigator->set_mission_result_updated();??????????????on_activation();????????????}?else?{????????????????????????????on_active();??????????}????????}?else?{????????????????????_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();??????????????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();??????}????????????if?(need_to_reset_mission(true))?{??????????reset_offboard_mission(_offboard_mission);??????????update_offboard_mission();??????????offboard_updated?=?true;??????}????????????if?(onboard_updated?||?offboard_updated)?{??????????set_mission_items();??????}????????????if?(_mission_type?!=?MISSION_TYPE_NONE?&&?is_mission_item_reached())?{??????????set_mission_item_reached();??????????if?(_mission_item.autocontinue)?{????????????????????????????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?&&?_mission_item.nav_cmd?!=?NAV_CMD_IDLE)?{??????????????_navigator->set_can_loiter_at_sp(true);??????????}??????}????????????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();做準備
[cpp] view plaincopy
void??Mission::set_mission_items()??{????????????updateParams();??????????????altitude_sp_foh_reset();????????struct?position_setpoint_triplet_s?*pos_sp_triplet?=?_navigator->get_position_setpoint_triplet();??????????????bool?user_feedback_done?=?false;??????????????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;??????????????if?(item_contains_position(&_mission_item)?&&?pos_sp_triplet->current.valid)?{????????????????????_mission_item_previous_alt?=?get_absolute_altitude_for_item(_mission_item);??????}??????????????if?(_param_onboard_enabled.get()?&&?prepare_mission_items(true,?&_mission_item,?&mission_item_next_position,?&has_next_position_item))?{????????????????????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;??????????????}?else?if?(prepare_mission_items(false,?&_mission_item,?&mission_item_next_position,?&has_next_position_item))?{????????????????????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?{????????????????????if?(_mission_type?!=?MISSION_TYPE_NONE)?{????????????????????????????mavlink_log_critical(_navigator->get_mavlink_log_pub(),?"mission?finished,?loitering");??????????????user_feedback_done?=?true;??????????????????????????????_navigator->set_can_loiter_at_sp(true);????????????}????????????_mission_type?=?MISSION_TYPE_NONE;??????????????????????set_loiter_item(&_mission_item,?_param_takeoff_alt.get());??????????????????????pos_sp_triplet->previous.valid?=?false;??????????mission_item_to_position_setpoint(&_mission_item,?&pos_sp_triplet->current);??????????pos_sp_triplet->next.valid?=?false;??????????????????????_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type?==?position_setpoint_s::SETPOINT_TYPE_LOITER);????????????set_mission_finished();????????????if?(!user_feedback_done)?{?????????????????????????????????if?(_navigator->get_land_detected()->landed)?{??????????????????????????????????????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;??????}??????????????????????????if?(item_contains_position(&_mission_item))?{??????????????????????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;??????????}??????????????????????set_previous_pos_setpoint();??????????????????????if?(do_need_takeoff()?&&?_work_item_type?!=?WORK_ITEM_TYPE_TAKEOFF)?{??????????????new_work_item_type?=?WORK_ITEM_TYPE_TAKEOFF;??????????????????????????????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;????????????????????????????_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?(_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)?{????????????????????????????????????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;????????????????????????????????????_mission_item.yaw?=?NAN;??????????????}????????????}??????????????????????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;??????????}??????????????????????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;????????????????????????????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;??????????}??????????????????????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;??????????}??????????????????????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;??????????????????????????????memcpy(&mission_item_next_position,?&_mission_item,?sizeof(struct?mission_item_s));??????????????has_next_position_item?=?true;????????????????????????????????????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;??????????}??????????????????????if?(_work_item_type?==?WORK_ITEM_TYPE_MOVE_TO_LAND??????????????????&&?_navigator->get_vstatus()->is_rotary_wing)?{??????????????new_work_item_type?=?WORK_ITEM_TYPE_DEFAULT;??????????}?????????????????????????????????????if?(_mission_item.nav_cmd?==?NAV_CMD_LAND?||?_mission_item.nav_cmd?==?NAV_CMD_VTOL_LAND?)?{??????????????_mission_item.yaw?=?NAN;??????????}??????????????}?else?{??????????????????????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);??????????}??????????????????????if?(_work_item_type?==?WORK_ITEM_TYPE_ALIGN)?{??????????????new_work_item_type?=?WORK_ITEM_TYPE_DEFAULT;??????????}????????}??????????????????????mission_item_to_position_setpoint(&_mission_item,?&pos_sp_triplet->current);??????????????issue_command(&_mission_item);??????????????_work_item_type?=?new_work_item_type;??????????????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();??????}??????????????if?(_mission_item.autocontinue?&&?_mission_item.time_inside?<=?0.001f)?{??????????????????????if?(has_next_position_item)?{????????????????????????????mission_item_to_position_setpoint(&mission_item_next_position,?&pos_sp_triplet->next);??????????}?else?{????????????????????????????pos_sp_triplet->next.valid?=?false;??????????}????????}?else?{????????????????????pos_sp_triplet->next.valid?=?false;??????}??????????????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中調用
[cpp] view plaincopy
if?(_pos_sp_triplet_updated)?{??????publish_position_setpoint_triplet();??????_pos_sp_triplet_updated?=?false;??}??
最后發布出去給各modules
總結
以上是生活随笔為你收集整理的pixhawk commander--navigator--modules之间的联系的全部內容,希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網站內容還不錯,歡迎將生活随笔推薦給好友。