Files
rdsp-experiments/src/main.rs

675 lines
22 KiB
Rust

#![allow(dead_code)]
mod bfsk;
mod complex;
pub mod fft;
mod filtering;
mod iq;
mod math;
mod nco;
mod squelch;
mod ted;
mod units;
mod windows;
use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
use egui_plot::{Legend, Line, Plot};
use hound::WavWriter;
use rand::{Rng, rand_core::le, seq::index::sample};
use std::{
cell::{Cell, RefCell},
collections::VecDeque,
env::{self, args},
fmt::Display,
fs::File,
io::{BufWriter, Read, Sink, Write, stdout},
ops::DerefMut,
sync::{Arc, atomic::AtomicU64, mpsc::RecvTimeoutError},
time::Duration,
u64,
};
use tokio::{
io::{self, AsyncReadExt, AsyncWriteExt},
join,
net::{TcpSocket, TcpStream, UdpSocket},
select,
sync::mpsc::{UnboundedSender, error::TryRecvError, unbounded_channel},
time::timeout,
};
use crate::{
bfsk::BFSKMod,
complex::Complex32,
filtering::{dc_block::DCBlocker, fir::FIRFilter},
iq::IQSampler,
nco::Nco,
squelch::Squelch,
ted::elg::ELGate,
units::frequency::hz_to_rad_per_sample,
};
use eframe::egui::{self, CentralPanel, Color32, RichText};
use tokio::sync::RwLock;
use tokio::sync::mpsc::{Receiver, Sender, channel};
const BAUD_RATE: u32 = 1000;
const SAMPLE_RATE: u32 = 48000;
// Modulation parameters
const CENTER_FREQ: f32 = 1700.;
const DEVIATION: f32 = 500.;
static mut INSTANCE_ID: u32 = 0;
pub enum SampleSenderCommand {
Open,
Close,
Sample(f32),
}
pub trait SampleSender {
async fn open_link(&mut self);
async fn send_sample(&mut self, sample: f32);
async fn close_link(&mut self);
}
struct WavSampleSender {
writer: Option<WavWriter<BufWriter<File>>>,
}
impl Default for WavSampleSender {
fn default() -> Self {
Self { writer: None }
}
}
impl SampleSender for WavSampleSender {
async fn open_link(&mut self) {
let spec = hound::WavSpec {
channels: 1,
sample_rate: SAMPLE_RATE,
bits_per_sample: 16,
sample_format: hound::SampleFormat::Int,
};
self.writer = Some(hound::WavWriter::create("audio/modulated.wav", spec).unwrap());
}
async fn send_sample(&mut self, sample: f32) {
let out_sample = (sample * i16::MAX as f32) as i16;
self.writer
.as_mut()
.unwrap()
.write_sample(out_sample)
.unwrap();
}
async fn close_link(&mut self) {
self.writer = None;
}
}
struct FSKReceiver {
pos_correllator: FIRFilter,
neg_correllator: FIRFilter,
eye_sender: Sender<Vec<f32>>,
matched_lowpass: FIRFilter,
elg: ELGate,
dc_block: DCBlocker,
last_byte: u8,
frame_constructor: FrameConstructor,
bit_count: Option<u32>,
}
impl FSKReceiver {
fn new(eye_sender: Sender<Vec<f32>>) -> Self {
let samples_per_symbol = (SAMPLE_RATE as f32) / (BAUD_RATE as f32);
let correllator_length = samples_per_symbol as usize;
let mut pos_nco = Nco::new(hz_to_rad_per_sample(DEVIATION, SAMPLE_RATE as f32));
let mut neg_nco = Nco::new(hz_to_rad_per_sample(-DEVIATION, SAMPLE_RATE as f32));
let pos_ir = (0..correllator_length).map(|i| {
pos_nco.step();
pos_nco.cexp() * windows::blackmann(i as f32 / correllator_length as f32)
});
let neg_ir = (0..correllator_length).map(|i| {
neg_nco.step();
neg_nco.cexp() * windows::blackmann(i as f32 / correllator_length as f32)
});
let mut pos_correllator = FIRFilter::new(&pos_ir.collect::<Vec<_>>());
let mut neg_correllator = FIRFilter::new(&neg_ir.collect::<Vec<_>>());
pos_correllator.normalize_freq(hz_to_rad_per_sample(DEVIATION, SAMPLE_RATE as f32));
neg_correllator.normalize_freq(hz_to_rad_per_sample(-DEVIATION, SAMPLE_RATE as f32));
let mut matched_lowpass = FIRFilter::new(&vec![
Complex32::new(1., 0.);
samples_per_symbol as usize / 2
]);
matched_lowpass.normalize_freq(hz_to_rad_per_sample(DEVIATION, SAMPLE_RATE as f32));
//let mut dc_block = DCBlocker::new(0.999);
let mut dc_block = DCBlocker::new(1.);
let loop_i = 0.03;
let loop_p = 0.1;
let mut loop_ir = vec![Complex32::new(loop_i, 0.); samples_per_symbol as usize / 2];
loop_ir.push(Complex32::new(loop_p, 0.));
let mut elg = ELGate::new(samples_per_symbol, FIRFilter::new(&loop_ir));
Self {
//iq_sampler: IQSampler::new(hz_to_rad_per_sample(CENTER_FREQ, SAMPLE_RATE as f32)),
pos_correllator,
neg_correllator,
matched_lowpass,
dc_block,
elg,
last_byte: 0x00u8,
frame_constructor: FrameConstructor::new(),
bit_count: None,
eye_sender,
}
}
async fn receive(&mut self, iq: Complex32) -> Result<Option<Frame>, FrameConstructionError> {
// Frame reconstruction
let matched =
self.matched_lowpass.next_real(self.dc_block.next_real(
self.pos_correllator.next(iq).mag() - self.neg_correllator.next(iq).mag(),
));
if let Some((bit_sample, eye)) = self.elg.next_eye(matched) {
let _ = self.eye_sender.send(eye).await;
self.last_byte >>= 1;
self.last_byte |= ((bit_sample > 0.) as u8) << 7;
//last_byte <<= 1;
//last_byte |= ((bit_sample < 0.) as u8);
self.bit_count = self.bit_count.map(|x| x + 1);
if let None = self.bit_count
&& self.last_byte == 0xD8
{
// Potential frame starts
self.last_byte = 0;
self.frame_constructor = FrameConstructor::new();
self.bit_count = Some(0);
}
if let Some(8) = self.bit_count {
let frame_opt = self.frame_constructor.add_byte(self.last_byte);
self.bit_count = Some(0);
//print!("{}", last_byte as char);
print!(".{:x}.", self.last_byte);
let _ = std::io::stdout().flush();
return frame_opt;
}
}
return Ok(None);
}
}
#[derive(Debug)]
enum TransceiverState {
Waiting,
Receiving,
EOT,
SendingAck,
Sending,
Listening,
}
struct Transceiver {
tx_stream: Sender<Vec<u8>>,
rx_stream: Receiver<Vec<u8>>,
eye_receiver: Receiver<Vec<f32>>,
state_receiver: Receiver<TransceiverState>,
}
impl Transceiver {
pub async fn send(&self, data: Vec<u8>) {
self.tx_stream.send(data).await;
}
pub fn get_sender(&self) -> Sender<Vec<u8>> {
self.tx_stream.clone()
}
pub fn try_recv(&mut self) -> Result<Vec<u8>, TryRecvError> {
self.rx_stream.try_recv()
}
pub fn try_recv_eye(&mut self) -> Result<Vec<f32>, TryRecvError> {
self.eye_receiver.try_recv()
}
pub fn try_recv_state(&mut self) -> Result<TransceiverState, TryRecvError> {
self.state_receiver.try_recv()
}
pub fn start(mut sample_stream: Receiver<f32>, mut sample_sender: Sender<Vec<f32>>) -> Self {
let mut resend: Option<Vec<u8>> = None;
let (mut eyes_tx, eyes_rx) = channel::<Vec<f32>>(1024);
let (mut state_tx, state_rx) = channel::<TransceiverState>(1024);
state_tx.try_send(TransceiverState::Waiting);
let (rx_stream_sender, mut rx_stream_receiver) = channel::<Vec<u8>>(1024);
let (tx_stream_sender, mut tx_stream_receiver) = channel::<Vec<u8>>(1024);
let receiving = Arc::new(RwLock::new(false));
tokio::spawn(async move {
let mut squelch = Squelch::new(200, 0.1);
let mut iq_sampler =
IQSampler::new(hz_to_rad_per_sample(CENTER_FREQ, SAMPLE_RATE as f32));
let mut current_message = None;
loop {
select! {
_ = async {
while squelch.next(iq_sampler.sample(sample_stream.recv().await.unwrap())).is_none() {}
}
=>
{
state_tx.try_send(TransceiverState::Receiving);
// Wait for end of tranmission
let mut recv = Some(FSKReceiver::new(eyes_tx.clone()));
let mut send_ack = false;
while let Some(iq) = squelch.next(iq_sampler.sample(sample_stream.recv().await.unwrap()))
{
if recv.as_ref().is_some()
{
match recv.as_mut().unwrap().receive(iq).await
{
Ok(Some(Frame::Data(_))) => {println!("GOT DATA"); send_ack = true; recv = None; state_tx.try_send(TransceiverState::EOT);},
Ok(Some(Frame::Ack)) => {current_message = None; recv = None; state_tx.try_send(TransceiverState::EOT);},
Err(()) => {recv = None;},
_ => {}
}
}
}
if send_ack
{
state_tx.try_send(TransceiverState::SendingAck);
Self::transmit(Frame::Ack, &mut sample_sender).await;
}
state_tx.try_send(TransceiverState::Waiting);
},
message = async
{
if current_message.is_none()
{
current_message = Some((tx_stream_receiver).recv().await.unwrap());
}
state_tx.try_send(TransceiverState::Listening);
tokio::time::sleep(Duration::from_millis(500 * rand::random_range(1..6))).await;
current_message.as_ref().unwrap()
} =>
{
state_tx.try_send(TransceiverState::Sending);
println!("Sending message");
Self::transmit(Frame::Data(message.clone()), &mut sample_sender).await;
//current_message = None;
println!("Sent message");
state_tx.try_send(TransceiverState::Waiting);
}
};
}
});
Self {
eye_receiver: eyes_rx,
state_receiver: state_rx,
tx_stream: tx_stream_sender,
rx_stream: rx_stream_receiver,
}
}
pub async fn transmit(frame: Frame, samples_sender: &mut Sender<Vec<f32>>) {
let bytes = frame.bytes();
let mut bit_stream = bytes.iter().flat_map(|x| byte_to_bits(*x));
let modulator = BFSKMod::new(
(SAMPLE_RATE as f32 / BAUD_RATE as f32).round() as u32,
hz_to_rad_per_sample(DEVIATION, SAMPLE_RATE as f32),
&mut bit_stream,
);
let up_lo = Nco::new(hz_to_rad_per_sample(CENTER_FREQ, SAMPLE_RATE as f32));
let mut samples = vec![];
for (m, up) in modulator.zip(up_lo) {
let sample = m * up;
samples.push(sample.re);
}
let len = samples.len();
samples_sender.send(samples).await.unwrap();
tokio::time::sleep(Duration::from_secs_f32(len as f32 / SAMPLE_RATE as f32)).await;
}
}
enum Frame {
Data(Vec<u8>),
Ack,
}
type FrameConstructionError = ();
pub struct FrameConstructor {
frame: Vec<u8>,
frame_countdown: Option<u16>,
checksum: u8,
started: bool,
}
impl FrameConstructor {
pub fn new() -> Self {
Self {
frame: Vec::new(),
frame_countdown: None,
checksum: 0u8,
started: false,
}
}
pub fn add_byte(&mut self, byte: u8) -> Result<Option<Frame>, FrameConstructionError> {
if self.frame.is_empty() && byte != 0xC4 && byte != 0x4C && !self.started {
println!("Wrong type {:x}", byte);
self.started = true;
return Err(());
}
if self.frame.is_empty() && byte == 0xC4 && !self.started {
self.started = true;
return Ok(Some(Frame::Ack));
}
if self.frame.is_empty() && byte == 0x4C && !self.started {
self.started = true;
return Ok(None);
}
self.frame.push(byte);
// Retrieve length
if self.frame.len() == 1 {
self.frame_countdown = Some(self.frame[0] as u16);
return Ok(None);
}
if self.frame.len() == 2 {
*self.frame_countdown.as_mut().unwrap() |= (self.frame[1] as u16) << 8;
return Ok(None);
}
if self.frame_countdown.unwrap() == 0 {
// All data has been received
if self.checksum == byte {
return Ok(Some(Frame::Data(
self.frame.iter().skip(2).copied().collect(),
)));
}
println!("Checksum failed");
return Err(());
}
//self.frame.push(byte);
self.checksum ^= byte;
*self.frame_countdown.as_mut().unwrap() -= 1;
Ok(None)
}
}
impl Frame {
pub fn bytes(&self) -> Vec<u8> {
let mut output_bytes = vec![];
// Initial training sequence
output_bytes.append(&mut vec![0b01010101; 32]);
// Preamble byte
output_bytes.push(0xD8);
// Command
match self {
Frame::Data(x) => {
assert!(x.len() < 65536, "Data size over MTU");
output_bytes.push(0x4C); // DATA FRAME
let len_u16 = x.len() as u16;
output_bytes.push((len_u16 & 0xFF).try_into().unwrap());
output_bytes.push(((len_u16 >> 8) & 0xFF).try_into().unwrap());
let mut checksum = 0u8;
x.iter().for_each(|x| checksum ^= x);
output_bytes.extend(x.iter());
output_bytes.push(checksum);
}
Frame::Ack => {
output_bytes.push(0xC4); // ACK FRAME
}
}
// SEND EOT
output_bytes.extend(std::iter::repeat_n(4, 16));
output_bytes
}
}
#[tokio::main]
async fn main() {
// Read instance
let id = std::env::args().collect::<Vec<_>>()[1]
.parse::<u32>()
.expect("NO INPUT ID");
assert!(id == 0 || id == 1);
unsafe {
INSTANCE_ID = id;
};
//Transceiver::transmit(Frame::Data("Skibditoilet".repeat(100).bytes().collect::<Vec<_>>()), &mut WavSampleSender{}).await;
//Transceiver::transmit(Frame::Ack, &mut WavSampleSender::default()).await;
//return;
let native_options = eframe::NativeOptions::default();
let _ = eframe::run_native(
"Egui",
native_options,
Box::new(|cc| Ok(Box::new(EguiApp::new(cc)))),
);
}
//#[derive(Default)]
struct DummySampleSender();
impl SampleSender for DummySampleSender {
async fn open_link(&mut self) {}
async fn send_sample(&mut self, _sample: f32) {}
async fn close_link(&mut self) {}
}
struct EguiApp {
transceiver: Transceiver,
eyes: VecDeque<Vec<f32>>,
current_state: TransceiverState,
}
impl EguiApp {
fn new(_cc: &eframe::CreationContext<'_>) -> Self {
let (up_sender, mut up_receiver) = channel::<Vec<f32>>(16);
let (down_sender, down_receiver) = channel::<f32>(1024);
let transceiver = Transceiver::start(down_receiver, up_sender);
let instance_id = unsafe { INSTANCE_ID };
tokio::task::spawn(async move {
println!("Waiting for connection ...");
// let socket = Arc::new(
// UdpSocket::bind(format!("0.0.0.0:{}", 9000 + instance_id))
// .await
// .unwrap(),
// );
// socket
// .connect(format!("127.0.0.1:{}", 9000 + (1 - instance_id)))
// .await
// .unwrap();
// Receiving end
let host = cpal::default_host();
let device = host.default_input_device().expect("No input device");
let mut config = device
.supported_input_configs()
.unwrap()
.next()
.unwrap()
.with_sample_rate(cpal::SampleRate(48000));
let stream = device
.build_input_stream(
&config.into(),
move |data: &[f32], _| {
for x in data.iter() {
let _ = down_sender.blocking_send(*x * 30.); // non-blocking send
}
},
move |err| eprintln!("Stream error: {}", err),
None,
)
.unwrap();
stream.play().unwrap();
let device = host.default_output_device().unwrap();
let mut supported_configs_range = device.supported_output_configs().unwrap();
let supported_config = supported_configs_range
.find(|config| {
config.sample_format() == cpal::SampleFormat::F32
&& config.min_sample_rate().0 <= 48000
&& config.max_sample_rate().0 >= 48000
})
.expect("Device does not support 48kHz f32 output");
let config = supported_config
.with_sample_rate(cpal::SampleRate(48_000))
.config();
while let Some(stream) = up_receiver.recv().await {
let stream_len = stream.len();
let progression = Arc::new(AtomicU64::new(0));
let (finished_tx, mut finished_rx) = channel::<()>(16);
let send_stream = device
.build_output_stream(
&config,
move |data: &mut [f32], _: &cpal::OutputCallbackInfo| {
for d in data.iter_mut() {
if progression.load(std::sync::atomic::Ordering::Relaxed) as usize
== stream.len()
{
let _ = finished_tx.blocking_send(());
break;
}
*d = stream[progression
.fetch_add(1, std::sync::atomic::Ordering::Relaxed)
as usize]
* 0.1; // TODO
}
},
move |err| {
eprintln!("Stream error: {}", err);
},
None,
)
.unwrap();
send_stream.play().unwrap();
let _ = finished_rx.recv().await;
}
});
EguiApp {
transceiver,
eyes: VecDeque::new(),
current_state: TransceiverState::Waiting,
}
}
}
impl eframe::App for EguiApp {
fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) {
egui::CentralPanel::default().show(ctx, |ui| {
let max_eyes = 100;
while let Ok(eye) = self.transceiver.try_recv_eye() {
self.eyes.push_back(eye);
}
while self.eyes.len() > max_eyes {
self.eyes.pop_front();
}
if let Ok(new_state) = self.transceiver.try_recv_state() {
self.current_state = new_state;
}
ui.horizontal(|ui| {
if ui.button("Start").clicked() {
let snd = self.transceiver.get_sender();
let data = (0..rand::random_range(50..250))
.map(|_| rand::random::<char>() as u8)
.collect::<Vec<_>>();
tokio::spawn(async move {
let _ = snd.send(data).await;
});
}
ui.label(
RichText::new(format!("{:?}", self.current_state))
.size(35.)
.color(Color32::LIGHT_GREEN),
);
});
Plot::new("EyeA")
.legend(Legend::default())
.show(ui, |plot_ui| {
//plot_ui.set_auto_bounds(Vec2b { x: false, y: false });
for eye in self.eyes.iter() {
let line = Line::new(
"EyeA",
eye.iter()
.enumerate()
.map(|(i, x)| [i as f64, *x as f64])
.collect::<Vec<_>>(),
)
.color(Color32::LIGHT_GREEN);
plot_ui.line(line);
}
});
}); // Central panel
std::thread::sleep(Duration::from_millis(16));
ctx.request_repaint();
}
}
fn byte_to_bits(byte: u8) -> Vec<bool> {
vec![
byte & 1 == 1,
(byte >> 1) & 1 == 1,
(byte >> 2) & 1 == 1,
(byte >> 3) & 1 == 1,
(byte >> 4) & 1 == 1,
(byte >> 5) & 1 == 1,
(byte >> 6) & 1 == 1,
(byte >> 7) & 1 == 1,
]
}
fn bits_to_byte(bits: &[bool]) -> u8 {
bits[0] as u8
| (bits[1] as u8) << 1
| (bits[2] as u8) << 2
| (bits[3] as u8) << 3
| (bits[4] as u8) << 4
| (bits[5] as u8) << 5
| (bits[6] as u8) << 6
| (bits[7] as u8) << 7
}