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
#![allow(unused)]
use blackbox_logger::drivers::SdStorage;
use blackbox_logger::sd_card::MockSdCard;
use blackbox_logger::{Blackbox, SliceWriter, StateMachine};
use log::info;
use static_cell::StaticCell;
use crate::dispatch::{GyroPidReceiver, SetpointReceiver};
pub(crate) static BLACKBOX_CTX: StaticCell<BlackboxContext> = StaticCell::new();
pub struct BlackboxContext {
pub gyro_pid_receiver: GyroPidReceiver,
pub setpoint_receiver: SetpointReceiver,
pub blackbox: Blackbox,
pub sd_card: MockSdCard,
pub buffer: [u8; 1024],
pub pos: usize,
//pub slice_writer: SliceWriter<'static>,
}
impl BlackboxContext {
// We take the buffer as a mutable reference to the array
pub fn slice_writer(buffer: &mut [u8; 1024], pos: usize) -> SliceWriter<'_> {
SliceWriter {
// Rust automatically coerces &mut [u8; 1024] to &mut [u8]
buffer,
pos,
}
}
}
#[embassy_executor::task]
pub async fn blackbox_task(ctx: &'static mut BlackboxContext) {
info!("BLACKBOX: task started");
let mut time_us: u32 = 0;
let mut loop_count: u32 = 0;
ctx.blackbox.set_state(StateMachine::LogFileHeader);
// write the Blackbox header.
loop {
let len = {
let mut slice_writer = BlackboxContext::slice_writer(&mut ctx.buffer, ctx.pos);
ctx.blackbox.update(&mut slice_writer, time_us)
};
_ = ctx.sd_card.write_all(&ctx.buffer[..len]).await;
info!("BLACKBOX: loop {loop_count}");
loop_count = loop_count.wrapping_add(1); // use wrapping_add to handle when time rolls over at max u32.
if ctx.blackbox.state() == StateMachine::Running {
break;
}
}
loop_count = 0;
let mut index = 0;
loop {
time_us = time_us.wrapping_add(125);
let gyro_pid_msg = ctx.gyro_pid_receiver.changed().await; // blocking
let setpoint_msg = ctx.setpoint_receiver.get().await; // non-blocking
ctx.blackbox.load_telemetry(time_us, gyro_pid_msg, setpoint_msg);
let len = {
let mut slice_writer = BlackboxContext::slice_writer(&mut ctx.buffer, ctx.pos);
ctx.blackbox.update(&mut slice_writer, time_us)
};
if index == 512 {
// write End of log
let len = {
let mut slice_writer = BlackboxContext::slice_writer(&mut ctx.buffer, ctx.pos);
ctx.blackbox.logger.log_e_frame(&mut slice_writer, 255)
};
#[allow(unused)]
ctx.sd_card.write_all(&ctx.buffer[..len]).await;
info!("BLACKBOX: END OF LOG");
}
_ = ctx.sd_card.write_all(&ctx.buffer[..len]).await;
if loop_count.is_multiple_of(10) {
info!("BLACKBOX: loop {loop_count}");
}
loop_count = loop_count.wrapping_add(1); // use wrapping_add to handle when time rolls over at max u32.
index += 1;
}
/*loop {
// Wait until the sender updates the value.
// The primary client of the ahrs_receiver is the motor_mixer
let gyro_pid_msg = ctx.gyro_pid_receiver.changed().await;
ctx.blackbox.load_telemetry(time_us, gyro_pid_msg, setpoint_msg);
info!("BLACKBOX: Received time_us {}", gyro_pid_msg.time_us);
let len = 0;
info!(
"BLACKBOX: Encoded frame in {} bytes. (x: {}, y: {}, z: {}),(x: {}, y: {}, z: {})",
len,
gyro_pid_msg.gyro_rps.x,
gyro_pid_msg.gyro_rps.y,
gyro_pid_msg.gyro_rps.z,
gyro_pid_msg.gyro_rps_unfiltered.x,
gyro_pid_msg.gyro_rps_unfiltered.y,
gyro_pid_msg.gyro_rps_unfiltered.z
);
// Process logging.
/*let buf = [b'a', b'b', b'c', b'd', b'e', b'f'];
let len = 6;
ctx.sd_card.write_all(&buf[..len]).await;*/
// 3. Increment fake time (e.g., 1000us per sample for 1kHz)
time_us = time_us.wrapping_add(1000); // use wrapping_add to handle when time rolls over at max u32.
}*/
}