# Copyright 2014 The Android Open Source Project # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. """Verify IMU has stable output when device is stationary.""" import logging import math import os import time from matplotlib import pyplot as plt from mobly import test_runner import numpy as np import its_base_test import camera_properties_utils import imu_processing_utils import its_session_utils import video_processing_utils _ADV_FEATURE_GYRO_DRIFT_ATOL = 0.1 # deg/min _RAD_TO_DEG = 180/math.pi _GYRO_DRIFT_ATOL = 0.01*_RAD_TO_DEG # PASS/FAIL for gyro accumulated drift _GYRO_MEAN_THRESH = 0.01*_RAD_TO_DEG # PASS/FAIL for gyro mean drift _GYRO_VAR_ATOL = 1E-7 # rad^2/sec^2/Hz from CDD C-1-7 _IMU_EVENTS_WAIT_TIME = 180 # seconds (Increased from 30s in Android 15) _NAME = os.path.basename(__file__).split('.')[0] _NSEC_TO_SEC = 1E-9 _NUM_PREVIEW_RECORDINGS = 2 # do 2 recordings to get test time to 2 minutes _PREVIEW_RECORDING_TIME = 60 # seconds (>60 often crashes) _REAR_MAIN_CAMERA_ID = '0' _RV_DRIFT_THRESH = 0.01*_RAD_TO_DEG # PASS/FAIL for rotation vector drift _SEC_TO_MIN = 1/60 def calc_effective_sampling_rate(times, sensor): """Calculate the effective sampling rate for gyro & RV. Args: times: array/list of times sensor: string of sensor type Returns: effective_sampling_rate """ duration = times[-1] - times[0] num_pts = len(times) sampling_rate = num_pts / duration logging.debug('%s time: %.2fs, num_pts: %d, effective sampling rate: %.2f Hz', sensor, duration, num_pts, sampling_rate) if sensor == 'gyro': # print duration 1x print(f'{_NAME}_duration_seconds: {duration:.2f}') print(f'{_NAME}_{sensor}_sampling_rate_hz: {sampling_rate:.2f}') return sampling_rate def define_3axis_plot(x, y, z, t, plot_name): """Define common 3-axis plot figure, data, and title with RGB coloring. Args: x: list of x values y: list of y values z: list of z values t: list of time values for x, y, z data plot_name: str name of plot and figure """ plt.figure(plot_name) plt.plot(t, x, 'r.', label='x', alpha=0.5, clip_on=False) plt.plot(t, y, 'g.', label='y', alpha=0.5, clip_on=False) plt.plot(t, z, 'b.', label='z', alpha=0.5, clip_on=False) plt.xlabel('Time (seconds)') plt.title(plot_name) plt.legend() def plot_rotation_vector_data(x, y, z, t, log_path): """Plot raw gyroscope output data. Args: x: list of rotation vector x values y: list of rotation vector y values z: list of rotation vector z values t: list of time values for x, y, z data log_path: str of location for output path """ # plot RV data plot_name = f'{_NAME}_rotation_vector' define_3axis_plot(x, y, z, t, plot_name) plt.ylabel('RV (degrees)') plt.ylim([-180, 180]) plt.yticks([-180, -135, -90, -45, 0, 45, 90, 135, 180]) plt.savefig(f'{os.path.join(log_path, plot_name)}.png') # find drift per sample and min/max x_drift = imu_processing_utils.calc_rv_drift(x) y_drift = imu_processing_utils.calc_rv_drift(y) z_drift = imu_processing_utils.calc_rv_drift(z) x_drift_min, x_drift_max = min(x_drift), max(x_drift) y_drift_min, y_drift_max = min(y_drift), max(y_drift) z_drift_min, z_drift_max = min(z_drift), max(z_drift) logging.debug('RV drift (degrees) x: %.3f/%.3f, y: %.3f/%.3f, z: %.3f/%.3f', x_drift_min, x_drift_max, y_drift_min, y_drift_max, z_drift_min, z_drift_max) print(f'{_NAME}_rv_drift_degrees_xyz: [{(x_drift_max-x_drift_min):.2f}, ' f'{(y_drift_max-y_drift_min):.2f}, {(z_drift_max-z_drift_min):.2f}]') # plot RV drift plot_name = f'{_NAME}_rotation_vector_drift' define_3axis_plot(x_drift, y_drift, z_drift, t, plot_name) plt.ylabel('RV drift (degrees)') plt.ylim([min([x_drift_min, y_drift_min, z_drift_min, -_RV_DRIFT_THRESH]), max([x_drift_max, y_drift_max, z_drift_max, _RV_DRIFT_THRESH])]) plt.savefig(f'{os.path.join(log_path, plot_name)}.png') def plot_raw_gyro_data(x, y, z, t, log_path): """Plot raw gyroscope output data. Args: x: list of x values y: list of y values z: list of z values t: list of time values for x, y, z data log_path: str of location for output path """ plot_name = f'{_NAME}_gyro_raw' define_3axis_plot(x, y, z, t, plot_name) plt.ylabel('Gyro raw output (degrees)') plt.ylim([min([np.amin(x), np.amin(y), np.amin(z), -_GYRO_MEAN_THRESH]), max([np.amax(x), np.amax(y), np.amax(x), _GYRO_MEAN_THRESH])]) plt.savefig(f'{os.path.join(log_path, plot_name)}.png') def do_riemann_sums(x, y, z, t, log_path): """Do integration estimation using Riemann sums and plot. Args: x: list of x values y: list of y values z: list of z values t: list of time values for x, y, z data log_path: str of location for output path Returns: gyro drifts defined as x, y & z (max-min) values over test time """ x_int, y_int, z_int = 0, 0, 0 x_sums, y_sums, z_sums = [0], [0], [0] for i in range(len(t)): if i > 0: x_int += x[i] * (t[i] - t[i-1]) y_int += y[i] * (t[i] - t[i-1]) z_int += z[i] * (t[i] - t[i-1]) x_sums.append(x_int) y_sums.append(y_int) z_sums.append(z_int) # find min/maxes x_min, x_max = min(x_sums), max(x_sums) y_min, y_max = min(y_sums), max(y_sums) z_min, z_max = min(z_sums), max(z_sums) logging.debug('Integrated gyro drift min/max (degrees) ' 'x: %.3f/%.3f, y: %.3f/%.3f, z: %.3f/%.3f', x_min, x_max, y_min, y_max, z_min, z_max) print(f'{_NAME}_gyro_drift_degrees_xyz: [{(x_max-x_min):.2f}, ' f'{(y_max-y_min):.2f}, {(z_max-z_min):.2f}]') # plot accumulated gyro drift plot_name = f'{_NAME}_gyro_drift' define_3axis_plot(x_sums, y_sums, z_sums, t, plot_name) plt.ylabel('Drift (degrees)') plt.ylim([min([x_min, y_min, z_min, -_GYRO_DRIFT_ATOL]), max([x_max, y_max, z_max, _GYRO_DRIFT_ATOL])]) plt.savefig(f'{os.path.join(log_path, plot_name)}.png') return x_max-x_min, y_max-y_min, z_max-z_min def convert_events_to_arrays(events, t_factor, xyz_factor): """Convert data from get_sensor_events() into x, y, z, t. Args: events: dict from cam.get_sensor_events() t_factor: time multiplication factor ie. NSEC_TO_SEC xyz_factor: xyz multiplicaiton factor ie. RAD_TO_DEG Returns: x, y, z, t numpy arrays """ t = np.array([(e['time'] - events[0]['time'])*t_factor for e in events]) x = np.array([e['x']*xyz_factor for e in events]) y = np.array([e['y']*xyz_factor for e in events]) z = np.array([e['z']*xyz_factor for e in events]) return x, y, z, t class ImuDriftTest(its_base_test.ItsBaseTest): """Test if the IMU has stable output when device is stationary.""" def test_imu_drift(self): with its_session_utils.ItsSession( device_id=self.dut.serial, camera_id=self.camera_id, hidden_physical_id=self.hidden_physical_id) as cam: props = cam.get_camera_properties() props = cam.override_with_hidden_physical_camera_props(props) has_hifi_sensors = cam.has_hifi_sensors() logging.debug('Device has HIFI sensors? %s', has_hifi_sensors) # check SKIP conditions camera_properties_utils.skip_unless( camera_properties_utils.sensor_fusion(props) and cam.get_sensors().get('gyro') and self.camera_id == _REAR_MAIN_CAMERA_ID) # load scene its_session_utils.load_scene(cam, props, self.scene, self.tablet, self.chart_distance) # get largest size from get_preview_video_sizes_union preview_size = ( video_processing_utils.get_preview_video_sizes_union( cam, self.camera_id).largest_size ) logging.debug('Testing preview resolution: %s', preview_size) # start collecting IMU events logging.debug('Collecting IMU events') cam.start_sensor_events() # do preview recording preview_stabilization_supported = ( camera_properties_utils.preview_stabilization_supported(props) ) for _ in range(_NUM_PREVIEW_RECORDINGS): cam.do_preview_recording( video_size=preview_size, duration=_PREVIEW_RECORDING_TIME, stabilize=preview_stabilization_supported ) if (_IMU_EVENTS_WAIT_TIME > _NUM_PREVIEW_RECORDINGS * _PREVIEW_RECORDING_TIME): time.sleep(_IMU_EVENTS_WAIT_TIME - _NUM_PREVIEW_RECORDINGS * _PREVIEW_RECORDING_TIME) # dump IMU events sensor_events = cam.get_sensor_events() gyro_events = sensor_events['gyro'] # raw gyro output if 'rv' in sensor_events.keys(): rv_events = sensor_events['rv'] # rotation vector # process gyro data x_gyro, y_gyro, z_gyro, times = convert_events_to_arrays( gyro_events, _NSEC_TO_SEC, _RAD_TO_DEG) # gyro sampling rate is SENSOR_DELAY_FASTEST in ItsService.java gyro_sampling_rate = calc_effective_sampling_rate(times, 'gyro') plot_raw_gyro_data(x_gyro, y_gyro, z_gyro, times, self.log_path) x_gyro_drift, y_gyro_drift, z_gyro_drift = do_riemann_sums( x_gyro, y_gyro, z_gyro, times, self.log_path) if rv_events: # process rotation vector data x_rv, y_rv, z_rv, t_rv = convert_events_to_arrays( rv_events, _NSEC_TO_SEC, 1) # Rotation Vector sampling rate is SENSOR_DELAY_FASTEST in ItsService.java calc_effective_sampling_rate(t_rv, 'rv') # plot rotation vector data plot_rotation_vector_data(x_rv, y_rv, z_rv, t_rv, self.log_path) # assert correct gyro behavior gyro_var_atol = _GYRO_VAR_ATOL * gyro_sampling_rate * _RAD_TO_DEG**2 for i, samples in enumerate([x_gyro, y_gyro, z_gyro]): gyro_mean = samples.mean() gyro_var = np.var(samples) logging.debug('%s gyro_mean: %.3e', 'XYZ'[i], gyro_mean) logging.debug('%s gyro_var: %.3e', 'XYZ'[i], gyro_var) if gyro_mean >= _GYRO_MEAN_THRESH: raise AssertionError(f'gyro_mean: {gyro_mean:.3e}, ' f'ATOL={_GYRO_MEAN_THRESH}') if gyro_var >= gyro_var_atol: raise AssertionError(f'gyro_var: {gyro_var:.3e}, ' f'ATOL={gyro_var_atol:.3e}') # Determine common parameters between Android 15 & high performance checks test_duration = times[-1] - times[0] gyro_drift_total = math.sqrt(x_gyro_drift**2 + y_gyro_drift**2 + z_gyro_drift**2) e_msg_stem = ( f'accumulated gyro drift is too large! x, y, z, total: ' f'{x_gyro_drift:.3f}, {y_gyro_drift:.3f}, {z_gyro_drift:.3f}, ' f'{gyro_drift_total:.3f}, test duration: {test_duration:.3f} (sec)' ) # Performance checks for advanced features if has_hifi_sensors: logging.debug('Check for advanced features gyro drift.') gyro_drift_atol_efv = ( _ADV_FEATURE_GYRO_DRIFT_ATOL * test_duration * _SEC_TO_MIN ) if gyro_drift_total > gyro_drift_atol_efv: e_msg = f'{e_msg_stem}, ATOL: {gyro_drift_atol_efv:.3f}' raise AssertionError( f'{its_session_utils.NOT_YET_MANDATED_MESSAGE}\n\n{e_msg}\n' 'Please try possible solution: Temperature based Gyroscope' 'calibration\n' 'For example: ' # pylint: disable=line-too-long 'https://android.googlesource.com/device/google/contexthub/+/e6bd5ca5207f04fa720c9274f506ef15b9412b49/firmware/os/algos/calibration/over_temp') if __name__ == '__main__': test_runner.main()