IMU Accelerometer & Gyroscope

View as Markdown

1#!/usr/bin/env python3
2import depthai as dai
3
4
5def timeDeltaToMilliS(delta) -> float:
6    return delta.total_seconds()*1000
7
8
9# Create pipeline
10with dai.Pipeline() as pipeline:
11    # Define sources and outputs
12    imu = pipeline.create(dai.node.IMU)
13
14    # enable ACCELEROMETER_UNCALIBRATED at 500 hz rate
15    imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_UNCALIBRATED, 480)
16    # enable GYROSCOPE_UNCALIBRATED at 400 hz rate
17    imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_UNCALIBRATED, 400)
18    # it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
19    # above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
20    imu.setBatchReportThreshold(1)
21    # maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
22    # if lower or equal to batchReportThreshold then the sending is always blocking on device
23    # useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
24    imu.setMaxBatchReports(10)
25
26    imuQueue = imu.out.createOutputQueue(maxSize=50, blocking=False)
27
28    pipeline.start()
29    baseTs = None
30    while pipeline.isRunning():
31        try:
32            imuData = imuQueue.get()
33        except KeyboardInterrupt:
34            break
35        assert isinstance(imuData, dai.IMUData)
36        imuPackets = imuData.packets
37        for imuPacket in imuPackets:
38            acceleroValues = imuPacket.acceleroMeter
39            gyroValues = imuPacket.gyroscope
40
41            acceleroTs = acceleroValues.getTimestamp()
42            gyroTs = gyroValues.getTimestamp()
43
44            imuF = "{:.06f}"
45            tsF  = "{:.03f}"
46
47            print(f"Accelerometer timestamp: {acceleroTs}")
48            print(f"Latency [ms]: {dai.Clock.now() - acceleroValues.getTimestamp()}")
49            print(f"Accelerometer [m/s^2]: x: {imuF.format(acceleroValues.x)} y: {imuF.format(acceleroValues.y)} z: {imuF.format(acceleroValues.z)}")
50            print(f"Gyroscope timestamp: {gyroTs}")
51            print(f"Gyroscope [rad/s]: x: {imuF.format(gyroValues.x)} y: {imuF.format(gyroValues.y)} z: {imuF.format(gyroValues.z)} ")
52            print()

1#include <atomic>
2#include <chrono>
3#include <csignal>
4#include <iomanip>
5#include <iostream>
6#include <memory>
7
8#include "depthai/depthai.hpp"
9
10std::atomic<bool> quitEvent(false);
11
12void signalHandler(int) {
13    quitEvent = true;
14}
15
16// Helper function to convert time delta to milliseconds
17float timeDeltaToMilliS(const std::chrono::steady_clock::duration& delta) {
18    return std::chrono::duration_cast<std::chrono::milliseconds>(delta).count();
19}
20
21int main() {
22    signal(SIGTERM, signalHandler);
23    signal(SIGINT, signalHandler);
24
25    // Create pipeline
26    dai::Pipeline pipeline;
27
28    // Define sources and outputs
29    auto imu = pipeline.create<dai::node::IMU>();
30
31    // Enable ACCELEROMETER_UNCALIBRATED at 480 hz rate
32    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_UNCALIBRATED, 480);
33    // Enable GYROSCOPE_UNCALIBRATED at 400 hz rate
34    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_UNCALIBRATED, 400);
35
36    // Set batch report threshold and max batch reports
37    imu->setBatchReportThreshold(1);
38    imu->setMaxBatchReports(10);
39
40    // Create output queue
41    auto imuQueue = imu->out.createOutputQueue(50, false);
42
43    // Start pipeline
44    pipeline.start();
45    std::cout << "IMU pipeline started. Press Ctrl+C to stop." << std::endl;
46
47    // Set up output formatting
48    std::cout << std::fixed << std::setprecision(6);
49
50    while(pipeline.isRunning() && !quitEvent) {
51        auto imuData = imuQueue->get<dai::IMUData>();
52        if(imuData == nullptr) continue;
53
54        for(const auto& imuPacket : imuData->packets) {
55            auto acceleroValues = imuPacket.acceleroMeter;
56            auto gyroValues = imuPacket.gyroscope;
57
58            auto acceleroTs = acceleroValues.getTimestamp();
59            auto gyroTs = gyroValues.getTimestamp();
60
61            // Print accelerometer data
62            std::cout << "Accelerometer timestamp: " << acceleroTs.time_since_epoch().count() << std::endl;
63            std::cout << "Latency [ms]: " << timeDeltaToMilliS(std::chrono::steady_clock::now() - acceleroValues.getTimestamp()) << std::endl;
64            std::cout << "Accelerometer [m/s^2]: x: " << acceleroValues.x << " y: " << acceleroValues.y << " z: " << acceleroValues.z << std::endl;
65
66            // Print gyroscope data
67            std::cout << "Gyroscope timestamp: " << gyroTs.time_since_epoch().count() << std::endl;
68            std::cout << "Gyroscope [rad/s]: x: " << gyroValues.x << " y: " << gyroValues.y << " z: " << gyroValues.z << std::endl;
69        }
70    }
71
72    // Cleanup
73    pipeline.stop();
74    pipeline.wait();
75
76    return 0;
77}