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

Fix a few style issues and remove LOG spam

Browse Source 
Change-Id: I6b6f75373f4ac28f98dea6a6f1c2567a6aa02243
This commit is contained in:
Mathias Agopian 2011-08-24 18:40:33 -07:00
parent 3e87d8dade
commit a83f45c6c7
1 changed files with 28 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
services/sensorservice/Fusion.cpp
View File

@ -50,33 +50,38 @@ static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)
static const float SYMMETRY_TOLERANCE = 1e-10f;
/*
* Accelerometer updates will not be performed near free fall to avoid ill-conditioning and
* div by zeros.
* Accelerometer updates will not be performed near free fall to avoid
* 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 FREE_FALL_THRESHOLD_SQ = FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
static const float FREE_FALL_THRESHOLD_SQ =
FREE_FALL_THRESHOLD*FREE_FALL_THRESHOLD;
/*
* The geomagnetic-field should be between 30uT and 60uT.
* Fields strengths greater than this likely indicate a local magnetic disturbance which
* we do not want to update into the fused frame.
* Fields strengths greater than this likely indicate a local magnetic
* disturbance which we do not want to update into the fused frame.
*/
static const float MAX_VALID_MAGNETIC_FIELD = 100; // uT
static const float MAX_VALID_MAGNETIC_FIELD_SQ = MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;
static const float MAX_VALID_MAGNETIC_FIELD_SQ =
MAX_VALID_MAGNETIC_FIELD*MAX_VALID_MAGNETIC_FIELD;
/*
* Values of the field smaller than this should be ignored in fusion to avoid ill-conditioning.
* This state can happen with anomalous local magnetic disturbances canceling the Earth field.
* Values of the field smaller than this should be ignored in fusion to avoid
* ill-conditioning. This state can happen with anomalous local magnetic
* disturbances canceling the Earth field.
*/
static const float MIN_VALID_MAGNETIC_FIELD = 10; // uT
static const float MIN_VALID_MAGNETIC_FIELD_SQ = MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;
static const float MIN_VALID_MAGNETIC_FIELD_SQ =
MIN_VALID_MAGNETIC_FIELD*MIN_VALID_MAGNETIC_FIELD;
/*
* If the cross product of two vectors has magnitude squared less than this, we reject it as
* invalid due to alignment of the vectors.
* This threshold is used to check for the case where the magnetic field sample is parallel to
* the gravity field, which can happen in certain places due to magnetic field disturbances.
* If the cross product of two vectors has magnitude squared less than this,
* we reject it as invalid due to alignment of the vectors.
* This threshold is used to check for the case where the magnetic field sample
* is parallel to the gravity field, which can happen in certain places due
* to magnetic field disturbances.
*/
static const float MIN_VALID_CROSS_PRODUCT_MAG = 1.0e-3;
static const float MIN_VALID_CROSS_PRODUCT_MAG_SQ =
@ -273,7 +278,6 @@ void Fusion::handleGyro(const vec3_t& w, float dT) {
status_t Fusion::handleAcc(const vec3_t& a) {
// ignore acceleration data if we're close to free-fall
if (length_squared(a) < FREE_FALL_THRESHOLD_SQ) {
LOGW("handleAcc: near free fall, not updating!");
return BAD_VALUE;
}
@ -290,29 +294,31 @@ status_t Fusion::handleMag(const vec3_t& m) {
// reject if too large to avoid spurious magnetic sources
const float magFieldSq = length_squared(m);
if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ) {
LOGW("handleMag: magnetic field too large, not updating!");
return BAD_VALUE;
} else if (magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
// Also reject if too small since we will get ill-defined (zero mag) cross-products below
LOGW("handleMag: magnetic field too small, not updating!");
// Also reject if too small since we will get ill-defined (zero mag)
// cross-products below
return BAD_VALUE;
}
if (!checkInitComplete(MAG, m))
return BAD_VALUE;
// Orthogonalize the magnetic field to the gravity field, mapping it into tangent to Earth.
// Orthogonalize the magnetic field to the gravity field, mapping it into
// tangent to Earth.
const vec3_t up( getRotationMatrix() * Ba );
const vec3_t east( cross_product(m, up) );
// If the m and up vectors align, the cross product magnitude will approach 0.
// Reject this case as well to avoid div by zero problems and ill-conditioning below.
// If the m and up vectors align, the cross product magnitude will
// approach 0.
// Reject this case as well to avoid div by zero problems and
// ill-conditioning below.
if (length_squared(east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
LOGW("handleMag: magnetic field too aligned with up vector, not updating!");
return BAD_VALUE;
}
// If we have created an orthogonal magnetic field successfully, then pass it in as the update.
// If we have created an orthogonal magnetic field successfully,
// then pass it in as the update.
vec3_t north( cross_product(up, east) );
const float l = 1 / length(north);