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
//use dosio::{ios, DOSIOSError, Dos, IOTags, IOVec, IO};
/*
pub struct MacroController<'a> {
hardpoints_load_cells: hp_load_cells::Controller<'a>,
hardpoints_dynamics: hp_dynamics::Controller<'a>,
actuators_force_loops: actuators::M1ForceLoops<'a>,
}
impl<'a> IOTags for MacroController<'a> {
fn outputs_tags(&self) -> Vec<IO<()>> {
let mut tags = self.actuators_force_loops.outputs_tags();
tags.push(ios!(HPFcmd));
tags
}
fn inputs_tags(&self) -> Vec<IO<()>> {
ios!(M1RBMcmd, M1S1BMcmd, M1S2BMcmd, M1S3BMcmd, M1S4BMcmd, M1S5BMcmd, M1S6BMcmd, M1S7BMcmd)
}
}
impl<'a> Iterator for MacroController<'a> {
type Item = ();
fn next(&mut self) -> Option<Self::Item> {
self.hardpoints_load_cells
.step()
.ok()
.and(self.hardpoints_dynamics.step().ok())
.and(self.actuators_force_loops.step().ok())
.and(Some(()))
}
}
impl<'a> Dos for MacroController<'a> {
type Input = Vec<f64>;
type Output = Vec<f64>;
fn outputs(&mut self) -> Option<Vec<IO<Self::Output>>> {
Some(
Some(
self.hardpoints_load_cells
.outputs()
.into_iter()
.chain(self.hardpoints_dynamics.outputs().into_iter())
.flatten()
.collect::<Vec<IO<Vec<f64>>>>(),
)
.into_iter()
.chain(self.actuators_force_loops.outputs().into_iter())
.flatten()
.collect(),
)
}
fn inputs(
&mut self,
data: Option<Vec<IO<Self::Input>>>,
) -> Result<&mut Self, dosio::DOSIOSError> {
if let Some(mut data) = data {
self.hardpoints_load_cells
.inputs(<Vec<IO<Vec<f64>>> as IOVec>::pop_these(
&mut data,
ios!(OSSHardpointD, M1HPCmd),
))?;
self.hardpoints_dynamics
.inputs(<Vec<IO<Vec<f64>>> as IOVec>::pop_these(
&mut data,
vec![ios!(M1RBMcmd)],
))?;
self.actuators_force_loops.inputs(Some(data))?;
Ok(self)
} else {
Err(DOSIOSError::Inputs(
"Empty inputs passed to M1 macro controller".into(),
))
}
}
}
*/