Mercurial > public > hwos_code

comparison src/compass.c @ 628:cd58f7fc86db

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.05 stable work
author heinrichsweikamp
date Thu, 19 Sep 2019 12:01:29 +0200
parents c40025d8e750
children bc214815deb2
comparison
equal deleted inserted replaced
627:bf5fee575701 628:cd58f7fc86db
245 iBpy = compass_DY_f - compass_CY_f; // Y 245 iBpy = compass_DY_f - compass_CY_f; // Y
246 iBpz = compass_DZ_f - compass_CZ_f; // Z 246 iBpz = compass_DZ_f - compass_CZ_f; // Z
247 247
248 //---- Calculate sine and cosine of roll angle Phi ----------------------- 248 //---- Calculate sine and cosine of roll angle Phi -----------------------
249 sincos(accel_DZ_f, accel_DY_f, &sin, &cos); 249 sincos(accel_DZ_f, accel_DY_f, &sin, &cos);
250 250
251 //---- rotate by roll angle (-Phi) --------------------------------------- 251 //---- rotate by roll angle (-Phi) ---------------------------------------
252 iBfy = imul(iBpy, cos) - imul(iBpz, sin); 252 iBfy = imul(iBpy, cos) - imul(iBpz, sin);
253 iBpz = imul(iBpy, sin) + imul(iBpz, cos); 253 iBpz = imul(iBpy, sin) + imul(iBpz, cos);
254 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos); 254 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos);
255 255
256 //---- calculate sin and cosine of pitch angle Theta --------------------- 256 //---- calculate sin and cosine of pitch angle Theta ---------------------
257 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign 257 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign
258 258
259 /* correct cosine if pitch not in range -90 to 90 degrees */ 259 /* correct cosine if pitch not in range -90 to 90 degrees */
260 if( cos < 0 ) cos = -cos; 260 if( cos < 0 ) cos = -cos;
261 261
262 ///---- de-rotate by pitch angle Theta ----------------------------------- 262 ///---- de-rotate by pitch angle Theta -----------------------------------
263 iBfx = imul(iBpx, cos) + imul(iBpz, sin); 263 iBfx = imul(iBpx, cos) + imul(iBpz, sin);