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
  1. void
  2. Navigator::vehicle_status_update()
  3. {
  4. if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
  5. /* in case the commander is not be running */
  6. _vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
  7. }
  8. }

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

[cpp] view plaincopy
  1. /* Do stuff according to navigation state set by commander */
  2. switch (_vstatus.nav_state) {
  3. case vehicle_status_s::NAVIGATION_STATE_MANUAL:
  4. case vehicle_status_s::NAVIGATION_STATE_ACRO:
  5. case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
  6. case vehicle_status_s::NAVIGATION_STATE_POSCTL:
  7. case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
  8. case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
  9. _navigation_mode = nullptr;
  10. _can_loiter_at_sp = false;
  11. break;
  12. case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
  13. if (_fw_pos_ctrl_status.abort_landing) {
  14. // pos controller aborted landing, requests loiter
  15. // above landing waypoint
  16. _navigation_mode = &_loiter;
  17. _pos_sp_triplet_published_invalid_once = false;
  18. } else {
  19. _pos_sp_triplet_published_invalid_once = false;
  20. _navigation_mode = &_mission;
  21. }
  22. break;
  23. case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
  24. _pos_sp_triplet_published_invalid_once = false;
  25. _navigation_mode = &_loiter;
  26. break;
  27. case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
  28. _pos_sp_triplet_published_invalid_once = false;
  29. if (_param_rcloss_act.get() == 1) {
  30. _navigation_mode = &_loiter;
  31. } else if (_param_rcloss_act.get() == 3) {
  32. _navigation_mode = &_land;
  33. } else if (_param_rcloss_act.get() == 4) {
  34. _navigation_mode = &_rcLoss;
  35. } else { /* if == 2 or unknown, RTL */
  36. _navigation_mode = &_rtl;
  37. }
  38. break;
  39. case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
  40. _pos_sp_triplet_published_invalid_once = false;
  41. _navigation_mode = &_rtl;
  42. break;
  43. case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
  44. _pos_sp_triplet_published_invalid_once = false;
  45. _navigation_mode = &_takeoff;
  46. break;
  47. case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
  48. _pos_sp_triplet_published_invalid_once = false;
  49. _navigation_mode = &_land;
  50. break;
  51. case vehicle_status_s::NAVIGATION_STATE_DESCEND:
  52. _pos_sp_triplet_published_invalid_once = false;
  53. _navigation_mode = &_land;
  54. break;
  55. case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
  56. /* Use complex data link loss mode only when enabled via param
  57. * otherwise use rtl */
  58. _pos_sp_triplet_published_invalid_once = false;
  59. if (_param_datalinkloss_act.get() == 1) {
  60. _navigation_mode = &_loiter;
  61. } else if (_param_datalinkloss_act.get() == 3) {
  62. _navigation_mode = &_land;
  63. } else if (_param_datalinkloss_act.get() == 4) {
  64. _navigation_mode = &_dataLinkLoss;
  65. } else { /* if == 2 or unknown, RTL */
  66. _navigation_mode = &_rtl;
  67. }
  68. break;
  69. case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
  70. _pos_sp_triplet_published_invalid_once = false;
  71. _navigation_mode = &_engineFailure;
  72. break;
  73. case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
  74. _pos_sp_triplet_published_invalid_once = false;
  75. _navigation_mode = &_gpsFailure;
  76. break;
  77. case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
  78. _pos_sp_triplet_published_invalid_once = false;
  79. _navigation_mode = &_follow_target;
  80. break;
  81. default:
  82. _navigation_mode = nullptr;
  83. _can_loiter_at_sp = false;
  84. break;
  85. }
  86. /* iterate through navigation modes and set active/inactive for each */
  87. for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
  88. _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
  89. }

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

导航模式有10种

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

请注意

[cpp] view plaincopy
  1. for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
  2. _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
  3. }

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

