ACM simple功能简析

　　在分析simple功能先，先复习下二维空间坐轴或向量的旋转，摘自WIKI。

　　在讨论旋转的时候理解参照系是重要的。从一个观点，你可以保持坐标轴固定旋转向量从另一个观点，你可以保持向量固定旋转坐标系。

　　在第一种观点看来，坐标或向量关于原点的逆时针旋转；或者从第二种观点看来，平面或轴关于原点的顺时针旋转。这里的 (x,y) 被旋转了 θ 并希望知道旋转后的坐标 (x',y'):

　　

或

　　

　　

　　平面或轴关于原点的逆时针旋转，在新平面中的坐标将顺时针旋转到新坐标。在这种情况下，如果在旧平面中的坐标是 (x,y)，同一个向量在新平面中的坐标是 (x',y')，则:

　　

或

　　

　　

向量 (x, y) 的大小同于向量 (x′, y′) 的大小。

 　　ACM中simple功能的实现代码：

 1     //read_radio();
 2     if(do_simple && new_radio_frame){
 3         new_radio_frame = false;
 4         simple_timer++;
 5         //算出当前航向和初始航向之向的夹角
 6 　　　   //机体顺时钟旋转delta>0，逆时钟旋转delta<0。决定于载机坐标系的编排。
 7         int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
 8         //分解成roll pitch的独立动作，大概是为了简化运算
 9         //不这样分解时，会是什么情况呢？？？
10         if (simple_timer == 1){
11             // roll
12             simple_cos_x = sin(radians(90 - delta));
14         }else if (simple_timer > 2){
15             // pitch
16             simple_sin_y = cos(radians(90 - delta));
17             simple_timer = 0;
18         }
19         //由于上面算夹角的关系，由机体当前坐标系要逆时钟旋转delta，得到
20         //初始的机体坐标系。所以这里采用逆时钟旋转的旋转向量。
21         // Rotate input by the initial bearing
22         control_roll   =  g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y;
23         control_pitch  = -g.rc_1.control_in * simple_sin_y + g.rc_2.control_in * simple_cos_x;
24         
25         //更新上面算出的roll pitch为控制输出量
26         g.rc_1.control_in = control_roll;
27         g.rc_2.control_in = control_pitch;
28     }

　　由上面的代码可以看出，simple功能是把遥控的roll pitch的控制量旋转delta后，得到在初始航向对应的坐标系中对应的roll pitch控制量。实际运行的效果就是，不管当前机头向何处，打副翼和俯仰杆时，四轴的滚转和俯仰动作都是保持初始化时的滚转和俯仰动作方向。只要操作者的方向未改变，则永远相当于是对尾控制。

　　有一点要注意的是，当机架初始方向不是正北的话，要注意在航向稳定后，再解锁。飞控会在解锁时，重新记录当前的航向。不然飞控记录的初始航向和机架当前方向不一致，simple功能用起来肯定就会让你觉得怪怪的。

　　至于能不能不分解成roll pitch的独立动作，则需要改代码后验证，有空试试看是什么效果。

转载于:https://www.cnblogs.com/ChangeYoung/archive/2012/01/10/2317873.html