1use crate::{
2 board::BoardIo,
3 comm::messages::{
4 enums::{RosflightCmd, RosflightCmdResponse},
5 messages::{RosflightCmdAckMsg, RosflightVersionMsg},
6 },
7 command::CommandManager,
8 controller::RcTrimCalibrator,
9 events::{CommEventQueues, CommResponse, CommandEventQueues, ParamEventQueues},
10 params::service::{mark_all_params_changed, set_param_and_emit_change},
11 params::{ParamId, ParamValue, Params},
12 sensors::processors::CalibrationFlags,
13 state_machine::StateManager,
14};
15
16fn emit_cmd_ack(
17 comm_events: &mut CommEventQueues,
18 command: RosflightCmd,
19 success: RosflightCmdResponse,
20) {
21 comm_events.responses.push_or_log(
22 CommResponse::CmdAck(RosflightCmdAckMsg { command, success }),
23 "command ack response",
24 );
25}
26
27pub struct CommandRequestCtx<'a, B, C>
28where
29 B: BoardIo,
30 C: RcTrimCalibrator,
31{
32 pub requests: &'a mut CommandEventQueues,
33 pub param_events: &'a mut ParamEventQueues,
34 pub comm_events: &'a mut CommEventQueues,
35 pub state: &'a StateManager,
36 pub command: &'a mut CommandManager,
37 pub controller: &'a mut C,
38 pub board: &'a mut B,
39 pub flags: &'a mut CalibrationFlags,
40 pub params: &'a mut Params,
41}
42
43pub fn apply_command_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
44where
45 B: BoardIo,
46 C: RcTrimCalibrator,
47{
48 apply_calibration_requests(ctx);
49 apply_offboard_control_requests(ctx);
50 apply_param_defaults_requests(ctx);
51 apply_rc_trim_calibration_requests(ctx);
52 apply_board_command_requests(ctx);
53 apply_version_requests(ctx);
54 apply_reset_origin_requests(ctx);
55 apply_config_info_requests(ctx);
56}
57
58pub fn apply_calibration_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
59where
60 B: BoardIo,
61 C: RcTrimCalibrator,
62{
63 while let Some(request) = ctx.requests.calibration_requests.pop() {
64 if ctx.state.is_armed() {
65 emit_cmd_ack(
66 ctx.comm_events,
67 request.command,
68 RosflightCmdResponse::RosflightCmdFailed,
69 );
70 continue;
71 }
72
73 match request.command {
74 RosflightCmd::AccelCalibration => {
75 ctx.flags
76 .remove(CalibrationFlags::GYRO_FAILED | CalibrationFlags::ACCEL_FAILED);
77 ctx.flags.insert(CalibrationFlags::IMU);
78 zero_gyro_biases(ctx.params, ctx.param_events);
79 zero_accel_biases(ctx.params, ctx.param_events);
80 }
81 RosflightCmd::GyroCalibration => {
82 ctx.flags.remove(CalibrationFlags::GYRO_FAILED);
83 ctx.flags.insert(CalibrationFlags::GYRO);
84 zero_gyro_biases(ctx.params, ctx.param_events);
85 }
86 RosflightCmd::BaroCalibration => {
87 ctx.flags.remove(CalibrationFlags::BARO_FAILED);
88 ctx.flags.insert(CalibrationFlags::BARO);
89 }
90 RosflightCmd::AirspeedCalibration => {
91 ctx.flags.remove(CalibrationFlags::PITOT_FAILED);
92 ctx.flags.insert(CalibrationFlags::PITOT);
93 set_param_and_emit_change(
94 ctx.params,
95 &mut ctx.param_events.changes,
96 ParamId::PARAM_DIFF_PRESS_BIAS,
97 ParamValue::Float(0.0),
98 );
99 }
100 _ => {}
101 }
102 emit_cmd_ack(
103 ctx.comm_events,
104 request.command,
105 RosflightCmdResponse::RosflightCmdSuccess,
106 );
107 }
108}
109
110fn zero_gyro_biases(params: &mut Params, events: &mut ParamEventQueues) {
111 set_param_and_emit_change(
112 params,
113 &mut events.changes,
114 ParamId::PARAM_GYRO_X_BIAS,
115 ParamValue::Float(0.0),
116 );
117 set_param_and_emit_change(
118 params,
119 &mut events.changes,
120 ParamId::PARAM_GYRO_Y_BIAS,
121 ParamValue::Float(0.0),
122 );
123 set_param_and_emit_change(
124 params,
125 &mut events.changes,
126 ParamId::PARAM_GYRO_Z_BIAS,
127 ParamValue::Float(0.0),
128 );
129}
130
131fn zero_accel_biases(params: &mut Params, events: &mut ParamEventQueues) {
132 set_param_and_emit_change(
133 params,
134 &mut events.changes,
135 ParamId::PARAM_ACC_X_BIAS,
136 ParamValue::Float(0.0),
137 );
138 set_param_and_emit_change(
139 params,
140 &mut events.changes,
141 ParamId::PARAM_ACC_Y_BIAS,
142 ParamValue::Float(0.0),
143 );
144 set_param_and_emit_change(
145 params,
146 &mut events.changes,
147 ParamId::PARAM_ACC_Z_BIAS,
148 ParamValue::Float(0.0),
149 );
150}
151
152pub fn apply_offboard_control_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
153where
154 B: BoardIo,
155 C: RcTrimCalibrator,
156{
157 while let Some(request) = ctx.requests.offboard_control_requests.pop() {
158 ctx.command
159 .set_new_offboard_command(request.now_us, &request.msg, ctx.params);
160 }
161}
162
163pub fn apply_param_defaults_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
164where
165 B: BoardIo,
166 C: RcTrimCalibrator,
167{
168 while let Some(request) = ctx.requests.param_defaults_requests.pop() {
169 let success = if ctx.state.is_armed() {
170 RosflightCmdResponse::RosflightCmdFailed
171 } else {
172 ctx.params.set_defaults();
173 mark_all_params_changed(ctx.param_events);
174 RosflightCmdResponse::RosflightCmdSuccess
175 };
176 emit_cmd_ack(ctx.comm_events, request.command, success);
177 }
178}
179
180pub fn apply_board_command_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
181where
182 B: BoardIo,
183 C: RcTrimCalibrator,
184{
185 while let Some(request) = ctx.requests.board_command_requests.pop() {
186 if !ctx.state.is_armed()
187 && matches!(
188 request.command,
189 RosflightCmd::Reboot | RosflightCmd::RebootToBootloader
190 )
191 {
192 emit_cmd_ack(
193 ctx.comm_events,
194 request.command,
195 RosflightCmdResponse::RosflightCmdSuccess,
196 );
197 match request.command {
198 RosflightCmd::Reboot => {
199 let _ = ctx.board.reboot();
200 }
201 RosflightCmd::RebootToBootloader => {
202 let _ = ctx.board.reboot_to_bootloader();
203 }
204 _ => {}
205 }
206 continue;
207 }
208
209 let completed = if ctx.state.is_armed() {
210 false
211 } else {
212 match request.command {
213 RosflightCmd::ReadParams => ctx.board.read_params(ctx.params),
214 RosflightCmd::WriteParams => ctx.board.write_params(ctx.params),
215 _ => false,
216 }
217 };
218 if completed && matches!(request.command, RosflightCmd::ReadParams) {
219 mark_all_params_changed(ctx.param_events);
220 }
221
222 let success = if completed {
223 RosflightCmdResponse::RosflightCmdSuccess
224 } else {
225 RosflightCmdResponse::RosflightCmdFailed
226 };
227 ctx.comm_events.responses.push_or_log(
228 CommResponse::CmdAck(RosflightCmdAckMsg {
229 command: request.command,
230 success,
231 }),
232 "board command ack response",
233 );
234 }
235}
236
237pub fn apply_rc_trim_calibration_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
238where
239 B: BoardIo,
240 C: RcTrimCalibrator,
241{
242 while let Some(request) = ctx.requests.rc_trim_calibration_requests.pop() {
243 if !ctx.state.is_armed() {
244 let torques = ctx
245 .controller
246 .calculate_equilibrium_torques_from_rc(ctx.command.rc_control(), ctx.params);
247 set_param_and_emit_change(
248 ctx.params,
249 &mut ctx.param_events.changes,
250 ParamId::PARAM_X_EQ_TORQUE,
251 ParamValue::Float(param_float(ctx.params, ParamId::PARAM_X_EQ_TORQUE) + torques[0]),
252 );
253 set_param_and_emit_change(
254 ctx.params,
255 &mut ctx.param_events.changes,
256 ParamId::PARAM_Y_EQ_TORQUE,
257 ParamValue::Float(param_float(ctx.params, ParamId::PARAM_Y_EQ_TORQUE) + torques[1]),
258 );
259 set_param_and_emit_change(
260 ctx.params,
261 &mut ctx.param_events.changes,
262 ParamId::PARAM_Z_EQ_TORQUE,
263 ParamValue::Float(param_float(ctx.params, ParamId::PARAM_Z_EQ_TORQUE) + torques[2]),
264 );
265 }
266
267 let success = if ctx.state.is_armed() {
268 RosflightCmdResponse::RosflightCmdFailed
269 } else {
270 RosflightCmdResponse::RosflightCmdSuccess
271 };
272 ctx.comm_events.responses.push_or_log(
273 CommResponse::CmdAck(RosflightCmdAckMsg {
274 command: request.command,
275 success,
276 }),
277 "rc trim ack response",
278 );
279 }
280}
281
282fn param_float(params: &Params, id: ParamId) -> f32 {
283 match params.get_by_id(id) {
284 ParamValue::Float(value) => value,
285 _ => 0.0,
286 }
287}
288
289pub fn apply_version_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
290where
291 B: BoardIo,
292 C: RcTrimCalibrator,
293{
294 while let Some(request) = ctx.requests.version_requests.pop() {
295 if ctx.state.is_armed() {
296 emit_cmd_ack(
297 ctx.comm_events,
298 request.command,
299 RosflightCmdResponse::RosflightCmdFailed,
300 );
301 continue;
302 }
303
304 let version_str = "Veloxity 1.0";
305 let mut version_bytes = [0u8; 50];
306 let len = version_str.len().min(version_bytes.len());
307 version_bytes[..len].copy_from_slice(version_str.as_bytes());
308 ctx.comm_events.responses.push_or_log(
309 CommResponse::Version(RosflightVersionMsg {
310 version: version_bytes,
311 }),
312 "version response",
313 );
314 emit_cmd_ack(
315 ctx.comm_events,
316 request.command,
317 RosflightCmdResponse::RosflightCmdSuccess,
318 );
319 }
320}
321
322pub fn apply_reset_origin_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
323where
324 B: BoardIo,
325 C: RcTrimCalibrator,
326{
327 while let Some(request) = ctx.requests.reset_origin_requests.pop() {
328 ctx.comm_events.responses.push_or_log(
329 CommResponse::CmdAck(RosflightCmdAckMsg {
330 command: request.command,
331 success: RosflightCmdResponse::RosflightCmdFailed,
332 }),
333 "reset origin ack response",
334 );
335 }
336}
337
338pub fn apply_config_info_requests<B, C>(ctx: &mut CommandRequestCtx<'_, B, C>)
339where
340 B: BoardIo,
341 C: RcTrimCalibrator,
342{
343 while let Some(request) = ctx.requests.config_info_requests.pop() {
344 ctx.comm_events.responses.push_or_log(
345 CommResponse::CmdAck(RosflightCmdAckMsg {
346 command: request.command,
347 success: RosflightCmdResponse::RosflightCmdFailed,
348 }),
349 "config info ack response",
350 );
351 }
352}
353
354#[cfg(test)]
355mod tests {
356 use super::*;
357 use crate::{
358 board::BoardIo,
359 comm::messages::{
360 enums::{OffboardControlIgnore, OffboardControlMode},
361 messages::OffboardControlMsg,
362 },
363 controller::quad::QuadController,
364 errors,
365 events::{
366 BoardCommandRequested, CalibrationRequested, CommEventQueues, CommandEventQueues,
367 ConfigInfoRequested, OffboardControlRequested, ParamDefaultsRequested,
368 ParamEventQueues, RcTrimCalibrationRequested, ResetOriginRequested, VersionRequested,
369 },
370 packets::{RcPacket, RosflightPacketHeader},
371 rc::Rc,
372 state_machine::{Event, StateManager},
373 test_support::TestBoard,
374 };
375
376 #[derive(Default)]
377 struct PersistBoard {
378 read_count: usize,
379 write_count: usize,
380 stored_system_id: i32,
381 }
382
383 impl BoardIo for PersistBoard {
384 fn serial_rx_read(&mut self, _buf: &mut [u8]) -> Option<Result<usize, errors::TelemError>> {
385 None
386 }
387
388 fn serial_tx_write(&mut self, bytes: &[u8]) -> Option<Result<usize, errors::TelemError>> {
389 Some(Ok(bytes.len()))
390 }
391
392 fn clock_millis(&self) -> u32 {
393 0
394 }
395
396 fn clock_micros(&self) -> u64 {
397 0
398 }
399
400 fn read_params(&mut self, params: &mut Params) -> bool {
401 self.read_count += 1;
402 params.set_by_id(
403 ParamId::PARAM_SYSTEM_ID,
404 ParamValue::Int(self.stored_system_id),
405 );
406 true
407 }
408
409 fn write_params(&mut self, params: &Params) -> bool {
410 self.write_count += 1;
411 self.stored_system_id = match params.get_by_id(ParamId::PARAM_SYSTEM_ID) {
412 ParamValue::Int(value) => value,
413 _ => 0,
414 };
415 true
416 }
417 }
418
419 fn initialized_state() -> StateManager {
420 let params = Params::new();
421 let mut state = StateManager::new();
422 state.update(Event::INITIALIZED, ¶ms);
423 state
424 }
425
426 fn armed_state() -> StateManager {
427 let mut params = Params::new();
428 params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.1));
429 let mut state = initialized_state();
430 state.update_arming_safety(true, true);
431 state.update(Event::REQUEST_ARM, ¶ms);
432 assert!(state.is_armed());
433 state
434 }
435
436 fn test_ctx<'a, B, C>(
437 requests: &'a mut CommandEventQueues,
438 param_events: &'a mut ParamEventQueues,
439 comm_events: &'a mut CommEventQueues,
440 state: &'a StateManager,
441 command: &'a mut CommandManager,
442 controller: &'a mut C,
443 board: &'a mut B,
444 flags: &'a mut CalibrationFlags,
445 params: &'a mut Params,
446 ) -> CommandRequestCtx<'a, B, C>
447 where
448 B: BoardIo,
449 C: RcTrimCalibrator,
450 {
451 CommandRequestCtx {
452 requests,
453 param_events,
454 comm_events,
455 state,
456 command,
457 controller,
458 board,
459 flags,
460 params,
461 }
462 }
463
464 #[test]
465 fn apply_calibration_requests_sets_requested_flags() {
466 let mut requests = CommandEventQueues::default();
467 let mut comm_events = CommEventQueues::default();
468 let mut param_events = ParamEventQueues::default();
469 let mut flags = CalibrationFlags::empty();
470 let mut params = Params::new();
471 let mut command = CommandManager::new();
472 let mut controller = QuadController::<f64>::default();
473 let mut board = TestBoard::default();
474 params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.4));
475 params.set_by_id(ParamId::PARAM_BARO_BIAS, ParamValue::Float(1000.0));
476
477 let _ = requests.calibration_requests.push(CalibrationRequested {
478 command: RosflightCmd::GyroCalibration,
479 });
480 let _ = requests.calibration_requests.push(CalibrationRequested {
481 command: RosflightCmd::BaroCalibration,
482 });
483 let state = initialized_state();
484
485 apply_calibration_requests(&mut test_ctx(
486 &mut requests,
487 &mut param_events,
488 &mut comm_events,
489 &state,
490 &mut command,
491 &mut controller,
492 &mut board,
493 &mut flags,
494 &mut params,
495 ));
496
497 assert!(flags.contains(CalibrationFlags::GYRO));
498 assert!(flags.contains(CalibrationFlags::BARO));
499 assert_eq!(
500 params.get_by_id(ParamId::PARAM_GYRO_X_BIAS),
501 ParamValue::Float(0.0)
502 );
503 assert_eq!(
506 params.get_by_id(ParamId::PARAM_BARO_BIAS),
507 ParamValue::Float(1000.0)
508 );
509 assert_eq!(comm_events.responses.len(), 2);
510 for expected in [RosflightCmd::GyroCalibration, RosflightCmd::BaroCalibration] {
511 match comm_events.responses.pop().unwrap() {
512 CommResponse::CmdAck(ack) => {
513 assert!(matches!(ack.command, command if command == expected));
514 assert!(matches!(
515 ack.success,
516 RosflightCmdResponse::RosflightCmdSuccess
517 ));
518 }
519 _ => panic!("expected command ack response"),
520 }
521 }
522 assert!(requests.calibration_requests.is_empty());
523 }
524
525 #[test]
526 fn accel_calibration_starts_full_imu_calibration_and_zeros_biases() {
527 let mut requests = CommandEventQueues::default();
528 let mut comm_events = CommEventQueues::default();
529 let mut param_events = ParamEventQueues::default();
530 let mut flags = CalibrationFlags::empty();
531 let mut params = Params::new();
532 let mut command = CommandManager::new();
533 let mut controller = QuadController::<f64>::default();
534 let mut board = TestBoard::default();
535 params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.4));
536 params.set_by_id(ParamId::PARAM_ACC_Z_BIAS, ParamValue::Float(-0.2));
537 let _ = requests.calibration_requests.push(CalibrationRequested {
538 command: RosflightCmd::AccelCalibration,
539 });
540 let state = initialized_state();
541
542 apply_calibration_requests(&mut test_ctx(
543 &mut requests,
544 &mut param_events,
545 &mut comm_events,
546 &state,
547 &mut command,
548 &mut controller,
549 &mut board,
550 &mut flags,
551 &mut params,
552 ));
553
554 assert!(flags.contains(CalibrationFlags::IMU));
555 assert_eq!(
556 params.get_by_id(ParamId::PARAM_GYRO_X_BIAS),
557 ParamValue::Float(0.0)
558 );
559 assert_eq!(
560 params.get_by_id(ParamId::PARAM_ACC_Z_BIAS),
561 ParamValue::Float(0.0)
562 );
563 match comm_events.responses.pop().unwrap() {
564 CommResponse::CmdAck(ack) => {
565 assert!(matches!(ack.command, RosflightCmd::AccelCalibration));
566 assert!(matches!(
567 ack.success,
568 RosflightCmdResponse::RosflightCmdSuccess
569 ));
570 }
571 _ => panic!("expected command ack response"),
572 }
573 }
574
575 #[test]
576 fn apply_offboard_control_requests_updates_command_manager() {
577 let mut params = Params::new();
578 let mut command = CommandManager::new();
579 let mut requests = CommandEventQueues::default();
580 let mut comm_events = CommEventQueues::default();
581 let mut param_events = ParamEventQueues::default();
582 let state = initialized_state();
583 let mut flags = CalibrationFlags::empty();
584 let mut controller = QuadController::<f64>::default();
585 let mut board = TestBoard::default();
586
587 let _ = requests
588 .offboard_control_requests
589 .push(OffboardControlRequested {
590 now_us: 42_000,
591 msg: OffboardControlMsg {
592 mode: OffboardControlMode::ModeRollratePitchrateYawrateThrottle,
593 ignore: OffboardControlIgnore::IGNORE_QY,
594 qx: 0.1,
595 qy: 0.2,
596 qz: 0.3,
597 fx: 0.4,
598 fy: 0.5,
599 fz: 0.6,
600 passthrough: [0.0; 4],
601 },
602 });
603
604 apply_offboard_control_requests(&mut test_ctx(
605 &mut requests,
606 &mut param_events,
607 &mut comm_events,
608 &state,
609 &mut command,
610 &mut controller,
611 &mut board,
612 &mut flags,
613 &mut params,
614 ));
615
616 assert!(command.is_offboard_active());
617 assert!(requests.offboard_control_requests.is_empty());
618 }
619
620 #[test]
621 fn apply_offboard_control_requests_stores_requests_while_disarmed() {
622 let mut params = Params::new();
623 let mut command = CommandManager::new();
624 let mut requests = CommandEventQueues::default();
625 let mut comm_events = CommEventQueues::default();
626 let mut param_events = ParamEventQueues::default();
627 let state = initialized_state();
628 let mut flags = CalibrationFlags::empty();
629 let mut controller = QuadController::<f64>::default();
630 let mut board = TestBoard::default();
631
632 let _ = requests
633 .offboard_control_requests
634 .push(OffboardControlRequested {
635 now_us: 42_000,
636 msg: OffboardControlMsg {
637 mode: OffboardControlMode::ModePassThrough,
638 ignore: OffboardControlIgnore::empty(),
639 qx: 0.1,
640 qy: 0.2,
641 qz: 0.3,
642 fx: 0.4,
643 fy: 0.5,
644 fz: 30.0,
645 passthrough: [0.0; 4],
646 },
647 });
648
649 apply_offboard_control_requests(&mut test_ctx(
650 &mut requests,
651 &mut param_events,
652 &mut comm_events,
653 &state,
654 &mut command,
655 &mut controller,
656 &mut board,
657 &mut flags,
658 &mut params,
659 ));
660
661 assert!(command.is_offboard_active());
662 assert!(requests.offboard_control_requests.is_empty());
663 }
664
665 #[test]
666 fn apply_param_defaults_requests_resets_params_and_reports_command() {
667 let mut params = Params::new();
668 params.set_by_id(ParamId::PARAM_SYSTEM_ID, ParamValue::Int(42));
669 let mut requests = CommandEventQueues::default();
670 let mut comm_events = CommEventQueues::default();
671 let mut param_events = ParamEventQueues::default();
672 let mut command = CommandManager::new();
673 let mut controller = QuadController::<f64>::default();
674 let mut board = TestBoard::default();
675 let mut flags = CalibrationFlags::empty();
676
677 let _ = requests
678 .param_defaults_requests
679 .push(ParamDefaultsRequested {
680 command: RosflightCmd::SetParamDefaults,
681 });
682 let state = initialized_state();
683
684 apply_param_defaults_requests(&mut test_ctx(
685 &mut requests,
686 &mut param_events,
687 &mut comm_events,
688 &state,
689 &mut command,
690 &mut controller,
691 &mut board,
692 &mut flags,
693 &mut params,
694 ));
695
696 assert_eq!(
697 params.get_by_id(ParamId::PARAM_SYSTEM_ID),
698 ParamValue::Int(1)
699 );
700 assert!(param_events.full_refresh);
701 match comm_events.responses.pop().unwrap() {
702 CommResponse::CmdAck(ack) => {
703 assert!(matches!(ack.command, RosflightCmd::SetParamDefaults));
704 assert!(matches!(
705 ack.success,
706 RosflightCmdResponse::RosflightCmdSuccess
707 ));
708 }
709 _ => panic!("expected command ack response"),
710 }
711 assert!(requests.param_defaults_requests.is_empty());
712 }
713
714 #[test]
715 fn apply_board_command_requests_reports_unsupported_as_failed_ack() {
716 let mut board = TestBoard::default();
717 let mut params = Params::new();
718 let mut requests = CommandEventQueues::default();
719 let mut comm_events = CommEventQueues::default();
720 let mut param_events = ParamEventQueues::default();
721 let mut command = CommandManager::new();
722 let mut controller = QuadController::<f64>::default();
723 let mut flags = CalibrationFlags::empty();
724 let state = initialized_state();
725
726 let _ = requests.board_command_requests.push(BoardCommandRequested {
727 command: RosflightCmd::WriteParams,
728 });
729
730 apply_board_command_requests(&mut test_ctx(
731 &mut requests,
732 &mut param_events,
733 &mut comm_events,
734 &state,
735 &mut command,
736 &mut controller,
737 &mut board,
738 &mut flags,
739 &mut params,
740 ));
741
742 match comm_events.responses.pop().unwrap() {
743 CommResponse::CmdAck(ack) => {
744 assert!(matches!(ack.command, RosflightCmd::WriteParams));
745 assert!(matches!(
746 ack.success,
747 RosflightCmdResponse::RosflightCmdFailed
748 ));
749 }
750 _ => panic!("expected command ack response"),
751 }
752 assert!(requests.board_command_requests.is_empty());
753 }
754
755 #[test]
756 fn apply_board_command_requests_round_trips_persistent_params_when_disarmed() {
757 let mut board = PersistBoard {
758 stored_system_id: 77,
759 ..Default::default()
760 };
761 let mut params = Params::new();
762 params.set_by_id(ParamId::PARAM_SYSTEM_ID, ParamValue::Int(42));
763 let mut requests = CommandEventQueues::default();
764 let mut comm_events = CommEventQueues::default();
765 let mut param_events = ParamEventQueues::default();
766 let mut command = CommandManager::new();
767 let mut controller = QuadController::<f64>::default();
768 let mut flags = CalibrationFlags::empty();
769 let state = initialized_state();
770
771 let _ = requests.board_command_requests.push(BoardCommandRequested {
772 command: RosflightCmd::WriteParams,
773 });
774 let _ = requests.board_command_requests.push(BoardCommandRequested {
775 command: RosflightCmd::ReadParams,
776 });
777
778 apply_board_command_requests(&mut test_ctx(
779 &mut requests,
780 &mut param_events,
781 &mut comm_events,
782 &state,
783 &mut command,
784 &mut controller,
785 &mut board,
786 &mut flags,
787 &mut params,
788 ));
789
790 assert_eq!(board.write_count, 1);
791 assert_eq!(board.read_count, 1);
792 assert_eq!(
793 params.get_by_id(ParamId::PARAM_SYSTEM_ID),
794 ParamValue::Int(42)
795 );
796 for expected in [RosflightCmd::WriteParams, RosflightCmd::ReadParams] {
797 match comm_events.responses.pop().unwrap() {
798 CommResponse::CmdAck(ack) => {
799 assert!(matches!(ack.command, command if command == expected));
800 assert!(matches!(
801 ack.success,
802 RosflightCmdResponse::RosflightCmdSuccess
803 ));
804 }
805 _ => panic!("expected command ack response"),
806 }
807 }
808 assert!(requests.board_command_requests.is_empty());
809 }
810
811 #[test]
812 fn apply_rc_trim_calibration_requests_sets_equilibrium_torques_and_acks() {
813 let mut params = Params::new();
814 params.set_by_id(ParamId::PARAM_RC_ATTITUDE_MODE, ParamValue::Int(0));
815 params.set_by_id(ParamId::PARAM_RC_NUM_CHANNELS, ParamValue::Int(4));
816 params.set_by_id(ParamId::PARAM_RC_MAX_ROLLRATE, ParamValue::Float(1.0));
817 params.set_by_id(ParamId::PARAM_RC_MAX_PITCHRATE, ParamValue::Float(1.0));
818 params.set_by_id(ParamId::PARAM_RC_MAX_YAWRATE, ParamValue::Float(1.0));
819 params.set_by_id(ParamId::PARAM_PID_ROLL_RATE_P, ParamValue::Float(2.0));
820 params.set_by_id(ParamId::PARAM_PID_PITCH_RATE_P, ParamValue::Float(3.0));
821 params.set_by_id(ParamId::PARAM_PID_YAW_RATE_P, ParamValue::Float(4.0));
822 params.set_by_id(ParamId::PARAM_X_EQ_TORQUE, ParamValue::Float(0.5));
823 params.set_by_id(ParamId::PARAM_Y_EQ_TORQUE, ParamValue::Float(-0.5));
824 params.set_by_id(ParamId::PARAM_Z_EQ_TORQUE, ParamValue::Float(0.25));
825 let mut rc = Rc::new();
826 let mut state = crate::state_machine::StateManager::new();
827 state.update(Event::INITIALIZED, ¶ms);
828 rc.init(¶ms);
829 let mut channels = [0.5; crate::packets::RC_PACKET_CHANNELS];
830 channels[0] = 0.55;
831 channels[1] = 0.45;
832 channels[3] = 0.60;
833 rc.receive(&RcPacket {
834 header: RosflightPacketHeader {
835 timestamp: 1,
836 status: 0,
837 },
838 n_chan: 4,
839 chan: channels,
840 lol: false,
841 });
842 rc.run(0, ¶ms, &mut state);
843 let mut command = CommandManager::new();
844 command.run(0, ¶ms, &mut rc, &mut state);
845 let mut controller = QuadController::<f64>::default();
846 let mut requests = CommandEventQueues::default();
847 let mut comm_events = CommEventQueues::default();
848 let mut param_events = ParamEventQueues::default();
849 let mut flags = CalibrationFlags::empty();
850 let mut board = TestBoard::default();
851 let state = initialized_state();
852
853 let _ = requests
854 .rc_trim_calibration_requests
855 .push(RcTrimCalibrationRequested {
856 command: RosflightCmd::RcCalibration,
857 });
858
859 apply_rc_trim_calibration_requests(&mut test_ctx(
860 &mut requests,
861 &mut param_events,
862 &mut comm_events,
863 &state,
864 &mut command,
865 &mut controller,
866 &mut board,
867 &mut flags,
868 &mut params,
869 ));
870
871 assert_eq!(
872 params.get_by_id(ParamId::PARAM_X_EQ_TORQUE),
873 ParamValue::Float(0.70000005)
874 );
875 assert_eq!(
876 params.get_by_id(ParamId::PARAM_Y_EQ_TORQUE),
877 ParamValue::Float(-0.8000001)
878 );
879 assert_eq!(
880 params.get_by_id(ParamId::PARAM_Z_EQ_TORQUE),
881 ParamValue::Float(1.0500002)
882 );
883
884 match comm_events.responses.pop().unwrap() {
885 CommResponse::CmdAck(ack) => {
886 assert!(matches!(ack.command, RosflightCmd::RcCalibration));
887 assert!(matches!(
888 ack.success,
889 RosflightCmdResponse::RosflightCmdSuccess
890 ));
891 }
892 _ => panic!("expected command ack response"),
893 }
894 assert!(requests.rc_trim_calibration_requests.is_empty());
895 }
896
897 #[test]
898 fn command_requests_fail_without_mutation_when_armed() {
899 let armed = armed_state();
900 let mut comm_events = CommEventQueues::default();
901 let mut param_events = ParamEventQueues::default();
902 let mut flags = CalibrationFlags::empty();
903 let mut params = Params::new();
904 let mut requests = CommandEventQueues::default();
905 let mut command = CommandManager::new();
906 let mut controller = QuadController::<f64>::default();
907 let mut board = TestBoard::default();
908 params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(0.4));
909 let _ = requests.calibration_requests.push(CalibrationRequested {
910 command: RosflightCmd::GyroCalibration,
911 });
912
913 apply_calibration_requests(&mut test_ctx(
914 &mut requests,
915 &mut param_events,
916 &mut comm_events,
917 &armed,
918 &mut command,
919 &mut controller,
920 &mut board,
921 &mut flags,
922 &mut params,
923 ));
924
925 assert!(!flags.contains(CalibrationFlags::GYRO));
926 assert_eq!(
927 params.get_by_id(ParamId::PARAM_GYRO_X_BIAS),
928 ParamValue::Float(0.4)
929 );
930 match comm_events.responses.pop().unwrap() {
931 CommResponse::CmdAck(ack) => {
932 assert!(matches!(ack.command, RosflightCmd::GyroCalibration));
933 assert!(matches!(
934 ack.success,
935 RosflightCmdResponse::RosflightCmdFailed
936 ));
937 }
938 _ => panic!("expected command ack response"),
939 }
940
941 let mut params = Params::new();
942 params.set_by_id(ParamId::PARAM_SYSTEM_ID, ParamValue::Int(42));
943 let mut requests = CommandEventQueues::default();
944 let _ = requests
945 .param_defaults_requests
946 .push(ParamDefaultsRequested {
947 command: RosflightCmd::SetParamDefaults,
948 });
949
950 apply_param_defaults_requests(&mut test_ctx(
951 &mut requests,
952 &mut param_events,
953 &mut comm_events,
954 &armed,
955 &mut command,
956 &mut controller,
957 &mut board,
958 &mut flags,
959 &mut params,
960 ));
961
962 assert_eq!(
963 params.get_by_id(ParamId::PARAM_SYSTEM_ID),
964 ParamValue::Int(42)
965 );
966 match comm_events.responses.pop().unwrap() {
967 CommResponse::CmdAck(ack) => {
968 assert!(matches!(ack.command, RosflightCmd::SetParamDefaults));
969 assert!(matches!(
970 ack.success,
971 RosflightCmdResponse::RosflightCmdFailed
972 ));
973 }
974 _ => panic!("expected command ack response"),
975 }
976 }
977
978 #[test]
979 fn apply_version_requests_sends_version_only_when_disarmed() {
980 let state = initialized_state();
981 let mut requests = CommandEventQueues::default();
982 let mut comm_events = CommEventQueues::default();
983 let mut param_events = ParamEventQueues::default();
984 let mut command = CommandManager::new();
985 let mut controller = QuadController::<f64>::default();
986 let mut board = TestBoard::default();
987 let mut flags = CalibrationFlags::empty();
988 let mut params = Params::new();
989
990 let _ = requests.version_requests.push(VersionRequested {
991 command: RosflightCmd::SendVersion,
992 });
993
994 apply_version_requests(&mut test_ctx(
995 &mut requests,
996 &mut param_events,
997 &mut comm_events,
998 &state,
999 &mut command,
1000 &mut controller,
1001 &mut board,
1002 &mut flags,
1003 &mut params,
1004 ));
1005
1006 match comm_events.responses.pop().unwrap() {
1007 CommResponse::Version(version) => {
1008 assert_eq!(&version.version[..12], b"Veloxity 1.0");
1009 }
1010 _ => panic!("expected version response"),
1011 }
1012 match comm_events.responses.pop().unwrap() {
1013 CommResponse::CmdAck(ack) => {
1014 assert!(matches!(ack.command, RosflightCmd::SendVersion));
1015 assert!(matches!(
1016 ack.success,
1017 RosflightCmdResponse::RosflightCmdSuccess
1018 ));
1019 }
1020 _ => panic!("expected command ack response"),
1021 }
1022
1023 let armed = armed_state();
1024 let mut requests = CommandEventQueues::default();
1025 let _ = requests.version_requests.push(VersionRequested {
1026 command: RosflightCmd::SendVersion,
1027 });
1028
1029 apply_version_requests(&mut test_ctx(
1030 &mut requests,
1031 &mut param_events,
1032 &mut comm_events,
1033 &armed,
1034 &mut command,
1035 &mut controller,
1036 &mut board,
1037 &mut flags,
1038 &mut params,
1039 ));
1040
1041 match comm_events.responses.pop().unwrap() {
1042 CommResponse::CmdAck(ack) => {
1043 assert!(matches!(ack.command, RosflightCmd::SendVersion));
1044 assert!(matches!(
1045 ack.success,
1046 RosflightCmdResponse::RosflightCmdFailed
1047 ));
1048 }
1049 _ => panic!("expected command ack response"),
1050 }
1051 assert!(comm_events.responses.is_empty());
1052 }
1053
1054 #[test]
1055 fn apply_reset_origin_requests_reports_unsupported_as_failed_ack() {
1056 let mut requests = CommandEventQueues::default();
1057 let mut comm_events = CommEventQueues::default();
1058 let mut param_events = ParamEventQueues::default();
1059 let state = initialized_state();
1060 let mut command = CommandManager::new();
1061 let mut controller = QuadController::<f64>::default();
1062 let mut board = TestBoard::default();
1063 let mut flags = CalibrationFlags::empty();
1064 let mut params = Params::new();
1065
1066 let _ = requests.reset_origin_requests.push(ResetOriginRequested {
1067 command: RosflightCmd::ResetOrigin,
1068 });
1069
1070 apply_reset_origin_requests(&mut test_ctx(
1071 &mut requests,
1072 &mut param_events,
1073 &mut comm_events,
1074 &state,
1075 &mut command,
1076 &mut controller,
1077 &mut board,
1078 &mut flags,
1079 &mut params,
1080 ));
1081
1082 match comm_events.responses.pop().unwrap() {
1083 CommResponse::CmdAck(ack) => {
1084 assert!(matches!(ack.command, RosflightCmd::ResetOrigin));
1085 assert!(matches!(
1086 ack.success,
1087 RosflightCmdResponse::RosflightCmdFailed
1088 ));
1089 }
1090 _ => panic!("expected command ack response"),
1091 }
1092 assert!(requests.reset_origin_requests.is_empty());
1093 }
1094
1095 #[test]
1096 fn apply_config_info_requests_reports_unsupported_as_failed_ack() {
1097 let mut requests = CommandEventQueues::default();
1098 let mut comm_events = CommEventQueues::default();
1099 let mut param_events = ParamEventQueues::default();
1100 let state = initialized_state();
1101 let mut command = CommandManager::new();
1102 let mut controller = QuadController::<f64>::default();
1103 let mut board = TestBoard::default();
1104 let mut flags = CalibrationFlags::empty();
1105 let mut params = Params::new();
1106
1107 let _ = requests.config_info_requests.push(ConfigInfoRequested {
1108 command: RosflightCmd::SendAllConfigInfos,
1109 });
1110
1111 apply_config_info_requests(&mut test_ctx(
1112 &mut requests,
1113 &mut param_events,
1114 &mut comm_events,
1115 &state,
1116 &mut command,
1117 &mut controller,
1118 &mut board,
1119 &mut flags,
1120 &mut params,
1121 ));
1122
1123 match comm_events.responses.pop().unwrap() {
1124 CommResponse::CmdAck(ack) => {
1125 assert!(matches!(ack.command, RosflightCmd::SendAllConfigInfos));
1126 assert!(matches!(
1127 ack.success,
1128 RosflightCmdResponse::RosflightCmdFailed
1129 ));
1130 }
1131 _ => panic!("expected command ack response"),
1132 }
1133 assert!(requests.config_info_requests.is_empty());
1134 }
1135}