IMU加速计与陀螺仪

1#!/usr/bin/env python3
2import time
3
4import depthai as dai
5
6
7def timeDeltaToMilliS(delta) -> float:
8    return delta.total_seconds()*1000
9
10
11# Create pipeline
12with dai.Pipeline() as pipeline:
13    # Define sources and outputs
14    imu = pipeline.create(dai.node.IMU)
15
16    # enable ACCELEROMETER_UNCALIBRATED at 500 hz rate
17    imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_UNCALIBRATED, 480)
18    # enable GYROSCOPE_UNCALIBRATED at 400 hz rate
19    imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_UNCALIBRATED, 400)
20    # it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
21    # above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
22    imu.setBatchReportThreshold(1)
23    # maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
24    # if lower or equal to batchReportThreshold then the sending is always blocking on device
25    # useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
26    imu.setMaxBatchReports(10)
27
28    imuQueue = imu.out.createOutputQueue(maxSize=50, blocking=False)
29
30    pipeline.start()
31    baseTs = None
32    lastPrintTime = 0.0
33    while pipeline.isRunning():
34        try:
35            imuData = imuQueue.get()
36        except KeyboardInterrupt:
37            break
38        assert isinstance(imuData, dai.IMUData)
39        imuPackets = imuData.packets
40        if not imuPackets:
41            continue
42
43        now = time.monotonic()
44        if now - lastPrintTime < 1.0:
45            continue
46        lastPrintTime = now
47
48        imuPacket = imuPackets[-1]
49        acceleroValues = imuPacket.acceleroMeter
50        gyroValues = imuPacket.gyroscope
51
52        acceleroTs = acceleroValues.getTimestamp()
53        gyroTs = gyroValues.getTimestamp()
54
55        imuF = "{:.06f}"
56
57        print(f"Accelerometer timestamp: {acceleroTs}")
58        print(f"Latency [ms]: {dai.Clock.now() - acceleroValues.getTimestamp()}")
59        print(f"Accelerometer [m/s^2]: x: {imuF.format(acceleroValues.x)} y: {imuF.format(acceleroValues.y)} z: {imuF.format(acceleroValues.z)}")
60        print(f"Gyroscope timestamp: {gyroTs}")
61        print(f"Gyroscope [rad/s]: x: {imuF.format(gyroValues.x)} y: {imuF.format(gyroValues.y)} z: {imuF.format(gyroValues.z)} ")
62        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    auto lastPrintTime = std::chrono::steady_clock::now() - std::chrono::seconds(1);
51    while(pipeline.isRunning() && !quitEvent) {
52        auto imuData = imuQueue->get<dai::IMUData>();
53        if(imuData == nullptr || imuData->packets.empty()) continue;
54
55        const auto now = std::chrono::steady_clock::now();
56
57        const auto& imuPacket = imuData->packets.back();
58        auto acceleroValues = imuPacket.acceleroMeter;
59        auto gyroValues = imuPacket.gyroscope;
60
61        auto acceleroTs = acceleroValues.getTimestamp();
62        auto gyroTs = gyroValues.getTimestamp();
63        if(now - lastPrintTime < std::chrono::seconds(1)) continue;
64        lastPrintTime = now;
65
66        // Print the latest IMU sample at most once per second.
67        std::cout << "Accelerometer timestamp: " << acceleroTs.time_since_epoch().count() << std::endl;
68        std::cout << "Latency [ms]: " << timeDeltaToMilliS(std::chrono::steady_clock::now() - acceleroValues.getTimestamp()) << std::endl;
69        std::cout << "Accelerometer [m/s^2]: x: " << acceleroValues.x << " y: " << acceleroValues.y << " z: " << acceleroValues.z << std::endl;
70
71        std::cout << "Gyroscope timestamp: " << gyroTs.time_since_epoch().count() << std::endl;
72        std::cout << "Gyroscope [rad/s]: x: " << gyroValues.x << " y: " << gyroValues.y << " z: " << gyroValues.z << std::endl;
73    }
74
75    // Cleanup
76    pipeline.stop();
77    pipeline.wait();
78
79    return 0;
80}