kos_stub/
imu.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
use crate::Operation;
use async_trait::async_trait;
use eyre::Result;
use kos_core::services::OperationsServiceImpl;
use kos_core::{
    hal::{
        CalibrateImuMetadata, CalibrationStatus, EulerAnglesResponse, ImuValuesResponse,
        QuaternionResponse, IMU,
    },
    kos_proto::common::ActionResponse,
};
use std::sync::Arc;
use std::time::Duration;
use uuid::Uuid;

pub struct StubIMU {
    operations_service: Arc<OperationsServiceImpl>,
}

impl StubIMU {
    pub fn new(operations_service: Arc<OperationsServiceImpl>) -> Self {
        StubIMU { operations_service }
    }
}

impl Default for StubIMU {
    fn default() -> Self {
        unimplemented!("StubIMU cannot be default, it requires an operations store")
    }
}

#[async_trait]
impl IMU for StubIMU {
    async fn get_values(&self) -> Result<ImuValuesResponse> {
        Ok(ImuValuesResponse {
            accel_x: 1.0,
            accel_y: 2.0,
            accel_z: 3.0,
            gyro_x: 0.0,
            gyro_y: 0.0,
            gyro_z: 0.0,
            mag_x: None,
            mag_y: None,
            mag_z: None,
            error: None,
        })
    }

    async fn calibrate(&self) -> Result<Operation> {
        let operation = Operation {
            name: format!("operations/imu/calibrate/{}", Uuid::new_v4()),
            metadata: None,
            done: false,
            result: None,
        };
        let metadata = CalibrateImuMetadata {
            status: CalibrationStatus::Calibrating.to_string(),
        };
        let operation = self
            .operations_service
            .create(
                operation.name,
                metadata,
                "type.googleapis.com/kos.imu.CalibrateIMUMetadata",
            )
            .await?;

        Ok(operation)
    }

    async fn zero(&self, _duration: Duration) -> Result<ActionResponse> {
        Ok(ActionResponse {
            success: true,
            error: None,
        })
    }

    async fn get_euler(&self) -> Result<EulerAnglesResponse> {
        Ok(EulerAnglesResponse {
            roll: 0.0,
            pitch: 30.0,
            yaw: 0.0,
            error: None,
        })
    }

    async fn get_quaternion(&self) -> Result<QuaternionResponse> {
        Ok(QuaternionResponse {
            w: 1.0,
            x: 0.0,
            y: 0.0,
            z: 0.0,
            error: None,
        })
    }
}