150 lines
4.5 KiB
C
150 lines
4.5 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 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_WHO_AM_I_VALUE 0x44u
|
|
|
|
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_select();
|
|
delay_us(10u);
|
|
(void)bsp_pmod_spi_xfer8(reg & 0x7fu);
|
|
delay_us(10u);
|
|
(void)bsp_pmod_spi_xfer8(value);
|
|
delay_us(10u);
|
|
bsp_pmod_spi_deselect();
|
|
delay_us(10u);
|
|
}
|
|
|
|
static void icm_read_regs(uint8_t reg, uint8_t *data, unsigned len)
|
|
{
|
|
bsp_pmod_spi_select();
|
|
delay_us(10u);
|
|
(void)bsp_pmod_spi_xfer8(reg | 0x80u);
|
|
delay_us(10u);
|
|
for (unsigned i = 0; i < len; ++i) {
|
|
data[i] = bsp_pmod_spi_xfer8(0xffu);
|
|
delay_us(10u);
|
|
}
|
|
bsp_pmod_spi_deselect();
|
|
delay_us(10u);
|
|
}
|
|
|
|
static uint8_t icm_read_reg(uint8_t reg)
|
|
{
|
|
uint8_t value;
|
|
icm_read_regs(reg, &value, 1u);
|
|
return value;
|
|
}
|
|
|
|
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];
|
|
|
|
printf("\nICM-42686-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);
|
|
delay_us(1000u);
|
|
|
|
printf("WHO_AM_I before SPI_MODE write:");
|
|
for (unsigned attempt = 0; attempt < 4u; ++attempt) {
|
|
printf(" %02x", icm_read_reg(ICM_WHO_AM_I));
|
|
}
|
|
printf("\n");
|
|
|
|
/*
|
|
* The FPGA master is CPOL=0/CPHA=1. ICM-42686-P powers up accepting
|
|
* modes 0/3, so first switch DEVICE_CONFIG.SPI_MODE to modes 1/2.
|
|
* A mode-1-master/mode-0-slave write is valid because the slave samples
|
|
* MOSI on the rising edge, before the master changes it on the falling edge.
|
|
*/
|
|
icm_write_reg(ICM_DEVICE_CONFIG, 0x10u);
|
|
delay_us(100u);
|
|
|
|
const uint8_t device_config = icm_read_reg(ICM_DEVICE_CONFIG);
|
|
const uint8_t who_am_i = icm_read_reg(ICM_WHO_AM_I);
|
|
printf("DEVICE_CONFIG=0x%02x (expected 0x10)\n", device_config);
|
|
printf("WHO_AM_I=0x%02x (expected 0x%02x): %s\n",
|
|
who_am_i, ICM_WHO_AM_I_VALUE,
|
|
who_am_i == ICM_WHO_AM_I_VALUE ? "PASS" : "FAIL");
|
|
|
|
if (who_am_i != ICM_WHO_AM_I_VALUE) {
|
|
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 (default +/-32g, +/-4000dps)\n");
|
|
|
|
for (unsigned sample = 0; sample < 40u; ++sample) {
|
|
const uint8_t status = icm_read_reg(ICM_INT_STATUS);
|
|
icm_read_regs(ICM_TEMP_DATA1, 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 * 32000) / 32768);
|
|
const long ay_mg = (long)(((int64_t)ay * 32000) / 32768);
|
|
const long az_mg = (long)(((int64_t)az * 32000) / 32768);
|
|
const long gx_mdps = (long)(((int64_t)gx * 4000000) / 32768);
|
|
const long gy_mdps = (long)(((int64_t)gy * 4000000) / 32768);
|
|
const long gz_mdps = (long)(((int64_t)gz * 4000000) / 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("ICM-42686-P read complete\n");
|
|
bsp_uart_wait_tx_idle();
|
|
return 0;
|
|
}
|