261 lines
8.4 KiB
C
261 lines
8.4 KiB
C
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
#include "bsp/irq.h"
|
|
#include "bsp/mmio.h"
|
|
#include "bsp/plic.h"
|
|
#include "bsp/soc.h"
|
|
#include "bsp/uart.h"
|
|
#include "lsm6dsox.h"
|
|
|
|
#ifndef LSM6DSOX_ODR_HZ
|
|
#define LSM6DSOX_ODR_HZ 104u
|
|
#endif
|
|
|
|
#ifndef CAPTURE_DURATION_S
|
|
#define CAPTURE_DURATION_S 60u
|
|
#endif
|
|
|
|
#ifndef LSM6DSOX_IRQ_QUEUE_CAPACITY
|
|
#define LSM6DSOX_IRQ_QUEUE_CAPACITY 256u
|
|
#endif
|
|
|
|
#if (LSM6DSOX_IRQ_QUEUE_CAPACITY < 2u)
|
|
#error "LSM6DSOX_IRQ_QUEUE_CAPACITY must be at least 2"
|
|
#endif
|
|
|
|
typedef struct {
|
|
lsm6dsox_sample_t sample;
|
|
uint32_t sequence;
|
|
} queued_sample_t;
|
|
|
|
static queued_sample_t sample_queue[LSM6DSOX_IRQ_QUEUE_CAPACITY];
|
|
static volatile unsigned sample_head;
|
|
static volatile unsigned sample_tail;
|
|
static volatile uint64_t interrupt_count;
|
|
static volatile uint64_t interrupt_sample_count;
|
|
static volatile uint64_t interrupt_read_fail_count;
|
|
static volatile uint64_t queue_overflow_count;
|
|
static volatile uint64_t unexpected_irq_count;
|
|
static volatile uint64_t unexpected_trap_cause;
|
|
|
|
void sensor_irq_handler(void)
|
|
__attribute__((interrupt("machine"), aligned(4)));
|
|
|
|
static uint64_t time_now(void)
|
|
{
|
|
return bsp_read64(BSP_CLINT_MTIME);
|
|
}
|
|
|
|
static unsigned next_queue_index(unsigned index)
|
|
{
|
|
index++;
|
|
return (index == LSM6DSOX_IRQ_QUEUE_CAPACITY) ? 0u : index;
|
|
}
|
|
|
|
static void queue_push_isr(const lsm6dsox_sample_t *sample,
|
|
uint32_t sequence)
|
|
{
|
|
unsigned head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
|
|
const unsigned next = next_queue_index(head);
|
|
if (next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE)) {
|
|
__atomic_fetch_add(&queue_overflow_count, 1u, __ATOMIC_RELAXED);
|
|
return;
|
|
}
|
|
sample_queue[head].sample = *sample;
|
|
sample_queue[head].sequence = sequence;
|
|
__atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
|
|
}
|
|
|
|
static int queue_pop(queued_sample_t *queued)
|
|
{
|
|
unsigned tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
|
|
if (tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE)) {
|
|
return 0;
|
|
}
|
|
*queued = sample_queue[tail];
|
|
__atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
|
|
return 1;
|
|
}
|
|
|
|
void sensor_irq_handler(void)
|
|
{
|
|
asm volatile(
|
|
".option push\n"
|
|
".option norelax\n"
|
|
"la gp, __global_pointer$\n"
|
|
".option pop\n"
|
|
: : : "memory");
|
|
|
|
const uint64_t interrupt_timestamp = time_now();
|
|
const uint64_t mcause = bsp_read_mcause();
|
|
if (((mcause >> 63) == 0u) ||
|
|
((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL)) {
|
|
unexpected_trap_cause = mcause;
|
|
return;
|
|
}
|
|
|
|
const uint32_t irq_id = bsp_plic_claim();
|
|
if (irq_id == 0u) {
|
|
__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
|
|
return;
|
|
}
|
|
if (irq_id == BSP_PMOD_IRQ) {
|
|
__atomic_fetch_add(&interrupt_count, 1u, __ATOMIC_RELAXED);
|
|
lsm6dsox_sample_t sample;
|
|
if (lsm6dsox_read_interrupt_sample(&sample, interrupt_timestamp)) {
|
|
const uint64_t sample_number =
|
|
__atomic_fetch_add(&interrupt_sample_count, 1u, __ATOMIC_RELAXED);
|
|
queue_push_isr(&sample, (uint32_t)sample_number);
|
|
} else {
|
|
__atomic_fetch_add(&interrupt_read_fail_count, 1u, __ATOMIC_RELAXED);
|
|
}
|
|
} else {
|
|
__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
|
|
}
|
|
bsp_plic_complete(irq_id);
|
|
}
|
|
|
|
static void print_identity_probe(void)
|
|
{
|
|
printf("WHO_AM_I probes:");
|
|
for (unsigned i = 0u; i < 8u; ++i) {
|
|
printf(" %02x", lsm6dsox_read_who_am_i());
|
|
}
|
|
printf(" (expected 6c)\n");
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
printf("\nSTEVAL-MKI217V1 / LSM6DSOX INT1 data-ready test\n");
|
|
printf("Genesys2 JC: CS=JC1 SDA/MOSI=JC2 SDO/MISO=JC3 SCL/SCK=JC4\n");
|
|
printf("Power: VDD=3.3V VDDIO=3.3V common GND; sensor INT1=JB1/PLIC%lu\n",
|
|
(unsigned long)BSP_PMOD_IRQ);
|
|
printf("SPI MMIO=0x%08lx CLINT=%lu Hz UART=%lu baud\n",
|
|
(unsigned long)BSP_PMOD_SPI_BASE,
|
|
(unsigned long)BSP_CLINT_TIMEBASE_HZ,
|
|
(unsigned long)bsp_uart_baudrate());
|
|
|
|
if (!lsm6dsox_init(LSM6DSOX_ODR_HZ)) {
|
|
print_identity_probe();
|
|
printf("INIT FAIL: check CS, MOSI, MISO, SCK, 3.3V and common GND.\n");
|
|
printf("0x00 usually means MISO held low; 0xff usually means no selected device.\n");
|
|
bsp_uart_wait_tx_idle();
|
|
return 1;
|
|
}
|
|
|
|
print_identity_probe();
|
|
printf("CONFIG CTRL1_XL=%02x CTRL2_G=%02x CTRL3_C=%02x STATUS=%02x\n",
|
|
lsm6dsox_read_register(0x10u),
|
|
lsm6dsox_read_register(0x11u),
|
|
lsm6dsox_read_register(0x12u),
|
|
lsm6dsox_read_register(0x1eu));
|
|
|
|
sample_head = 0u;
|
|
sample_tail = 0u;
|
|
interrupt_count = 0u;
|
|
interrupt_sample_count = 0u;
|
|
interrupt_read_fail_count = 0u;
|
|
queue_overflow_count = 0u;
|
|
unexpected_irq_count = 0u;
|
|
unexpected_trap_cause = 0u;
|
|
bsp_plic_disable_all();
|
|
bsp_plic_set_threshold(0u);
|
|
bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
|
|
/* Recover the PMOD LevelGateway if a previous GDB-loaded image stopped
|
|
* after claim but before complete. The sensor route is still disabled. */
|
|
bsp_plic_complete(BSP_PMOD_IRQ);
|
|
bsp_set_mtvec(sensor_irq_handler);
|
|
bsp_plic_enable(BSP_PMOD_IRQ);
|
|
bsp_enable_machine_external_interrupts();
|
|
if (!lsm6dsox_enable_data_ready_interrupt()) {
|
|
printf("INT INIT FAIL: INT1_CTRL=%02x INT2_CTRL=%02x TAP_CFG2=%02x "
|
|
"COUNTER_BDR_REG1=%02x\n",
|
|
lsm6dsox_read_register(0x0du),
|
|
lsm6dsox_read_register(0x0eu),
|
|
lsm6dsox_read_register(0x58u),
|
|
lsm6dsox_read_register(0x0bu));
|
|
bsp_plic_disable(BSP_PMOD_IRQ);
|
|
lsm6dsox_power_down();
|
|
bsp_uart_wait_tx_idle();
|
|
return 2;
|
|
}
|
|
printf("INT_CONFIG,pin=INT1,INT1_CTRL=%02x,INT2_CTRL=%02x,"
|
|
"TAP_CFG2=%02x,COUNTER_BDR_REG1=%02x,mode=latched\n",
|
|
lsm6dsox_read_register(0x0du),
|
|
lsm6dsox_read_register(0x0eu),
|
|
lsm6dsox_read_register(0x58u),
|
|
lsm6dsox_read_register(0x0bu));
|
|
|
|
const lsm6dsox_scale_t *scale = lsm6dsox_scale();
|
|
printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu,"
|
|
"accel_ug_lsb=%lu,gyro_udps_lsb=%lu,temp_lsb_c=%lu,"
|
|
"temp_offset_millic=%ld\n",
|
|
(unsigned long)BSP_CLINT_TIMEBASE_HZ,
|
|
(unsigned long)scale->odr_hz,
|
|
(unsigned long)scale->accel_ug_per_lsb,
|
|
(unsigned long)scale->gyro_udps_per_lsb,
|
|
(unsigned long)scale->temperature_lsb_per_c,
|
|
(long)scale->temperature_offset_millic);
|
|
printf("IMU_COLUMNS,sensor,seq,tick,ax_raw,ay_raw,az_raw,"
|
|
"gx_raw,gy_raw,gz_raw,temp_raw\n");
|
|
|
|
const uint64_t start = time_now();
|
|
const uint64_t duration_ticks =
|
|
(uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S;
|
|
uint32_t samples = 0u;
|
|
uint32_t sequence_discontinuities = 0u;
|
|
uint32_t expected_sequence = 0u;
|
|
uint64_t previous_tick = 0u;
|
|
uint64_t maximum_gap = 0u;
|
|
|
|
while ((time_now() - start) < duration_ticks) {
|
|
/* This diagnostic must still time out and report zero IRQs when INT1 is
|
|
* disconnected. The production application can sleep on its other IRQs. */
|
|
asm volatile("nop");
|
|
queued_sample_t queued;
|
|
while (queue_pop(&queued)) {
|
|
const lsm6dsox_sample_t *sample = &queued.sample;
|
|
if (queued.sequence != expected_sequence) {
|
|
sequence_discontinuities++;
|
|
}
|
|
expected_sequence = queued.sequence + 1u;
|
|
|
|
if (previous_tick != 0u) {
|
|
const uint64_t gap = sample->timestamp_ticks - previous_tick;
|
|
if (gap > maximum_gap) {
|
|
maximum_gap = gap;
|
|
}
|
|
}
|
|
previous_tick = sample->timestamp_ticks;
|
|
|
|
printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n",
|
|
(unsigned long)queued.sequence,
|
|
(unsigned long)sample->timestamp_ticks,
|
|
(int)sample->accel[0], (int)sample->accel[1],
|
|
(int)sample->accel[2], (int)sample->gyro[0],
|
|
(int)sample->gyro[1], (int)sample->gyro[2],
|
|
(int)sample->temperature);
|
|
samples++;
|
|
}
|
|
}
|
|
|
|
bsp_plic_disable(BSP_PMOD_IRQ);
|
|
lsm6dsox_disable_data_ready_interrupt();
|
|
lsm6dsox_power_down();
|
|
printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu,"
|
|
"irq=%lu,irq_samples=%lu,read_fail=%lu,queue_overflow=%lu,"
|
|
"seq_discontinuities=%lu,unexpected_irq=%lu,trap=%lu\n",
|
|
(unsigned long)samples, (unsigned)CAPTURE_DURATION_S,
|
|
(unsigned long)maximum_gap,
|
|
(unsigned long)interrupt_count,
|
|
(unsigned long)interrupt_sample_count,
|
|
(unsigned long)interrupt_read_fail_count,
|
|
(unsigned long)queue_overflow_count,
|
|
(unsigned long)sequence_discontinuities,
|
|
(unsigned long)unexpected_irq_count,
|
|
(unsigned long)unexpected_trap_cause);
|
|
bsp_uart_wait_tx_idle();
|
|
return 0;
|
|
}
|