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_valid 在commander.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 进行赋值
        }
    }
    

    ​