一、Navigator模块作用
Navigator模块是由一个navigator_main及多个任务模式（如loiter、mission）组成。navigator_main负责订阅topic、处理command及切换不同任务的基础功能。各个任务模式负责根据具体的任务类型生成_pos_sp_triplet这个topic并发布。
从名字来说，navigator是导航的意思，但是在px4中它跟导航有点区别。_pos_sp_triplet并不是最终的期望的位置点，比如对于mission模式中的多个任务点，_pos_sp_triplet只是当前的任务点，算是一个比较粗略的点，实际执行的期望位置是在flight_mode_manager中生成的。
本次只分析navigator_main的代码执行流程，具体的任务模式都是相对独立的模块，待以后详细分析
-
二、Navigator功能结构图
-
三、代码分析
-
Navigator::run() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME); _geofence.loadFromFile(GEOFENCE_FILENAME); } params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[2] {}; /* Setup of loop */ //订阅最重要的两个topic即local_pos本地位置信息和vehicle_status飞机当前的状态 fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; fds[1].fd = _vehicle_status_sub; fds[1].events = POLLIN; /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); hrt_abstime last_geofence_check = 0; while (!should_exit()) { /* wait for up to 1000ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_ERR("poll error %d, %d", pret, errno); px4_usleep(10000); continue; } else { if (fds[0].revents & POLLIN) { /* success, local pos is available */ orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } } perf_begin(_loop_perf); //拷贝无人机当前状态 orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus); //gps数据更新 /* gps updated */ if (_gps_pos_sub.updated()) { _gps_pos_sub.copy(&_gps_pos); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } //全局位置更新 /* global position updated */ if (_global_pos_sub.updated()) { _global_pos_sub.copy(&_global_pos); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } } //参数更新 // check for parameter updates if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); // update parameters from storage params_update(); } _land_detected_sub.update(&_land_detected); _position_controller_status_sub.update(); _home_pos_sub.update(&_home_pos); //处理和导航相关的命令 if (_vehicle_command_sub.updated()) { const unsigned last_generation = _vehicle_command_sub.get_last_generation(); vehicle_command_s cmd{}; _vehicle_command_sub.copy(&cmd); if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation()); } //处理DO_GO_AROUND命令 if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { // DO_GO_AROUND is currently handled by the position controller (unacknowledged) // TODO: move DO_GO_AROUND handling to navigator publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {//处理重新定位的指令 bool reposition_valid = true; if (have_geofence_position_data && ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) { if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { vehicle_global_position_s test_reposition_validity {}; test_reposition_validity.lat = cmd.param5; test_reposition_validity.lon = cmd.param6; if (PX4_ISFINITE(cmd.param7)) { test_reposition_validity.alt = cmd.param7; } else { test_reposition_validity.alt = get_global_position()->alt; } reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos, home_position_valid()); } } if (reposition_valid) { position_setpoint_triplet_s *rep = get_reposition_triplet(); position_setpoint_triplet_s *curr = get_position_setpoint_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_local_position()->heading; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // If no argument for ground speed, use default value. if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) { rep->current.cruising_speed = get_cruising_speed(); } else { rep->current.cruising_speed = cmd.param1; } rep->current.cruising_throttle = get_cruising_throttle(); rep->current.acceptance_radius = get_acceptance_radius(); // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; rep->current.yaw_valid = true; } else { rep->current.yaw = NAN; rep->current.yaw_valid = false; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { // Position change with optional altitude change rep->current.lat = cmd.param5; rep->current.lon = cmd.param6; if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } } else if (PX4_ISFINITE(cmd.param7)) { // Altitude without position change // This condition is necessary for altitude changes just after takeoff where lat and lon are still nan if (curr->current.valid && PX4_ISFINITE(curr->current.lat) && PX4_ISFINITE(curr->current.lon)) { rep->current.lat = curr->current.lat; rep->current.lon = curr->current.lon; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } rep->current.alt = cmd.param7; } else { // All three set to NaN - hold in current position rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->previous.timestamp = hrt_absolute_time(); rep->current.valid = true; rep->current.timestamp = hrt_absolute_time(); rep->next.valid = false; } else { mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence"); } // CMD_DO_REPOSITION is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {//处理起飞的指令 position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next //将当前的位置保存为takoff_triplet的前一个点的信息 rep->previous.yaw = get_local_position()->heading; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; //如果home的位置点有效，则根据命令中传的参数4(param4)设置yaw方向，否则将当前的yaw方向作为期望的yaw if (home_position_valid()) { rep->current.yaw = cmd.param4; rep->previous.valid = true; rep->previous.timestamp = hrt_absolute_time(); } else { rep->current.yaw = get_local_position()->heading; rep->previous.valid = false; } //通过参数传递的目的经纬度有效的话，则作为起飞的目标，否则用当前位置的经纬度作为起飞的目标经纬度 if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = cmd.param5; rep->current.lon = cmd.param6; } else { // If one of them is non-finite set the current global position as target rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } //期望的起飞的高度通过参数传递，但是后面的代码分析高度还是由很多其它的因素决定 rep->current.alt = cmd.param7; rep->current.valid = true; rep->current.timestamp = hrt_absolute_time(); rep->next.valid = false; // CMD_NAV_TAKEOFF is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {//处理开始降落的指令 /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ if (_mission.land_start()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = _mission.get_land_start_index(); publish_vehicle_cmd(&vcmd); } else { PX4_WARN("planned mission landing not available"); } publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {//处理启动任务的指令 if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { if (!_mission.set_current_mission_index(cmd.param1)) { PX4_WARN("CMD_MISSION_START failed"); } } // CMD_MISSION_START is acknowledged by commander } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {//处理改变速度的指令 if (cmd.param2 > FLT_EPSILON) { // XXX not differentiating ground and airspeed yet set_cruising_speed(cmd.param2); } else { set_cruising_speed(); /* if no speed target was given try to set throttle */ if (cmd.param3 > FLT_EPSILON) { set_cruising_throttle(cmd.param3 / 100); } else { set_cruising_throttle(); } } // TODO: handle responses for supported DO_CHANGE_SPEED options? publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) { _vroi = {}; switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI: case vehicle_command_s::VEHICLE_CMD_NAV_ROI: _vroi.mode = cmd.param1; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: _vroi.mode = vehicle_command_s::VEHICLE_ROI_LOCATION; _vroi.lat = cmd.param5; _vroi.lon = cmd.param6; _vroi.alt = cmd.param7; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: _vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT; _vroi.pitch_offset = (float)cmd.param5 * M_DEG_TO_RAD_F; _vroi.roll_offset = (float)cmd.param6 * M_DEG_TO_RAD_F; _vroi.yaw_offset = (float)cmd.param7 * M_DEG_TO_RAD_F; break; case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: _vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE; break; default: _vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE; break; } _vroi.timestamp = hrt_absolute_time(); _vehicle_roi_pub.publish(_vroi); publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } } /* Check for traffic */ check_traffic(); /* Check geofence violation */ if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.check(_global_pos, _gps_pos, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.timestamp = hrt_absolute_time(); _geofence_result.geofence_action = _geofence.getGeofenceAction(); _geofence_result.home_required = _geofence.isHomeRequired(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); /* If we are already in loiter it is very likely that we are doing a reposition * so we should block that by repositioning in the current location */ if (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN && get_vstatus()->nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { position_setpoint_triplet_s *rep = get_reposition_triplet(); rep->current.yaw = get_local_position()->heading; rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; rep->current.alt = get_global_position()->alt; rep->current.valid = true; _pos_sp_triplet_updated = true; } _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } _geofence_result_pub.publish(_geofence_result); } /* Do stuff according to navigation state set by commander */ NavigatorMode *navigation_mode_new{nullptr}; //根据飞机当前的导航状态，切换到不同的任务模式 switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); navigation_mode_new = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { _pos_sp_triplet_published_invalid_once = false; const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; switch (rtl_type()) { case RTL::RTL_LAND: // use mission landing case RTL::RTL_CLOSEST: if (rtl_activated) { if (rtl_type() == RTL::RTL_LAND) { mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated"); } else { mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated"); } } // if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly if (on_mission_landing() && !get_land_detected()->landed) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); navigation_mode_new = &_mission; } else { navigation_mode_new = &_rtl; } break; case RTL::RTL_MISSION: if (_mission.get_land_start_available() && !get_land_detected()->landed) { // the mission contains a landing spot _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); if (_navigation_mode != &_mission) { if (_navigation_mode == nullptr) { // switching from an manual mode, go to landing if not already landing if (!on_mission_landing()) { start_mission_landing(); } } else { // switching from an auto mode, continue the mission from the closest item _mission.set_closest_item_as_current(); } } if (rtl_activated) { mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission"); } navigation_mode_new = &_mission; } else { // fly the mission in reverse if switching from a non-manual mode _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE); if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) && (! _mission.get_mission_finished()) && (!get_land_detected()->landed)) { // determine the closest mission item if switching from a non-mission mode, and we are either not already // mission mode or the mission waypoints changed. // The seconds condition is required so that when no mission was uploaded and one is available the closest // mission item is determined and also that if the user changes the active mission index while rtl is active // always that waypoint is tracked first. if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) { _mission.set_closest_item_as_current(); } if (rtl_activated) { mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse"); } navigation_mode_new = &_mission; } else { if (rtl_activated) { mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home"); } navigation_mode_new = &_rtl; } } break; default: if (rtl_activated) { mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated"); } navigation_mode_new = &_rtl; break; } break; } case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_precland; _precland.set_mode(PrecLandMode::Required); break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_follow_target; break; 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_DESCEND: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: navigation_mode_new = nullptr; _can_loiter_at_sp = false; break; } // Do not execute any state machine while we are disarmed if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { navigation_mode_new = nullptr; } // update the vehicle status _previous_nav_state = _vstatus.nav_state; /* we have a new navigation mode: reset triplet */ if (_navigation_mode != navigation_mode_new) { // We don't reset the triplet if we just did an auto-takeoff and are now // going to loiter. Otherwise, we lose the takeoff altitude and end up lower // than where we wanted to go. // // FIXME: a better solution would be to add reset where they are needed and remove // this general reset here. if (!(_navigation_mode == &_takeoff && navigation_mode_new == &_loiter)) { reset_triplets(); } } _navigation_mode = navigation_mode_new; /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { if (_navigation_mode_array[i]) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; reset_triplets(); } if (_pos_sp_triplet_updated) { publish_position_setpoint_triplet(); } if (_mission_result_updated) { publish_mission_result(); } perf_end(_loop_perf); } }
963
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
