251 lines
8.3 KiB
Rust
251 lines
8.3 KiB
Rust
#![feature(iter_collect_into)]
|
|
use std::{ops::ControlFlow, result, sync::Arc, thread::sleep, time::Duration};
|
|
|
|
use anyhow::{Context, Ok, Result};
|
|
use common::{ControlPacket, TelemetryPacket};
|
|
use cpal::traits::{DeviceTrait, HostTrait, StreamTrait};
|
|
use pitch_detection::{detector::{mcleod::McLeodDetector, PitchDetector}, utils};
|
|
use rust_music_theory::note::{Note, NoteLetter, Pitch, Tuning};
|
|
use tokio::{io::{AsyncReadExt, AsyncWriteExt, BufWriter, WriteHalf}, net::{tcp::{OwnedReadHalf, OwnedWriteHalf}, TcpStream}, spawn, sync::{self, mpsc, RwLock}, time::timeout};
|
|
|
|
fn main() -> Result<()> {
|
|
// assumes pulseaudio system with f32 samples and 2204 sample packets
|
|
dbg!(cpal::available_hosts());
|
|
let host = cpal::default_host();
|
|
let device = host.devices().unwrap().find(|d|d.name().unwrap() == "pulse").context("no pulse")?;
|
|
|
|
let config = device.default_input_config()?;
|
|
|
|
dbg!(config.sample_format());
|
|
let rate = config.sample_rate();
|
|
|
|
let (sender, notes) = mpsc::channel(2);
|
|
|
|
const POWER_THRESHOLD: f32 = 5.0;
|
|
const CLARITY_THRESHOLD: f32 = 0.85;
|
|
const PACKET_LEN: usize = 2204;
|
|
|
|
let stream = device.build_input_stream(&config.into(),
|
|
move | data: &[f32], _: &_| {
|
|
assert!(data.len() >= PACKET_LEN);
|
|
let data = &data[..PACKET_LEN];
|
|
|
|
// reinitialized every packet as it is not thread safe
|
|
let mut detctor = McLeodDetector::new(PACKET_LEN, PACKET_LEN/2);
|
|
let vol = utils::buffer::square_sum(data);
|
|
sender.blocking_send((detctor.get_pitch(data, rate.0 as usize, POWER_THRESHOLD, CLARITY_THRESHOLD),vol)).unwrap();
|
|
|
|
},
|
|
move |err| {eprintln!("{err}")} ,
|
|
Some(Duration::from_secs(1)))?;
|
|
|
|
stream.play()?;
|
|
|
|
let executor = tokio::runtime::Runtime::new()?;
|
|
|
|
//let latest_telem = Arc::new(RwLock::new(None));
|
|
|
|
let control = executor.block_on(async {
|
|
let cruisecontrol = TcpStream::connect("169.254.1.1:1234").await?;
|
|
println!("connected");
|
|
cruisecontrol.set_nodelay(true)?;
|
|
let (telem, control) = cruisecontrol.into_split();
|
|
|
|
tokio::spawn(telemetry_handler(telem));
|
|
|
|
Ok(control)
|
|
})?;
|
|
|
|
|
|
executor.block_on(controller(notes, control)).unwrap();
|
|
|
|
drop(stream);
|
|
|
|
Ok(())
|
|
}
|
|
|
|
async fn telemetry_handler(mut telem: OwnedReadHalf) -> Result<()> {
|
|
let mut buf = vec![0; 2048];
|
|
loop {
|
|
let len = telem.read_u32().await.context("bad length")? as usize;
|
|
let data = &mut buf[0..len];
|
|
telem.read_exact(data).await?;
|
|
let telem: TelemetryPacket = postcard::from_bytes(&data)?;
|
|
|
|
println!("telem: {telem:?}");
|
|
|
|
}
|
|
}
|
|
|
|
async fn controller(mut notes: mpsc::Receiver<(Option<pitch_detection::Pitch<f32>>, f32)>, controller: OwnedWriteHalf) -> Result<()> {
|
|
let mut controller = BufWriter::new(controller);
|
|
//send_packet(&mut controller, ControlPacket::Arm(true)).await?;
|
|
//println!("armed flipper");
|
|
|
|
//let (auto_tx, mut auto_rx) = mpsc::channel(1);
|
|
//let mut auto = None;
|
|
|
|
//auto = Some(spawn(seek))
|
|
|
|
loop {
|
|
let mut control = ControlPacket::Stop;
|
|
|
|
let result::Result::Ok(note) = timeout(Duration::from_millis(95), notes.recv()).await else {
|
|
println!("timeout");
|
|
send_packet(&mut controller, ControlPacket::Stop).await?;
|
|
continue;
|
|
};
|
|
|
|
let (note,vol) = note.context("channel closed")?;
|
|
if let Some(note) = note {
|
|
dbg!(note.frequency);
|
|
if let ControlFlow::Break(_) = sax_control(&mut control, vol, ¬e) {
|
|
if let ControlFlow::Break(_) = recorder_control(&mut control, vol, note) {
|
|
continue;
|
|
}
|
|
}
|
|
|
|
send_packet(&mut controller, control).await?;
|
|
} else {
|
|
send_packet(&mut controller, ControlPacket::Twist(0.0, 0.0)).await?;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Weapon enabled
|
|
const ARMED: bool = true;
|
|
|
|
fn sax_control(control: &mut ControlPacket, vol: f32, note: &pitch_detection::Pitch<f32>) -> ControlFlow<()> {
|
|
if note.frequency < 150. {
|
|
println!("too low");
|
|
return ControlFlow::Break(());
|
|
}
|
|
if note.frequency > 270. {
|
|
println!("too high");
|
|
return ControlFlow::Break(());
|
|
}
|
|
let note = Note::from_freq(note.frequency, Tuning::EqualTemperament);
|
|
//dbg!(note.clarity);
|
|
//dbg!(vol);
|
|
match note.pitch {
|
|
Pitch { letter: NoteLetter::C, accidental: 1} => {
|
|
println!("forward");
|
|
*control = ControlPacket::Twist(1.0, 0.0);
|
|
}
|
|
//Pitch { letter: NoteLetter::A, accidental: 1} => {
|
|
// println!("backward");
|
|
// *control = ControlPacket::Twist(-1.0, 0.0);
|
|
//}
|
|
Pitch { letter: NoteLetter::B, accidental: 0} => {
|
|
println!("right");
|
|
*control = ControlPacket::Twist(0.0, 1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::A, accidental: 0} => {
|
|
println!("left");
|
|
*control = ControlPacket::Twist(0.0, -1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::G, accidental: 1} => {
|
|
if ARMED {
|
|
println!("fire");
|
|
*control = ControlPacket::Fire;
|
|
}
|
|
}
|
|
Pitch { letter: NoteLetter::F, accidental: 1} => {
|
|
println!("stop flat");
|
|
*control = ControlPacket::Stop;
|
|
}
|
|
pitch => {
|
|
if vol > 3000. {
|
|
println!("rly loud");
|
|
//control = ControlPacket::FireOverride(1.0);
|
|
dbg!(pitch);
|
|
} else {
|
|
dbg!(pitch);
|
|
}
|
|
}
|
|
}
|
|
ControlFlow::Continue(())
|
|
}
|
|
|
|
|
|
fn recorder_control(control: &mut ControlPacket, vol: f32, note: pitch_detection::Pitch<f32>) -> ControlFlow<()> {
|
|
if note.frequency < 300. {
|
|
println!("too low");
|
|
return ControlFlow::Break(());
|
|
}
|
|
if note.frequency > 600. {
|
|
println!("too high");
|
|
return ControlFlow::Break(());
|
|
}
|
|
let note = Note::from_freq(note.frequency, Tuning::EqualTemperament);
|
|
//dbg!(note.clarity);
|
|
//dbg!(vol);
|
|
match note.pitch {
|
|
Pitch { letter: NoteLetter::A, accidental: 0} => {
|
|
println!("forward");
|
|
*control = ControlPacket::Twist(1.0, 0.0);
|
|
}
|
|
Pitch { letter: NoteLetter::A, accidental: 1} => {
|
|
println!("backward");
|
|
*control = ControlPacket::Twist(-1.0, 0.0);
|
|
}
|
|
Pitch { letter: NoteLetter::C, accidental: 0} => {
|
|
println!("right");
|
|
*control = ControlPacket::Twist(0.0, 1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::C, accidental: 1} => {
|
|
println!("left");
|
|
*control = ControlPacket::Twist(0.0, -1.0);
|
|
}
|
|
Pitch { letter: NoteLetter::G, accidental: 0|1} => {
|
|
if ARMED {
|
|
println!("fire");
|
|
*control = ControlPacket::Fire;
|
|
}
|
|
}
|
|
Pitch { letter: NoteLetter::F, accidental: 1} => {
|
|
println!("stop flat");
|
|
*control = ControlPacket::Stop;
|
|
}
|
|
Pitch { letter: NoteLetter::F, accidental: 0} => {
|
|
println!("stop");
|
|
*control = ControlPacket::Stop;
|
|
}
|
|
Pitch { letter: NoteLetter::E, accidental: 0} => {
|
|
println!("stop");
|
|
*control = ControlPacket::Stop;
|
|
//if let result::Result::Ok(command) = auto_rx.try_recv() {
|
|
// control = command;
|
|
//}
|
|
}
|
|
pitch => {
|
|
if vol > 3000. {
|
|
println!("rly loud");
|
|
//control = ControlPacket::FireOverride(1.0);
|
|
} else {
|
|
dbg!(pitch);
|
|
}
|
|
}
|
|
}
|
|
ControlFlow::Continue(())
|
|
}
|
|
|
|
async fn send_packet(controller: &mut BufWriter<OwnedWriteHalf>, control: ControlPacket) -> Result<(), anyhow::Error> {
|
|
let control: heapless::Vec<u8, 2048> = postcard::to_vec(&control)?;
|
|
controller.write_u32(control.len() as u32).await?;
|
|
controller.write_all(&control).await?;
|
|
controller.flush().await?;
|
|
Ok(())
|
|
}
|
|
|
|
//async fn seek(data: TelemetryPacket) -> Result<()>{
|
|
//
|
|
// let left_tof = data.sensors.tof1.context("missing tof1")?;
|
|
// let right_tof = data.sensors.tof2.context("missing tof2")?;
|
|
//
|
|
// /// Distance (mm) where both tofs point at the same thing
|
|
// const CONVERGANCE_DISTANCE: u16 = 80;
|
|
// const DETECT_DISTANCE: u16 = 380;
|
|
//
|
|
//
|
|
//}
|