感谢LZ的分享
发现科创的火箭代码较少,大多只对于单独开伞控制,或为更复杂的姿态控制。所以决定写一套小型火箭入门但完整的火箭代码,包括飞行过程中的三轴加速度,三轴角度,高度记录,和利用姿态和加速度双重判定控制开伞。开伞模式为火工品开伞。所有使用到的原件和安装方式均在下文。希望能对各位有帮助,有任何问题还请指出。
电路一开始的连接参考了彩虹之巅,但是进行了改进。
使用的元件有:1.Arduino NANO,作为入门单片机,基本性能完全够用,价格也可接受,市场价在17元左右。作用为导入代码,根据代码内容控制其他原件,以及接受其他原件的数据。建议买Type-C接口的,就不用买转化头了,手机充电器就可以传数据。
2.MPU6050,可以记录加速度和角速度,注意要用z轴作为竖直轴,因为另外两个轴日常发癫。板子上面一般都印有方向轴,板子焊有原件的那一面为Z轴正方向,安装时朝火箭头锥方向。
3.BMP280,记录气压和温度等,中间由于飞行时受到气流影响可能计算不准确,但是到最高点附近速度很小可以正常读数,故可作为开伞判定方式之一。
4.黑匣子串口数据储存器,用于将所有数据记录到内存卡中。这里之所以不使用Arduino的SD卡模块,是因为实际测试后发现该模块经常出Bug,而且代码也更麻烦,要反复开关文件,所以直接使用该模块记录串口输出数据,试验后发现非常好用。记录同样是计入TF内存卡。只是注意当串口模块连接时不可以将电脑和NANO板连接,会导致串口冲突,要将串口模块的TX和RX两根线暂时拔下再进行代码载入等操作。
5.继电器,用于控制点火头的点燃。
6.其他配件:点火头和电池,我使用了一节9V电池用于电子设备供电,和一节1.5V电池用于点燃点火头。
电路连接图如下,画图使用了easyEDA:
注意部分接口为共享,所以需要买一拖二和一拖四的杜邦线连接。除了MPU以外,其他原件没有方向要求。
上代码:
C++#include <I2Cdev.h>
#include <Wire.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <Adafruit_BMP280.h>
MPU6050 mpu;
Adafruit_BMP280 bmp;
const int relayPin = 9; // Relay control pin
const float AScale = 16.0 / 32768.0 * 9.81; // ±16g 对应的比例因子
const float GScale = 250.0 / 32768.0 * (PI / 180.0); // ±250°/s 对应的比例因子
unsigned long currentTime, lastTime;
float deltaTime;
int16_t ax, ay, az, gx, gy, gz;
float pitch, roll, yaw;
float q[4] = {1.0, 0.0, 0.0, 0.0}; // 初始化四元数
float InitPressure = 0; // 存储发射时的基准气压
float AccZ = 0;
float AltBMP = 0;
float AltMPU = 0;
float velocity = 0;
long ax_offset, ay_offset, az_offset;
long gx_offset, gy_offset, gz_offset;
long yaw_offset, pitch_offset, roll_offset;
float Kp = 30.0f; // 比例增益
float Ki = 0.01f; // 积分增益
void setup() {
Serial.begin(9600);
while (!Serial) {
; // wait for serial port to connect.
}
pinMode(relayPin, OUTPUT);
digitalWrite(relayPin, LOW);
Wire.begin();
delay(5000);
Init_MPU_6050();
Init_BMP280();
lastTime = millis();
Serial.println("Preparation done! Ready to fly!");
Serial.println("Time Altitude Acceleration attitude");
}
void loop() {
if (millis() - lastTime >= 10) {
currentTime = millis();
deltaTime = (float)(currentTime - lastTime) / 1000.00; // get deltaT
lastTime = currentTime;
mpu.getAcceleration(&ax, &ay, &az);
mpu.getRotation(&gx, &gy, &gz);
ax = (ax - ax_offset) * AScale;
ay = (ay - ay_offset) * AScale;
az = (az - az_offset) * AScale;
gx = (gx - gx_offset) * GScale;
gy = (gy - gy_offset) * GScale;
gz = (gz - gz_offset) * GScale;
Mahony_update(ax, ay, az, gx, gy, gz, deltaTime);
// 更新 BMP 高度
AltBMP = bmp.readAltitude(InitPressure); // 传入基准气压计算高度
yaw = CurrentInclination(0) - yaw_offset;
pitch = CurrentInclination(1) - pitch_offset;
roll = CurrentInclination(2) - roll_offset;
// 将数据写入黑匣子模块
Serial.print(millis()/1000.0);
Serial.print("s, AltitudeBMP: ");
Serial.print(AltBMP);
Serial.print("m, Ax: ");
Serial.print(ax);
Serial.print(", Ay: ");
Serial.print(ay);
Serial.print(", Az: ");
Serial.print(az);
Serial.print(", Yaw: ");
Serial.print(yaw);
Serial.print(", Pitch: ");
Serial.print(pitch);
Serial.print(", Roll: ");
Serial.println(roll);
if((yaw < -10 || yaw > 10 || pitch < -10 || pitch > 10) && az < 0 && millis() > 10000)
{
yaw = CurrentInclination(0) - yaw_offset;
pitch = CurrentInclination(1) - pitch_offset;
roll = CurrentInclination(2) - roll_offset;
mpu.getAcceleration(&ax, &ay, &az);
az = (az - az_offset) * AScale;
if((yaw < -10 || yaw > 10 || pitch < -10 || pitch > 10) && az < 0 && millis() > 10000) //二次判断防止误开
{
digitalWrite(relayPin, HIGH); // 释放降落伞
Serial.println("Parachute released!");
}
}
}
}
void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
float recipNorm;
float vx, vy, vz;
float ex, ey, ez; //error terms
float qa, qb, qc;
static float ix = 0.0, iy = 0.0, iz = 0.0; //integral feedback terms
float tmp;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
tmp = ax * ax + ay * ay + az * az;
if (tmp > 0.0)
{
// Normalise accelerometer (assumed to measure the direction of gravity in body frame)
recipNorm = 1.0 / sqrt(tmp);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity in the body frame (factor of two divided out)
vx = q[1] * q[3] - q[0] * q[2];
vy = q[0] * q[1] + q[2] * q[3];
vz = q[0] * q[0] - 0.5f + q[3] * q[3];
// Error is cross product between estimated and measured direction of gravity in body frame
// (half the actual magnitude)
ex = (ay * vz - az * vy);
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
// Compute and apply to gyro term the integral feedback, if enabled
if (Ki > 0.0f) {
ix += Ki * ex * deltat; // integral error scaled by Ki
iy += Ki * ey * deltat;
iz += Ki * ez * deltat;
gx += ix; // apply integral feedback
gy += iy;
gz += iz;
}
// Apply proportional feedback to gyro term
gx += Kp * ex;
gy += Kp * ey;
gz += Kp * ez;
}
// Integrate rate of change of quaternion, q cross gyro term
deltat = 0.5 * deltat;
gx *= deltat; // pre-multiply common factors
gy *= deltat;
gz *= deltat;
qa = q[0];
qb = q[1];
qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz);
q[1] += (qa * gx + qc * gz - q[3] * gy);
q[2] += (qa * gy - qb * gz + q[3] * gx);
q[3] += (qa * gz + qb * gy - qc * gx);
// renormalise quaternion
recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] * recipNorm;
q[1] = q[1] * recipNorm;
q[2] = q[2] * recipNorm;
q[3] = q[3] * recipNorm;
}
// Function to get current inclination
float CurrentInclination(int axis) {
switch(axis) {
case 0: // Yaw
return atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), 1.0f - 2.0f * (q[1] * q[1] + q[2] * q[2])) * 180.0 / M_PI;
case 1: // Pitch
return asin(2.0f * (q[0] * q[2] - q[3] * q[1])) * 180.0 / M_PI;
case 2: // Roll
return atan2(2.0f * (q[0] * q[3] + q[1] * q[2]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3])) * 180.0 / M_PI;
default:
return 0.0f; // Invalid axis
}
}
// Function to initialize MPU6050
void Init_MPU_6050() {
mpu.initialize();
if (!mpu.testConnection()) {
Serial.println("MPU6050 connection failed!");
while (1);
}
mpu.setFullScaleGyroRange(0);
mpu.setFullScaleAccelRange(3);
Serial.println("MPU6050 initialized successfully!");
}
// Function to initialize BMP280
void Init_BMP280() {
if (bmp.begin(0x76)) {
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
Adafruit_BMP280::SAMPLING_X2,
Adafruit_BMP280::SAMPLING_X16,
Adafruit_BMP280::FILTER_X4,
Adafruit_BMP280::STANDBY_MS_1);
InitPressure = bmp.readPressure() / 100.0F; // 转换为 hPa(1 Pa = 0.01 hPa)
Serial.print("Initial Pressure: ");
Serial.println(InitPressure);
Serial.println("BMP280 initialized successfully!");
} else {
Serial.println("BMP280 initialization failed!");
}
}
代码很长,但是测试后发现数据记录较为稳定。代码载入前要下载#include里的所有库。该代码可以直接上箭使用,但是建议学明白Arduino的基本用法,这样可以自定义修改,比如给自己预留安装时间,更改开伞判定等。
将代码传入板子之前先选好板子类型和串口:
原件安装方式其实塔式更好,有能力的话可以自己画板子,但是能力限制我就只是尽可能占用少的空间摆下的原则放置的,自己打印了摆放架,仅供参考。其实是不建议使用我的方法的,插线会难插到怀疑人生。。。:
坏处是内存卡记录的话如果火箭丢了数据就都没有了,所以下一次设计我会尝试使用通讯,并且改为自己画板子和塔式安装。
第一次制作航电,有任何问题还请各位多多指教,非常感谢!
之前我用esp32和mpu6050做了一个加速度计,但是在静止时数据也在大幅度变化,不是正常的小扰动,百思不得其解
引用ManGo_Mouse发表于2楼的内容之前我用esp32和mpu6050做了一个加速度计,但是在静止时数据也在大幅度变化,不是正常的小扰动
是的,所以我加了Mahony起滤波作用,如果还不准可以原件单独校准一下
200字以内,仅用于支线交流,主线讨论请采用回复功能。