217 lines
6.5 KiB
C
217 lines
6.5 KiB
C
/*
|
||
* Copyright (C) 2016 The Android Open Source Project
|
||
*
|
||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
* you may not use this file except in compliance with the License.
|
||
* You may obtain a copy of the License at
|
||
*
|
||
* http://www.apache.org/licenses/LICENSE-2.0
|
||
*
|
||
* Unless required by applicable law or agreed to in writing, software
|
||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
* See the License for the specific language governing permissions and
|
||
* limitations under the License.
|
||
*/
|
||
|
||
/* This module estimates the accelerometer offsets using the KASA sphere fit.
|
||
* The algorithm senses stillness and classifies the data into seven sphere caps
|
||
* (nx,nxb,ny,nyb,nz,nzb,nle). Once the buckets are full the data is used to
|
||
* fit the sphere calculating the offsets and the radius. This can be done,
|
||
* because when the accelerometer is still it sees only gravity and hence all
|
||
* the vectors should end onto a sphere. Furthermore the offset values are
|
||
* subtracted from the accelerometer data calibrating the sensor.
|
||
*/
|
||
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
|
||
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
|
||
|
||
#include <stdint.h>
|
||
#include <sys/types.h>
|
||
|
||
#include "common/math/kasa.h"
|
||
#include "common/math/mat.h"
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
#define ACCEL_CAL_NUM_TEMP_WINDOWS 2
|
||
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
#define DGB_HISTORY 10
|
||
#define TEMP_HISTOGRAM 25
|
||
#endif
|
||
|
||
// Data struct for the accel stillness detection.
|
||
struct AccelStillDet {
|
||
// Start timer for a new still detection (in ns).
|
||
uint64_t start_time;
|
||
|
||
// Save accumulate variables to calc. mean and var.
|
||
float acc_x, acc_y, acc_z;
|
||
float acc_xx, acc_yy, acc_zz;
|
||
|
||
// Mean and var.
|
||
float mean_x, mean_y, mean_z;
|
||
float var_x, var_y, var_z;
|
||
|
||
// # of samples used in the stillness detector.
|
||
uint32_t nsamples;
|
||
|
||
// Controling the Stillness algo with T0 and Th
|
||
// time the sensor must be still to trigger still detection.
|
||
uint32_t min_batch_window;
|
||
uint32_t max_batch_window;
|
||
|
||
// Need a minimum amount of samples, filters out low sample rates.
|
||
uint32_t min_batch_size;
|
||
|
||
// Setting Th to var_th.
|
||
float var_th;
|
||
|
||
// Total number of stillness.
|
||
uint32_t n_still;
|
||
};
|
||
|
||
/* Struct for good data function.
|
||
* Counts the vectors that fall into the 7
|
||
* Sphere caps.
|
||
*/
|
||
struct AccelGoodData {
|
||
// Bucket counters.
|
||
uint32_t nx, nxb, ny, nyb, nz, nzb, nle;
|
||
|
||
// Bucket full values.
|
||
uint32_t nfx, nfxb, nfy, nfyb, nfz, nfzb, nfle;
|
||
|
||
// Temp check (in degree C).
|
||
float acc_t, acc_tt;
|
||
float var_t, mean_t;
|
||
|
||
// Eigen Values.
|
||
float e_x, e_y, e_z;
|
||
};
|
||
|
||
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
// Struct for stats and debug.
|
||
struct AccelStatsMem {
|
||
// Temp (in degree C).
|
||
uint32_t t_hist[TEMP_HISTOGRAM];
|
||
uint64_t start_time_nanos;
|
||
|
||
// Offset update counter.
|
||
uint32_t noff;
|
||
uint32_t noff_max;
|
||
|
||
// Offset history.
|
||
float var_t[DGB_HISTORY];
|
||
float mean_t[DGB_HISTORY];
|
||
float x_o[DGB_HISTORY];
|
||
float y_o[DGB_HISTORY];
|
||
float z_o[DGB_HISTORY];
|
||
float e_x[DGB_HISTORY];
|
||
float e_y[DGB_HISTORY];
|
||
float e_z[DGB_HISTORY];
|
||
float rad[DGB_HISTORY];
|
||
|
||
uint8_t n_o;
|
||
uint64_t cal_time[DGB_HISTORY];
|
||
|
||
// Total Buckets counter.
|
||
uint32_t ntx, ntxb, nty, ntyb, ntz, ntzb, ntle;
|
||
};
|
||
#endif
|
||
|
||
// Struct for an accel calibration for a single temperature window.
|
||
struct AccelCalAlgo {
|
||
struct AccelGoodData agd;
|
||
// TODO(mkramerm): Replace all abbreviations.
|
||
struct KasaFit akf;
|
||
};
|
||
|
||
// AccelCal algorithm parameters (see the AccelCal for details).
|
||
struct AccelCalParameters {
|
||
// t0 -> Sets the time how long the accel has to be still in ns.
|
||
// n_s -> Defines the minimum number of samples for the stillness.
|
||
// th -> Sets the threshold for the stillness VAR in (g rms)^2.
|
||
// fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
|
||
// sphere cap (Bucket) is needed to reach full.
|
||
uint32_t t0;
|
||
uint32_t n_s;
|
||
uint32_t fx;
|
||
uint32_t fxb;
|
||
uint32_t fy;
|
||
uint32_t fyb;
|
||
uint32_t fz;
|
||
uint32_t fzb;
|
||
uint32_t fle;
|
||
float th;
|
||
};
|
||
|
||
// Complete accel calibration struct.
|
||
struct AccelCal {
|
||
struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS];
|
||
struct AccelStillDet asd;
|
||
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
struct AccelStatsMem adf;
|
||
#endif
|
||
|
||
// Offsets are only updated while the accelerometer is not running. Hence need
|
||
// to store a new offset, which gets updated during a power down event.
|
||
float x_bias_new, y_bias_new, z_bias_new;
|
||
|
||
// Average temperature of the bias update.
|
||
float average_temperature_celsius;
|
||
|
||
// Offset values that get subtracted from live data
|
||
float x_bias, y_bias, z_bias;
|
||
|
||
#ifdef IMU_TEMP_DBG_ENABLED
|
||
// Temporary time variable used to to print an IMU temperature value with a
|
||
// lower custom sample rate.
|
||
uint64_t temp_time_nanos;
|
||
#endif
|
||
};
|
||
|
||
/* This function runs the accel calibration algorithm.
|
||
* sample_time_nanos -> is the sensor timestamp in ns and
|
||
* is used to check the stillness time.
|
||
* x,y,z -> is the sensor data (m/s^2) for the three axes.
|
||
* Data is converted to g’s inside the function.
|
||
* temp -> is the temperature of the IMU (degree C).
|
||
*/
|
||
void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
|
||
float y, float z, float temp);
|
||
|
||
/* This function initializes the accCalRun data struct.
|
||
* [parameters]:
|
||
* t0 -> Sets the time how long the accel has to be still in ns.
|
||
* n_s -> Defines the minimum number of samples for the stillness.
|
||
* th -> Sets the threshold for the stillness VAR in (g rms)^2.
|
||
* fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
|
||
* sphere cap (Bucket) is needed to reach full.
|
||
*/
|
||
void accelCalInit(struct AccelCal *acc,
|
||
const struct AccelCalParameters *parameters);
|
||
|
||
void accelCalDestroy(struct AccelCal *acc);
|
||
|
||
// Ensures that the offset is only updated during Sensor power down.
|
||
bool accelCalUpdateBias(struct AccelCal *acc, float *x, float *y, float *z);
|
||
|
||
void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z);
|
||
|
||
void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z);
|
||
|
||
// Returns true when a new accel calibration is available.
|
||
bool accelCalNewBiasAvailable(struct AccelCal *acc);
|
||
|
||
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
void accelCalDebPrint(struct AccelCal *acc, float temp);
|
||
#endif
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_
|