Looking for your help on the APM flight control algorithm

ArduCopter Copter 3.3
1

Recently I’m learning about the APM flight control algorithm, and I have a question about the pos_to_rate_z function.
Here is it:
void AC_PosControl::pos_to_rate_z()
{
。。。。。。

// check kP to avoid division by zero
if (_p_alt_pos.kP() != 0.0f) {
    linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
    if (_pos_error.z > 2*linear_distance ) {
        _vel_target.z = safe_sqrt(2.0f*_accel_z_cms*(_pos_error.z-linear_distance));
    }else if (_pos_error.z < -2.0f*linear_distance) {
        _vel_target.z = -safe_sqrt(2.0f*_accel_z_cms*(-_pos_error.z-linear_distance));
    }else{
        _vel_target.z = _p_alt_pos.get_p(_pos_error.z);
    }
}else{
    _vel_target.z = 0;
}

。。。。。。
}

I have no ieda about the linear_distance variable, why should we need it? Or How it works?

2

Shark,
This is more of a development question so feel free to email drones-discuss@googlegroups.com.
Leonard Hall is the designer of the position controller so he knows better but my understanding is that this “linear_distance” is the distance (in cm) from the target location when the position controller’s switches from using a linear to a square-root response. So taking a step back, this part of the code is responsible for converting a position error into a desired velocity. So when the vehicle is very close to the target position, the response will be linear. For example, a 1m position error would lead to a 1m/s desired velocity response. When the vehicle is much further from the target position the response is non-linear. So for example if the vehicle is 40m from the target the desired velocity might be 6.3m/s (sqrt of 40). At 50m from the target it might be 7m/s.