gmt_dos-clients_m1-ctrl 6.0.0

GMT DOS M1 Control Client
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
150
151
152
153
154
155
156
157
158
159
160
161
162

use std::marker::PhantomData;

use gmt_dos_clients_io::gmt_m1::segment;
use interface::{Data, Read, Size, Update, Write};
use serde::{Deserialize, Serialize};

use crate::Calibration;

type M = nalgebra::Matrix6<f64>;
type V = nalgebra::Vector6<f64>;

pub enum New {}
pub enum LC2CG {}

#[derive(Debug, Clone)]
pub struct LoadCellsBuilder<T> {
    lc_2_cg: M,
    m1_hpk: Option<f64>,
    checks: PhantomData<T>,
}
impl Default for LoadCellsBuilder<New> {
    fn default() -> Self {
        Self {
            lc_2_cg: Default::default(),
            m1_hpk: None,
            checks: PhantomData,
        }
    }
}
impl LoadCellsBuilder<New> {
    pub fn hardpoints_barycentric_transform(self, lc_2_cg: M) -> LoadCellsBuilder<LC2CG> {
        LoadCellsBuilder {
            lc_2_cg,
            m1_hpk: None,
            checks: PhantomData,
        }
    }
}
impl LoadCellsBuilder<LC2CG> {
    pub fn hardpoints_stiffness(mut self, m1_hk: f64) -> Self {
        self.m1_hpk = Some(m1_hk);
        self
    }
    pub fn build<const ID: u8>(self) -> LoadCells<ID> {
        LoadCells {
            m1_hpk: self.m1_hpk,
            hp_f_cmd: vec![0f64; 6],
            hp_d_cell: vec![0f64; 6],
            hp_d_face: vec![0f64; 6],
            hp_f_meas: vec![0f64; 6],
            lc_2_cg: self.lc_2_cg,
        }
    }
}

/// [gmt_dos_actors](https://docs.rs/gmt_dos-actors) client interface for hardpoints loadcells
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct LoadCells<const ID: u8> {
    pub(super) hp_f_cmd: Vec<f64>,
    pub(super) hp_d_cell: Vec<f64>,
    pub(super) hp_d_face: Vec<f64>,
    hp_f_meas: Vec<f64>,
    m1_hpk: Option<f64>,
    lc_2_cg: M,
}
impl<const ID: u8> From<&Calibration> for LoadCells<ID> {
    fn from(c: &Calibration) -> Self {
        if cfg!(any(
            not(m1_hp_force_extension),
            feature = "explicit-loadcells"
        )) {
            Self {
                m1_hpk: Some(c.stiffness),
                hp_f_cmd: vec![0f64; 6],
                hp_d_cell: vec![0f64; 6],
                hp_d_face: vec![0f64; 6],
                hp_f_meas: vec![0f64; 6],
                lc_2_cg: c.lc_2_cg[ID as usize - 1],
            }
        } else {
            Self {
                m1_hpk: None,
                hp_f_cmd: vec![0f64; 6],
                hp_d_cell: vec![0f64; 6],
                hp_d_face: vec![0f64; 6],
                hp_f_meas: vec![0f64; 6],
                lc_2_cg: c.lc_2_cg[ID as usize - 1],
            }
        }
    }
}
impl<const ID: u8> LoadCells<ID> {
    /// Creates a new loadcells client
    ///
    /// The hardpoints stiffness and the matrix transformation
    /// from local to center of gravity coordinates are provided.
    #[deprecated = "replaced with builder"]
    pub fn new(m1_hpk: f64, lc_2_cg: M) -> Self {
        Self {
            m1_hpk: Some(m1_hpk),
            hp_f_cmd: vec![0f64; 6],
            hp_d_cell: vec![0f64; 6],
            hp_d_face: vec![0f64; 6],
            hp_f_meas: vec![0f64; 6],
            lc_2_cg,
        }
    }
    pub fn builder() -> LoadCellsBuilder<New> {
        Default::default()
    }
}

impl<const ID: u8> Size<segment::HardpointsMotion<ID>> for LoadCells<ID> {
    fn len(&self) -> usize {
        12
    }
}

impl<const ID: u8> Size<segment::BarycentricForce<ID>> for LoadCells<ID> {
    fn len(&self) -> usize {
        6
    }
}

impl<const ID: u8> Update for LoadCells<ID> {
    fn update(&mut self) {
        if let Some(m1_hpk) = self.m1_hpk {
            self.hp_d_cell
                .iter()
                .zip(self.hp_d_face.iter())
                .map(|(hp_d_cell, hp_d_face)| hp_d_face - hp_d_cell)
                .map(|hp_relative_displacements| hp_relative_displacements * m1_hpk)
                .zip(self.hp_f_cmd.iter())
                .map(|(hp_relative_force, hp_f_cmd)| hp_relative_force - hp_f_cmd)
                .zip(&mut self.hp_f_meas)
                .for_each(|(hp_f_diff_force, hp_f_meas)| *hp_f_meas = hp_f_diff_force);
        } else {
            self.hp_f_meas.copy_from_slice(&self.hp_f_cmd);
        }
    }
}

impl<const ID: u8> Read<segment::HardpointsForces<ID>> for LoadCells<ID> {
    fn read(&mut self, data: Data<segment::HardpointsForces<ID>>) {
        self.hp_f_cmd = (**data).to_vec();
    }
}

impl<const ID: u8> Read<segment::HardpointsMotion<ID>> for LoadCells<ID> {
    fn read(&mut self, data: Data<segment::HardpointsMotion<ID>>) {
        let (cell, face) = (&data).split_at(6);
        self.hp_d_cell.copy_from_slice(cell);
        self.hp_d_face.copy_from_slice(face);
    }
}

impl<const ID: u8> Write<segment::BarycentricForce<ID>> for LoadCells<ID> {
    fn write(&mut self) -> Option<Data<segment::BarycentricForce<ID>>> {
        let cg = self.lc_2_cg * V::from_column_slice(self.hp_f_meas.as_slice());
        Some(Data::new(cg.as_slice().to_vec()))
    }
}