veloxity_core/rc/
command_state.rs

1use crate::{
2    command::{ATTITUDE_RATE_MODE, CommandManager},
3    events::ParamEventQueues,
4    packets::RcPacket,
5    params::{ParamId, ParamValue, Params},
6    rc::Rc,
7    state_machine::StateManager,
8};
9
10pub struct RcCommandStateCtx<'a> {
11    pub now_ms: u32,
12    /// A newly ingested RC sample for this system pass. The long-lived RC
13    /// resource retains interpreted stick/switch state; cached sensor state is
14    /// never replayed as a fresh command.
15    pub fresh_rc: Option<RcPacket>,
16    pub rc: &'a mut Rc,
17    pub command: &'a mut CommandManager,
18    pub state: &'a mut StateManager,
19    pub params: &'a mut Params,
20    pub param_events: Option<&'a mut ParamEventQueues>,
21}
22
23pub fn run_rc_command_state(ctx: RcCommandStateCtx<'_>) {
24    if let Some(rc_packet) = ctx.fresh_rc {
25        ctx.rc.receive(&rc_packet);
26        ctx.rc.run(ctx.now_ms, ctx.params, ctx.state);
27    }
28
29    let command_result = ctx.command.run(ctx.now_ms, ctx.params, ctx.rc, ctx.state);
30    if command_result.force_rc_attitude_mode_rate {
31        if let Some(param_events) = ctx.param_events {
32            crate::params::service::set_param_and_emit_change(
33                ctx.params,
34                &mut param_events.changes,
35                ParamId::PARAM_RC_ATTITUDE_MODE,
36                ParamValue::Int(ATTITUDE_RATE_MODE),
37            );
38        } else {
39            ctx.params.set_by_id(
40                ParamId::PARAM_RC_ATTITUDE_MODE,
41                ParamValue::Int(ATTITUDE_RATE_MODE),
42            );
43        }
44    }
45    ctx.state.run(ctx.params);
46}
47
48#[cfg(test)]
49mod tests {
50    use super::*;
51    use crate::{
52        events::ParamEventQueues,
53        packets::{RC_PACKET_CHANNELS, RcPacket, RosflightPacketHeader},
54        params::{ParamId, ParamValue},
55        state_machine::{ErrorFlag, Event},
56    };
57
58    #[test]
59    fn rc_command_state_consumes_named_rc_packet() {
60        let mut params = Params::new();
61        params.set_by_id(ParamId::PARAM_RC_NUM_CHANNELS, ParamValue::Int(1));
62        let fresh_rc = Some(RcPacket {
63            header: RosflightPacketHeader {
64                timestamp: 1,
65                status: 0,
66            },
67            n_chan: 1,
68            chan: [0.5; RC_PACKET_CHANNELS],
69            lol: false,
70        });
71        let mut rc = Rc::new();
72        let mut command = CommandManager::new();
73        let mut state = StateManager::new();
74
75        run_rc_command_state(RcCommandStateCtx {
76            now_ms: 1,
77            fresh_rc,
78            rc: &mut rc,
79            command: &mut command,
80            state: &mut state,
81            params: &mut params,
82            param_events: None,
83        });
84
85        assert!(!state.get_errors().contains(ErrorFlag::RC_LOST));
86    }
87
88    #[test]
89    fn rc_command_state_does_not_publish_command_from_lost_frame() {
90        let mut params = Params::new();
91        params.set_by_id(ParamId::PARAM_RC_NUM_CHANNELS, ParamValue::Int(1));
92        let fresh_rc = Some(RcPacket {
93            header: RosflightPacketHeader {
94                timestamp: 1,
95                status: 0,
96            },
97            n_chan: 1,
98            chan: [0.5; RC_PACKET_CHANNELS],
99            lol: true,
100        });
101        let mut rc = Rc::new();
102        let mut command = CommandManager::new();
103        let mut state = StateManager::new();
104
105        run_rc_command_state(RcCommandStateCtx {
106            now_ms: 1,
107            fresh_rc,
108            rc: &mut rc,
109            command: &mut command,
110            state: &mut state,
111            params: &mut params,
112            param_events: None,
113        });
114
115        assert!(state.get_errors().contains(ErrorFlag::RC_LOST));
116        assert!(!rc.new_command());
117    }
118
119    #[test]
120    fn rc_command_state_does_not_treat_protocol_status_bits_as_link_loss() {
121        let mut params = Params::new();
122        params.set_by_id(ParamId::PARAM_RC_NUM_CHANNELS, ParamValue::Int(1));
123        let fresh_rc = Some(RcPacket {
124            header: RosflightPacketHeader {
125                timestamp: 1,
126                // SBUS uses bits 0 and 1 for digital channels 17 and 18.
127                status: 0x03,
128            },
129            n_chan: 1,
130            chan: [0.5; RC_PACKET_CHANNELS],
131            lol: false,
132        });
133        let mut rc = Rc::new();
134        let mut command = CommandManager::new();
135        let mut state = StateManager::new();
136
137        run_rc_command_state(RcCommandStateCtx {
138            now_ms: 1,
139            fresh_rc,
140            rc: &mut rc,
141            command: &mut command,
142            state: &mut state,
143            params: &mut params,
144            param_events: None,
145        });
146
147        assert!(!state.get_errors().contains(ErrorFlag::RC_LOST));
148    }
149
150    #[test]
151    fn lost_frame_with_arm_channel_low_enters_failsafe_without_disarming() {
152        let mut params = Params::new();
153        params.set_by_id(ParamId::PARAM_RC_NUM_CHANNELS, ParamValue::Int(8));
154        params.set_by_id(ParamId::PARAM_RC_ARM_CHANNEL, ParamValue::Int(4));
155
156        let mut channels = [0.5; RC_PACKET_CHANNELS];
157        channels[2] = 0.0;
158        channels[4] = 0.0;
159        let fresh_rc = Some(RcPacket {
160            header: RosflightPacketHeader {
161                timestamp: 1,
162                status: 0,
163            },
164            n_chan: 8,
165            chan: channels,
166            lol: true,
167        });
168
169        let mut rc = Rc::new();
170        rc.init(&params);
171        let mut command = CommandManager::new();
172        let mut state = StateManager::new();
173        state.update(Event::INITIALIZED, &params);
174        state.update_arming_safety(true, true);
175        state.update(Event::REQUEST_ARM, &params);
176        assert!(state.is_armed());
177
178        run_rc_command_state(RcCommandStateCtx {
179            now_ms: 1,
180            fresh_rc,
181            rc: &mut rc,
182            command: &mut command,
183            state: &mut state,
184            params: &mut params,
185            param_events: None,
186        });
187
188        assert!(state.is_armed());
189        assert!(state.is_in_failsafe());
190        assert!(state.get_errors().contains(ErrorFlag::RC_LOST));
191    }
192
193    #[test]
194    fn rc_command_state_emits_param_change_when_lockout_forces_rate_mode() {
195        let mut params = Params::new();
196        params.set_by_id(ParamId::PARAM_RC_ATTITUDE_MODE, ParamValue::Int(1));
197        params.set_by_id(ParamId::PARAM_EST_ANGLE_LOCKOUT, ParamValue::Int(1));
198        let fresh_rc = Some(RcPacket {
199            header: RosflightPacketHeader {
200                timestamp: 1,
201                status: 0,
202            },
203            n_chan: 8,
204            chan: [0.5; RC_PACKET_CHANNELS],
205            lol: false,
206        });
207        let mut rc = Rc::new();
208        rc.init(&params);
209        let mut command = CommandManager::new();
210        let mut state = StateManager::new();
211        state.set_error_flag(ErrorFlag::UNHEALTHY_ESTIMATOR, true, &params);
212        let mut param_events = ParamEventQueues::default();
213
214        run_rc_command_state(RcCommandStateCtx {
215            now_ms: 1,
216            fresh_rc,
217            rc: &mut rc,
218            command: &mut command,
219            state: &mut state,
220            params: &mut params,
221            param_events: Some(&mut param_events),
222        });
223
224        assert_eq!(
225            params.get_by_id(ParamId::PARAM_RC_ATTITUDE_MODE),
226            ParamValue::Int(ATTITUDE_RATE_MODE)
227        );
228        let change = param_events.changes.pop().unwrap();
229        assert_eq!(change.id, ParamId::PARAM_RC_ATTITUDE_MODE);
230        assert_eq!(change.old, ParamValue::Int(1));
231        assert_eq!(change.new, ParamValue::Int(ATTITUDE_RATE_MODE));
232    }
233
234    #[test]
235    fn rc_command_state_does_not_force_rate_mode_when_lockout_disabled() {
236        let mut params = Params::new();
237        params.set_by_id(ParamId::PARAM_RC_ATTITUDE_MODE, ParamValue::Int(1));
238        let fresh_rc = Some(RcPacket {
239            header: RosflightPacketHeader {
240                timestamp: 1,
241                status: 0,
242            },
243            n_chan: 8,
244            chan: [0.5; RC_PACKET_CHANNELS],
245            lol: false,
246        });
247        let mut rc = Rc::new();
248        rc.init(&params);
249        let mut command = CommandManager::new();
250        let mut state = StateManager::new();
251        state.set_error_flag(ErrorFlag::UNHEALTHY_ESTIMATOR, true, &params);
252        let mut param_events = ParamEventQueues::default();
253
254        run_rc_command_state(RcCommandStateCtx {
255            now_ms: 1,
256            fresh_rc,
257            rc: &mut rc,
258            command: &mut command,
259            state: &mut state,
260            params: &mut params,
261            param_events: Some(&mut param_events),
262        });
263
264        assert_eq!(
265            params.get_by_id(ParamId::PARAM_RC_ATTITUDE_MODE),
266            ParamValue::Int(1)
267        );
268        assert!(param_events.changes.pop().is_none());
269    }
270}
veloxity_core/rc/command_state.rs

veloxity_core/rc/
command_state.rs
