status_t CorrectedGyroSensor::activate(void* ident, bool enabled) {
mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t CorrectedGyroSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
mSensorDevice.setDelay(ident, mGyro.getHandle(), ns);
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor CorrectedGyroSensor::getSensor() const {
// -----------------------------------------------------------------------
+/*==================== BEGIN FUSION SENSOR PARAMETER =========================*/
+
+/* Note:
+ * If a platform uses software fusion, it is necessary to tune the following
+ * parameters to fit the hardware sensors prior to release.
+ *
+ * The DEFAULT_ parameters will be used in FUSION_9AXIS and FUSION_NOMAG mode.
+ * The GEOMAG_ parameters will be used in FUSION_NOGYRO mode.
+ */
+
/*
- * gyroVAR gives the measured variance of the gyro's output per
+ * GYRO_VAR gives the measured variance of the gyro's output per
* Hz (or variance at 1 Hz). This is an "intrinsic" parameter of the gyro,
* which is independent of the sampling frequency.
*
* The variance of gyro's output at a given sampling period can be
* calculated as:
- * variance(T) = gyroVAR / T
+ * variance(T) = GYRO_VAR / T
*
* The variance of the INTEGRATED OUTPUT at a given sampling period can be
* calculated as:
- * variance_integrate_output(T) = gyroVAR * T
- *
+ * variance_integrate_output(T) = GYRO_VAR * T
*/
-static const float gyroVAR = 1e-7; // (rad/s)^2 / Hz
-static const float biasVAR = 1e-8; // (rad/s)^2 / s (guessed)
+static const float DEFAULT_GYRO_VAR = 1e-7; // (rad/s)^2 / Hz
+static const float DEFAULT_GYRO_BIAS_VAR = 1e-12; // (rad/s)^2 / s (guessed)
+static const float GEOMAG_GYRO_VAR = 1e-4; // (rad/s)^2 / Hz
+static const float GEOMAG_GYRO_BIAS_VAR = 1e-8; // (rad/s)^2 / s (guessed)
/*
* Standard deviations of accelerometer and magnetometer
*/
-static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
-static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)
+static const float DEFAULT_ACC_STDEV = 0.015f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float DEFAULT_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+static const float GEOMAG_ACC_STDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
+static const float GEOMAG_MAG_STDEV = 0.1f; // uT (measured 0.7 / CDD 0.5)
+
+
+/* ====================== END FUSION SENSOR PARAMETER ========================*/
static const float SYMMETRY_TOLERANCE = 1e-10f;
* ill-conditioning and div by zeros.
* Threshhold: 10% of g, in m/s^2
*/
-static const float FREE_FALL_THRESHOLD = 0.981f;
+static const float NOMINAL_GRAVITY = 9.81f;
+static const float FREE_FALL_THRESHOLD = 0.1f * (NOMINAL_GRAVITY);
static const float FREE_FALL_THRESHOLD_SQ =
FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
MIN_VALID_CROSS_PRODUCT_MAG*MIN_VALID_CROSS_PRODUCT_MAG;
+static const float W_EPS = 1e-4f;
+static const float SQRT_3 = 1.732f;
+static const float WVEC_EPS = 1e-4f/SQRT_3;
// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
init();
}
-void Fusion::init() {
+void Fusion::init(int mode) {
mInitState = 0;
mGyroRate = 0;
mCount[2] = 0;
mData = 0;
+ mMode = mode;
+
+ if (mMode != FUSION_NOGYRO) { //normal or game rotation
+ mParam.gyroVar = DEFAULT_GYRO_VAR;
+ mParam.gyroBiasVar = DEFAULT_GYRO_BIAS_VAR;
+ mParam.accStdev = DEFAULT_ACC_STDEV;
+ mParam.magStdev = DEFAULT_MAG_STDEV;
+ } else {
+ mParam.gyroVar = GEOMAG_GYRO_VAR;
+ mParam.gyroBiasVar = GEOMAG_GYRO_BIAS_VAR;
+ mParam.accStdev = GEOMAG_ACC_STDEV;
+ mParam.magStdev = GEOMAG_MAG_STDEV;
+ }
}
void Fusion::initFusion(const vec4_t& q, float dT)
const float dT3 = dT2*dT;
// variance of integrated output at 1/dT Hz (random drift)
- const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;
+ const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
// variance of drift rate ramp
- const float q11 = biasVAR * dT;
- const float q10 = 0.5f * biasVAR * dT2;
+ const float q11 = mParam.gyroBiasVar * dT;
+ const float q10 = 0.5f * mParam.gyroBiasVar * dT2;
const float q01 = q10;
GQGt[0][0] = q00; // rad^2
}
bool Fusion::hasEstimate() const {
- return (mInitState == (MAG|ACC|GYRO));
+ return ((mInitState & MAG) || (mMode == FUSION_NOMAG)) &&
+ ((mInitState & GYRO) || (mMode == FUSION_NOGYRO)) &&
+ (mInitState & ACC);
}
bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
mData[0] += d * (1/length(d));
mCount[0]++;
mInitState |= ACC;
+ if (mMode == FUSION_NOGYRO ) {
+ mGyroRate = dT;
+ }
} else if (what == MAG) {
mData[1] += d * (1/length(d));
mCount[1]++;
mGyroRate = dT;
mData[2] += d*dT;
mCount[2]++;
- if (mCount[2] == 64) {
- // 64 samples is good enough to estimate the gyro drift and
- // doesn't take too much time.
- mInitState |= GYRO;
- }
+ mInitState |= GYRO;
}
- if (mInitState == (MAG|ACC|GYRO)) {
+ if (hasEstimate()) {
// Average all the values we collected so far
mData[0] *= 1.0f/mCount[0];
- mData[1] *= 1.0f/mCount[1];
+ if (mMode != FUSION_NOMAG) {
+ mData[1] *= 1.0f/mCount[1];
+ }
mData[2] *= 1.0f/mCount[2];
// calculate the MRPs from the data collection, this gives us
// a rough estimate of our initial state
mat33_t R;
- vec3_t up(mData[0]);
- vec3_t east(cross_product(mData[1], up));
- east *= 1/length(east);
+ vec3_t up(mData[0]);
+ vec3_t east;
+
+ if (mMode != FUSION_NOMAG) {
+ east = normalize(cross_product(mData[1], up));
+ } else {
+ east = getOrthogonal(up);
+ }
+
vec3_t north(cross_product(up, east));
R << east << north << up;
const vec4_t q = matrixToQuat(R);
predict(w, dT);
}
-status_t Fusion::handleAcc(const vec3_t& a) {
+status_t Fusion::handleAcc(const vec3_t& a, float dT) {
+ if (!checkInitComplete(ACC, a, dT))
+ return BAD_VALUE;
+
// ignore acceleration data if we're close to free-fall
- if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
+ const float l = length(a);
+ if (l < FREE_FALL_THRESHOLD) {
return BAD_VALUE;
}
- if (!checkInitComplete(ACC, a))
- return BAD_VALUE;
+ const float l_inv = 1.0f/l;
+
+ if ( mMode == FUSION_NOGYRO ) {
+ //geo mag
+ vec3_t w_dummy;
+ w_dummy = x1; //bias
+ predict(w_dummy, dT);
+ }
+
+ if ( mMode == FUSION_NOMAG) {
+ vec3_t m;
+ m = getRotationMatrix()*Bm;
+ update(m, Bm, mParam.magStdev);
+ }
- const float l = 1/length(a);
- update(a*l, Ba, accSTDEV*l);
+ vec3_t unityA = a * l_inv;
+ const float d = sqrtf(fabsf(l- NOMINAL_GRAVITY));
+ const float p = l_inv * mParam.accStdev*expf(d);
+
+ update(unityA, Ba, p);
return NO_ERROR;
}
status_t Fusion::handleMag(const vec3_t& m) {
+ if (!checkInitComplete(MAG, m))
+ return BAD_VALUE;
+
// the geomagnetic-field should be between 30uT and 60uT
// reject if too large to avoid spurious magnetic sources
const float magFieldSq = length_squared(m);
return BAD_VALUE;
}
- if (!checkInitComplete(MAG, m))
- return BAD_VALUE;
-
// Orthogonalize the magnetic field to the gravity field, mapping it into
// tangent to Earth.
const vec3_t up( getRotationMatrix() * Ba );
// then pass it in as the update.
vec3_t north( cross_product(up, east) );
- const float l = 1 / length(north);
- north *= l;
+ const float l_inv = 1 / length(north);
+ north *= l_inv;
- update(north, Bm, magSTDEV*l);
+ update(north, Bm, mParam.magStdev*l_inv);
return NO_ERROR;
}
void Fusion::predict(const vec3_t& w, float dT) {
const vec4_t q = x0;
const vec3_t b = x1;
- const vec3_t we = w - b;
+ vec3_t we = w - b;
+ if (length(we) < WVEC_EPS) {
+ we = (we[0]>0.f)?WVEC_EPS:-WVEC_EPS;
+ }
// q(k+1) = O(we)*q(k)
// --------------------
//
const mat33_t wx2(wx*wx);
const float lwedT = length(we)*dT;
const float hlwedT = 0.5f*lwedT;
- const float ilwe = 1/length(we);
+ const float ilwe = 1.f/length(we);
const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
const float k1 = sinf(lwedT);
const float k2 = cosf(hlwedT);
Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
x0 = O*q;
+
if (x0.w < 0)
x0 = -x0;
const vec3_t e(z - Bb);
const vec3_t dq(K[0]*e);
- const vec3_t db(K[1]*e);
q += getF(q)*(0.5f*dq);
x0 = normalize_quat(q);
- x1 += db;
+
+ if (mMode != FUSION_NOMAG) {
+ const vec3_t db(K[1]*e);
+ x1 += db;
+ }
checkState();
}
+vec3_t Fusion::getOrthogonal(const vec3_t &v) {
+ vec3_t w;
+ if (fabsf(v[0])<= fabsf(v[1]) && fabsf(v[0]) <= fabsf(v[2])) {
+ w[0]=0.f;
+ w[1] = v[2];
+ w[2] = -v[1];
+ } else if (fabsf(v[1]) <= fabsf(v[2])) {
+ w[0] = v[2];
+ w[1] = 0.f;
+ w[2] = -v[0];
+ }else {
+ w[0] = v[1];
+ w[1] = -v[0];
+ w[2] = 0.f;
+ }
+ return normalize(w);
+}
+
+
// -----------------------------------------------------------------------
}; // namespace android
typedef mat<float, 3, 4> mat34_t;
+enum FUSION_MODE{
+ FUSION_9AXIS, // use accel gyro mag
+ FUSION_NOMAG, // use accel gyro (game rotation, gravity)
+ FUSION_NOGYRO, // use accel mag (geomag rotation)
+ NUM_FUSION_MODE
+};
+
class Fusion {
/*
* the state vector is made of two sub-vector containing respectively:
public:
Fusion();
- void init();
+ void init(int mode = FUSION_9AXIS);
void handleGyro(const vec3_t& w, float dT);
- status_t handleAcc(const vec3_t& a);
+ status_t handleAcc(const vec3_t& a, float dT);
status_t handleMag(const vec3_t& m);
vec4_t getAttitude() const;
vec3_t getBias() const;
bool hasEstimate() const;
private:
+ struct Parameter {
+ float gyroVar;
+ float gyroBiasVar;
+ float accStdev;
+ float magStdev;
+ } mParam;
+
mat<mat33_t, 2, 2> Phi;
vec3_t Ba, Bm;
uint32_t mInitState;
float mGyroRate;
vec<vec3_t, 3> mData;
size_t mCount[3];
+ int mMode;
+
enum { ACC=0x1, MAG=0x2, GYRO=0x4 };
bool checkInitComplete(int, const vec3_t& w, float d = 0);
void initFusion(const vec4_t& q0, float dT);
void predict(const vec3_t& w, float dT);
void update(const vec3_t& z, const vec3_t& Bi, float sigma);
static mat34_t getF(const vec4_t& p);
+ static vec3_t getOrthogonal(const vec3_t &v);
};
}; // namespace android
const static double NS2S = 1.0 / 1000000000.0;
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
vec3_t g;
- if (!mSensorFusion.hasEstimate())
+ if (!mSensorFusion.hasEstimate(FUSION_NOMAG))
return false;
- const mat33_t R(mSensorFusion.getRotationMatrix());
+ const mat33_t R(mSensorFusion.getRotationMatrix(FUSION_NOMAG));
// FIXME: we need to estimate the length of gravity because
// the accelerometer may have a small scaling error. This
// translates to an offset in the linear-acceleration sensor.
}
status_t GravitySensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_NOMAG, ident, enabled);
}
status_t GravitySensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_NOMAG, ident, ns);
}
Sensor GravitySensor::getSensor() const {
}
status_t OrientationSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t OrientationSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor OrientationSensor::getSensor() const {
namespace android {
// ---------------------------------------------------------------------------
-RotationVectorSensor::RotationVectorSensor()
+RotationVectorSensor::RotationVectorSensor(int mode)
: mSensorDevice(SensorDevice::getInstance()),
- mSensorFusion(SensorFusion::getInstance())
+ mSensorFusion(SensorFusion::getInstance()),
+ mMode(mode)
{
}
const sensors_event_t& event)
{
if (event.type == SENSOR_TYPE_ACCELEROMETER) {
- if (mSensorFusion.hasEstimate()) {
- const vec4_t q(mSensorFusion.getAttitude());
+ if (mSensorFusion.hasEstimate(mMode)) {
+ const vec4_t q(mSensorFusion.getAttitude(mMode));
*outEvent = event;
outEvent->data[0] = q.x;
outEvent->data[1] = q.y;
outEvent->data[2] = q.z;
outEvent->data[3] = q.w;
- outEvent->sensor = '_rov';
- outEvent->type = SENSOR_TYPE_ROTATION_VECTOR;
+ outEvent->sensor = getSensorToken();
+ outEvent->type = getSensorType();
return true;
}
}
}
status_t RotationVectorSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(mMode, ident, enabled);
}
status_t RotationVectorSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(mMode, ident, ns);
}
Sensor RotationVectorSensor::getSensor() const {
sensor_t hwSensor;
- hwSensor.name = "Rotation Vector Sensor";
+ hwSensor.name = getSensorName();
hwSensor.vendor = "AOSP";
hwSensor.version = 3;
- hwSensor.handle = '_rov';
- hwSensor.type = SENSOR_TYPE_ROTATION_VECTOR;
+ hwSensor.handle = getSensorToken();
+ hwSensor.type = getSensorType();
hwSensor.maxRange = 1;
hwSensor.resolution = 1.0f / (1<<24);
hwSensor.power = mSensorFusion.getPowerUsage();
return sensor;
}
+int RotationVectorSensor::getSensorType() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return SENSOR_TYPE_ROTATION_VECTOR;
+ case FUSION_NOMAG:
+ return SENSOR_TYPE_GAME_ROTATION_VECTOR;
+ case FUSION_NOGYRO:
+ return SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+ default:
+ assert(0);
+ return 0;
+ }
+}
+
+const char* RotationVectorSensor::getSensorName() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return "Rotation Vector Sensor";
+ case FUSION_NOMAG:
+ return "Game Rotation Vector Sensor";
+ case FUSION_NOGYRO:
+ return "GeoMag Rotation Vector Sensor";
+ default:
+ assert(0);
+ return NULL;
+ }
+}
+
+int RotationVectorSensor::getSensorToken() const {
+ switch(mMode) {
+ case FUSION_9AXIS:
+ return '_rov';
+ case FUSION_NOMAG:
+ return '_gar';
+ case FUSION_NOGYRO:
+ return '_geo';
+ default:
+ assert(0);
+ return 0;
+ }
+}
+
// ---------------------------------------------------------------------------
GyroDriftSensor::GyroDriftSensor()
}
status_t GyroDriftSensor::activate(void* ident, bool enabled) {
- return mSensorFusion.activate(ident, enabled);
+ return mSensorFusion.activate(FUSION_9AXIS, ident, enabled);
}
status_t GyroDriftSensor::setDelay(void* ident, int /*handle*/, int64_t ns) {
- return mSensorFusion.setDelay(ident, ns);
+ return mSensorFusion.setDelay(FUSION_9AXIS, ident, ns);
}
Sensor GyroDriftSensor::getSensor() const {
class RotationVectorSensor : public SensorInterface {
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
+ int mMode;
+
+ int getSensorType() const;
+ const char* getSensorName() const ;
+ int getSensorToken() const ;
public:
- RotationVectorSensor();
+ RotationVectorSensor(int mode = FUSION_9AXIS);
virtual bool process(sensors_event_t* outEvent,
const sensors_event_t& event);
virtual status_t activate(void* ident, bool enabled);
virtual bool isVirtual() const { return true; }
};
+class GameRotationVectorSensor : public RotationVectorSensor {
+public:
+ GameRotationVectorSensor() : RotationVectorSensor(FUSION_NOMAG) {}
+};
+
+class GeoMagRotationVectorSensor : public RotationVectorSensor {
+public:
+ GeoMagRotationVectorSensor() : RotationVectorSensor(FUSION_NOGYRO) {}
+};
+
class GyroDriftSensor : public SensorInterface {
SensorDevice& mSensorDevice;
SensorFusion& mSensorFusion;
SensorFusion::SensorFusion()
: mSensorDevice(SensorDevice::getInstance()),
- mEnabled(false), mGyroTime(0)
+ mAttitude(mAttitudes[FUSION_9AXIS]),
+ mGyroTime(0), mAccTime(0)
{
sensor_t const* list;
Sensor uncalibratedGyro;
ssize_t count = mSensorDevice.getSensorList(&list);
+
+ mEnabled[FUSION_9AXIS] = false;
+ mEnabled[FUSION_NOMAG] = false;
+ mEnabled[FUSION_NOGYRO] = false;
+
if (count > 0) {
for (size_t i=0 ; i<size_t(count) ; i++) {
if (list[i].type == SENSOR_TYPE_ACCELEROMETER) {
// and power/cpu usage.
mEstimatedGyroRate = 200;
mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
- mFusion.init();
+
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ mFusions[i].init(i);
+ }
}
}
void SensorFusion::process(const sensors_event_t& event) {
+
if (event.type == mGyro.getType()) {
- if (mGyroTime != 0) {
- const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
- mFusion.handleGyro(vec3_t(event.data), dT);
+ float dT;
+ if ( event.timestamp - mGyroTime> 0 &&
+ event.timestamp - mGyroTime< (int64_t)(5e7) ) { //0.05sec
+
+ dT = (event.timestamp - mGyroTime) / 1000000000.0f;
// here we estimate the gyro rate (useful for debugging)
const float freq = 1 / dT;
if (freq >= 100 && freq<1000) { // filter values obviously wrong
const float alpha = 1 / (1 + dT); // 1s time-constant
mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
}
+
+ const vec3_t gyro(event.data);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ // fusion in no gyro mode will ignore
+ mFusions[i].handleGyro(gyro, dT);
+ }
+ }
}
mGyroTime = event.timestamp;
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
const vec3_t mag(event.data);
- mFusion.handleMag(mag);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ mFusions[i].handleMag(mag);// fusion in no mag mode will ignore
+ }
+ }
} else if (event.type == SENSOR_TYPE_ACCELEROMETER) {
- const vec3_t acc(event.data);
- mFusion.handleAcc(acc);
- mAttitude = mFusion.getAttitude();
+ float dT;
+ if ( event.timestamp - mAccTime> 0 &&
+ event.timestamp - mAccTime< (int64_t)(1e8) ) { //0.1sec
+ dT = (event.timestamp - mAccTime) / 1000000000.0f;
+
+ const vec3_t acc(event.data);
+ for (int i = 0; i<NUM_FUSION_MODE; ++i) {
+ if (mEnabled[i]) {
+ mFusions[i].handleAcc(acc, dT);
+ mAttitudes[i] = mFusions[i].getAttitude();
+ }
+ }
+ }
+ mAccTime = event.timestamp;
}
}
template <typename T> inline T min(T a, T b) { return a<b ? a : b; }
template <typename T> inline T max(T a, T b) { return a>b ? a : b; }
-status_t SensorFusion::activate(void* ident, bool enabled) {
+status_t SensorFusion::activate(int mode, void* ident, bool enabled) {
ALOGD_IF(DEBUG_CONNECTIONS,
- "SensorFusion::activate(ident=%p, enabled=%d)",
- ident, enabled);
+ "SensorFusion::activate(mode=%d, ident=%p, enabled=%d)",
+ mode, ident, enabled);
- const ssize_t idx = mClients.indexOf(ident);
+ const ssize_t idx = mClients[mode].indexOf(ident);
if (enabled) {
if (idx < 0) {
- mClients.add(ident);
+ mClients[mode].add(ident);
}
} else {
if (idx >= 0) {
- mClients.removeItemsAt(idx);
+ mClients[mode].removeItemsAt(idx);
}
}
- mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
- mSensorDevice.activate(ident, mMag.getHandle(), enabled);
- mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
-
- const bool newState = mClients.size() != 0;
- if (newState != mEnabled) {
- mEnabled = newState;
+ const bool newState = mClients[mode].size() != 0;
+ if (newState != mEnabled[mode]) {
+ mEnabled[mode] = newState;
if (newState) {
- mFusion.init();
- mGyroTime = 0;
+ mFusions[mode].init(mode);
}
}
+
+ mSensorDevice.activate(ident, mAcc.getHandle(), enabled);
+ if (mode != FUSION_NOMAG) {
+ mSensorDevice.activate(ident, mMag.getHandle(), enabled);
+ }
+ if (mode != FUSION_NOGYRO) {
+ mSensorDevice.activate(ident, mGyro.getHandle(), enabled);
+ }
+
return NO_ERROR;
}
-status_t SensorFusion::setDelay(void* ident, int64_t ns) {
+status_t SensorFusion::setDelay(int mode, void* ident, int64_t ns) {
// Call batch with timeout zero instead of setDelay().
+ if (ns > (int64_t)5e7) {
+ ns = (int64_t)(5e7);
+ }
mSensorDevice.batch(ident, mAcc.getHandle(), 0, ns, 0);
- mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
- mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
+ if (mode != FUSION_NOMAG) {
+ mSensorDevice.batch(ident, mMag.getHandle(), 0, ms2ns(20), 0);
+ }
+ if (mode != FUSION_NOGYRO) {
+ mSensorDevice.batch(ident, mGyro.getHandle(), 0, mTargetDelayNs, 0);
+ }
return NO_ERROR;
}
-float SensorFusion::getPowerUsage() const {
+float SensorFusion::getPowerUsage(int mode) const {
float power = mAcc.getPowerUsage() +
- mMag.getPowerUsage() +
- mGyro.getPowerUsage();
+ ((mode != FUSION_NOMAG) ? mMag.getPowerUsage() : 0) +
+ ((mode != FUSION_NOGYRO) ? mGyro.getPowerUsage() : 0);
return power;
}
}
void SensorFusion::dump(String8& result) {
- const Fusion& fusion(mFusion);
+ const Fusion& fusion_9axis(mFusions[FUSION_9AXIS]);
result.appendFormat("9-axis fusion %s (%zd clients), gyro-rate=%7.2fHz, "
"q=< %g, %g, %g, %g > (%g), "
"b=< %g, %g, %g >\n",
- mEnabled ? "enabled" : "disabled",
- mClients.size(),
+ mEnabled[FUSION_9AXIS] ? "enabled" : "disabled",
+ mClients[FUSION_9AXIS].size(),
+ mEstimatedGyroRate,
+ fusion_9axis.getAttitude().x,
+ fusion_9axis.getAttitude().y,
+ fusion_9axis.getAttitude().z,
+ fusion_9axis.getAttitude().w,
+ length(fusion_9axis.getAttitude()),
+ fusion_9axis.getBias().x,
+ fusion_9axis.getBias().y,
+ fusion_9axis.getBias().z);
+
+ const Fusion& fusion_nomag(mFusions[FUSION_NOMAG]);
+ result.appendFormat("game fusion(no mag) %s (%zd clients), "
+ "gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
+ mEnabled[FUSION_NOMAG] ? "enabled" : "disabled",
+ mClients[FUSION_NOMAG].size(),
+ mEstimatedGyroRate,
+ fusion_nomag.getAttitude().x,
+ fusion_nomag.getAttitude().y,
+ fusion_nomag.getAttitude().z,
+ fusion_nomag.getAttitude().w,
+ length(fusion_nomag.getAttitude()),
+ fusion_nomag.getBias().x,
+ fusion_nomag.getBias().y,
+ fusion_nomag.getBias().z);
+
+ const Fusion& fusion_nogyro(mFusions[FUSION_NOGYRO]);
+ result.appendFormat("geomag fusion (no gyro) %s (%zd clients), "
+ "gyro-rate=%7.2fHz, "
+ "q=< %g, %g, %g, %g > (%g), "
+ "b=< %g, %g, %g >\n",
+ mEnabled[FUSION_NOGYRO] ? "enabled" : "disabled",
+ mClients[FUSION_NOGYRO].size(),
mEstimatedGyroRate,
- fusion.getAttitude().x,
- fusion.getAttitude().y,
- fusion.getAttitude().z,
- fusion.getAttitude().w,
- length(fusion.getAttitude()),
- fusion.getBias().x,
- fusion.getBias().y,
- fusion.getBias().z);
+ fusion_nogyro.getAttitude().x,
+ fusion_nogyro.getAttitude().y,
+ fusion_nogyro.getAttitude().z,
+ fusion_nogyro.getAttitude().w,
+ length(fusion_nogyro.getAttitude()),
+ fusion_nogyro.getBias().x,
+ fusion_nogyro.getBias().y,
+ fusion_nogyro.getBias().z);
}
// ---------------------------------------------------------------------------
Sensor mAcc;
Sensor mMag;
Sensor mGyro;
- Fusion mFusion;
- bool mEnabled;
+
+ Fusion mFusions[NUM_FUSION_MODE]; // normal, no_mag, no_gyro
+
+ bool mEnabled[NUM_FUSION_MODE];
+
+ vec4_t &mAttitude;
+ vec4_t mAttitudes[NUM_FUSION_MODE];
+
+ SortedVector<void*> mClients[3];
+
float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
+
nsecs_t mGyroTime;
- vec4_t mAttitude;
- SortedVector<void*> mClients;
+ nsecs_t mAccTime;
SensorFusion();
public:
void process(const sensors_event_t& event);
- bool isEnabled() const { return mEnabled; }
- bool hasEstimate() const { return mFusion.hasEstimate(); }
- mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
- vec4_t getAttitude() const { return mAttitude; }
- vec3_t getGyroBias() const { return mFusion.getBias(); }
+ bool isEnabled() const {
+ return mEnabled[FUSION_9AXIS] ||
+ mEnabled[FUSION_NOMAG] ||
+ mEnabled[FUSION_NOGYRO];
+ }
+
+ bool hasEstimate(int mode = FUSION_9AXIS) const {
+ return mFusions[mode].hasEstimate();
+ }
+
+ mat33_t getRotationMatrix(int mode = FUSION_9AXIS) const {
+ return mFusions[mode].getRotationMatrix();
+ }
+
+ vec4_t getAttitude(int mode = FUSION_9AXIS) const {
+ return mAttitudes[mode];
+ }
+
+ vec3_t getGyroBias() const { return mFusions[FUSION_9AXIS].getBias(); }
float getEstimatedRate() const { return mEstimatedGyroRate; }
- status_t activate(void* ident, bool enabled);
- status_t setDelay(void* ident, int64_t ns);
+ status_t activate(int mode, void* ident, bool enabled);
+ status_t setDelay(int mode, void* ident, int64_t ns);
- float getPowerUsage() const;
+ float getPowerUsage(int mode=FUSION_9AXIS) const;
int32_t getMinDelay() const;
void dump(String8& result);
uint32_t virtualSensorsNeeds =
(1<<SENSOR_TYPE_GRAVITY) |
(1<<SENSOR_TYPE_LINEAR_ACCELERATION) |
- (1<<SENSOR_TYPE_ROTATION_VECTOR);
+ (1<<SENSOR_TYPE_ROTATION_VECTOR) |
+ (1<<SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR) |
+ (1<<SENSOR_TYPE_GAME_ROTATION_VECTOR);
mLastEventSeen.setCapacity(count);
for (ssize_t i=0 ; i<count ; i++) {
- registerSensor( new HardwareSensor(list[i]) );
+ bool useThisSensor=true;
+
switch (list[i].type) {
case SENSOR_TYPE_ACCELEROMETER:
hasAccel = true;
case SENSOR_TYPE_GRAVITY:
case SENSOR_TYPE_LINEAR_ACCELERATION:
case SENSOR_TYPE_ROTATION_VECTOR:
- virtualSensorsNeeds &= ~(1<<list[i].type);
+ case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
+ case SENSOR_TYPE_GAME_ROTATION_VECTOR:
+ if (IGNORE_HARDWARE_FUSION) {
+ useThisSensor = false;
+ } else {
+ virtualSensorsNeeds &= ~(1<<list[i].type);
+ }
break;
}
+ if (useThisSensor) {
+ registerSensor( new HardwareSensor(list[i]) );
+ }
}
// it's safe to instantiate the SensorFusion object here
mUserSensorList = mSensorList;
if (hasGyro && hasAccel && hasMag) {
- Sensor aSensor;
-
// Add Android virtual sensors if they're not already
// available in the HAL
+ Sensor aSensor;
aSensor = registerVirtualSensor( new RotationVectorSensor() );
if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
mUserSensorList.add(aSensor);
}
- aSensor = registerVirtualSensor( new GravitySensor(list, count) );
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
- mUserSensorList.add(aSensor);
- }
-
- aSensor = registerVirtualSensor( new LinearAccelerationSensor(list, count) );
- if (virtualSensorsNeeds & (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
- mUserSensorList.add(aSensor);
- }
-
aSensor = registerVirtualSensor( new OrientationSensor() );
if (virtualSensorsNeeds & (1<<SENSOR_TYPE_ROTATION_VECTOR)) {
// if we are doing our own rotation-vector, also add
mUserSensorList.replaceAt(aSensor, orientationIndex);
}
+ aSensor = registerVirtualSensor(
+ new LinearAccelerationSensor(list, count) );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_LINEAR_ACCELERATION)) {
+ mUserSensorList.add(aSensor);
+ }
+
// virtual debugging sensors are not added to mUserSensorList
registerVirtualSensor( new CorrectedGyroSensor(list, count) );
registerVirtualSensor( new GyroDriftSensor() );
}
+ if (hasAccel && hasGyro) {
+ Sensor aSensor;
+
+ aSensor = registerVirtualSensor(
+ new GravitySensor(list, count) );
+ if (virtualSensorsNeeds & (1<<SENSOR_TYPE_GRAVITY)) {
+ mUserSensorList.add(aSensor);
+ }
+
+ aSensor = registerVirtualSensor(
+ new GameRotationVectorSensor() );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_GAME_ROTATION_VECTOR)) {
+ mUserSensorList.add(aSensor);
+ }
+ }
+
+ if (hasAccel && hasMag) {
+ Sensor aSensor;
+
+ aSensor = registerVirtualSensor(
+ new GeoMagRotationVectorSensor() );
+ if (virtualSensorsNeeds &
+ (1<<SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR)) {
+ mUserSensorList.add(aSensor);
+ }
+ }
+
// debugging sensor list
mUserSensorListDebug = mSensorList;
#endif
// ---------------------------------------------------------------------------
-
+#define IGNORE_HARDWARE_FUSION false
#define DEBUG_CONNECTIONS false
// Max size is 100 KB which is enough to accept a batch of about 1000 events.
#define MAX_SOCKET_BUFFER_SIZE_BATCHED 100 * 1024
OSDN Git Service
