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