ADHOC
/
replicant-frameworks_native
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge changes Id5584bc1,I8ca826d0 

* changes:
  revert parts of dc5b63e40, which made gyro drift estimation unstable
  use gyro timestamp directly in fusion
This commit is contained in:
Mathias Agopian 2013-05-30 21:30:17 +00:00 committed by Android (Google) Code Review
parent af0f8c0a56 24f9bea8eb
commit b2397fff6f
3 changed files with 8 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
services/sensorservice/Fusion.cpp
View File

@ -220,22 +220,6 @@ void Fusion::initFusion(const vec4_t& q, float dT)
// initial covariance: Var{ x(t0) }
// TODO: initialize P correctly
P = 0;
// it is unclear how to set the initial covariance. It does affect
// how quickly the fusion converges. Experimentally it would take
// about 10 seconds at 200 Hz to estimate the gyro-drift with an
// initial covariance of 0, and about a second with an initial covariance
// of about 1 deg/s.
const float covv = 0;
const float covu = 0.5f * (float(M_PI) / 180);
mat33_t& Pv = P[0][0];
Pv[0][0] = covv;
Pv[1][1] = covv;
Pv[2][2] = covv;
mat33_t& Pu = P[1][1];
Pu[0][0] = covu;
Pu[1][1] = covu;
Pu[2][2] = covu;
}
bool Fusion::hasEstimate() const {

11
services/sensorservice/SensorFusion.cpp
View File

@ -53,8 +53,8 @@ SensorFusion::SensorFusion()
// 200 Hz for gyro events is a good compromise between precision
// and power/cpu usage.
mGyroRate = 200;
mTargetDelayNs = 1000000000LL/mGyroRate;
mEstimatedGyroRate = 200;
mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
mFusion.init();
}
}
@ -63,14 +63,15 @@ 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);
// 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
mGyroRate = freq + (mGyroRate - freq)*alpha;
mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
}
}
mGyroTime = event.timestamp;
mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);
} else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
const vec3_t mag(event.data);
mFusion.handleMag(mag);
@ -142,7 +143,7 @@ void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
"b=< %g, %g, %g >\n",
mEnabled ? "enabled" : "disabled",
mClients.size(),
mGyroRate,
mEstimatedGyroRate,
fusion.getAttitude().x,
fusion.getAttitude().y,
fusion.getAttitude().z,

4
services/sensorservice/SensorFusion.h
View File

@ -44,7 +44,7 @@ class SensorFusion : public Singleton<SensorFusion> {
Sensor mGyro;
Fusion mFusion;
bool mEnabled;
float mGyroRate;
float mEstimatedGyroRate;
nsecs_t mTargetDelayNs;
nsecs_t mGyroTime;
vec4_t mAttitude;
@ -60,7 +60,7 @@ public:
mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
vec4_t getAttitude() const { return mAttitude; }
vec3_t getGyroBias() const { return mFusion.getBias(); }
float getEstimatedRate() const { return mGyroRate; }
float getEstimatedRate() const { return mEstimatedGyroRate; }
status_t activate(void* ident, bool enabled);
status_t setDelay(void* ident, int64_t ns);