[cpp] view plaincopy
  1. void
  2. NavigatorMode::run(bool active)
  3. {
  4. if (active) {
  5. if (_first_run) {
  6. /* first run */
  7. _first_run = false;
  8. /* Reset stay in failsafe flag */
  9. _navigator->get_mission_result()->stay_in_failsafe = false;
  10. _navigator->set_mission_result_updated();
  11. on_activation();
  12. } else {
  13. /* periodic updates when active */
  14. on_active();
  15. }
  16. } else {
  17. /* periodic updates when inactive */
  18. _first_run = true;
  19. on_inactive();
  20. }
  21. }

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

[cpp] view plaincopy
  1. void
  2. RTL::on_activation()
  3. {
  4. /* reset starting point so we override what the triplet contained from the previous navigation state */
  5. _rtl_start_lock = false;
  6. set_current_position_item(&_mission_item);
  7. struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
  8. mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
  9. /* check if we are pretty close to home already */
  10. float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
  11. _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
  12. /* decide where to enter the RTL procedure when we switch into it */
  13. if (_rtl_state == RTL_STATE_NONE) {
  14. /* for safety reasons don't go into RTL if landed */
  15. if (_navigator->get_land_detected()->landed) {
  16. _rtl_state = RTL_STATE_LANDED;
  17. mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
  18. /* if lower than return altitude, climb up first */
  19. } else if (home_dist > _param_rtl_min_dist.get() && _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
  20. + _param_return_alt.get()) {
  21. _rtl_state = RTL_STATE_CLIMB;
  22. /* otherwise go straight to return */
  23. } else {
  24. /* set altitude setpoint to current altitude */
  25. _rtl_state = RTL_STATE_RETURN;
  26. _mission_item.altitude_is_relative = false;
  27. _mission_item.altitude = _navigator->get_global_position()->alt;
  28. }
  29. }
  30. set_rtl_item();
  31. }
[cpp] view plaincopy
  1. void
  2. RTL::on_active()
  3. {
  4. if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
  5. advance_rtl();
  6. set_rtl_item();
  7. }
  8. }

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

