code.octet-stream.net Git - m17rt/blob - m17core/src/modem.rs

   1 use crate::decode::{
   2     parse_lsf, parse_packet, parse_stream, sync_burst_correlation, SyncBurst, SYNC_THRESHOLD,
   3 };
   4 use crate::encode::{
   5     encode_lsf, encode_packet, encode_stream, generate_end_of_transmission, generate_preamble,
   6 };
   7 use crate::protocol::{Frame, LsfFrame, PacketFrame, StreamFrame};
   8 use crate::shaping::RRC_48K;
   9 use log::debug;
  10
  11 pub trait Demodulator {
  12     fn demod(&mut self, sample: i16) -> Option<Frame>;
  13     fn data_carrier_detect(&self) -> bool;
  14 }
  15
  16 /// Converts a sequence of samples into frames.
  17 pub struct SoftDemodulator {
  18     /// Circular buffer of incoming samples for calculating the RRC filtered value
  19     filter_win: [i16; 81],
  20     /// Current position in filter_win
  21     filter_cursor: usize,
  22     /// Circular buffer of shaped samples for performing decodes based on the last 192 symbols
  23     rx_win: [f32; 1920],
  24     /// Current position in rx_cursor
  25     rx_cursor: usize,
  26     /// A position that we are considering decoding due to decent sync
  27     candidate: Option<DecodeCandidate>,
  28     /// How many samples have we received?
  29     sample: u64,
  30     /// Remaining samples to ignore so once we already parse a frame we flush it out in full
  31     suppress: u16,
  32 }
  33
  34 impl SoftDemodulator {
  35     pub fn new() -> Self {
  36         SoftDemodulator {
  37             filter_win: [0i16; 81],
  38             filter_cursor: 0,
  39             rx_win: [0f32; 1920],
  40             rx_cursor: 0,
  41             candidate: None,
  42             sample: 0,
  43             suppress: 0,
  44         }
  45     }
  46 }
  47
  48 impl Demodulator for SoftDemodulator {
  49     fn demod(&mut self, sample: i16) -> Option<Frame> {
  50         self.filter_win[self.filter_cursor] = sample;
  51         self.filter_cursor = (self.filter_cursor + 1) % 81;
  52         let mut out: f32 = 0.0;
  53         for i in 0..81 {
  54             let filter_idx = (self.filter_cursor + i) % 81;
  55             out += RRC_48K[i] * self.filter_win[filter_idx] as f32;
  56         }
  57
  58         self.rx_win[self.rx_cursor] = out;
  59         self.rx_cursor = (self.rx_cursor + 1) % 1920;
  60
  61         self.sample += 1;
  62
  63         if self.suppress > 0 {
  64             self.suppress -= 1;
  65             return None;
  66         }
  67
  68         let mut burst_window = [0f32; 71];
  69         for i in 0..71 {
  70             let c = (self.rx_cursor + i) % 1920;
  71             burst_window[i] = self.rx_win[c];
  72         }
  73
  74         for burst in [
  75             SyncBurst::Lsf,
  76             SyncBurst::Bert,
  77             SyncBurst::Stream,
  78             SyncBurst::Packet,
  79         ] {
  80             let (diff, max, shift) = sync_burst_correlation(burst.target(), &burst_window);
  81             if diff < SYNC_THRESHOLD {
  82                 let mut new_candidate = true;
  83                 if let Some(c) = self.candidate.as_mut() {
  84                     if diff > c.diff {
  85                         c.age += 1;
  86                         new_candidate = false;
  87                     }
  88                 }
  89                 if new_candidate {
  90                     self.candidate = Some(DecodeCandidate {
  91                         burst,
  92                         age: 1,
  93                         diff,
  94                         gain: max,
  95                         shift,
  96                     });
  97                 }
  98             }
  99             if diff >= SYNC_THRESHOLD
 100                 && self
 101                     .candidate
 102                     .as_ref()
 103                     .map(|c| c.burst == burst)
 104                     .unwrap_or(false)
 105             {
 106                 if let Some(c) = self.candidate.take() {
 107                     let start_idx = self.rx_cursor + 1920 - (c.age as usize);
 108                     let start_sample = self.sample - c.age as u64;
 109                     let mut pkt_samples = [0f32; 192];
 110                     for i in 0..192 {
 111                         let rx_idx = (start_idx + i * 10) % 1920;
 112                         pkt_samples[i] = (self.rx_win[rx_idx] - c.shift) / c.gain;
 113                     }
 114                     match c.burst {
 115                         SyncBurst::Lsf => {
 116                             debug!(
 117                                 "Found LSF at sample {} diff {} max {} shift {}",
 118                                 start_sample, c.diff, c.gain, c.shift
 119                             );
 120                             if let Some(frame) = parse_lsf(&pkt_samples) {
 121                                 self.suppress = 191 * 10;
 122                                 return Some(Frame::Lsf(frame));
 123                             }
 124                         }
 125                         SyncBurst::Bert => {
 126                             debug!("Found BERT at sample {} diff {}", start_sample, c.diff);
 127                         }
 128                         SyncBurst::Stream => {
 129                             debug!(
 130                                 "Found STREAM at sample {} diff {} max {} shift {}",
 131                                 start_sample, c.diff, c.gain, c.shift
 132                             );
 133                             if let Some(frame) = parse_stream(&pkt_samples) {
 134                                 self.suppress = 191 * 10;
 135                                 return Some(Frame::Stream(frame));
 136                             }
 137                         }
 138                         SyncBurst::Packet => {
 139                             debug!("Found PACKET at sample {} diff {}", start_sample, c.diff);
 140                             if let Some(frame) = parse_packet(&pkt_samples) {
 141                                 self.suppress = 191 * 10;
 142                                 return Some(Frame::Packet(frame));
 143                             }
 144                         }
 145                     }
 146                 }
 147             }
 148         }
 149
 150         None
 151     }
 152
 153     fn data_carrier_detect(&self) -> bool {
 154         false
 155     }
 156 }
 157
 158 impl Default for SoftDemodulator {
 159     fn default() -> Self {
 160         Self::new()
 161     }
 162 }
 163
 164 pub trait Modulator {
 165     /// Inform the modulator how many samples remain pending for output and latency updates.
 166     ///
 167     /// For the buffer between `Modulator` and the process which is supplying samples to the
 168     /// output sound card, `samples_to_play` is the number of bytes which the modulator has
 169     /// provided that have not yet been picked up, and `capacity` is the maximum size we can
 170     /// fill this particular buffer, i.e., maximum number of samples.
 171     ///
 172     /// Furthermore we attempt to track and account for the latency between the output
 173     /// soundcard callback, and when those samples will actually be on the wire. CPAL helpfully
 174     /// gives us an estimate. The latest estimate of latency is converted to a duration in terms
 175     /// of number of samples and provided as `output_latency`. Added to this is the current
 176     /// number of samples we expect remain to be processed from the last read.
 177     ///
 178     /// Call this whenever bytes have been read out of the buffer.
 179     fn update_output_buffer(
 180         &mut self,
 181         samples_to_play: usize,
 182         capacity: usize,
 183         output_latency: usize,
 184     );
 185
 186     /// Supply the next frame available from the TNC, if it was requested.
 187     fn provide_next_frame(&mut self, frame: Option<ModulatorFrame>);
 188
 189     /// Calculate and write out output samples for the soundcard.
 190     ///
 191     /// Returns the number of bytes valid in `out`. Should generally be called in a loop until
 192     /// 0 is returned.
 193     fn read_output_samples(&mut self, out: &mut [i16]) -> usize;
 194
 195     /// Run the modulator and receive actions to process.
 196     ///
 197     /// Should be called in a loop until it returns `None`.
 198     fn run(&mut self) -> Option<ModulatorAction>;
 199 }
 200
 201 pub enum ModulatorAction {
 202     /// If true, once all samples have been exhausted output should revert to equilibrium.
 203     ///
 204     /// If false, failure to pick up enough samples for output sound card is an underrun error.
 205     SetIdle(bool),
 206
 207     /// Check with the TNC if there is a frame available for transmission.
 208     ///
 209     /// Call `next_frame()` with either the next frame, or `None` if TNC has nothing more to offer.
 210     GetNextFrame,
 211
 212     /// Modulator wishes to send samples to the output buffer - call `read_output_samples`.
 213     ReadOutput,
 214
 215     /// Advise the TNC that we will complete sending End Of Transmission after the given number of
 216     /// samples has elapsed, and therefore PTT should be deasserted at this time.
 217     TransmissionWillEnd(usize),
 218 }
 219
 220 /// Frames for transmission, emitted by the TNC and received by the Modulator.
 221 ///
 222 /// The TNC is responsible for all timing decisions, making sure these frames are emitted in the
 223 /// correct order, breaks between transmissions, PTT and CSMA. If the modulator is given a
 224 /// `ModulatorFrame` value, its job is to transmit it immediately by modulating it into the output
 225 /// buffer, or otherwise directly after any previously-supplied frames.
 226 ///
 227 /// The modulator controls the rate at which frames are drawn out of the TNC. Therefore if the send
 228 /// rate is too high (or there is too much channel activity) then the effect of this backpressure is
 229 /// that the TNC's internal queues will overflow and it will either discard earlier frames in the
 230 /// current stream, or some packets awaiting transmission.
 231 pub enum ModulatorFrame {
 232     Preamble {
 233         /// TNC's configured TxDelay setting, increments of 10ms.
 234         ///
 235         /// TNC fires PTT and it's up to modulator to apply the setting, taking advantage of whatever
 236         /// buffering already exists in the sound card to reduce the artificial delay.
 237         tx_delay: u8,
 238     },
 239     Lsf(LsfFrame),
 240     Stream(StreamFrame),
 241     Packet(PacketFrame),
 242     // TODO: BertFrame
 243     EndOfTransmission,
 244 }
 245
 246 pub struct SoftModulator {
 247     /// Next modulated frame to output - 1920 samples for 40ms frame plus 80 for ramp-down
 248     next_transmission: [i16; 2000],
 249     /// How much of next_transmission should in fact be transmitted
 250     next_len: usize,
 251     /// How much of next_transmission has been read out
 252     next_read: usize,
 253     /// How many pending zero samples to emit to align start of preamble with PTT taking effect
 254     tx_delay_padding: usize,
 255
 256     /// Do we need to update idle state?
 257     update_idle: bool,
 258     /// What is that idle status?
 259     idle: bool,
 260
 261     /// Do we need to calculate a transmission end time?
 262     ///
 263     /// (True after we encoded an EOT.) We will wait until we get a precise timing update.
 264     calculate_tx_end: bool,
 265     /// Do we need to report a transmission end time?
 266     ///
 267     /// This is a duration expressed in number of samples.
 268     report_tx_end: Option<usize>,
 269
 270     /// Circular buffer of most recently output samples for calculating the RRC filtered value.
 271     ///
 272     /// This should naturally degrade to an oldest value plus 80 zeroes after an EOT.
 273     filter_win: [f32; 81],
 274     /// Current position in filter_win
 275     filter_cursor: usize,
 276
 277     /// Should we ask the TNC for another frame. True after each call to update_output_buffer.
 278     try_get_frame: bool,
 279
 280     /// Expected delay beyond the buffer to reach the DAC
 281     output_latency: usize,
 282     /// Number of samples we have placed in the buffer for the output soundcard not yet picked up.
 283     samples_in_buf: usize,
 284     /// Total size to which the output buffer is allowed to expand.
 285     buf_capacity: usize,
 286 }
 287
 288 impl SoftModulator {
 289     pub fn new() -> Self {
 290         Self {
 291             next_transmission: [0i16; 2000],
 292             next_len: 0,
 293             next_read: 0,
 294             tx_delay_padding: 0,
 295             update_idle: true,
 296             idle: true,
 297             calculate_tx_end: false,
 298             report_tx_end: None,
 299             filter_win: [0f32; 81],
 300             filter_cursor: 0,
 301             try_get_frame: false,
 302             output_latency: 0,
 303             samples_in_buf: 0,
 304             buf_capacity: 0,
 305         }
 306     }
 307
 308     fn push_sample(&mut self, dibit: f32) {
 309         // Right now we are encoding everything as 1.0-scaled dibit floats
 310         // This is a bit silly but it will do for a minute
 311         // Max theoretical gain from the RRC filter is 4.328
 312         // Let's bump everything to a baseline of 16383 / 4.328 = 3785.35
 313         // This is not particularly high but at least we won't ever hit the top
 314         self.filter_win[self.filter_cursor] = dibit * 3785.0;
 315         self.filter_cursor = (self.filter_cursor + 1) % 81;
 316         let mut out: f32 = 0.0;
 317         for i in 0..81 {
 318             let filter_idx = (self.filter_cursor + i) % 81;
 319             out += RRC_48K[i] * self.filter_win[filter_idx];
 320         }
 321         self.next_transmission[self.next_len] = out as i16;
 322         self.next_len += 1;
 323     }
 324
 325     fn request_frame_if_space(&mut self) {
 326         if self.buf_capacity - self.samples_in_buf >= 2000 {
 327             self.try_get_frame = true;
 328         }
 329     }
 330 }
 331
 332 impl Modulator for SoftModulator {
 333     fn update_output_buffer(
 334         &mut self,
 335         samples_to_play: usize,
 336         capacity: usize,
 337         output_latency: usize,
 338     ) {
 339         self.output_latency = output_latency;
 340         self.buf_capacity = capacity;
 341         self.samples_in_buf = samples_to_play;
 342
 343         if self.calculate_tx_end {
 344             self.calculate_tx_end = false;
 345             // next_transmission should already have been read out to the buffer by now
 346             // so we don't have to consider it
 347             self.report_tx_end = Some(self.samples_in_buf + self.output_latency);
 348         }
 349
 350         self.request_frame_if_space();
 351     }
 352
 353     fn provide_next_frame(&mut self, frame: Option<ModulatorFrame>) {
 354         let Some(frame) = frame else {
 355             self.try_get_frame = false;
 356             return;
 357         };
 358
 359         self.next_len = 0;
 360         self.next_read = 0;
 361
 362         match frame {
 363             ModulatorFrame::Preamble { tx_delay } => {
 364                 // TODO: Stop assuming 48 kHz everywhere. 24 kHz should be fine too.
 365                 let tx_delay_samples = tx_delay as usize * 480;
 366                 // TxDelay and output latency have the same effect - account for whichever is bigger.
 367                 // We want our sound card DAC hitting preamble right when PTT fully engages.
 368                 // The modulator calls the shots here - TNC hands over Preamble and asserts PTT, then
 369                 // waits to be told when transmission will be complete. This estimate will not be
 370                 // made and delivered until we generate the EOT frame.
 371                 self.tx_delay_padding = tx_delay_samples.max(self.output_latency);
 372
 373                 // We should be starting from a filter_win of zeroes
 374                 // Transmission is effectively smeared by 80 taps and we'll capture that in EOT
 375                 for dibit in generate_preamble() {
 376                     self.push_sample(dibit);
 377                 }
 378             }
 379             ModulatorFrame::Lsf(lsf_frame) => {
 380                 for dibit in encode_lsf(&lsf_frame) {
 381                     self.push_sample(dibit);
 382                 }
 383             }
 384             ModulatorFrame::Stream(stream_frame) => {
 385                 for dibit in encode_stream(&stream_frame) {
 386                     self.push_sample(dibit);
 387                 }
 388             }
 389             ModulatorFrame::Packet(packet_frame) => {
 390                 for dibit in encode_packet(&packet_frame) {
 391                     self.push_sample(dibit);
 392                 }
 393             }
 394             ModulatorFrame::EndOfTransmission => {
 395                 for dibit in generate_end_of_transmission() {
 396                     self.push_sample(dibit);
 397                 }
 398                 for _ in 0..80 {
 399                     // This is not a real symbol value
 400                     // However we want to flush the filter
 401                     self.push_sample(0f32);
 402                 }
 403                 self.calculate_tx_end = true;
 404             }
 405         }
 406     }
 407
 408     fn read_output_samples(&mut self, out: &mut [i16]) -> usize {
 409         let mut written = 0;
 410
 411         // if we have pre-TX padding to accommodate TxDelay then expend that first
 412         if self.tx_delay_padding > 0 {
 413             let len = out.len().max(self.tx_delay_padding);
 414             self.tx_delay_padding -= len;
 415             for x in 0..len {
 416                 out[x] = 0;
 417             }
 418             written += len;
 419         }
 420
 421         // then follow it with whatever might be left in next_transmission
 422         let next_remaining = self.next_len - self.next_read;
 423         if next_remaining > 0 {
 424             let len = (out.len() - written).max(next_remaining);
 425             out[written..(written + len)]
 426                 .copy_from_slice(&self.next_transmission[self.next_read..(self.next_read + len)]);
 427             self.next_read += len;
 428             written += len;
 429         }
 430
 431         written
 432     }
 433
 434     fn run(&mut self) -> Option<ModulatorAction> {
 435         // Time-sensitive for accuracy, so handle it first
 436         if let Some(end) = self.report_tx_end.take() {
 437             return Some(ModulatorAction::TransmissionWillEnd(end));
 438         }
 439
 440         if self.next_read < self.next_len {
 441             return Some(ModulatorAction::ReadOutput);
 442         }
 443
 444         if self.update_idle {
 445             self.update_idle = false;
 446             return Some(ModulatorAction::SetIdle(self.idle));
 447         }
 448
 449         if self.try_get_frame {
 450             return Some(ModulatorAction::GetNextFrame);
 451         }
 452
 453         None
 454     }
 455 }
 456
 457 #[derive(Debug)]
 458 pub(crate) struct DecodeCandidate {
 459     burst: SyncBurst,
 460     age: u8,
 461     diff: f32,
 462     gain: f32,
 463     shift: f32,
 464 }