四轴飞控总结2：结构体与数据类型2

最新推荐文章于 2024-03-06 13:18:12 发布

步履不停。

最新推荐文章于 2024-03-06 13:18:12 发布

阅读量350

点赞数

文章标签：嵌入式

本文链接：https://blog.youkuaiyun.com/qq_43543041/article/details/121092308

版权

//LSM6DSL
void processlsm6dslAccGyro(const uint8_t *buffer)
{

	int16_t gx1 = (((int16_t) buffer[1]) << 8) | buffer[0];
	int16_t gy1 = -((((int16_t) buffer[3]) << 8) | buffer[2]);
	int16_t gz1 = (((int16_t) buffer[5]) << 8) | buffer[4];
	int16_t ax1 = (((int16_t) buffer[7]) << 8) | buffer[6];
	int16_t ay1 = -((((int16_t) buffer[9]) << 8) | buffer[8]);
	int16_t az1 = (((int16_t) buffer[11]) << 8) | buffer[10];
	

	
	accRaw1.x = ax1;/*用于传到上位机*/
	accRaw1.y = ay1;
	accRaw1.z = az1;
	gyroRaw1.x = gx1;
	gyroRaw1.y = gy1;
	gyroRaw1.z = gz1;
	
	gyroBiasFound1 = processGyroBias1(gx1, gy1, gz1, &gyroBias1);
	
	if (gyroBiasFound1)
	{
		processAccScale(ax1, ay1, az1);	
		
	}
	
	sensors1.gyro.x = -(gx1 - gyroBias1.x)* SENSORS_DEG_PER_LSB_CFG_LSM;	
	sensors1.gyro.y =  (gy1 - gyroBias1.y)* SENSORS_DEG_PER_LSB_CFG_LSM;
	sensors1.gyro.z =  (gz1 - gyroBias1.z)* SENSORS_DEG_PER_LSB_CFG_LSM;

	applyAxis3fLpf(gyroLpf1, &sensors1.gyro);	

	sensors1.acc.x = -(ax1) * SENSORS_G_PER_LSB_CFG / accScale;	/ g(9.8m/s^2)*/
	sensors1.acc.y =  (ay1) * SENSORS_G_PER_LSB_CFG / accScale;	
	sensors1.acc.z =  (az1) * SENSORS_G_PER_LSB_CFG / accScale;

	applyAxis3fLpf(accLpf1, &sensors1.acc);
	
	LDS_a[0] = accRaw1.x;
	LDS_a[1] = accRaw1.y;
	LDS_a[2] = accRaw1.z;
	
	LDS_g[0] = gyroRaw1.x;
    LDS_g[1] = gyroRaw1.y;
	LDS_g[2] = gyroRaw1.z;
}

sensors1.gyro.x
sensors1.gyro.y
sensors1.gyro.z

QUESTION：

LSM6DSL

sensors1.gyro.x
sensors1.gyro.y
sensors1.gyro.z出来的数据不对

思考：sensors1.gyro.x与（gx1 - gyroBias1.x）有关，实时值正确，说明gyroBias1.x有问题。

gyroBias1.x是由

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

//计算陀螺方差得出的。

进入的值为gyro的实时值gyroRaw1，出来为结构体指针gyroBiasOut，

所以赋值为gyroBiasOut->x=(*gyroBiasOut).x。

static bool processGyroBias1(int16_t gx1, int16_t gy1, int16_t gz1, Axis3f *gyroBiasOut1)
{
	sensorsAddBiasValue(&gyroBiasRunning1, gx1, gy1, gz1);

	if (!gyroBiasRunning1.isBiasValueFound)
	{
		sensorsFindBiasValue1(&gyroBiasRunning1);
	}

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

	return gyroBiasRunning1.isBiasValueFound;
}

processGyroBias()函数先进入sensorsAddBiasValue()

gx/gy/gz进入sensorsAddBiasValue()，将其值储存在指针Axis3i16* bufHead中，并且累加。

如果bias储存在bufHead中的值大于1024个，那么buffer满了,重新计数。

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;
	}
}