[cpp] view plaincopy
  1. void
  2. RTL::set_rtl_item()
  3. {
  4. struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
  5. /* make sure we have the latest params */
  6. updateParams();
  7. if (!_rtl_start_lock) {
  8. set_previous_pos_setpoint();
  9. }
  10. _navigator->set_can_loiter_at_sp(false);
  11. switch (_rtl_state) {
  12. case RTL_STATE_CLIMB: {
  13. float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
  14. _mission_item.lat = _navigator->get_global_position()->lat;
  15. _mission_item.lon = _navigator->get_global_position()->lon;
  16. _mission_item.altitude_is_relative = false;
  17. _mission_item.altitude = climb_alt;
  18. _mission_item.yaw = NAN;
  19. _mission_item.loiter_radius = _navigator->get_loiter_radius();
  20. _mission_item.loiter_direction = 1;
  21. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  22. _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
  23. _mission_item.time_inside = 0.0f;
  24. _mission_item.pitch_min = 0.0f;
  25. _mission_item.autocontinue = true;
  26. _mission_item.origin = ORIGIN_ONBOARD;
  27. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
  28. (int)(climb_alt),
  29. (int)(climb_alt - _navigator->get_home_position()->alt));
  30. break;
  31. }
  32. case RTL_STATE_RETURN: {
  33. _mission_item.lat = _navigator->get_home_position()->lat;
  34. _mission_item.lon = _navigator->get_home_position()->lon;
  35. // don't change altitude
  36. // use home yaw if close to home
  37. /* check if we are pretty close to home already */
  38. float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon,
  39. _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
  40. if (home_dist < _param_rtl_min_dist.get()) {
  41. _mission_item.yaw = _navigator->get_home_position()->yaw;
  42. } else {
  43. if (pos_sp_triplet->previous.valid) {
  44. /* if previous setpoint is valid then use it to calculate heading to home */
  45. _mission_item.yaw = get_bearing_to_next_waypoint(
  46. pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
  47. _mission_item.lat, _mission_item.lon);
  48. } else {
  49. /* else use current position */
  50. _mission_item.yaw = get_bearing_to_next_waypoint(
  51. _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
  52. _mission_item.lat, _mission_item.lon);
  53. }
  54. }
  55. _mission_item.loiter_radius = _navigator->get_loiter_radius();
  56. _mission_item.loiter_direction = 1;
  57. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  58. _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
  59. _mission_item.time_inside = 0.0f;
  60. _mission_item.pitch_min = 0.0f;
  61. _mission_item.autocontinue = true;
  62. _mission_item.origin = ORIGIN_ONBOARD;
  63. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
  64. (int)(_mission_item.altitude),
  65. (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
  66. _rtl_start_lock = true;
  67. break;
  68. }
  69. case RTL_STATE_TRANSITION_TO_MC: {
  70. _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
  71. _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
  72. break;
  73. }
  74. case RTL_STATE_DESCEND: {
  75. _mission_item.lat = _navigator->get_home_position()->lat;
  76. _mission_item.lon = _navigator->get_home_position()->lon;
  77. _mission_item.altitude_is_relative = false;
  78. _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
  79. // check if we are already lower - then we will just stay there
  80. if (_mission_item.altitude > _navigator->get_global_position()->alt) {
  81. _mission_item.altitude = _navigator->get_global_position()->alt;
  82. }
  83. _mission_item.yaw = _navigator->get_home_position()->yaw;
  84. // except for vtol which might be still off here and should point towards this location
  85. float d_current = get_distance_to_next_waypoint(
  86. _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
  87. _mission_item.lat, _mission_item.lon);
  88. if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
  89. _mission_item.yaw = get_bearing_to_next_waypoint(
  90. _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
  91. _mission_item.lat, _mission_item.lon);
  92. }
  93. _mission_item.loiter_radius = _navigator->get_loiter_radius();
  94. _mission_item.loiter_direction = 1;
  95. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  96. _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
  97. _mission_item.time_inside = 0.0f;
  98. _mission_item.pitch_min = 0.0f;
  99. _mission_item.autocontinue = false;
  100. _mission_item.origin = ORIGIN_ONBOARD;
  101. /* disable previous setpoint to prevent drift */
  102. pos_sp_triplet->previous.valid = false;
  103. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
  104. (int)(_mission_item.altitude),
  105. (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
  106. break;
  107. }
  108. case RTL_STATE_LOITER: {
  109. bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
  110. _mission_item.lat = _navigator->get_home_position()->lat;
  111. _mission_item.lon = _navigator->get_home_position()->lon;
  112. // don't change altitude
  113. _mission_item.yaw = _navigator->get_home_position()->yaw;
  114. _mission_item.loiter_radius = _navigator->get_loiter_radius();
  115. _mission_item.loiter_direction = 1;
  116. _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
  117. _mission_item.acceptance_radius = _navigator->get_acceptance_radius();
  118. _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
  119. _mission_item.pitch_min = 0.0f;
  120. _mission_item.autocontinue = autoland;
  121. _mission_item.origin = ORIGIN_ONBOARD;
  122. _navigator->set_can_loiter_at_sp(true);
  123. if (autoland && (_mission_item.time_inside > FLT_EPSILON)) {
  124. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
  125. } else {
  126. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
  127. }
  128. break;
  129. }
  130. case RTL_STATE_LAND: {
  131. _mission_item.yaw = _navigator->get_home_position()->yaw;
  132. set_land_item(&_mission_item, false);
  133. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
  134. break;
  135. }
  136. case RTL_STATE_LANDED: {
  137. set_idle_item(&_mission_item);
  138. mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
  139. break;
  140. }
  141. default:
  142. break;
  143. }
  144. reset_mission_item_reached();
  145. /* execute command if set */
  146. if (!item_contains_position(&_mission_item)) {
  147. issue_command(&_mission_item);
  148. }
  149. /* convert mission item to current position setpoint and make it valid */
  150. mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
  151. pos_sp_triplet->next.valid = false;
  152. _navigator->set_position_setpoint_triplet_updated();
  153. }

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

