C++ 如何使arduino无人机在横摇、俯仰和偏航值下保持稳定？

我目前正在建造一架自动无人机，现在我正试图简单地使它在空中“稳定”，以便它停留在同一地点

我已经做了一些代码，工作正常（我将在下面展示），但我想知道是否有一种方法可以用数学来计算它，使它更加准确？现在我正在改变我的浮动（名为

W_1

）和俯仰和滚动值，以使其稳定，但很难找到“最佳点”。有没有办法用数学解来简化事情

这是我正在使用的代码：

float W_1 = 3;   

void loop ()
{  
if((-10<angle_pitch_output)&&(angle_pitch_output<10))
{
ESC5.write(1400);
ESC9.write(1400); 
Serial.println ("pitch stable");              
}

else if(-10>angle_pitch_output)
{
ESC5.write(1400-(W_1)); 
ESC9.write(1400+(W_1));
Serial.println ("pitch not stable");          
}
else if(angle_pitch_output>10)
{
ESC5.write(1400+(W_1)); 
ESC9.write(1400-(W_1)); 
Serial.println ("pitch not stable");          
}

if((-10<angle_roll_output)&&(angle_roll_output<10))
{
ESC6.write(1400);  
ESC10.write(1400);
Serial.println ("roll stable");          
}
else if(-10>angle_roll_output)
{
ESC6.write(1400-(W_1)); 
ESC10.write(1400+(W_1));
Serial.println ("roll not stable");          
}

else if(angle_roll_output>10)
{
ESC6.write(1400+(W_1)); 
ESC10.write(1400-(W_1)); 
Serial.println ("roll not stable");          
}
}

float W_1=3；
空循环（）
{  
如果（-1010）
{
ESC5.写入（1400+（W_1））；
ESC9.写入（1400-（W_1））；
Serial.println（“音高不稳定”）；
}
如果（-1010）
{
ESC6.写入（1400+（W_1））；
ESC10.写入（1400-（W_1））；
Serial.println（“卷不稳定”）；
}
}

您需要一个PID控制器，作为PID实现，使您能够更精确地调整所有横摇/俯仰/偏航值，而不是任意的增量/减量。在这里，请阅读更多关于PID的信息

是到arduino实现的PID库的链接。

为了您自己（和我们）的利益，请正确缩进您的代码。