summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorSebastian Reichel <sre@ring0.de>2012-10-14 22:43:03 +0200
committerSebastian Reichel <sre@ring0.de>2012-10-14 22:43:03 +0200
commit25ac6a9d3096ec740db3102a6a2ca33b20d6b1c5 (patch)
tree7faff8e5700607c3427cae200d5026af1192fa2d
parent0886c595b29dca6a708694a781f22c6d12a758d7 (diff)
downloadmicrocopterd-25ac6a9d3096ec740db3102a6a2ca33b20d6b1c5.tar.bz2
add log info to kinematics module
-rw-r--r--ctrl/kinematics.vala4
1 files changed, 4 insertions, 0 deletions
diff --git a/ctrl/kinematics.vala b/ctrl/kinematics.vala
index 0e90a8f..52c4921 100644
--- a/ctrl/kinematics.vala
+++ b/ctrl/kinematics.vala
@@ -68,6 +68,8 @@ public class Kinematics {
double exMag, eyMag, ezMag;
double q0i, q1i, q2i, q3i;
+ log("Kinematics", LogLevelFlags.LEVEL_DEBUG, @"sensors: GYRO: $gx $gy $gz, ACCEL: $ax $ay $az, COMPASS: $mx $my $mz");
+
halfT = time/2;
/* normalise the accelerometer measurements */
@@ -144,5 +146,7 @@ public class Kinematics {
kinematicsAngle[AXIS.X] = Math.atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2));
kinematicsAngle[AXIS.Y] = Math.asin(2 * (q0*q2 - q1*q3));
kinematicsAngle[AXIS.Z] = Math.atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));
+
+ log("Kinematics", LogLevelFlags.LEVEL_INFO, @"angles: $(kinematicsAngle[AXIS.X]), $(kinematicsAngle[AXIS.Y]), $(kinematicsAngle[AXIS.Z])");
}
}