Mercurial > public > hwos_code

comparison src/compass.c @ 214:a17359244d93

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
compass changes
author heinrichsweikamp
date Sun, 14 Dec 2014 13:56:05 +0100
parents 11d4fc797f74
children 7d9edd3b8c86
comparison
equal deleted inserted replaced
213:3b5df65d53fc 214:a17359244d93
208 iBpy = compass_DY_f - compass_CY_f; // Y 208 iBpy = compass_DY_f - compass_CY_f; // Y
209 iBpz = compass_DZ_f - compass_CZ_f; // Z 209 iBpz = compass_DZ_f - compass_CZ_f; // Z
210 210
211 //---- Calculate sine and cosine of roll angle Phi ----------------------- 211 //---- Calculate sine and cosine of roll angle Phi -----------------------
212 sincos(accel_DZ_f, accel_DY_f, &sin, &cos); 212 sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
213 compass_roll = itan(sin, cos) / 100; 213 // compass_roll = itan(sin, cos) / 100;
214 214
215 //---- rotate by roll angle (-Phi) --------------------------------------- 215 //---- rotate by roll angle (-Phi) ---------------------------------------
216 iBfy = imul(iBpy, cos) - imul(iBpz, sin); 216 iBfy = imul(iBpy, cos) - imul(iBpz, sin);
217 iBpz = imul(iBpy, sin) + imul(iBpz, cos); 217 iBpz = imul(iBpy, sin) + imul(iBpz, cos);
218 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos); 218 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
219 219
220 //---- calculate sin and cosine of pitch angle Theta --------------------- 220 //---- calculate sin and cosine of pitch angle Theta ---------------------
221 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign. 221 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign.
222 compass_pitch = itan(sin, cos) / 100; 222 // compass_pitch = itan(sin, cos) / 100;
223 223
224 /* correct cosine if pitch not in range -90 to 90 degrees */ 224 /* correct cosine if pitch not in range -90 to 90 degrees */
225 if( cos < 0 ) cos = -cos; 225 if( cos < 0 ) cos = -cos;
226 226
227 ///---- de-rotate by pitch angle Theta ----------------------------------- 227 ///---- de-rotate by pitch angle Theta -----------------------------------