diff options
| -rwxr-xr-x | core/java/android/view/WindowOrientationListener.java | 386 | ||||
| -rwxr-xr-x | tools/orientationplot/orientationplot.py | 116 |
2 files changed, 303 insertions, 199 deletions
diff --git a/core/java/android/view/WindowOrientationListener.java b/core/java/android/view/WindowOrientationListener.java index c3c74a7c59c5..c28b22050519 100755 --- a/core/java/android/view/WindowOrientationListener.java +++ b/core/java/android/view/WindowOrientationListener.java @@ -21,6 +21,7 @@ import android.hardware.Sensor; import android.hardware.SensorEvent; import android.hardware.SensorEventListener; import android.hardware.SensorManager; +import android.util.FloatMath; import android.util.Log; import android.util.Slog; @@ -48,6 +49,8 @@ public abstract class WindowOrientationListener { private static final boolean DEBUG = false; private static final boolean localLOGV = DEBUG || false; + private static final boolean USE_GRAVITY_SENSOR = false; + private SensorManager mSensorManager; private boolean mEnabled; private int mRate; @@ -79,7 +82,8 @@ public abstract class WindowOrientationListener { private WindowOrientationListener(Context context, int rate) { mSensorManager = (SensorManager)context.getSystemService(Context.SENSOR_SERVICE); mRate = rate; - mSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER); + mSensor = mSensorManager.getDefaultSensor(USE_GRAVITY_SENSOR + ? Sensor.TYPE_GRAVITY : Sensor.TYPE_ACCELEROMETER); if (mSensor != null) { // Create listener only if sensors do exist mSensorEventListener = new SensorEventListenerImpl(this); @@ -179,7 +183,7 @@ public abstract class WindowOrientationListener { * cartesian space because the orientation calculations are sensitive to the * absolute magnitude of the acceleration. In particular, there are singularities * in the calculation as the magnitude approaches 0. By performing the low-pass - * filtering early, we can eliminate high-frequency impulses systematically. + * filtering early, we can eliminate most spurious high-frequency impulses due to noise. * * - Convert the acceleromter vector from cartesian to spherical coordinates. * Since we're dealing with rotation of the device, this is the sensible coordinate @@ -204,11 +208,17 @@ public abstract class WindowOrientationListener { * new orientation proposal. * * Details are explained inline. + * + * See http://en.wikipedia.org/wiki/Low-pass_filter#Discrete-time_realization for + * signal processing background. */ static final class SensorEventListenerImpl implements SensorEventListener { // We work with all angles in degrees in this class. private static final float RADIANS_TO_DEGREES = (float) (180 / Math.PI); + // Number of nanoseconds per millisecond. + private static final long NANOS_PER_MS = 1000000; + // Indices into SensorEvent.values for the accelerometer sensor. private static final int ACCELEROMETER_DATA_X = 0; private static final int ACCELEROMETER_DATA_Y = 1; @@ -216,38 +226,41 @@ public abstract class WindowOrientationListener { private final WindowOrientationListener mOrientationListener; - /* State for first order low-pass filtering of accelerometer data. - * See http://en.wikipedia.org/wiki/Low-pass_filter#Discrete-time_realization for - * signal processing background. - */ - - private long mLastTimestamp = Long.MAX_VALUE; // in nanoseconds - private float mLastFilteredX, mLastFilteredY, mLastFilteredZ; - - // The current proposal. We wait for the proposal to be stable for a - // certain amount of time before accepting it. - // - // The basic idea is to ignore intermediate poses of the device while the - // user is picking up, putting down or turning the device. - private int mProposalRotation; - private long mProposalAgeMS; - - // A historical trace of tilt and orientation angles. Used to determine whether - // the device posture has settled down. - private static final int HISTORY_SIZE = 20; - private int mHistoryIndex; // index of most recent sample - private int mHistoryLength; // length of historical trace - private final long[] mHistoryTimestampMS = new long[HISTORY_SIZE]; - private final float[] mHistoryMagnitudes = new float[HISTORY_SIZE]; - private final int[] mHistoryTiltAngles = new int[HISTORY_SIZE]; - private final int[] mHistoryOrientationAngles = new int[HISTORY_SIZE]; + // The minimum amount of time that a predicted rotation must be stable before it + // is accepted as a valid rotation proposal. This value can be quite small because + // the low-pass filter already suppresses most of the noise so we're really just + // looking for quick confirmation that the last few samples are in agreement as to + // the desired orientation. + private static final long PROPOSAL_SETTLE_TIME_NANOS = 40 * NANOS_PER_MS; + + // The minimum amount of time that must have elapsed since the device last exited + // the flat state (time since it was picked up) before the proposed rotation + // can change. + private static final long PROPOSAL_MIN_TIME_SINCE_FLAT_ENDED_NANOS = 500 * NANOS_PER_MS; + + // The mininum amount of time that must have elapsed since the device stopped + // swinging (time since device appeared to be in the process of being put down + // or put away into a pocket) before the proposed rotation can change. + private static final long PROPOSAL_MIN_TIME_SINCE_SWING_ENDED_NANOS = 300 * NANOS_PER_MS; + + // If the tilt angle remains greater than the specified angle for a minimum of + // the specified time, then the device is deemed to be lying flat + // (just chillin' on a table). + private static final float FLAT_ANGLE = 75; + private static final long FLAT_TIME_NANOS = 1000 * NANOS_PER_MS; + + // If the tilt angle has increased by at least delta degrees within the specified amount + // of time, then the device is deemed to be swinging away from the user + // down towards flat (tilt = 90). + private static final float SWING_AWAY_ANGLE_DELTA = 20; + private static final long SWING_TIME_NANOS = 300 * NANOS_PER_MS; // The maximum sample inter-arrival time in milliseconds. // If the acceleration samples are further apart than this amount in time, we reset the // state of the low-pass filter and orientation properties. This helps to handle // boundary conditions when the device is turned on, wakes from suspend or there is // a significant gap in samples. - private static final float MAX_FILTER_DELTA_TIME_MS = 1000; + private static final long MAX_FILTER_DELTA_TIME_NANOS = 1000 * NANOS_PER_MS; // The acceleration filter time constant. // @@ -267,8 +280,10 @@ public abstract class WindowOrientationListener { // // Filtering adds latency proportional the time constant (inversely proportional // to the cutoff frequency) so we don't want to make the time constant too - // large or we can lose responsiveness. - private static final float FILTER_TIME_CONSTANT_MS = 100.0f; + // large or we can lose responsiveness. Likewise we don't want to make it too + // small or we do a poor job suppressing acceleration spikes. + // Empirically, 100ms seems to be too small and 500ms is too large. + private static final float FILTER_TIME_CONSTANT_MS = 200.0f; /* State for orientation detection. */ @@ -286,9 +301,9 @@ public abstract class WindowOrientationListener { // // In both cases, we postpone choosing an orientation. private static final float MIN_ACCELERATION_MAGNITUDE = - SensorManager.STANDARD_GRAVITY * 0.5f; + SensorManager.STANDARD_GRAVITY * 0.3f; private static final float MAX_ACCELERATION_MAGNITUDE = - SensorManager.STANDARD_GRAVITY * 1.5f; + SensorManager.STANDARD_GRAVITY * 1.25f; // Maximum absolute tilt angle at which to consider orientation data. Beyond this (i.e. // when screen is facing the sky or ground), we completely ignore orientation data. @@ -306,10 +321,10 @@ public abstract class WindowOrientationListener { // The ideal tilt angle is 0 (when the device is vertical) so the limits establish // how close to vertical the device must be in order to change orientation. private static final int[][] TILT_TOLERANCE = new int[][] { - /* ROTATION_0 */ { -20, 70 }, - /* ROTATION_90 */ { -20, 60 }, - /* ROTATION_180 */ { -20, 50 }, - /* ROTATION_270 */ { -20, 60 } + /* ROTATION_0 */ { -25, 70 }, + /* ROTATION_90 */ { -25, 65 }, + /* ROTATION_180 */ { -25, 60 }, + /* ROTATION_270 */ { -25, 65 } }; // The gap angle in degrees between adjacent orientation angles for hysteresis. @@ -319,29 +334,38 @@ public abstract class WindowOrientationListener { // orientation. private static final int ADJACENT_ORIENTATION_ANGLE_GAP = 45; - // The number of milliseconds for which the device posture must be stable - // before we perform an orientation change. If the device appears to be rotating - // (being picked up, put down) then we keep waiting until it settles. - private static final int SETTLE_TIME_MS = 200; + // Timestamp and value of the last accelerometer sample. + private long mLastFilteredTimestampNanos; + private float mLastFilteredX, mLastFilteredY, mLastFilteredZ; + + // The last proposed rotation, -1 if unknown. + private int mProposedRotation; + + // Value of the current predicted rotation, -1 if unknown. + private int mPredictedRotation; + + // Timestamp of when the predicted rotation most recently changed. + private long mPredictedRotationTimestampNanos; - // The maximum change in magnitude that can occur during the settle time. - // Tuning this constant particularly helps to filter out situations where the - // device is being picked up or put down by the user. - private static final float SETTLE_MAGNITUDE_MAX_DELTA = - SensorManager.STANDARD_GRAVITY * 0.2f; + // Timestamp when the device last appeared to be flat for sure (the flat delay elapsed). + private long mFlatTimestampNanos; - // The maximum change in tilt angle that can occur during the settle time. - private static final int SETTLE_TILT_ANGLE_MAX_DELTA = 5; + // Timestamp when the device last appeared to be swinging. + private long mSwingTimestampNanos; - // The maximum change in orientation angle that can occur during the settle time. - private static final int SETTLE_ORIENTATION_ANGLE_MAX_DELTA = 5; + // History of observed tilt angles. + private static final int TILT_HISTORY_SIZE = 40; + private float[] mTiltHistory = new float[TILT_HISTORY_SIZE]; + private long[] mTiltHistoryTimestampNanos = new long[TILT_HISTORY_SIZE]; + private int mTiltHistoryIndex; public SensorEventListenerImpl(WindowOrientationListener orientationListener) { mOrientationListener = orientationListener; + reset(); } public int getProposedRotation() { - return mProposalAgeMS >= SETTLE_TIME_MS ? mProposalRotation : -1; + return mProposedRotation; } @Override @@ -359,8 +383,9 @@ public abstract class WindowOrientationListener { float z = event.values[ACCELEROMETER_DATA_Z]; if (log) { - Slog.v(TAG, "Raw acceleration vector: " + - "x=" + x + ", y=" + y + ", z=" + z); + Slog.v(TAG, "Raw acceleration vector: " + + "x=" + x + ", y=" + y + ", z=" + z + + ", magnitude=" + FloatMath.sqrt(x * x + y * y + z * z)); } // Apply a low-pass filter to the acceleration up vector in cartesian space. @@ -368,14 +393,16 @@ public abstract class WindowOrientationListener { // or when we see values of (0, 0, 0) which indicates that we polled the // accelerometer too soon after turning it on and we don't have any data yet. final long now = event.timestamp; - final float timeDeltaMS = (now - mLastTimestamp) * 0.000001f; - boolean skipSample; - if (timeDeltaMS <= 0 || timeDeltaMS > MAX_FILTER_DELTA_TIME_MS + final long then = mLastFilteredTimestampNanos; + final float timeDeltaMS = (now - then) * 0.000001f; + final boolean skipSample; + if (now < then + || now > then + MAX_FILTER_DELTA_TIME_NANOS || (x == 0 && y == 0 && z == 0)) { if (log) { Slog.v(TAG, "Resetting orientation listener."); } - clearProposal(); + reset(); skipSample = true; } else { final float alpha = timeDeltaMS / (FILTER_TIME_CONSTANT_MS + timeDeltaMS); @@ -383,27 +410,28 @@ public abstract class WindowOrientationListener { y = alpha * (y - mLastFilteredY) + mLastFilteredY; z = alpha * (z - mLastFilteredZ) + mLastFilteredZ; if (log) { - Slog.v(TAG, "Filtered acceleration vector: " + - "x=" + x + ", y=" + y + ", z=" + z); + Slog.v(TAG, "Filtered acceleration vector: " + + "x=" + x + ", y=" + y + ", z=" + z + + ", magnitude=" + FloatMath.sqrt(x * x + y * y + z * z)); } skipSample = false; } - mLastTimestamp = now; + mLastFilteredTimestampNanos = now; mLastFilteredX = x; mLastFilteredY = y; mLastFilteredZ = z; - final int oldProposedRotation = getProposedRotation(); + boolean isFlat = false; + boolean isSwinging = false; if (!skipSample) { // Calculate the magnitude of the acceleration vector. - final float magnitude = (float) Math.sqrt(x * x + y * y + z * z); + final float magnitude = FloatMath.sqrt(x * x + y * y + z * z); if (magnitude < MIN_ACCELERATION_MAGNITUDE || magnitude > MAX_ACCELERATION_MAGNITUDE) { if (log) { - Slog.v(TAG, "Ignoring sensor data, magnitude out of range: " - + "magnitude=" + magnitude); + Slog.v(TAG, "Ignoring sensor data, magnitude out of range."); } - clearProposal(); + clearPredictedRotation(); } else { // Calculate the tilt angle. // This is the angle between the up vector and the x-y plane (the plane of @@ -414,14 +442,25 @@ public abstract class WindowOrientationListener { final int tiltAngle = (int) Math.round( Math.asin(z / magnitude) * RADIANS_TO_DEGREES); + // Determine whether the device appears to be flat or swinging. + if (isFlat(now)) { + isFlat = true; + mFlatTimestampNanos = now; + } + if (isSwinging(now, tiltAngle)) { + isSwinging = true; + mSwingTimestampNanos = now; + } + addTiltHistoryEntry(now, tiltAngle); + // If the tilt angle is too close to horizontal then we cannot determine // the orientation angle of the screen. if (Math.abs(tiltAngle) > MAX_TILT) { if (log) { Slog.v(TAG, "Ignoring sensor data, tilt angle too high: " - + "magnitude=" + magnitude + ", tiltAngle=" + tiltAngle); + + "tiltAngle=" + tiltAngle); } - clearProposal(); + clearPredictedRotation(); } else { // Calculate the orientation angle. // This is the angle between the x-y projection of the up vector onto @@ -439,89 +478,93 @@ public abstract class WindowOrientationListener { nearestRotation = 0; } - // Determine the proposed orientation. - // The confidence of the proposal is 1.0 when it is ideal and it - // decays exponentially as the proposal moves further from the ideal - // angle, tilt and magnitude of the proposed orientation. - if (!isTiltAngleAcceptable(nearestRotation, tiltAngle) - || !isOrientationAngleAcceptable(nearestRotation, + // Determine the predicted orientation. + if (isTiltAngleAcceptable(nearestRotation, tiltAngle) + && isOrientationAngleAcceptable(nearestRotation, orientationAngle)) { + updatePredictedRotation(now, nearestRotation); if (log) { - Slog.v(TAG, "Ignoring sensor data, no proposal: " - + "magnitude=" + magnitude + ", tiltAngle=" + tiltAngle - + ", orientationAngle=" + orientationAngle); + Slog.v(TAG, "Predicted: " + + "tiltAngle=" + tiltAngle + + ", orientationAngle=" + orientationAngle + + ", predictedRotation=" + mPredictedRotation + + ", predictedRotationAgeMS=" + + ((now - mPredictedRotationTimestampNanos) + * 0.000001f)); } - clearProposal(); } else { if (log) { - Slog.v(TAG, "Proposal: " - + "magnitude=" + magnitude - + ", tiltAngle=" + tiltAngle - + ", orientationAngle=" + orientationAngle - + ", proposalRotation=" + mProposalRotation); + Slog.v(TAG, "Ignoring sensor data, no predicted rotation: " + + "tiltAngle=" + tiltAngle + + ", orientationAngle=" + orientationAngle); } - updateProposal(nearestRotation, now / 1000000L, - magnitude, tiltAngle, orientationAngle); + clearPredictedRotation(); } } } } + // Determine new proposed rotation. + final int oldProposedRotation = mProposedRotation; + if (mPredictedRotation < 0 || isPredictedRotationAcceptable(now)) { + mProposedRotation = mPredictedRotation; + } + // Write final statistics about where we are in the orientation detection process. - final int proposedRotation = getProposedRotation(); if (log) { - final float proposalConfidence = Math.min( - mProposalAgeMS * 1.0f / SETTLE_TIME_MS, 1.0f); Slog.v(TAG, "Result: currentRotation=" + mOrientationListener.mCurrentRotation - + ", proposedRotation=" + proposedRotation + + ", proposedRotation=" + mProposedRotation + + ", predictedRotation=" + mPredictedRotation + ", timeDeltaMS=" + timeDeltaMS - + ", proposalRotation=" + mProposalRotation - + ", proposalAgeMS=" + mProposalAgeMS - + ", proposalConfidence=" + proposalConfidence); + + ", isFlat=" + isFlat + + ", isSwinging=" + isSwinging + + ", timeUntilSettledMS=" + remainingMS(now, + mPredictedRotationTimestampNanos + PROPOSAL_SETTLE_TIME_NANOS) + + ", timeUntilFlatDelayExpiredMS=" + remainingMS(now, + mFlatTimestampNanos + PROPOSAL_MIN_TIME_SINCE_FLAT_ENDED_NANOS) + + ", timeUntilSwingDelayExpiredMS=" + remainingMS(now, + mSwingTimestampNanos + PROPOSAL_MIN_TIME_SINCE_SWING_ENDED_NANOS)); } // Tell the listener. - if (proposedRotation != oldProposedRotation && proposedRotation >= 0) { + if (mProposedRotation != oldProposedRotation && mProposedRotation >= 0) { if (log) { - Slog.v(TAG, "Proposed rotation changed! proposedRotation=" + proposedRotation + Slog.v(TAG, "Proposed rotation changed! proposedRotation=" + mProposedRotation + ", oldProposedRotation=" + oldProposedRotation); } - mOrientationListener.onProposedRotationChanged(proposedRotation); + mOrientationListener.onProposedRotationChanged(mProposedRotation); } } /** - * Returns true if the tilt angle is acceptable for a proposed - * orientation transition. + * Returns true if the tilt angle is acceptable for a given predicted rotation. */ - private boolean isTiltAngleAcceptable(int proposedRotation, - int tiltAngle) { - return tiltAngle >= TILT_TOLERANCE[proposedRotation][0] - && tiltAngle <= TILT_TOLERANCE[proposedRotation][1]; + private boolean isTiltAngleAcceptable(int rotation, int tiltAngle) { + return tiltAngle >= TILT_TOLERANCE[rotation][0] + && tiltAngle <= TILT_TOLERANCE[rotation][1]; } /** - * Returns true if the orientation angle is acceptable for a proposed - * orientation transition. + * Returns true if the orientation angle is acceptable for a given predicted rotation. * * This function takes into account the gap between adjacent orientations * for hysteresis. */ - private boolean isOrientationAngleAcceptable(int proposedRotation, int orientationAngle) { + private boolean isOrientationAngleAcceptable(int rotation, int orientationAngle) { // If there is no current rotation, then there is no gap. // The gap is used only to introduce hysteresis among advertised orientation // changes to avoid flapping. final int currentRotation = mOrientationListener.mCurrentRotation; if (currentRotation >= 0) { - // If the proposed rotation is the same or is counter-clockwise adjacent, - // then we set a lower bound on the orientation angle. + // If the specified rotation is the same or is counter-clockwise adjacent + // to the current rotation, then we set a lower bound on the orientation angle. // For example, if currentRotation is ROTATION_0 and proposed is ROTATION_90, // then we want to check orientationAngle > 45 + GAP / 2. - if (proposedRotation == currentRotation - || proposedRotation == (currentRotation + 1) % 4) { - int lowerBound = proposedRotation * 90 - 45 + if (rotation == currentRotation + || rotation == (currentRotation + 1) % 4) { + int lowerBound = rotation * 90 - 45 + ADJACENT_ORIENTATION_ANGLE_GAP / 2; - if (proposedRotation == 0) { + if (rotation == 0) { if (orientationAngle >= 315 && orientationAngle < lowerBound + 360) { return false; } @@ -532,15 +575,15 @@ public abstract class WindowOrientationListener { } } - // If the proposed rotation is the same or is clockwise adjacent, + // If the specified rotation is the same or is clockwise adjacent, // then we set an upper bound on the orientation angle. - // For example, if currentRotation is ROTATION_0 and proposed is ROTATION_270, + // For example, if currentRotation is ROTATION_0 and rotation is ROTATION_270, // then we want to check orientationAngle < 315 - GAP / 2. - if (proposedRotation == currentRotation - || proposedRotation == (currentRotation + 3) % 4) { - int upperBound = proposedRotation * 90 + 45 + if (rotation == currentRotation + || rotation == (currentRotation + 3) % 4) { + int upperBound = rotation * 90 + 45 - ADJACENT_ORIENTATION_ANGLE_GAP / 2; - if (proposedRotation == 0) { + if (rotation == 0) { if (orientationAngle <= 45 && orientationAngle > upperBound) { return false; } @@ -554,58 +597,97 @@ public abstract class WindowOrientationListener { return true; } - private void clearProposal() { - mProposalRotation = -1; - mProposalAgeMS = 0; - } + /** + * Returns true if the predicted rotation is ready to be advertised as a + * proposed rotation. + */ + private boolean isPredictedRotationAcceptable(long now) { + // The predicted rotation must have settled long enough. + if (now < mPredictedRotationTimestampNanos + PROPOSAL_SETTLE_TIME_NANOS) { + return false; + } + + // The last flat state (time since picked up) must have been sufficiently long ago. + if (now < mFlatTimestampNanos + PROPOSAL_MIN_TIME_SINCE_FLAT_ENDED_NANOS) { + return false; + } - private void updateProposal(int rotation, long timestampMS, - float magnitude, int tiltAngle, int orientationAngle) { - if (mProposalRotation != rotation) { - mProposalRotation = rotation; - mHistoryIndex = 0; - mHistoryLength = 0; + // The last swing state (time since last movement to put down) must have been + // sufficiently long ago. + if (now < mSwingTimestampNanos + PROPOSAL_MIN_TIME_SINCE_SWING_ENDED_NANOS) { + return false; } - final int index = mHistoryIndex; - mHistoryTimestampMS[index] = timestampMS; - mHistoryMagnitudes[index] = magnitude; - mHistoryTiltAngles[index] = tiltAngle; - mHistoryOrientationAngles[index] = orientationAngle; - mHistoryIndex = (index + 1) % HISTORY_SIZE; - if (mHistoryLength < HISTORY_SIZE) { - mHistoryLength += 1; + // Looks good! + return true; + } + + private void reset() { + mLastFilteredTimestampNanos = Long.MIN_VALUE; + mProposedRotation = -1; + mFlatTimestampNanos = Long.MIN_VALUE; + mSwingTimestampNanos = Long.MIN_VALUE; + clearPredictedRotation(); + clearTiltHistory(); + } + + private void clearPredictedRotation() { + mPredictedRotation = -1; + mPredictedRotationTimestampNanos = Long.MIN_VALUE; + } + + private void updatePredictedRotation(long now, int rotation) { + if (mPredictedRotation != rotation) { + mPredictedRotation = rotation; + mPredictedRotationTimestampNanos = now; } + } - long age = 0; - for (int i = 1; i < mHistoryLength; i++) { - final int olderIndex = (index + HISTORY_SIZE - i) % HISTORY_SIZE; - if (Math.abs(mHistoryMagnitudes[olderIndex] - magnitude) - > SETTLE_MAGNITUDE_MAX_DELTA) { + private void clearTiltHistory() { + mTiltHistoryTimestampNanos[0] = Long.MIN_VALUE; + mTiltHistoryIndex = 1; + } + + private void addTiltHistoryEntry(long now, float tilt) { + mTiltHistory[mTiltHistoryIndex] = tilt; + mTiltHistoryTimestampNanos[mTiltHistoryIndex] = now; + mTiltHistoryIndex = (mTiltHistoryIndex + 1) % TILT_HISTORY_SIZE; + mTiltHistoryTimestampNanos[mTiltHistoryIndex] = Long.MIN_VALUE; + } + + private boolean isFlat(long now) { + for (int i = mTiltHistoryIndex; (i = nextTiltHistoryIndex(i)) >= 0; ) { + if (mTiltHistory[i] < FLAT_ANGLE) { break; } - if (angleAbsoluteDelta(mHistoryTiltAngles[olderIndex], - tiltAngle) > SETTLE_TILT_ANGLE_MAX_DELTA) { - break; + if (mTiltHistoryTimestampNanos[i] + FLAT_TIME_NANOS <= now) { + // Tilt has remained greater than FLAT_TILT_ANGLE for FLAT_TIME_NANOS. + return true; } - if (angleAbsoluteDelta(mHistoryOrientationAngles[olderIndex], - orientationAngle) > SETTLE_ORIENTATION_ANGLE_MAX_DELTA) { + } + return false; + } + + private boolean isSwinging(long now, float tilt) { + for (int i = mTiltHistoryIndex; (i = nextTiltHistoryIndex(i)) >= 0; ) { + if (mTiltHistoryTimestampNanos[i] + SWING_TIME_NANOS < now) { break; } - age = timestampMS - mHistoryTimestampMS[olderIndex]; - if (age >= SETTLE_TIME_MS) { - break; + if (mTiltHistory[i] + SWING_AWAY_ANGLE_DELTA <= tilt) { + // Tilted away by SWING_AWAY_ANGLE_DELTA within SWING_TIME_NANOS. + return true; } } - mProposalAgeMS = age; + return false; } - private static int angleAbsoluteDelta(int a, int b) { - int delta = Math.abs(a - b); - if (delta > 180) { - delta = 360 - delta; - } - return delta; + private int nextTiltHistoryIndex(int index) { + index = (index == 0 ? TILT_HISTORY_SIZE : index) - 1; + return mTiltHistoryTimestampNanos[index] != Long.MIN_VALUE ? index : -1; + } + + private static float remainingMS(long now, long until) { + return now >= until ? 0 : (until - now) * 0.000001f; } } } diff --git a/tools/orientationplot/orientationplot.py b/tools/orientationplot/orientationplot.py index 3a44cb2cbfd8..f4e6b453c9df 100755 --- a/tools/orientationplot/orientationplot.py +++ b/tools/orientationplot/orientationplot.py @@ -82,6 +82,7 @@ class Plotter: self.raw_acceleration_x = self._make_timeseries() self.raw_acceleration_y = self._make_timeseries() self.raw_acceleration_z = self._make_timeseries() + self.raw_acceleration_magnitude = self._make_timeseries() self.raw_acceleration_axes = self._add_timeseries_axes( 1, 'Raw Acceleration', 'm/s^2', [-20, 20], yticks=range(-15, 16, 5)) @@ -91,6 +92,8 @@ class Plotter: self.raw_acceleration_axes, 'y', 'green') self.raw_acceleration_line_z = self._add_timeseries_line( self.raw_acceleration_axes, 'z', 'blue') + self.raw_acceleration_line_magnitude = self._add_timeseries_line( + self.raw_acceleration_axes, 'magnitude', 'orange', linewidth=2) self._add_timeseries_legend(self.raw_acceleration_axes) shared_axis = self.raw_acceleration_axes @@ -98,7 +101,7 @@ class Plotter: self.filtered_acceleration_x = self._make_timeseries() self.filtered_acceleration_y = self._make_timeseries() self.filtered_acceleration_z = self._make_timeseries() - self.magnitude = self._make_timeseries() + self.filtered_acceleration_magnitude = self._make_timeseries() self.filtered_acceleration_axes = self._add_timeseries_axes( 2, 'Filtered Acceleration', 'm/s^2', [-20, 20], sharex=shared_axis, @@ -109,7 +112,7 @@ class Plotter: self.filtered_acceleration_axes, 'y', 'green') self.filtered_acceleration_line_z = self._add_timeseries_line( self.filtered_acceleration_axes, 'z', 'blue') - self.magnitude_line = self._add_timeseries_line( + self.filtered_acceleration_line_magnitude = self._add_timeseries_line( self.filtered_acceleration_axes, 'magnitude', 'orange', linewidth=2) self._add_timeseries_legend(self.filtered_acceleration_axes) @@ -133,32 +136,46 @@ class Plotter: self.current_rotation = self._make_timeseries() self.proposed_rotation = self._make_timeseries() - self.proposal_rotation = self._make_timeseries() + self.predicted_rotation = self._make_timeseries() self.orientation_axes = self._add_timeseries_axes( - 5, 'Current / Proposed Orientation and Confidence', 'rotation', [-1, 4], + 5, 'Current / Proposed Orientation', 'rotation', [-1, 4], sharex=shared_axis, yticks=range(0, 4)) self.current_rotation_line = self._add_timeseries_line( self.orientation_axes, 'current', 'black', linewidth=2) - self.proposal_rotation_line = self._add_timeseries_line( - self.orientation_axes, 'proposal', 'purple', linewidth=3) + self.predicted_rotation_line = self._add_timeseries_line( + self.orientation_axes, 'predicted', 'purple', linewidth=3) self.proposed_rotation_line = self._add_timeseries_line( self.orientation_axes, 'proposed', 'green', linewidth=3) self._add_timeseries_legend(self.orientation_axes) - self.proposal_confidence = [[self._make_timeseries(), self._make_timeseries()] - for i in range(0, 4)] - self.proposal_confidence_polys = [] + self.time_until_settled = self._make_timeseries() + self.time_until_flat_delay_expired = self._make_timeseries() + self.time_until_swing_delay_expired = self._make_timeseries() + self.stability_axes = self._add_timeseries_axes( + 6, 'Proposal Stability', 'ms', [-10, 600], + sharex=shared_axis, + yticks=range(0, 600, 100)) + self.time_until_settled_line = self._add_timeseries_line( + self.stability_axes, 'time until settled', 'black', linewidth=2) + self.time_until_flat_delay_expired_line = self._add_timeseries_line( + self.stability_axes, 'time until flat delay expired', 'green') + self.time_until_swing_delay_expired_line = self._add_timeseries_line( + self.stability_axes, 'time until swing delay expired', 'blue') + self._add_timeseries_legend(self.stability_axes) self.sample_latency = self._make_timeseries() self.sample_latency_axes = self._add_timeseries_axes( - 6, 'Accelerometer Sampling Latency', 'ms', [-10, 500], + 7, 'Accelerometer Sampling Latency', 'ms', [-10, 500], sharex=shared_axis, yticks=range(0, 500, 100)) self.sample_latency_line = self._add_timeseries_line( self.sample_latency_axes, 'latency', 'black') self._add_timeseries_legend(self.sample_latency_axes) + self.fig.canvas.mpl_connect('button_press_event', self._on_click) + self.paused = False + self.timer = self.fig.canvas.new_timer(interval=100) self.timer.add_callback(lambda: self.update()) self.timer.start() @@ -166,13 +183,22 @@ class Plotter: self.timebase = None self._reset_parse_state() + # Handle a click event to pause or restart the timer. + def _on_click(self, ev): + if not self.paused: + self.paused = True + self.timer.stop() + else: + self.paused = False + self.timer.start() + # Initialize a time series. def _make_timeseries(self): return [[], []] # Add a subplot to the figure for a time series. def _add_timeseries_axes(self, index, title, ylabel, ylim, yticks, sharex=None): - num_graphs = 6 + num_graphs = 7 height = 0.9 / num_graphs top = 0.95 - height * index axes = self.fig.add_axes([0.1, top, 0.8, height], @@ -214,16 +240,19 @@ class Plotter: self.parse_raw_acceleration_x = None self.parse_raw_acceleration_y = None self.parse_raw_acceleration_z = None + self.parse_raw_acceleration_magnitude = None self.parse_filtered_acceleration_x = None self.parse_filtered_acceleration_y = None self.parse_filtered_acceleration_z = None - self.parse_magnitude = None + self.parse_filtered_acceleration_magnitude = None self.parse_tilt_angle = None self.parse_orientation_angle = None self.parse_current_rotation = None self.parse_proposed_rotation = None - self.parse_proposal_rotation = None - self.parse_proposal_confidence = None + self.parse_predicted_rotation = None + self.parse_time_until_settled = None + self.parse_time_until_flat_delay_expired = None + self.parse_time_until_swing_delay_expired = None self.parse_sample_latency = None # Update samples. @@ -252,14 +281,13 @@ class Plotter: self.parse_raw_acceleration_x = self._get_following_number(line, 'x=') self.parse_raw_acceleration_y = self._get_following_number(line, 'y=') self.parse_raw_acceleration_z = self._get_following_number(line, 'z=') + self.parse_raw_acceleration_magnitude = self._get_following_number(line, 'magnitude=') if line.find('Filtered acceleration vector:') != -1: self.parse_filtered_acceleration_x = self._get_following_number(line, 'x=') self.parse_filtered_acceleration_y = self._get_following_number(line, 'y=') self.parse_filtered_acceleration_z = self._get_following_number(line, 'z=') - - if line.find('magnitude=') != -1: - self.parse_magnitude = self._get_following_number(line, 'magnitude=') + self.parse_filtered_acceleration_magnitude = self._get_following_number(line, 'magnitude=') if line.find('tiltAngle=') != -1: self.parse_tilt_angle = self._get_following_number(line, 'tiltAngle=') @@ -270,17 +298,20 @@ class Plotter: if line.find('Result:') != -1: self.parse_current_rotation = self._get_following_number(line, 'currentRotation=') self.parse_proposed_rotation = self._get_following_number(line, 'proposedRotation=') - self.parse_proposal_rotation = self._get_following_number(line, 'proposalRotation=') - self.parse_proposal_confidence = self._get_following_number(line, 'proposalConfidence=') + self.parse_predicted_rotation = self._get_following_number(line, 'predictedRotation=') self.parse_sample_latency = self._get_following_number(line, 'timeDeltaMS=') + self.parse_time_until_settled = self._get_following_number(line, 'timeUntilSettledMS=') + self.parse_time_until_flat_delay_expired = self._get_following_number(line, 'timeUntilFlatDelayExpiredMS=') + self.parse_time_until_swing_delay_expired = self._get_following_number(line, 'timeUntilSwingDelayExpiredMS=') self._append(self.raw_acceleration_x, timeindex, self.parse_raw_acceleration_x) self._append(self.raw_acceleration_y, timeindex, self.parse_raw_acceleration_y) self._append(self.raw_acceleration_z, timeindex, self.parse_raw_acceleration_z) + self._append(self.raw_acceleration_magnitude, timeindex, self.parse_raw_acceleration_magnitude) self._append(self.filtered_acceleration_x, timeindex, self.parse_filtered_acceleration_x) self._append(self.filtered_acceleration_y, timeindex, self.parse_filtered_acceleration_y) self._append(self.filtered_acceleration_z, timeindex, self.parse_filtered_acceleration_z) - self._append(self.magnitude, timeindex, self.parse_magnitude) + self._append(self.filtered_acceleration_magnitude, timeindex, self.parse_filtered_acceleration_magnitude) self._append(self.tilt_angle, timeindex, self.parse_tilt_angle) self._append(self.orientation_angle, timeindex, self.parse_orientation_angle) self._append(self.current_rotation, timeindex, self.parse_current_rotation) @@ -288,17 +319,13 @@ class Plotter: self._append(self.proposed_rotation, timeindex, self.parse_proposed_rotation) else: self._append(self.proposed_rotation, timeindex, None) - if self.parse_proposal_rotation >= 0: - self._append(self.proposal_rotation, timeindex, self.parse_proposal_rotation) + if self.parse_predicted_rotation >= 0: + self._append(self.predicted_rotation, timeindex, self.parse_predicted_rotation) else: - self._append(self.proposal_rotation, timeindex, None) - for i in range(0, 4): - self._append(self.proposal_confidence[i][0], timeindex, i) - if i == self.parse_proposal_rotation: - self._append(self.proposal_confidence[i][1], timeindex, - i + self.parse_proposal_confidence) - else: - self._append(self.proposal_confidence[i][1], timeindex, i) + self._append(self.predicted_rotation, timeindex, None) + self._append(self.time_until_settled, timeindex, self.parse_time_until_settled) + self._append(self.time_until_flat_delay_expired, timeindex, self.parse_time_until_flat_delay_expired) + self._append(self.time_until_swing_delay_expired, timeindex, self.parse_time_until_swing_delay_expired) self._append(self.sample_latency, timeindex, self.parse_sample_latency) self._reset_parse_state() @@ -309,45 +336,40 @@ class Plotter: self._scroll(self.raw_acceleration_x, bottom) self._scroll(self.raw_acceleration_y, bottom) self._scroll(self.raw_acceleration_z, bottom) + self._scroll(self.raw_acceleration_magnitude, bottom) self._scroll(self.filtered_acceleration_x, bottom) self._scroll(self.filtered_acceleration_y, bottom) self._scroll(self.filtered_acceleration_z, bottom) - self._scroll(self.magnitude, bottom) + self._scroll(self.filtered_acceleration_magnitude, bottom) self._scroll(self.tilt_angle, bottom) self._scroll(self.orientation_angle, bottom) self._scroll(self.current_rotation, bottom) self._scroll(self.proposed_rotation, bottom) - self._scroll(self.proposal_rotation, bottom) - for i in range(0, 4): - self._scroll(self.proposal_confidence[i][0], bottom) - self._scroll(self.proposal_confidence[i][1], bottom) + self._scroll(self.predicted_rotation, bottom) + self._scroll(self.time_until_settled, bottom) + self._scroll(self.time_until_flat_delay_expired, bottom) + self._scroll(self.time_until_swing_delay_expired, bottom) self._scroll(self.sample_latency, bottom) # Redraw the plots. self.raw_acceleration_line_x.set_data(self.raw_acceleration_x) self.raw_acceleration_line_y.set_data(self.raw_acceleration_y) self.raw_acceleration_line_z.set_data(self.raw_acceleration_z) + self.raw_acceleration_line_magnitude.set_data(self.raw_acceleration_magnitude) self.filtered_acceleration_line_x.set_data(self.filtered_acceleration_x) self.filtered_acceleration_line_y.set_data(self.filtered_acceleration_y) self.filtered_acceleration_line_z.set_data(self.filtered_acceleration_z) - self.magnitude_line.set_data(self.magnitude) + self.filtered_acceleration_line_magnitude.set_data(self.filtered_acceleration_magnitude) self.tilt_angle_line.set_data(self.tilt_angle) self.orientation_angle_line.set_data(self.orientation_angle) self.current_rotation_line.set_data(self.current_rotation) self.proposed_rotation_line.set_data(self.proposed_rotation) - self.proposal_rotation_line.set_data(self.proposal_rotation) + self.predicted_rotation_line.set_data(self.predicted_rotation) + self.time_until_settled_line.set_data(self.time_until_settled) + self.time_until_flat_delay_expired_line.set_data(self.time_until_flat_delay_expired) + self.time_until_swing_delay_expired_line.set_data(self.time_until_swing_delay_expired) self.sample_latency_line.set_data(self.sample_latency) - for poly in self.proposal_confidence_polys: - poly.remove() - self.proposal_confidence_polys = [] - for i in range(0, 4): - self.proposal_confidence_polys.append(self.orientation_axes.fill_between( - self.proposal_confidence[i][0][0], - self.proposal_confidence[i][0][1], - self.proposal_confidence[i][1][1], - facecolor='goldenrod', edgecolor='goldenrod')) - self.fig.canvas.draw_idle() # Scroll a time series. |