浅谈APM系列-----update_home_from_EKF

函数：void Copter::update_home_from_EKF()

位置：X:\ardupilot\ArduCopter\commands.cpp

/*
 * the home_state has a number of possible values (see enum HomeState in defines.h's)
 *   HOME_UNSET             = home is not set, no GPS positions yet received(家里没有设置，也没有GPS定位)
 *   HOME_SET_NOT_LOCKED    = home is set to EKF origin or armed location (can be moved)
 *   HOME_SET_AND_LOCKED    = home has been set by user, cannot be moved except by user initiated do-set-home command
 */

// checks if we should update ahrs/RTL home position from the EKF
void Copter::update_home_from_EKF()
{
    // exit immediately if home already set
    if (ap.home_state != HOME_UNSET) {//只有在没有GPS，没有设置HOME坐标的时候执行；否则就退出
        return;
    }

    // special logic if home is set in-flight
    if (motors->armed()) {//检查是否解锁
        set_home_to_current_location_inflight();
    } else {
        // move home to current ekf location (this will set home_state to HOME_SET)
        set_home_to_current_location(false);
    }
}

===========================================================================================

motors->armed()，在解锁的情况下

===========================================================================================

函数：void Copter::set_home_to_current_location_inflight()

位置：X:\ardupilot\ArduCopter\commands.cpp

// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
void Copter::set_home_to_current_location_inflight() {
    // get current location from EKF
    Location temp_loc;//记录临时位置
    if (inertial_nav.get_location(temp_loc)) {//根据惯性导航，获取现在位置
        const struct Location &ekf_origin = inertial_nav.get_origin();//根据惯性导航，获取原点位置
        temp_loc.alt = ekf_origin.alt;//将高度设置为原点位置的高度
        if (!set_home(temp_loc, false)) {//设置temp_loc，END坐标系的原点
            return;
        }
        // we have successfully set AHRS home, set it for SmartRTL（我们已经成功地将AHRS设置为home，设置为SmartRTL）
        g2.smart_rtl.set_home(true);
    }
}

===========================================================================================

第一个set_home函数

===========================================================================================

函数：bool Copter::set_home(const Location& loc, bool lock)

位置：X:\ardupilot\ArduCopter\commands.cpp

// set_home - sets ahrs home (used for RTL) to specified location
//  initialises inertial nav and compass on first call
//  returns true if home location set successfully
bool Copter::set_home(const Location& loc, bool lock)
{
    // check location is valid（检查位置是否有效）
    if (loc.lat == 0 && loc.lng == 0) {
        return false;
    }

    // check EKF origin has been set（检查EKF的来源已经设置好了）
    Location ekf_origin;
    if (!ahrs.get_origin(ekf_origin)) {
        return false;//没有设置EKF origin 
    }

    // check home is close to EKF origin（检查HOME接近于EKF的来源）
    if (far_from_EKF_origin(loc)) {//相距超过50千米，返回TRUE
        return false;
    }

    // set ahrs home (used for RTL)（设置姿态航向参考系统的HOME点）
    ahrs.set_home(loc);

    // init inav and compass declination（初始化惯性导航和磁力计偏差）
    if (ap.home_state == HOME_UNSET) {
        // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
        //更新导航标量。当我们向两极移动时，用来抵消经度的收缩
        scaleLongDown = longitude_scale(loc);
        // record home is set
        set_home_state(HOME_SET_NOT_LOCKED);//修改HOME点的设置状态

        // log new home position which mission library will pull from ahrs
        if (should_log(MASK_LOG_CMD)) {
            AP_Mission::Mission_Command temp_cmd;
            if (mission.read_cmd_from_storage(0, temp_cmd)) {
                DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
            }
        }
    }

    // lock home position
    if (lock) {//false
        set_home_state(HOME_SET_AND_LOCKED);
    }

    // log ahrs home and ekf origin dataflash（记录AHRS的HOME点和EKF的原点）
    Log_Write_Home_And_Origin();

    // send new home and ekf origin to GCS（发送新的点的位置到地面站）
    gcs().send_home(loc);
    gcs().send_ekf_origin(ekf_origin);

    // return success
    return true;
}

===========================================================================================

第二个set_home函数

===========================================================================================

函数：void AP_SmartRTL::set_home(bool position_ok)

位置：X:\ardupilot\libraries\AP_SmartRTL\AP_SmartRTL.cpp

// clear return path and set home location.  This should be called as part of the arming procedure
void AP_SmartRTL::set_home(bool position_ok)//true
{
    Vector3f current_pos;
    position_ok &= _ahrs.get_relative_position_NED_origin(current_pos);//只有在惯性导航时，返回TRUE
    set_home(position_ok, current_pos);
}

void AP_SmartRTL::set_home(bool position_ok, const Vector3f& current_pos)//current_pos相对于原点的位置
{
    if (_path == nullptr) {
        return;
    }

    // clear path
    _path_points_count = 0;

    // reset simplification and pruning.  These functions access members that should normally only
    // be touched by the background thread but it will not be running because active should be false
    reset_simplification();//重置简化算法，这样它就会重新检查路径中的所有点
    reset_pruning();//重置修剪算法，这样它就会重新检查路径中的所有点

    // don't continue if no position at take-off（如果在起飞时没有位置，就不要继续）
    if (!position_ok) {
        return;
    }

    // save current position as first point in path（保存当前位置作为路径中的第一个点）
    if (!add_point(current_pos)) {
        return;
    }

    // successfully added point and reset path
    _last_good_position_ms = AP_HAL::millis(); // the last system time a last good position was reported. If no position is available for a while, SmartRTL will be disabled.
    _active = true;// true if SmartRTL is usable.  may become unusable if the path becomes too long to keep in memory, and too convoluted to be cleaned up, SmartRTL will be permanently deactivated (for the remainder of the flight)
    _home_saved = true;// true once home has been saved successfully by the set_home or update methods
}