[cpp] view plaincopy
  1. void
  2. MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
  3. {
  4. /* set the correct setpoint for vtol transition */
  5. if(item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
  6. && item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
  7. sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
  8. waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
  9. _navigator->get_global_position()->lon,
  10. item->yaw,
  11. 1000000.0f,
  12. &sp->lat,
  13. &sp->lon);
  14. sp->alt = _navigator->get_global_position()->alt;
  15. }
  16. /* don't change the setpoint for non-position items */
  17. if (!item_contains_position(item)) {
  18. return;
  19. }
  20. sp->valid = true;
  21. sp->lat = item->lat;
  22. sp->lon = item->lon;
  23. sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
  24. sp->yaw = item->yaw;
  25. sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius :
  26. _navigator->get_loiter_radius();
  27. sp->loiter_direction = item->loiter_direction;
  28. sp->pitch_min = item->pitch_min;
  29. sp->acceptance_radius = item->acceptance_radius;
  30. sp->disable_mc_yaw_control = false;
  31. sp->cruising_speed = _navigator->get_cruising_speed();
  32. switch (item->nav_cmd) {
  33. case NAV_CMD_IDLE:
  34. sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
  35. break;
  36. case NAV_CMD_TAKEOFF:
  37. case NAV_CMD_VTOL_TAKEOFF:
  38. sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
  39. break;
  40. case NAV_CMD_LAND:
  41. case NAV_CMD_VTOL_LAND:
  42. sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
  43. if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){
  44. sp->disable_mc_yaw_control = true;
  45. }
  46. break;
  47. case NAV_CMD_LOITER_TIME_LIMIT:
  48. case NAV_CMD_LOITER_TURN_COUNT:
  49. case NAV_CMD_LOITER_UNLIMITED:
  50. sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
  51. if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
  52. sp->disable_mc_yaw_control = true;
  53. }
  54. break;
  55. default:
  56. sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
  57. break;
  58. }
  59. }
[cpp] view plaincopy
  1. if (_pos_sp_triplet_updated) {
  2. publish_position_setpoint_triplet();
  3. _pos_sp_triplet_updated = false;
  4. }
[cpp] view plaincopy
  1. void
  2. Navigator::publish_position_setpoint_triplet()
  3. {
  4. /* update navigation state */
  5. _pos_sp_triplet.nav_state = _vstatus.nav_state;
  6. /* do not publish an empty triplet */
  7. if (!_pos_sp_triplet.current.valid) {
  8. return;
  9. }
  10. /* lazily publish the position setpoint triplet only once available */
  11. if (_pos_sp_triplet_pub != nullptr) {
  12. orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
  13. } else {
  14. _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
  15. }
  16. }

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

[cpp] view plaincopy
  1. void MulticopterPositionControl::control_auto(float dt)
  2. {
  3. ……
  4. if (updated) {
  5. orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
  6. //Make sure that the position setpoint is valid
  7. if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
  8. !PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
  9. !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
  10. _pos_sp_triplet.current.valid = false;
  11. }
  12. }
  13. ……
  14. }

接着分析mission.cpp

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

[cpp] view plaincopy
  1. void
  2. NavigatorMode::run(bool active)
  3. {
  4. if (active) {
  5. if (_first_run) {
  6. /* first run */
  7. _first_run = false;
  8. /* Reset stay in failsafe flag */
  9. _navigator->get_mission_result()->stay_in_failsafe = false;
  10. _navigator->set_mission_result_updated();
  11. on_activation();
  12. } else {
  13. /* periodic updates when active */
  14. on_active();
  15. }
  16. } else {
  17. /* periodic updates when inactive */
  18. _first_run = true;
  19. on_inactive();
  20. }
  21. }
[cpp] view plaincopy
  1. void
  2. Mission::on_activation()
  3. {
  4. set_mission_items();
  5. }
