PX4 code analysis – (3) Navigator code analysis

1. Function of Navigator module

The Navigator module is composed of a navigator_main and multiple task modes (such as loiter, mission). navigator_main is responsible for the basic functions of subscribing topic, processing command and switching different tasks. Each task mode is responsible for generating and publishing the topic _pos_sp_triplet according to the specific task type.

From the name, navigator means navigation, but it is a little different from navigation in px4. _pos_sp_triplet is not the final expected position point. For example, for multiple task points in mission mode, _pos_sp_triplet is only the current task point, which is a relatively rough point. The actual execution expected position is generated in flight_mode_manager.

This time, only the code execution process of navigator_main is analyzed. The specific task modes are relatively independent modules, which will be analyzed in detail later

• 2. Navigator function structure diagram

• 

• 3. Code Analysis

• 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 */
  //Subscribe to the two most important topics, namely local_pos local location information and vehicle_status current state of the aircraft
  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( & amp; 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 & amp; POLLIN) {
  /* success, local pos is available */
  orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, & amp;_local_pos);
  }
  }
  
  perf_begin(_loop_perf);
  //Copy the current status of the drone
  orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, & amp;_vstatus);
  //gps data update
  /* gps updated */
  if (_gps_pos_sub.updated()) {
  _gps_pos_sub.copy( & amp;_gps_pos);
  
  if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
  have_geofence_position_data = true;
  }
  }
  //Global location update
  /* global position updated */
  if (_global_pos_sub.updated()) {
  _global_pos_sub.copy( & amp;_global_pos);
  
  if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
  have_geofence_position_data = true;
  }
  }
  //Parameter update
  // check for parameter updates
  if (_parameter_update_sub.updated()) {
  // clear update
  parameter_update_s update;
  _parameter_update_sub.copy( & amp;pupdate);
  
  //update parameters from storage
  params_update();
  }
  
  _land_detected_sub.update( & amp;_land_detected);
  _position_controller_status_sub.update();
  _home_pos_sub.update( & amp;_home_pos);
  //Process navigation-related commands
  if (_vehicle_command_sub.updated()) {
  const unsigned last_generation = _vehicle_command_sub.get_last_generation();
  vehicle_command_s cmd{};
  _vehicle_command_sub.copy( & amp;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());
  }
  //Process the DO_GO_AROUND command
  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) {//Process relocation instructions
  
  bool reposition_valid = true;
  
  if (have_geofence_position_data & amp; & amp;
  ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) & amp; & amp;
  (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) {
  
  if (PX4_ISFINITE(cmd.param5) & amp; & amp; 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) & amp; & amp; 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 & amp; & amp; PX4_ISFINITE(curr->current.lat) & amp; & amp; 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( & amp;_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) {//Process takeoff command
  position_setpoint_triplet_s *rep = get_takeoff_triplet();
  
  // store current position as previous position and goal as next
  //Save the current position as the information of the previous point of 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;
  //If the home position is valid, set the yaw direction according to the parameter 4 (param4) passed in the command, otherwise use the current yaw direction as the expected 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 the destination longitude and latitude passed through the parameters is valid, it will be used as the take-off target. Otherwise, the longitude and latitude of the current position will be used as the take-off target longitude and latitude.
  if (PX4_ISFINITE(cmd.param5) & amp; & amp; 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;
  }
  //The expected take-off height is passed through parameters, but the analysis height of the following code is still determined by many other factors
  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) {//Process the command to start landing
  
  /* 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) {//Process the instruction to start the task
  if (_mission_result.valid & amp; & amp; PX4_ISFINITE(cmd.param1) & amp; & amp; (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) {//Process the command to change the 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 & amp; & amp;
  (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) & amp; & amp;
  (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( & amp;_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
  & amp; & amp; 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 to different mission modes based on the current navigation status of the aircraft
  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() & amp; & amp; !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() & amp; & amp; !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 & amp; & amp; (_navigation_mode != & amp;_rtl || _mission.get_mission_changed())) & amp; & amp;
  (! _mission.get_mission_finished()) & amp; & amp;
  (!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 != & amp;_mission) & amp; & amp; (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 = & amp;_rtl;
  }
  }
  
  break;
  
  default:
  if (rtl_activated) {
  mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
  }
  
  navigation_mode_new = & amp;_rtl;
  break;
  
  }
  
  break;
  }
  
  case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
  _pos_sp_triplet_published_invalid_once = false;
  navigation_mode_new = & amp;_takeoff;
  break;
  
  case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
  _pos_sp_triplet_published_invalid_once = false;
  navigation_mode_new = & amp;_land;
  break;
  
  case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
  _pos_sp_triplet_published_invalid_once = false;
  navigation_mode_new = & amp;_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 = & amp;_engineFailure;
  break;
  
  case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
  _pos_sp_triplet_published_invalid_once = false;
  navigation_mode_new = & amp;_gpsFailure;
  break;
  
  case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
  _pos_sp_triplet_published_invalid_once = false;
  navigation_mode_new = & amp;_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 == & amp;_takeoff & amp; & amp;
  navigation_mode_new == & amp;_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 & amp; & amp; !_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);
  }
  }