社区导航

 
查看: 316|回复: 5

[问题讨论] BlueMicrosystem2使用MahonyAHRS融合算法解出来的数据不对

[复制链接]

3960

TA的帖子

8

TA的资源

版主

Rank: 6Rank: 6

发表于 2017-6-19 22:52:33 | 显示全部楼层 |阅读模式

因为ST官方提供的MotionFX没用明白
在网上找了一个传感器融合的代码,输入9轴数据输出四元数用MahonyAHRS替换原来的MotionFX_manager_run

将四元数按照原来的方式输出到手机,用BLUEMS查看骰子是一直转动的
不知道问题出在哪,官方是直接把寄存器值输入到MotionFX库
单位是LSB
我在调用 MahonyAHRSupdate函数时把对应的寄存器做了转换
加速度的单位是g
磁场的单位是gauss
陀螺仪的单位是dps
难道问题出在这?

[C] 纯文本查看 复制代码
static void ComputeQuaternions(void)
{
  static SensorAxes_t quat_axes[SEND_N_QUATERNIONS];
  static int32_t calibIndex =0;
  static int32_t CounterFX  =0;
  static int32_t CounterEC  =0;
  SensorAxes_t ACC_Value;
  SensorAxesRaw_t ACC_Value_Raw;
  SensorAxes_t GYR_Value;
  SensorAxes_t MAG_Value;

  float ax,ay,az,gx,gy,gz,mx,my,mz;
  /* Increment the Counter */
  if(W2ST_CHECK_CONNECTION(W2ST_CONNECT_EC)) {
    CounterEC++;
  } else {
    CounterFX++;
  }

  /* Read the Acc RAW values */
  BSP_ACCELERO_Get_AxesRaw(TargetBoardFeatures.HandleAccSensor,&ACC_Value_Raw);

  /* Read the Magneto values */
  BSP_MAGNETO_Get_Axes(TargetBoardFeatures.HandleMagSensor,&MAG_Value);

  /* Read the Gyro values */
  BSP_GYRO_Get_Axes(TargetBoardFeatures.HandleGyroSensor,&GYR_Value);

//  MotionFX_manager_run(ACC_Value_Raw,GYR_Value,MAG_Value);
      
/*
LSM303AGR
Linear acceleration sensitivity mg/LSB
FS = ±2 g and in high-resolution mode -7% 0.98 +7%
FS = ±4 g and in high-resolution mode -7% 1.95 +7%
FS = ±8 g and in high-resolution mode -7% 3.9 +7%
FS = ±16 g and in high-resolution mode -7% 11.72 +7%
FS = ±2 g and in normal mode -7% 3.9 +7%
FS = ±4 g and in normal mode -7% 7.82 +7%
FS = ±8 g and in normal mode -7% 15.63 +7%
FS = ±16 g and in normal mode -7% 46.9 +7%
FS = ±2 g and in low-power mode -7% 15.63 +7%
FS = ±4 g and in low-power mode -7% 31.26 +7%
FS = ±8 g and in low-power mode -7% 62.52 +7%
FS = ±16 g and in low-power mode -7% 187.58 +7%
Magnetic sensitivity(2) mgauss/LSB
-7% 1.5 +7% 
  
LSM6DSM 
Linear acceleration sensitivity(2) mg/LSB
FS = ±2 0.061
FS = ±4 0.122
FS = ±8 0.244
FS = ±16 0.488
  
 Angular rate sensitivity(2) mdps/LSB
FS = ±125 4.375
FS = ±245 8.75
FS = ±500 17.50
FS = ±1000 35
FS = ±2000 70
*/
  //+-2g = 0.061mg/LSB
  ax = ACC_Value_Raw.AXIS_X * 0.061 / 1000.00;
  ay = ACC_Value_Raw.AXIS_Y * 0.061 / 1000.00;
  az = ACC_Value_Raw.AXIS_Z * 0.061 / 1000.00;
  //1.5 mgauss/LSB
  mx = MAG_Value.AXIS_X * 1.5 / 1000.00;
  my = MAG_Value.AXIS_Y * 1.5 / 1000.00;
  mz = MAG_Value.AXIS_Z * 1.5 / 1000.00;
  //+-2000 = 70mdps/LSB
  gx = GYR_Value.AXIS_X * 70.00 / 1000.00;
  gy = GYR_Value.AXIS_Y * 70.00 / 1000.00;
  gz = GYR_Value.AXIS_Z * 70.00 / 1000.00;
  
  MahonyAHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz);
  int32_t QuaternionNumber = (CounterFX>SEND_N_QUATERNIONS) ? (SEND_N_QUATERNIONS-1) : (CounterFX-1);

  /* Scaling quaternions data by a factor of 10000
    (Scale factor to handle float during data transfer BT) */

  /* Save the quaternions values */
  if(q3 < 0){
    quat_axes[QuaternionNumber].AXIS_X = (int32_t)(q0 * (-10000));
    quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(q1 * (-10000));
    quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(q2 * (-10000));
  } else {
    quat_axes[QuaternionNumber].AXIS_X = (int32_t)(q0 * 10000);
    quat_axes[QuaternionNumber].AXIS_Y = (int32_t)(q1 * 10000);
    quat_axes[QuaternionNumber].AXIS_Z = (int32_t)(q2 * 10000);
  }
    
  /* Every QUAT_UPDATE_MUL_10MS*10 mSeconds Send Quaternions informations via bluetooth */
  if(CounterFX==QUAT_UPDATE_MUL_10MS){
    Quat_Update(quat_axes);
    CounterFX=0;
  }
}