[cpp] view plaincopy
  1. void
  2. Mission::on_active()
  3. {
  4. check_mission_valid();
  5. /* check if anything has changed */
  6. bool onboard_updated = false;
  7. orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
  8. if (onboard_updated) {
  9. update_onboard_mission();
  10. }
  11. bool offboard_updated = false;
  12. orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
  13. if (offboard_updated) {
  14. update_offboard_mission();
  15. }
  16. /* reset the current offboard mission if needed */
  17. if (need_to_reset_mission(true)) {
  18. reset_offboard_mission(_offboard_mission);
  19. update_offboard_mission();
  20. offboard_updated = true;
  21. }
  22. /* reset mission items if needed */
  23. if (onboard_updated || offboard_updated) {
  24. set_mission_items();
  25. }
  26. /* lets check if we reached the current mission item */
  27. if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
  28. set_mission_item_reached();
  29. if (_mission_item.autocontinue) {
  30. /* switch to next waypoint if 'autocontinue' flag set */
  31. advance_mission();
  32. set_mission_items();
  33. }
  34. } else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
  35. altitude_sp_foh_update();
  36. } else {
  37. /* if waypoint position reached allow loiter on the setpoint */
  38. if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
  39. _navigator->set_can_loiter_at_sp(true);
  40. }
  41. }
  42. /* see if we need to update the current yaw heading */
  43. if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
  44. && _param_yawmode.get() < MISSION_YAWMODE_MAX
  45. && _mission_type != MISSION_TYPE_NONE)
  46. || _navigator->get_vstatus()->is_vtol) {
  47. heading_sp_update();
  48. }
  49. }

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

