/*此版程序作为自己发射火箭测试用，将一些保险验证代码都去掉了，所以一开始定义的一些变量常量没用到，最简单的才是最可靠的。经实际发射，证实此程序识别开伞点可靠。用这个程序请让火箭在发射架上（一定要用发射架，防止火箭在打开电源后发射前倾倒导致意外开伞）就位后再打开系统供电电池电源（9V电池盒自带开关，火箭上掏个小洞对着电池盒上开关，发射前用牙签拨开开关就行），电源打开后由于系统要加载以及要识别SD卡，所以过五秒后再点火。
另外，这个小程序开源，大家想用就用，不反对将此程序用于利润低于30%的商业行为，同时欢迎有兴趣的朋友对这个程序进行改良。另外用此程序发射火箭的朋友请遵守国家的相关法律，本人对用此程序发射火箭所带来的后果不承担任何法律责任。*/
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "SD.h"
#define Vertical_acceleration_judgment_value 15 //设置加速度为多少时判断火箭为已发射的值，本人经验值为15.
#define Launched 1 //用来描述火箭状态为已发射.
#define Not_launched 0 //用来描述火箭状态为尚未发射.
#define Parachute_ignition_switch 3 //用来定义火箭开伞继电器IN1口接在NANO的D3口上
#define Connect HIGH //开伞点火器接通值.
#define Disconnect LOW //开伞点器断开值.
#define Open_the_parachute_inclination -10 //设置箭体与地平线的倾角达到多少度时开伞，可以根据你想要的开伞角度来调整该数值，-10度时火箭刚刚过顶点开始下降约五、六米左右
#define The_correct_flight_angle_limit_of_rocket 30 //设置火箭正常发射的角度极值
unsigned long time;
int16_t ax, ay, az;//三轴加速度
const int chipSelect = 4; //设定SD_CS接口
int OPEN;
int rocket_status; //该变量代表火箭当前的状态，分别为未发射或已发射。
float inclination[3]; //箭体三轴倾角,是一个数组,inclination[0] inclination[1] inclination[2] 分别代表三个轴的倾角
int zz,yy,Current_rocket_inclination; //该变量代表火箭与地平线的倾角.zz和yy是后加上去了，三个轴都采集了，可以再现火箭整个飞行过程中的箭体三维姿态。
int16_t Vertical_acceleration; //火箭垂直加速度,用来判断火箭是否已发射.
int last_rocket_inclination=0; //上一次通过传感器获得的火箭倾角.
//下面为计算三轴角度的一些中间变量
MPU6050 mpu;
uint8_t a1;
uint16_t a2;
uint16_t a3;
uint8_t a4[64];
Quaternion a5;
VectorFloat a6;
//上面为计算三轴角度的一些中间变量
void setup()
{
OPEN=0;
pinMode(Parachute_ignition_switch,OUTPUT); //设置NANO上D3口作为开伞点火开关的输出口.
digitalWrite(Parachute_ignition_switch,Disconnect); //将开伞点火开关初始化为断开.
XXXXXXXgin();
XXXXXXXXXgin(115200);
Init_MPU_6050(); //初始化MPU6050.
rocket_status=Not_launched; //将火箭状态初始化为未发射.
XXXXXgin(chipSelect);
}
void loop()
{
zz=Get_Current_Rocket_Inclination(0);
yy=Get_Current_Rocket_Inclination(2);
Current_rocket_inclination=Filter_Current_Rocket_Inclination(1);//取得火箭当前的倾角,参数这里取值为1,代表X轴.
Vertical_acceleration=XXXXXXtAccelerationX()/2048;//还是因为传感器安装位置的关系，火箭垂直轴为X轴。这里表示的是当前火箭的垂直加速度，程序里用作判断火箭是否已经发射。其实就是表示X轴的当前加速度
File dataFile = SD.open("data.txt",FILE_WRITE);
if (dataFile)
{
XXXXXXXXXXXint(int(Current_rocket_inclination),DEC); //将箭体倾角数据写入文件
XXXXXXXXXXXint(",");
XXXXXXXXXXXint(int(yy),DEC);
XXXXXXXXXXXint(",");
XXXXXXXXXXXint(int(zz),DEC); //箭体姿态的另外两个轴数据写入文件
XXXXXXXXXXXint(",");
XXXXXXXXXXXintln(Vertical_acceleration,DEC); //将火箭垂直加速度写入文件
XXXXXXXXXXXose();
}
if(Current_rocket_inclination<Open_the_parachute_inclination)//判断箭体倾角是否达到程序开头所设置的开伞倾角,如果达到开伞倾角，则进入下面的倾角再判断。
{
Current_rocket_inclination=Filter_Current_Rocket_Inclination(1); //再通过传感器获得一次倾角数据。
if(Current_rocket_inclination<Open_the_parachute_inclination)//通过第二次获得的倾角数据来重复上面的判断，防止他妈的传感器发抽风病输出有问题的数据导致误开伞，这里基本能把所有的噪波都过滤了。如果达到开伞倾角，则进入下面最后一步的判断。
{
Current_rocket_inclination=Filter_Current_Rocket_Inclination(1);
if(Current_rocket_inclination<Open_the_parachute_inclination)//第三次对当前的倾角进行判断，经过三次在不同时间点上对火箭倾角取值应该可以把所有的错误数据都过滤了，下面可以开伞了
{
digitalWrite(Parachute_ignition_switch,Connect);//接通开伞点火开关。
if(OPEN==0)
{
File dataFile = SD.open("data.txt",FILE_WRITE);
if (dataFile)
{
XXXXXXXXXXXintln("Opened"); //在数据文件中写入开伞的标记点
XXXXXXXXXXXose();
}
OPEN=1;
}
}
}
}
}
int Filter_Current_Rocket_Inclination(int b)
{
int temp;
temp=Get_Current_Rocket_Inclination(b);
if(temp==0)
{
return last_rocket_inclination;
}
else
{
last_rocket_inclination=temp;
return last_rocket_inclination;
}
}
/*上面的函数是过滤传感器传回的无效值,参数b可以为0,1,2,分别代表传感器的Z,X,Y三个轴,本程序中取X轴,如果用其它轴来判断,则部分程序需要相应的修改*/
float Get_Current_Rocket_Inclination(int c)
{
float rocket_inclination=0;
a1=XXXXXXtIntStatus();
a3=XXXXXXtFIFOCount();
if (a1&0x02)
{
while (a3<a2) a3=XXXXXXtFIFOCount();
XXXXXXtFIFOBytes(a4, a2);
a3-=a2;
XXXXXXsetFIFO();
XXXXXXpGetQuaternion(&a5, a4);
XXXXXXpGetGravity(&a6, &a5);
XXXXXXpGetYawPitchRoll(inclination, &a5, &a6);
rocket_inclination=inclination[c]*180/M_PI;
return rocket_inclination;
}
}
/*上面的函数是通过传感器来获得当前X轴与地平线的倾角数据,由于不可知的原因,获得的数据可能有噪点,不能拿来直接使用,需要进行适当的过滤.参数c可以为0,1,2,分别代表传感器的Z,X,Y三个轴,本程序中取X轴,如果用其它轴来判断,则部分程序需要相应的修改*/
void Init_MPU_6050()
{
XXXXXXitialize();
XXXXXXtFullScaleGyroRange(3);
XXXXXXtFullScaleAccelRange(3);
XXXXXXtDLPFMode(6);
XXXXXXtDHPFMode(1);
XXXXXXpInitialize();
XXXXXXtDMPEnabled(true);
a2 = XXXXXXpGetFIFOPacketSize();
}
/*上面的函数是对MPU6050进行初始化和一些必要的设置*/
