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

services/sensorservice/SensorFusion.cpp

+21 −41

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

	void SensorFusion::process(const sensors_event_t& event) {		void SensorFusion::process(const sensors_event_t& event) {
	// sensor additional info is not currently used in fusion algorithm		// 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;		return;
	}

	if (mGyro.has_value() && event.sensor == mGyro.value().getHandle()) {		if (event.sensor == mGyro.getHandle()) {
	float dT;		float dT;
	if ( event.timestamp - mGyroTime> 0 &&		if ( event.timestamp - mGyroTime> 0 &&
	event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec		event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec
	@@ -108,20 +107,19 @@ void SensorFusion::process(const sensors_event_t& event) {
	}		}
	}		}
	mGyroTime = event.timestamp;		mGyroTime = event.timestamp;
	} else if (mMag.has_value() && event.sensor == mMag.value().getHandle()) {		} else if (event.sensor == mMag.getHandle()) {
	const vec3_t mag(event.data);		const vec3_t mag(event.data);
	for (int i = 0; i<NUM_FUSION_MODE; ++i) {		for (int i = 0; i<NUM_FUSION_MODE; ++i) {
	if (mEnabled[i]) {		if (mEnabled[i]) {
	// fusion in no mag mode will ignore		mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
	mFusions[i].handleMag(mag);
	}		}
	}		}
	} else if (event.sensor == mAcc.getHandle()) {		} else if (event.sensor == mAcc.getHandle()) {
	float dT;		float dT;
	if ( event.timestamp - mAccTime> 0 &&		if ( event.timestamp - mAccTime> 0 &&
	event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec		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);		const vec3_t acc(event.data);
	for (int i = 0; i<NUM_FUSION_MODE; ++i) {		for (int i = 0; i<NUM_FUSION_MODE; ++i) {
	if (mEnabled[i]) {		if (mEnabled[i]) {
	@@ -162,22 +160,14 @@ 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);		mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
	if (mode != FUSION_NOMAG) {		if (mode != FUSION_NOMAG) {
	mSensorDevice.activate(ident, mMag.value().getHandle(), enabled);		mSensorDevice.activate(ident, mMag.getHandle(), enabled);
	}		}
	if (mode != FUSION_NOGYRO) {		if (mode != FUSION_NOGYRO) {
	mSensorDevice.batch(ident, mGyro.value().getHandle(), 0, mTargetDelayNs, 0);		mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
	}		}

	return NO_ERROR;		return NO_ERROR;
	}		}

	@@ -186,22 +176,12 @@ status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
	if (ns > (int64_t)5e7) {		if (ns > (int64_t)5e7) {
	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);		mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
	if (mode != FUSION_NOMAG) {		if (mode != FUSION_NOMAG) {
	mSensorDevice.batch(ident, mMag.value().getHandle(), 0, ms2ns(10), 0);		mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(10), 0);
	}		}
	if (mode != FUSION_NOGYRO) {		if (mode != FUSION_NOGYRO) {
	mSensorDevice.batch(ident, mGyro.value().getHandle(), 0, mTargetDelayNs, 0);		mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
	}		}
	return NO_ERROR;		return NO_ERROR;
	}		}
	@@ -209,8 +189,8 @@ status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {

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

services/sensorservice/SensorFusion.h

+2 −2

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

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

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