git.gag.com Git - fw/altos/blob - src/drivers/ao_bmx160.c

   1 /*
   2  * Copyright © 2019 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #include <ao.h>
  20 #include <ao_bmx160.h>
  21 #include <ao_exti.h>
  22
  23 static uint8_t  ao_bmx160_configured;
  24
  25 static struct ao_bmm150_trim ao_bmm150_trim;
  26
  27 #define AO_BMX160_SPI_SPEED     ao_spi_speed(10000000)
  28
  29 #define ao_bmx160_spi_get()     ao_spi_get(AO_BMX160_SPI_BUS, AO_BMX160_SPI_SPEED)
  30 #define ao_bmx160_spi_put()     ao_spi_put(AO_BMX160_SPI_BUS)
  31
  32 #define ao_bmx160_spi_start()   ao_spi_set_cs(AO_BMX160_SPI_CS_PORT,    \
  33                                               (1 << AO_BMX160_SPI_CS_PIN))
  34
  35 #define ao_bmx160_spi_end()     ao_spi_clr_cs(AO_BMX160_SPI_CS_PORT,    \
  36                                               (1 << AO_BMX160_SPI_CS_PIN))
  37
  38 static void
  39 _ao_bmx160_reg_write(uint8_t addr, uint8_t value)
  40 {
  41         uint8_t d[2] = { addr, value };
  42         ao_bmx160_spi_start();
  43         ao_spi_send(d, 2, AO_BMX160_SPI_BUS);
  44         ao_bmx160_spi_end();
  45 }
  46
  47 static void
  48 _ao_bmx160_read(uint8_t addr, void *data, uint8_t len)
  49 {
  50         addr |= 0x80;
  51         ao_bmx160_spi_start();
  52         ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS);
  53         ao_spi_recv(data, len, AO_BMX160_SPI_BUS);
  54         ao_bmx160_spi_end();
  55 }
  56
  57 static uint8_t
  58 _ao_bmx160_reg_read(uint8_t addr)
  59 {
  60         uint8_t value;
  61         addr |= 0x80;
  62         ao_bmx160_spi_start();
  63         ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS);
  64         ao_spi_recv(&value, 1, AO_BMX160_SPI_BUS);
  65         ao_bmx160_spi_end();
  66         return value;
  67 }
  68
  69 static void
  70 _ao_bmx160_cmd(uint8_t cmd)
  71 {
  72         int i;
  73         _ao_bmx160_reg_write(BMX160_CMD, cmd);
  74         for (i = 0; i < 50; i++) {
  75                 uint8_t cmd_read;
  76                 cmd_read = _ao_bmx160_reg_read(BMX160_CMD);
  77                 if (cmd_read != cmd)
  78                         break;
  79         }
  80 }
  81
  82 static void
  83 _ao_bmm150_wait_manual(void)
  84 {
  85         while (_ao_bmx160_reg_read(BMX160_STATUS) & (1 << BMX160_STATUS_MAG_MAN_OP))
  86                 ;
  87 }
  88
  89 static void
  90 _ao_bmm150_reg_write(uint8_t addr, uint8_t data)
  91 {
  92         _ao_bmx160_reg_write(BMX160_MAG_IF_3, data);
  93         _ao_bmx160_reg_write(BMX160_MAG_IF_2, addr);
  94         _ao_bmm150_wait_manual();
  95 }
  96
  97 static uint8_t
  98 _ao_bmm150_reg_read(uint8_t addr)
  99 {
 100         _ao_bmx160_reg_write(BMX160_MAG_IF_1, addr);
 101         _ao_bmm150_wait_manual();
 102         uint8_t ret = _ao_bmx160_reg_read(BMX160_DATA_0);
 103         return ret;
 104 }
 105
 106 static uint16_t
 107 _ao_bmm150_reg_read2(uint8_t lo_addr, uint8_t hi_addr)
 108 {
 109         uint8_t lo = _ao_bmm150_reg_read(lo_addr);
 110         uint8_t hi = _ao_bmm150_reg_read(hi_addr);
 111
 112         return (uint16_t) (((uint16_t) hi << 8) | (uint16_t) lo);
 113 }
 114
 115 /*
 116  * The compensate functions are taken from the BMM150 sample
 117  * driver which has the following copyright:
 118  *
 119  * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved.
 120  *
 121  * BSD-3-Clause
 122  *
 123  * Redistribution and use in source and binary forms, with or without
 124  * modification, are permitted provided that the following conditions are met:
 125  *
 126  * 1. Redistributions of source code must retain the above copyright
 127  *    notice, this list of conditions and the following disclaimer.
 128  *
 129  * 2. Redistributions in binary form must reproduce the above copyright
 130  *    notice, this list of conditions and the following disclaimer in the
 131  *    documentation and/or other materials provided with the distribution.
 132  *
 133  * 3. Neither the name of the copyright holder nor the names of its
 134  *    contributors may be used to endorse or promote products derived from
 135  *    this software without specific prior written permission.
 136  *
 137  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 138  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 139  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 140  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 141  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 142  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 143  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 144  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 145  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 146  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 147  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 148  * POSSIBILITY OF SUCH DAMAGE.
 149  *
 150  * @file bmm150.c
 151  * @date 10/01/2020
 152  * @version  1.0.3
 153  *
 154  */
 155
 156 /*!
 157  * @brief This internal API is used to obtain the compensated
 158  * magnetometer X axis data(micro-tesla) in int16_t.
 159  */
 160 static int16_t compensate_x(int16_t mag_data_x, uint16_t data_rhall)
 161 {
 162     int16_t retval;
 163     uint16_t process_comp_x0 = 0;
 164     int32_t process_comp_x1;
 165     uint16_t process_comp_x2;
 166     int32_t process_comp_x3;
 167     int32_t process_comp_x4;
 168     int32_t process_comp_x5;
 169     int32_t process_comp_x6;
 170     int32_t process_comp_x7;
 171     int32_t process_comp_x8;
 172     int32_t process_comp_x9;
 173     int32_t process_comp_x10;
 174
 175     /* Overflow condition check */
 176     if (mag_data_x != BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL)
 177     {
 178         if (data_rhall != 0)
 179         {
 180             /* Availability of valid data*/
 181             process_comp_x0 = data_rhall;
 182             //printf("using data_rhall %d\n", data_rhall);
 183         }
 184         else if (ao_bmm150_trim.dig_xyz1 != 0)
 185         {
 186             process_comp_x0 = ao_bmm150_trim.dig_xyz1;
 187             //printf("using trim value %d\n", process_comp_x0);
 188         }
 189         else
 190         {
 191             process_comp_x0 = 0;
 192             //printf("no valid rhall\n");
 193         }
 194         if (process_comp_x0 != 0)
 195         {
 196             /* Processing compensation equations*/
 197             process_comp_x1 = ((int32_t)ao_bmm150_trim.dig_xyz1) * 16384;
 198             //printf("comp_x1 %d\n", process_comp_x1);
 199             process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
 200             //printf("comp_x2 %d\n", process_comp_x2);
 201             retval = ((int16_t)process_comp_x2);
 202             process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
 203             //printf("comp_x3 %d\n", process_comp_x3);
 204             process_comp_x4 = (((int32_t)ao_bmm150_trim.dig_xy2) * (process_comp_x3 / 128));
 205             //printf("comp_x4 %d\n", process_comp_x4);
 206             process_comp_x5 = (int32_t)(((int16_t)ao_bmm150_trim.dig_xy1) * 128);
 207             //printf("comp_x5 %d\n", process_comp_x5);
 208             process_comp_x6 = ((int32_t)retval) * process_comp_x5;
 209             //printf("comp_x6 %d\n", process_comp_x6);
 210             process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
 211             //printf("comp_x7 %d\n", process_comp_x7);
 212             process_comp_x8 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_x2) + ((int16_t)0xA0)));
 213             //printf("comp_x8 %d\n", process_comp_x8);
 214             process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
 215             //printf("comp_x9 %d\n", process_comp_x9);
 216             process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
 217             //printf("comp_x10 %d\n", process_comp_x10);
 218             retval = ((int16_t)(process_comp_x10 / 8192));
 219             //printf("ret 1 %d\n", retval);
 220             retval = (int16_t) ((retval + (((int16_t)ao_bmm150_trim.dig_x1) * 8)) / 16);
 221             //printf("final %d\n", retval);
 222         }
 223         else
 224         {
 225             retval = BMM150_OVERFLOW_OUTPUT;
 226         }
 227     }
 228     else
 229     {
 230         /* Overflow condition */
 231         retval = BMM150_OVERFLOW_OUTPUT;
 232     }
 233
 234     return retval;
 235 }
 236
 237 /*!
 238  * @brief This internal API is used to obtain the compensated
 239  * magnetometer Y axis data(micro-tesla) in int16_t.
 240  */
 241 static int16_t compensate_y(int16_t mag_data_y, uint16_t data_rhall)
 242 {
 243     int16_t retval;
 244     uint16_t process_comp_y0 = 0;
 245     int32_t process_comp_y1;
 246     uint16_t process_comp_y2;
 247     int32_t process_comp_y3;
 248     int32_t process_comp_y4;
 249     int32_t process_comp_y5;
 250     int32_t process_comp_y6;
 251     int32_t process_comp_y7;
 252     int32_t process_comp_y8;
 253     int32_t process_comp_y9;
 254
 255     /* Overflow condition check */
 256     if (mag_data_y != BMM150_XYAXES_FLIP_OVERFLOW_ADCVAL)
 257     {
 258         if (data_rhall != 0)
 259         {
 260             /* Availability of valid data*/
 261             process_comp_y0 = data_rhall;
 262         }
 263         else if (ao_bmm150_trim.dig_xyz1 != 0)
 264         {
 265             process_comp_y0 = ao_bmm150_trim.dig_xyz1;
 266         }
 267         else
 268         {
 269             process_comp_y0 = 0;
 270         }
 271         if (process_comp_y0 != 0)
 272         {
 273             /*Processing compensation equations*/
 274             process_comp_y1 = (((int32_t)ao_bmm150_trim.dig_xyz1) * 16384) / process_comp_y0;
 275             process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
 276             retval = ((int16_t)process_comp_y2);
 277             process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
 278             process_comp_y4 = ((int32_t)ao_bmm150_trim.dig_xy2) * (process_comp_y3 / 128);
 279             process_comp_y5 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_xy1) * 128));
 280             process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
 281             process_comp_y7 = ((int32_t)(((int16_t)ao_bmm150_trim.dig_y2) + ((int16_t)0xA0)));
 282             process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
 283             process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
 284             retval = (int16_t)(process_comp_y9 / 8192);
 285             retval = (int16_t) ((retval + (((int16_t)ao_bmm150_trim.dig_y1) * 8)) / 16);
 286         }
 287         else
 288         {
 289             retval = BMM150_OVERFLOW_OUTPUT;
 290         }
 291     }
 292     else
 293     {
 294         /* Overflow condition*/
 295         retval = BMM150_OVERFLOW_OUTPUT;
 296     }
 297
 298     return retval;
 299 }
 300
 301 /*!
 302  * @brief This internal API is used to obtain the compensated
 303  * magnetometer Z axis data(micro-tesla) in int16_t.
 304  */
 305 static int16_t compensate_z(int16_t mag_data_z, uint16_t data_rhall)
 306 {
 307     int32_t retval;
 308     int16_t process_comp_z0;
 309     int32_t process_comp_z1;
 310     int32_t process_comp_z2;
 311     int32_t process_comp_z3;
 312     int16_t process_comp_z4;
 313
 314     if (mag_data_z != BMM150_ZAXIS_HALL_OVERFLOW_ADCVAL)
 315     {
 316         if ((ao_bmm150_trim.dig_z2 != 0) && (ao_bmm150_trim.dig_z1 != 0) && (data_rhall != 0) &&
 317             (ao_bmm150_trim.dig_xyz1 != 0))
 318         {
 319             /*Processing compensation equations*/
 320             process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) ao_bmm150_trim.dig_xyz1);
 321             process_comp_z1 = (((int32_t)ao_bmm150_trim.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
 322             process_comp_z2 = (((int32_t)(mag_data_z - ao_bmm150_trim.dig_z4)) * 32768);
 323             process_comp_z3 = ((int32_t)ao_bmm150_trim.dig_z1) * (((int16_t)data_rhall) * 2);
 324             process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
 325             retval = ((process_comp_z2 - process_comp_z1) / (ao_bmm150_trim.dig_z2 + process_comp_z4));
 326
 327             /* saturate result to +/- 2 micro-tesla */
 328             if (retval > BMM150_POSITIVE_SATURATION_Z)
 329             {
 330                 retval = BMM150_POSITIVE_SATURATION_Z;
 331             }
 332             else if (retval < BMM150_NEGATIVE_SATURATION_Z)
 333             {
 334                 retval = BMM150_NEGATIVE_SATURATION_Z;
 335             }
 336
 337             /* Conversion of LSB to micro-tesla*/
 338             retval = retval / 16;
 339         }
 340         else
 341         {
 342             retval = BMM150_OVERFLOW_OUTPUT;
 343         }
 344     }
 345     else
 346     {
 347         /* Overflow condition*/
 348         retval = BMM150_OVERFLOW_OUTPUT;
 349     }
 350
 351     return (int16_t)retval;
 352 }
 353
 354 static void
 355 _ao_bmx160_sample(struct ao_bmx160_sample *sample)
 356 {
 357         _ao_bmx160_read(BMX160_MAG_X_0_7, sample, sizeof (*sample));
 358 #if __BYTE_ORDER != __LITTLE_ENDIAN
 359         int             i = sizeof (*sample) / 2;
 360         uint16_t        *d = (uint16_t *) sample;
 361
 362         /* byte swap */
 363         while (i--) {
 364                 uint16_t        t = *d;
 365                 *d++ = (t >> 8) | (t << 8);
 366         }
 367 #endif
 368         uint16_t rhall = sample->rhall >> 2;
 369         sample->mag_x = compensate_x(sample->mag_x >> 3, rhall);
 370         sample->mag_y = compensate_y(sample->mag_y >> 3, rhall);
 371         sample->mag_z = compensate_z(sample->mag_z >> 1, rhall);
 372 }
 373
 374 #define G       981     /* in cm/s² */
 375
 376 #if 0
 377 static int16_t /* cm/s² */
 378 ao_bmx160_accel(int16_t v)
 379 {
 380         return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767);
 381 }
 382
 383 static int16_t  /* deg*10/s */
 384 ao_bmx160_gyro(int16_t v)
 385 {
 386         return (int16_t) ((v * (int32_t) 20000) / 32767);
 387 }
 388
 389 static uint8_t
 390 ao_bmx160_accel_check(int16_t normal, int16_t test)
 391 {
 392         int16_t diff = test - normal;
 393
 394         if (diff < BMX160_ST_ACCEL(16) / 4) {
 395                 return 1;
 396         }
 397         if (diff > BMX160_ST_ACCEL(16) * 4) {
 398                 return 1;
 399         }
 400         return 0;
 401 }
 402
 403 static uint8_t
 404 ao_bmx160_gyro_check(int16_t normal, int16_t test)
 405 {
 406         int16_t diff = test - normal;
 407
 408         if (diff < 0)
 409                 diff = -diff;
 410         if (diff < BMX160_ST_GYRO(2000) / 4) {
 411                 return 1;
 412         }
 413         if (diff > BMX160_ST_GYRO(2000) * 4) {
 414                 return 1;
 415         }
 416         return 0;
 417 }
 418 #endif
 419
 420 static void
 421 _ao_bmx160_wait_alive(void)
 422 {
 423         uint8_t i;
 424
 425         /* Wait for the chip to wake up */
 426         for (i = 0; i < 30; i++) {
 427                 ao_delay(AO_MS_TO_TICKS(100));
 428                 if (_ao_bmx160_reg_read(BMX160_CHIPID) == BMX160_CHIPID_BMX160)
 429                         break;
 430         }
 431         if (i == 30)
 432                 ao_panic(AO_PANIC_SELF_TEST_BMX160);
 433 }
 434
 435 #define ST_TRIES        10
 436 #define MAG_TRIES       10
 437
 438 static void
 439 _ao_bmx160_setup(void)
 440 {
 441         int r;
 442
 443         if (ao_bmx160_configured)
 444                 return;
 445
 446         /* Dummy read of 0x7f register to enable SPI interface */
 447         (void) _ao_bmx160_reg_read(0x7f);
 448
 449         /* Make sure the chip is responding */
 450         _ao_bmx160_wait_alive();
 451
 452         /* Force SPI mode */
 453         _ao_bmx160_reg_write(BMX160_NV_CONF, 1 << BMX160_NV_CONF_SPI_EN);
 454
 455         /* Enable acc and gyr
 456          */
 457
 458         _ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL));
 459
 460         for (r = 0; r < 20; r++) {
 461                 ao_delay(AO_MS_TO_TICKS(100));
 462                 if (((_ao_bmx160_reg_read(BMX160_PMU_STATUS)
 463                       >> BMX160_PMU_STATUS_ACC_PMU_STATUS)
 464                      & BMX160_PMU_STATUS_ACC_PMU_STATUS_MASK)
 465                     == BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL)
 466                 {
 467                         r = 0;
 468                         break;
 469                 }
 470         }
 471         if (r != 0)
 472                 AO_SENSOR_ERROR(AO_DATA_BMX160);
 473
 474         _ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL));
 475
 476         for (r = 0; r < 20; r++) {
 477                 ao_delay(AO_MS_TO_TICKS(100));
 478                 if (((_ao_bmx160_reg_read(BMX160_PMU_STATUS)
 479                       >> BMX160_PMU_STATUS_GYR_PMU_STATUS)
 480                      & BMX160_PMU_STATUS_GYR_PMU_STATUS_MASK)
 481                     == BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL)
 482                 {
 483                         r = 0;
 484                         break;
 485                 }
 486         }
 487         if (r != 0)
 488                 AO_SENSOR_ERROR(AO_DATA_BMX160);
 489
 490         /* Configure accelerometer:
 491          *
 492          *      undersampling disabled
 493          *      normal filter
 494          *      200Hz sampling rate
 495          *      16g range
 496          *
 497          * This yields a 3dB cutoff frequency of 80Hz
 498          */
 499         _ao_bmx160_reg_write(BMX160_ACC_CONF,
 500                              (0 << BMX160_ACC_CONF_ACC_US) |
 501                              (BMX160_ACC_CONF_ACC_BWP_NORMAL << BMX160_ACC_CONF_ACC_BWP) |
 502                              (BMX160_ACC_CONF_ACC_ODR_200 << BMX160_ACC_CONF_ACC_ODR));
 503         _ao_bmx160_reg_write(BMX160_ACC_RANGE,
 504                              BMX160_ACC_RANGE_16G);
 505
 506         for (r = 0x3; r <= 0x1b; r++)
 507                 (void) _ao_bmx160_reg_read((uint8_t) r);
 508
 509         /* Configure gyro:
 510          *
 511          *      200Hz sampling rate
 512          *      Normal filter mode
 513          *      ±2000°/s
 514          */
 515         _ao_bmx160_reg_write(BMX160_GYR_CONF,
 516                              (BMX160_GYR_CONF_GYR_BWP_NORMAL << BMX160_GYR_CONF_GYR_BWP) |
 517                              (BMX160_GYR_CONF_GYR_ODR_200 << BMX160_GYR_CONF_GYR_ODR));
 518         _ao_bmx160_reg_write(BMX160_GYR_RANGE,
 519                              BMX160_GYR_RANGE_2000);
 520
 521
 522         /* Configure magnetometer:
 523          *
 524          *      30Hz sampling rate
 525          *      power on
 526          *      axes enabled
 527          */
 528         _ao_bmx160_cmd(BMX160_CMD_MAG_IF_SET_PMU_MODE(BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL));
 529
 530         _ao_bmx160_reg_write(BMX160_IF_CONF,
 531                              (BMX160_IF_CONF_IF_MODE_AUTO_MAG << BMX160_IF_CONF_IF_MODE));
 532
 533         /* Enter setup mode */
 534         _ao_bmx160_reg_write(BMX160_MAG_IF_0,
 535                              (1 << BMX160_MAG_IF_0_MAG_MANUAL_EN) |
 536                              (0 << BMX160_MAG_IF_0_MAG_OFFSET) |
 537                              (0 << BMX160_MAG_IF_0_MAG_RD_BURST));
 538
 539         /* Place in suspend mode to reboot the chip */
 540         _ao_bmm150_reg_write(BMM150_POWER_MODE,
 541                              (0 << BMM150_POWER_MODE_POWER_CONTROL));
 542
 543         /* Power on */
 544         _ao_bmm150_reg_write(BMM150_POWER_MODE,
 545                              (1 << BMM150_POWER_MODE_POWER_CONTROL));
 546
 547         /* Set data rate and place in sleep mode */
 548         _ao_bmm150_reg_write(BMM150_CONTROL,
 549                              (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) |
 550                              (BMM150_CONTROL_OP_MODE_SLEEP << BMM150_CONTROL_OP_MODE));
 551
 552         /* enable all axes (should already be enabled) */
 553         _ao_bmm150_reg_write(BMM150_INT_CONF,
 554                              (0 << BMM150_INT_CONF_X_DISABLE) |
 555                              (0 << BMM150_INT_CONF_Y_DISABLE) |
 556                              (0 << BMM150_INT_CONF_Z_DISABLE));
 557
 558         /* Set repetition values (?) */
 559         _ao_bmm150_reg_write(BMM150_REPXY, BMM150_REPXY_VALUE(9));
 560         _ao_bmm150_reg_write(BMM150_REPZ, BMM150_REPZ_VALUE(15));
 561
 562         /* Read Trim values */
 563         ao_bmm150_trim.dig_x1   = (int8_t) _ao_bmm150_reg_read(BMM150_DIG_X1);
 564         ao_bmm150_trim.dig_y1   = (int8_t) _ao_bmm150_reg_read(BMM150_DIG_Y1);
 565         ao_bmm150_trim.dig_z4   = (int8_t) _ao_bmm150_reg_read2(BMM150_DIG_Z4_LSB, BMM150_DIG_Z4_MSB);
 566         ao_bmm150_trim.dig_x2   = (int8_t) _ao_bmm150_reg_read(BMM150_DIG_X2);
 567         ao_bmm150_trim.dig_y2   = (int8_t) _ao_bmm150_reg_read(BMM150_DIG_Y2);
 568         ao_bmm150_trim.dig_z2   = (int8_t) _ao_bmm150_reg_read2(BMM150_DIG_Z2_LSB, BMM150_DIG_Z2_MSB);
 569         ao_bmm150_trim.dig_z1   = _ao_bmm150_reg_read2(BMM150_DIG_Z1_LSB, BMM150_DIG_Z1_MSB);
 570         ao_bmm150_trim.dig_xyz1 = _ao_bmm150_reg_read2(BMM150_DIG_XYZ1_LSB, BMM150_DIG_XYZ1_MSB);
 571         ao_bmm150_trim.dig_z3   = (int8_t) _ao_bmm150_reg_read2(BMM150_DIG_Z3_LSB, BMM150_DIG_Z3_MSB);
 572         ao_bmm150_trim.dig_xy2  = (int8_t) _ao_bmm150_reg_read(BMM150_DIG_XY2);
 573         ao_bmm150_trim.dig_xy1  = _ao_bmm150_reg_read(BMM150_DIG_XY1);
 574
 575         /* To get data out of the magnetometer, set the control op mode to 'forced', then read
 576          * from the data registers
 577          */
 578         _ao_bmx160_reg_write(BMX160_MAG_IF_3,
 579                              (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) |
 580                              (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE));
 581         _ao_bmx160_reg_write(BMX160_MAG_IF_2, BMM150_CONTROL);
 582         _ao_bmx160_reg_write(BMX160_MAG_IF_1, BMM150_DATA_X_0_4);
 583
 584         /* Put magnetometer interface back into 'normal mode'
 585          */
 586         _ao_bmx160_reg_write(BMX160_MAG_IF_0,
 587                              (0 << BMX160_MAG_IF_0_MAG_MANUAL_EN) |
 588                              (0 << BMX160_MAG_IF_0_MAG_OFFSET) |
 589                              (3 << BMX160_MAG_IF_0_MAG_RD_BURST));
 590
 591         /* Set data rate to 200Hz */
 592         _ao_bmx160_reg_write(BMX160_MAG_CONF,
 593                              (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR));
 594
 595         ao_bmx160_configured = 1;
 596 }
 597
 598 struct ao_bmx160_sample ao_bmx160_current;
 599
 600 static void
 601 ao_bmx160(void)
 602 {
 603         struct ao_bmx160_sample sample;
 604
 605         /* ao_bmx160_init already grabbed the SPI bus and mutex */
 606         _ao_bmx160_setup();
 607         ao_bmx160_spi_put();
 608         for (;;)
 609         {
 610                 ao_bmx160_spi_get();
 611                 _ao_bmx160_sample(&sample);
 612                 ao_bmx160_spi_put();
 613                 ao_arch_block_interrupts();
 614                 ao_bmx160_current = sample;
 615                 AO_DATA_PRESENT(AO_DATA_BMX160);
 616                 AO_DATA_WAIT();
 617                 ao_arch_release_interrupts();
 618         }
 619 }
 620
 621 static struct ao_task ao_bmx160_task;
 622
 623 static void
 624 ao_bmx160_show(void)
 625 {
 626         printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d Mag: %7d %7d %7d\n",
 627                 ao_bmx160_current.acc_x,
 628                 ao_bmx160_current.acc_y,
 629                 ao_bmx160_current.acc_z,
 630                 ao_bmx160_current.gyr_x,
 631                 ao_bmx160_current.gyr_y,
 632                 ao_bmx160_current.gyr_z,
 633                 ao_bmx160_current.mag_x,
 634                 ao_bmx160_current.mag_y,
 635                 ao_bmx160_current.mag_z);
 636 }
 637
 638 #if BMX160_TEST
 639
 640 static void
 641 ao_bmx160_read(void)
 642 {
 643         uint8_t addr;
 644         uint8_t val;
 645
 646         addr = ao_cmd_hex();
 647         if (ao_cmd_status != ao_cmd_success)
 648                 return;
 649         ao_bmx160_spi_get();
 650         val = _ao_bmx160_reg_read(addr);
 651         ao_bmx160_spi_put();
 652         printf("Addr %02x val %02x\n", addr, val);
 653 }
 654
 655 static void
 656 ao_bmx160_write(void)
 657 {
 658         uint8_t addr;
 659         uint8_t val;
 660
 661         addr = ao_cmd_hex();
 662         if (ao_cmd_status != ao_cmd_success)
 663                 return;
 664         val = ao_cmd_hex();
 665         if (ao_cmd_status != ao_cmd_success)
 666                 return;
 667         printf("Addr %02x val %02x\n", addr, val);
 668         ao_bmx160_spi_get();
 669         _ao_bmx160_reg_write(addr, val);
 670         ao_bmx160_spi_put();
 671 }
 672
 673 static void
 674 ao_bmm150_read(void)
 675 {
 676         uint8_t addr;
 677         uint8_t val;
 678
 679         addr = ao_cmd_hex();
 680         if (ao_cmd_status != ao_cmd_success)
 681                 return;
 682         ao_bmx160_spi_get();
 683         val = _ao_bmm150_reg_read(addr);
 684         ao_bmx160_spi_put();
 685         printf("Addr %02x val %02x\n", addr, val);
 686 }
 687
 688 static void
 689 ao_bmm150_write(void)
 690 {
 691         uint8_t addr;
 692         uint8_t val;
 693
 694         addr = ao_cmd_hex();
 695         if (ao_cmd_status != ao_cmd_success)
 696                 return;
 697         val = ao_cmd_hex();
 698         if (ao_cmd_status != ao_cmd_success)
 699                 return;
 700         printf("Addr %02x val %02x\n", addr, val);
 701         ao_bmx160_spi_get();
 702         _ao_bmm150_reg_write(addr, val);
 703         ao_bmx160_spi_put();
 704 }
 705
 706 #endif /* BMX160_TEST */
 707
 708 static const struct ao_cmds ao_bmx160_cmds[] = {
 709         { ao_bmx160_show,       "I\0Show BMX160 status" },
 710 #if BMX160_TEST
 711         { ao_bmx160_read,       "R <addr>\0Read BMX160 register" },
 712         { ao_bmx160_write,      "W <addr> <val>\0Write BMX160 register" },
 713         { ao_bmm150_read,       "M <addr>\0Read BMM150 register" },
 714         { ao_bmm150_write,      "N <addr> <val>\0Write BMM150 register" },
 715 #endif
 716         { 0, NULL }
 717 };
 718
 719 void
 720 ao_bmx160_init(void)
 721 {
 722         ao_spi_init_cs(AO_BMX160_SPI_CS_PORT, (1 << AO_BMX160_SPI_CS_PIN));
 723
 724         ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160");
 725
 726         /* Pretend to be the bmx160 task. Grab the SPI bus right away and
 727          * hold it for the task so that nothing else uses the SPI bus before
 728          * we get the I2C mode disabled in the chip
 729          */
 730
 731         ao_cur_task = &ao_bmx160_task;
 732         ao_bmx160_spi_get();
 733         ao_cur_task = NULL;
 734
 735         ao_cmd_register(&ao_bmx160_cmds[0]);
 736 }