static uint8_t ao_bmx160_configured;
+static struct ao_bmm150_trim ao_bmm150_trim;
+
#define ao_bmx160_spi_get() ao_spi_get(AO_BMX160_SPI_BUS, AO_SPI_SPEED_8MHz)
#define ao_bmx160_spi_put() ao_spi_put(AO_BMX160_SPI_BUS)
static void
_ao_bmx160_cmd(uint8_t cmd)
{
+ int i;
_ao_bmx160_reg_write(BMX160_CMD, cmd);
- ao_delay(AO_MS_TO_TICKS(100));
-}
-
-static void
-_ao_bmx160_mag_setup(void)
-{
- _ao_bmx160_reg_write(BMX160_MAG_IF_0, 0x80);
+ for (i = 0; i < 50; i++) {
+ uint8_t cmd_read;
+ cmd_read = _ao_bmx160_reg_read(BMX160_CMD);
+ if (cmd_read != cmd)
+ break;
+ }
}
static void
_ao_bmm150_wait_manual();
}
-#if BMX160_TEST
static uint8_t
_ao_bmm150_reg_read(uint8_t addr)
{
_ao_bmx160_reg_write(BMX160_MAG_IF_1, addr);
_ao_bmm150_wait_manual();
- return _ao_bmx160_reg_read(BMX160_DATA_0);
+ uint8_t ret = _ao_bmx160_reg_read(BMX160_DATA_0);
+ return ret;
+}
+
+static uint16_t
+_ao_bmm150_reg_read2(uint8_t lo_addr, uint8_t hi_addr)
+{
+ uint8_t lo = _ao_bmm150_reg_read(lo_addr);
+ uint8_t hi = _ao_bmm150_reg_read(hi_addr);
+
+ return ((uint16_t) hi << 8) | lo;
+}
+
+/*
+ * The compensate functions are taken from the BMM150 sample
+ * driver which has the following copyright:
+ *
+ * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
+ *
+ * BSD-3-Clause
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ *
+ * 2. 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.
+ *
+ * 3. Neither the name of the copyright holder 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 HOLDER 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.
+ *
+ * @file bmm150.c
+ * @date 10/01/2020
+ * @version 1.0.3
+ *
+ */
+
+/*!
+ * @brief This internal API is used to obtain the compensated
+ * magnetometer X axis data(micro-tesla) in int16_t.
+ */
+static int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall)
+{
+ int16_t retval;
+ uint16_t process_comp_x0 = 0;
+ int32_t process_comp_x1;
+ uint16_t process_comp_x2;
+ int32_t process_comp_x3;
+ int32_t process_comp_x4;
+ int32_t process_comp_x5;
+ int32_t process_comp_x6;
+ int32_t process_comp_x7;
+ int32_t process_comp_x8;
+ int32_t process_comp_x9;
+ int32_t process_comp_x10;
+
+ /* Overflow condition check */
+ if (mag_data_x != BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL)
+ {
+ if (data_rhall != 0)
+ {
+ /* Availability of valid data*/
+ process_comp_x0 = data_rhall;
+ //printf("using data_rhall %d\n", data_rhall);
+ }
+ else if (ao_bmm150_trim.dig_xyz1 != 0)
+ {
+ process_comp_x0 = ao_bmm150_trim.dig_xyz1;
+ //printf("using trim value %d\n", process_comp_x0);
+ }
+ else
+ {
+ process_comp_x0 = 0;
+ //printf("no valid rhall\n");
+ }
+ if (process_comp_x0 != 0)
+ {
+ /* Processing compensation equations*/
+ process_comp_x1 = ((int32_t)ao_bmm150_trim.dig_xyz1) * 16384;
+ //printf("comp_x1 %d\n", process_comp_x1);
+ process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
+ //printf("comp_x2 %d\n", process_comp_x2);
+ retval = ((int16_t)process_comp_x2);
+ process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
+ //printf("comp_x3 %d\n", process_comp_x3);
+ process_comp_x4 = (((int32_t)ao_bmm150_trim.dig_xy2) * (process_comp_x3 / 128));
+ //printf("comp_x4 %d\n", process_comp_x4);
+ process_comp_x5 = (int32_t)(((int16_t)ao_bmm150_trim.dig_xy1) * 128);
+ //printf("comp_x5 %d\n", process_comp_x5);
+ process_comp_x6 = ((int32_t)retval) * process_comp_x5;
+ //printf("comp_x6 %d\n", process_comp_x6);
+ process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
+ //printf("comp_x7 %d\n", process_comp_x7);
+ process_comp_x8 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_x2) + ((int16_t)0xA0)));
+ //printf("comp_x8 %d\n", process_comp_x8);
+ process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
+ //printf("comp_x9 %d\n", process_comp_x9);
+ process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
+ //printf("comp_x10 %d\n", process_comp_x10);
+ retval = ((int16_t)(process_comp_x10 / 8192));
+ //printf("ret 1 %d\n", retval);
+ retval = (retval + (((int16_t)ao_bmm150_trim.dig_x1) * 8)) / 16;
+ //printf("final %d\n", retval);
+ }
+ else
+ {
+ retval = BMM150_OVERFLOW_OUTPUT;
+ }
+ }
+ else
+ {
+ /* Overflow condition */
+ retval = BMM150_OVERFLOW_OUTPUT;
+ }
+
+ return retval;
+}
+
+/*!
+ * @brief This internal API is used to obtain the compensated
+ * magnetometer Y axis data(micro-tesla) in int16_t.
+ */
+static int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall)
+{
+ int16_t retval;
+ uint16_t process_comp_y0 = 0;
+ int32_t process_comp_y1;
+ uint16_t process_comp_y2;
+ int32_t process_comp_y3;
+ int32_t process_comp_y4;
+ int32_t process_comp_y5;
+ int32_t process_comp_y6;
+ int32_t process_comp_y7;
+ int32_t process_comp_y8;
+ int32_t process_comp_y9;
+
+ /* Overflow condition check */
+ if (mag_data_y != BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL)
+ {
+ if (data_rhall != 0)
+ {
+ /* Availability of valid data*/
+ process_comp_y0 = data_rhall;
+ }
+ else if (ao_bmm150_trim.dig_xyz1 != 0)
+ {
+ process_comp_y0 = ao_bmm150_trim.dig_xyz1;
+ }
+ else
+ {
+ process_comp_y0 = 0;
+ }
+ if (process_comp_y0 != 0)
+ {
+ /*Processing compensation equations*/
+ process_comp_y1 = (((int32_t)ao_bmm150_trim.dig_xyz1) * 16384) / process_comp_y0;
+ process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
+ retval = ((int16_t)process_comp_y2);
+ process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
+ process_comp_y4 = ((int32_t)ao_bmm150_trim.dig_xy2) * (process_comp_y3 / 128);
+ process_comp_y5 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_xy1) * 128));
+ process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
+ process_comp_y7 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_y2) + ((int16_t)0xA0)));
+ process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
+ process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
+ retval = (int16_t)(process_comp_y9 / 8192);
+ retval = (retval + (((int16_t)ao_bmm150_trim.dig_y1) * 8)) / 16;
+ }
+ else
+ {
+ retval = BMM150_OVERFLOW_OUTPUT;
+ }
+ }
+ else
+ {
+ /* Overflow condition*/
+ retval = BMM150_OVERFLOW_OUTPUT;
+ }
+
+ return retval;
+}
+
+/*!
+ * @brief This internal API is used to obtain the compensated
+ * magnetometer Z axis data(micro-tesla) in int16_t.
+ */
+static int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall)
+{
+ int32_t retval;
+ int16_t process_comp_z0;
+ int32_t process_comp_z1;
+ int32_t process_comp_z2;
+ int32_t process_comp_z3;
+ int16_t process_comp_z4;
+
+ if (mag_data_z != BMM150_ZAXIS_HALL_OVERFLOW_ADCVAL)
+ {
+ if ((ao_bmm150_trim.dig_z2 != 0) && (ao_bmm150_trim.dig_z1 != 0) && (data_rhall != 0) &&
+ (ao_bmm150_trim.dig_xyz1 != 0))
+ {
+ /*Processing compensation equations*/
+ process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) ao_bmm150_trim.dig_xyz1);
+ process_comp_z1 = (((int32_t)ao_bmm150_trim.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
+ process_comp_z2 = (((int32_t)(mag_data_z - ao_bmm150_trim.dig_z4)) * 32768);
+ process_comp_z3 = ((int32_t)ao_bmm150_trim.dig_z1) * (((int16_t)data_rhall) * 2);
+ process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
+ retval = ((process_comp_z2 - process_comp_z1) / (ao_bmm150_trim.dig_z2 + process_comp_z4));
+
+ /* saturate result to +/- 2 micro-tesla */
+ if (retval > BMM150_POSITIVE_SATURATION_Z)
+ {
+ retval = BMM150_POSITIVE_SATURATION_Z;
+ }
+ else if (retval < BMM150_NEGATIVE_SATURATION_Z)
+ {
+ retval = BMM150_NEGATIVE_SATURATION_Z;
+ }
+
+ /* Conversion of LSB to micro-tesla*/
+ retval = retval / 16;
+ }
+ else
+ {
+ retval = BMM150_OVERFLOW_OUTPUT;
+ }
+ }
+ else
+ {
+ /* Overflow condition*/
+ retval = BMM150_OVERFLOW_OUTPUT;
+ }
+
+ return (int16_t)retval;
}
-#endif
static void
_ao_bmx160_sample(struct ao_bmx160_sample *sample)
*d++ = (t >> 8) | (t << 8);
}
#endif
+ uint16_t rhall = sample->rhall >> 2;
+ sample->mag_x = compensate_x(sample->mag_x >> 3, rhall);
+ sample->mag_y = compensate_y(sample->mag_y >> 3, rhall);
+ sample->mag_z = compensate_z(sample->mag_z >> 1, rhall);
}
#define G 981 /* in cm/s² */
if (ao_bmx160_configured)
return;
+ /* Dummy read of 0x7f register to enable SPI interface */
+ (void) _ao_bmx160_reg_read(0x7f);
+
/* Make sure the chip is responding */
_ao_bmx160_wait_alive();
*/
_ao_bmx160_cmd(BMX160_CMD_MAG_IF_SET_PMU_MODE(BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL));
+ _ao_bmx160_reg_write(BMX160_IF_CONF,
+ (BMX160_IF_CONF_IF_MODE_AUTO_MAG << BMX160_IF_CONF_IF_MODE));
+
/* Enter setup mode */
- _ao_bmx160_mag_setup();
+ _ao_bmx160_reg_write(BMX160_MAG_IF_0,
+ (1 << BMX160_MAG_IF_0_MAG_MANUAL_EN) |
+ (0 << BMX160_MAG_IF_0_MAG_OFFSET) |
+ (0 << BMX160_MAG_IF_0_MAG_RD_BURST));
/* Place in suspend mode to reboot the chip */
_ao_bmm150_reg_write(BMM150_POWER_MODE,
_ao_bmm150_reg_write(BMM150_REPXY, BMM150_REPXY_VALUE(9));
_ao_bmm150_reg_write(BMM150_REPZ, BMM150_REPZ_VALUE(15));
+ /* Read Trim values */
+ ao_bmm150_trim.dig_x1 = _ao_bmm150_reg_read(BMM150_DIG_X1);
+ ao_bmm150_trim.dig_y1 = _ao_bmm150_reg_read(BMM150_DIG_Y1);
+ ao_bmm150_trim.dig_z4 = _ao_bmm150_reg_read2(BMM150_DIG_Z4_LSB, BMM150_DIG_Z4_MSB);
+ ao_bmm150_trim.dig_x2 = _ao_bmm150_reg_read(BMM150_DIG_X2);
+ ao_bmm150_trim.dig_y2 = _ao_bmm150_reg_read(BMM150_DIG_Y2);
+ ao_bmm150_trim.dig_z2 = _ao_bmm150_reg_read2(BMM150_DIG_Z2_LSB, BMM150_DIG_Z2_MSB);
+ ao_bmm150_trim.dig_z1 = _ao_bmm150_reg_read2(BMM150_DIG_Z1_LSB, BMM150_DIG_Z1_MSB);
+ ao_bmm150_trim.dig_xyz1 = _ao_bmm150_reg_read2(BMM150_DIG_XYZ1_LSB, BMM150_DIG_XYZ1_MSB);
+ ao_bmm150_trim.dig_z3 = _ao_bmm150_reg_read2(BMM150_DIG_Z3_LSB, BMM150_DIG_Z3_MSB);
+ ao_bmm150_trim.dig_xy2 = _ao_bmm150_reg_read(BMM150_DIG_XY2);
+ ao_bmm150_trim.dig_xy1 = _ao_bmm150_reg_read(BMM150_DIG_XY1);
+
/* To get data out of the magnetometer, set the control op mode to 'forced', then read
* from the data registers
*/
- _ao_bmx160_reg_write(BMX160_MAG_IF_3, (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE));
+ _ao_bmx160_reg_write(BMX160_MAG_IF_3,
+ (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) |
+ (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE));
_ao_bmx160_reg_write(BMX160_MAG_IF_2, BMM150_CONTROL);
_ao_bmx160_reg_write(BMX160_MAG_IF_1, BMM150_DATA_X_0_4);
- /* Set data rate to 200Hz */
- _ao_bmx160_reg_write(BMX160_MAG_CONF,
- (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR));
-
/* Put magnetometer interface back into 'normal mode'
*/
_ao_bmx160_reg_write(BMX160_MAG_IF_0,
(0 << BMX160_MAG_IF_0_MAG_MANUAL_EN) |
(0 << BMX160_MAG_IF_0_MAG_OFFSET) |
- (0 << BMX160_MAG_IF_0_MAG_RD_BURST));
+ (3 << BMX160_MAG_IF_0_MAG_RD_BURST));
+
+ /* Set data rate to 200Hz */
+ _ao_bmx160_reg_write(BMX160_MAG_CONF,
+ (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR));
ao_bmx160_configured = 1;
}
void
ao_bmx160_init(void)
{
- ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160");
-
ao_spi_init_cs(AO_BMX160_SPI_CS_PORT, (1 << AO_BMX160_SPI_CS_PIN));
+ ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160");
+
/* Pretend to be the bmx160 task. Grab the SPI bus right away and
* hold it for the task so that nothing else uses the SPI bus before
* we get the I2C mode disabled in the chip
ao_cur_task = &ao_bmx160_task;
ao_bmx160_spi_get();
ao_cur_task = NULL;
+
ao_cmd_register(&ao_bmx160_cmds[0]);
}
projects / fw / altos / blobdiff