最新正点原子MiniFly Firmware V1(3)，C C++进阶

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上C C++开发知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、讲解视频，并且后续会持续更新

如果你需要这些资料，可以戳这里获取

{
bool testStatus = true;

if (!isInit)
{
	printf("Uninitialized\n");
	testStatus = false;
}

testStatus&=isBaroPresent;

return testStatus;

}

/计算方差和平均值/
static void sensorsCalculateVarianceAndMean(BiasObj* bias, Axis3f* varOut, Axis3f* meanOut)
{
u32 i;
int64_t sum[3] = {0};
int64_t sumsq[3] = {0};

for (i = 0; i < SENSORS_NBR_OF_BIAS_SAMPLES; i++)
{
	sum[0] += bias->buffer[i].x;
	sum[1] += bias->buffer[i].y;
	sum[2] += bias->buffer[i].z;
	sumsq[0] += bias->buffer[i].x * bias->buffer[i].x;
	sumsq[1] += bias->buffer[i].y * bias->buffer[i].y;
	sumsq[2] += bias->buffer[i].z * bias->buffer[i].z;
}

varOut->x = (sumsq[0] - ((int64_t)sum[0] * sum[0]) / SENSORS_NBR_OF_BIAS_SAMPLES);
varOut->y = (sumsq[1] - ((int64_t)sum[1] * sum[1]) / SENSORS_NBR_OF_BIAS_SAMPLES);
varOut->z = (sumsq[2] - ((int64_t)sum[2] * sum[2]) / SENSORS_NBR_OF_BIAS_SAMPLES);

meanOut->x = (float)sum[0] / SENSORS_NBR_OF_BIAS_SAMPLES;
meanOut->y = (float)sum[1] / SENSORS_NBR_OF_BIAS_SAMPLES;
meanOut->z = (float)sum[2] / SENSORS_NBR_OF_BIAS_SAMPLES;

}
/传感器查找偏置值/
static bool sensorsFindBiasValue(BiasObj* bias)
{
bool foundbias = false;

if (bias->isBufferFilled)
{
	
	Axis3f mean;
	Axis3f variance;
	sensorsCalculateVarianceAndMean(bias, &variance, &mean);

	if (variance.x < GYRO_VARIANCE_BASE && variance.y < GYRO_VARIANCE_BASE && variance.z < GYRO_VARIANCE_BASE)
	{
		bias->bias.x = mean.x;
		bias->bias.y = mean.y;
		bias->bias.z = mean.z;
		foundbias = true;
		bias->isBiasValueFound= true;
	}else
		bias->isBufferFilled=false;
}
return foundbias;

}

