kos_core 0.1.2

The K-Scale Operating System
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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149

use crate::grpc_interface::google::longrunning::Operation;
use crate::hal::IMU;
use crate::kos_proto::common::ActionResponse;
use crate::kos_proto::imu::imu_service_server::ImuService;
use crate::kos_proto::imu::*;
use crate::telemetry::Telemetry;
use crate::telemetry_types::{EulerAngles, ImuValues, Quaternion};
use std::sync::Arc;
use tonic::{Request, Response, Status};
use tracing::trace;

pub struct IMUServiceImpl {
    imu: Arc<dyn IMU>,
}

impl IMUServiceImpl {
    pub fn new(imu: Arc<dyn IMU>) -> Self {
        Self { imu }
    }
}

#[tonic::async_trait]
impl ImuService for IMUServiceImpl {
    async fn get_values(
        &self,
        _request: Request<()>,
    ) -> Result<Response<ImuValuesResponse>, Status> {
        let values = self
            .imu
            .get_values()
            .await
            .map_err(|e| Status::internal(format!("Failed to get IMU values, {:?}", e)))?;

        let telemetry = Telemetry::get().await;
        if let Some(telemetry) = telemetry {
            if let Err(e) = telemetry
                .publish("imu/values", &ImuValues::from(&values))
                .await
            {
                tracing::warn!("Failed to publish telemetry: {}", e);
            }
        }

        trace!("Getting IMU values, response: {:?}", values);

        Ok(Response::new(values))
    }

    async fn get_advanced_values(
        &self,
        _request: Request<()>,
    ) -> Result<Response<ImuAdvancedValuesResponse>, Status> {
        let values =
            self.imu.get_advanced_values().await.map_err(|e| {
                Status::internal(format!("Failed to get IMU advanced values, {:?}", e))
            })?;

        Ok(Response::new(values))
    }

    async fn calibrate(&self, _request: Request<()>) -> Result<Response<Operation>, Status> {
        let _status = self
            .imu
            .calibrate()
            .await
            .map_err(|e| Status::internal(format!("Failed to calibrate IMU, {:?}", e)))?;

        Ok(Response::new(Operation {
            name: "operations/calibrate_imu/0".to_string(),
            metadata: None,
            done: false,
            result: None,
        }))
    }

    async fn zero(
        &self,
        request: Request<ZeroImuRequest>,
    ) -> Result<Response<ActionResponse>, Status> {
        let request: ZeroImuRequest = request.into_inner();

        let duration = request.duration;

        // Convert proto duration to std::time::Duration
        let duration = duration.map(|d| {
            std::time::Duration::from_nanos(d.nanos as u64)
                + std::time::Duration::from_secs(d.seconds as u64)
        });

        let max_retries = request.max_retries;

        let max_angular_error = request.max_angular_error.map(|e| e as f32);
        let max_vel = request.max_velocity.map(|v| v as f32);
        let max_accel = request.max_acceleration.map(|a| a as f32);

        let response = self
            .imu
            .zero(duration, max_retries, max_angular_error, max_vel, max_accel)
            .await
            .map_err(|e| Status::internal(format!("Failed to zero IMU, {:?}", e)))?;
        Ok(Response::new(response))
    }

    async fn get_euler(
        &self,
        _request: Request<()>,
    ) -> Result<Response<EulerAnglesResponse>, Status> {
        let euler = self
            .imu
            .get_euler()
            .await
            .map_err(|e| Status::internal(format!("Failed to get euler, {:?}", e)))?;

        let telemetry = Telemetry::get().await;
        if let Some(telemetry) = telemetry {
            if let Err(e) = telemetry
                .publish("imu/euler", &EulerAngles::from(&euler))
                .await
            {
                tracing::warn!("Failed to publish telemetry: {}", e);
            }
        }

        Ok(Response::new(euler))
    }

    async fn get_quaternion(
        &self,
        _request: Request<()>,
    ) -> Result<Response<QuaternionResponse>, Status> {
        let quaternion = self
            .imu
            .get_quaternion()
            .await
            .map_err(|e| Status::internal(format!("Failed to get quaternion, {:?}", e)))?;

        let telemetry = Telemetry::get().await;
        if let Some(telemetry) = telemetry {
            if let Err(e) = telemetry
                .publish("imu/quaternion", &Quaternion::from(&quaternion))
                .await
            {
                tracing::warn!("Failed to publish telemetry: {}", e);
            }
        }

        Ok(Response::new(quaternion))
    }
}