Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 1010:08f76d3e9856 GasConsumption

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Bugfix: Pitch compensation The pitch was not calculated correct during calibration process. As result the heading value is sensetive to pitch movment. The issue has been solved by applying the correct formular for calculation.
author Ideenmodellierer
date Tue, 06 May 2025 22:07:39 +0200
parents 65d35e66efb9
children
comparison
equal deleted inserted replaced
1009:3bcf592e9cc9 1010:08f76d3e9856
1105 Phi= atan2f(accel_DY_f, accel_DZ_f) ; 1105 Phi= atan2f(accel_DY_f, accel_DZ_f) ;
1106 compass_roll = Phi * 180.0f /PI; 1106 compass_roll = Phi * 180.0f /PI;
1107 sinPhi = sinf(Phi); 1107 sinPhi = sinf(Phi);
1108 cosPhi = cosf(Phi); 1108 cosPhi = cosf(Phi);
1109 1109
1110 Teta = atan2((double)accel_DX_f, sqrt((double)accel_DY_f * accel_DY_f + (double)accel_DZ_f * accel_DZ_f)); 1110 Teta = atanf(-(float)accel_DX_f/(accel_DY_f * sinPhi + accel_DZ_f * cosPhi));
1111 compass_pitch = Teta * (180.0 / PI); 1111 compass_pitch = Teta * (180.0 / PI);
1112 1112
1113 sinTeta = sinf(Teta); 1113 sinTeta = sinf(Teta);
1114 cosTeta = cosf(Teta); 1114 cosTeta = cosf(Teta);
1115 1115