/* 传感器初始化 */
void sensorsInit(void)
{
if(isInit) return;

sensorsDataReady = xSemaphoreCreateBinary();/*创建传感器数据就绪二值信号量*/
sensorsBiasObjInit(&gyroBiasRunning);
sensorsDeviceInit();	/*传感器器件初始化*/
sensorsInterruptInit();	/*传感器中断初始化*/

isInit = true;

}
/设置传感器从模式读取/
static void sensorsSetupSlaveRead(void)
{
mpu6500SetSlave4MasterDelay(19); // 从机读取速率: 100Hz = (1000Hz / (1 + 9))

mpu6500SetI2CBypassEnabled(false);	//主机模式
mpu6500SetWaitForExternalSensorEnabled(true); 	
mpu6500SetInterruptMode(0); 		// 中断高电平有效
mpu6500SetInterruptDrive(0); 		// 推挽输出
mpu6500SetInterruptLatch(0); 		// 中断锁存模式(0=50us-pulse, 1=latch-until-int-cleared)
mpu6500SetInterruptLatchClear(1); 	// 中断清除模式(0=status-read-only, 1=any-register-read)
mpu6500SetSlaveReadWriteTransitionEnabled(false); // 关闭从机读写传输
mpu6500SetMasterClockSpeed(13); 	// 设置i2c速度400kHz

#ifdef SENSORS_ENABLE_MAG_AK8963
if (isMagPresent)
{
// 设置MPU6500主机要读取的寄存器
mpu6500SetSlaveAddress(0, 0x80 | AK8963_ADDRESS_00); // 设置磁力计为0号从机
mpu6500SetSlaveRegister(0, AK8963_RA_ST1); // 从机0需要读取的寄存器
mpu6500SetSlaveDataLength(0, SENSORS_MAG_BUFF_LEN); // 读取8个字节(ST1, x, y, z heading, ST2 (overflow check))
mpu6500SetSlaveDelayEnabled(0, true);
mpu6500SetSlaveEnabled(0, true);
}
#endif

if (isBaroPresent && baroType == BMP280)
{
	// 设置MPU6500主机要读取BMP280的寄存器
	mpu6500SetSlaveAddress(1, 0x80 | BMP280_I2C_ADDR);		// 设置气压计状态寄存器为1号从机
	mpu6500SetSlaveRegister(1, BMP280_STAT_REG);			// 从机1需要读取的寄存器
	mpu6500SetSlaveDataLength(1, SENSORS_BARO_STATUS_LEN);	// 读取1个字节
	mpu6500SetSlaveDelayEnabled(1, true);
	mpu6500SetSlaveEnabled(1, true);

	mpu6500SetSlaveAddress(2, 0x80 | BMP280_I2C_ADDR);		// 设置气压计数据寄存器为2号从机
	mpu6500SetSlaveRegister(2, BMP280_PRESSURE_MSB_REG);	// 从机2需要读取的寄存器
	mpu6500SetSlaveDataLength(2, SENSORS_BARO_DATA_LEN);	// 读取6个字节
	mpu6500SetSlaveDelayEnabled(2, true);
	mpu6500SetSlaveEnabled(2, true);
}
if (isBaroPresent && baroType == SPL06)
{
	// 设置MPU6500主机要读取SPL06的寄存器
	mpu6500SetSlaveAddress(1, 0x80 | SPL06_I2C_ADDR);		// 设置气压计状态寄存器为1号从机
	mpu6500SetSlaveRegister(1, SPL06_MODE_CFG_REG);			// 从机1需要读取的寄存器
	mpu6500SetSlaveDataLength(1, SENSORS_BARO_STATUS_LEN);	// 读取1个字节
	mpu6500SetSlaveDelayEnabled(1, true);
	mpu6500SetSlaveEnabled(1, true);

	mpu6500SetSlaveAddress(2, 0x80 | SPL06_I2C_ADDR);		// 设置气压计数据寄存器为2号从机
	mpu6500SetSlaveRegister(2, SPL06_PRESSURE_MSB_REG);		// 从机2需要读取的寄存器
	mpu6500SetSlaveDataLength(2, SENSORS_BARO_DATA_LEN);	// 读取6个字节
	mpu6500SetSlaveDelayEnabled(2, true);
	mpu6500SetSlaveEnabled(2, true);
}

mpu6500SetI2CMasterModeEnabled(true);	//使能mpu6500主机模式
mpu6500SetIntDataReadyEnabled(true);	//数据就绪中断使能

}

/**

• 往方差缓冲区（循环缓冲区）添加一个新值，缓冲区满后，替换旧的的值
  /
  static void sensorsAddBiasValue(BiasObj bias, int16_t x, int16_t y, int16_t z)
  {
  bias->bufHead->x = x;
  bias->bufHead->y = y;
  bias->bufHead->z = z;
  bias->bufHead++;

  if (bias->bufHead >= &bias->buffer[SENSORS_NBR_OF_BIAS_SAMPLES])
  {
  bias->bufHead = bias->buffer;
  bias->isBufferFilled = true;
  }
  }

/**

• 根据样本计算重力加速度缩放因子
  */
  static bool processAccScale(int16_t ax, int16_t ay, int16_t az)
  {
  static bool accBiasFound = false;
  static uint32_t accScaleSumCount = 0;

  if (!accBiasFound)
  {
  accScaleSum += sqrtf(powf(ax * SENSORS_G_PER_LSB_CFG, 2) + powf(ay * SENSORS_G_PER_LSB_CFG, 2) + powf(az * SENSORS_G_PER_LSB_CFG, 2));
  accScaleSumCount++;

   if (accScaleSumCount == SENSORS_ACC_SCALE_SAMPLES)
   {
   	accScale = accScaleSum / SENSORS_ACC_SCALE_SAMPLES;
   	accBiasFound = true;
   }
  

  }

  return accBiasFound;
  }

/**

• 计算陀螺方差
  */
  static bool processGyroBias(int16_t gx, int16_t gy, int16_t gz, Axis3f *gyroBiasOut)
  {
  sensorsAddBiasValue(&gyroBiasRunning, gx, gy, gz);

  if (!gyroBiasRunning.isBiasValueFound)
  {
  sensorsFindBiasValue(&gyroBiasRunning);
  }

  gyroBiasOut->x = gyroBiasRunning.bias.x;
  gyroBiasOut->y = gyroBiasRunning.bias.y;
  gyroBiasOut->z = gyroBiasRunning.bias.z;

  return gyroBiasRunning.isBiasValueFound;
  }

