veloxity_core/
companion.rs

1use crate::{
2    comm::messages::messages::RosflightHardErrorMsg,
3    comm::messages::messages::{ExternalAttitudeMsg, HeartbeatMsg, RosflightAuxCmdMsg},
4    events::{CommEventQueues, CommResponse, CompanionEventQueues},
5};
6
7#[derive(Default)]
8pub struct CompanionLinkState {
9    pub connected: bool,
10    pub last_heartbeat: Option<HeartbeatMsg>,
11}
12
13#[derive(Default)]
14pub struct AuxCommandState {
15    pub latest: Option<RosflightAuxCmdMsg>,
16}
17
18#[derive(Default)]
19pub struct ExternalAttitudeState {
20    pub latest: Option<ExternalAttitudeMsg>,
21}
22
23pub struct CompanionInputCtx<'a> {
24    pub events: &'a mut CompanionEventQueues,
25    pub comm_events: &'a mut CommEventQueues,
26    pub link: &'a mut CompanionLinkState,
27    pub aux_commands: &'a mut AuxCommandState,
28    pub external_attitude: &'a mut ExternalAttitudeState,
29    pub pending_hard_error: &'a mut Option<RosflightHardErrorMsg>,
30}
31
32pub fn apply_companion_inputs(ctx: &mut CompanionInputCtx<'_>) {
33    apply_companion_heartbeats(ctx);
34    emit_pending_hard_error_if_connected(ctx);
35    apply_aux_commands(ctx);
36    apply_external_attitudes(ctx);
37}
38
39pub fn apply_companion_heartbeats(ctx: &mut CompanionInputCtx<'_>) {
40    while let Some(event) = ctx.events.heartbeats.pop() {
41        ctx.link.connected = true;
42        ctx.link.last_heartbeat = Some(event.msg);
43    }
44}
45
46fn emit_pending_hard_error_if_connected(ctx: &mut CompanionInputCtx<'_>) {
47    if ctx.link.connected
48        && let Some(msg) = ctx.pending_hard_error.take()
49    {
50        ctx.comm_events
51            .responses
52            .push_or_log(CommResponse::HardError(msg), "hard error");
53    }
54}
55
56pub fn apply_aux_commands(ctx: &mut CompanionInputCtx<'_>) {
57    while let Some(event) = ctx.events.aux_commands.pop() {
58        ctx.aux_commands.latest = Some(event.msg);
59    }
60}
61
62pub fn apply_external_attitudes(ctx: &mut CompanionInputCtx<'_>) {
63    while let Some(event) = ctx.events.external_attitudes.pop() {
64        ctx.external_attitude.latest = Some(event.msg);
65    }
66}
67
68#[cfg(test)]
69mod tests {
70    use super::*;
71    use crate::{
72        comm::messages::{
73            enums::RosflightAuxCmdType,
74            messages::{ExternalAttitudeMsg, HeartbeatMsg, RosflightAuxCmdMsg},
75        },
76        events::{
77            AuxCommandReceived, CommEventQueues, CompanionEventQueues, CompanionHeartbeatReceived,
78            ExternalAttitudeReceived,
79        },
80    };
81
82    fn test_ctx<'a>(
83        events: &'a mut CompanionEventQueues,
84        comm_events: &'a mut CommEventQueues,
85        link: &'a mut CompanionLinkState,
86        aux_commands: &'a mut AuxCommandState,
87        external_attitude: &'a mut ExternalAttitudeState,
88        pending_hard_error: &'a mut Option<RosflightHardErrorMsg>,
89    ) -> CompanionInputCtx<'a> {
90        CompanionInputCtx {
91            events,
92            comm_events,
93            link,
94            aux_commands,
95            external_attitude,
96            pending_hard_error,
97        }
98    }
99
100    #[test]
101    fn companion_heartbeat_marks_link_connected_and_records_latest() {
102        let mut events = CompanionEventQueues::default();
103        let mut comm_events = CommEventQueues::default();
104        let mut link = CompanionLinkState::default();
105        let mut aux_commands = AuxCommandState::default();
106        let mut external_attitude = ExternalAttitudeState::default();
107        let mut pending_hard_error = None;
108        let heartbeat = HeartbeatMsg {
109            type_: 1,
110            autopilot: 2,
111            base_mode: 3,
112            custom_mode: 4,
113            system_status: 5,
114            mavlink_version: 6,
115        };
116
117        let _ = events
118            .heartbeats
119            .push(CompanionHeartbeatReceived { msg: heartbeat });
120
121        apply_companion_heartbeats(&mut test_ctx(
122            &mut events,
123            &mut comm_events,
124            &mut link,
125            &mut aux_commands,
126            &mut external_attitude,
127            &mut pending_hard_error,
128        ));
129
130        assert!(link.connected);
131        assert_eq!(link.last_heartbeat.unwrap().system_status, 5);
132        assert!(events.heartbeats.is_empty());
133    }
134
135    #[test]
136    fn aux_command_records_latest_command() {
137        let mut events = CompanionEventQueues::default();
138        let mut comm_events = CommEventQueues::default();
139        let mut link = CompanionLinkState::default();
140        let mut aux_commands = AuxCommandState::default();
141        let mut external_attitude = ExternalAttitudeState::default();
142        let mut pending_hard_error = None;
143        let mut msg = RosflightAuxCmdMsg {
144            type_array: [RosflightAuxCmdType::Disabled; 14],
145            aux_cmd_array: [0.0; 14],
146        };
147        msg.type_array[2] = RosflightAuxCmdType::Servo;
148        msg.aux_cmd_array[2] = 0.75;
149
150        let _ = events.aux_commands.push(AuxCommandReceived { msg });
151
152        apply_aux_commands(&mut test_ctx(
153            &mut events,
154            &mut comm_events,
155            &mut link,
156            &mut aux_commands,
157            &mut external_attitude,
158            &mut pending_hard_error,
159        ));
160
161        let latest = aux_commands.latest.unwrap();
162        assert!(matches!(latest.type_array[2], RosflightAuxCmdType::Servo));
163        assert_eq!(latest.aux_cmd_array[2], 0.75);
164        assert!(events.aux_commands.is_empty());
165    }
166
167    #[test]
168    fn external_attitude_records_latest_attitude() {
169        let mut events = CompanionEventQueues::default();
170        let mut comm_events = CommEventQueues::default();
171        let mut link = CompanionLinkState::default();
172        let mut aux_commands = AuxCommandState::default();
173        let mut external_attitude = ExternalAttitudeState::default();
174        let mut pending_hard_error = None;
175
176        let _ = events.external_attitudes.push(ExternalAttitudeReceived {
177            msg: ExternalAttitudeMsg {
178                qw: 1.0,
179                qx: 0.1,
180                qy: 0.2,
181                qz: 0.3,
182            },
183        });
184
185        apply_external_attitudes(&mut test_ctx(
186            &mut events,
187            &mut comm_events,
188            &mut link,
189            &mut aux_commands,
190            &mut external_attitude,
191            &mut pending_hard_error,
192        ));
193
194        let latest = external_attitude.latest.unwrap();
195        assert_eq!(latest.qw, 1.0);
196        assert_eq!(latest.qz, 0.3);
197        assert!(events.external_attitudes.is_empty());
198    }
199}
veloxity_core/companion.rs

veloxity_core/
companion.rs
