无人机定高模式一键起飞和降落逻辑分析

该代码片段展示了无人机定高模式下，一键起飞和降落功能的实现逻辑。

主要功能:

一键降落: 当 commander.keyLand 为真时，调用 flyerAutoLand 函数执行降落操作。
自动降落: 如果 count 达到 500，也调用 flyerAutoLand 函数执行降落操作。
一键起飞: 当 commander.keyFlight 为真时，根据高度调整状态进行相应操作。
- 高度大于120cm: 设置 ctrlValLpf.roll 为 12，并计数。
- 高度小于等于120cm: 设置 setpoint->thrust 为 0， setpoint->mode.z 为 modeAbs。
  - 初始起飞: 设置初始高度为 125cm，并进行 XY 位置调整。
  - XY 位置调整: 根据当前位置和目标位置进行调整，直到误差小于 0.1cm，结束调整。
  - Z 位置调整: 根据当前高度和目标高度 125cm 进行调整，直到误差小于 0.1cm，结束调整。
  - 自主降落: 执行悬停 1 秒后，调用 flyerAutoLand 函数执行降落操作。

代码分析:

void commanderGetSetpoint(setpoint_t *setpoint, state_t *state)
{
    static float maxAccZ = 0.f;

    ctrlDataUpdate(); /*更新控制数据*/

    state->isRCLocked = isRCLocked; /*更新遥控器锁定状态*/

    if (commander.ctrlMode & 0x01) /* 定高模式 */
    {
        if (commander.keyLand) /* 一键降落 */
        {
            flyerAutoLand(setpoint, state);
        }
        else if (count == 500)
        {
            flyerAutoLand(setpoint, state);
        }
        else if (state->position.z > 120.f && count < 500) /* 当高度大于120cm并且在5s内时 */
        {
            ctrlValLpf.roll = 12;
            count++;
        }
        else if (commander.keyFlight && count < 500) /* 按下飞行按键并且时间在5秒内起飞 */
        {
            setpoint->thrust = 0;
            setpoint->mode.z = modeAbs;

            if (initHigh == false)
            {
                initHigh = true;
                isAdjustingPosXY = true;
                errorPosX = 0.f;
                errorPosY = 0.f;
                errorPosZ = 0.f;

                setFastAdjustPosParam(0, 1, 125.f); /* 一键起飞125cm */
            }
            else if (isAdjustingPosXY) /* XY位置调整 */
            {
                errorPosX = state->position.x - 0.f; /* 目标X位置 */
                errorPosY = state->position.y - 1.f; /* 目标Y位置 */

                if (errorPosX < 0.1f && errorPosX > -0.1f && errorPosY < 0.1f && errorPosY > -0.1f)
                {
                    isAdjustingPosXY = false;
                    setFastAdjustPosParam(0, 0, 125.f); /* 悬停在当前位置 */
                }
            }
            else if (isAdjustingPosZ) /* Z位置调整 */
            {
                errorPosZ = state->position.z - 125.f; /* 目标Z位置 */

                if (errorPosZ < 0.1f && errorPosZ > -0.1f)
                {
                    isAdjustingPosZ = false;
                    // 在这里可以实现3秒直线飞行逻辑
                }
            }
            else /* 自主降落 */
            {
                // 在这里可以实现悬停1秒后自主降落逻辑
            }
        }
    }
}

实现3秒直线飞行逻辑:

可以使用一个计时器 flyTime 来记录飞行时间。在每个控制周期内，增加计时器的值。当 flyTime 达到 3 秒时，即可执行直线飞行的逻辑。

// ...
            else if (isAdjustingPosZ) /* Z位置调整 */
            {
                // ...

                if (errorPosZ < 0.1f && errorPosZ > -0.1f)
                {
                    isAdjustingPosZ = false;
                    // 在这里可以实现3秒直线飞行逻辑
                    if (flyTime < 3000) {
                        // 控制飞行器直线飞行
                        setpoint->velocity.x = 1.0f; // 设置x方向速度为1m/s，可以根据实际需求修改
                        setpoint->velocity.y = 0.0f; // 设置y方向速度为0m/s

                        flyTime += controlPeriod; // 增加计时器的值，单位为控制周期
                    } else {
                        // 飞行时间达到3秒，执行其他逻辑或停止飞行
                    }
                }
            }
// ...

实现悬停1秒后自主降落逻辑:

可以使用一个计时器 hoverTime 来记录悬停时间。在每个控制周期内，增加计时器的值。当 hoverTime 达到 1 秒时，即可执行自主降落的逻辑。

// ...
            else /* 自主降落 */
            {
                // 在这里可以实现悬停1秒后自主降落逻辑
                if (hoverTime < 1000) {
                    // 控制飞行器悬停
                    setpoint->velocity.x = 0.0f; // 设置x方向速度为0m/s
                    setpoint->velocity.y = 0.0f; // 设置y方向速度为0m/s

                    hoverTime += controlPeriod; // 增加计时器的值，单位为控制周期
                } else {
                    // 悬停时间达到1秒，执行自主降落逻辑
                    flyerAutoLand(setpoint, state);
                }
            }
// ...

注意:

代码中 controlPeriod 为控制周期，需要根据实际情况进行设置。
速度和计时器的单位需要根据实际需求进行修改。
需要根据实际情况进行代码调试和测试。