问渠网-自制小四轴-第四节传感器驱动

本文作者：李德强

第四节传感器驱动

在这一节中我们将来完成传感器的驱动程序。我们的小四轴采用的是MPU6050传感器，这是一个3轴陀螺仪和3轴加速度计的传感器。我们可以根据管方所给定的手册进行配置。来完成它的驱动程序，而驱动程序的最终目的是可以让我们可以将其测量的角度、角速度、加速度等数据读出并加以使用。

首先，我们需要知道的是MPU6050在我们的电路原理图中是采用I2C1接口，因此我们需要先来完成I2C1的接口程序，好在我们可以使用现成的代码库来完成I2C1的配置，如下：

void MX_I2C1_Init(void)
{
	hi2c1.Instance = I2C1;
	hi2c1.Init.ClockSpeed = 100000;
	hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
	hi2c1.Init.OwnAddress1 = 0;
	hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
	hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
	hi2c1.Init.OwnAddress2 = 0;
	hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
	hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
	if (HAL_I2C_Init(&hi2c1) != HAL_OK)
	{
		Error_Handler();
	}
}

这部分代码是我们在使用CubeIDE中已经配置好的内容，由其自动生成的代码，其中我们需要配置I2C1的时钟频率为100KHz，也就是hi2c1.Init.ClockSpeed = 100000;

在stm32f1xx_hal_msp.c文件中也有I2C1的配置，如下：

void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c)
{
	if (hi2c->Instance == I2C1)
	{
		/* Peripheral clock disable */
		__HAL_RCC_I2C1_CLK_DISABLE();
		/**I2C1 GPIO Configuration
		PB6     ------> I2C1_SCL
		PB7     ------> I2C1_SDA
		*/
		HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6);
		HAL_GPIO_DeInit(GPIOB, GPIO_PIN_7);
	}
}

可以看到I2C1采用的是PB6和PB7两个数据传输脚，PB6为I2C1的SCL时钟脉冲，而PB7为I2C1的SDA数据传输。同样这部分代码也是由CubeIDE自动生成的，我们不再赘述。

接下来我们需要编写I2C的数据读写功能，这里我们只给出几个简单的函数示例，完成代码请读者参见附件下载。

char i2cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
	extern I2C_HandleTypeDef hi2c1;
	HAL_I2C_Mem_Read(&hi2c1, devAddr, regAddr, I2C_MEMADD_SIZE_8BIT, data, length, 0xfff);

	return length;
}

char i2cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data)
{
	return i2cdev_readBytes(devAddr, regAddr, 1, data);
}

int i2cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
	extern I2C_HandleTypeDef hi2c1;
	HAL_I2C_Mem_Write(&hi2c1, devAddr, regAddr, I2C_MEMADD_SIZE_8BIT, data, length, 0xfff);

	return 1;
}

int i2cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
	return i2cdev_writeBytes(devAddr, regAddr, 1, &data);
}

之后我们就可以根据I2C的读写函数来完成MPU6050的数据读写了，MPU6050中需要配置的寄存器比较多，我们根据手册先将这些内容定义出来：

#define MPU6050_ADDRESS_AD0_LOW 0x68  // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
#define MPU6050_ADDRESS_DEFAULT (MPU6050_ADDRESS_AD0_LOW << 1)

#define MPU6050_RA_XG_OFFS_TC 0x00
#define MPU6050_RA_YG_OFFS_TC 0x01
#define MPU6050_RA_ZG_OFFS_TC 0x02
#define MPU6050_RA_X_FINE_GAIN 0x03
#define MPU6050_RA_Y_FINE_GAIN 0x04
#define MPU6050_RA_Z_FINE_GAIN 0x05
……
//其它内容省略
#define MPU6050_RA_DMP_CFG_1 0x70
#define MPU6050_RA_DMP_CFG_2 0x71
#define MPU6050_RA_FIFO_COUNTH 0x72
#define MPU6050_RA_FIFO_COUNTL 0x73
#define MPU6050_RA_FIFO_R_W 0x74
#define MPU6050_RA_WHO_AM_I 0x75

这些内容我们都可以在MPU6050的官方手册中找到其意义，并根据实际需要做一些配置调整，但在我们的小四轴程序中，并不需要一个一个的了解它们的作用，我们只需要根据一些开源的驱动代码做一些移植即可。这里我们给出已经移植好的一些配置函数，完成代码请参考附件。

void mpu6050_initialize(void)
{
	mpu6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
	mpu6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
	mpu6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
	mpu6050_setSleepEnabled(0);
}

int mpu6050_testConnection(void)
{
	return mpu6050_getDeviceID() == 0x34;
}

uint8_t mpu6050_dmpInitialize(void)
{
	mpu6050_reset();
	mpu6050_setSleepEnabled(0);
	mpu6050_setMemoryBank(0x10, 1, 1);
	mpu6050_setMemoryStartAddress(0x06);
	mpu6050_setMemoryBank(0, 0, 0);
	
	int8_t xgOffset = mpu6050_getXGyroOffset();
	int8_t ygOffset = mpu6050_getYGyroOffset();
	int8_t zgOffset = mpu6050_getZGyroOffset();

	mpu6050_setSlaveAddress(0, 0x7F);
	mpu6050_setI2CMasterModeEnabled(0);
	mpu6050_setSlaveAddress(0, 0x68);
	mpu6050_resetI2CMaster();

	if (mpu6050_writeProgMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 0))
	{
		if (mpu6050_writeProgDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE))
		{
			mpu6050_setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
			mpu6050_resetDMP();
			mpu6050_setDMPEnabled(0);
			mpu6050_resetFIFO();
			mpu6050_getIntStatus();
		}
		else
		{
			return 2; 
		}
	}
	else
	{
		return 1;
	}
	return 0;
}

