diff options
Diffstat (limited to 'examples/imx7d_sdb_m4/demo_apps/sensor_demo/common/fxos8700.c')
-rw-r--r-- | examples/imx7d_sdb_m4/demo_apps/sensor_demo/common/fxos8700.c | 198 |
1 files changed, 120 insertions, 78 deletions
diff --git a/examples/imx7d_sdb_m4/demo_apps/sensor_demo/common/fxos8700.c b/examples/imx7d_sdb_m4/demo_apps/sensor_demo/common/fxos8700.c index 207c594..a2130fe 100644 --- a/examples/imx7d_sdb_m4/demo_apps/sensor_demo/common/fxos8700.c +++ b/examples/imx7d_sdb_m4/demo_apps/sensor_demo/common/fxos8700.c @@ -36,88 +36,75 @@ /*FUNCTION**************************************************************** * -* Function Name : fxos8700_init +* Function Name : FXOS_Init * Returned Value : result * Comments : Initialize FXOS8700 Acc and Mag sensor. * *END*********************************************************************/ -bool fxos8700_init(void) +bool FXOS_Init(fxos_handle_t *handle, const fxos_init_t *fxos_config) { - uint8_t txBuffer; - uint8_t cmdBuffer[2]; + // Place FXOS8700 into standby + if (!FXOS_WriteReg(handle, FXOS8700_CTRL_REG1, 0x00)) + return false; + + // Set oversampling to reduce magnetometer noise + // Select hybrid mode with accel and magnetometer active + if (!FXOS_WriteReg(handle, FXOS8700_M_CTRL_REG1, (uint8_t)fxos_config->osr | (uint8_t)fxos_config->hms)) + return false; - // write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS8700 into standby - // [7-1] = 0000 000 - // [0]: active=0 - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; - cmdBuffer[1] = FXOS8700_CTRL_REG1; - txBuffer = 0x00; - if (!I2C_XFER_SendDataBlocking(cmdBuffer, 2, &txBuffer, 1)) + // Map the magnetometer registers to follow the accelerometer registers + if (!FXOS_WriteReg(handle, FXOS8700_M_CTRL_REG2, 0x20)) return false; - // write 0001 1111 = 0x1F to M_CTRL_REG1 - // [7]: m_acal=0: auto calibration disabled - // [6]: m_rst=0: one-shot magnetic reset disabled - // [5]: m_ost=0: one-shot magnetic measurement disabled - // [4-2]: m_os=111=7: 8x oversampling (for 200Hz) to reduce magnetometer noise - // [1-0]: m_hms=11=3: select hybrid mode with accel and magnetometer active - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; - cmdBuffer[1] = FXOS8700_M_CTRL_REG1; - txBuffer = 0x1F; - if (!I2C_XFER_SendDataBlocking(cmdBuffer, 2, &txBuffer, 1)) + // Set accelerometer range(2G, 4G or 8G) + if (!FXOS_WriteReg(handle, FXOS8700_XYZ_DATA_CFG, (uint8_t)fxos_config->range)) return false; - // write 0010 0000 = 0x20 to magnetometer control register 2 - // [7]: reserved - // [6]: reserved - // [5]: hyb_autoinc_mode=1 to map the magnetometer registers to follow the accelerometer registers - // [4]: m_maxmin_dis=0 to retain default min/max latching even though not used - // [3]: m_maxmin_dis_ths=0 - // [2]: m_maxmin_rst=0 - // [1-0]: m_rst_cnt=00 to enable magnetic reset each cycle - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; - cmdBuffer[1] = FXOS8700_M_CTRL_REG2; - txBuffer = 0x20; - if (!I2C_XFER_SendDataBlocking(cmdBuffer, 2, &txBuffer, 1)) + // Setting for high resolution (maximum over sampling) + if (!FXOS_WriteReg(handle, FXOS8700_CTRL_REG2, 0x02)) return false; - // write 0000 0001= 0x01 to XYZ_DATA_CFG register - // [7]: reserved - // [6]: reserved - // [5]: reserved - // [4]: hpf_out=0 - // [3]: reserved - // [2]: reserved - // [1-0]: fs=01 for 4g mode: 2048 counts / g = 8192 counts / g after 2 bit left shift - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; - cmdBuffer[1] = FXOS8700_XYZ_DATA_CFG; - txBuffer = 0x01; - if (!I2C_XFER_SendDataBlocking(cmdBuffer, 2, &txBuffer, 1)) + // Set the data rate. + if (!FXOS_WriteReg(handle, FXOS8700_CTRL_REG1, ((uint8_t)fxos_config->dataRate))) return false; - // write 0000 0010 = 0x02 to CTRL_REG2 to set MODS bits - // [7]: st=0: self test disabled - // [6]: rst=0: reset disabled - // [5]: unused - // [4-3]: smods=00 - // [2]: slpe=0: auto sleep disabled - // [1-0]: mods=10 for high resolution (maximum over sampling) - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; - cmdBuffer[1] = FXOS8700_CTRL_REG2; - txBuffer = 0x02; - if (!I2C_XFER_SendDataBlocking(cmdBuffer, 2, &txBuffer, 1)) + return true; +} + +/*FUNCTION********************************************************************** + * + * Function Name : FXOS_Deinit + * Description : Disable the FXOS8700 and reset register content. + * + *END**************************************************************************/ +bool FXOS_Deinit(fxos_handle_t *handle) +{ + // Write 0000 0000 = 0x00 to CTRL_REG1 to place FXOS8700 into standby. + if (!FXOS_WriteReg(handle, FXOS8700_CTRL_REG1, 0x00)) + return false; + + // Reset Register content. + return FXOS_WriteReg(handle, FXOS8700_CTRL_REG2, 0x40); +} + +/*FUNCTION**************************************************************** +* +* Function Name : FXOS_Enable +* Returned Value : result +* Comments : Enable the FXOS8700 sensor. +* +*END*********************************************************************/ +bool FXOS_Enable(fxos_handle_t *handle) +{ + uint8_t regVal; + + if (!FXOS_ReadReg(handle, FXOS8700_CTRL_REG1, ®Val)) return false; - // write 0000 1101 = 0x0D to accelerometer control register 1 - // [7-6]: aslp_rate=00 - // [5-3]: dr=001=1 for 200Hz data rate (when in hybrid mode) - // [2]: lnoise=1 for low noise mode (since we're in 4g mode) - // [1]: f_read=0 for normal 16 bit reads - // [0]: active=1 to take the part out of standby and enable sampling - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; - cmdBuffer[1] = FXOS8700_CTRL_REG1; - txBuffer = 0x0D; - if (!I2C_XFER_SendDataBlocking(cmdBuffer, 2, &txBuffer, 1)) + /* Active FXOS8700 and Enable Reduced noise mode */ + regVal |= 0x05; + + if (!FXOS_WriteReg(handle, FXOS8700_CTRL_REG1, regVal)) return false; return true; @@ -125,30 +112,85 @@ bool fxos8700_init(void) /*FUNCTION**************************************************************** * -* Function Name : fxos8700_read_data +* Function Name : FXOS_Disable +* Returned Value : result +* Comments : Disable the FXOS8700 sensor. +* +*END*********************************************************************/ +bool FXOS_Disable(fxos_handle_t *handle) +{ + uint8_t regVal; + + if (!FXOS_ReadReg(handle, FXOS8700_CTRL_REG1, ®Val)) + return false; + + /* De-active FXOS8700 & Disable Reduced noise mode*/ + regVal &= ~0x05; + + if (!FXOS_WriteReg(handle, FXOS8700_CTRL_REG1, regVal)) + return false; + + return true; +} + +/*FUNCTION********************************************************************** + * + * Function Name : FXOS_WriteReg + * Description : Write the specified register of fxos8700. + * The writing process is through I2C. + *END**************************************************************************/ +bool FXOS_WriteReg(fxos_handle_t *handle, uint8_t regAddr, uint8_t regVal) +{ + uint8_t cmdBuffer[2]; + + cmdBuffer[0] = handle->address << 1; + cmdBuffer[1] = regAddr; + + return I2C_XFER_SendDataBlocking(handle->device, cmdBuffer, 2, ®Val, 1); +} + +/*FUNCTION********************************************************************** + * + * Function Name : FXOS_ReadReg + * Description : Read the specified register value of fxos8700. + * The reading process is through I2C. + *END**************************************************************************/ +bool FXOS_ReadReg(fxos_handle_t *handle, uint8_t regAddr, uint8_t *regValPtr) +{ + uint8_t cmdBuffer[3]; + + cmdBuffer[0] = handle->address << 1; + cmdBuffer[1] = regAddr; + cmdBuffer[2] = (handle->address << 1) + 1; + + return I2C_XFER_ReceiveDataBlocking(handle->device, cmdBuffer, 3, regValPtr, 1); +} + +/*FUNCTION**************************************************************** +* +* Function Name : FXOS_ReadData * Returned Value : result * Comments : Get current Acc and Mag from FXOS8700 6-axis sensor. * *END*********************************************************************/ -bool fxos8700_read_data(int16_t *Ax, int16_t *Ay, int16_t *Az, - int16_t *Mx, int16_t *My, int16_t *Mz) +bool FXOS_ReadData(fxos_handle_t *handle, fxos_data_t *val) { uint8_t rxBuffer[12]; uint8_t cmdBuffer[3]; // Fetch Current Acc and Mag in all Axis - cmdBuffer[0] = BOARD_I2C_FXOS8700_ADDR << 1; + cmdBuffer[0] = handle->address << 1; cmdBuffer[1] = FXOS8700_OUT_X_MSB; - cmdBuffer[2] = (BOARD_I2C_FXOS8700_ADDR << 1) + 1; - if (!I2C_XFER_ReceiveDataBlocking(cmdBuffer, 3, rxBuffer, 12)) + cmdBuffer[2] = (handle->address << 1) + 1; + if (!I2C_XFER_ReceiveDataBlocking(handle->device, cmdBuffer, 3, rxBuffer, 12)) return false; - *Ax = (rxBuffer[2] << 8) | rxBuffer[3]; - *Ay = (rxBuffer[0] << 8) | rxBuffer[1]; - *Az = (rxBuffer[4] << 8) | rxBuffer[5]; - *Mx = (rxBuffer[8] << 8) | rxBuffer[9]; - *My = (rxBuffer[6] << 8) | rxBuffer[7]; - *Mz = (rxBuffer[10] << 8) | rxBuffer[11]; + val->accX = (rxBuffer[2] << 8) | rxBuffer[3]; + val->accY = (rxBuffer[0] << 8) | rxBuffer[1]; + val->accZ = (rxBuffer[4] << 8) | rxBuffer[5]; + val->magX = (rxBuffer[8] << 8) | rxBuffer[9]; + val->magY = (rxBuffer[6] << 8) | rxBuffer[7]; + val->magZ = (rxBuffer[10] << 8) | rxBuffer[11]; return true; } |