summaryrefslogtreecommitdiffstats
path: root/libsensors/mlsdk/mllite/compass.c
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors/mlsdk/mllite/compass.c')
-rw-r--r--libsensors/mlsdk/mllite/compass.c538
1 files changed, 538 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/compass.c b/libsensors/mlsdk/mllite/compass.c
new file mode 100644
index 0000000..c008fbf
--- /dev/null
+++ b/libsensors/mlsdk/mllite/compass.c
@@ -0,0 +1,538 @@
+/*
+ $License:
+ Copyright 2011 InvenSense, Inc.
+
+ 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.
+ $
+ */
+/*******************************************************************************
+ *
+ * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $
+ *
+ *******************************************************************************/
+
+/**
+ * @defgroup COMPASSDL
+ * @brief Motion Library - Compass Driver Layer.
+ * Provides the interface to setup and handle an compass
+ * connected to either the primary or the seconday I2C interface
+ * of the gyroscope.
+ *
+ * @{
+ * @file compass.c
+ * @brief Compass setup and handling methods.
+ */
+
+/* ------------------ */
+/* - Include Files. - */
+/* ------------------ */
+
+#include <string.h>
+#include <stdlib.h>
+#include "compass.h"
+
+#include "ml.h"
+#include "mlinclude.h"
+#include "dmpKey.h"
+#include "mlFIFO.h"
+#include "mldl.h"
+#include "mldl_cfg.h"
+#include "mlMathFunc.h"
+#include "mlsl.h"
+#include "mlos.h"
+
+#include "log.h"
+#undef MPL_LOG_TAG
+#define MPL_LOG_TAG "MPL-compass"
+
+#define COMPASS_DEBUG 0
+
+/* --------------------- */
+/* - Global Variables. - */
+/* --------------------- */
+
+/* --------------------- */
+/* - Static Variables. - */
+/* --------------------- */
+
+/* --------------- */
+/* - Prototypes. - */
+/* --------------- */
+
+/* -------------- */
+/* - Externs. - */
+/* -------------- */
+
+/* -------------- */
+/* - Functions. - */
+/* -------------- */
+
+static float square(float data)
+{
+ return data * data;
+}
+
+static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise)
+{
+ int i;
+
+ adap_filter->num = 0;
+ adap_filter->index = 0;
+ adap_filter->noise = noise;
+ adap_filter->len = len;
+
+ for (i = 0; i < adap_filter->len; ++i) {
+ adap_filter->sequence[i] = 0;
+ }
+}
+
+static int cmpfloat(const void *p1, const void *p2)
+{
+ return *(float*)p1 - *(float*)p2;
+}
+
+
+static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in)
+{
+ float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN];
+ int i;
+
+ if (adap_filter->len <= 1) {
+ return in;
+ }
+ if (adap_filter->num < adap_filter->len) {
+ adap_filter->sequence[adap_filter->index++] = in;
+ adap_filter->num++;
+ return in;
+ }
+ if (adap_filter->len <= adap_filter->index) {
+ adap_filter->index = 0;
+ }
+ adap_filter->sequence[adap_filter->index++] = in;
+
+ avg = 0;
+ for (i = 0; i < adap_filter->len; i++) {
+ avg += adap_filter->sequence[i];
+ }
+ avg /= adap_filter->len;
+
+ memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float));
+ qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat);
+ median = sorted[adap_filter->len/2];
+
+ sum = 0;
+ for (i = 0; i < adap_filter->len; i++) {
+ sum += square(avg - adap_filter->sequence[i]);
+ }
+ sum /= adap_filter->len;
+
+ if (sum <= adap_filter->noise) {
+ return median;
+ }
+
+ return ((in - avg) * (sum - adap_filter->noise) / sum + avg);
+}
+
+static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold)
+{
+ thresh_filter->threshold = threshold;
+ thresh_filter->last = 0;
+}
+
+static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in)
+{
+ if (in < thresh_filter->last - thresh_filter->threshold
+ || thresh_filter->last + thresh_filter->threshold < in) {
+ thresh_filter->last = in;
+ return in;
+ }
+ else {
+ return thresh_filter->last;
+ }
+}
+
+static int init(yas_filter_handle_t *t)
+{
+ float noise[] = {
+ YAS_DEFAULT_FILTER_NOISE,
+ YAS_DEFAULT_FILTER_NOISE,
+ YAS_DEFAULT_FILTER_NOISE,
+ };
+ int i;
+
+ if (t == NULL) {
+ return -1;
+ }
+
+ for (i = 0; i < 3; i++) {
+ adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]);
+ thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH);
+ }
+
+ return 0;
+}
+
+static int update(yas_filter_handle_t *t, float *input, float *output)
+{
+ int i;
+
+ if (t == NULL || input == NULL || output == NULL) {
+ return -1;
+ }
+
+ for (i = 0; i < 3; i++) {
+ output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]);
+ output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]);
+ }
+
+ return 0;
+}
+
+int yas_filter_init(yas_filter_if_s *f)
+{
+ if (f == NULL) {
+ return -1;
+ }
+ f->init = init;
+ f->update = update;
+
+ return 0;
+}
+
+/**
+ * @brief Used to determine if a compass is
+ * configured and used by the MPL.
+ * @return INV_SUCCESS if the compass is present.
+ */
+unsigned char inv_compass_present(void)
+{
+ INVENSENSE_FUNC_START;
+ struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+ if (NULL != mldl_cfg->compass &&
+ NULL != mldl_cfg->compass->resume &&
+ mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
+ return TRUE;
+ else
+ return FALSE;
+}
+
+/**
+ * @brief Query the compass slave address.
+ * @return The 7-bit compass slave address.
+ */
+unsigned char inv_get_compass_slave_addr(void)
+{
+ INVENSENSE_FUNC_START;
+ struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+ if (NULL != mldl_cfg->pdata)
+ return mldl_cfg->pdata->compass.address;
+ else
+ return 0;
+}
+
+/**
+ * @brief Get the ID of the compass in use.
+ * @return ID of the compass in use.
+ */
+unsigned short inv_get_compass_id(void)
+{
+ INVENSENSE_FUNC_START;
+ struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+ if (NULL != mldl_cfg->compass) {
+ return mldl_cfg->compass->id;
+ }
+ return ID_INVALID;
+}
+
+/**
+ * @brief Get a sample of compass data from the device.
+ * @param data
+ * the buffer to store the compass raw data for
+ * X, Y, and Z axes.
+ * @return INV_SUCCESS or a non-zero error code.
+ */
+inv_error_t inv_get_compass_data(long *data)
+{
+ inv_error_t result;
+ int ii;
+ struct mldl_cfg *mldl_cfg = inv_get_dl_config();
+
+ unsigned char *tmp = inv_obj.compass_raw_data;
+
+ if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) {
+ LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
+ return INV_ERROR_INVALID_CONFIGURATION;
+ }
+
+ if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY ||
+ !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
+ /*--- read the compass sensor data.
+ The compass read function may return an INV_ERROR_COMPASS_* errors
+ when the data is not ready (read/refresh frequency mismatch) or
+ the internal data sampling timing of the device was not respected.
+ Returning the error code will make the sensor fusion supervisor
+ ignore this compass data sample. ---*/
+ result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
+ inv_get_serial_handle(),
+ inv_get_serial_handle(),
+ tmp);
+ if (result) {
+ if (COMPASS_DEBUG) {
+ MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
+ }
+ return result;
+ }
+ for (ii = 0; ii < 3; ii++) {
+ if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian)
+ data[ii] =
+ ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
+ else
+ data[ii] =
+ ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
+ }
+
+ inv_obj.compass_overunder = (int)tmp[6];
+
+ } else {
+ return INV_ERROR_INVALID_CONFIGURATION;
+ }
+ data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]);
+ data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]);
+ data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]);
+
+ return INV_SUCCESS;
+}
+
+/**
+ * @brief Sets the compass bias.
+ * @param bias
+ * Compass bias, length 3. Scale is micro Tesla's * 2^16.
+ * Frame is mount frame which may be different from body frame.
+ * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_set_compass_bias(long *bias)
+{
+ inv_error_t result = INV_SUCCESS;
+ long biasC[3];
+ struct mldl_cfg *mldlCfg = inv_get_dl_config();
+
+ inv_obj.compass_bias[0] = bias[0];
+ inv_obj.compass_bias[1] = bias[1];
+ inv_obj.compass_bias[2] = bias[2];
+
+ // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
+ biasC[0] =
+ (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
+ inv_obj.init_compass_bias[0] * (1L << 16);
+ biasC[1] =
+ (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
+ inv_obj.init_compass_bias[1] * (1L << 16);
+ biasC[2] =
+ (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
+ inv_obj.init_compass_bias[2] * (1L << 16);
+
+ if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
+ unsigned char reg[4];
+ long biasB[3];
+ signed char *orC = mldlCfg->pdata->compass.orientation;
+
+ // Now transform to body frame
+ biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
+ biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
+ biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
+
+ result =
+ inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
+ inv_int32_to_big8(biasB[0], reg));
+ result =
+ inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
+ inv_int32_to_big8(biasB[1], reg));
+ result =
+ inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
+ inv_int32_to_big8(biasB[2], reg));
+ }
+ return result;
+}
+
+/**
+ * @brief Write a single register on the compass slave device, regardless
+ * of the bus it is connected to and the MPU's configuration.
+ * @param reg
+ * the register to write to on the slave compass device.
+ * @param val
+ * the value to write.
+ * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
+{
+ struct ext_slave_config config;
+ unsigned char data[2];
+ inv_error_t result;
+
+ data[0] = reg;
+ data[1] = val;
+
+ config.key = MPU_SLAVE_WRITE_REGISTERS;
+ config.len = 2;
+ config.apply = TRUE;
+ config.data = data;
+
+ result = inv_mpu_config_compass(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ return result;
+}
+
+/**
+ * @brief Read values from the compass slave device registers, regardless
+ * of the bus it is connected to and the MPU's configuration.
+ * @param reg
+ * the register to read from on the slave compass device.
+ * @param val
+ * a buffer of 3 elements to store the values read from the
+ * compass device.
+ * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val)
+{
+ struct ext_slave_config config;
+ unsigned char data[2];
+ inv_error_t result;
+
+ data[0] = reg;
+
+ config.key = MPU_SLAVE_READ_REGISTERS;
+ config.len = 2;
+ config.apply = TRUE;
+ config.data = data;
+
+ result = inv_mpu_get_compass_config(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ *val = data[1];
+ return result;
+}
+
+/**
+ * @brief Read values from the compass slave device scale registers, regardless
+ * of the bus it is connected to and the MPU's configuration.
+ * @param reg
+ * the register to read from on the slave compass device.
+ * @param val
+ * a buffer of 3 elements to store the values read from the
+ * compass device.
+ * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
+ */
+inv_error_t inv_compass_read_scale(long *val)
+{
+ struct ext_slave_config config;
+ unsigned char data[3];
+ inv_error_t result;
+
+ config.key = MPU_SLAVE_READ_SCALE;
+ config.len = 3;
+ config.apply = TRUE;
+ config.data = data;
+
+ result = inv_mpu_get_compass_config(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+ val[0] = ((data[0] - 128) << 22) + (1L << 30);
+ val[1] = ((data[1] - 128) << 22) + (1L << 30);
+ val[2] = ((data[2] - 128) << 22) + (1L << 30);
+ return result;
+}
+
+inv_error_t inv_set_compass_offset(void)
+{
+ struct ext_slave_config config;
+ unsigned char data[3];
+ inv_error_t result;
+
+ config.key = MPU_SLAVE_OFFSET_VALS;
+ config.len = 3;
+ config.apply = TRUE;
+ config.data = data;
+
+ if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
+ /* push stored values */
+ data[0] = (char)inv_obj.compass_offsets[0];
+ data[1] = (char)inv_obj.compass_offsets[1];
+ data[2] = (char)inv_obj.compass_offsets[2];
+ MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
+ result = inv_mpu_config_compass(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ } else {
+ /* compute new values and store them */
+ result = inv_mpu_get_compass_config(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
+ if(result == INV_SUCCESS) {
+ inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
+ inv_obj.compass_offsets[0] = data[0];
+ inv_obj.compass_offsets[1] = data[1];
+ inv_obj.compass_offsets[2] = data[2];
+ }
+ }
+
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ return result;
+}
+
+inv_error_t inv_compass_check_range(void)
+{
+ struct ext_slave_config config;
+ unsigned char data[3];
+ inv_error_t result;
+
+ config.key = MPU_SLAVE_RANGE_CHECK;
+ config.len = 3;
+ config.apply = TRUE;
+ config.data = data;
+
+ result = inv_mpu_get_compass_config(inv_get_dl_config(),
+ inv_get_serial_handle(),
+ inv_get_serial_handle(), &config);
+ if (result) {
+ LOG_RESULT_LOCATION(result);
+ return result;
+ }
+
+ if(data[0] || data[1] || data[2]) {
+ /* some value clipped */
+ return INV_ERROR_COMPASS_DATA_ERROR;
+ }
+ return INV_SUCCESS;
+}
+
+/**
+ * @}
+ */
generated by cgit v1.1 at 2024-04-30 15:06:35 +0000