PX4 REJECT_POSITION_CTRL 源码部分分析

  • 飞机基本状态的定义在vehicle_status.msg

    uint8 MAIN_STATE_MANUAL = 0
    uint8 MAIN_STATE_ALTCTL = 1
    uint8 MAIN_STATE_POSCTL = 2
    uint8 MAIN_STATE_AUTO_MISSION = 3
    uint8 MAIN_STATE_AUTO_LOITER = 4
    uint8 MAIN_STATE_AUTO_RTL = 5
    uint8 MAIN_STATE_ACRO = 6
    uint8 MAIN_STATE_OFFBOARD = 7
    uint8 MAIN_STATE_STAB = 8
    uint8 MAIN_STATE_RATTITUDE = 9
    uint8 MAIN_STATE_AUTO_TAKEOFF = 10
    uint8 MAIN_STATE_AUTO_LAND = 11
    uint8 MAIN_STATE_MAX = 12
    
  • state_machine_helper.cpp 描述允许状态转换的条件

    ....
    case vehicle_status_s::MAIN_STATE_POSCTL:
          /* need at minimum local position estimate */
          // 也就是只要 Local 或 Global 其中一个定位有效,即可切换 POSCTL
          if (status->condition_local_position_valid ||
              status->condition_global_position_valid) {
              ret = TRANSITION_CHANGED;
          }
          break;
    ....
    
    • status->condition_global_position_valid 的判断在commander.cpp

      主要是看GPS的时间更新是否有效

      ...
      //update condition_global_position_valid
              //Global positions are only published by the estimators if they are valid
      if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
        //We have had no good fix for POSITION_TIMEOUT amount of time
        if (status.condition_global_position_valid) {
          set_tune_override(TONE_GPS_WARNING_TUNE);
          status_changed = true;
          status.condition_global_position_valid = false;
        }
      } else if (global_position.timestamp != 0) {
        // Got good global position estimate
        if (!status.condition_global_position_valid) {
          status_changed = true;
          status.condition_global_position_valid = true;
        }
      }
      ...
      
    • status->condition_local_position_validcommander.cpp

      /* update condition_local_position_valid and condition_local_altitude_valid */
      /* hysteresis for EPH */
      bool local_eph_good;
      
      // 无论当前 condition_local_position_valid 是否有效,都有判断 local_position.eph 的大小
      if (status.condition_local_position_valid) {
        if (local_position.eph > eph_threshold * 2.5f) {
          local_eph_good = false;
        } else {
          local_eph_good = true;
        }
      
      } else {
        if (local_position.eph < eph_threshold) {
          local_eph_good = true;
      
        } else {
          local_eph_good = false;
        }
      }
      
      // 根据上述判断结果,对 condition_local_position_valid 进行更新
      check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
      

      void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
      // timestamp = local_position.timestamp
      // timeout = POSITION_TIMEOUT
      // valid_in = local_position.xy_valid && local_eph_good  判断位置精度是否满足
      // valid_out = &(status.condition_local_position_valid)
      // changed = &status_changed
      {
          hrt_abstime t = hrt_absolute_time();
          bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);  // 判断是否超时 和 xy_valid 与 local_eph_good 是否同时有效
          if (*valid_out != valid_new) {
              *valid_out = valid_new;   // 对 condition_local_position_valid 进行赋值 = true
              *changed = true;   // 对 status_changed 进行赋值
          }
      }
      

推荐阅读更多精彩内容