[cpp] view plaincopy
  1. void
  2. Mission::set_mission_items()
  3. {
  4. /* make sure param is up to date */
  5. updateParams();
  6. /* reset the altitude foh logic, if altitude foh is enabled (param) a new foh element starts now */
  7. altitude_sp_foh_reset();
  8. struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
  9. /* the home dist check provides user feedback, so we initialize it to this */
  10. bool user_feedback_done = false;
  11. /* mission item that comes after current if available */
  12. struct mission_item_s mission_item_next_position;
  13. bool has_next_position_item = false;
  14. work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
  15. /* copy information about the previous mission item */
  16. if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {
  17. /* Copy previous mission item altitude */
  18. _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
  19. }
  20. /* try setting onboard mission item */
  21. if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
  22. /* if mission type changed, notify */
  23. if (_mission_type != MISSION_TYPE_ONBOARD) {
  24. mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");
  25. user_feedback_done = true;
  26. }
  27. _mission_type = MISSION_TYPE_ONBOARD;
  28. /* try setting offboard mission item */
  29. } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
  30. /* if mission type changed, notify */
  31. if (_mission_type != MISSION_TYPE_OFFBOARD) {
  32. mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");
  33. user_feedback_done = true;
  34. }
  35. _mission_type = MISSION_TYPE_OFFBOARD;
  36. } else {
  37. /* no mission available or mission finished, switch to loiter */
  38. if (_mission_type != MISSION_TYPE_NONE) {
  39. /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
  40. mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
  41. user_feedback_done = true;
  42. /* use last setpoint for loiter */
  43. _navigator->set_can_loiter_at_sp(true);
  44. }
  45. _mission_type = MISSION_TYPE_NONE;
  46. /* set loiter mission item and ensure that there is a minimum clearance from home */
  47. set_loiter_item(&_mission_item, _param_takeoff_alt.get());
  48. /* update position setpoint triplet  */
  49. pos_sp_triplet->previous.valid = false;
  50. mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
  51. pos_sp_triplet->next.valid = false;
  52. /* reuse setpoint for LOITER only if it's not IDLE */
  53. _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
  54. set_mission_finished();
  55. if (!user_feedback_done) {
  56. /* only tell users that we got no mission if there has not been any
  57. * better, more specific feedback yet
  58. * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
  59. */
  60. if (_navigator->get_land_detected()->landed) {
  61. /* landed, refusing to take off without a mission */
  62. mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
  63. } else {
  64. mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
  65. }
  66. user_feedback_done = true;
  67. }
  68. _navigator->set_position_setpoint_triplet_updated();
  69. return;
  70. }
  71. /*********************************** handle mission item *********************************************/
  72. /* handle position mission items */
  73. if (item_contains_position(&_mission_item)) {
  74. /* force vtol land */
  75. if(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get()
  76. && !_navigator->get_vstatus()->is_rotary_wing){
  77. _mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
  78. }
  79. /* we have a new position item so set previous position setpoint to current */
  80. set_previous_pos_setpoint();
  81. /* do takeoff before going to setpoint if needed and not already in takeoff */
  82. if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
  83. new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
  84. /* use current mission item as next position item */
  85. memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
  86. mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
  87. has_next_position_item = true;
  88. float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
  89. mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
  90. _mission_item.nav_cmd = NAV_CMD_TAKEOFF;
  91. _mission_item.lat = _navigator->get_global_position()->lat;
  92. _mission_item.lon = _navigator->get_global_position()->lon;
  93. /* ignore yaw for takeoff items */
  94. _mission_item.yaw = NAN;
  95. _mission_item.altitude = takeoff_alt;
  96. _mission_item.altitude_is_relative = false;
  97. _mission_item.autocontinue = true;
  98. _mission_item.time_inside = 0;
  99. }
  100. /* if we just did a takeoff navigate to the actual waypoint now */
  101. if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
  102. if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
  103. && _navigator->get_vstatus()->is_rotary_wing
  104. && !_navigator->get_land_detected()->landed
  105. && has_next_position_item) {
  106. /* check if the vtol_takeoff command is on top of us */
  107. if(do_need_move_to_takeoff()){
  108. new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
  109. } else {
  110. new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
  111. }
  112. _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
  113. _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
  114. _mission_item.yaw = _navigator->get_global_position()->yaw;
  115. } else {
  116. new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
  117. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  118. /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
  119. _mission_item.yaw = NAN;
  120. }
  121. }
  122. /* takeoff completed and transitioned, move to takeoff wp as fixed wing */
  123. if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
  124. && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
  125. new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
  126. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  127. _mission_item.autocontinue = true;
  128. _mission_item.time_inside = 0.0f;
  129. }
  130. /* move to land wp as fixed wing */
  131. if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
  132. && _work_item_type == WORK_ITEM_TYPE_DEFAULT
  133. && !_navigator->get_land_detected()->landed) {
  134. new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
  135. /* use current mission item as next position item */
  136. memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
  137. has_next_position_item = true;
  138. float altitude = _navigator->get_global_position()->alt;
  139. if (pos_sp_triplet->current.valid) {
  140. altitude = pos_sp_triplet->current.alt;
  141. }
  142. _mission_item.altitude = altitude;
  143. _mission_item.altitude_is_relative = false;
  144. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  145. _mission_item.autocontinue = true;
  146. _mission_item.time_inside = 0;
  147. }
  148. /* transition to MC */
  149. if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
  150. && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
  151. && !_navigator->get_vstatus()->is_rotary_wing
  152. && !_navigator->get_land_detected()->landed) {
  153. _mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
  154. _mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
  155. _mission_item.autocontinue = true;
  156. new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
  157. }
  158. /* move to landing waypoint before descent if necessary */
  159. if (do_need_move_to_land() &&
  160. (_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
  161. _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
  162. new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
  163. /* use current mission item as next position item */
  164. memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
  165. has_next_position_item = true;
  166. /*
  167. * Ignoring waypoint altitude:
  168. * Set altitude to the same as we have now to prevent descending too fast into
  169. * the ground. Actual landing will descend anyway until it touches down.
  170. * XXX: We might want to change that at some point if it is clear to the user
  171. * what the altitude means on this waypoint type.
  172. */
  173. float altitude = _navigator->get_global_position()->alt;
  174. _mission_item.altitude = altitude;
  175. _mission_item.altitude_is_relative = false;
  176. _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
  177. _mission_item.autocontinue = true;
  178. _mission_item.time_inside = 0;
  179. }
  180. /* we just moved to the landing waypoint, now descend */
  181. if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
  182. && _navigator->get_vstatus()->is_rotary_wing) {
  183. new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
  184. }
  185. /* ignore yaw for landing items */
  186. /* XXX: if specified heading for landing is desired we could add another step before the descent
  187. * that aligns the vehicle first */
  188. if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND ) {
  189. _mission_item.yaw = NAN;
  190. }
  191. /* handle non-position mission items such as commands */
  192. } else {
  193. /* turn towards next waypoint before MC to FW transition */
  194. if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
  195. && _work_item_type != WORK_ITEM_TYPE_ALIGN
  196. && _navigator->get_vstatus()->is_rotary_wing
  197. && !_navigator->get_land_detected()->landed
  198. && has_next_position_item) {
  199. new_work_item_type = WORK_ITEM_TYPE_ALIGN;
  200. set_align_mission_item(&_mission_item, &mission_item_next_position);
  201. }
  202. /* yaw is aligned now */
  203. if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
  204. new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
  205. }
  206. }
  207. /*********************************** set setpoints and check next *********************************************/
  208. /* set current position setpoint from mission item (is protected agains non-position items) */
  209. mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
  210. /* issue command if ready (will do nothing for position mission items) */
  211. issue_command(&_mission_item);
  212. /* set current work item type */
  213. _work_item_type = new_work_item_type;
  214. /* require takeoff after landing or idle */
  215. if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
  216. _need_takeoff = true;
  217. }
  218. _navigator->set_can_loiter_at_sp(false);
  219. reset_mission_item_reached();
  220. if (_mission_type == MISSION_TYPE_OFFBOARD) {
  221. set_current_offboard_mission_item();
  222. }
  223. // TODO: report onboard mission item somehow
  224. if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
  225. /* try to process next mission item */
  226. if (has_next_position_item) {
  227. /* got next mission item, update setpoint triplet */
  228. mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
  229. } else {
  230. /* next mission item is not available */
  231. pos_sp_triplet->next.valid = false;
  232. }
  233. } else {
  234. /* vehicle will be paused on current waypoint, don't set next item */
  235. pos_sp_triplet->next.valid = false;
  236. }
  237. /* Save the distance between the current sp and the previous one */
  238. if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
  239. _distance_current_previous = get_distance_to_next_waypoint(
  240. pos_sp_triplet->current.lat,
  241. pos_sp_triplet->current.lon,
  242. pos_sp_triplet->previous.lat,
  243. pos_sp_triplet->previous.lon);
  244. }
  245. _navigator->set_position_setpoint_triplet_updated();
  246. }

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

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
  1. if (_pos_sp_triplet_updated) {
  2. publish_position_setpoint_triplet();
  3. _pos_sp_triplet_updated = false;
  4. }

