Arduino&水火箭开伞航电的制作

#include <Adafruit_Sensor.h>  
#include <Adafruit_BMP280.h> 
#include <basicMPU6050.h> 
#include "U8glib.h" 
#include <Servo.h>
#include <math.h>
#include <Wire.h> 
#include <SPI.h> 
//以上库文件没有的话直接在IDE的库管理里面搜索就能找到了


Servo myservo;
basicMPU6050<> imu;
float H,P_init,H_K,H_P,G_P_X,G_P_Y,G_P_Z,G_Z,H_Z=0;
uint8_t angle;
bool G=0;


uint8_t ON = 0; //开伞舵机角度
uint8_t OFF = 90; //未开伞舵机角度
uint8_t OPEN_H = 2; //下降多少米开伞
uint8_t G_OPEN_Init = 4;//判断发射加速度值
uint8_t Reset = 3; //设置复位按钮引脚
uint8_t LED = 6; //设置LED信号引脚
uint8_t Servo_P = 9; //设置舵机信号引脚
/*定义BMP280引脚*/
#define BMP_SCK 13   //SCL引脚 
#define BMP_MISO 10   //SDO引脚
#define BMP_MOSI 12   //SDA引脚
#define BMP_CS 11     //CSB引脚
Adafruit_BMP280 bmp(BMP_CS, BMP_MOSI, BMP_MISO,  BMP_SCK);  


/////////////////////////////////////////////////////////////////////////
//1. 结构体类型定义
typedef struct 
{
    float LastP;//上次估算协方差 初始化值为0.02
    float Now_P;//当前估算协方差 初始化值为0
    float out;//卡尔曼滤波器输出 初始化值为0
    float Kg;//卡尔曼增益 初始化值为0
    float Q;//过程噪声协方差 初始化值为0.001
    float R;//观测噪声协方差 初始化值为0.543
}KFP;//Kalman Filter parameter

//2.  定义卡尔曼结构体并初始化参数
KFP KFP_height={0.008,0,0,0,0.05,0.3};

/**
 *卡尔曼滤波器
 *@param KFP *kfp 卡尔曼结构体参数
 *   float input 需要滤波的参数的测量值（即传感器的采集值）
 *@return 滤波后的参数（最优值）
 */
 float kalmanFilter(KFP *kfp,float input)
{
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 为下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out;
}
/////////////////////////////////////////////////////////////////////////


//void draw(void);
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);
void draw(){//OLED显示数据
 u8g.drawStr(0, 20, "REAL:");
 u8g.drawStr(0, 40, "MAX:");
 u8g.drawStr(0, 60, "MAX_G:");
  u8g.setPrintPos(64,20);
 u8g.print(H_K,1);
 u8g.setPrintPos(64,40);
 u8g.print(H_Z,1);
 u8g.setPrintPos(80,60);
 u8g.print(G_Z,1);
}


void Init_Referencepressure()
{
  for(char i=0;i<20;i++)
  {
    H_P+=(bmp.readPressure())/100;
    delay(50);
  }
  P_init=H_P/20;
  H_P=0;
}

void setup() {
  Serial.begin(9600);     //设置波特率
  bmp.begin();
  imu.setup();
  imu.setBias();
  Init_Referencepressure();
  u8g.setFont(u8g_font_fub14);//设置字体和字号
  myservo.attach(Servo_P);
  myservo.write(OFF);
  pinMode(LED,OUTPUT);
  pinMode(Reset, INPUT_PULLUP); //按钮引脚设置为输入上拉模式
}

void loop() {
  bool Reset_State = digitalRead(Reset);
  H_K=kalmanFilter(&KFP_height,bmp.readAltitude(P_init));
  G_P_X=abs(imu.ax());
  G_P_Y=abs(imu.ay());
  G_P_Z=abs(imu.az());
 
  if(G_Z<G_P_X){G_Z=G_P_X;}
  if(G_Z<G_P_Y){G_Z=G_P_Y;}
  if(G_Z<G_P_Z){G_Z=G_P_Z;}
  if(H_Z<H_K&&G==1){H_Z=H_K;}
  if(G_P_X>=G_OPEN_Init || G_P_Y>=G_OPEN_Init || G_P_Z>=G_OPEN_Init){G=1;}

  if((H_K<H_Z-OPEN_H)&&G==1)//判断下降X米开伞
  {
    digitalWrite(LED,HIGH);
    myservo.write(ON);
  }

  if(Reset_State==0)//复位按钮按下
  {
    G=0;
    G_Z=0;
    H_Z=0;
    Init_Referencepressure();
    myservo.write(OFF);
    digitalWrite(LED,LOW);
  }

  u8g.firstPage();  
  do {
     draw();
  } while( u8g.nextPage() );
 }

200字以内，仅用于支线交流，主线讨论请采用回复功能。

当前账号的附件下载数量限制如下：

请选择违规类型：

空空如也