目录
前言
笔者刚接触太空工程师脚本编写不久，学习开发过几个小项目（导弹、卫星组网）。发现太空工程师的脚本开发中，有相当一部分是以飞船为基础的，而飞船的基础则是飞行，如何飞行，或者说如何更好地飞行是在开发这类脚本中最先要面对的事情。
最开始做完导弹脚本，转手开发卫星组网项目时，我发现我又要重新开始写飞行这部分的代码，也就是工程师们经常提到的“造轮子”，每次都要从造轮子开始，最初我也并没有想过什么。直到有一次一个MOD中的NPC路过我的头顶，把我的惯性抑制器关掉了，我的飞船开始不受控制，这时我才意识到，在失去系统的飞控系统后，自己的飞船有多么脆弱。因此我萌生了写一套自己的飞控系统的想法，随后就诞生了精灵飞控系统。
一、Elf精灵飞控系统是什么？
“精灵”是我给这套系统起的名字。我希望飞船可以像精灵一样灵活自由，甚至在移动过程中拥有自己“思考”的能力。因此我在最初开发这套飞控系统时做的非常复杂，加入了安全高度（通过检测地面斜率来判断是否即将撞击，调整并保证安全高度）、定高度巡航、卫星模式，系统有很多冗余。代码也达到了惊人的3000行，但是很显然这些技术并不成熟，之后我开始做减法，将代码精简到了900多行，保留的内容都是此飞控系统的核心部分。那么一个较为完善的飞控系统应该包含哪些内容呢？
1.获取方块，即获取本网格中各方向推进器以及陀螺仪、远程控制器等关键方块
2.速度控制，即输入目标速度，飞船会自动加速到该速度
3.位置控制，即输入目标位置，飞船会自动移动到该位置
4.方向控制，即输入目标方向，飞船会自动转动到该方向
并且要考虑到如何抵消重力、如何让移动和旋转更平稳，把以上这些综合起来，这就是精灵飞控系统。
二、基于Elf精灵飞控系统的光学避障无人机
如果你的新项目是和无人机移动相关，那么使用了Elf精灵飞控系统，新项目的开发工作会有多简单呢？ 以我开发的一个光学避障无人机为例。
1.避障代码部分
首先是本避障项目特有的部分：障碍物识别程序
private Vector3D ObstacleScan(int scan_frequency, Vector3D target_position, double Min_safe_distance, double first_radius, double max_angle, double surround_radius, double safedis_coeff, double avoiddis);
该代码较长，并不展示全部内容，主要包含初始化扫描矩阵，摄像机扫描，分析矩阵返回值并进行路径规划，这些内容与飞控系统无关，是在飞控系统基础上新开发的内容。
可以看出，上方的函数返回值是Vector3D类型，代表着无人机下一个该移动的坐标，知道了下一步要移动的坐标了，接下来就是移动到该位置即可，而有了Elf精灵飞控系统，你可以直接使用一条函数就可以实现这一步。
我们来看总代码代码部分。
2.总代码部分
private void Obstacle_moveto(Vector3D target_position, double end_dis, double max_speed){
double ang_gap = Elf_KeepShipOrientation(target_position, 0, true, 0.01f, 1.0f, 1);
max_speed = max_speed * Math.Pow(0.001, Math.Abs(ang_gap));
if(cal_positiongap(target_position, my_elf_worldposition) >= end_dis){
Vector3D new_position = ObstacleScan(1, target_position, 10, 6, 42, 12, 2, 10);
if(scan_finish){
Elf_Move_To(new_position, 0.5f, 0.5f, 0.5f, max_speed);
}
}
else{
Elf_Move_To(target_position, 0.5f, 0.5f, 0.5f, max_speed);
}
}
其中包含有“Elf”部分的即是精灵飞控系统的控制函数，我们先不考虑其中的参数是什么意义。在Main函数中，基本上我们只需要一直循环这一函数：
private void Obstacle_moveto(Vector3D target_position, double end_dis, double max_speed);
就可以实现避障。
也就是说，Elf精灵飞控系统为你大大简化了在无人机移动方面的任务量，你需要做的只是告诉无人机去哪里，到了之后该做什么，而怎么去的问题就交给飞控系统来完成。
我们现在来看一下在精灵飞控系统基础上完成的避障无人机有多出色吧。
3.避障完成度演示
基于Elf精灵飞控系统的无人机避障过程演示
4.避障调试过程演示
避障无人机的实际调试视频
三、Elf精灵飞控系统的常用接口函数
1.智能移动
/****************************
函数名： Elf_Smart_Move_TO
作者： 风中的聂先森
日期： 2022-06-09
功能： 智能移动到目标点
输入参数：target_position 目标点的坐标，参考系为世界坐标系
max_speed 最大速度，若不设置该参数，飞船加速到系统限制最大速度后，无法在无重力环境下熄火巡航，浪费能源。
mod不同，最大速度不同，原版最大速度为100m/s，在不同应用场景下也可以更改此参数，比如室内限速5m/s
返回值： 类型(bool)
返回false表示还未到达该点
返回true表示已经到达该点
修改记录：无
备注：为了再次简化使用，选取了使用率较广的参数，若使用者只是需要飞船移动到某个坐标，只要使用这个函数，就能自动调头并且走到目标点了。
****************************/
private bool Elf_Smart_Move_TO(Vector3D target_position, double max_speed);
虽然是叫智能移动，只是对最大速度和旋转模式进行了小小的控制，也算不上智能。该函数参数较少，只需要设置目标点的世界坐标以及限制的最大速度，飞船即可自动飞到该位置。
该函数会返回布尔变量，true表示已经到达该点，false则表示还未到达。
这个函数之所以叫智能移动，是因为我已经为使用者调好了一些适用性广的参数。加速度、角加速度、精度和速度都比较合适。但是会自动掉头对准目标位置，因此在一些特定情况下并不满足使用需求。
不过总体来讲，如果你只是希望飞船移动到某一位置，那这个函数会更加方便你的操作。
2.方向控制
/****************************
函数名： Elf_KeepShipOrientation
作者： 风中的聂先森
日期： 2022-06-11
功能： 调整飞船方向
输入参数：target_position 目标坐标或者目标方向（取决于第三个参数），参考系为世界坐标系
set_dir 想要朝向的方向，0~5代表前后左右上下
Is_position_model 坐标模式，设置为true代表目标为一个世界坐标，设置为false代表目标为一个方向向量
minAngleRad 精度，单位rad，两个向量对齐的精度，一般设置为0.01就已经非常足够了
acc 角加速度，单位rad/s^2，越大调整的越快，个人通常设置为0.3-0.8，并且这个加速度和飞船上的陀螺仪数量有关，设定太大陀螺仪数量不足也达不到该加速度
Orientation_mode 方向模式，0代表正常模式，即移动中不考虑横滚轴是否水平；
1和2是在重力环境下有效，1为半水平模式，即飞船横滚轴会永远水平，只有俯仰和偏航对齐，在前进和后退两方向推进充足时可以采用；
2为完全水平模式，即飞船的横滚轴和俯仰均永远水平，只有偏航会对齐，这样保证飞船的下方永远指向重力，不会有其他部分推进不足导致翻船的可能。
返回值： 类型(double)
返回当前方向与目标方向的误差的一个替代值，并不是完全精确的误差角，为什么不返回true or false呢，因为有很多情况下需要先转到目标方 向，再进行移动，但是这样又会太不平稳
因此我们将最大速度和方向差值写成函数关系，当误差过大时，限制最大速度，当对齐的差不多了，再提高最大速度，这样的移动就会平稳很多，因此需要用到方向差值
修改记录：无
备注：
疑问1：为什么要设置Orientation_mode呢？
回答：应用场景不同，并且飞船的各个方向推进器数量不同，通常星球重力井内使用的飞船底部推进器最多，如果任意翻转，推进器数量不足的方向朝下的话，很有可能支撑不住自身重力
比如你要设计一个垂直挖矿无人机，肯定是向上的推进器非常多，装满矿物后如果有倾斜很可能就炸机了，这时候就需要设置Orientation_mode = 2，在无人机调头前往另一个方向
也能保持水平，使用者也可以分别试试这三个模式（如果你的飞船本身就水平，由于我设置了转向逻辑，先进行偏航角的旋转，因此三个模式测试结果可能大差不差）
****************************/
private double Elf_KeepShipOrientation(Vector3D target_position, int set_dir, bool Is_position_model, double minAngleRad, double acc, int Orientation_mode);
该函数可以将飞船的六个方向（前后左右上下）的某一特定设置方向朝向目标位置或者目标向量。可以控制角加速度和精度。同时可以设置方向模式。目前包含三种模式：正常模式、半水平模式和完全水平模式。
该函数会返回当前方向与目标方向的误差角的一个替代值，注意，这里并不是误差角的确切值。 因为想要得到精确值，需要进行欧拉角到轴角的逆运算，比较复杂因此暂时没有加入，如果有特定项目需要该值可以随时向我反馈。
3.位置控制
/****************************
函数名： Elf_Move_To
作者： 风中的聂先森
日期： 2022-06-09
功能： 通过直线向目标世界坐标点移动（其实由于各个方向的最大加速度不同，并不是直线运动）
输入参数：target_position 目标点的坐标，参考系为世界坐标系
velocity_coeff 速度系数,推荐该值在0.3-0.8范围，数值越小越慢，但越平稳，适合小范围内移动。
acc_coeff 加速度系数，推荐值为0.5左右，数值越小，响应越剧烈，如果过小飞船可能会在目标点范围抖动，该值不能大于等于1
precision 精度，单位m，飞船在到达目标点的精度，通常设置1m，若想要提高精度，速度系数应适量减小，加速度系数适量增大。
max_speed 最大速度，若不设置该参数，飞船加速到系统限制最大速度后，无法在无重力环境下熄火巡航，浪费能源。
mod不同，最大速度不同，原版最大速度为100m/s，在不同应用场景下也可以更改此参数，比如室内限速5m/s
返回值： 类型(bool)
返回false表示还未到达该点
返回true表示已经到达该点
修改记录：无
备注：无
****************************/
private bool Elf_Move_To(Vector3D target_position, double velocity_coeff, double acc_coeff, double precision, double max_speed);
该函数可以控制无人机自动飞到目标位置，并且不会改变飞船朝向。可以设置速度系数、加速度系数、精度和最大速度。该函数会返回布尔变量，true表示已经到达该点，false则表示还未到达。
4.速度控制
/****************************
函数名： Elf_KeepShipVelocity
作者： 风中的聂先森
日期： 2022-06-09
功能： 将飞船的速度逼近为设定值
输入参数：velocity_E 设定速度，参考系为本地坐标系，向右为X，向上为Y，向后为Z，如果设定velocity_E.X=1,velocity_E.Y=0,velocity_E.Z=0,飞船就会向右侧走。
acc_coeff 加速度系数，推荐值为0.5，飞船速度越逼近设定值，加速度应该越小以避免超调，该值不能大于等于1
该值越小，飞船出现抖动的概率越大，但是设置过大，飞船对速度的反应又会太慢。
返回值： 类型(bool)
返回false表示还未到达该速度
返回true表示已经到达该速度
修改记录：无
备注：无
****************************/
private bool Elf_KeepShipVelocity(Vector3D velocity_E, double acc_coeff);
该函数可以将飞船速度控制在设定值。可以设置加速度系数值。并且此加速度系数和上面的位置控制函数加速度是同一参数。该加速度系数并不能大于等于一。原因是我希望当前速度和预期速度的差值越小，加速度越小，因此使用了以下函数进行加速度拟合：
其中最大加速度则是该方向推进器能产生的最大加速度(已计算重力影响)
从该式子中就可以看出，加速度系数acc_coeff不能大于等于1，并且该系数越接近1，预期加速度越小，越不容易产生超调，反之该系数越接近0，移动过程中越容易产生超调，但随之响应速度也会变快，实际体验中，其实0.5甚至0.8就足够了。
5.水平调整
/****************************
函数名： Elf_KeepShipHorizontal
作者： 风中的聂先森
日期： 2022-06-09
功能： 调整飞船方向水平
输入参数：set_down_dir 设定飞船哪一方向朝向重力，0~5分别表示前后左右上下，一般都设置为5，飞船下方朝向重力，即保持水平
minAngleRad 精度，单位rad，两个向量对齐的精度，一般设置为0.01，过于精确或者设置为0，可能会出现抖动，需要相应调整acc
acc 角加速度，单位rad/s^2，越大调整的越快，个人通常设置为0.3-0.8
返回值： 无
修改记录：无
备注：考虑到使用方便，专门写了个保持飞船水平的函数
****************************/
private void Elf_KeepShipHorizontal(int set_down_dir, double minAngleRad, double acc);
该函数并不常用在控制过程中，因为在方向控制函数中如果设置了方向模式为1或者2，飞船则会自动半水平或者全水平。因此该函数常用在程序的开头或者结尾，把飞船调整到水平。
该函数没有返回值。可调整的参数可以参考方向控制函数。
四、常用组合
1.等待状态
Vector3D velocity = new Vector3D();
Elf_KeepShipVelocity(velocity, 0.5f);
无人机的工作思路通常情况是：
等待→进行任务A→等待→进行任务A
而在“等待”这一过程中，如果不进行任何操作，在关闭惯性抑制器的情况下，飞船就会不自主的移动。因此我们通常会这样写程序：
if(标志位A完成){
进行任务A;
}
else if(标志位B完成){
进行任务B;
}
...
else{
//等待状态，保持静止
Vector3D velocity = new Vector3D();
Elf_KeepShipVelocity(velocity, 0.5f);
}
或者
switch(state){
case 0:进行任务0;break;
case 1:进行任务1;break;
...
default:
//等待状态，保持静止
Vector3D velocity = new Vector3D();
Elf_KeepShipVelocity(velocity, 0.5f);break;
}
2.更平稳的朝某坐标移动
double angle_gap = Elf_KeepShipOrientation(target_position, 0, true, 0.01, 0.5, 2);
max_speed = max_speed * Math.Pow(0.001, Math.Abs(angle_gap));
Elf_Move_To(target_position, 0.5, 0.8, 1.0, max_speed);
通常情况下，我们的移动顺序是：
转头朝向目标位置→移动到目标位置
因此到达一个地点分为两步，转向和移动
如果将这两步完全的分开，则整个移动过程会显得特别僵硬
如果将这两步不加限制的合成一步，边转向边移动，如果你的无人机各个方向的推力差异非常大，有可能有减速不及时造成的危险（虽然我已经调的足够好，应该不至于发生）。不过还是有很大可能在姿态还未调整到合适状态的时候飞船就加速到了很大的速度，确实容易产生危险。
因此我们用以上三个式子来实现更平稳和更安全的组合移动
第一步，获得当前方向和目标方向的差值角度替代值（再次强调并不是准确值，并不能用来精确计算）
double angle_gap = Elf_KeepShipOrientation(target_position, 0, true, 0.01, 0.5, 2);
第二步，将设定的最大速度max_speed乘以一个和上一步得到的angle_gap指数相关的系数
max_speed = max_speed * Math.Pow(0.001, Math.Abs(angle_gap));
这一步很关键，用了指数函数，以0.001作为底，angle_gap作为指数，当angle_gap很大的时候，即角度差很大的时候，限制max_speed为一个很小的值，而当角度差越来越小，小到接近0的时候，该系数接近1，而max_speed越接近设定值
第三步，将计算后的max_speed输入到位移控制函数中
Elf_Move_To(target_position, 0.5, 0.8, 1.0, max_speed);
五、我想使用此飞控系统，我该怎么做？
1.复制源代码
将最下面的源代码复制到自己的工程软件中
如果你经常开发太空工程师脚本，还是推荐你使用Visual studio，太空工程师官方的API文件是可以导入Visual studio中，调试起来非常方便。
但是我比较懒，一直用的NotePad++写的代码
实际上，你用什么都可以，记事本都是可以得，只要自己顺手的就可以。
复制完成后，如果你用的较为专业的软件，可以将有关Elf精灵飞控系统的代码折叠，这样就不会在观感上影响开发自己项目了。效果图如下：
2.设置方块分组
必须：
1.将陀螺仪和远程控制器添加到一个组，命名为：ELF_FLIGHT_CONTROL
可选：
2.命名LCD：MY_ELF_LCD_PARA，即可显示获取到的所有物理参数
3.命名LCD：MY_ELF_LCD_INFO，即可显示获取到的方块信息
3.我想随便先试试效果，我该怎么做？
使用以下函数（源代码中已经包含）获取自定义数据中的GPS信息
/****************************
函数名： Elf_GetPosFromcustomData
作者： 风中的聂先森
日期： 2022-06-7
功能： 从自定义数据中获取GPS信息
输入参数：无
返回值： 类型(double)
返回获取到的坐标
修改记录：无
备注：无
****************************/
private Vector3D Elf_GetPosFromcustomData(){
string customData = Me.CustomData;
if (string.IsNullOrEmpty(customData)){
if(Is_Elf_debug)
Echo($"函数：Elf_GetPosFromcustomData，情况：从自定义数据中获取GPS信息失败，数据为空");
return Vector3D.Zero;
}
else{
string[] sArray=customData.Split(':');
Vector3D position = new Vector3D();
position.X = Convert.ToDouble(sArray[2]);
position.Y = Convert.ToDouble(sArray[3]);
position.Z = Convert.ToDouble(sArray[4]);
return position;
}
}
之后我们使用上面讲过的五种常用函数来测试，以智能移动为例，在Main函数中添加以下函数，注意一定要在“获取所有信息”的那部分后面，之后的所有项目也应该在那部分后面。
if(Elf_GetPosFromcustomData() != Vector3D.Zero){
Elf_Smart_Move_TO(Elf_GetPosFromcustomData(), 100);
}
else{
Vector3D velocity = new Vector3D();
Elf_KeepShipVelocity(velocity, 0.5f);
}
该函数会判断自定义数据中是否包含GPS信息，如果包含则会执行智能移动函数，否则就会静止（一定不要忘了写静止状态！）。
接下来，在你的飞船上的编程块里进行编译，然后从GPS信息里复制一个GPS坐标，输入到自定义数据中就可以看到飞船前往该坐标啦！
总结
写到这里已经差不多介绍完了，本文并没有过多的讲解原理，并没有解释如何获得了六个方向的推进器，也并没有解释如何计算预期速度，如何抵消重力，如何进行坐标转换。只是对使用方法进行了简单说明。
我个人的能力十分有限，此飞控系统仍然有非常多的不足，目前并没有适配驾驶舱，因此无法手动控制，而且没有一些花里胡哨的功能，比如输入目标坐标和半径，就可以绕该点进行圆周运动的函数。如果你有一些需求，此飞控系统无法满足，请随时反馈给我，我会及时更新，或者我们可以一起完善它。
最后，谢谢你看到了这里，祝热爱太空工程师的你能有一些收获！
Elf精灵飞控系统-源代码
#region 所有定义 ----- Elf精灵飞控系统 -------------------------------------------------------------------------------------------------
/******************************************************
-----------------------使用说明------------------------
必须：
1.将陀螺仪和远程控制器添加到一个组，命名为：ELF_FLIGHT_CONTROL
可选：
2.命名LCD：MY_ELF_LCD_PARA，即可显示获取到的所有物理参数
3.命名LCD：MY_ELF_LCD_INFO，即可显示获取到的方块信息
******************************************************/
const string ELF_GROUP_TAG = "ELF_FLIGHT_CONTROL";
const string NAME_LCD_my_elf_PARA = "MY_ELF_LCD_PARA";
const string NAME_LCD_my_elf_INFO = "MY_ELF_LCD_INFO";
#region 推进器信息类定义 ----- Elf精灵飞控系统
class thrust_cal{
private double _need;
private double _max;
private double _max_acc;
public thrust_cal(double Need, double Max, double Max_acc){
this._need = Need;
this._max = Max;
this._max_acc = Max_acc;
}
public double Need{
get { return _need; }
set { _need = value; }
}
public double Max{
get { return _max; }
set { _max = value; }
}
public double Max_acc{
get { return _max_acc; }
set { _max_acc = value; }
}
}
#endregion 推进器信息类定义 ----- Elf精灵飞控系统
#region 需要在ELF_FLIGHT_CONTROL组中的方块 ----- Elf精灵飞控系统
private IMyRemoteControl my_elf_remoteControl = null;//远程遥控
private IMyGyro my_elf_gyro = null;//陀螺仪
#endregion 需要在ELF_FLIGHT_CONTROL组中的方块 ----- Elf精灵飞控系统
#region 推进器方块定义 ----- Elf精灵飞控系统
private List<List<IMyThrust>> my_elf_thrusts = new List<List<IMyThrust>>();//六个方向推进器
private List<thrust_cal> thrust_info = new List<thrust_cal>();//推进器参数列表
Vector3D need_thrust = new Vector3D();//各方向需要的推力
#endregion 推进器方块定义 ----- Elf精灵飞控系统
#region 物理参数定义 ----- Elf精灵飞控系统
double my_elf_mass = 0;//飞船质量
Vector3D my_elf_angularvelocity = new Vector3D();//飞船角速度
Vector3D my_elf_linearvelocity = new Vector3D();//飞船线速度
Vector3D my_elf_worldposition = new Vector3D();//飞船世界坐标系下的位置
double my_elf_altitude = 0;//距离地表的高度
double my_elf_Seeelevation = 0;//海平面高度
Vector3D my_elf_gravity = new Vector3D();//重力矢量
Vector3D my_elf_gyroset = new Vector3D();//陀螺仪设定值
Matrix my_elf_Orientaion;//飞船的方向矩阵
#endregion 物理参数定义 ----- Elf精灵飞控系统
#region 其他参数定义 ----- Elf精灵飞控系统
bool Is_my_elf_under_gravity = false;//若为true则飞船处于重力环境下
bool Is_Elf_debug = false;//是否开启debug模式，开启后，会Echo一定的消息
#endregion 其他参数定义 ----- Elf精灵飞控系统
#endregion 所有定义 ----- Elf精灵飞控系统 -------------------------------------------------------------------------------------------------
public Program()
{
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
public void Save()
{
}
public void Main(string argument, UpdateType updateSource)
{
#region 获取所需信息(必须放在最前面) ----- Elf精灵飞控系统
Elf_GetInfo();
#endregion 获取所需信息 ----- Elf精灵飞控系统
}
#region 所有函数 ----- Elf精灵飞控系统 -------------------------------------------------------------------------------------------------
#region 获取自定义数据中的输入（测试用） ----- Elf精灵飞控系统
/****************************
函数名： Elf_GetPosFromcustomData
作者： 风中的聂先森
日期： 2022-06-7
功能： 从自定义数据中获取GPS信息
输入参数：无
返回值： 类型(double)
返回获取到的坐标
修改记录：无
备注：无
****************************/
private Vector3D Elf_GetPosFromcustomData(){
string customData = Me.CustomData;
if (string.IsNullOrEmpty(customData)){
if(Is_Elf_debug)
Echo($"函数：Elf_GetPosFromcustomData，情况：从自定义数据中获取GPS信息失败，数据为空");
return Vector3D.Zero;
}
else{
string[] sArray=customData.Split(':');
Vector3D position = new Vector3D();
position.X = Convert.ToDouble(sArray[2]);
position.Y = Convert.ToDouble(sArray[3]);
position.Z = Convert.ToDouble(sArray[4]);
return position;
}
}
#endregion 获取自定义数据中的输入（测试用） ----- Elf精灵飞控系统
#region 获取飞船信息函数（必须放在Main函数中） ----- Elf精灵飞控系统
/****************************
函数名： Elf_GetInfo
作者： 风中的聂先森
日期： 2022-06-09
功能： 获取信息的函数综合，必须放在Main函数中
输入参数：无
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_GetInfo(){
Elf_Setup();
Elf_CheckSetup();
Elf_GetShipDetails();
Elf_LoadThrusters();
Elf_Get_thrust_Info(1.0);
}
#endregion 获取飞船信息函数（必须放在Main函数中） ----- Elf精灵飞控系统
#region 计算或工具类函数 ----- Elf精灵飞控系统
/****************************
函数名： Elf_cal_positiongap
作者： 风中的聂先森
日期： 2022-06-7
功能： 计算两个坐标之间的距离
输入参数：vector_0 第一个坐标
vector_1 第二个坐标
可以是世界坐标系也可以是本地坐标系，但是两个的坐标系要一致
返回值： 类型(double)
返回计算后的两个坐标之间的距离
修改记录：无
备注：无
****************************/
private double Elf_cal_positiongap(Vector3D vector_0, Vector3D vector_1){
var distance = Vector3D.Subtract(vector_0, vector_1);
return distance.Length();
}
/****************************
函数名： Elf_cal_Anglegap
作者： 风中的聂先森
日期： 2022-06-7
功能： 计算两个向量之间的夹角
输入参数：vector_0 第一个向量
vector_1 第二个向量
可以是世界坐标系也可以是本地坐标系，但是两个的坐标系要一致
返回值： 类型(double)
返回计算后的两个向量之间的夹角
修改记录：无
备注：无
****************************/
private double Elf_cal_Anglegap(Vector3D vector_0, Vector3D vector_1){
vector_0.Normalize();
vector_1.Normalize();
//两向量叉乘
var rot = Vector3D.Cross(vector_0, vector_1);
//两向量点乘
double dot = Vector3D.Dot(vector_0, vector_1);
//计算向量点乘长度
double ang = rot.Length();
ang = Math.Atan2(ang, Math.Sqrt(Math.Max(0.0, 1.0 - ang * ang)));
if (dot < 0)
ang = Math.PI - ang; // compensate for >+/-90
return ang;
}
/****************************
函数名： Elf_EulerBetweenVectors
作者： 风中的聂先森
日期： 2022-06-09
功能： 计算一个向量到另一个向量的欧拉角
输入参数：From 转动前的向量，注意参考系要转换到自身坐标系
To 目标向量，注意参考系要转换到自身坐标系
返回值： 类型(Vector3D)
返回计算后的欧拉角，可以使用以下公式直接输入到陀螺仪函数中，但是一般会乘以一些系数以避免过大或者过小
my_elf_gyro.Yaw = Vector3D.Y;
my_elf_gyro.Pitch = Vector3D.X;
my_elf_gyro.Roll = Vector3D.Z;
修改记录：无
备注：无
****************************/
private Vector3D Elf_EulerBetweenVectors(Vector3D From, Vector3D To){
From.Normalize();
To.Normalize();
var rot = Vector3D.Cross(From, To);
double dot = Vector3D.Dot(From, To);
double ang = rot.Length();
ang = Math.Atan2(ang, Math.Sqrt(Math.Max(0.0, 1.0 - ang * ang)));
if (dot < 0)
ang = Math.PI - ang; // compensate for >+/-90
Vector3D Euler = new Vector3D();
double s=Math.Sin(ang);
double c=Math.Cos(ang);
double t=1-c;
double x = rot.X;
double y = rot.Y;
double z = rot.Z;
if ((x*y*t + z*s) > 0.998) { // north pole singularity detected
Euler.Y = 2*Math.Atan2(x*Math.Sin(ang/2),Math.Cos(ang/2));
Euler.Z = Math.PI/2;
Euler.X = 0;
return Euler;
}
if ((x*y*t + z*s) < -0.998) { // south pole singularity detected
Euler.Y = -2*Math.Atan2(x*Math.Sin(ang/2),Math.Cos(ang/2));
Euler.Z = -Math.PI/2;
Euler.X = 0;
return Euler;
}
Euler.Z = Math.Asin(x * y * t + z * s) ;
Euler.Y = Math.Atan2(y * s- x * z * t , 1 - (y*y+ z*z ) * t);
Euler.X = Math.Atan2(x * s - y * z * t , 1 - (x*x + z*z) * t);
return Euler;
}
#endregion 计算或工具类函数 ----- Elf精灵飞控系统
#region *常用飞船控制函数* ----- Elf精灵飞控系统
/****************************
函数名： Elf_Smart_Move_TO
作者： 风中的聂先森
日期： 2022-06-09
功能： 智能移动到目标点
输入参数：target_position 目标点的坐标，参考系为世界坐标系
max_speed 最大速度，若不设置该参数，飞船加速到系统限制最大速度后，无法在无重力环境下熄火巡航，浪费能源。
mod不同，最大速度不同，原版最大速度为100m/s，在不同应用场景下也可以更改此参数，比如室内限速5m/s
返回值： 类型(bool)
返回false表示还未到达该点
返回true表示已经到达该点
修改记录：无
备注：为了再次简化使用，选取了使用率较广的参数，若使用者只是需要飞船移动到某个坐标，只要使用这个函数，就能自动调头并且走到目标点了。
****************************/
private bool Elf_Smart_Move_TO(Vector3D target_position, double max_speed){
int Orientation_mode = 0;
if(Is_my_elf_under_gravity) Orientation_mode = 2;
double angle_gap = Elf_KeepShipOrientation(target_position, 0, true, 0.01, 0.5, Orientation_mode);
max_speed = max_speed * Math.Pow(0.001, Math.Abs(angle_gap));
return Elf_Move_To(target_position, 0.5, 0.8, 1.0, max_speed);
}
/****************************
函数名： Elf_KeepShipOrientation
作者： 风中的聂先森
日期： 2022-06-11
功能： 调整飞船方向
输入参数：target_position 目标坐标或者目标方向（取决于第三个参数），参考系为世界坐标系
set_dir 想要朝向的方向，0~5代表前后左右上下
Is_position_model 坐标模式，设置为true代表目标为一个世界坐标，设置为false代表目标为一个方向向量
minAngleRad 精度，单位rad，两个向量对齐的精度，一般设置为0.01就已经非常足够了
acc 角加速度，单位rad/s^2，越大调整的越快，个人通常设置为0.3-0.8，并且这个加速度和飞船上的陀螺仪数量有关，设定太大陀螺仪数量不足也达不到该加速度
Orientation_mode 方向模式，0代表正常模式，即移动中不考虑横滚轴是否水平；
1和2是在重力环境下有效，1为半水平模式，即飞船横滚轴会永远水平，只有俯仰和偏航对齐，在前进和后退两方向推进充足时可以采用；
2为完全水平模式，即飞船的横滚轴和俯仰均永远水平，只有偏航会对齐，这样保证飞船的下方永远指向重力，不会有其他部分推进不足导致翻船的可能。
返回值： 类型(double)
返回当前方向与目标方向的误差的一个替代值，并不是完全精确的误差角，为什么不返回true or false呢，因为有很多情况下需要先转到目标方向，再进行移动，但是这样又会太不平稳
因此我们将最大速度和方向差值写成函数关系，当误差过大时，限制最大速度，当对齐的差不多了，再提高最大速度，这样的移动就会平稳很多，因此需要用到方向差值
修改记录：无
备注：
疑问1：为什么要设置Orientation_mode呢？
回答：应用场景不同，并且飞船的各个方向推进器数量不同，通常星球重力井内使用的飞船底部推进器最多，如果任意翻转，推进器数量不足的方向朝下的话，很有可能支撑不住自身重力
比如你要设计一个垂直挖矿无人机，肯定是向上的推进器非常多，装满矿物后如果有倾斜很可能就炸机了，这时候就需要设置Orientation_mode = 2，在无人机调头前往另一个方向
也能保持水平，使用者也可以分别试试这三个模式（如果你的飞船本身就水平，由于我设置了转向逻辑，先进行偏航角的旋转，因此三个模式测试结果可能大差不差）
****************************/
private double Elf_KeepShipOrientation(Vector3D target_position, int set_dir, bool Is_position_model, double minAngleRad, double acc, int Orientation_mode){
if(target_position != Vector3D.Zero){
Vector3D targetDirection;
//如果是坐标模式，则计算目标位置和当前位置的差作为方向向量
if(Is_position_model == true){
targetDirection = Vector3D.Normalize(Vector3D.Subtract(target_position, my_elf_worldposition));
}
//如果是方向模式，则直接赋值方向向量
else{
targetDirection = Vector3D.Normalize(target_position);
}
//根据设定值获取当前方向
Vector3D localCurrent = new Vector3D();
switch(set_dir)
{
case 0:localCurrent = Vector3D.Transform(my_elf_Orientaion.Forward, MatrixD.Transpose(my_elf_Orientaion));break;
case 1:localCurrent = Vector3D.Transform(my_elf_Orientaion.Backward, MatrixD.Transpose(my_elf_Orientaion));break;
case 2:localCurrent = Vector3D.Transform(my_elf_Orientaion.Left, MatrixD.Transpose(my_elf_Orientaion));break;
case 3:localCurrent = Vector3D.Transform(my_elf_Orientaion.Right, MatrixD.Transpose(my_elf_Orientaion));break;
case 4:localCurrent = Vector3D.Transform(my_elf_Orientaion.Up, MatrixD.Transpose(my_elf_Orientaion));break;
case 5:localCurrent = Vector3D.Transform(my_elf_Orientaion.Down, MatrixD.Transpose(my_elf_Orientaion));break;
default:
break;
}
//将目标向量从世界坐标系转换到自身坐标系
var localTarget = Vector3D.Transform(targetDirection, MatrixD.Transpose(my_elf_gyro.WorldMatrix.GetOrientation()));
Vector3D Euler_Sum = new Vector3D();
Vector3D Euler_Angle = Elf_EulerBetweenVectors(localCurrent, localTarget);
Vector3D Euler_gravity = new Vector3D();
if(Is_my_elf_under_gravity){
Vector3D gravityDirection = Vector3D.Normalize(my_elf_gravity);
var localCurrent_Down = Vector3D.Transform(my_elf_Orientaion.Down, MatrixD.Transpose(my_elf_Orientaion));
var localgravity = Vector3D.Transform(gravityDirection, MatrixD.Transpose(my_elf_gyro.WorldMatrix.GetOrientation()));
Euler_gravity = Elf_EulerBetweenVectors(localCurrent_Down, localgravity);
}
else{
Orientation_mode = 0;
}
switch(Orientation_mode){
case 0://正常模式
Euler_Sum.X = Euler_Angle.X;
Euler_Sum.Y = Euler_Angle.Y;
Euler_Sum.Z = Euler_Angle.Z;break;
case 1://半水平模式
Euler_Sum.X = Euler_Angle.X;
Euler_Sum.Y = Euler_Angle.Y + Euler_gravity.Y;
Euler_Sum.Z = Euler_Angle.Z + Euler_gravity.Z;break;
case 2://完全水平模式
Euler_Sum.X = Euler_gravity.X;
Euler_Sum.Y = Euler_Angle.Y + Euler_gravity.Y;
Euler_Sum.Z = Euler_Angle.Z + Euler_gravity.Z;break;
default:
Euler_Sum.X = Euler_Angle.X;
Euler_Sum.Y = Euler_Angle.Y;
Euler_Sum.Z = Euler_Angle.Z;break;
}
if(10*Euler_Sum.Length() > minAngleRad){
//通常我们掉头都是先左右转，之后进行其他方向转动，因此pitch和roll则乘以一个关于yaw的系数，yaw越大，pitch和roll越小
float yaw = -(float)(Math.Sqrt(2 * acc * Math.Abs(Euler_Sum.Y)) * (Euler_Sum.Y / Math.Abs(Euler_Sum.Y)));
float pitch = -(float)(Math.Sqrt(2 * acc * Math.Abs(Euler_Sum.X)) * (Euler_Sum.X / Math.Abs(Euler_Sum.X)) * Math.Pow(0.1, Math.Abs(10*yaw)));
float roll = -(float)(Math.Sqrt(2 * acc * Math.Abs(Euler_Sum.Z)) * (Euler_Sum.Z / Math.Abs(Euler_Sum.Z)) * Math.Pow(0.1, Math.Abs(10*yaw)));
Elf_TurningGyro(pitch, yaw, roll);
return 10*Euler_Sum.Length();
}
else{
Elf_TurningGyro(0, 0, 0);
return 10*Euler_Sum.Length();
}
}
else{
if(Is_Elf_debug)
Echo($"函数：KeepShipOrientation\r\n情况：输入坐标或方向为空");
return 0;
}
}
/****************************
函数名： Elf_Move_To
作者： 风中的聂先森
日期： 2022-06-09
功能： 通过直线向目标世界坐标点移动（其实由于各个方向的最大加速度不同，并不是直线运动）
输入参数：target_position 目标点的坐标，参考系为世界坐标系
velocity_coeff 速度系数,推荐该值在0.3-0.8范围，数值越小越慢，但越平稳，适合小范围内移动。
acc_coeff 加速度系数，推荐值为0.5左右，数值越小，响应越剧烈，如果过小飞船可能会在目标点范围抖动，该值不能大于等于1
precision 精度，单位m，飞船在到达目标点的精度，通常设置1m，若想要提高精度，速度系数应适量减小，加速度系数适量增大。
max_speed 最大速度，若不设置该参数，飞船加速到系统限制最大速度后，无法在无重力环境下熄火巡航，浪费能源。
mod不同，最大速度不同，原版最大速度为100m/s，在不同应用场景下也可以更改此参数，比如室内限速5m/s
返回值： 类型(bool)
返回false表示还未到达该点
返回true表示已经到达该点
修改记录：无
备注：无
****************************/
private bool Elf_Move_To(Vector3D target_position, double velocity_coeff, double acc_coeff, double precision, double max_speed){
if(target_position != Vector3D.Zero){
var localTarget = Vector3D.Transform(Vector3D.Subtract(target_position, my_elf_worldposition), MatrixD.Transpose(my_elf_gyro.WorldMatrix.GetOrientation()));
var localCurrent_X = Vector3D.Transform(my_elf_Orientaion.Right, MatrixD.Transpose(my_elf_Orientaion));
var localCurrent_Y = Vector3D.Transform(my_elf_Orientaion.Up, MatrixD.Transpose(my_elf_Orientaion));
var localCurrent_Z = Vector3D.Transform(my_elf_Orientaion.Backward, MatrixD.Transpose(my_elf_Orientaion));
Vector3D target_distance = new Vector3D();
target_distance.X = Vector3D.Dot(Vector3D.Normalize(localCurrent_X), localTarget);
target_distance.Y = Vector3D.Dot(Vector3D.Normalize(localCurrent_Y), localTarget);
target_distance.Z = Vector3D.Dot(Vector3D.Normalize(localCurrent_Z), localTarget);
if(localTarget.Length() > precision){
Elf_KeepShipVelocity(Elf_DistanceToVelocity(target_distance, velocity_coeff, precision, max_speed), acc_coeff);
return false;
}
else{
Vector3D velocity = new Vector3D();
Elf_KeepShipVelocity(velocity, 0.5f);
return true;
}
}
else{
Vector3D velocity = new Vector3D();
Elf_KeepShipVelocity(velocity, 0.5f);
if(Is_Elf_debug)
Echo($"函数：Elf_Move_To\r\n情况：输入坐标为空，飞船保持静止");
return false;
}
}
/****************************
函数名： Elf_KeepShipHorizontal
作者： 风中的聂先森
日期： 2022-06-09
功能： 调整飞船方向水平
输入参数：set_down_dir 设定飞船哪一方向朝向重力，0~5分别表示前后左右上下，一般都设置为5，飞船下方朝向重力，即保持水平
minAngleRad 精度，单位rad，两个向量对齐的精度，一般设置为0.01，过于精确或者设置为0，可能会出现抖动，需要相应调整acc
acc 角加速度，单位rad/s^2，越大调整的越快，个人通常设置为0.3-0.8
返回值： 无
修改记录：无
备注：考虑到使用方便，专门写了个保持飞船水平的函数
****************************/
private void Elf_KeepShipHorizontal(int set_down_dir, double minAngleRad, double acc){
if(Is_my_elf_under_gravity){
//将重力方向的向量转变成单位向量
Vector3D targetDirection = Vector3D.Normalize(my_elf_gravity);
Vector3D localCurrent = new Vector3D();
switch(set_down_dir)
{
case 0:localCurrent = Vector3D.Transform(my_elf_Orientaion.Forward, MatrixD.Transpose(my_elf_Orientaion));break;
case 1:localCurrent = Vector3D.Transform(my_elf_Orientaion.Backward, MatrixD.Transpose(my_elf_Orientaion));break;
case 2:localCurrent = Vector3D.Transform(my_elf_Orientaion.Left, MatrixD.Transpose(my_elf_Orientaion));break;
case 3:localCurrent = Vector3D.Transform(my_elf_Orientaion.Right, MatrixD.Transpose(my_elf_Orientaion));break;
case 4:localCurrent = Vector3D.Transform(my_elf_Orientaion.Up, MatrixD.Transpose(my_elf_Orientaion));break;
case 5:localCurrent = Vector3D.Transform(my_elf_Orientaion.Down, MatrixD.Transpose(my_elf_Orientaion));break;
default:
break;
}
//将目标向量从世界坐标系转换到自身坐标系
var localTarget = Vector3D.Transform(targetDirection, MatrixD.Transpose(my_elf_gyro.WorldMatrix.GetOrientation()));
Vector3D Euler_Angle = Elf_EulerBetweenVectors(localCurrent, localTarget);
float pitch = -(float)(Math.Sqrt(2 * acc * Math.Abs(Euler_Angle.X)) * (Euler_Angle.X / Math.Abs(Euler_Angle.X)));
float yaw = -(float)(Math.Sqrt(2 * acc * Math.Abs(Euler_Angle.Y)) * (Euler_Angle.Y / Math.Abs(Euler_Angle.Y)));
float roll = -(float)(Math.Sqrt(2 * acc * Math.Abs(Euler_Angle.Z)) * (Euler_Angle.Z / Math.Abs(Euler_Angle.Z)));
if(10*Euler_Angle.Length() >= minAngleRad){
Elf_TurningGyro(pitch, yaw, roll);
}
else{
Elf_TurningGyro(0, 0, 0);
}
}
else{
if(Is_Elf_debug)
Echo($"函数：Elf_KeepShipHorizontal\r\n情况：不在重力井内");
}
}
/****************************
函数名： Elf_KeepShipVelocity
作者： 风中的聂先森
日期： 2022-06-09
功能： 将飞船的速度逼近为设定值
输入参数：velocity_E 设定速度，参考系为本地坐标系，向右为X，向上为Y，向后为Z，如果设定velocity_E.X=1,velocity_E.Y=0,velocity_E.Z=0,飞船就会向右侧走。
acc_coeff 加速度系数，推荐值为0.5，飞船速度越逼近设定值，加速度应该越小以避免超调，该值不能大于等于1
该值越小，飞船出现抖动的概率越大，但是设置过大，飞船对速度的反应又会太慢。
返回值： 类型(bool)
返回false表示还未到达该速度
返回true表示已经到达该速度
修改记录：无
备注：无
****************************/
private bool Elf_KeepShipVelocity(Vector3D velocity_E, double acc_coeff){
Vector3D err_vel;
err_vel = velocity_E - my_elf_linearvelocity;
if(err_vel.Length() < 0.01)
return true;
if(err_vel.X >= 0){
need_thrust.X -= thrust_info[2].Max_acc * (1 - Math.Pow(acc_coeff, err_vel.X)) * my_elf_mass;
}
else{
need_thrust.X += thrust_info[3].Max_acc * (1 - Math.Pow(acc_coeff, -err_vel.X)) * my_elf_mass;
}
if(err_vel.Y >= 0){
need_thrust.Y -= thrust_info[5].Max_acc * (1 - Math.Pow(acc_coeff, err_vel.Y)) * my_elf_mass;
}
else{
need_thrust.Y += thrust_info[4].Max_acc * (1 - Math.Pow(acc_coeff, -err_vel.Y)) * my_elf_mass;
}
if(err_vel.Z >= 0){
need_thrust.Z -= thrust_info[0].Max_acc * (1 - Math.Pow(acc_coeff, err_vel.Z)) * my_elf_mass;
}
else{
need_thrust.Z += thrust_info[1].Max_acc * (1 - Math.Pow(acc_coeff, -err_vel.Z)) * my_elf_mass;
}
thrust_info[0].Need -= need_thrust.Z;
thrust_info[1].Need += need_thrust.Z;
thrust_info[2].Need -= need_thrust.X;
thrust_info[3].Need += need_thrust.X;
thrust_info[4].Need += need_thrust.Y;
thrust_info[5].Need -= need_thrust.Y;
for(int i = 0;i < 6;i++){
if(thrust_info[i].Need < 0) thrust_info[i].Need = 0;
}
for(int i = 0;i < 6;i++){
foreach (IMyThrust thrust in my_elf_thrusts[i])
{
if(thrust_info[i].Need > 0)
thrust.ThrustOverridePercentage = (float)(thrust_info[i].Need / thrust_info[i].Max);
else
thrust.ThrustOverridePercentage = 0.0f;
}
}
return false;
}
#endregion 常用飞船控制函数 ----- Elf精灵飞控系统
#region 其余中间或辅助类函数(几乎不会用到) ----- Elf精灵飞控系统
/****************************
函数名： Elf_TurningGyro
作者： 风中的聂先森
日期： 2022-06-09
功能： 设置陀螺仪数值，向后为Z，向右为X，向上为Y，陀螺仪旋转方向遵循右手坐标系
YAW大于0向左转偏航，PITCH大于0向后转俯仰，ROLL轴大于0向左转滚转
输入参数：pitch 俯仰速度，单位rad/s
yaw 偏航速度，单位rad/s
roll 滚转速度，单位rad/s
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_TurningGyro(float pitch, float yaw, float roll){
my_elf_gyro.Yaw = yaw;
my_elf_gyro.Pitch = pitch;
my_elf_gyro.Roll = roll;
my_elf_gyro.GyroOverride = true;
}
/****************************
函数名： Elf_DistanceToVelocity
作者： 风中的聂先森
日期： 2022-06-09
功能： 将距离转换成速度
输入参数：distance
velocity_coeff
precision
max_speed
返回值： 类型(Vector3D)
返回为移动该距离所需要的预期速度
注意，飞船本地坐标系：向后为Z，向右为X，向上为Y
修改记录：无
备注：无
****************************/
private Vector3D Elf_DistanceToVelocity(Vector3D distance, double velocity_coeff, double precision, double max_speed){
if(distance.Length() < precision){
return new Vector3D();
}
else{
//计算预期速度
Vector3D expect_vel;
if(distance.X > 0){
expect_vel.X = Math.Sqrt(2 * thrust_info[2].Max_acc * distance.X) * velocity_coeff;
}
else{
expect_vel.X = -Math.Sqrt(-2 * thrust_info[3].Max_acc * distance.X) * velocity_coeff;
}
if(distance.Y > 0){
expect_vel.Y = Math.Sqrt(2 * thrust_info[5].Max_acc * distance.Y) * velocity_coeff;
}
else{
expect_vel.Y = -Math.Sqrt(-2 * thrust_info[4].Max_acc * distance.Y) * velocity_coeff;
}
if(distance.Z > 0){
expect_vel.Z = Math.Sqrt(2 * thrust_info[0].Max_acc * distance.Z) * velocity_coeff;
}
else{
expect_vel.Z = -Math.Sqrt(-2 * thrust_info[1].Max_acc * distance.Z) * velocity_coeff;
}
if(expect_vel.Length() >= max_speed){
expect_vel = expect_vel / (expect_vel.Length() / max_speed);
}
return expect_vel;
}
}
/****************************
函数名： Elf_Get_thrust_Info
作者： 风中的聂先森
日期： 2022-06-09
功能： 计算推进器参数的所有值，若有重力则包含重力
输入参数：gravity_coeff 重力系数，一般设置为1.0
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_Get_thrust_Info(double gravity_coeff){
thrust_info.Clear();
//更新推力信息
for(int i = 0;i < 6;i++){
thrust_cal new_thrust_cal = new thrust_cal(0, 0, 0);
thrust_info.Add(new_thrust_cal);
}
//计算最大加速度
for(int i = 0;i < 6;i++){
foreach (IMyThrust thrust in my_elf_thrusts[i])
{
thrust_info[i].Max += thrust.MaxEffectiveThrust;
}
}
if(Is_my_elf_under_gravity){
//将重力方向的向量转变成单位向量
Vector3D gravityDirection = Vector3D.Normalize(my_elf_gravity);
//取得各个方向的方向向量，并从世界坐标系转换到自身坐标系
var localCurrent_X = Vector3D.Transform(my_elf_Orientaion.Right, MatrixD.Transpose(my_elf_Orientaion));
var localCurrent_Y = Vector3D.Transform(my_elf_Orientaion.Up, MatrixD.Transpose(my_elf_Orientaion));
var localCurrent_Z = Vector3D.Transform(my_elf_Orientaion.Backward, MatrixD.Transpose(my_elf_Orientaion));
//将目标向量从世界坐标系转换到自身坐标系
var localgravity = Vector3D.Transform(gravityDirection, MatrixD.Transpose(my_elf_gyro.WorldMatrix.GetOrientation()));
double dot_x = Vector3D.Dot(Vector3D.Normalize(localCurrent_X), localgravity);
double dot_y = Vector3D.Dot(Vector3D.Normalize(localCurrent_Y), localgravity);
double dot_z = Vector3D.Dot(Vector3D.Normalize(localCurrent_Z), localgravity);
Vector3D need_thrust_gravity = new Vector3D();
need_thrust_gravity.X = dot_x * my_elf_mass * my_elf_gravity.Length() * gravity_coeff;
need_thrust_gravity.Y = dot_y * my_elf_mass * my_elf_gravity.Length() * gravity_coeff;
need_thrust_gravity.Z = dot_z * my_elf_mass * my_elf_gravity.Length() * gravity_coeff;
thrust_info[0].Max_acc = (thrust_info[0].Max + need_thrust_gravity.Z) / my_elf_mass;
thrust_info[1].Max_acc = (thrust_info[1].Max - need_thrust_gravity.Z) / my_elf_mass;
thrust_info[2].Max_acc = (thrust_info[2].Max + need_thrust_gravity.X) / my_elf_mass;
thrust_info[3].Max_acc = (thrust_info[3].Max - need_thrust_gravity.X) / my_elf_mass;
thrust_info[4].Max_acc = (thrust_info[4].Max - need_thrust_gravity.Y) / my_elf_mass;
thrust_info[5].Max_acc = (thrust_info[5].Max + need_thrust_gravity.Y) / my_elf_mass;
need_thrust = need_thrust_gravity;
}
}
/****************************
函数名： Elf_CheckSetup
作者： 风中的聂先森
日期： 2022-06-09
功能： 检查是否获取陀螺仪、远程控制器和推进器
输入参数：无
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_CheckSetup(){
string gryo_info;
string remoteControl_info;
string thrustget_info = $"";
if (my_elf_gyro == null){
gryo_info = $"陀螺仪： 离线\r\n";
return;
}
else{
gryo_info = $"陀螺仪： 在线\r\n";
}
if (my_elf_remoteControl == null){
remoteControl_info = $"远程控制器： 离线\r\n";
return;
}
else{
remoteControl_info = $"远程控制器： 在线\r\n";
}
if(my_elf_thrusts.Count() == 6){
thrustget_info = $"推进器参数：\r\n前：{my_elf_thrusts[0].Count()}个 最大推力：{Math.Round(thrust_info[0].Max, 2)}KN\r\n";
thrustget_info += $"后：{my_elf_thrusts[1].Count()}个 最大推力：{Math.Round(thrust_info[1].Max, 2)}KN\r\n";
thrustget_info += $"左：{my_elf_thrusts[2].Count()}个 最大推力：{Math.Round(thrust_info[2].Max, 2)}KN\r\n";
thrustget_info += $"右：{my_elf_thrusts[3].Count()}个 最大推力：{Math.Round(thrust_info[3].Max, 2)}KN\r\n";
thrustget_info += $"上：{my_elf_thrusts[4].Count()}个 最大推力：{Math.Round(thrust_info[4].Max, 2)}KN\r\n";
thrustget_info += $"下：{my_elf_thrusts[5].Count()}个 最大推力：{Math.Round(thrust_info[5].Max, 2)}KN\r\n";
}
string title = $"功能模块信息：\r\n";
string all_info = title + gryo_info + remoteControl_info + thrustget_info;
PrintInfo(all_info);
}
/****************************
函数名： Elf_Setup
作者： 风中的聂先森
日期： 2022-06-09
功能： 在群组中搜索所需方块
输入参数：无
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_Setup(){
var groups = new List<IMyBlockGroup>();
GridTerminalSystem.GetBlockGroups(groups);
foreach (IMyBlockGroup g in groups)
{
if (g.Name.ToLower().Contains(ELF_GROUP_TAG.ToLower()))
{
var blocks = new List<IMyTerminalBlock>();
g.GetBlocks(blocks);
foreach (IMyTerminalBlock block in blocks)
{
Elf_SetupBlocks(block);
}
}
}
}
/****************************
函数名： Elf_SetupBlocks
作者： 风中的聂先森
日期： 2022-06-09
功能： 给搜索到的方块赋值
输入参数：无
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_SetupBlocks(IMyTerminalBlock block){
if (block is IMyGyro)
{
my_elf_gyro = block as IMyGyro;
}
else if (block is IMyRemoteControl)
{
my_elf_remoteControl = block as IMyRemoteControl;
}
}
/****************************
函数名： Elf_LoadThrusters
作者： 风中的聂先森
日期： 2022-06-09
功能： 加载所有的推进器并分成6个组
输入参数：无
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_LoadThrusters(){
List<IMyTerminalBlock> all_thrusts = new List<IMyTerminalBlock>();
GridTerminalSystem.GetBlocksOfType(all_thrusts, block => block is IMyThrust);
my_elf_thrusts.Clear();
//取得飞机各个方向的方向向量，并从世界坐标系转换到自身坐标系
var elf_F = Vector3D.Transform(my_elf_Orientaion.Forward, MatrixD.Transpose(my_elf_Orientaion));
var elf_B = Vector3D.Transform(my_elf_Orientaion.Backward, MatrixD.Transpose(my_elf_Orientaion));
var elf_L = Vector3D.Transform(my_elf_Orientaion.Left, MatrixD.Transpose(my_elf_Orientaion));
var elf_R = Vector3D.Transform(my_elf_Orientaion.Right, MatrixD.Transpose(my_elf_Orientaion));
var elf_U = Vector3D.Transform(my_elf_Orientaion.Up, MatrixD.Transpose(my_elf_Orientaion));
var elf_D = Vector3D.Transform(my_elf_Orientaion.Down, MatrixD.Transpose(my_elf_Orientaion));
List<IMyThrust> my_elf_F_thrusts = new List<IMyThrust>();
List<IMyThrust> my_elf_B_thrusts = new List<IMyThrust>();
List<IMyThrust> my_elf_L_thrusts = new List<IMyThrust>();
List<IMyThrust> my_elf_R_thrusts = new List<IMyThrust>();
List<IMyThrust> my_elf_U_thrusts = new List<IMyThrust>();
List<IMyThrust> my_elf_D_thrusts = new List<IMyThrust>();
int i = 0;
foreach (IMyTerminalBlock block in all_thrusts){
i++;
Matrix block_orientation;
block.Orientation.GetMatrix(out block_orientation);
//取得喷气的方向向量，并从世界坐标系转换到自身坐标系
var block_localCurrent = Vector3D.Transform(block_orientation.Forward, MatrixD.Transpose(my_elf_Orientaion));
double dot = Vector3D.Dot(block_localCurrent, elf_F);
if(dot == 1){
my_elf_F_thrusts.Add(block as IMyThrust);
}
dot = Vector3D.Dot(block_localCurrent, elf_B);
if(dot == 1){
my_elf_B_thrusts.Add(block as IMyThrust);
}
dot = Vector3D.Dot(block_localCurrent, elf_L);
if(dot == 1){
my_elf_L_thrusts.Add(block as IMyThrust);
}
dot = Vector3D.Dot(block_localCurrent, elf_R);
if(dot == 1){
my_elf_R_thrusts.Add(block as IMyThrust);
}
dot = Vector3D.Dot(block_localCurrent, elf_U);
if(dot == 1){
my_elf_U_thrusts.Add(block as IMyThrust);
}
dot = Vector3D.Dot(block_localCurrent, elf_D);
if(dot == 1){
my_elf_D_thrusts.Add(block as IMyThrust);
}
}
my_elf_thrusts.Add(my_elf_F_thrusts);
my_elf_thrusts.Add(my_elf_B_thrusts);
my_elf_thrusts.Add(my_elf_L_thrusts);
my_elf_thrusts.Add(my_elf_R_thrusts);
my_elf_thrusts.Add(my_elf_U_thrusts);
my_elf_thrusts.Add(my_elf_D_thrusts);
}
/****************************
函数名： Elf_GetShipDetails
作者： 风中的聂先森
日期： 2022-06-09
功能： 获取飞船的几乎所有常用的物理参数
输入参数：无
返回值： 无
修改记录：无
备注：无
****************************/
private void Elf_GetShipDetails(){
string mass_details;
string position_details;
string angularvelocity_details;
string linearvelocity_details;
string elevation_details;
string gravity_details;
string gyro_details;
my_elf_remoteControl.Orientation.GetMatrix(out my_elf_Orientaion);
my_elf_mass = my_elf_remoteControl.CalculateShipMass().TotalMass;
mass_details = $"无人机质量：{Math.Round(my_elf_mass, 3)}\r\n";
my_elf_worldposition = my_elf_remoteControl.CenterOfMass;
position_details = $"无人机位置：\r\nX: {Math.Round(my_elf_worldposition.X, 3)} Y: {Math.Round(my_elf_worldposition.Y, 3)} Z: {Math.Round(my_elf_worldposition.Z, 3)}\r\n";
my_elf_angularvelocity = my_elf_remoteControl.GetShipVelocities().AngularVelocity;
angularvelocity_details = $"无人机角速度：\r\nX: {Math.Round(my_elf_angularvelocity.X, 3)} Y: {Math.Round(my_elf_angularvelocity.Y, 3)} Z: {Math.Round(my_elf_angularvelocity.Z, 3)}\r\n";
Vector3D world_satellite_vel = my_elf_remoteControl.GetShipVelocities().LinearVelocity;
my_elf_linearvelocity = Vector3D.Transform(world_satellite_vel, MatrixD.Transpose(my_elf_gyro.WorldMatrix.GetOrientation()));
linearvelocity_details = $"无人机线速度：\r\nX: {Math.Round(my_elf_linearvelocity.X, 3)} Y: {Math.Round(my_elf_linearvelocity.Y, 3)} Z: {Math.Round(my_elf_linearvelocity.Z, 3)}\r\n";
if(!my_elf_remoteControl.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out my_elf_Seeelevation)){
elevation_details = $"不在星球重力范围内\r\n";
gravity_details = $"星球重力为: 0 \r\n";
Is_my_elf_under_gravity = false;
my_elf_altitude = 0;
my_elf_Seeelevation = 0;
my_elf_gravity = Vector3D.Zero;
}
else{
my_elf_remoteControl.TryGetPlanetElevation(MyPlanetElevation.Surface, out my_elf_altitude);
my_elf_gravity = my_elf_remoteControl.GetTotalGravity();
elevation_details = $"地表高度：{Math.Round(my_elf_altitude, 3)} \r\n海平面高度：{Math.Round(my_elf_Seeelevation, 3)} 重力大小： {Math.Round(my_elf_gravity.Length(), 3)}\r\n";
gravity_details = $"星球重力方向：\r\nX: {Math.Round(my_elf_gravity.X, 3)} Y: {Math.Round(my_elf_gravity.Y, 3)} Z: {Math.Round(my_elf_gravity.Z, 3)}\r\n";
Is_my_elf_under_gravity = true;
}
my_elf_gyroset.X = my_elf_gyro.Pitch;
my_elf_gyroset.Y = my_elf_gyro.Yaw;
my_elf_gyroset.Z = my_elf_gyro.Roll;
gyro_details = $"陀螺仪数据：\r\nX: {Math.Round(my_elf_gyroset.X, 3)} Y: {Math.Round(my_elf_gyroset.Y, 3)} Z: {Math.Round(my_elf_gyroset.Z, 3)}\r\n";
string title = $"物理参数信息：\r\n";
string all_details = title + mass_details + position_details + angularvelocity_details + linearvelocity_details + elevation_details + gravity_details + gyro_details;
PrintPara(all_details);
}
#region 打印函数
private IMyTextSurface my_elf_lcd_info = null;
private IMyTextSurface my_elf_lcd_para = null;
private void PrintInfo(string message)
{
if (my_elf_lcd_info == null)
my_elf_lcd_info = GridTerminalSystem.GetBlockWithName(NAME_LCD_my_elf_INFO) as IMyTextSurface;
if (my_elf_lcd_info != null)
SetLCDText(my_elf_lcd_info, message, Color.DarkGreen);
}
private void PrintPara(string message)
{
if (my_elf_lcd_para == null)
my_elf_lcd_para = GridTerminalSystem.GetBlockWithName(NAME_LCD_my_elf_PARA) as IMyTextSurface;
if (my_elf_lcd_para != null)
SetLCDText(my_elf_lcd_para, message, Color.DarkGreen);
}
private void SetLCDText(IMyTextSurface lcd, string text, Color color)
{
lcd.FontColor = color;
lcd.WriteText(text, false);
}
#endregion 打印函数
#endregion 其余中间或辅助类函数 ----- Elf精灵飞控系统
#endregion 所有函数 ----- Elf精灵飞控系统 -------------------------------------------------------------------------------------------------
2415
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
