无人机三轴稳定控制（2）____根据目标俯仰角，实现俯仰稳定化控制，计算出升降舵输出

这段代码是飞行控制系统中飞机俯仰（Pitch）稳定化的主函数，主要负责根据导航设定的目标俯仰角度（nav_pitch），计算出升降舵（Elevator）的输出值，从而稳定飞机的姿态。

javascript 复制代码

/*
  this is the main pitch stabilization function. It takes the
  previously set nav_pitch and calculates servo_out values to try to
  stabilize the plane at the given attitude.
 */
void Plane::stabilize_pitch()
{
    int8_t force_elevator = takeoff_tail_hold();
    if (force_elevator != 0) {
        // we are holding the tail down during takeoff. Just convert
        // from a percentage to a -4500..4500 centidegree angle
        SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);
        return;
    }

    const float pitch_out = stabilize_pitch_get_pitch_out();
    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
}

以下是详细解析：

cpp 复制代码

 this is the main pitch stabilization function. It takes the  previously set nav_pitch and calculates servo_out values to try to  stabilize the plane at the given attitude. 


void Plane::stabilize_pitch(){    int8_t force_elevator = takeoff_tail_hold();  // 起飞时尾翼保持逻辑    
if (force_elevator != 0) {        // 我们正在起飞时保持尾翼下压。将百分比转换为-4500..4500的centidegree角度        
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);        return;  // 结束函数    
}    
const float pitch_out = stabilize_pitch_get_pitch_out();  // 获取俯仰稳定化输出值    
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);  // 设置升降舵输出

}

功能解析

起飞时尾翼保持逻辑

cpp 复制代码

int8_t force_elevator = takeoff_tail_hold();if (force_elevator != 0) {    SRV_Channels::set_output(S_scaledRV_Channel::k_elevator, 45*force_elevator);    
return;
}

takeoff_tail_hold()：这是一个函数调用，用于检测飞机是否处于起飞阶段，并决定是否需要强制保持尾翼下压。 - 如果返回值force_elevator不等于0，表示需要强制保持尾翼。
返回值范围可能是-1到1（百分比），例如-1表示完全下压，1表示完全上拉。
逻辑： - 如果处于起飞阶段，将force_elevator转换为升降舵的角度值（单位为centidegree，1度=100 centidegree）。
例如，force_elevator = -1时，45 * -1 =4 -5，即升降舵下压45度。- 目的：在飞机起飞时，通过强制下压尾翼帮助飞机平稳离地。

常规俯仰稳定化逻辑

cpp 复制代码

const float pitch_out = stabilize_pitch_get_pitch_out();SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);

stabilize_pitch_get_pitch_out()：这是一个关键函数，用于计算实现俯仰稳定的升降舵输出值。
可能基于以下因素计算：

当前姿态（imu传感器数据）：飞机的实际俯仰角度。
目标姿态（nav_pitch）：导航系统设定的目标俯仰角度。
控制算法（如PID控制器）：根据误差调整输出。- SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out)： - 将计算出的升降舵输出值（pitch_out）写入到升降舵通道伺服。 - 单位为centidegree（-4500到4500，对应-45度到45度）。

代码逻辑总结

起飞阶段： - 如果处于起飞阶段（force_elevator != 0），强制设置升降舵角度以保持尾翼下压。 - 直接返回，跳过后续逻辑。
常规飞行阶段： - 如果不在起飞阶段，调用stabilize_pitch_get_pitch_out()计算升降舵输出值。- 将输出值写入升降舵伺服通道，实现俯仰稳定化。

用于实现：

起飞辅助：帮助飞机在起飞时平稳离地。2. 姿态稳定：在飞行过程中，通过升降舵调整飞机的俯仰姿态。关键术语解释1. Centidegree：百分之一度（1° = 100 centidegree），用于更精确的角度表示。
levatorE（升降舵）：控制飞机俯仰的舵面，位于水平尾翼。
PID控制器：一种常见的反馈控制算法，用于调整输出以减小误差。如果需要更深入的代码分析（如stabilize_pitch_get_pitch_out的实现），可以进一步提供上下文或源码。