Merge "SensorFusion: Do not use the sensor variables if they are not init'ed" into main (5b4671a0) · Commits · e / os / android_frameworks_native

services/sensorservice/SensorFusion.cpp

+41 −21

Original line number	Diff line number	Diff line
		@@ -82,10 +82,11 @@ SensorFusion::SensorFusion()

		void SensorFusion::process(const sensors_event_t& event) {
		// sensor additional info is not currently used in fusion algorithm
		if (event.type == SENSOR_TYPE_ADDITIONAL_INFO)
		if (event.type == SENSOR_TYPE_ADDITIONAL_INFO) {
		return;
		}

		if (event.sensor == mGyro.getHandle()) {
		if (mGyro.has_value() && event.sensor == mGyro.value().getHandle()) {
		float dT;
		if (event.timestamp - mGyroTime > 0 &&
		event.timestamp - mGyroTime < (int64_t)(5e7) ) { // 0.05sec
		@@ -107,19 +108,20 @@ void SensorFusion::process(const sensors_event_t& event) {
		}
		}
		mGyroTime = event.timestamp;
		} else if (event.sensor == mMag.getHandle()) {
		} else if (mMag.has_value() && event.sensor == mMag.value().getHandle()) {
		const vec3_t mag(event.data);
		for (int i = 0; i < NUM_FUSION_MODE; ++i) {
		if (mEnabled[i]) {
		mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
		// fusion in no mag mode will ignore
		mFusions[i].handleMag(mag);
		}
		}
		} else if (event.sensor == mAcc.getHandle()) {
		float dT;
		if (event.timestamp - mAccTime > 0 &&
		event.timestamp - mAccTime < (int64_t)(1e8) ) { // 0.1sec
		dT = (event.timestamp - mAccTime) / 1000000000.0f;

		dT = (event.timestamp - mAccTime) / 1000000000.0f;
		const vec3_t acc(event.data);
		for (int i = 0; i < NUM_FUSION_MODE; ++i) {
		if (mEnabled[i]) {
		@@ -160,14 +162,22 @@ status_t SensorFusion::activate(int mode, void* ident, bool enabled) {
		}
		}

		if (mode != FUSION_NOMAG && !mMag.has_value()) {
		ALOGE("%s: magnetic sensor is expected but not present!", __func__);
		return NO_INIT;
		}
		if (mode != FUSION_NOGYRO && !mGyro.has_value()) {
		ALOGE("%s: gyroscope is expected but not present!", __func__);
		return NO_INIT;
		}

		mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
		if (mode != FUSION_NOMAG) {
		mSensorDevice.activate(ident, mMag.getHandle(), enabled);
		mSensorDevice.activate(ident, mMag.value().getHandle(), enabled);
		}
		if (mode != FUSION_NOGYRO) {
		mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
		mSensorDevice.batch(ident, mGyro.value().getHandle(), 0, mTargetDelayNs, 0);
		}

		return NO_ERROR;
		}

		@@ -176,12 +186,22 @@ status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
		if (ns > (int64_t)5e7) {
		ns = (int64_t)(5e7);
		}

		if (mode != FUSION_NOMAG && !mMag.has_value()) {
		ALOGE("%s: magnetic sensor is expected but not present!", __func__);
		return NO_INIT;
		}
		if (mode != FUSION_NOGYRO && !mGyro.has_value()) {
		ALOGE("%s: gyroscope is expected but not present!", __func__);
		return NO_INIT;
		}

		mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
		if (mode != FUSION_NOMAG) {
		mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(10), 0);
		mSensorDevice.batch(ident, mMag.value().getHandle(), 0, ms2ns(10), 0);
		}
		if (mode != FUSION_NOGYRO) {
		mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
		mSensorDevice.batch(ident, mGyro.value().getHandle(), 0, mTargetDelayNs, 0);
		}
		return NO_ERROR;
		}
		@@ -189,8 +209,8 @@ status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {

		float SensorFusion::getPowerUsage(int mode) const {
		float power = mAcc.getPowerUsage() +
		((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +
		((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);
		((mode != FUSION_NOMAG) ? mMag.value().getPowerUsage() : 0) +
		((mode != FUSION_NOGYRO) ? mGyro.value().getPowerUsage() : 0);
		return power;
		}

services/sensorservice/SensorFusion.h

+2 −2

Original line number	Diff line number	Diff line
		@@ -42,8 +42,8 @@ class SensorFusion : public Singleton<SensorFusion> {

		SensorDevice& mSensorDevice;
		Sensor mAcc;
		Sensor mMag;
		Sensor mGyro;
		std::optional<Sensor> mMag;
		std::optional<Sensor> mGyro;

		Fusion mFusions[NUM_FUSION_MODE]; // normal, no_mag, no_gyro