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