Doubts about Sensor values convertion and filtering

Firmware/software/electronics/mechanics
Post Reply
Yaro
Member
Posts: 31
Joined: Wed May 20, 2015 11:28 am

Doubts about Sensor values convertion and filtering

Post by Yaro »

Hi all,

I'm tring to understand how CF firmware get values from sensor and elaborate them. I've some newbie question that I can't solve.

- In convertion between Mag values to Gauss it's used 666.7 gauss*LSB. But since precision is +/- 4800uT, from where this value come from? I've tried to convert uT to Gauss and then calculate LSB but I get: 96/16bit=1.46mG/LSB

- I've found that Acc-Gyro are filtred inside IMU with a LPF, then Accelerator output is filtred again with a IIRLPF, then again(I suppose) in complementary filter. Why filter so much times?

- Also, since LPF is active for Acc-Gyro on IMU, why not to activate also HPF for Gyro to reduce drift?

- Order to get Accelerometer values in Imu6Read is: Read, Take Bias, Filter, Apply Bias and make conversion. I can't understand logic of this. Should it be more correct if I Filter before Take Bias or Filter after I've Applied bias and made conversion?

- Also, if I filter Accelerometer values, why don't do the same for Gyro and apply a HPF, and ARW or something to eliminate drift?

It's ok even if you answer only to some of my question!

Thank you!
tobias
Bitcraze
Posts: 2339
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: Doubts about Sensor values convertion and filtering

Post by tobias »

Hi Yaro,

Well to begin with the sensor reading and surrounding code is definitely a subject of improvement and I'm glad someone is looking at it. I'll try to answer your questions as good as I can.
- In convertion between Mag values to Gauss it's used 666.7 gauss*LSB. But since precision is +/- 4800uT, from where this value come from? I've tried to convert uT to Gauss and then calculate LSB but I get: 96/16bit=1.46mG/LSB
You are probably right. I don't think the absolute Gauss value ever been validated so the scaling can definitely be wrong.
- I've found that Acc-Gyro are filtred inside IMU with a LPF, then Accelerator output is filtred again with a IIRLPF, then again(I suppose) in complementary filter. Why filter so much times?
If I don't recall wrongly only the gyro should be filtered inside the MPU9250. The accelerometer is filtered outside because at one point we had different accelerometer chip and for convenience we then filtered it outside to easier switch between chips. As the accelerometer is very noisy because of vibrations the filtering helps for the stability.
- Also, since LPF is active for Acc-Gyro on IMU, why not to activate also HPF for Gyro to reduce drift?
Sounds like a good thing to try!
- Order to get Accelerometer values in Imu6Read is: Read, Take Bias, Filter, Apply Bias and make conversion. I can't understand logic of this. Should it be more correct if I Filter before Take Bias or Filter after I've Applied bias and made conversion?
As the bias for the accelerometer isn't used and the IIRLPF isn't for the gyro it doesn't look so strange but could be arranged clearer and better.
GYRO: LPF[MPU] -> BIAS -> CONVERSION
ACC: RAW[MPU] -> IIRLPF -> ALIGN (-> BIAS) -> CONVERSION
- Also, if I filter Accelerometer values, why don't do the same for Gyro and apply a HPF, and ARW or something to eliminate drift?
Definitely worth a try.
Yaro
Member
Posts: 31
Joined: Wed May 20, 2015 11:28 am

Re: Doubts about Sensor values convertion and filtering

Post by Yaro »

Thank you for your answers!
If I don't recall wrongly only the gyro should be filtered inside the MPU9250. The accelerometer is filtered outside because at one point we had different accelerometer chip and for convenience we then filtered it outside to easier switch between chips. As the accelerometer is very noisy because of vibrations the filtering helps for the stability.
I've checked better about this and found that MPU setting are Initialized inside imu6Init with this registers:

Code: Select all

mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
mpu6500SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);

	#ifdef IMU_MPU6500_DLPF_256HZ
		// 256Hz digital low-pass filter only works with little vibrations (high freq vibrations)
		// Set output rate (15): 8000 / (1 + 15) = 500Hz
		mpu6500SetRate(15);
		// Set digital low-pass bandwidth
		mpu6500SetDLPFMode(MPU6500_DLPF_BW_256);
	#else
		// Too low DLPF bandwidth might cause instability and decrease agility
		// but it works well for handling vibrations and unbalanced propellers (that have low freq vibration)
		// Set output rate (1): 1000 / (1 + 1) = 500Hz
		mpu6500SetRate(1);
		// Set digital low-pass bandwidth
		mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
	#endif
I've found that to activate DLPF it's necessary to set FCHOICE_B[1:0] and ACCEL_FCHOICE_B + A_DLPF_CFG respectively on Gyro and Acc registers. But as I see only DLPF Configuration registers are setted that seems not enough, so I suppose that any DLPF are active on MPU with this configurations.
Also mpu6500SetRate(1); set both Gyro+Acc sample rate, so I tought if I have 8Khz from Gyro and 1Khz from Acc I'll never have same freq from both if I doesn't apply any filter to Gyro, that seems strange, maybe I miss something.
About HPF I've found that on MPU-9250 Register Map Datasheet, address of HPF (0x1C Acc. Register) is "Reserved", so I didn't found nothing on their datasheet.

Of course I've get this informations form "MPU-9250 Register Map and Descriptions Revision 1.4" and imu6Init function on CF Firmware. I'm not sure if datasheet is updated or if this setting are loaded in another function of CF Firmware (or even if I understood well what I've read).
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

Re: Doubts about Sensor values convertion and filtering

Post by DarkKnight »

- Order to get Accelerometer values in Imu6Read is: Read, Take Bias, Filter, Apply Bias and make conversion. I can't understand logic of this. Should it be more correct if I Filter before Take Bias or Filter after I've Applied bias and made conversion?
As the bias for the accelerometer isn't used and the IIRLPF isn't for the gyro it doesn't look so strange but could be arranged clearer and better.
GYRO: LPF[MPU] -> BIAS -> CONVERSION
ACC: RAW[MPU] -> IIRLPF -> ALIGN (-> BIAS) -> CONVERSION

hello, what do you mean the bias of the accelerometer isn't used? Does this means that the raw data of the acceleration contains the bias? And we have to eliminate bias of acceleration by ourselves?

What does the "ALIGN (-> BIAS)" means in your reply "ACC: RAW[MPU] -> IIRLPF -> ALIGN (-> BIAS) -> CONVERSION"?
tobias
Bitcraze
Posts: 2339
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: Doubts about Sensor values convertion and filtering

Post by tobias »

I think at some point the bias was not used for the accelerometer as it is not important for keeping a level flight and optional. For the CF2.1 the accelerometer bias at startup is taken by default.
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

Re: Doubts about Sensor values convertion and filtering

Post by DarkKnight »

Okay.Has the accelerometer bias at startup been compensated or not?
tobias
Bitcraze
Posts: 2339
Joined: Mon Jan 28, 2013 7:17 pm
Location: Sweden

Re: Doubts about Sensor values convertion and filtering

Post by tobias »

Yes, to a certain degree. It takes the earth acceleration vector and scales toward this. It does not do a full calibration on all axis.
Post Reply