int mpu6050_setup(void)
{
	mpu6050_initialize();
	if (!mpu6050_testConnection())
	{
		return -1;
	}
	devStatus = mpu6050_dmpInitialize();
	if (devStatus == 0)
	{
		mpu6050_setDMPEnabled(1);
		mpuIntStatus = mpu6050_getIntStatus();
		dmpReady = 1;
		packetSize = mpu6050_dmpGetFIFOPacketSize();
	}
	else
	{
		return -1;
	}
	return 0;
}

这里比较关键的是我们使用了MPU6050传感器的DMP功能，这是一个芯片的自动融合滤波输出，这样我们就可以直接读出其输出内容了，我们可以直接得到3轴的角度、3轴角速度以及3轴的加速度。注意，这里我们省略了一个关于角速度和加速度解算为角度的过程，这个过程本身比较复杂，在我们的小四轴里就不展开来讲述了，有兴趣的读者可参见《多旋翼无人机—姿态解算》章节。

之后，我们编写一个函数来完成其相关数据的读取。

int mpu6050_value(float *x, float *y, float *z, float *gx, float *gy, float *gz, float *ax, float *ay, float *az)
{
	fifoCount = mpu6050_getFIFOCount();
	if (fifoCount >= 42 && fifoCount % 42 == 0)
	{
		short aax, aay, aaz;
		short ggx, ggy, ggz;
		mpu6050_getFIFOBytes(fifoBuffer, packetSize);
		mpu6050_resetFIFO();
		mpu6050_dmpGetQuaternion(&q, fifoBuffer);
		mpu6050_dmpGetGravity(&gravity, &q);
		mpu6050_dmpGetYawPitchRoll(ypr, &q, &gravity);
		*x = ypr[2];
		*y = ypr[1];
		*z = ypr[0];
		last_z = ypr[0];
		mpu6050_dmpGetQuaternion(&q, fifoBuffer);
		mpu6050_dmpGetAccel(&aa, fifoBuffer);
		mpu6050_dmpGetGravity(&gravity, &q);
		mpu6050_dmpGetLinearAccel(&aaReal, &aa, &gravity);	
		mpu6050_dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
		*ax = (float) aaReal.x / 163.84;
		*ay = (float) aaReal.y / 163.84;
		*az = (float) aaReal.z / 163.84;
		mpu6050_getRotation(&ggx, &ggy, &ggz);
		float _gx = (float) (ggx) / 131.0;
		float _gy = (float) (-ggy) / 131.0;
		float _gz = (float) (-ggz) / 131.0;
		*gx = _gx;
		*gy = _gy;
		*gz = _gz;
		return 0;
	}
	else if (fifoCount >= 42 * 8)
	{
		mpu6050_resetFIFO();
		return -1;;
	}
	return -1;
}

最后，我们modules文件夹下编写一个controller_task.c文件并在里面创建一个线程来完成MPU6050的驱动初始化和数据读取内容：

#include 
#include 
#include 
#include 

float mpu_value[9] = { 0 };

void* controller_pthread(void* arg)
{
	mpu6050_setup();

	float sx = 0;
	float sy = 0;
	float sz = 0;

	uint32_t tk = 0;

	while (tk++ < 20)
	{
		mpu6050_value(&mpu_value[0], &mpu_value[1], &mpu_value[2], &mpu_value[3], &mpu_value[4], &mpu_value[5], &mpu_value[6], &mpu_value[7], &mpu_value[8]);
		sx += mpu_value[0];
		sy += mpu_value[1];
		sz += mpu_value[2];
		sleep_ticks(5);
	}

	if (sx == 0 && sy == 0 && sz == 0)
	{
		printf("Init mpu6050 error, reset mpu.\n");
		HAL_NVIC_SystemReset();
		while (1)
		{
		}
	}
	printf("Init mpu6050 OK.\n");

	while (1)
	{
		//读取姿态信息
		if (mpu6050_value(&mpu_value[0], &mpu_value[1], &mpu_value[2], &mpu_value[3], &mpu_value[4], &mpu_value[5], &mpu_value[6], &mpu_value[7], &mpu_value[8]) == 0)
		{
			for (int i = 0; i < 9; i++)
			{
				printf("%d ", (int)(mpu_value[i] * 1000.0));
			}
			printf("\n");
		}

		sleep_ticks(10);
	}
	return NULL;
}

void controller_task(void)
{
	pcb_create(PCB_CTL_PRIO, &controller_pthread, NULL, PCB_CTL_SIZE);
}

编译程序并将程序烧录到我们的小四轴电路板中，就可以得到MPU6050的数据了。如下：

-76 12 177 -335 -7 213 123 615 9798 
-76 12 177 -351 -30 198 118 598 9796 
-76 12 178 -328 -7 213 106 588 9783 
-76 12 179 -335 0 229 97 589 9776 
-76 12 179 -343 -22 206 91 595 9790 
-76 12 180 -335 -15 206 101 591 9791 
-76 12 180 -335 -15 206 104 604 9777 
-76 12 181 -343 -15 198 109 600 9771 
-76 12 182 -335 -15 221 108 604 9778 
-76 13 182 -335 -15 213 109 604 9772 
-76 13 183 -335 -7 198 116 604 9777 
-76 12 185 -343 -30 198 103 599 9792 
-76 12 185 -343 -15 198 103 600 9795

这样我们就得到了MPU6050的数据，后续我们会根据这些数据来对我们的小四轴做飞行控制。

最后我们给出完整的程序代码：下载地址