[C] 纯文本查看 复制代码
//=====================================================================================================
// MahonyAHRS.c
//=====================================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: [url]http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms[/url]
//
// Date			Author			Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include "MahonyAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq	1000.0f			// sample frequency in Hz
#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float recipNorm;
    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
	float hx, hy, bx, bz;
	float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
		MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
		return;
	}

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;     

		// Normalise magnetometer measurement
		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
		mx *= recipNorm;
		my *= recipNorm;
		mz *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;   

        // Reference direction of Earth's magnetic field
        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
        bx = sqrt(hx * hx + hy * hy);
        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

		// Estimated direction of gravity and magnetic field
		halfvx = q1q3 - q0q2;
		halfvy = q0q1 + q2q3;
		halfvz = q0q0 - 0.5f + q3q3;
        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
	
		// Error is sum of cross product between estimated direction and measured direction of field vectors
		halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
		halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
		halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = q0;
	qb = q1;
	qc = q2;
	q0 += (-qb * gx - qc * gy - q3 * gz);
	q1 += (qa * gx + qc * gz - q3 * gy);
	q2 += (qa * gy - qb * gz + q3 * gx);
	q3 += (qa * gz + qb * gy - qc * gx); 
	
	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;        

		// Estimated direction of gravity and vector perpendicular to magnetic flux
		halfvx = q1 * q3 - q0 * q2;
		halfvy = q0 * q1 + q2 * q3;
		halfvz = q0 * q0 - 0.5f + q3 * q3;
	
		// Error is sum of cross product between estimated and measured direction of gravity
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		// Compute and apply integral feedback if enabled
		if(twoKi > 0.0f) {
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// Apply proportional feedback
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// Integrate rate of change of quaternion
	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = q0;
	qb = q1;
	qc = q2;
	q0 += (-qb * gx - qc * gy - q3 * gz);
	q1 += (qa * gx + qc * gz - q3 * gy);
	q2 += (qa * gy - qb * gz + q3 * gx);
	q3 += (qa * gz + qb * gy - qc * gx); 
	
	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: [url]http://en.wikipedia.org/wiki/Fast_inverse_square_root[/url]

float invSqrt(float x) {
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

虾扯蛋

回复

使用道具 举报

3960

TA的帖子

8

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 7 天前 | 显示全部楼层


效果如图,感觉应该是陀螺仪输出的数据不对
ezgif-1-c469e6ceba.gif
虾扯蛋

回复 支持 反对

使用道具 举报

15

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 6 天前 | 显示全部楼层
陀螺仪,磁力计没有做校准?MotionFX库里调用的Acc应该是用的寄存器原始数据,Gyro应该是mdps单位,而Mag应该是mGauss的单位。

点评

陀螺仪有一个轴静止时输出数据比较大 可能是这个原因 例程里掉用MahonyAHRSupdate函数的频率是100次每秒 MahonyAHRS算法里的sampleFreq改成100 陀螺仪使用DPS数据单位会出现轻微转动传感器骰子就高速转动的情况  详情 回复 发表于 6 天前

回复 支持 反对

使用道具 举报

3960

TA的帖子

8

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 6 天前 来自手机 | 显示全部楼层
yujie2510 发表于 2017-6-21 20:57
陀螺仪,磁力计没有做校准?MotionFX库里调用的Acc应该是用的寄存器原始数据,Gyro应该是mdps单位,而Mag应 ...

陀螺仪有一个轴静止时输出数据比较大
可能是这个原因
例程里掉用MahonyAHRSupdate函数的频率是100次每秒
MahonyAHRS算法里的sampleFreq改成100
陀螺仪使用DPS数据单位会出现轻微转动传感器骰子就高速转动的情况
适当的修改这些数值能变正常
但是找不出这几个数字之间的关系

还有一个问题是加速度和地磁在算法里不起作用
修改了KP和KI参数也没见到加速度补偿陀螺仪误差的效果

回复 支持 反对

使用道具 举报

15

TA的帖子

0

TA的资源

一粒金砂(中级)

Rank: 2

发表于 前天 20:00 | 显示全部楼层
在 osxMotionFX 的库里会用磁力计来补偿陀螺仪,如果你用的算法如果没有用到磁力计的数据的话,就需要保证所有的输入都是校准过后的数据。

点评

允许输入9轴数据,应该全通过加速度和磁力计补偿 可能是设置的问题,在实际测试时加速度和磁力计几乎看不到作用  详情 回复 发表于 昨天 11:31

回复 支持 反对

使用道具 举报

3960

TA的帖子

8

TA的资源

版主

Rank: 6Rank: 6

 楼主| 发表于 昨天 11:31 | 显示全部楼层
yujie2510 发表于 2017-6-25 20:00
在 osxMotionFX 的库里会用磁力计来补偿陀螺仪,如果你用的算法如果没有用到磁力计的数据的话,就需要保证 ...

允许输入9轴数据,应该全通过加速度和磁力计补偿
可能是设置的问题,在实际测试时加速度和磁力计几乎看不到作用
虾扯蛋

回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

  • 论坛活动 E手掌握

    扫码关注
    EEWORLD 官方微信

  • EE福利  唾手可得

    扫码关注
    EE福利 唾手可得

小黑屋|手机版|Archiver|电子工程世界 ( 京ICP证 060456

GMT+8, 2017-6-27 16:52 , Processed in 0.299858 second(s), 16 queries , Redis On.

快速回复 返回顶部 返回列表
关闭