1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/******************************************************************************* 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $ 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *******************************************************************************/ 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup COMPASSDL 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Motion Library - Compass Driver Layer. 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Provides the interface to setup and handle an compass 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * connected to either the primary or the seconday I2C interface 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of the gyroscope. 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file compass.c 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Compass setup and handling methods. 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ------------------ */ 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Include Files. - */ 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* ------------------ */ 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <stdlib.h> 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlinclude.h" 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpKey.h" 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc.h" 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "log.h" 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-compass" 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define COMPASS_DEBUG 0 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------------- */ 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Global Variables. - */ 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------------- */ 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------------- */ 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Static Variables. - */ 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------------- */ 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------- */ 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Prototypes. - */ 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* --------------- */ 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* -------------- */ 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Externs. - */ 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* -------------- */ 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* -------------- */ 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* - Functions. - */ 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* -------------- */ 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic float square(float data) 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return data * data; 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise) 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->num = 0; 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->index = 0; 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->noise = noise; 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->len = len; 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < adap_filter->len; ++i) { 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->sequence[i] = 0; 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int cmpfloat(const void *p1, const void *p2) 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return *(float*)p1 - *(float*)p2; 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in) 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN]; 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (adap_filter->len <= 1) { 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return in; 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (adap_filter->num < adap_filter->len) { 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->sequence[adap_filter->index++] = in; 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->num++; 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return in; 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (adap_filter->len <= adap_filter->index) { 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->index = 0; 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adap_filter->sequence[adap_filter->index++] = in; 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall avg = 0; 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < adap_filter->len; i++) { 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall avg += adap_filter->sequence[i]; 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall avg /= adap_filter->len; 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float)); 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat); 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall median = sorted[adap_filter->len/2]; 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum = 0; 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < adap_filter->len; i++) { 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum += square(avg - adap_filter->sequence[i]); 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sum /= adap_filter->len; 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sum <= adap_filter->noise) { 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return median; 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return ((in - avg) * (sum - adap_filter->noise) / sum + avg); 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold) 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall thresh_filter->threshold = threshold; 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall thresh_filter->last = 0; 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in) 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (in < thresh_filter->last - thresh_filter->threshold 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || thresh_filter->last + thresh_filter->threshold < in) { 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall thresh_filter->last = in; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return in; 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else { 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return thresh_filter->last; 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int init(yas_filter_handle_t *t) 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float noise[] = { 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall YAS_DEFAULT_FILTER_NOISE, 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall YAS_DEFAULT_FILTER_NOISE, 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall YAS_DEFAULT_FILTER_NOISE, 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (t == NULL) { 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return -1; 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]); 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH); 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int update(yas_filter_handle_t *t, float *input, float *output) 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (t == NULL || input == NULL || output == NULL) { 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return -1; 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]); 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]); 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallint yas_filter_init(yas_filter_if_s *f) 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (f == NULL) { 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return -1; 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall f->init = init; 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall f->update = update; 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Used to determine if a compass is 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * configured and used by the MPL. 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if the compass is present. 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char inv_compass_present(void) 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (NULL != mldl_cfg->compass && 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall NULL != mldl_cfg->compass->resume && 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS) 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return TRUE; 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return FALSE; 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Query the compass slave address. 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return The 7-bit compass slave address. 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned char inv_get_compass_slave_addr(void) 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (NULL != mldl_cfg->pdata) 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return mldl_cfg->pdata->compass.address; 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return 0; 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get the ID of the compass in use. 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return ID of the compass in use. 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallunsigned short inv_get_compass_id(void) 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (NULL != mldl_cfg->compass) { 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return mldl_cfg->compass->id; 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return ID_INVALID; 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Get a sample of compass data from the device. 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param data 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the buffer to store the compass raw data for 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * X, Y, and Z axes. 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or a non-zero error code. 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_get_compass_data(long *data) 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int ii; 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char *tmp = inv_obj.compass_raw_data; 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) { 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_CONFIGURATION; 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY || 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*--- read the compass sensor data. 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall The compass read function may return an INV_ERROR_COMPASS_* errors 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall when the data is not ready (read/refresh frequency mismatch) or 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall the internal data sampling timing of the device was not respected. 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Returning the error code will make the sensor fusion supervisor 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ignore this compass data sample. ---*/ 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp); 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (COMPASS_DEBUG) { 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("inv_mpu_read_compass returned %d\n", result); 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (ii = 0; ii < 3; ii++) { 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian) 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii] = 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall else 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[ii] = 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_overunder = (int)tmp[6]; 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_external_sensor_data(data, 3); 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static unsigned char first = TRUE; 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // one-off write to AKM 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (first) { 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[] = { 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // beginning Mantis register for one-off slave R/W 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_I2C_SLV4_ADDR, 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // the slave to write to 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mldl_cfg->pdata->compass.address, 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // the register to write to 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*mldl_cfg->compass->trigger->reg */ 0x0A, 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // the value to write 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /*mldl_cfg->compass->trigger->value */ 0x01, 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // enable the write 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 0xC0 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr, 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ARRAY_SIZE(regs), regs); 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall first = FALSE; 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[] = { 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPUREG_I2C_SLV4_CTRL, 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 0xC0 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr, 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ARRAY_SIZE(regs), regs); 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#else 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_INVALID_CONFIGURATION; 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif // CONFIG_MPU_SENSORS_xxxx 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]); 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]); 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]); 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Sets the compass bias. 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param bias 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Compass bias, length 3. Scale is micro Tesla's * 2^16. 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Frame is mount frame which may be different from body frame. 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_compass_bias(long *bias) 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long biasC[3]; 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldlCfg = inv_get_dl_config(); 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias[0] = bias[0]; 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias[1] = bias[1]; 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias[2] = bias[2]; 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasC[0] = 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) + 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[0] * (1L << 16); 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasC[1] = 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) + 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[1] * (1L << 16); 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasC[2] = 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) + 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[2] * (1L << 16); 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) { 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char reg[4]; 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long biasB[3]; 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall signed char *orC = mldlCfg->pdata->compass.orientation; 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Now transform to body frame 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2]; 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5]; 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8]; 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4, 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_int32_to_big8(biasB[0], reg)); 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4, 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_int32_to_big8(biasB[1], reg)); 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4, 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_int32_to_big8(biasB[2], reg)); 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Write a single register on the compass slave device, regardless 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of the bus it is connected to and the MPU's configuration. 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param reg 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the register to write to on the slave compass device. 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param val 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the value to write. 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val) 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char data[2]; 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = reg; 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = val; 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_WRITE_REGISTERS; 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = 2; 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = TRUE; 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = data; 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_config_compass(inv_get_dl_config(), 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Read values from the compass slave device registers, regardless 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of the bus it is connected to and the MPU's configuration. 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param reg 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the register to read from on the slave compass device. 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param val 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a buffer of 3 elements to store the values read from the 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * compass device. 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val) 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char data[2]; 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = reg; 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_READ_REGISTERS; 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = 2; 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = TRUE; 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = data; 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_get_compass_config(inv_get_dl_config(), 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *val = data[1]; 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Read values from the compass slave device scale registers, regardless 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * of the bus it is connected to and the MPU's configuration. 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param reg 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * the register to read from on the slave compass device. 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param val 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * a buffer of 3 elements to store the values read from the 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * compass device. 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_compass_read_scale(long *val) 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char data[3]; 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_READ_SCALE; 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = 3; 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = TRUE; 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = data; 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_get_compass_config(inv_get_dl_config(), 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall val[0] = ((data[0] - 128) << 22) + (1L << 30); 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall val[1] = ((data[1] - 128) << 22) + (1L << 30); 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall val[2] = ((data[2] - 128) << 22) + (1L << 30); 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_compass_offset(void) 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char data[3]; 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_OFFSET_VALS; 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = 3; 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = TRUE; 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = data; 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) { 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* push stored values */ 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[0] = (char)inv_obj.compass_offsets[0]; 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[1] = (char)inv_obj.compass_offsets[1]; 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall data[2] = (char)inv_obj.compass_offsets[2]; 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]); 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_config_compass(inv_get_dl_config(), 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* compute new values and store them */ 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_get_compass_config(inv_get_dl_config(), 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]); 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(result == INV_SUCCESS) { 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1; 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_offsets[0] = data[0]; 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_offsets[1] = data[1]; 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_offsets[2] = data[2]; 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_compass_check_range(void) 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct ext_slave_config config; 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char data[3]; 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.key = MPU_SLAVE_RANGE_CHECK; 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.len = 3; 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.apply = TRUE; 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall config.data = data; 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_mpu_get_compass_config(inv_get_dl_config(), 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_get_serial_handle(), &config); 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if(data[0] || data[1] || data[2]) { 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* some value clipped */ 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR_COMPASS_DATA_ERROR; 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @} 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 580