IMU Accelerometer & Gyroscope

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import time
6import math
7
8# Create pipeline
9pipeline = dai.Pipeline()
10
11# Define sources and outputs
12imu = pipeline.create(dai.node.IMU)
13xlinkOut = pipeline.create(dai.node.XLinkOut)
14
15xlinkOut.setStreamName("imu")
16
17# enable ACCELEROMETER_RAW at 500 hz rate
18imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 500)
19# enable GYROSCOPE_RAW at 400 hz rate
20imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 400)
21# it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
22# above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
23imu.setBatchReportThreshold(1)
24# maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
25# if lower or equal to batchReportThreshold then the sending is always blocking on device
26# useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
27imu.setMaxBatchReports(10)
28
29# Link plugins IMU -> XLINK
30imu.out.link(xlinkOut.input)
31
32# Pipeline is defined, now we can connect to the device
33with dai.Device(pipeline) as device:
34
35    def timeDeltaToMilliS(delta) -> float:
36        return delta.total_seconds()*1000
37
38    # Output queue for imu bulk packets
39    imuQueue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
40    baseTs = None
41    while True:
42        imuData = imuQueue.get()  # blocking call, will wait until a new data has arrived
43
44        imuPackets = imuData.packets
45        for imuPacket in imuPackets:
46            acceleroValues = imuPacket.acceleroMeter
47            gyroValues = imuPacket.gyroscope
48
49            acceleroTs = acceleroValues.getTimestampDevice()
50            gyroTs = gyroValues.getTimestampDevice()
51            if baseTs is None:
52                baseTs = acceleroTs if acceleroTs < gyroTs else gyroTs
53            acceleroTs = timeDeltaToMilliS(acceleroTs - baseTs)
54            gyroTs = timeDeltaToMilliS(gyroTs - baseTs)
55
56            imuF = "{:.06f}"
57            tsF  = "{:.03f}"
58
59            print(f"Accelerometer timestamp: {tsF.format(acceleroTs)} ms")
60            print(f"Accelerometer [m/s^2]: x: {imuF.format(acceleroValues.x)} y: {imuF.format(acceleroValues.y)} z: {imuF.format(acceleroValues.z)}")
61            print(f"Gyroscope timestamp: {tsF.format(gyroTs)} ms")
62            print(f"Gyroscope [rad/s]: x: {imuF.format(gyroValues.x)} y: {imuF.format(gyroValues.y)} z: {imuF.format(gyroValues.z)} ")
63
64        if cv2.waitKey(1) == ord('q'):
65            break

1#include <cstdio>
2#include <iostream>
3
4#include "utility.hpp"
5
6// Includes common necessary includes for development using depthai library
7#include "depthai/depthai.hpp"
8
9int main() {
10    using namespace std;
11    using namespace std::chrono;
12
13    // Create pipeline
14    dai::Pipeline pipeline;
15
16    // Define sources and outputs
17    auto imu = pipeline.create<dai::node::IMU>();
18    auto xlinkOut = pipeline.create<dai::node::XLinkOut>();
19
20    xlinkOut->setStreamName("imu");
21
22    // enable ACCELEROMETER_RAW at 500 hz rate
23    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
24    // enable GYROSCOPE_RAW at 400 hz rate
25    imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400);
26    // it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
27    // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
28    imu->setBatchReportThreshold(1);
29    // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
30    // if lower or equal to batchReportThreshold then the sending is always blocking on device
31    // useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
32    imu->setMaxBatchReports(10);
33
34    // Link plugins IMU -> XLINK
35    imu->out.link(xlinkOut->input);
36
37    // Pipeline is defined, now we can connect to the device
38    dai::Device d(pipeline);
39
40    bool firstTs = false;
41
42    auto imuQueue = d.getOutputQueue("imu", 50, false);
43    auto baseTs = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>();
44
45    while(true) {
46        auto imuData = imuQueue->get<dai::IMUData>();
47
48        auto imuPackets = imuData->packets;
49        for(auto& imuPacket : imuPackets) {
50            auto& acceleroValues = imuPacket.acceleroMeter;
51            auto& gyroValues = imuPacket.gyroscope;
52
53            auto acceleroTs1 = acceleroValues.getTimestampDevice();
54            auto gyroTs1 = gyroValues.getTimestampDevice();
55            if(!firstTs) {
56                baseTs = std::min(acceleroTs1, gyroTs1);
57                firstTs = true;
58            }
59
60            auto acceleroTs = acceleroTs1 - baseTs;
61            auto gyroTs = gyroTs1 - baseTs;
62
63            printf("Accelerometer timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(acceleroTs).count()));
64            printf("Accelerometer [m/s^2]: x: %.3f y: %.3f z: %.3f \n", acceleroValues.x, acceleroValues.y, acceleroValues.z);
65            printf("Gyroscope timestamp: %ld ms\n", static_cast<long>(duration_cast<milliseconds>(gyroTs).count()));
66            printf("Gyroscope [rad/s]: x: %.3f y: %.3f z: %.3f \n", gyroValues.x, gyroValues.y, gyroValues.z);
67        }
68
69        int key = cv::waitKey(1);
70        if(key == 'q') {
71            return 0;
72        }
73    }
74
75    return 0;
76}