如果gyroBiasRunning.isBiasValueFound没被找到，我们需要计算方差和平均值。sensorsFindBiasValue(&gyroBiasRunning);出来的是gyroBiasRunning.bais.x

来找出偏置值

static bool sensorsFindBiasValue1(BiasObj* bias)
{
	bool foundbias = false;

	if (bias->isBufferFilled)
	{
		
		Axis3f mean;
		Axis3f variance;
		sensorsCalculateVarianceAndMean1(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;
}

//计算方差和平均值。

sensorsCalculateVarianceAndMean(BiasObj* bias, Axis3f* varOut, Axis3f* meanOut)
进入的是gyroBiasRunning.bais.x,出来的是方差和平均值，看计算出来的方差是否小于40000，小于的话将平均值赋给gyroBiasRunning.bais.x，输出gyrobias(平均值)

int64_t sum[3] = {0};
int64_t sumsq[3] = {0};
static void sensorsCalculateVarianceAndMean1(BiasObj* bias1, Axis3f* varOut1, Axis3f* meanOut1)
{
	u32 i;
	//int64_t sum[3] = {0};
	//int64_t sumsq[3] = {0};

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

	varOut1->x = (sumsq1[0] - ((int64_t)sum1[0] * sum1[0]) / SENSORS_NBR_OF_BIAS_SAMPLES);
	varOut1->y = (sumsq1[1] - ((int64_t)sum1[1] * sum1[1]) / SENSORS_NBR_OF_BIAS_SAMPLES);
	varOut1->z = (sumsq1[2] - ((int64_t)sum1[2] * sum1[2]) / SENSORS_NBR_OF_BIAS_SAMPLES);

	meanOut1->x = (float)sum1[0] / SENSORS_NBR_OF_BIAS_SAMPLES;
	meanOut1->y = (float)sum1[1] / SENSORS_NBR_OF_BIAS_SAMPLES;
	meanOut1->z = (float)sum1[2] / SENSORS_NBR_OF_BIAS_SAMPLES;
}

QUESTION :

1.是将int64_t sum[3] = {0};int64_t sumsq[3] = {0};定义成了全局变量的原因？

还是计算出的平均值方差混掉了？

答：gyro的数据混乱的原因是因为int64_t sum[3] = {0};int64_t sumsq[3] = {0};被定义成了全局变量，重复使用，导致计算的方差和平均值是错误的。

sum2方案

gyroBiasFound1 = processGyroBias1(gx1, gy1, gz1, &gyroBias1);//新重新定义一个函数来找偏置

sensorsAddBiasValue(&gyroBiasRunning1, gx1, gy1, gz1);//不变调用的sensorsAddBiasValue()不用变

sensorsFindBiasValue(&gyroBiasRunning1);

//不变调用的sensorsFindBiasValue()不用变,直接使用

sensorsCalculateVarianceAndMean(bias, &variance, &mean);//不变

或者

sum方案

gyroBiasFound1 = processGyroBias1(gx1, gy1, gz1, &gyroBias1);//新重新定义一个函数来找偏置

sensorsAddBiasValue(&gyroBiasRunning1, gx1, gy1, gz1);//不变调用的sensorsAddBiasValue()不用变

sensorsFindBiasValue1(&gyroBiasRunning1);//新重新定义一个函数来找偏置,然后再重新定义函数找方差和平均值

sensorsCalculateVarianceAndMean1(bias, &variance, &mean);//新

2.我们将

static void sensorsCalculateVarianceAndMean1(BiasObj* bias1, Axis3f* varOut1, Axis3f* meanOut1)

改为

static void sensorsCalculateVarianceAndMean1(BiasObj* bias, Axis3f* varOut1, Axis3f* meanOut1)

可以输出对的数据吗？

答：不可以，在sum为全局变量的前提下必须重新定义一个bias1来区分进入函数的值。

3.将xxxx1()里的bias全部调用成bias1，是否依旧可以出来值？

答：可以，但是最复杂，没必要。