logo头像
Snippet 博客主题

apm 气压定高不稳方案

2019/03/25
  1. 由于APM代码定高不稳，故现在重新做定高部分。

可以调节的参数：
PSC_POSZ_P：位置（垂直）控制器P增益
位置（垂直）控制器P增益。将所需高度和实际高度之间的差值转换为传递到节流率控制器的爬升率或下降率
PILOT_THR_FILT：油门滤波器截止
油门滤波器截止频率（Hz） - 每当高度控制无效时激活 - 0表示禁用
PILOT_TKOFF_ALT：飞行员起飞高度
当油门杆触发起飞时，高度控制模式将爬升到的高度
PILOT_ACCEL_Z：飞行员垂直加速度
飞行员控制高度时使用的垂直加速度
ATC_ANG_LIM_TC：角度限制（保持高度）时间常数
角度限制（保持高度）时间常数
EK2_WIND_PSCALE: Height rate to wind process noise scaler
GND_ALT_OFFSET：海拔偏移
高度偏移量，以米为单位添加到气压高度。这用于允许通过配备气压计的地面站自动调节基本气压高度。该值被添加到飞机读取的气压高度。当每次重启时校准气压计或执行预检校准时，它会自动重置为0。

1.1 先分析高度控制代码，从althold代码分析。
分析Copter::ModeAltHold::run()
初始化垂直速度和加速度：
从这里可以看到有两个参数可以调节：1. g.pilot_accel_z 2. g.pilot_speed_up

1
2
3
> // initialize vertical speeds and acceleration
    pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
    pos_control->set_accel_z(g.pilot_accel_z);

此地方就是获得倾角的地方。

1
2
3
4
5
6
7
8
9
10
>  // get pilot desired lean angles
>   float target_roll, target_pitch;
>   get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());

>  // get pilot's desired yaw rate
>   float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

>  // get pilot desired climb rate
>    float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
>    target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
  1. 气压计选型

2.1. spl06-001/DPS310(玩具使用)
压力传感器分辨率：±0.005 hPa（或±0.05 m）（高精度模式）
相对精度：±0.06 hPa（或±0.5 m）
绝对精度：±1 hPa（或±8 m）

2.2. LPS22HH（目前性能最优）
压力传感器分辨率：±0.005 hPa（或±0.05 m）（高精度模式）
相对精度 0.025 hpa
绝对压力精度: ±0.5 hPa

2.3. MS5611(目前在用)
压力传感器分辨率：±0.012 hPa（或±0.12 m）（高精度模式）
绝对压力精度: ±1.5 hPa

  1. 分析定高控制代码：

3.1 pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);

  • target_climb_rate：target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); 让输入油门杆量转换为期望速率。若是在油门死区以上或者以下就判定为手动操作，当油门在中位时，让desired_rate = 0;
  • 在定高时，目标攀爬率为0
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    34
    35
    /// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
    ///     should be called continuously (with dt set to be the expected time between calls)
    ///     actual position target will be moved no faster than the speed_down and speed_up
    ///     target will also be stopped if the motors hit their limits or leash length is exceeded
    ///     set force_descend to true during landing to allow target to move low enough to slow the motors
    void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
    {
        // calculated increased maximum acceleration if over speed
        float accel_z_cms = _accel_z_cms;
        if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
            accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
        }
        if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {
            accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
        }
        accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);

        // jerk_z is calculated to reach full acceleration in 1000ms.
        float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;

        float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));

        _accel_last_z_cms += jerk_z * dt;
        _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);

        float vel_change_limit = _accel_last_z_cms * dt;
        _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
        _flags.use_desvel_ff_z = true;

        // adjust desired alt if motors have not hit their limits
        // To-Do: add check of _limit.pos_down?
        if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
            _pos_target.z += _vel_desired.z * dt;
        }
    }

问题：_vel_desired.z 是哪里得到的呢？
在底下_vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);会发现climb_rate_cms此值在定高时候0 定高时候最后得到_vel_desired.z这个值。

  • 接下来看void AC_PosControl::run_z_controller();

    下面解释了为什么目标高度会跟着实际高度走，实际高度会把目标高度拉着上升。

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
     // do not let target altitude get too far from current altitude
    if (_pos_error.z > _leash_up_z) {
        _pos_target.z = curr_alt + _leash_up_z;
        _pos_error.z = _leash_up_z;
        _limit.pos_up = true;
    }
    if (_pos_error.z < -_leash_down_z) {
        _pos_target.z = curr_alt - _leash_down_z;
        _pos_error.z = -_leash_down_z;
        _limit.pos_down = true;
    }

接下来速度前馈就被加进来了：

1
2
3
4
// add feed forward component
if (_flags.use_desvel_ff_z) {
    _vel_target.z += _vel_desired.z;
}

不知道这个pid是怎么来的，待会打点看看这个pid如何得到？

Copyright © 2017 | Powered by Hexo | Theme by Snippet