esp-csi-rs 0.4.2

ESP CSI Driver for Rust
Documentation 
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
110
111
112
113
114
115
116

use core::sync::atomic::Ordering;
use core::time;

#[cfg(feature = "statistics")]
use crate::STATS;
use crate::log_ln;
use crate::set_runtime_collection_mode;
use crate::ControlPacket;
use crate::PeripheralPacket;
use crate::CENTRAL_MAGIC_NUMBER;
use crate::IS_COLLECTOR;
use crate::STOP_SIGNAL;

use embassy_futures::select::select;
use embassy_futures::select::Either;
use embassy_time::Instant;
use embassy_time::with_timeout;
use embassy_time::Duration;
use embassy_time::Timer;
use embassy_time::WithTimeout;
use esp_radio::esp_now::BROADCAST_ADDRESS;
use esp_radio::esp_now::{EspNow, PeerInfo};

use heapless::Vec;
use zerocopy::FromBytes;
use zerocopy::IntoBytes;

use crate::EspNowConfig;

/// Run ESP-NOW in Peripheral mode.
///
/// Configures the channel and starts the responder loop that listens for
/// `ControlPacket`s from a Central node and replies with `PeripheralPacket`s.
pub async fn run_esp_now_peripheral(
    esp_now: &mut EspNow<'static>,
    config: &EspNowConfig,
    freq_hz: Option<u16>,
) {
    esp_now.set_channel(config.channel).unwrap();
    log_ln!("esp-now version {}", esp_now.version().unwrap());

    let freq = match freq_hz {
        Some(freq) => freq as u64,
        None => u16::MAX as u64,
    };

    responder(esp_now, freq).await;
}

/// Responder loop that handles ESP-NOW control packets and sends replies.
async fn responder(esp_now: &mut EspNow<'static>, frequency_hz: u64) {
    let mut is_collector = IS_COLLECTOR.load(Ordering::Relaxed);
    let initial_is_collector = is_collector;
    let mut central_mac: [u8; 6] = [0; 6];
    let mut is_connected = false;

    loop {
        match select(STOP_SIGNAL.wait(), esp_now.receive_async()).await {
            Either::First(_) => {
                STOP_SIGNAL.signal(());
                break;
            }
            Either::Second(r) => {
                let res = postcard::from_bytes::<ControlPacket>(r.data());
                match res {
                    Ok(packet) => {
                        let recv_time = Instant::now().as_micros();
                        if packet.magic_number == CENTRAL_MAGIC_NUMBER {
                            if !is_connected {
                                let _ = esp_now.add_peer(PeerInfo {
                                    interface: esp_radio::esp_now::EspNowWifiInterface::Sta,
                                    peer_address: r.info.src_address,
                                    lmk: None,
                                    channel: Some(11),
                                    encrypt: false,
                                });
                                central_mac = r.info.src_address;
                                is_connected = true;
                            }
                            if central_mac == r.info.src_address {
                                #[cfg(feature = "statistics")]
                                STATS.rx_count.fetch_add(1, Ordering::Relaxed);
                                if packet.is_collector != !is_collector {
                                    set_runtime_collection_mode(!is_collector);
                                    is_collector = !is_collector;
                                }

                                #[cfg(feature = "statistics")]
                                if packet.latency_offset != -1 {
                                    let one_way_latency = (recv_time as i64
                                        - (packet.central_send_uptime as i64 + packet.latency_offset))
                                        as i64;
                                    STATS.one_way_latency.store(one_way_latency, Ordering::Relaxed);
                                }

                                let peripheral_packet = PeripheralPacket::new(
                                    recv_time,
                                    packet.central_send_uptime.into(),
                                );
                                let message_u8: Vec<u8, 32> =
                                    postcard::to_vec(&peripheral_packet).unwrap();
                                let res = esp_now.send_async(&central_mac, &message_u8).await;
                                #[cfg(feature = "statistics")]
                                if res.is_ok() {
                                    STATS.tx_count.fetch_add(1, Ordering::Relaxed);
                                }
                            }
                        }
                    }
                    Err(_) => {}
                }
            }
        }
    }
    log_ln!("Node Stopped. Halting CSI Sending.");
}