最后发布出去给各modules

pixhawk commander--navigator--modules之间的联系相关推荐

  1. pixhawk commander.cpp的飞行模式切换解读

    commander.cpp逻辑性太强了,涉及整个系统的运作,所以分别拆分成小块看 另此篇blog大部分是参考(Pixhawk原生固件解读)飞行模式,控制模式的思路,笔者重新整理一下 此部分探究是因为进 ...

  2. modules runAllManagedModulesForAllRequests=true /（转1）

    最近在使用 MVC 开发的时候,遇到一个对我来说"奇怪的问题",就是使用 BundleTable 进行 CSS.JS 文件绑定,然后使用 Styles.Render.Scripts ...

  3. px4原生源码学习四--Nuttx 实时操作系统编程

    前面说到px4是基于Nuttx实时操作系统上的,那么px4也是由一些程序所构成,这些程序实现了飞行器的自主控制,只不过这些程序并不是我们通常所见到的单片机或者windows编程那样的程序,但基本编程思 ...

  4. Android Studio vs Eclipse：你需要知道的那些事

    转用Android Studio开发前,你需要知道 我写的这个指引里,包含了一些当你要把Eclipse项目转到Andorid Studio前需要知道的基本信息.如果你有一个大型项目还没完成从Eclip ...

  5. px4flow智能光学流动传感器

    PX4Flow 是一款智能光学流动传感器.传感器拥有原生 752×480 像素分辨率,计算光学流的过程中采用了4倍分级和剪裁算法,计算速度达到250Hz(白天,室外),具备非常高的感光度.与其他滑鼠传 ...

  6. Android NDK 编译选项设置[zhuan]

    http://crash.163.com/#news/!newsId=24 在Android NDK开发中,有两个重要的文件:Android.mk和Application.mk,各尽其责,指导编译器如 ...

  7. Nginx的11个phases

    Nginx的11个phases 一个请求经过nginx处理的过程中,会经过一系列的阶段(phases),下面这个表格列出了nginx的所有phases,每个阶段可选的退出方式,包含的模块和对应的指令 ...

  8. pytorch gather_Pytorch 单机并行训练

    未经授权,严禁转载! 另外,近一年没用过这个了,所以内容自己也记不得了,各位读者朋友们,抱歉不能回答你们的问题. 个人主页: - 会飞的咸鱼​tankzhoufirst.github.io 参考: O ...

  9. Azure IoT Edge on Windows 10 IoT Core

    在今年的Build大会上,微软推出了Azure IoT Edge的第一个版本(https://github.com/Azure/iot-edge ).该版本的主要特点就是将计算能力由Azure端推送至 ...

  10. python c++混合编程文档缩减版笔记 -2

    官方文档 The Basics What we're showing here is the traditional way of defining static extension types. I ...

最新文章

  1. git 从远程主服务器当中创建新分支
  2. mybatis-spring 项目简介
  3. IT人员需要获得的6个顶级的数据中心教育和认证
  4. C语言中使用静态函数的好处
  5. vim学习（2）小幅提升
  6. 关于es6的const跟vuex里的getter
  7. EffectiveJava读书笔记--01继承的使用注意
  8. nginx常见错误之（CreateFile() “D:\LCJ\下载\nginx/conf/nginx.conf“ failed (3: The system cannot find the path
  9. 网络中没有 计算机,计算机中没有检测到任何网络硬件是什么原因
  10. cni k8s 插件安装_使用kind来快速部署k8s环境
  11. 网站能ping通 但是打不开_SEO网站建设的三要素：域名、空间、网站程序
  12. [4G5G专题-23]：架构-5G无线接入内部网元架构与空口协议功能切分
  13. 如何避免B端产品失败（近万字解析）
  14. Error opening dll library错误的解决
  15. 浏览器与web服务器的交互过程
  16. apmserv 5.2.6 升级php,Windows + APMServ5.2.6/PHP5以上
  17. 深度学习（七）梯度弥散（消散）和梯度爆炸
  18. 加载java连接sqlserver驱动_sqlserverdriver配置方法 jdbc连接sqlserver
  19. 横滚角，俯仰角，航向角
  20. java闭锁_Java并发工具类（闭锁CountDownLatch）

热门文章

  1. 苹果开发账号过期不续费会怎样？
  2. WebDriver API 元素定位（三）
  3. ERROR：非静态成员引用必须与特定对象相对
  4. 一个查看Cookie的便捷工具——EditThisCookie
  5. Uva 11600 期望DP
  6. JDBC链接oracle已经mysql的测试
  7. 文本输入框内实时检测输入的字数
  8. ACM学习历程—Hihocoder 1290 Demo Day（动态规划）
  9. 在Vista以上版本运行WTL程序，有时候会提示“这个程序可能安装补正确...”的错误...
  10. windows远程下载