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