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

   1 use crate::decode::{parse_lsf, parse_stream, sync_burst_correlation, SyncBurst, SYNC_THRESHOLD};
   2 use crate::protocol::Frame;
   3 use crate::shaping::RRC_48K;
   4 use log::debug;
   5
   6 pub trait Demodulator {
   7     fn demod(&mut self, sample: i16) -> Option<Frame>;
   8     fn data_carrier_detect(&self) -> bool;
   9 }
  10
  11 /// Converts a sequence of samples into frames.
  12 pub struct SoftDemodulator {
  13     /// Circular buffer of incoming samples for calculating the RRC filtered value
  14     filter_win: [i16; 81],
  15     /// Current position in filter_win
  16     filter_cursor: usize,
  17     /// Circular buffer of shaped samples for performing decodes based on the last 192 symbols
  18     rx_win: [f32; 1920],
  19     /// Current position in rx_cursor
  20     rx_cursor: usize,
  21     /// A position that we are considering decoding due to decent sync
  22     candidate: Option<DecodeCandidate>,
  23     /// How many samples have we received?
  24     sample: u64,
  25     /// Remaining samples to ignore so once we already parse a frame we flush it out in full
  26     suppress: u16,
  27 }
  28
  29 impl SoftDemodulator {
  30     pub fn new() -> Self {
  31         SoftDemodulator {
  32             filter_win: [0i16; 81],
  33             filter_cursor: 0,
  34             rx_win: [0f32; 1920],
  35             rx_cursor: 0,
  36             candidate: None,
  37             sample: 0,
  38             suppress: 0,
  39         }
  40     }
  41 }
  42
  43 impl Demodulator for SoftDemodulator {
  44     fn demod(&mut self, sample: i16) -> Option<Frame> {
  45         self.filter_win[self.filter_cursor] = sample;
  46         self.filter_cursor = (self.filter_cursor + 1) % 81;
  47         let mut out: f32 = 0.0;
  48         for i in 0..81 {
  49             let filter_idx = (self.filter_cursor + i) % 81;
  50             out += RRC_48K[i] * self.filter_win[filter_idx] as f32;
  51         }
  52
  53         self.rx_win[self.rx_cursor] = out;
  54         self.rx_cursor = (self.rx_cursor + 1) % 1920;
  55
  56         self.sample += 1;
  57
  58         if self.suppress > 0 {
  59             self.suppress -= 1;
  60             return None;
  61         }
  62
  63         let mut burst_window = [0f32; 71];
  64         for i in 0..71 {
  65             let c = (self.rx_cursor + i) % 1920;
  66             burst_window[i] = self.rx_win[c];
  67         }
  68
  69         for burst in [
  70             SyncBurst::Lsf,
  71             SyncBurst::Bert,
  72             SyncBurst::Stream,
  73             SyncBurst::Packet,
  74         ] {
  75             let (diff, max, shift) = sync_burst_correlation(burst.target(), &burst_window);
  76             if diff < SYNC_THRESHOLD {
  77                 let mut new_candidate = true;
  78                 if let Some(c) = self.candidate.as_mut() {
  79                     if diff > c.diff {
  80                         c.age += 1;
  81                         new_candidate = false;
  82                     }
  83                 }
  84                 if new_candidate {
  85                     self.candidate = Some(DecodeCandidate {
  86                         burst,
  87                         age: 1,
  88                         diff,
  89                         gain: max,
  90                         shift,
  91                     });
  92                 }
  93             }
  94             if diff >= SYNC_THRESHOLD
  95                 && self
  96                     .candidate
  97                     .as_ref()
  98                     .map(|c| c.burst == burst)
  99                     .unwrap_or(false)
 100             {
 101                 if let Some(c) = self.candidate.take() {
 102                     let start_idx = self.rx_cursor + 1920 - (c.age as usize);
 103                     let start_sample = self.sample - c.age as u64;
 104                     let mut pkt_samples = [0f32; 192];
 105                     for i in 0..192 {
 106                         let rx_idx = (start_idx + i * 10) % 1920;
 107                         pkt_samples[i] = (self.rx_win[rx_idx] - c.shift) / c.gain;
 108                     }
 109                     match c.burst {
 110                         SyncBurst::Lsf => {
 111                             debug!(
 112                                 "Found LSF at sample {} diff {} max {} shift {}",
 113                                 start_sample, c.diff, c.gain, c.shift
 114                             );
 115                             if let Some(frame) = parse_lsf(&pkt_samples) {
 116                                 self.suppress = 191 * 10;
 117                                 return Some(Frame::Lsf(frame));
 118                             }
 119                         }
 120                         SyncBurst::Bert => {
 121                             debug!("Found BERT at sample {} diff {}", start_sample, c.diff);
 122                         }
 123                         SyncBurst::Stream => {
 124                             debug!(
 125                                 "Found STREAM at sample {} diff {} max {} shift {}",
 126                                 start_sample, c.diff, c.gain, c.shift
 127                             );
 128                             if let Some(frame) = parse_stream(&pkt_samples) {
 129                                 self.suppress = 191 * 10;
 130                                 return Some(Frame::Stream(frame));
 131                             }
 132                         }
 133                         SyncBurst::Packet => {
 134                             debug!("Found PACKET at sample {} diff {}", start_sample, c.diff)
 135                         }
 136                     }
 137                 }
 138             }
 139         }
 140
 141         None
 142     }
 143
 144     fn data_carrier_detect(&self) -> bool {
 145         false
 146     }
 147 }
 148
 149 impl Default for SoftDemodulator {
 150     fn default() -> Self {
 151         Self::new()
 152     }
 153 }
 154
 155 #[derive(Debug)]
 156 pub(crate) struct DecodeCandidate {
 157     burst: SyncBurst,
 158     age: u8,
 159     diff: f32,
 160     gain: f32,
 161     shift: f32,
 162 }