函数：bool AP_SmartRTL::add_point(const Vector3f& point)

位置：X:\ardupilot\libraries\AP_SmartRTL\AP_SmartRTL.cpp

// add point to end of path (if necessary), returns true on success（在路径的末尾添加点（如果需要的话），在成功时返回true）
bool AP_SmartRTL::add_point(const Vector3f& point)
{
    // get semaphore
    if (!_path_sem->take_nonblocking()) {
        log_action(SRTL_ADD_FAILED_NO_SEMAPHORE, point);
        return false;
    }

    // check if we have traveled far enough（检查我们是否已经走得够远了）
    if (_path_points_count > 0) {// number of points in the path array
        const Vector3f& last_pos = _path[_path_points_count-1];
        if (last_pos.distance_squared(point) < sq(_accuracy.get())) {//判断距离是否太近了
            _path_sem->give();
            return true;
        }
    }

    // check we have space in the path
    if (_path_points_count >= _path_points_max) {// after the array has been allocated, we will need to know how big it is. We can't use the parameter, because a user could change the parameter in-flight
        _path_sem->give();
        log_action(SRTL_ADD_FAILED_PATH_FULL, point);
        return false;
    }

    // add point to path（在路径中，添加点）
    _path[_path_points_count++] = point;
    log_action(SRTL_POINT_ADD, point);

    _path_sem->give();
    return true;
}

函数：void AP_SmartRTL::reset_simplification()

位置：X:\ardupilot\libraries\AP_SmartRTL\AP_SmartRTL.cpp

// reset simplification algorithm so that it will re-check all points in the path
void AP_SmartRTL::reset_simplification()
{
    restart_simplification(0);
    _simplify.path_points_completed = 0;
}

函数：void AP_SmartRTL::restart_simplification(uint16_t path_points_count)

位置：X:\ardupilot\libraries\AP_SmartRTL\AP_SmartRTL.cpp

// restart simplification algorithm so that it will check new points in the path
void AP_SmartRTL::restart_simplification(uint16_t path_points_count)
{
    _simplify.complete = false;
    _simplify.removal_required = false;
    _simplify.bitmask.setall();
    _simplify.stack_count = 0;
    _simplify.path_points_count = path_points_count;
}

函数：void AP_SmartRTL::reset_pruning()

位置：X:\ardupilot\libraries\AP_SmartRTL\AP_SmartRTL.cpp

// reset pruning algorithm so that it will re-check all points in the path
void AP_SmartRTL::reset_pruning()
{
    restart_pruning(0);
    _prune.loops_count = 0; // clear the loops that we've recorded
    _prune.path_points_completed = 0;
}

// restart pruning algorithm to check new points that have arrived
void AP_SmartRTL::restart_pruning(uint16_t path_points_count)
{
    _prune.complete = false;
    _prune.i = (path_points_count > 0) ? path_points_count - 1 : 0;
    _prune.j = 0;
    _prune.path_points_count = path_points_count;
}

===========================================================================================

motors->armed()，在未解锁的情况下

===========================================================================================

函数：bool Copter::set_home_to_current_location(bool lock)

位置：X:\ardupilot\ArduCopter\commands.cpp

// set_home_to_current_location - set home to current GPS location（设置HOME点用当前GPS定位）
bool Copter::set_home_to_current_location(bool lock) {//false
    // get current location from EKF
    Location temp_loc;
    if (inertial_nav.get_location(temp_loc)) {
        if (!set_home(temp_loc, lock)) {//这个set_home跟前面第一个set_hom是同一个函数
            return false;
        }
        // we have successfully set AHRS home, set it for SmartRTL
        g2.smart_rtl.set_home(true);//这个set_home跟前面第二个set_hom是同一个函数
        return true;
    }
    return false;
}

根据不同解锁状态分别调用下面两个函数，区别如下：

// set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
void Copter::set_home_to_current_location_inflight() {
    // get current location from EKF
    Location temp_loc;
    if (inertial_nav.get_location(temp_loc)) {
        const struct Location &ekf_origin = inertial_nav.get_origin();
        temp_loc.alt = ekf_origin.alt;
//        if (!set_home(temp_loc, false)) {
//            return;
//        }
        // we have successfully set AHRS home, set it for SmartRTL
//        g2.smart_rtl.set_home(true);
    }
}

// set_home_to_current_location - set home to current GPS location
bool Copter::set_home_to_current_location(bool lock) {//false
    // get current location from EKF
    Location temp_loc;
    if (inertial_nav.get_location(temp_loc)) {
//        if (!set_home(temp_loc, lock)) {
//            return false;
//        }
        // we have successfully set AHRS home, set it for SmartRTL
//        g2.smart_rtl.set_home(true);
//        return true;
    }
//    return false;
}

看代码真的很无聊

---

请期待。。。