141 lines
4.6 KiB
C
141 lines
4.6 KiB
C
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
#include "bsp/mmio.h"
|
|
#include "bsp/pmod_spi.h"
|
|
#include "bsp/soc.h"
|
|
#include "bsp/uart.h"
|
|
|
|
/* ICM-42686-P/ICM-42688-P user bank 0 registers. */
|
|
#define ICM_DEVICE_CONFIG 0x11u
|
|
#define ICM_TEMP_DATA1 0x1du
|
|
#define ICM_INT_STATUS 0x2du
|
|
#define ICM_PWR_MGMT0 0x4eu
|
|
#define ICM_GYRO_CONFIG0 0x4fu
|
|
#define ICM_ACCEL_CONFIG0 0x50u
|
|
#define ICM_WHO_AM_I 0x75u
|
|
|
|
#define ICM_SPI_READ 0x80u
|
|
#define ICM_SPI_DUMMY 0xffu
|
|
|
|
#define ICM42686_WHO_AM_I 0x44u
|
|
#define ICM42688_WHO_AM_I 0x47u
|
|
|
|
static void delay_us(uint32_t us)
|
|
{
|
|
const uint64_t ticks =
|
|
((uint64_t)BSP_CLINT_TIMEBASE_HZ * us + 999999u) / 1000000u;
|
|
const uint64_t start = bsp_read64(BSP_CLINT_MTIME);
|
|
while ((bsp_read64(BSP_CLINT_MTIME) - start) < ticks) {
|
|
}
|
|
}
|
|
|
|
static void icm_write_reg(uint8_t reg, uint8_t value)
|
|
{
|
|
bsp_pmod_spi_write_reg8(reg & (uint8_t)~ICM_SPI_READ, value);
|
|
}
|
|
|
|
static uint8_t icm_read_reg(uint8_t reg)
|
|
{
|
|
return bsp_pmod_spi_read_reg8(reg | ICM_SPI_READ, ICM_SPI_DUMMY);
|
|
}
|
|
|
|
static int16_t be16(const uint8_t *p)
|
|
{
|
|
return (int16_t)(((uint16_t)p[0] << 8) | p[1]);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
uint8_t raw[14];
|
|
long accel_full_scale_mg;
|
|
long gyro_full_scale_mdps;
|
|
|
|
printf("\nICM-42686-P/ICM-42688-P PMOD SPI test\n");
|
|
printf("SPI MMIO: 0x%08lx, Genesys2 PMOD JC\n",
|
|
(unsigned long)BSP_PMOD_SPI_BASE);
|
|
printf("pins: CS=JC1 MOSI=JC2 MISO=JC3 SCK=JC4\n");
|
|
|
|
bsp_pmod_spi_init();
|
|
bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
|
|
bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u);
|
|
delay_us(1000u);
|
|
|
|
printf("WHO_AM_I reads:");
|
|
uint8_t who_am_i = 0u;
|
|
for (unsigned attempt = 0; attempt < 4u; ++attempt) {
|
|
who_am_i = icm_read_reg(ICM_WHO_AM_I);
|
|
printf(" %02x", who_am_i);
|
|
}
|
|
printf("\n");
|
|
|
|
/*
|
|
* Keep DEVICE_CONFIG.SPI_MODE at its reset value. Although the RTL shifts
|
|
* and samples on the falling edge, board propagation makes it interoperate
|
|
* with the sensor's default mode-0 timing. Switching the sensor to mode 1
|
|
* breaks this working timing relationship on the current bitstream.
|
|
*/
|
|
const uint8_t device_config = icm_read_reg(ICM_DEVICE_CONFIG);
|
|
printf("DEVICE_CONFIG=0x%02x (expected 0x00)\n", device_config);
|
|
|
|
if (who_am_i == ICM42686_WHO_AM_I) {
|
|
accel_full_scale_mg = 32000l;
|
|
gyro_full_scale_mdps = 4000000l;
|
|
printf("WHO_AM_I=0x%02x: ICM-42686-P PASS\n", who_am_i);
|
|
} else if (who_am_i == ICM42688_WHO_AM_I) {
|
|
accel_full_scale_mg = 16000l;
|
|
gyro_full_scale_mdps = 2000000l;
|
|
printf("WHO_AM_I=0x%02x: ICM-42688-P PASS\n", who_am_i);
|
|
} else {
|
|
printf("WHO_AM_I=0x%02x: FAIL (expected 0x44 or 0x47)\n", who_am_i);
|
|
printf("SPI communication failed. DK J4: CS=1 SCK=2 MOSI=3 MISO=4 GND=7.\n");
|
|
printf("Hold the DK SAMG55 in reset so it cannot contend for the sensor SPI bus.\n");
|
|
bsp_uart_wait_tx_idle();
|
|
return 1;
|
|
}
|
|
|
|
/* Enable accelerometer and gyroscope in low-noise mode. */
|
|
icm_write_reg(ICM_PWR_MGMT0, 0x0fu);
|
|
delay_us(50000u);
|
|
|
|
const uint8_t pwr = icm_read_reg(ICM_PWR_MGMT0);
|
|
const uint8_t gyro_cfg = icm_read_reg(ICM_GYRO_CONFIG0);
|
|
const uint8_t accel_cfg = icm_read_reg(ICM_ACCEL_CONFIG0);
|
|
printf("PWR_MGMT0=0x%02x GYRO_CONFIG0=0x%02x ACCEL_CONFIG0=0x%02x\n",
|
|
pwr, gyro_cfg, accel_cfg);
|
|
printf("units: temperature=mC accel=mg gyro=mdps\n");
|
|
|
|
for (unsigned sample = 0; sample < 40u; ++sample) {
|
|
const uint8_t status = icm_read_reg(ICM_INT_STATUS);
|
|
bsp_pmod_spi_read_regs(ICM_TEMP_DATA1 | ICM_SPI_READ,
|
|
ICM_SPI_DUMMY,
|
|
raw,
|
|
sizeof(raw));
|
|
|
|
const int16_t temp = be16(&raw[0]);
|
|
const int16_t ax = be16(&raw[2]);
|
|
const int16_t ay = be16(&raw[4]);
|
|
const int16_t az = be16(&raw[6]);
|
|
const int16_t gx = be16(&raw[8]);
|
|
const int16_t gy = be16(&raw[10]);
|
|
const int16_t gz = be16(&raw[12]);
|
|
|
|
const long temp_mc = 25000l + (long)(((int64_t)temp * 100000) / 13248);
|
|
const long ax_mg = (long)(((int64_t)ax * accel_full_scale_mg) / 32768);
|
|
const long ay_mg = (long)(((int64_t)ay * accel_full_scale_mg) / 32768);
|
|
const long az_mg = (long)(((int64_t)az * accel_full_scale_mg) / 32768);
|
|
const long gx_mdps = (long)(((int64_t)gx * gyro_full_scale_mdps) / 32768);
|
|
const long gy_mdps = (long)(((int64_t)gy * gyro_full_scale_mdps) / 32768);
|
|
const long gz_mdps = (long)(((int64_t)gz * gyro_full_scale_mdps) / 32768);
|
|
|
|
printf("%02u st=%02x T=%ld A=%ld,%ld,%ld G=%ld,%ld,%ld\n",
|
|
sample, status, temp_mc,
|
|
ax_mg, ay_mg, az_mg, gx_mdps, gy_mdps, gz_mdps);
|
|
delay_us(100000u);
|
|
}
|
|
|
|
printf("IMU read complete\n");
|
|
bsp_uart_wait_tx_idle();
|
|
return 0;
|
|
}
|