/处理气压计数据/
void processBarometerMeasurements(const u8 *buffer)
{
static float temp;
static float pressure;

if (baroType == BMP280)
{
	// Check if there is a new data update
	if ((buffer[0] & 0x08)) /*bit3=1 转换完成*/
	{
		s32 rawPressure = (s32)((((u32)(buffer[1])) << 12) | (((u32)(buffer[2])) << 4) | ((u32)buffer[3] >> 4));
		s32 rawTemp = (s32)((((u32)(buffer[4])) << 12) | (((u32)(buffer[5])) << 4) | ((u32)buffer[6] >> 4));
		temp = bmp280CompensateT(rawTemp)/100.0f;		
		pressure = bmp280CompensateP(rawPressure)/25600.0f;			

// pressureFilter(&pressure, &sensors.baro.pressure);
sensors.baro.pressure = pressure;
sensors.baro.temperature = (float)temp; /单位度/
sensors.baro.asl = bmp280PressureToAltitude(&pressure) * 100.f; /转换成海拔/
}
}
else if (baroType == SPL06)
{
s32 rawPressure = (int32_t)buffer[1]<<16 | (int32_t)buffer[2]<<8 | (int32_t)buffer[3];
rawPressure = (rawPressure & 0x800000) ? (0xFF000000 | rawPressure) : rawPressure;

	s32 rawTemp = (int32_t)buffer[4]<<16 | (int32_t)buffer[5]<<8 | (int32_t)buffer[6];
	rawTemp = (rawTemp & 0x800000) ? (0xFF000000 | rawTemp) : rawTemp;
	
	temp = spl0601_get_temperature(rawTemp);
	pressure = spl0601_get_pressure(rawPressure, rawTemp);
	sensors.baro.pressure = pressure / 100.0f;
	sensors.baro.temperature = (float)temp; /*单位度*/
	sensors.baro.asl = SPL06PressureToAltitude(sensors.baro.pressure) * 100.f; //cm
}

}
/处理磁力计数据/
void processMagnetometerMeasurements(const uint8_t *buffer)
{
if (buffer[0] & (1 << AK8963_ST1_DRDY_BIT))
{
int16_t headingx = (((int16_t) buffer[2]) << 8) | buffer[1];
int16_t headingy = (((int16_t) buffer[4]) << 8) | buffer[3];
int16_t headingz = (((int16_t) buffer[6]) << 8) | buffer[5];

	sensors.mag.x = (float)headingx / MAG_GAUSS_PER_LSB;
	sensors.mag.y = (float)headingy / MAG_GAUSS_PER_LSB;
	sensors.mag.z = (float)headingz / MAG_GAUSS_PER_LSB;		
	magRaw.x = headingx;/*用于上传到上位机*/
	
	magRaw.y = headingy;
	magRaw.z = headingz;
}

}
/处理加速计和陀螺仪数据/
void processAccGyroMeasurements(const uint8_t *buffer)
{
/注意传感器读取方向(旋转270°x和y交换)/
int16_t ay = (((int16_t) buffer[0]) << 8) | buffer[1];
int16_t ax = ((((int16_t) buffer[2]) << 8) | buffer[3]);
int16_t az = (((int16_t) buffer[4]) << 8) | buffer[5];
int16_t gy = (((int16_t) buffer[8]) << 8) | buffer[9];
int16_t gx = (((int16_t) buffer[10]) << 8) | buffer[11];
int16_t gz = (((int16_t) buffer[12]) << 8) | buffer[13];

accRaw.x = ax;/*用于上传到上位机*/
accRaw.y = ay;
accRaw.z = az;
gyroRaw.x = gx - gyroBias.x;
gyroRaw.y = gy - gyroBias.y;
gyroRaw.z = gz - gyroBias.z;

gyroBiasFound = processGyroBias(gx, gy, gz, &gyroBias);

if (gyroBiasFound)
{
	processAccScale(ax, ay, az);	/*计算accScale*/
}

sensors.gyro.x = -(gx - gyroBias.x) * SENSORS_DEG_PER_LSB_CFG;	/*单位 °/s */
sensors.gyro.y =  (gy - gyroBias.y) * SENSORS_DEG_PER_LSB_CFG;
sensors.gyro.z =  (gz - gyroBias.z) * SENSORS_DEG_PER_LSB_CFG;
applyAxis3fLpf(gyroLpf, &sensors.gyro);	

sensors.acc.x = -(ax) * SENSORS_G_PER_LSB_CFG / accScale;	/*单位 g(9.8m/s^2)*/
sensors.acc.y =  (ay) * SENSORS_G_PER_LSB_CFG / accScale;	/*重力加速度缩放因子accScale 根据样本计算得出*/
sensors.acc.z =  (az) * SENSORS_G_PER_LSB_CFG / accScale;

applyAxis3fLpf(accLpf, &sensors.acc);

}
/传感器任务/
void sensorsTask(void *param)
{
sensorsInit(); /传感器初始化/
vTaskDelay(150);
sensorsSetupSlaveRead();/设置传感器从模式读取/

while (1)
{
	if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
	{
		/*确定数据长度*/
		u8 dataLen = (u8) (SENSORS_MPU6500_BUFF_LEN +
			(isMagPresent ? SENSORS_MAG_BUFF_LEN : 0) +
			(isBaroPresent ? SENSORS_BARO_BUFF_LEN : 0));

		i2cdevRead(I2C1_DEV, MPU6500_ADDRESS_AD0_HIGH, MPU6500_RA_ACCEL_XOUT_H, dataLen, buffer);
		
		/*处理原始数据，并放入数据队列中*/
		processAccGyroMeasurements(&(buffer[0]));

		if (isMagPresent)
		{
			processMagnetometerMeasurements(&(buffer[SENSORS_MPU6500_BUFF_LEN]));
		}
		if (isBaroPresent)
		{
			processBarometerMeasurements(&(buffer[isMagPresent ?
				SENSORS_MPU6500_BUFF_LEN + SENSORS_MAG_BUFF_LEN : SENSORS_MPU6500_BUFF_LEN]));
		}
		
		vTaskSuspendAll();	/*确保同一时刻把数据放入队列中*/
		xQueueOverwrite(accelerometerDataQueue, &sensors.acc);
		xQueueOverwrite(gyroDataQueue, &sensors.gyro);
		if (isMagPresent)
		{
			xQueueOverwrite(magnetometerDataQueue, &sensors.mag);
		}
		if (isBaroPresent)
		{
			xQueueOverwrite(barometerDataQueue, &sensors.baro);
		}
		xTaskResumeAll();
	}
}	

}
/获取传感器数据/
void sensorsAcquire(sensorData_t *sensors, const u32 tick)
{
sensorsReadGyro(&sensors->gyro);
sensorsReadAcc(&sensors->acc);
sensorsReadMag(&sensors->mag);
sensorsReadBaro(&sensors->baro);
}

