Can anyone provide me .c code for direction cosine matrix to transform body frame accelerometer measurements into earth frame, and account for gravity.

You need to be a member of diydrones to add comments!

then conver all your accelerometer results to g ex: assume your accelerometer reads z = 256 x = 0 y = 0 when its leveled so accel_z_scaled = 256/256 = 1g, accel_x_scaled = 0/256 = 0g, accel_y_scaled= 0/256 = 0g

then conver all your accelerometer results to g ex: assume your accelerometer reads z = 256 x = 0 y = 0 when its leveled so accel_z_scaled = 256/256 = 1g, accel_x_scaled = 0/256 = 0g, accel_y_scaled= 0/256 = 0g

## Replies

can you clarify

is q0 q1 q2 q3 is w x y z of quat or x y z w.

my data is like this

Sample acceleration data is

-0.92 0.18 10.15

-2.14 0.83 10.07

-1.05 0.23 10.14

And sample quat data is (Not normalized)

16363 201 754 -300

16363 201 754 -273

16364 201 753 -234

what do mean by accel_x_scaled? And what should i do in my case

Baris said:

@Baris can you clarify

is q0 q1 q2 q3 is w x y z of quat or x y z w.

my data is like this

Sample acceleration data is

-0.92 0.18 10.15

-2.14 0.83 10.07

-1.05 0.23 10.14

And sample quat data is (Not normalized)

16363 201 754 -300

16363 201 754 -273

16364 201 753 -234

what do mean by accel_x_scaled? And what should i do in my case

basicly convert your imu readings to quaternion then

gx, gy, gz represents gravity in x, y and z axis respectively

gx = 2 * (q1 * q3 - q0 * q2);

gy = 2 * (q0 * q1 + q2 * q3);

gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

the result of gx gy and gz will be unit vector

then conver all your accelerometer results to g ex: assume your accelerometer reads z = 256 x = 0 y = 0 when its leveled so accel_z_scaled = 256/256 = 1g, accel_x_scaled = 0/256 = 0g, accel_y_scaled= 0/256 = 0g

acc_x_wog = (accel_x_scaled - gx) * 9.8;

acc_y_wog = (accel_y_scaled - gy) * 9.8;

acc_z_wog = (accel_z_scaled - gz) * 9.8;

will give you linear acceleration in each axis even in any attitude

note: wog means: w/o gravity

and sorry this is not in cosine matrix, I only know in quaternion