mag3110地磁传感器算法

/************************************************************************************************** * Filename: sensor.c * Revised: $Date: 2015-09-21 15:30:38 +0200 (fr, 21 sep 2015) $ * Revision: $Revision: 31581 $ * * Description: Sensor driver shared code. * * Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com/ * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of Texas Instruments Incorporated nor the names of * its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. **************************************************************************************************/ /* ------------------------------------------------------------------------------------------------ * Includes * ------------------------------------------------------------------------------------------------ */ #include <stdio.h> #include <stdint.h> #include <string.h> #include "twi_master.h" #include "app_timer.h" #include "AskCrc.h" #include "mag3110.h" #include "m24c64.h" #include "nrf_log.h" #include "nrf_delay.h" #if NRF_MODULE_ENABLED(BLE_STANDBY) #include "dev_config.h" #endif /* ----------------------------------------------------------------------------- * Macros and constants * ------------------------------------------------------------------------------ */ /* ----------------------------------------------------------------------------- * Local Variables * ------------------------------------------------------------------------------ */ APP_TIMER_DEF(mag3110_timer); static volatile uint32_t mag3110_event = 0; static uint8_t mag3110_address = MAG3110_IIC_ADDRESS; static int32_t x,y,z; static uint8_t per_second = 5; static sensor_infor_t my_sensor = {0, UNINIT, 0, 0}; static mag3110_state_t mag3110_state = SENSOR_UNINIT; static sensor_config_t my_config = {0,}; static sample_data_t temp_data; static uint32_t peak_sample_cnt, peak_in, peak_out; extern void restart_mag3110_timer(bool hi_speed); static bool mag3110_read_xyz(int16_t *x, int16_t *y, int16_t *z); /******************************************************************************* * @fn sensorReadReg * * @brief This function implements the I2C protocol to read from a sensor. * The sensor must be selected before this routine is called. * * @param addr - which register to read * @param pBuf - pointer to buffer to place data * @param nBytes - numbver of bytes to read * * @return TRUE if the required number of bytes are received ******************************************************************************/ static bool sensor_read_reg(uint8_t addr, uint8_t *pBuf, uint8_t nBytes) { bool transfer_succeeded; transfer_succeeded = twi_master_transfer(mag3110_address, &addr, 1, TWI_DONT_ISSUE_STOP); transfer_succeeded &= twi_master_transfer(mag3110_address|TWI_READ_BIT, pBuf, nBytes, TWI_ISSUE_STOP); return transfer_succeeded; } /******************************************************************************* * @fn sensorWriteReg * @brief This function implements the I2C protocol to write to a sensor. * The sensor must be selected before this routine is called. * * @param addr - which register to write * @param pBuf - pointer to buffer containing data to be written * @param nBytes - number of bytes to write * * @return TRUE if successful write */ static bool sensor_write_reg(uint8_t addr, uint8_t *pBuf, uint8_t nBytes) { uint8_t w2_data[33]; if (nBytes > 32) { return false; } w2_data[0] = addr; memcpy(&w2_data[1], pBuf, nBytes); return twi_master_transfer(mag3110_address, w2_data, nBytes + 1, TWI_ISSUE_STOP); } /******************************************************************************* * @fn sensorTestExecute * * @brief Run a self-test on all the sensors * * @param none * * @return bitmask of passed flags, one bit set for each sensor */ static bool mag3110_test(void) { byte val = 0; sensor_read_reg(WHO_AM_I_REG, &val, 1); if (val != MAG3110Q_ID) { return false; } return true; } /*********************************************************\ * Put MAG3110Q into Active Mode \*********************************************************/ static void mag3110_active(void) { byte val; sensor_read_reg(CTRL_REG1, &val, 1); val = (val & 0XFC | ACTIVE_MASK); sensor_write_reg(CTRL_REG1, &val, 1); } /*********************************************************\ * Put MAG3110Q into Standby Mode \*********************************************************/ static void mag3110_standby (void) { byte val; sensor_read_reg(CTRL_REG1, &val, 1); val = (val & 0XFC | STANDBY_MASK); sensor_write_reg(CTRL_REG1, &val, 1); } /********************************************************************* * @fn mag3110_init * * @brief mag3110初始化 * * @param None. * * @return None. */ bool mag3110_init(bool hi_speed) { bool success; byte val = 0, val1 = 0; static bool is_hi_speed = false; if (is_hi_speed == hi_speed) { return false; } else { is_hi_speed = hi_speed; } mag3110_standby(); if (!mag3110_test()) { my_sensor.sensor_state = ERROR; return false; } //必须等待上次采样完成后才能设置采样周期 if (hi_speed == true) { nrf_delay_ms(200); val = DATA_RATE_100MS; } else { nrf_delay_ms(100); val = DATA_RATE_200MS; } if (val == DATA_RATE_100MS) { per_second = 1000 / (100 * SAMPLE_WINDOW); } else if (val == DATA_RATE_200MS) { per_second = 1000 / (200 * SAMPLE_WINDOW); } success = sensor_write_reg(CTRL_REG1, &val, 1); sensor_read_reg(CTRL_REG1, &val1, 1); #if NRF_MODULE_ENABLED(BLE_STANDBY) ble_printf("write %02x, read %02x\r\n", val, val1); #endif val = AUTO_MRST_EN_MASK; success = sensor_write_reg(CTRL_REG2, &val, 1); mag3110_active(); restart_mag3110_timer(hi_speed); return success; } /********************************************************************* * @fn mag3110_read_xyz * * @brief 读取xyz三轴的值 * * @param None. * * @return None. */ static bool mag