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 #define ao_bmx160_spi_get()     ao_spi_get(AO_BMX160_SPI_BUS, AO_SPI_SPEED_8MHz)
  26 #define ao_bmx160_spi_put()     ao_spi_put(AO_BMX160_SPI_BUS)
  27
  28 #define ao_bmx160_spi_start()   ao_spi_set_cs(AO_BMX160_SPI_CS_PORT,    \
  29                                               (1 << AO_BMX160_SPI_CS_PIN))
  30
  31 #define ao_bmx160_spi_end()     ao_spi_clr_cs(AO_BMX160_SPI_CS_PORT,    \
  32                                               (1 << AO_BMX160_SPI_CS_PIN))
  33
  34 static void
  35 _ao_bmx160_reg_write(uint8_t addr, uint8_t value)
  36 {
  37         uint8_t d[2] = { addr, value };
  38         ao_bmx160_spi_start();
  39         ao_spi_send(d, 2, AO_BMX160_SPI_BUS);
  40         ao_bmx160_spi_end();
  41 }
  42
  43 static void
  44 _ao_bmx160_read(uint8_t addr, void *data, uint8_t len)
  45 {
  46         addr |= 0x80;
  47         ao_bmx160_spi_start();
  48         ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS);
  49         ao_spi_recv(data, len, AO_BMX160_SPI_BUS);
  50         ao_bmx160_spi_end();
  51 }
  52
  53 static uint8_t
  54 _ao_bmx160_reg_read(uint8_t addr)
  55 {
  56         uint8_t value;
  57         addr |= 0x80;
  58         ao_bmx160_spi_start();
  59         ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS);
  60         ao_spi_recv(&value, 1, AO_BMX160_SPI_BUS);
  61         ao_bmx160_spi_end();
  62         return value;
  63 }
  64
  65 static void
  66 _ao_bmx160_cmd(uint8_t cmd)
  67 {
  68         _ao_bmx160_reg_write(BMX160_CMD, cmd);
  69         ao_delay(AO_MS_TO_TICKS(100));
  70 }
  71
  72 static void
  73 _ao_bmx160_mag_setup(void)
  74 {
  75         _ao_bmx160_reg_write(BMX160_MAG_IF_0, 0x80);
  76 }
  77
  78 static void
  79 _ao_bmm150_wait_manual(void)
  80 {
  81         while (_ao_bmx160_reg_read(BMX160_STATUS) & (1 << BMX160_STATUS_MAG_MAN_OP))
  82                 ;
  83 }
  84
  85 static void
  86 _ao_bmm150_reg_write(uint8_t addr, uint8_t data)
  87 {
  88         _ao_bmx160_reg_write(BMX160_MAG_IF_3, data);
  89         _ao_bmx160_reg_write(BMX160_MAG_IF_2, addr);
  90         _ao_bmm150_wait_manual();
  91 }
  92
  93 #if BMX160_TEST
  94 static uint8_t
  95 _ao_bmm150_reg_read(uint8_t addr)
  96 {
  97         _ao_bmx160_reg_write(BMX160_MAG_IF_1, addr);
  98         _ao_bmm150_wait_manual();
  99         return _ao_bmx160_reg_read(BMX160_DATA_0);
 100 }
 101 #endif
 102
 103 static void
 104 _ao_bmx160_sample(struct ao_bmx160_sample *sample)
 105 {
 106         _ao_bmx160_read(BMX160_MAG_X_0_7, sample, sizeof (*sample));
 107 #if __BYTE_ORDER != __LITTLE_ENDIAN
 108         int             i = sizeof (*sample) / 2;
 109         uint16_t        *d = (uint16_t *) sample;
 110
 111         /* byte swap */
 112         while (i--) {
 113                 uint16_t        t = *d;
 114                 *d++ = (t >> 8) | (t << 8);
 115         }
 116 #endif
 117 }
 118
 119 #define G       981     /* in cm/s² */
 120
 121 #if 0
 122 static int16_t /* cm/s² */
 123 ao_bmx160_accel(int16_t v)
 124 {
 125         return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767);
 126 }
 127
 128 static int16_t  /* deg*10/s */
 129 ao_bmx160_gyro(int16_t v)
 130 {
 131         return (int16_t) ((v * (int32_t) 20000) / 32767);
 132 }
 133
 134 static uint8_t
 135 ao_bmx160_accel_check(int16_t normal, int16_t test)
 136 {
 137         int16_t diff = test - normal;
 138
 139         if (diff < BMX160_ST_ACCEL(16) / 4) {
 140                 return 1;
 141         }
 142         if (diff > BMX160_ST_ACCEL(16) * 4) {
 143                 return 1;
 144         }
 145         return 0;
 146 }
 147
 148 static uint8_t
 149 ao_bmx160_gyro_check(int16_t normal, int16_t test)
 150 {
 151         int16_t diff = test - normal;
 152
 153         if (diff < 0)
 154                 diff = -diff;
 155         if (diff < BMX160_ST_GYRO(2000) / 4) {
 156                 return 1;
 157         }
 158         if (diff > BMX160_ST_GYRO(2000) * 4) {
 159                 return 1;
 160         }
 161         return 0;
 162 }
 163 #endif
 164
 165 static void
 166 _ao_bmx160_wait_alive(void)
 167 {
 168         uint8_t i;
 169
 170         /* Wait for the chip to wake up */
 171         for (i = 0; i < 30; i++) {
 172                 ao_delay(AO_MS_TO_TICKS(100));
 173                 if (_ao_bmx160_reg_read(BMX160_CHIPID) == BMX160_CHIPID_BMX160)
 174                         break;
 175         }
 176         if (i == 30)
 177                 ao_panic(AO_PANIC_SELF_TEST_BMX160);
 178 }
 179
 180 #define ST_TRIES        10
 181 #define MAG_TRIES       10
 182
 183 static void
 184 _ao_bmx160_setup(void)
 185 {
 186         int r;
 187
 188         if (ao_bmx160_configured)
 189                 return;
 190
 191         /* Make sure the chip is responding */
 192         _ao_bmx160_wait_alive();
 193
 194         /* Reboot */
 195 //      _ao_bmx160_cmd(BMX160_CMD_SOFTRESET);
 196
 197         /* Force SPI mode */
 198         _ao_bmx160_reg_write(BMX160_NV_CONF, 1 << BMX160_NV_CONF_SPI_EN);
 199
 200         /* Enable acc and gyr
 201          */
 202
 203         _ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL));
 204
 205         _ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL));
 206
 207         /* Configure accelerometer:
 208          *
 209          *      undersampling disabled
 210          *      normal filter
 211          *      200Hz sampling rate
 212          *      16g range
 213          *
 214          * This yields a 3dB cutoff frequency of 80Hz
 215          */
 216         _ao_bmx160_reg_write(BMX160_ACC_CONF,
 217                              (0 << BMX160_ACC_CONF_ACC_US) |
 218                              (BMX160_ACC_CONF_ACC_BWP_NORMAL << BMX160_ACC_CONF_ACC_BWP) |
 219                              (BMX160_ACC_CONF_ACC_ODR_200 << BMX160_ACC_CONF_ACC_ODR));
 220         _ao_bmx160_reg_write(BMX160_ACC_RANGE,
 221                              BMX160_ACC_RANGE_16G);
 222         for (r = 0x4; r <= 0x17; r++)
 223                 (void) _ao_bmx160_reg_read(r);
 224
 225
 226         /* Configure gyro:
 227          *
 228          *      200Hz sampling rate
 229          *      Normal filter mode
 230          *      ±2000°/s
 231          */
 232         _ao_bmx160_reg_write(BMX160_GYR_CONF,
 233                              (BMX160_GYR_CONF_GYR_BWP_NORMAL << BMX160_GYR_CONF_GYR_BWP) |
 234                              (BMX160_GYR_CONF_GYR_ODR_200 << BMX160_GYR_CONF_GYR_ODR));
 235         _ao_bmx160_reg_write(BMX160_GYR_RANGE,
 236                              BMX160_GYR_RANGE_2000);
 237
 238
 239         /* Configure magnetometer:
 240          *
 241          *      30Hz sampling rate
 242          *      power on
 243          *      axes enabled
 244          */
 245         _ao_bmx160_cmd(BMX160_CMD_MAG_IF_SET_PMU_MODE(BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL));
 246
 247         /* Enter setup mode */
 248         _ao_bmx160_mag_setup();
 249
 250         /* Place in suspend mode to reboot the chip */
 251         _ao_bmm150_reg_write(BMM150_POWER_MODE,
 252                              (0 << BMM150_POWER_MODE_POWER_CONTROL));
 253
 254         /* Power on */
 255         _ao_bmm150_reg_write(BMM150_POWER_MODE,
 256                              (1 << BMM150_POWER_MODE_POWER_CONTROL));
 257
 258         /* Set data rate and place in sleep mode */
 259         _ao_bmm150_reg_write(BMM150_CONTROL,
 260                              (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) |
 261                              (BMM150_CONTROL_OP_MODE_SLEEP << BMM150_CONTROL_OP_MODE));
 262
 263         /* enable all axes (should already be enabled) */
 264         _ao_bmm150_reg_write(BMM150_INT_CONF,
 265                              (0 << BMM150_INT_CONF_X_DISABLE) |
 266                              (0 << BMM150_INT_CONF_Y_DISABLE) |
 267                              (0 << BMM150_INT_CONF_Z_DISABLE));
 268
 269         /* Set repetition values (?) */
 270         _ao_bmm150_reg_write(BMM150_REPXY, BMM150_REPXY_VALUE(9));
 271         _ao_bmm150_reg_write(BMM150_REPZ, BMM150_REPZ_VALUE(15));
 272
 273         /* To get data out of the magnetometer, set the control op mode to 'forced', then read
 274          * from the data registers
 275          */
 276         _ao_bmx160_reg_write(BMX160_MAG_IF_3, (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE));
 277         _ao_bmx160_reg_write(BMX160_MAG_IF_2, BMM150_CONTROL);
 278         _ao_bmx160_reg_write(BMX160_MAG_IF_1, BMM150_DATA_X_0_4);
 279
 280         /* Set data rate to 200Hz */
 281         _ao_bmx160_reg_write(BMX160_MAG_CONF,
 282                              (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR));
 283
 284         /* Put magnetometer interface back into 'normal mode'
 285          */
 286         _ao_bmx160_reg_write(BMX160_MAG_IF_0,
 287                              (0 << BMX160_MAG_IF_0_MAG_MANUAL_EN) |
 288                              (0 << BMX160_MAG_IF_0_MAG_OFFSET) |
 289                              (0 << BMX160_MAG_IF_0_MAG_RD_BURST));
 290
 291         ao_bmx160_configured = 1;
 292 }
 293
 294 struct ao_bmx160_sample ao_bmx160_current;
 295
 296 static void
 297 ao_bmx160(void)
 298 {
 299         struct ao_bmx160_sample sample;
 300
 301         /* ao_bmx160_init already grabbed the SPI bus and mutex */
 302         _ao_bmx160_setup();
 303         ao_bmx160_spi_put();
 304         for (;;)
 305         {
 306                 ao_bmx160_spi_get();
 307                 _ao_bmx160_sample(&sample);
 308                 ao_bmx160_spi_put();
 309                 ao_arch_block_interrupts();
 310                 ao_bmx160_current = sample;
 311                 AO_DATA_PRESENT(AO_DATA_BMX160);
 312                 AO_DATA_WAIT();
 313                 ao_arch_release_interrupts();
 314         }
 315 }
 316
 317 static struct ao_task ao_bmx160_task;
 318
 319 static void
 320 ao_bmx160_show(void)
 321 {
 322         ao_bmx160_spi_get();
 323         uint8_t acc_conf = _ao_bmx160_reg_read(BMX160_ACC_CONF);
 324         uint8_t acc_range = _ao_bmx160_reg_read(BMX160_ACC_RANGE);
 325         uint8_t gyr_conf = _ao_bmx160_reg_read(BMX160_GYR_CONF);
 326         uint8_t gyr_range = _ao_bmx160_reg_read(BMX160_GYR_RANGE);
 327         uint8_t mag_conf = _ao_bmx160_reg_read(BMX160_MAG_CONF);
 328         uint8_t status = _ao_bmx160_reg_read(BMX160_STATUS);
 329         uint8_t pmu_status = _ao_bmx160_reg_read(BMX160_PMU_STATUS);
 330         uint8_t acc_x_lo = _ao_bmx160_reg_read(BMX160_ACCEL_X_0_7);
 331         uint8_t acc_x_hi = _ao_bmx160_reg_read(BMX160_ACCEL_X_8_15);
 332         ao_bmx160_spi_put();
 333
 334         printf("ACC_CONF %02x ACC_RANGE %02x GYR_CONF %02x GYR_RANGE %02x MAG_CONF %02x\n",
 335                acc_conf, acc_range, gyr_conf, gyr_range, mag_conf);
 336
 337         printf("STATUS %02x PMU_STATUS %02x ACCEL_X_0_7 %02x ACCEL_X_8_15 %02x\n",
 338                status, pmu_status, acc_x_lo, acc_x_hi);
 339
 340         printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d Mag: %7d %7d %7d\n",
 341                 ao_bmx160_current.acc_x,
 342                 ao_bmx160_current.acc_y,
 343                 ao_bmx160_current.acc_z,
 344                 ao_bmx160_current.gyr_x,
 345                 ao_bmx160_current.gyr_y,
 346                 ao_bmx160_current.gyr_z,
 347                 ao_bmx160_current.mag_x,
 348                 ao_bmx160_current.mag_y,
 349                 ao_bmx160_current.mag_z);
 350
 351 }
 352
 353 #if BMX160_TEST
 354
 355 static void
 356 ao_bmx160_read(void)
 357 {
 358         uint8_t addr;
 359         uint8_t val;
 360
 361         addr = ao_cmd_hex();
 362         if (ao_cmd_status != ao_cmd_success)
 363                 return;
 364         ao_bmx160_spi_get();
 365         val = _ao_bmx160_reg_read(addr);
 366         ao_bmx160_spi_put();
 367         printf("Addr %02x val %02x\n", addr, val);
 368 }
 369
 370 static void
 371 ao_bmx160_write(void)
 372 {
 373         uint8_t addr;
 374         uint8_t val;
 375
 376         addr = ao_cmd_hex();
 377         if (ao_cmd_status != ao_cmd_success)
 378                 return;
 379         val = ao_cmd_hex();
 380         if (ao_cmd_status != ao_cmd_success)
 381                 return;
 382         printf("Addr %02x val %02x\n", addr, val);
 383         ao_bmx160_spi_get();
 384         _ao_bmx160_reg_write(addr, val);
 385         ao_bmx160_spi_put();
 386 }
 387
 388 static void
 389 ao_bmm150_read(void)
 390 {
 391         uint8_t addr;
 392         uint8_t val;
 393
 394         addr = ao_cmd_hex();
 395         if (ao_cmd_status != ao_cmd_success)
 396                 return;
 397         ao_bmx160_spi_get();
 398         val = _ao_bmm150_reg_read(addr);
 399         ao_bmx160_spi_put();
 400         printf("Addr %02x val %02x\n", addr, val);
 401 }
 402
 403 static void
 404 ao_bmm150_write(void)
 405 {
 406         uint8_t addr;
 407         uint8_t val;
 408
 409         addr = ao_cmd_hex();
 410         if (ao_cmd_status != ao_cmd_success)
 411                 return;
 412         val = ao_cmd_hex();
 413         if (ao_cmd_status != ao_cmd_success)
 414                 return;
 415         printf("Addr %02x val %02x\n", addr, val);
 416         ao_bmx160_spi_get();
 417         _ao_bmm150_reg_write(addr, val);
 418         ao_bmx160_spi_put();
 419 }
 420
 421 #endif /* BMX160_TEST */
 422
 423 static const struct ao_cmds ao_bmx160_cmds[] = {
 424         { ao_bmx160_show,       "I\0Show BMX160 status" },
 425 #if BMX160_TEST
 426         { ao_bmx160_read,       "R <addr>\0Read BMX160 register" },
 427         { ao_bmx160_write,      "W <addr> <val>\0Write BMX160 register" },
 428         { ao_bmm150_read,       "M <addr>\0Read BMM150 register" },
 429         { ao_bmm150_write,      "N <addr> <val>\0Write BMM150 register" },
 430 #endif
 431         { 0, NULL }
 432 };
 433
 434 void
 435 ao_bmx160_init(void)
 436 {
 437         ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160");
 438
 439         ao_spi_init_cs(AO_BMX160_SPI_CS_PORT, (1 << AO_BMX160_SPI_CS_PIN));
 440
 441         /* Pretend to be the bmx160 task. Grab the SPI bus right away and
 442          * hold it for the task so that nothing else uses the SPI bus before
 443          * we get the I2C mode disabled in the chip
 444          */
 445
 446         ao_cur_task = &ao_bmx160_task;
 447         ao_bmx160_spi_get();
 448         ao_cur_task = NULL;
 449         ao_cmd_register(&ao_bmx160_cmds[0]);
 450 }