void loop() {
// Accelerometer
ACx = AccelX();
ACy = AccelY();
ACz = AccelZ();
pitch = calcPitch();
roll = calcRoll();
// HMC5843
int ix,iy,iz, heading;
float fx,fy,fz;
getValues(&ix,&iy,&iz); // Returned HMC values are scaled to common max, not calculated to rotation,
getValues(&fx,&fy,&fz);
// get tilt compensated heading
Xcomp = getCompX(fx, fy, fz);
Ycomp = getCompY(fx, fy, fz);
heading = getCompHeading(Xcomp, Ycomp);
}
//-----------------------------------------------------------
float calcPitch(){
return atan2(-ACx, sqrt(ACy * ACy + ACz * ACz));
}
float calcRoll(){
return atan2(ACy,sqrt(ACx * ACx + ACz * ACz));
}
int deg(float rad){
return (int)round(rad * 180/M_PI);
}
//////////////////////////////////////////////
// tilt compensation of magnetic vectors
float getCompX(float fx, float fy, float fz){
return fx * cos(pitch) + fy * sin(roll) * sin(pitch) - fz * cos(roll) * sin(pitch);
}
float getCompY(float fx, float fy, float fz){
return fy * cos(roll) + fz * sin(pitch);
}
int getCompHeading(float Xcomp, float Ycomp){
return round(deg(atan2(Ycomp, Xcomp) + M_PI));
}
Can someone find out why these functions don't work to compensate the heading for tilt?
Thanks for your comments!
Andreas