1# Copyright 2014 The Android Open Source Project 2# 3# Licensed under the Apache License, Version 2.0 (the "License"); 4# you may not use this file except in compliance with the License. 5# You may obtain a copy of the License at 6# 7# http://www.apache.org/licenses/LICENSE-2.0 8# 9# Unless required by applicable law or agreed to in writing, software 10# distributed under the License is distributed on an "AS IS" BASIS, 11# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12# See the License for the specific language governing permissions and 13# limitations under the License. 14"""Verify IMU has stable output when device is stationary.""" 15 16import logging 17import math 18import os 19import time 20 21from matplotlib import pyplot as plt 22from mobly import test_runner 23import numpy as np 24 25import its_base_test 26import camera_properties_utils 27import imu_processing_utils 28import its_session_utils 29import video_processing_utils 30 31_ADV_FEATURE_GYRO_DRIFT_ATOL = 1 # deg/min 32_RAD_TO_DEG = 180/math.pi 33_GYRO_DRIFT_ATOL = 0.01*_RAD_TO_DEG # PASS/FAIL for gyro accumulated drift 34_GYRO_MEAN_THRESH = 0.01*_RAD_TO_DEG # PASS/FAIL for gyro mean drift 35_GYRO_VAR_ATOL = 1E-7 # rad^2/sec^2/Hz from CDD C-1-7 36_IMU_EVENTS_WAIT_TIME = 120 # seconds (Increased from 30s in Android 15) 37_NAME = os.path.basename(__file__).split('.')[0] 38_NSEC_TO_SEC = 1E-9 39_PREVIEW_RECORDING_TIME = 60 # seconds (>60 often crashes) 40_REAR_MAIN_CAMERA_ID = '0' 41_RV_DRIFT_THRESH = 0.01*_RAD_TO_DEG # PASS/FAIL for rotation vector drift 42_SEC_TO_MIN = 1/60 43 44 45def calc_effective_sampling_rate(times, sensor): 46 """Calculate the effective sampling rate for gyro & RV. 47 48 Args: 49 times: array/list of times 50 sensor: string of sensor type 51 52 Returns: 53 effective_sampling_rate 54 """ 55 duration = times[-1] - times[0] 56 num_pts = len(times) 57 sampling_rate = num_pts / duration 58 logging.debug('%s time: %.2fs, num_pts: %d, effective sampling rate: %.2f Hz', 59 sensor, duration, num_pts, sampling_rate) 60 if sensor == 'gyro': # print duration 1x 61 print(f'{_NAME}_duration_seconds: {duration:.2f}') 62 print(f'{_NAME}_{sensor}_sampling_rate_hz: {sampling_rate:.2f}') 63 return sampling_rate 64 65 66def define_3axis_plot(x, y, z, t, plot_name): 67 """Define common 3-axis plot figure, data, and title with RGB coloring. 68 69 Args: 70 x: list of x values 71 y: list of y values 72 z: list of z values 73 t: list of time values for x, y, z data 74 plot_name: str name of plot and figure 75 """ 76 plt.figure(plot_name) 77 plt.plot(t, x, 'r.', label='x', alpha=0.5, clip_on=False) 78 plt.plot(t, y, 'g.', label='y', alpha=0.5, clip_on=False) 79 plt.plot(t, z, 'b.', label='z', alpha=0.5, clip_on=False) 80 plt.xlabel('Time (seconds)') 81 plt.title(plot_name) 82 plt.legend() 83 84 85def plot_rotation_vector_data(x, y, z, t, log_path): 86 """Plot raw gyroscope output data. 87 88 Args: 89 x: list of rotation vector x values 90 y: list of rotation vector y values 91 z: list of rotation vector z values 92 t: list of time values for x, y, z data 93 log_path: str of location for output path 94 """ 95 96 # plot RV data 97 plot_name = f'{_NAME}_rotation_vector' 98 define_3axis_plot(x, y, z, t, plot_name) 99 plt.ylabel('RV (degrees)') 100 plt.ylim([-180, 180]) 101 plt.yticks([-180, -135, -90, -45, 0, 45, 90, 135, 180]) 102 plt.savefig(f'{os.path.join(log_path, plot_name)}.png') 103 104 # find drift per sample and min/max 105 x_drift = imu_processing_utils.calc_rv_drift(x) 106 y_drift = imu_processing_utils.calc_rv_drift(y) 107 z_drift = imu_processing_utils.calc_rv_drift(z) 108 x_drift_min, x_drift_max = min(x_drift), max(x_drift) 109 y_drift_min, y_drift_max = min(y_drift), max(y_drift) 110 z_drift_min, z_drift_max = min(z_drift), max(z_drift) 111 logging.debug('RV drift (degrees) x: %.3f/%.3f, y: %.3f/%.3f, z: %.3f/%.3f', 112 x_drift_min, x_drift_max, y_drift_min, y_drift_max, 113 z_drift_min, z_drift_max) 114 print(f'{_NAME}_rv_drift_degrees_xyz: [{(x_drift_max-x_drift_min):.2f}, ' 115 f'{(y_drift_max-y_drift_min):.2f}, {(z_drift_max-z_drift_min):.2f}]') 116 117 # plot RV drift 118 plot_name = f'{_NAME}_rotation_vector_drift' 119 define_3axis_plot(x_drift, y_drift, z_drift, t, plot_name) 120 plt.ylabel('RV drift (degrees)') 121 plt.ylim([min([x_drift_min, y_drift_min, z_drift_min, -_RV_DRIFT_THRESH]), 122 max([x_drift_max, y_drift_max, z_drift_max, _RV_DRIFT_THRESH])]) 123 plt.savefig(f'{os.path.join(log_path, plot_name)}.png') 124 125 126def plot_raw_gyro_data(x, y, z, t, log_path): 127 """Plot raw gyroscope output data. 128 129 Args: 130 x: list of x values 131 y: list of y values 132 z: list of z values 133 t: list of time values for x, y, z data 134 log_path: str of location for output path 135 """ 136 137 plot_name = f'{_NAME}_gyro_raw' 138 define_3axis_plot(x, y, z, t, plot_name) 139 plt.ylabel('Gyro raw output (degrees)') 140 plt.ylim([min([np.amin(x), np.amin(y), np.amin(z), -_GYRO_MEAN_THRESH]), 141 max([np.amax(x), np.amax(y), np.amax(x), _GYRO_MEAN_THRESH])]) 142 plt.savefig(f'{os.path.join(log_path, plot_name)}.png') 143 144 145def do_riemann_sums(x, y, z, t, log_path): 146 """Do integration estimation using Riemann sums and plot. 147 148 Args: 149 x: list of x values 150 y: list of y values 151 z: list of z values 152 t: list of time values for x, y, z data 153 log_path: str of location for output path 154 155 Returns: 156 gyro drifts defined as x, y & z (max-min) values over test time 157 """ 158 x_int, y_int, z_int = 0, 0, 0 159 x_sums, y_sums, z_sums = [0], [0], [0] 160 for i in range(len(t)): 161 if i > 0: 162 x_int += x[i] * (t[i] - t[i-1]) 163 y_int += y[i] * (t[i] - t[i-1]) 164 z_int += z[i] * (t[i] - t[i-1]) 165 x_sums.append(x_int) 166 y_sums.append(y_int) 167 z_sums.append(z_int) 168 169 # find min/maxes 170 x_min, x_max = min(x_sums), max(x_sums) 171 y_min, y_max = min(y_sums), max(y_sums) 172 z_min, z_max = min(z_sums), max(z_sums) 173 logging.debug('Integrated gyro drift min/max (degrees) ' 174 'x: %.3f/%.3f, y: %.3f/%.3f, z: %.3f/%.3f', 175 x_min, x_max, y_min, y_max, z_min, z_max) 176 print(f'{_NAME}_gyro_drift_degrees_xyz: [{(x_max-x_min):.2f}, ' 177 f'{(y_max-y_min):.2f}, {(z_max-z_min):.2f}]') 178 179 # plot accumulated gyro drift 180 plot_name = f'{_NAME}_gyro_drift' 181 define_3axis_plot(x_sums, y_sums, z_sums, t, plot_name) 182 plt.ylabel('Drift (degrees)') 183 plt.ylim([min([x_min, y_min, z_min, -_GYRO_DRIFT_ATOL]), 184 max([x_max, y_max, z_max, _GYRO_DRIFT_ATOL])]) 185 plt.savefig(f'{os.path.join(log_path, plot_name)}.png') 186 187 return x_max-x_min, y_max-y_min, z_max-z_min 188 189 190def convert_events_to_arrays(events, t_factor, xyz_factor): 191 """Convert data from get_sensor_events() into x, y, z, t. 192 193 Args: 194 events: dict from cam.get_sensor_events() 195 t_factor: time multiplication factor ie. NSEC_TO_SEC 196 xyz_factor: xyz multiplicaiton factor ie. RAD_TO_DEG 197 198 Returns: 199 x, y, z, t numpy arrays 200 """ 201 t = np.array([(e['time'] - events[0]['time'])*t_factor 202 for e in events]) 203 x = np.array([e['x']*xyz_factor for e in events]) 204 y = np.array([e['y']*xyz_factor for e in events]) 205 z = np.array([e['z']*xyz_factor for e in events]) 206 207 return x, y, z, t 208 209 210class ImuDriftTest(its_base_test.ItsBaseTest): 211 """Test if the IMU has stable output when device is stationary.""" 212 213 def test_imu_drift(self): 214 with its_session_utils.ItsSession( 215 device_id=self.dut.serial, 216 camera_id=self.camera_id, 217 hidden_physical_id=self.hidden_physical_id) as cam: 218 props = cam.get_camera_properties() 219 props = cam.override_with_hidden_physical_camera_props(props) 220 221 # check SKIP conditions 222 camera_properties_utils.skip_unless( 223 camera_properties_utils.sensor_fusion(props) and 224 cam.get_sensors().get('gyro') and 225 self.camera_id == _REAR_MAIN_CAMERA_ID) 226 227 # load scene 228 its_session_utils.load_scene(cam, props, self.scene, 229 self.tablet, self.chart_distance) 230 231 # get largest size from get_preview_video_sizes_union 232 preview_size = ( 233 video_processing_utils.get_preview_video_sizes_union( 234 cam, self.camera_id).largest_size 235 ) 236 logging.debug('Testing preview resolution: %s', preview_size) 237 238 # start collecting IMU events 239 logging.debug('Collecting IMU events') 240 cam.start_sensor_events() 241 242 # do preview recording 243 preview_stabilization_supported = ( 244 camera_properties_utils.preview_stabilization_supported(props) 245 ) 246 cam.do_preview_recording( 247 video_size=preview_size, duration=_PREVIEW_RECORDING_TIME, 248 stabilize=preview_stabilization_supported 249 ) 250 251 if _IMU_EVENTS_WAIT_TIME > _PREVIEW_RECORDING_TIME: 252 time.sleep(_IMU_EVENTS_WAIT_TIME - _PREVIEW_RECORDING_TIME) 253 254 # dump IMU events 255 sensor_events = cam.get_sensor_events() 256 gyro_events = sensor_events['gyro'] # raw gyro output 257 if 'rv' in sensor_events.keys(): 258 rv_events = sensor_events['rv'] # rotation vector 259 260 # process gyro data 261 x_gyro, y_gyro, z_gyro, times = convert_events_to_arrays( 262 gyro_events, _NSEC_TO_SEC, _RAD_TO_DEG) 263 # gyro sampling rate is SENSOR_DELAY_FASTEST in ItsService.java 264 gyro_sampling_rate = calc_effective_sampling_rate(times, 'gyro') 265 266 plot_raw_gyro_data(x_gyro, y_gyro, z_gyro, times, self.log_path) 267 x_gyro_drift, y_gyro_drift, z_gyro_drift = do_riemann_sums( 268 x_gyro, y_gyro, z_gyro, times, self.log_path) 269 270 if rv_events: 271 # process rotation vector data 272 x_rv, y_rv, z_rv, t_rv = convert_events_to_arrays( 273 rv_events, _NSEC_TO_SEC, 1) 274 # Rotation Vector sampling rate is SENSOR_DELAY_FASTEST in ItsService.java 275 calc_effective_sampling_rate(t_rv, 'rv') 276 277 # plot rotation vector data 278 plot_rotation_vector_data(x_rv, y_rv, z_rv, t_rv, self.log_path) 279 280 # assert correct gyro behavior 281 gyro_var_atol = _GYRO_VAR_ATOL * gyro_sampling_rate * _RAD_TO_DEG**2 282 for i, samples in enumerate([x_gyro, y_gyro, z_gyro]): 283 gyro_mean = samples.mean() 284 gyro_var = np.var(samples) 285 logging.debug('%s gyro_mean: %.3e', 'XYZ'[i], gyro_mean) 286 logging.debug('%s gyro_var: %.3e', 'XYZ'[i], gyro_var) 287 if gyro_mean >= _GYRO_MEAN_THRESH: 288 raise AssertionError(f'gyro_mean: {gyro_mean:.3e}, ' 289 f'ATOL={_GYRO_MEAN_THRESH}') 290 if gyro_var >= gyro_var_atol: 291 raise AssertionError(f'gyro_var: {gyro_var:.3e}, ' 292 f'ATOL={gyro_var_atol:.3e}') 293 294 # Determine common parameters between Android 15 & high performance checks 295 test_duration = times[-1] - times[0] 296 gyro_drift_total = math.sqrt(x_gyro_drift**2 + 297 y_gyro_drift**2 + 298 z_gyro_drift**2) 299 e_msg_stem = ( 300 f'accumulated gyro drift is too large! x, y, z, total: ' 301 f'{x_gyro_drift:.3f}, {y_gyro_drift:.3f}, {z_gyro_drift:.3f}, ' 302 f'{gyro_drift_total:.3f}, test duration: {test_duration:.3f} (sec)' 303 ) 304 305 # Performance checks for advanced features 306 logging.debug('Check for advanced features gyro drift.') 307 gyro_drift_atol_efv = ( 308 _ADV_FEATURE_GYRO_DRIFT_ATOL * test_duration * _SEC_TO_MIN 309 ) 310 if gyro_drift_total > gyro_drift_atol_efv: 311 e_msg = f'{e_msg_stem}, ATOL: {gyro_drift_atol_efv:.3f}' 312 raise AssertionError( 313 f'{its_session_utils.NOT_YET_MANDATED_MESSAGE}\n\n{e_msg}') 314 315 316if __name__ == '__main__': 317 test_runner.main() 318