Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.cpp
- Revision:
- 46:fd5a62296b12
- Parent:
- 43:6251c0169f4f
--- a/SensorFusion.cpp Wed May 27 11:45:00 2015 +0000 +++ b/SensorFusion.cpp Wed May 27 13:01:43 2015 +0000 @@ -23,14 +23,14 @@ lowpassX.reset(); lowpassY.reset(); lowpassZ.reset(); - + accel.powerOn(); accel.start(); - + // Since everything is synced to gyro interrupt, start it last gyro.powerOn(); gyro.start(); - + return true; } @@ -38,7 +38,7 @@ { gyro.stop(); gyro.powerOff(); - + accel.stop(); accel.powerOff(); } @@ -46,7 +46,7 @@ static float const deg_to_radian = 0.0174532925f; void SensorFusion6::sensorUpdate(Vector3 gyro_degrees) -{ +{ Vector3 const gyro_reading = gyro_degrees * deg_to_radian; Vector3 const accel_reading = accel.read(); Vector3 const filtered_accel = Vector3( lowpassX.filter(accel_reading.x), @@ -54,15 +54,15 @@ lowpassZ.filter(accel_reading.z)); updateFilter( filtered_accel.x, filtered_accel.y, filtered_accel.z, - gyro_reading.x, gyro_reading.y, gyro_reading.z); - + gyro_reading.x, gyro_reading.y, gyro_reading.z); + delegate->sensorTick(deltat, q.getEulerAngles(), filtered_accel, accel_reading, gyro_degrees, q); } void SensorFusion6::updateFilter(float ax, float ay, float az, float gx, float gy, float gz) { float q0 = q.w, q1 = q.v.x, q2 = q.v.y, q3 = q.v.z; // short name local variable for readability - + float recipNorm; float s0, s1, s2, s3; float qDot1, qDot2, qDot3, qDot4; @@ -81,7 +81,7 @@ recipNorm = 1.0 / sqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; - az *= recipNorm; + az *= recipNorm; // Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0 * q0; @@ -128,7 +128,7 @@ q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; - + // return q.w = q0; q.v.x = q1; @@ -145,7 +145,7 @@ { magneto.powerOn(); magneto.start(); - + return SensorFusion6::start(); } @@ -161,15 +161,15 @@ Vector3 const gyro_reading = gyro_degrees * deg_to_radian; Vector3 const accel_reading = accel.read(); Vector3 const magneto_reading = magneto.read(); - + Vector3 const filtered_accel = Vector3( lowpassX.filter(accel_reading.x), lowpassY.filter(accel_reading.y), lowpassZ.filter(accel_reading.z)); updateFilter( filtered_accel.x, filtered_accel.y, filtered_accel.z, - gyro_reading.x, gyro_reading.y, gyro_reading.z, - magneto_reading.x, magneto_reading.y, magneto_reading.z); - + gyro_reading.x, gyro_reading.y, gyro_reading.z, + magneto_reading.x, magneto_reading.y, magneto_reading.z); + delegate->sensorTick(deltat, q.getEulerAngles(), filtered_accel, magneto_reading, gyro_degrees, q); }