void attribute((used)) EXTI4_Callback(void)
{
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(sensorsDataReady, &xHigherPriorityTaskWoken);

if (xHigherPriorityTaskWoken)
{
	portYIELD();
}

}
/二阶低通滤波/
static void applyAxis3fLpf(lpf2pData data, Axis3f in)
{
for (u8 i = 0; i < 3; i++)
{
in->axis[i] = lpf2pApply(&data[i], in->axis[i]);
}
}
/传感器数据校准/
bool sensorsAreCalibrated()
{
return gyroBiasFound;
}
/上位机获取读取原始数据/
void getSensorRawData(Axis3i16* acc, Axis3i16* gyro, Axis3i16* mag)
{
*acc = accRaw;
*gyro = gyroRaw;
*mag = magRaw;
}

bool getIsMPU9250Present(void)
{
bool value = isMPUPresent;
#ifdef SENSORS_ENABLE_MAG_AK8963
value &= isMagPresent;
#endif
return value;
}

bool getIsBaroPresent(void)
{
return isBaroPresent;
}

数据融合，主要是将陀螺仪检测到的角度通过加速度计数值互补滤波得到校正后的角度


参考：[(69条消息) 姿态解算（四）四元数 - 姿态解算步骤\_四元数姿态解算原理\_InfiniteYuan的博客-CSDN博客](https://bbs.csdn.net/topics/618668825) 姿态解算（四）四元数 - 姿态解算步骤_四元数姿态解算原理_InfiniteYuan的博客-CSDN博客")


[四元数姿态解算求Pitch(俯仰角)Roll(横滚角)Yall(偏航角)的公式代码解析 - 挖窝网 (wawooo.com)](https://bbs.csdn.net/topics/618668825)Roll(横滚角)Yall(偏航角)的公式代码解析 - 挖窝网 (wawooo.com)")

#include <math.h>
#include “sensfusion6.h”
#include “config.h”
#include “ledseq.h”
#include “maths.h”

/********************************************************************************

• 本程序只供学习使用，未经作者许可，不得用于其它任何用途
• ALIENTEK MiniFly
• 6轴数据融合代码
• 正点原子@ALIENTEK
• 技术论坛:www.openedv.com
• 创建日期:2017/5/12
• 版本：V1.3
• 版权所有，盗版必究。
• Copyright© 广州市星翼电子科技有限公司 2014-2024
• All rights reserved
• 修改说明:
• 版本V1.3 互补滤波代码移植于inav-1.9.0
  ********************************************************************************/

#define ACCZ_SAMPLE 350

float Kp = 0.4f; /比例增益/
float Ki = 0.001f; /积分增益/
float exInt = 0.0f;
float eyInt = 0.0f;
float ezInt = 0.0f; /积分误差累计/

static float q0 = 1.0f; /四元数/
static float q1 = 0.0f;
static float q2 = 0.0f;
static float q3 = 0.0f;
static float rMat[3][3];/旋转矩阵/

static float maxError = 0.f; /最大误差/
bool isGravityCalibrated = false; /是否校校准完成/
static float baseAcc[3] = {0.f,0.f,1.0f}; /静态加速度/

static float invSqrt(float x); /快速开平方求倒/

static void calBaseAcc(float* acc) /计算静态加速度/
{
static u16 cnt = 0;
static float accZMin = 1.5;
static float accZMax = 0.5;
static float sumAcc[3] = {0.f};

for(u8 i=0; i<3; i++)
	sumAcc[i] += acc[i];
	
if(acc[2] < accZMin)	accZMin = acc[2];
if(acc[2] > accZMax)	accZMax = acc[2];

if(++cnt >= ACCZ_SAMPLE) /*缓冲区满*/
{
	cnt = 0;
	maxError = accZMax - accZMin;
	accZMin = 1.5;
	accZMax = 0.5;
	
	if(maxError < 0.015f)
	{
		for(u8 i=0; i<3; i++)
			baseAcc[i] = sumAcc[i] / ACCZ_SAMPLE;
		
		isGravityCalibrated = true;
		
		ledseqRun(SYS_LED, seq_calibrated);	/*校准通过指示灯*/
	}
	
	for(u8 i=0; i<3; i++)		
		sumAcc[i] = 0.f;		
}	

}

/计算旋转矩阵/
void imuComputeRotationMatrix(void)
{
float q1q1 = q1 * q1;
float q2q2 = q2 * q2;
float q3q3 = q3 * q3;

float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q3 = q2 * q3;

rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
rMat[0][2] = 2.0f * (q1q3 - -q0q2);

rMat[1][0] = 2.0f * (q1q2 - -q0q3);
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
rMat[1][2] = 2.0f * (q2q3 + -q0q1);

rMat[2][0] = 2.0f * (q1q3 + -q0q2);
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;

}
//姿态解算，得出pitch,roll,yawd,z轴加速度（除去重力加速度）
//姿态融合说白了就是将3轴加速度、3轴角速度和3轴磁场强度融合成四元数，再将四元数转化为欧拉角，
//最后将欧拉角最为控制量输送到所有电机以达控制飞行器姿态的目的。
//欧拉角包括偏航角Yaw、俯仰角Pitch和滚动角Roll。
//IMUupdate算法只融合了加速度计和陀螺仪的数据，还需要使用互补滤波算法来融合磁力计以修正偏航角Yaw，

/*
这个算法的基本思路是以陀螺仪所测的角度为主，把由加速度得到角度误差补偿到由陀螺仪所得的角度值当中。
之所以要这样做，是因为陀螺仪短期测量很准，但在长期测量时容易积累误差，而加速度则相反
*/
void imuUpdate(Axis3f acc, Axis3f gyro, state_t state , float dt) /数据融合 互补滤波/
{
float normalise;
float ex, ey, ez;
float halfT = 0.5f * dt;
float accBuf[3] = {0.f};
Axis3f tempacc = acc;
/

以上求出的角速度是以°为单位的，这里，我们还需要把°转化为弧度
*/
gyro.x = gyro.x * DEG2RAD;	/* 度转弧度 */
gyro.y = gyro.y * DEG2RAD;
gyro.z = gyro.z * DEG2RAD;

/* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
{

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

以上求出的角速度是以°为单位的，这里，我们还需要把°转化为弧度
*/
gyro.x = gyro.x * DEG2RAD;	/* 度转弧度 */
gyro.y = gyro.y * DEG2RAD;
gyro.z = gyro.z * DEG2RAD;

/* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
{

[外链图片转存中…(img-MDiySYmz-1715879295732)]
[外链图片转存中…(img-PIgzKDfn-1715879295732)]

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！