Hi guys,
I have a question concerning the mahony algorithm driver. So basically I tried out the driver that’s delivered with the navio2 and I saw that the axes are wrong. The arrow on the Navio2 in my opinion is meant to point in the forward direction of the vehicle. So using a normal convention of x = front, y = right, z = down, and using the arrow as x axis, I shoud get positive roll when rotating my board to the right with respect to the arrow. However it’s the pitch that changes.By looking at the datasheets of the LSM and MPU I see that they have different axes for the magnetometer and for gyro/acc. Also they seem to have weird placements on the board as they don’t point in the same direction.
Therefore on my motorbike ( I want to estimate roll on it by placing the navio2 pointing forward on it) the axes are wrong. So I copied some pieces of code from the on-line mahony driver thing, and changed the axes in the mahony call parameters. Let me show you below :
`imu->update();
imu->read_accelerometer(&ay, &ax, &az);
az *= -1;
imu->read_gyroscope(&gy, &gx, &gz);
gz *= -1;
imu->read_magnetometer(&mx, &my, &mz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.update(-ax, -ay, -az, gx*0.0175, gy*0.0175, gz*0.0175, mx, my, mz, dt);`
That is the call to the function that gets the roll pitch yaw. It gives me correct values for my bike after this modification, however I might have some weird behavior. I noticed that I had a roll trend on my motorbike ( from the physical system) but It seemed too high, and I believe that it might be because of the initialization for the gyro calibration that I did wrong.
for(int i = 0; i<100; i++)
{
imu->update();
imu->read_gyroscope(&gx, &gy, &gz);
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
offset[0] += (-gx*0.0175);
offset[1] += (-gy*0.0175);
offset[2] += (-gz*0.0175);
usleep(10000);
}
offset[0]/=100.0;
offset[1]/=100.0;
offset[2]/=100.0;
printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
This is the initital code, and I changed it as such :
imu->read_gyroscope(&gy, &gx, &gz);
gz *= -1;
I think that I might be wrong and keep the initial part ( at least for the gyro calibration) but it’s really difficult to prove that it’s wrong. So could you maybe give me some piece of advice on the matter ? I have never used mahony before and I find some difficulties finding a simple diagram on the axes used in the algorithm.
Thanks a lot for the help !
tl;dr
I changed the axes in the call for the mahony functions and I get some “kinda normal results” but did I mess up ?
Code available Here