让 MWC 飞控支持在 Baro 定高模式下起飞的思路。

布丁

已知 MWC 飞控在 Baro 定高模式下起飞，有这样的问题：

对于不同的机架，或同一机架搭载不同重量电池、设备。在不知道悬停时油门位置的情况下，可能 Baro 定高功能很不好用，甚至根本定不住高度(定高需要一个很大的 I，使得油门可以以一个能接受的变化速度，找到悬停油门；但是 I 过大，使得 PID 很难调)。


解决思路：

一 先看 MWC 的定高算法
MWC 的定高算法，首先在源码里定义了宏：
ALT_HOLD_THROTTLE_MIDPOINT

    #define ALT_HOLD_THROTTLE_MIDPOINT        1500  // in us    - if uncommented, this value is used in ALT_HOLD for throttle stick middle point instead of initialThrottleHold parameter.

而定高时油门的计算是：

rcCommand[THROTTLE] = initialThrottleHold + BaroPID;

其中 initialThrottleHold 来自于宏 ALT_HOLD_THROTTLE_MIDPOINT ：

        #if defined(ALT_HOLD_THROTTLE_MIDPOINT)
          initialThrottleHold = ALT_HOLD_THROTTLE_MIDPOINT;
        #else
          initialThrottleHold = rcCommand[THROTTLE];
        #endif
        BaroPID = 0;

二 再看 ArduCopter 定高算法的处理
(感谢 flyzero 同学的文章：APM源码分析之 油门跟踪)

最终姿态油门输出=p+i+d + _throttle_hover(悬停值，这个值时刻在计算修正)

AP_PosControl.h 文件中的相关代码：

#define POSCONTROL_THROTTLE_HOVER               450.0f  // default throttle required to maintain hover

    // parameters
    AP_Float    _throttle_hover;        // estimated throttle required to maintain a level hover

    /// set_throttle_hover - update estimated throttle required to maintain hover
    void set_throttle_hover(float throttle) { _throttle_hover = throttle; }

AP_PosControl.cpp 文件中的相关代码：

    // To-Do: we had a contraint here but it's now removed, is this ok?  with the motors library handle it ok?
    _attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);

Attitude.pde 文件中的相关代码：

// update_thr_cruise - update throttle cruise if necessary
//  should be called at 100hz
static void update_thr_cruise()
{
    // ensure throttle_avg has been initialised
    if( throttle_avg == 0 ) {
        throttle_avg = g.throttle_cruise;
        // update position controller
        pos_control.set_throttle_hover(throttle_avg);        // 如果遥控器油门持续最低，则设置为怠速。
    }

    // if not armed or landed exit
    if (!motors.armed() || ap.land_complete) {
        return;
    }

    // get throttle output
    int16_t throttle = g.rc_3.servo_out;

    // calc average throttle if we are in a level hover
    // 悬停时计算油门输出平均值, 即悬停值。
    if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
        throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f;
        g.throttle_cruise = throttle_avg;
        // update position controller
        pos_control.set_throttle_hover(throttle_avg);
    }
}

三 解决思路

MWC 中的 rcData[THROTTLE] 是遥控器原始数据。

MWC 中的 alt.vario 与 APM 的 climb_rate 相似。

MWC 中的 att.angle[ROLL] 与 APM 的 ahrs.roll_sensor 相似。

MWC 中的 att.angle[PITCH] 与 APM 的 ahrs.pitch_sensor 相似。

如果在 MWC 中 initialThrottleHold 的值，是动态变化的，与 ArduCopter 程序里面的 _throttle_hover 变量的算法相似。
是否就能明显改善 MWC 在定高模式起飞下的体验呢？

在 moz8 发布，请勿转载至 5imx 等无共享精神的论坛。

布丁

修改的代码主要在 MultiWii.cpp

    // 悬停油门刷新
    if ((throttle > MINCHECK) && (abs(alt.vario) < 15) && abs(att.angle[ROLL]) < 500 && abs(att.angle[PITCH]) < 500)
    {
      throttle_avg = throttle_avg - (throttle_avg >> 7) + ((uint32_t)(throttle) << 3);
      set_throttle_hover((uint16_t)(throttle_avg >> 10));
      initialThrottleHold = _throttle_hover;
    }

    rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
   
    #ifdef ALT_HOLD_THROTTLE_MIDPOINT
      if (lLockBaroTakeoff) // 定高模式起飞，当油门未超过一半时，油门输出最小值。
      {
        rcCommand[THROTTLE] = MINTHROTTLE;
      }
    #endif

    throttle = rcCommand[THROTTLE];

神经病

高手，我完全看不懂，帮顶了

GIMAN

jiaodabo88

不懂  帮顶个

左瘸子

帮顶

Listenning

楼主辛苦了，感谢楼主

958061720

不知道现在还有没有人跟我一样还在看这个

suyijiang

学习啦赞，谢谢分享

大同江

kvcool

学习了

姜宝林

感谢分享，慢慢学习。