alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-22 00:17:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity
upm/src/mmc35240/mmc35240.cxx
Lay, Kuan Loon fabf4287d6 upm: enable MMC35240 3-axis magnetic sensor library and example 
MMC35240 is 3-axis magnetic sensor from MEMSIC.

This sensor can measure magnetic fields within the full scale range of
+-24 Gauss (G).

The library provided is libupm-mmc35240.so
The example provided is mmc35240-example-cxx where it will print x,y,z axis
when trigger buffer data is ready. It's also print azimuth value.

This sensor requires calibration. Please shake the sensor in figure 8 pattern,
mmc35240-example will print the calibrated level.

As the sensor data is noisy, we have implemented denoise algorithm within the
sensor library.

The azimuth formula is provided by Han, He <he.han@intel.com>.

Signed-off-by: Lay, Kuan Loon <kuan.loon.lay@intel.com>
Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
2017-03-16 17:22:40 -07:00

812 lines
24 KiB
C++
Raw Blame History

/*
* Author: Lay, Kuan Loon <kuan.loon.lay@intel.com>
* Copyright (c) 2016 Intel Corporation.
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the
* "Software"), to deal in the Software without restriction, including
* without limitation the rights to use, copy, modify, merge, publish,
* distribute, sublicense, and/or sell copies of the Software, and to
* permit persons to whom the Software is furnished to do so, subject to
* the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include <iostream>
#include <string>
#include <stdexcept>
#include <string.h>
#include <math.h>
#include "mmc35240.hpp"
#define NUMBER_OF_BITS_IN_BYTE 8
/* Compass defines */
#define CONVERT_GAUSS_TO_MICROTESLA(x) ((x) *100)
#define EPSILON 0.000000001
#define MAGNETIC_LOW 960 /* 31 micro tesla squared */
#define CAL_STEPS 5
/* Filter defines */
#define FILTER_MAX_SAMPLE 20
#define FILTER_NUM_FILED 3
using namespace upm;
using namespace android;
/* We'll have multiple calibration levels so that we can provide an estimation as fast as possible
*/
static const float min_diffs[CAL_STEPS] = { 0.2, 0.25, 0.4, 0.6, 1.0 };
static const float max_sqr_errs[CAL_STEPS] = { 10.0, 10.0, 8.0, 5.0, 3.5 };
static const unsigned int lookback_counts[CAL_STEPS] = { 2, 3, 4, 5, 6 };
MMC35240::MMC35240(int device)
{
float mag_scale;
char trigger[64];
if (!(m_iio = mraa_iio_init(device))) {
throw std::invalid_argument(std::string(__FUNCTION__) +
": mraa_iio_init() failed, invalid device?");
return;
}
m_scale = 1;
m_iio_device_num = device;
sprintf(trigger, "hrtimer-mmc35240-hr-dev%d", device);
if (mraa_iio_create_trigger(m_iio, trigger) != MRAA_SUCCESS)
fprintf(stderr, "Create trigger %s failed\n", trigger);
if (mraa_iio_get_mount_matrix(m_iio, "in_mount_matrix", m_mount_matrix) == MRAA_SUCCESS)
m_mount_matrix_exist = true;
else
m_mount_matrix_exist = false;
if (mraa_iio_read_float(m_iio, "in_magn_scale", &mag_scale) == MRAA_SUCCESS)
m_scale = mag_scale;
// calibration init data
initCalibrate();
// filter init data
memset(&m_filter, 0, sizeof(filter_average_t));
m_filter.max_samples = FILTER_MAX_SAMPLE;
m_filter.num_fields = FILTER_NUM_FILED;
}
MMC35240::~MMC35240()
{
if (m_filter.history) {
free(m_filter.history);
m_filter.history = NULL;
}
if (m_filter.history_sum) {
free(m_filter.history_sum);
m_filter.history_sum = NULL;
}
if (m_iio)
mraa_iio_close(m_iio);
}
void
MMC35240::installISR(void (*isr)(char*), void* arg)
{
mraa_iio_trigger_buffer(m_iio, isr, NULL);
}
int64_t
MMC35240::getChannelValue(unsigned char* input, mraa_iio_channel* chan)
{
uint64_t u64 = 0;
int i;
int storagebits = chan->bytes * NUMBER_OF_BITS_IN_BYTE;
int realbits = chan->bits_used;
int zeroed_bits = storagebits - realbits;
uint64_t sign_mask;
uint64_t value_mask;
if (!chan->lendian)
for (i = 0; i < storagebits / NUMBER_OF_BITS_IN_BYTE; i++)
u64 = (u64 << 8) | input[i];
else
for (i = storagebits / NUMBER_OF_BITS_IN_BYTE - 1; i >= 0; i--)
u64 = (u64 << 8) | input[i];
u64 = (u64 >> chan->shift) & (~0ULL >> zeroed_bits);
if (!chan->signedd)
return (int64_t) u64; /* We don't handle unsigned 64 bits int */
/* Signed integer */
switch (realbits) {
case 0 ... 1:
return 0;
case 8:
return (int64_t)(int8_t) u64;
case 16:
return (int64_t)(int16_t) u64;
case 32:
return (int64_t)(int32_t) u64;
case 64:
return (int64_t) u64;
default:
sign_mask = 1 << (realbits - 1);
value_mask = sign_mask - 1;
if (u64 & sign_mask)
return -((~u64 & value_mask) + 1); /* Negative value: return 2-complement */
else
return (int64_t) u64; /* Positive value */
}
}
bool
MMC35240::enableBuffer(int length)
{
mraa_iio_write_int(m_iio, "buffer/length", length);
// enable must be last step, else will have error in writing above config
mraa_iio_write_int(m_iio, "buffer/enable", 1);
return true;
}
bool
MMC35240::disableBuffer()
{
mraa_iio_write_int(m_iio, "buffer/enable", 0);
return true;
}
bool
MMC35240::setScale(float scale)
{
m_scale = scale;
mraa_iio_write_float(m_iio, "in_magn_scale", scale);
return true;
}
bool
MMC35240::setSamplingFrequency(float sampling_frequency)
{
m_sampling_frequency = sampling_frequency;
mraa_iio_write_float(m_iio, "in_magn_sampling_frequency", sampling_frequency);
return true;
}
bool
MMC35240::enable3AxisChannel()
{
char trigger[64];
sprintf(trigger, "mmc35240-hr-dev%d", m_iio_device_num);
mraa_iio_write_string(m_iio, "trigger/current_trigger", trigger);
mraa_iio_write_int(m_iio, "scan_elements/in_magn_x_en", 1);
mraa_iio_write_int(m_iio, "scan_elements/in_magn_y_en", 1);
mraa_iio_write_int(m_iio, "scan_elements/in_magn_z_en", 1);
// need update channel data size after enable
mraa_iio_update_channels(m_iio);
return true;
}
void
MMC35240::extract3Axis(char* data, float* x, float* y, float* z)
{
mraa_iio_channel* channels = mraa_iio_get_channels(m_iio);
float tmp[3];
int64_t iio_x, iio_y, iio_z;
iio_x = getChannelValue((unsigned char*) (data), &channels[0]);
iio_y = getChannelValue((unsigned char*) (data + 4), &channels[1]);
iio_z = getChannelValue((unsigned char*) (data + 8), &channels[2]);
// Raw data is magnetic field along axis x, y, and z. Units after application of scale are Gauss
*x = CONVERT_GAUSS_TO_MICROTESLA(iio_x * m_scale);
*y = CONVERT_GAUSS_TO_MICROTESLA(iio_y * m_scale);
*z = CONVERT_GAUSS_TO_MICROTESLA(iio_z * m_scale);
if (m_mount_matrix_exist) {
tmp[0] = *x * m_mount_matrix[0] + *y * m_mount_matrix[1] + *z * m_mount_matrix[2];
tmp[1] = *x * m_mount_matrix[3] + *y * m_mount_matrix[4] + *z * m_mount_matrix[5];
tmp[2] = *x * m_mount_matrix[6] + *y * m_mount_matrix[7] + *z * m_mount_matrix[8];
*x = tmp[0];
*y = tmp[1];
*z = tmp[2];
}
calibrateCompass(x, y, z, &m_cal_data);
denoise_average(x, y, z);
}
int
MMC35240::getCalibratedLevel()
{
return m_cal_level;
}
void
MMC35240::initCalibrate()
{
m_cal_level = 0;
resetSample(&m_cal_data);
m_cal_data.offset[0][0] = 0;
m_cal_data.offset[1][0] = 0;
m_cal_data.offset[2][0] = 0;
m_cal_data.w_invert[0][0] = 1;
m_cal_data.w_invert[1][0] = 0;
m_cal_data.w_invert[2][0] = 0;
m_cal_data.w_invert[0][1] = 0;
m_cal_data.w_invert[1][1] = 1;
m_cal_data.w_invert[2][1] = 0;
m_cal_data.w_invert[0][2] = 0;
m_cal_data.w_invert[1][2] = 0;
m_cal_data.w_invert[2][2] = 1;
m_cal_data.bfield = 0;
}
void
MMC35240::getCalibratedData(int* cal_level, double offset[3][1], double w_invert[3][3], double* bfield)
{
*cal_level = m_cal_level;
offset[0][0] = m_cal_data.offset[0][0];
offset[1][0] = m_cal_data.offset[1][0];
offset[2][0] = m_cal_data.offset[2][0];
w_invert[0][0] = m_cal_data.w_invert[0][0];
w_invert[1][0] = m_cal_data.w_invert[1][0];
w_invert[2][0] = m_cal_data.w_invert[2][0];
w_invert[0][1] = m_cal_data.w_invert[0][1];
w_invert[1][1] = m_cal_data.w_invert[1][1];
w_invert[2][1] = m_cal_data.w_invert[2][1];
w_invert[0][2] = m_cal_data.w_invert[0][2];
w_invert[1][2] = m_cal_data.w_invert[1][2];
w_invert[2][2] = m_cal_data.w_invert[2][2];
*bfield = m_cal_data.bfield;
}
void
MMC35240::loadCalibratedData(int cal_level, double offset[3][1], double w_invert[3][3], double bfield)
{
m_cal_level = cal_level;
m_cal_data.offset[0][0] = offset[0][0];
m_cal_data.offset[1][0] = offset[1][0];
m_cal_data.offset[2][0] = offset[2][0];
m_cal_data.w_invert[0][0] = w_invert[0][0];
m_cal_data.w_invert[1][0] = w_invert[1][0];
m_cal_data.w_invert[2][0] = w_invert[2][0];
m_cal_data.w_invert[0][1] = w_invert[0][1];
m_cal_data.w_invert[1][1] = w_invert[1][1];
m_cal_data.w_invert[2][1] = w_invert[2][1];
m_cal_data.w_invert[0][2] = w_invert[0][2];
m_cal_data.w_invert[1][2] = w_invert[1][2];
m_cal_data.w_invert[2][2] = w_invert[2][2];
m_cal_data.bfield = bfield;
}
void
MMC35240::resetSample(compass_cal_t* data)
{
int i, j;
data->sample_count = 0;
for (i = 0; i < MAGN_DS_SIZE; i++)
for (j = 0; j < 3; j++)
data->sample[i][j] = 0;
data->average[0] = data->average[1] = data->average[2] = 0;
}
void
MMC35240::calibrateCompass(float* x, float* y, float* z, compass_cal_t* cal_data)
{
int cal_level;
/* Calibration is continuous */
compassCollect(x, y, z, cal_data);
cal_level = compassReady(cal_data);
switch (cal_level) {
case 0:
scale(x, y, z);
break;
default:
compassComputeCal(x, y, z, cal_data);
break;
}
}
int
MMC35240::compassCollect(float* x, float* y, float* z, compass_cal_t* cal_data)
{
float data[3] = { *x, *y, *z };
unsigned int index, j;
unsigned int lookback_count;
float min_diff;
/* Discard the point if not valid */
if (data[0] == 0 || data[1] == 0 || data[2] == 0)
return -1;
lookback_count = lookback_counts[m_cal_level];
min_diff = min_diffs[m_cal_level];
/* For the current point to be accepted, each x/y/z value must be different enough to the last
* several collected points */
if (cal_data->sample_count > 0 && cal_data->sample_count < MAGN_DS_SIZE) {
unsigned int lookback =
lookback_count < cal_data->sample_count ? lookback_count : cal_data->sample_count;
for (index = 0; index < lookback; index++)
for (j = 0; j < 3; j++)
if (fabsf(data[j] - cal_data->sample[cal_data->sample_count - 1 - index][j]) <
min_diff) {
#ifdef DEBUG
printf("CompassCalibration:point reject: [%f,%f,%f], selected_count=%d\n",
data[0],
data[1],
data[2],
cal_data->sample_count);
#endif
return 0;
}
}
if (cal_data->sample_count < MAGN_DS_SIZE) {
memcpy(cal_data->sample[cal_data->sample_count], data, sizeof(float) * 3);
cal_data->sample_count++;
cal_data->average[0] += data[0];
cal_data->average[1] += data[1];
cal_data->average[2] += data[2];
#ifdef DEBUG
printf("CompassCalibration:point collected [%f,%f,%f], selected_count=%d\n",
(double) data[0],
(double) data[1],
(double) data[2],
cal_data->sample_count);
#endif
}
return 1;
}
int
MMC35240::compassReady(compass_cal_t* cal_data)
{
mat_input_t mat;
int i;
float max_sqr_err;
compass_cal_t new_cal_data;
/*
* Some sensors take unrealistically long to calibrate at higher levels. We'll use a
* max_cal_level if we have such a property setup,
* or go with the default settings if not.
*/
int cal_steps = CAL_STEPS;
if (cal_data->sample_count < MAGN_DS_SIZE)
return m_cal_level;
max_sqr_err = max_sqr_errs[m_cal_level];
/* Enough points have been collected, do the ellipsoid calibration */
/* Compute average per axis */
cal_data->average[0] /= MAGN_DS_SIZE;
cal_data->average[1] /= MAGN_DS_SIZE;
cal_data->average[2] /= MAGN_DS_SIZE;
for (i = 0; i < MAGN_DS_SIZE; i++) {
mat[i][0] = cal_data->sample[i][0];
mat[i][1] = cal_data->sample[i][1];
mat[i][2] = cal_data->sample[i][2];
}
/* Check if result is good. The sample data must remain the same */
new_cal_data = *cal_data;
if (ellipsoidFit(mat, new_cal_data.offset, new_cal_data.w_invert, &new_cal_data.bfield)) {
double new_err = calcSquareErr(&new_cal_data);
#ifdef DEBUG
printf("new err is %f, max sqr err id %f\n", new_err, max_sqr_err);
#endif
if (new_err < max_sqr_err) {
double err = calcSquareErr(cal_data);
if (new_err < err) {
/* New cal data is better, so we switch to the new */
cal_data->offset = new_cal_data.offset;
cal_data->w_invert = new_cal_data.w_invert;
cal_data->bfield = new_cal_data.bfield;
if (m_cal_level < (cal_steps - 1))
m_cal_level++;
#ifdef DEBUG
printf("CompassCalibration: ready check success, caldata: %f %f %f %f %f %f %f %f "
"%f %f %f %f %f, err %f\n",
cal_data->offset[0][0],
cal_data->offset[1][0],
cal_data->offset[2][0],
cal_data->w_invert[0][0],
cal_data->w_invert[0][1],
cal_data->w_invert[0][2],
cal_data->w_invert[1][0],
cal_data->w_invert[1][1],
cal_data->w_invert[1][2],
cal_data->w_invert[2][0],
cal_data->w_invert[2][1],
cal_data->w_invert[2][2],
cal_data->bfield,
new_err);
#endif
}
}
}
resetSample(cal_data);
return m_cal_level;
}
double
MMC35240::calcSquareErr(compass_cal_t* data)
{
double err = 0;
mat<double, 3, 1> raw, result, mat_diff;
int i;
float stdev[3] = { 0, 0, 0 };
double diff;
for (i = 0; i < MAGN_DS_SIZE; i++) {
raw[0][0] = data->sample[i][0];
raw[1][0] = data->sample[i][1];
raw[2][0] = data->sample[i][2];
stdev[0] += (raw[0][0] - data->average[0]) * (raw[0][0] - data->average[0]);
stdev[1] += (raw[1][0] - data->average[1]) * (raw[1][0] - data->average[1]);
stdev[2] += (raw[2][0] - data->average[2]) * (raw[2][0] - data->average[2]);
mat_diff = raw - data->offset;
result = multiply(data->w_invert, mat_diff);
diff = sqrt(result[0][0] * result[0][0] + result[1][0] * result[1][0] +
result[2][0] * result[2][0]) -
data->bfield;
err += diff * diff;
}
stdev[0] = sqrt(stdev[0] / MAGN_DS_SIZE);
stdev[1] = sqrt(stdev[1] / MAGN_DS_SIZE);
stdev[2] = sqrt(stdev[2] / MAGN_DS_SIZE);
/* A sanity check - if we have too little variation for an axis it's best to reject the
* calibration than risking a wrong calibration */
if (stdev[0] <= 1 || stdev[1] <= 1 || stdev[2] <= 1)
return max_sqr_errs[0];
err /= MAGN_DS_SIZE;
return err;
}
int
MMC35240::ellipsoidFit(mat_input_t& m,
mat<double, 3, 1>& offset,
mat<double, 3, 3>& w_invert,
double* bfield)
{
int i;
mat<double, MAGN_DS_SIZE, 9> h;
mat<double, MAGN_DS_SIZE, 1> w;
mat<double, 9, MAGN_DS_SIZE> h_trans;
mat<double, 9, 9> p_temp1;
mat<double, 9, MAGN_DS_SIZE> p_temp2;
mat<double, 3, 3> temp1, temp;
mat<double, 3, 3> temp1_inv;
mat<double, 3, 1> temp2;
mat<double, 9, 9> result;
mat<double, 9, 1> p;
mat<double, 3, 3> a, sqrt_evals, evecs, evecs_trans;
mat<double, 3, 1> evec1, evec2, evec3;
for (i = 0; i < MAGN_DS_SIZE; i++) {
w[i][0] = m[i][0] * m[i][0];
h[i][0] = m[i][0];
h[i][1] = m[i][1];
h[i][2] = m[i][2];
h[i][3] = -1 * m[i][0] * m[i][1];
h[i][4] = -1 * m[i][0] * m[i][2];
h[i][5] = -1 * m[i][1] * m[i][2];
h[i][6] = -1 * m[i][1] * m[i][1];
h[i][7] = -1 * m[i][2] * m[i][2];
h[i][8] = 1;
}
h_trans = transpose(h);
result = multiply(h_trans, h);
p_temp1 = invert(result);
p_temp2 = multiply(p_temp1, h_trans);
p = multiply(p_temp2, w);
temp1[0][0] = 2;
temp1[0][1] = p[3][0];
temp1[0][2] = p[4][0];
temp1[1][0] = p[3][0];
temp1[1][1] = 2 * p[6][0];
temp1[1][2] = p[5][0];
temp1[2][0] = p[4][0];
temp1[2][1] = p[5][0];
temp1[2][2] = 2 * p[7][0];
temp2[0][0] = p[0][0];
temp2[1][0] = p[1][0];
temp2[2][0] = p[2][0];
temp1_inv = invert(temp1);
offset = multiply(temp1_inv, temp2);
double off_x = offset[0][0];
double off_y = offset[1][0];
double off_z = offset[2][0];
a[0][0] = 1.0 / (p[8][0] + off_x * off_x + p[6][0] * off_y * off_y + p[7][0] * off_z * off_z +
p[3][0] * off_x * off_y + p[4][0] * off_x * off_z + p[5][0] * off_y * off_z);
a[0][1] = p[3][0] * a[0][0] / 2;
a[0][2] = p[4][0] * a[0][0] / 2;
a[1][2] = p[5][0] * a[0][0] / 2;
a[1][1] = p[6][0] * a[0][0];
a[2][2] = p[7][0] * a[0][0];
a[2][1] = a[1][2];
a[1][0] = a[0][1];
a[2][0] = a[0][2];
double eig1 = 0, eig2 = 0, eig3 = 0;
computeEigenvalues(a, &eig1, &eig2, &eig3);
if (eig1 <= 0 || eig2 <= 0 || eig3 <= 0)
return 0;
sqrt_evals[0][0] = sqrt(eig1);
sqrt_evals[1][0] = 0;
sqrt_evals[2][0] = 0;
sqrt_evals[0][1] = 0;
sqrt_evals[1][1] = sqrt(eig2);
sqrt_evals[2][1] = 0;
sqrt_evals[0][2] = 0;
sqrt_evals[1][2] = 0;
sqrt_evals[2][2] = sqrt(eig3);
calcEvector(a, eig1, evec1);
calcEvector(a, eig2, evec2);
calcEvector(a, eig3, evec3);
evecs[0][0] = evec1[0][0];
evecs[1][0] = evec1[1][0];
evecs[2][0] = evec1[2][0];
evecs[0][1] = evec2[0][0];
evecs[1][1] = evec2[1][0];
evecs[2][1] = evec2[2][0];
evecs[0][2] = evec3[0][0];
evecs[1][2] = evec3[1][0];
evecs[2][2] = evec3[2][0];
temp1 = multiply(evecs, sqrt_evals);
evecs_trans = transpose(evecs);
temp = multiply(temp1, evecs_trans);
w_invert = transpose(temp);
*bfield = pow(sqrt(1 / eig1) * sqrt(1 / eig2) * sqrt(1 / eig3), 1.0 / 3.0);
if (*bfield < 0)
return 0;
w_invert = w_invert * (*bfield);
return 1;
}
/* Given an real symmetric 3x3 matrix A, compute the eigenvalues */
void
MMC35240::computeEigenvalues(mat<double, 3, 3>& A, double* eig1, double* eig2, double* eig3)
{
double p = A[0][1] * A[0][1] + A[0][2] * A[0][2] + A[1][2] * A[1][2];
if (p < EPSILON) {
*eig1 = A[0][0];
*eig2 = A[1][1];
*eig3 = A[2][2];
return;
}
double q = (A[0][0] + A[1][1] + A[2][2]) / 3;
double temp1 = A[0][0] - q;
double temp2 = A[1][1] - q;
double temp3 = A[2][2] - q;
p = temp1 * temp1 + temp2 * temp2 + temp3 * temp3 + 2 * p;
p = sqrt(p / 6);
mat<double, 3, 3> B = A;
B[0][0] -= q;
B[1][1] -= q;
B[2][2] -= q;
B = (1 / p) * B;
double r =
(B[0][0] * B[1][1] * B[2][2] + B[0][1] * B[1][2] * B[2][0] + B[0][2] * B[1][0] * B[2][1] -
B[0][2] * B[1][1] * B[2][0] - B[0][0] * B[1][2] * B[2][1] - B[0][1] * B[1][0] * B[2][2]) /
2;
double phi;
if (r <= -1.0)
phi = M_PI / 3;
else if (r >= 1.0)
phi = 0;
else
phi = acos(r) / 3;
*eig3 = q + 2 * p* cos(phi);
*eig1 = q + 2 * p* cos(phi + 2 * M_PI / 3);
*eig2 = 3 * q - *eig1 - *eig3;
}
void
MMC35240::calcEvector(mat<double, 3, 3>& A, double eig, mat<double, 3, 1>& vec)
{
mat<double, 3, 3> h;
mat<double, 2, 2> x_tmp;
h = A;
h[0][0] -= eig;
h[1][1] -= eig;
h[2][2] -= eig;
mat<double, 2, 2> x;
x[0][0] = h[1][1];
x[0][1] = h[1][2];
x[1][0] = h[2][1];
x[1][1] = h[2][2];
x_tmp = invert(x);
x = x_tmp;
double temp1 = x[0][0] * (-h[1][0]) + x[0][1] * (-h[2][0]);
double temp2 = x[1][0] * (-h[1][0]) + x[1][1] * (-h[2][0]);
double norm = sqrt(1 + temp1 * temp1 + temp2 * temp2);
vec[0][0] = 1.0 / norm;
vec[1][0] = temp1 / norm;
vec[2][0] = temp2 / norm;
}
void
MMC35240::scale(float* x, float* y, float* z)
{
float sqr_norm = 0;
float sanity_norm = 0;
float scale = 1;
sqr_norm = (*x * *x + *y * *y + *z * *z);
if (sqr_norm < MAGNETIC_LOW)
sanity_norm = MAGNETIC_LOW;
if (sanity_norm && sqr_norm) {
scale = sanity_norm / sqr_norm;
scale = sqrt(scale);
*x = *x* scale;
*y = *y* scale;
*z = *z* scale;
}
}
void
MMC35240::compassComputeCal(float* x, float* y, float* z, compass_cal_t* cal_data)
{
mat<double, 3, 1> result, raw, diff;
if (!m_cal_level)
return;
raw[0][0] = *x;
raw[1][0] = *y;
raw[2][0] = *z;
diff = raw - cal_data->offset;
result = multiply(cal_data->w_invert, diff);
*x = result[0][0];
*y = result[1][0];
*z = result[2][0];
scale(x, y, z);
}
void
MMC35240::denoise_average(float* x, float* y, float* z)
{
/*
* Smooth out incoming data using a moving average over a number of
* samples. We accumulate one second worth of samples, or max_samples,
* depending on which is lower.
*/
float* data[3];
int f;
int history_size;
int history_full = 0;
filter_average_t* filter;
data[0] = x;
data[1] = y;
data[2] = z;
/* Don't denoise anything if we have less than two samples per second */
if (m_sampling_frequency < 2)
return;
filter = (filter_average_t*) &m_filter;
if (!filter)
return;
/* Restrict window size to the min of sampling_rate and max_samples */
if (m_sampling_frequency > filter->max_samples)
history_size = filter->max_samples;
else
history_size = m_sampling_frequency;
/* Reset history if we're operating on an incorrect window size */
if (filter->history_size != history_size) {
filter->history_size = history_size;
filter->history_entries = 0;
filter->history_index = 0;
filter->history =
(float*) realloc(filter->history, filter->history_size * filter->num_fields * sizeof(float));
if (filter->history) {
filter->history_sum =
(float*) realloc(filter->history_sum, filter->num_fields * sizeof(float));
if (filter->history_sum)
memset(filter->history_sum, 0, filter->num_fields * sizeof(float));
}
}
if (!filter->history || !filter->history_sum)
return; /* Unlikely, but still... */
/* Update initialized samples count */
if (filter->history_entries < filter->history_size)
filter->history_entries++;
else
history_full = 1;
/* Record new sample and calculate the moving sum */
for (f = 0; f < filter->num_fields; f++) {
/** A field is going to be overwritten if history is full, so decrease the history sum */
if (history_full)
filter->history_sum[f] -=
filter->history[filter->history_index * filter->num_fields + f];
filter->history[filter->history_index * filter->num_fields + f] = *data[f];
filter->history_sum[f] += *data[f];
/* For now simply compute a mobile mean for each field and output filtered data */
*data[f] = filter->history_sum[f] / filter->history_entries;
}
/* Update our rolling index (next evicted cell) */
filter->history_index = (filter->history_index + 1) % filter->history_size;
}
Reference in New Issue View Git Blame Copy Permalink