From d0c5a8047abdb86f32f1d40a993345fd15a44395 Mon Sep 17 00:00:00 2001
From: Luca Erbetta <luca.erbetta@skywarder.eu>
Date: Sat, 7 Mar 2020 16:49:54 +0100
Subject: [PATCH] [L3GD20] better timestamp measurement
---
src/shared/sensors/L3GD20.h | 230 +++++++++++++++++------
src/tests/drivers/test-l3gd20-fifo.cpp | 244 +++++++++++++++++++++++++
src/tests/drivers/test-l3gd20.cpp | 188 ++++++++-----------
3 files changed, 493 insertions(+), 169 deletions(-)
create mode 100644 src/tests/drivers/test-l3gd20-fifo.cpp
diff --git a/src/shared/sensors/L3GD20.h b/src/shared/sensors/L3GD20.h
index 9133d5b50..65e47100a 100644
--- a/src/shared/sensors/L3GD20.h
+++ b/src/shared/sensors/L3GD20.h
@@ -1,3 +1,4 @@
+
/**
* Copyright (c) 2019 Skyward Experimental Rocketry
* Authors: Luca Erbetta
@@ -32,6 +33,12 @@
using miosix::GpioPin;
using std::array;
+struct L3GD20Data
+{
+ uint64_t timestamp;
+ Vec3 gyro;
+};
+
class L3GD20 : public GyroSensor
{
public:
@@ -44,11 +51,12 @@ public:
enum class OutPutDataRate
{
- ODR_95 = 0x00,
- ODR_190 = 0x01,
- ODR_380 = 0x02,
- ODR_760 = 0x03
+ ODR_95 = 95,
+ ODR_190 = 190,
+ ODR_380 = 380,
+ ODR_760 = 760
};
+
/**
* @brief Creates an instance of an L3GD20 sensor
*
@@ -57,21 +65,57 @@ public:
* @param range Full Scale Range (See datasheet)
* @param odr Output Data Rate (See datasheet)
* @param cutoff_freq Low pass filter cutoff frequency (See datasheet)
- * @param fifo_enabled Fifo enabled
- * @param fifo_watermark FIFO watermark level in range [1,32] (used for
- * interrupt generation, see datasheet).
*/
L3GD20(SPIBusInterface& bus, GpioPin cs,
FullScaleRange range = FullScaleRange::FS_250,
OutPutDataRate odr = OutPutDataRate::ODR_95,
- uint8_t cutoff_freq = 0x03, bool fifo_enabled = false,
- unsigned int fifo_watermark = 24)
- : fifo_enabled(fifo_enabled), fifo_watermark(fifo_watermark),
- spislave(bus, cs), fs(range), odr(odr), cutoff_freq(cutoff_freq)
+ uint8_t cutoff_freq = 0x03)
+ : L3GD20(bus, cs, {}, range, odr, cutoff_freq)
{
// Configure SPI
- spislave.config.br = SPIBaudRate::DIV_64;
- // memset(last_fifo, 0, sizeof(Vec3) * 32);
+ spislave.config.br = SPIBaudRate::DIV_32;
+ }
+
+ /**
+ * @brief Creates an instance of an L3GD20 sensor
+ *
+ * @param bus SPI bus the sensor is connected to
+ * @param cs Chip Select GPIO
+ * @param cfg Custom SPI bus configuration
+ * @param range Full Scale Range (See datasheet)
+ * @param odr Output Data Rate (See datasheet)
+ * @param cutoff_freq Low pass filter cutoff selector (See datasheet)
+ */
+ L3GD20(SPIBusInterface& bus, GpioPin cs, SPIBusConfig cfg,
+ FullScaleRange range = FullScaleRange::FS_250,
+ OutPutDataRate odr = OutPutDataRate::ODR_95,
+ uint8_t cutoff_freq = 0x03)
+ : spislave(bus, cs, cfg), fs(range), odr(odr), cutoff_freq(cutoff_freq)
+ {
+ switch (fs)
+ {
+ case FullScaleRange::FS_250:
+ sensitivity = SENSITIVITY_250;
+ break;
+ case FullScaleRange::FS_500:
+ sensitivity = SENSITIVITY_500;
+ break;
+ case FullScaleRange::FS_2000:
+ sensitivity = SENSITIVITY_2000;
+ break;
+ }
+ }
+
+ /**
+ * @brief Enables storing samples in a FIFO, must be called before init().
+ *
+ * @param fifo_watermark How many samples in the FIFO when the fifo
+ * watermark input is generated on INT2.
+ */
+ void enableFifo(unsigned int fifo_watermark)
+ {
+ fifo_enabled = true;
+ this->fifo_watermark = fifo_watermark;
}
bool init()
@@ -87,8 +131,6 @@ public:
return false;
}
- // uint8_t ctrl4 = spi.read(REG_CTRL4);
-
switch (fs)
{
case FullScaleRange::FS_250:
@@ -103,6 +145,7 @@ public:
default:
break;
}
+
if (fifo_enabled)
{
// Enable fifo
@@ -111,23 +154,42 @@ public:
// Set watermark level to fifo_watermark samples
uint8_t fifo_ctrl = fifo_watermark;
- // Set fifo to FIFO mode
+ // Set fifo to STREAM mode
fifo_ctrl |= 0x02 << 5;
spi.write(REG_FIFO_CTRL, fifo_ctrl);
- // Enable FIFO watermark interrupt
+ // Enable FIFO watermark interrupt on INT2
spi.write(REG_CTRL3, 0x04);
}
+ else
+ {
+ // Enable DRDY interrupt on INT2
+ spi.write(REG_CTRL3, 0x08);
+ }
+
// Enter normal mode, enable output
uint8_t ctrl1 = 0x0F;
- // Configure ODR
- ctrl1 |= static_cast<uint8_t>(odr) << 6;
-
// Configure cutoff frequency
ctrl1 |= (cutoff_freq & 0x03) << 4;
+ // Configure ODR
+ switch (odr)
+ {
+ case OutPutDataRate::ODR_95:
+ break;
+ case OutPutDataRate::ODR_190:
+ ctrl1 |= 1 << 6;
+ break;
+ case OutPutDataRate::ODR_380:
+ ctrl1 |= 2 << 6;
+ break;
+ case OutPutDataRate::ODR_760:
+ ctrl1 |= 3 << 6;
+ break;
+ }
+
spi.write(REG_CTRL1, ctrl1);
return true;
@@ -137,75 +199,123 @@ public:
bool onSimpleUpdate()
{
- float scale = static_cast<int>(fs);
-
- if (!fifo_enabled)
+ if (!fifo_enabled) // FIFO not enabled
{
- uint8_t data[6];
// Read output data registers (X, Y, Z)
{
SPITransaction spi(spislave);
- spi.read(REG_OUT_X_L | 0x40, data, 6);
+ spi.read(REG_OUT_X_L | 0x40, buf, 6);
}
- int16_t x = data[0] | data[1] << 8;
- int16_t y = data[2] | data[3] << 8;
- int16_t z = data[4] | data[5] << 8;
+ int16_t x = buf[0] | buf[1] << 8;
+ int16_t y = buf[2] | buf[3] << 8;
+ int16_t z = buf[4] | buf[5] << 8;
// printf("%02X,%02X,%02X\n", x, y, z);
- mLastGyro =
- Vec3(x * scale / 65535, y * scale / 65535, z * scale / 65535);
+ last_fifo[0] = {interrupt_us, toRadiansPerSecond(x, y, z)};
}
+
else // FIFO is enabled
{
- uint8_t buf[192];
-
SPITransaction spi(spislave);
// Read last fifo level
- uint8_t fifo_src = spi.read(REG_FIFO_SRC);
- uint8_t ovr = (fifo_src & 0x40);
- last_fifo_level = fifo_src & 0x1F;
-
- // if ovr --> fifo is full --> level = level + 1
- if (ovr)
- {
- ++last_fifo_level;
- }
+ uint8_t fifo_src = spi.read(REG_FIFO_SRC);
+ uint8_t ovr = (fifo_src & 0x40) >> 7; // Overrun bit
+ uint8_t fifo_level = (fifo_src & 0x1F) + ovr;
// Read fifo
- spi.read(REG_OUT_X_L | 0x40, buf, last_fifo_level * 6);
-
- // Convert units & store the FIFO content
- for (uint8_t i = 0; i < last_fifo_level; i++)
- {
- int16_t x = buf[i * 6] | buf[i * 6 + 1] << 8;
- int16_t y = buf[i * 6 + 2] | buf[i * 6 + 3] << 8;
- int16_t z = buf[i * 6 + 4] | buf[i * 6 + 5] << 8;
+ spi.read(REG_OUT_X_L | 0x40, buf, fifo_level * 6);
- Vec3 t = Vec3(x * scale / 65535, y * scale / 65535,
- z * scale / 65535);
+ uint64_t dt = dt_interrupt / last_fifo_level;
- last_fifo[i] = t;
+ uint8_t duplicates = 0;
+ for (uint8_t i = 0; i < fifo_level; ++i)
+ {
+ bool rmv;
+ // Check for duplicates: there seems to be a bug where the
+ // sensor occasionaly shifts out the same sample two times in a
+ // row: discard one
+ if (i < fifo_level - 1)
+ {
+ rmv = true;
+ for (uint8_t j = 0; j < 6; ++j)
+ {
+ if (buf[i * 6 + j] != buf[(i + 1) * 6 + j])
+ {
+ rmv = false;
+ break;
+ }
+ }
+ }
+ else
+ {
+ rmv = false;
+ }
+
+ if (rmv)
+ {
+ // Skip this sample;
+ ++duplicates;
+ continue;
+ }
+
+ // Assign a timestamp to each sample in the FIFO
+ // Samples before the watermark are older, after the
+ // watermark are younger.
+ last_fifo[i - duplicates].timestamp =
+ interrupt_us +
+ ((int)i - (int)fifo_watermark - (int)duplicates) * dt;
+ last_fifo[i - duplicates].gyro =
+ toRadiansPerSecond(buf[i * 6] | buf[i * 6 + 1] << 8,
+ buf[i * 6 + 2] | buf[i * 6 + 3] << 8,
+ buf[i * 6 + 4] | buf[i * 6 + 5] << 8);
}
+
+ last_fifo_level = fifo_level - duplicates;
}
return true;
}
- const array<Vec3, 32>& getLastFifo() const { return last_fifo; }
+ L3GD20Data getLastSample() { return last_fifo[last_fifo_level - 1]; }
+
+ void IRQupdateTimestamp(uint64_t ts)
+ {
+ dt_interrupt = ts - interrupt_us;
+ interrupt_us = ts;
+ }
+
+ const array<L3GD20Data, 32>& getLastFifo() const { return last_fifo; }
uint8_t getLastFifoSize() const { return last_fifo_level; }
private:
- bool fifo_enabled = false;
- unsigned int fifo_watermark;
+ Vec3 toRadiansPerSecond(int16_t x, int16_t y, int16_t z)
+ {
+ return Vec3(x * sensitivity, y * sensitivity, z * sensitivity);
+ }
- array<Vec3, 32> last_fifo;
- uint8_t last_fifo_level = 0;
+ static constexpr float SENSITIVITY_250 = 0.00875f;
+ static constexpr float SENSITIVITY_500 = 0.0175f;
+ static constexpr float SENSITIVITY_2000 = 0.070f;
SPISlave spislave;
- FullScaleRange fs;
- OutPutDataRate odr;
- uint8_t cutoff_freq;
+
+ FullScaleRange fs = FullScaleRange::FS_250;
+ OutPutDataRate odr = OutPutDataRate::ODR_760;
+ uint8_t cutoff_freq = 0x03;
+
+ bool fifo_enabled = false;
+ unsigned int fifo_watermark = 24;
+
+ float sensitivity = SENSITIVITY_250;
+
+ uint64_t interrupt_us = 0;
+ uint64_t dt_interrupt = 0;
+
+ array<L3GD20Data, 32> last_fifo;
+ uint8_t last_fifo_level = 1;
+
+ uint8_t buf[192];
constexpr static uint8_t WHO_AM_I_VAL = 212;
diff --git a/src/tests/drivers/test-l3gd20-fifo.cpp b/src/tests/drivers/test-l3gd20-fifo.cpp
new file mode 100644
index 000000000..aa3149c1a
--- /dev/null
+++ b/src/tests/drivers/test-l3gd20-fifo.cpp
@@ -0,0 +1,244 @@
+/**
+ * Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Luca Erbetta
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <array>
+
+#include "diagnostic/CpuMeter.h"
+#include "drivers/HardwareTimer.h"
+#include "drivers/spi/SPIDriver.h"
+#include "sensors/L3GD20-newt.h"
+
+using namespace miosix;
+using std::array;
+
+typedef Gpio<GPIOF_BASE, 7> GpioSck;
+typedef Gpio<GPIOF_BASE, 8> GpioMiso;
+typedef Gpio<GPIOF_BASE, 9> GpioMosi;
+
+typedef Gpio<GPIOA_BASE, 2> GpioINT2;
+
+static constexpr unsigned int FIFO_WATERMARK = 12;
+
+// Expected frequency from the datasheet is 760 Hz, but due to clock
+// misalignment / temperature errors and other factors, the observed clock (and
+// output data rate) is the following:
+static constexpr float SAMPLE_FREQUENCY = 782.3f;
+static constexpr int NUM_SAMPLES = SAMPLE_FREQUENCY * 120;
+
+// FD
+void enableInterrupt();
+void configure();
+
+struct GyroSample
+{
+ int fifo_num;
+ L3GD20Data gyro;
+ int level;
+ uint64_t wtm_delta;
+ float cpu;
+ uint64_t update;
+};
+
+// GyroSample data[NUM_SAMPLES];
+GyroSample data[NUM_SAMPLES];
+int data_counter = 0;
+
+// High resolution hardware timer using TIM5
+HardwareTimer<uint32_t> hrclock{
+ TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
+
+// Last interrupt received timer tick
+volatile uint32_t last_watermark_tick; // Stores the high-res tick of the last
+ // interrupt (L3GD20 watermark event)
+volatile uint32_t watermark_delta; // Tick delta between the last 2 watermark
+ // events
+
+// L3GD20 SPI
+SPIBus bus(SPI5);
+GpioPin cs(GPIOC_BASE, 1);
+
+L3GD20* gyro = nullptr;
+
+// Interrupt handlers
+void __attribute__((naked)) EXTI2_IRQHandler()
+{
+ saveContext();
+ asm volatile("bl _Z20EXTI2_IRQHandlerImplv");
+ restoreContext();
+}
+
+void __attribute__((used)) EXTI2_IRQHandlerImpl()
+{
+ // Current high resolution tick
+ uint32_t tick = hrclock.tick();
+ watermark_delta = tick - last_watermark_tick;
+ last_watermark_tick = tick;
+ // Pass tick microseconds to sensor
+ if(gyro != nullptr)
+ gyro->IRQupdateTimestamp(hrclock.toIntMicroSeconds(tick));
+
+ EXTI->PR |= EXTI_PR_PR2; // Reset pending register
+}
+
+int main()
+{
+ Thread::sleep(5000);
+
+ configure();
+
+ // Setup sensor
+ gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250,
+ L3GD20::OutPutDataRate::ODR_760, 0x03);
+
+ gyro->enableFifo(FIFO_WATERMARK);
+
+ // Init
+ while (!gyro->init())
+ {
+ }
+
+ // Sample NUM_SAMPLES data
+ int fifo_num = 0;
+ while (data_counter < NUM_SAMPLES)
+ {
+ long last_tick = miosix::getTick();
+
+ // Read the fifo
+ uint32_t update = hrclock.tick();
+ gyro->onSimpleUpdate();
+ update = hrclock.tick() - update;
+
+ uint8_t level =
+ gyro->getLastFifoSize(); // Number of samples in the FIFO
+
+ // Get the FIFO
+ const array<L3GD20Data, 32>& fifo = gyro->getLastFifo();
+
+ // Store everything in the data buffer
+ for (int i = 0; i < level; i++)
+ {
+ // data[data_counter++] = fifo[i];
+ data[data_counter++] = {fifo_num,
+ fifo[i],
+ level,
+ hrclock.toIntMicroSeconds(watermark_delta),
+ averageCpuUtilization(),
+ hrclock.toIntMicroSeconds(update)};
+
+ // Stop if we have enough data
+ if (data_counter >= NUM_SAMPLES)
+ {
+ break;
+ }
+ }
+ ++fifo_num;
+
+ // Wait until fifo has about 25 samples
+ Thread::sleepUntil(last_tick + 25.5 * 1000 / SAMPLE_FREQUENCY);
+ }
+
+ // Dump buffer content as CSV on the serial (might take a while)
+ // printf("t,x,y,z\n");
+ printf("FIFO_num,timestamp,int_delta,read_time,sample_delta,x,y,z,cpu\n");
+
+ for (int i = 1; i < data_counter; i++)
+ {
+ // clang-format off
+ printf("%d,%llu,%llu,%llu,%llu,%f,%f,%f,%.2f\n",
+ data[i].fifo_num,
+ data[i].gyro.timestamp,
+ data[i].wtm_delta,
+ data[i].update,
+ (data[i].gyro.timestamp - data[i - 1].gyro.timestamp),
+ data[i].gyro.gyro.getX(),
+ data[i].gyro.gyro.getY(),
+ data[i].gyro.gyro.getZ(),
+ data[i].cpu);
+ // clang-format on
+ }
+
+ printf("\n\n\nend.\n");
+ for (;;)
+ {
+ Thread::sleep(1000);
+ }
+}
+
+void configure()
+{
+ {
+ FastInterruptDisableLock dLock;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+ RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+
+ GpioSck::mode(Mode::ALTERNATE);
+ GpioMiso::mode(Mode::ALTERNATE);
+ GpioMosi::mode(Mode::ALTERNATE);
+
+ GpioSck::alternateFunction(5);
+ GpioMiso::alternateFunction(5);
+ GpioMosi::alternateFunction(5);
+
+ // Interrupt
+ GpioINT2::mode(Mode::INPUT_PULL_DOWN);
+
+ cs.mode(Mode::OUTPUT);
+ }
+
+ cs.high();
+
+ enableInterrupt();
+
+ // High resolution clock configuration
+ hrclock.setPrescaler(382);
+ hrclock.start();
+}
+
+void enableInterrupt()
+{
+ {
+ FastInterruptDisableLock l;
+ RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
+ }
+ // Refer to the datasheet for a detailed description on the procedure and
+ // interrupt registers
+
+ // Clear the mask on the wanted line
+ EXTI->IMR |= EXTI_IMR_MR2;
+
+ // Trigger the interrupt on a falling edge
+ // EXTI->FTSR |= EXTI_FTSR_TR2;
+
+ // Trigger the interrupt on a rising edge
+ EXTI->RTSR |= EXTI_RTSR_TR2;
+
+ EXTI->PR |= EXTI_PR_PR2; // Reset pending register
+
+ // Enable interrupt on PA2 in SYSCFG
+ SYSCFG->EXTICR[0] &= 0xFFFFF0FF;
+
+ // // Enable the interrput in the interrupt controller
+ NVIC_EnableIRQ(EXTI2_IRQn);
+ NVIC_SetPriority(EXTI2_IRQn, 15);
+}
\ No newline at end of file
diff --git a/src/tests/drivers/test-l3gd20.cpp b/src/tests/drivers/test-l3gd20.cpp
index f5a63718c..e37d13c76 100644
--- a/src/tests/drivers/test-l3gd20.cpp
+++ b/src/tests/drivers/test-l3gd20.cpp
@@ -37,28 +37,20 @@ typedef Gpio<GPIOF_BASE, 9> GpioMosi;
typedef Gpio<GPIOA_BASE, 2> GpioINT2;
-static constexpr bool FIFO_ENABLED = true;
-static constexpr unsigned int FIFO_WATERMARK = 24;
-// Expected frequency from the datasheet is 760 Hz, but due to clock
-// misalignment / temperature errors and other factors, the observed clock (and
-// output data rate) is the following:
-static constexpr float SAMPLE_FREQUENCY = 782.3f;
-static constexpr int NUM_SAMPLES = 10000;
+static constexpr int NUM_SAMPLES = 500 * 5; // 120 seconds of data
void enableInterrupt();
+void configure();
struct GyroSample
{
- int fifo_num;
- float timestamp;
+ uint64_t timestamp;
+ uint64_t sample_delta;
Vec3 data;
- int level;
- float wtm_delta;
float cpu;
};
-// 364 KB buffer to store up to 30 seconds of data @ 760 Hz
-GyroSample data[22800];
+GyroSample data[NUM_SAMPLES];
int data_counter = 0;
// High resolution hardware timer using TIM5
@@ -66,16 +58,17 @@ HardwareTimer<uint32_t> hrclock{
TIM5, TimerUtils::getPrescalerInputFrequency(TimerUtils::InputClock::APB1)};
// Last interrupt received timer tick
-volatile uint32_t last_watermark_tick; // Stores the high-res tick of the last
- // interrupt (L3GD20 watermark event)
-volatile uint32_t watermark_delta; // Tick delta between the last 2 watermark
- // events
+volatile uint32_t last_sample_tick; // Stores the high-res tick of the last
+ // interrupt (L3GD20 watermark event)
+volatile uint32_t sample_delta; // Tick delta between the last 2 watermark
+ // events
// L3GD20 SPI
SPIBus bus(SPI5);
GpioPin cs(GPIOC_BASE, 1);
SPIBusConfig cfg;
-uint8_t buf[192];
+
+L3GD20* gyro;
// Interrupt handlers
void __attribute__((naked)) EXTI2_IRQHandler()
@@ -87,121 +80,66 @@ void __attribute__((naked)) EXTI2_IRQHandler()
void __attribute__((used)) EXTI2_IRQHandlerImpl()
{
- uint32_t tick = hrclock.tick();
- watermark_delta = tick - last_watermark_tick;
- last_watermark_tick = tick;
+ // Current high resolution tick
+ uint32_t tick = hrclock.tick();
+ sample_delta = tick - last_sample_tick;
+ last_sample_tick = tick;
+
+ // Pass tick microseconds to sensor for timestamp estimation
+ if (gyro != nullptr)
+ gyro->IRQupdateTimestamp(hrclock.toIntMicroSeconds(tick));
EXTI->PR |= EXTI_PR_PR2; // Reset pending register
}
int main()
{
- cfg.br = SPIBaudRate::DIV_64;
-
- {
- FastInterruptDisableLock dLock;
-
- RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
- RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
-
- GpioSck::mode(Mode::ALTERNATE);
- GpioMiso::mode(Mode::ALTERNATE);
- GpioMosi::mode(Mode::ALTERNATE);
+ Thread::sleep(5000);
- GpioSck::alternateFunction(5);
- GpioMiso::alternateFunction(5);
- GpioMosi::alternateFunction(5);
+ configure();
- // Interrupt
- GpioINT2::mode(Mode::INPUT_PULL_DOWN);
-
- cs.mode(Mode::OUTPUT);
- }
- // High resolution clock configuration
- // 1.8 hours run time, 1.5 us resolution
- hrclock.setPrescaler(127);
- hrclock.start();
+ cfg.br = SPIBaudRate::DIV_64;
- enableInterrupt();
- cs.high();
+ gyro = new L3GD20(bus, cs, L3GD20::FullScaleRange::FS_250,
+ L3GD20::OutPutDataRate::ODR_760, 0x03);
- L3GD20 gyro(bus, cs, L3GD20::FullScaleRange::FS_250,
- L3GD20::OutPutDataRate::ODR_760, 0x03, FIFO_ENABLED,
- FIFO_WATERMARK);
+ // gyro->setRealSampleInterval(
+ // static_cast<uint64_t>(1000000 / SAMPLE_FREQUENCY));
- while (!gyro.init())
+ while (!gyro->init())
{
}
Thread::sleep(500);
- long long first_tick = miosix::getTick();
-
- // Sample NUM_SAMPLES data
- int fifo_num = 0;
while (data_counter < NUM_SAMPLES)
{
long last_tick = miosix::getTick();
- if (FIFO_ENABLED)
- {
- // Precise timestamp of the last sample in the FIFO
- float wtm_timestamp = hrclock.toSeconds(last_watermark_tick);
- float wtm_delta = hrclock.toMilliSeconds(watermark_delta);
-
- // Read the fifo
- gyro.onSimpleUpdate();
-
- uint8_t level =
- gyro.getLastFifoSize(); // Number of samples in the FIFO
-
- const array<Vec3, 32>& fifo = gyro.getLastFifo();
-
- for (int i = 0; i < level; i++)
- {
- // Assign a timestamp to each sample in the FIFO
- // Samples before the watermark are older, after the watermark
- // are younger. Time delta between samples is
- // (1 / SAMPLE_FREQUENCY) seconds
- float ts = wtm_timestamp +
- (i - (int)FIFO_WATERMARK) / SAMPLE_FREQUENCY;
-
- data[data_counter++] = {fifo_num, ts,
- fifo[i], level,
- wtm_delta, averageCpuUtilization()};
- if (data_counter >= NUM_SAMPLES)
- {
- break;
- }
- }
- ++fifo_num;
-
- // Wait until fifo has about 25 samples
- Thread::sleepUntil(last_tick + 34);
- }
- else
- {
- // Sample sensor @ 500 Hz
- gyro.onSimpleUpdate();
-
- data[data_counter++] = {0,
- (last_tick - first_tick) / 1000.0f,
- *(gyro.gyroDataPtr()),
- 0,
- 0,
- averageCpuUtilization()};
- Thread::sleepUntil(last_tick + 2);
- }
+ // Sample sensor @ 500 Hz
+ gyro->onSimpleUpdate();
+ L3GD20Data d = gyro->getLastSample();
+
+ data[data_counter++] = {d.timestamp, sample_delta, d.gyro,
+ averageCpuUtilization()};
+
+ Thread::sleepUntil(last_tick + 1);
}
// Dump buffer content as CSV on the serial (might take a while)
- printf("FIFO_num,timestamp,watermark_delta,sample_delta,x,y,z,level,cpu");
+ printf("FIFO_num,timestamp,int_delta,sample_delta,x,y,z,cpu\n");
for (int i = 1; i < data_counter; i++)
{
- printf("%d,%.3f,%.3f,%.3f,%f,%f,%f,%d,%.2f\n", data[i].fifo_num,
- data[i].timestamp * 1000, data[i].wtm_delta,
- (data[i].timestamp - data[i - 1].timestamp) * 1000,
- data[i].data.getX(), data[i].data.getY(), data[i].data.getZ(),
- data[i].level, data[i].cpu);
+ // clang-format off
+ printf("%d,%llu,%llu,%llu,%f,%f,%f,%.2f\n",
+ 0,
+ data[i].timestamp,
+ hrclock.toIntMicroSeconds(data[i].sample_delta),
+ (data[i].timestamp - data[i - 1].timestamp),
+ data[i].data.getX(),
+ data[i].data.getY(),
+ data[i].data.getZ(),
+ data[i].cpu);
+ // clang-format on
}
printf("\n\n\nend.\n");
@@ -211,6 +149,38 @@ int main()
}
}
+void configure()
+{
+ {
+ FastInterruptDisableLock dLock;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
+ RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+
+ GpioSck::mode(Mode::ALTERNATE);
+ GpioMiso::mode(Mode::ALTERNATE);
+ GpioMosi::mode(Mode::ALTERNATE);
+
+ GpioSck::alternateFunction(5);
+ GpioMiso::alternateFunction(5);
+ GpioMosi::alternateFunction(5);
+
+ // Interrupt
+ GpioINT2::mode(Mode::INPUT_PULL_DOWN);
+
+ cs.mode(Mode::OUTPUT);
+ }
+
+ cs.high();
+
+ enableInterrupt();
+
+ // High resolution clock configuration
+ // 1.8 hours run time, 1.5 us resolution
+ hrclock.setPrescaler(127);
+ hrclock.start();
+}
+
void enableInterrupt()
{
{
--
GitLab