ModemBase.h

Go to the documentation of this file.

#pragma once
 
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
 
#include "AudioTools.h"

namespace tinymodem {

template <class T = int16_t>
class FSKToneGenerator : public audio_tools::FloatSineGenerator<T> {
 public:
  void setFrequency(float frequency) override {
    this->m_frequency = frequency;
    if (frequency == 0.0f) this->m_cycles = 0.0f;
  }
 
  T readSample() override {
    if (this->m_frequency == 0.0f) return 0;
    float value = this->m_amplitude * sinf(2.0f * (float)M_PI * this->m_cycles);
    this->m_cycles += this->m_frequency * this->m_deltaTime;
    this->m_cycles -= floorf(this->m_cycles);
    return (T)lroundf(value);
  }
};

class ToneOutput {
 public:
  void setOutput(Print& out) { p_print = &out; }
 
  void setGenerator(audio_tools::SoundGenerator<int16_t>& generator) {
    p_generator = &generator;
  }
 
  audio_tools::SoundGenerator<int16_t>& generator() { return *p_generator; }

  bool begin(audio_tools::AudioInfo info, float volume) {
    channels = info.channels;
    audio_tools::AudioInfo gen_info = info;
    gen_info.channels = 1;  // we copy the sample to all channels ourselves
    if (!p_generator->begin(gen_info)) {
      LOGE("tone generator could not be started");
      return false;
    }
    gain = amplitude(volume);
    return true;
  }

  void tone(float freq, size_t nsamples) {
    if (p_print == nullptr) return;
    p_generator->setFrequency(freq);
    int frames_per_buffer = BUFFER_FRAMES * 2 / channels;
    while (nsamples > 0) {
      int frames = nsamples < (size_t)frames_per_buffer ? nsamples
                                                        : frames_per_buffer;
      int idx = 0;
      for (int j = 0; j < frames; j++) {
        int16_t sample = freq == 0.0f ? 0 : scale(p_generator->readSample());
        for (int ch = 0; ch < channels; ch++) buffer[idx++] = sample;
      }
      // blocking write
      const uint8_t* data = (const uint8_t*)buffer;
      size_t open = idx * sizeof(int16_t);
      while (open > 0) {
        size_t written = p_print->write(data, open);
        data += written;
        open -= written;
      }
      nsamples -= frames;
    }
  }

  static int32_t amplitude(float volume) {
    float mag = 32767.0f * volume + 0.5f;
    if (volume > 1.0f) mag = 32767;  // clamp to 1.0 to avoid overflow
    if (mag < 1) mag = 1;            // "short epsilon"
    return (int)mag;
  }
 
 protected:
  static constexpr int BUFFER_FRAMES = 64;
  Print* p_print = nullptr;
  FSKToneGenerator<int16_t> default_generator;
  audio_tools::SoundGenerator<int16_t>* p_generator = &default_generator;
  int32_t gain = 32767;  // volume in Q15
  int channels = 1;
  int16_t buffer[BUFFER_FRAMES * 2];
 
  int16_t scale(int16_t sample) {
    if (gain == 32767) return sample;
    return (int16_t)((int32_t)sample * gain / 32767);
  }
};

class SampleOutput {
 public:
  void setOutput(Print& out) { p_print = &out; }
 
  void begin(int channels, float volume) {
    this->channels = channels;
    gain = ToneOutput::amplitude(volume);
    idx = 0;
  }

  void write(float sample) {
    if (sample > 1.0f) sample = 1.0f;
    if (sample < -1.0f) sample = -1.0f;
    int16_t value = lroundf(sample * gain);
    for (int ch = 0; ch < channels; ch++) {
      buffer[idx++] = value;
      if (idx == BUFFER_SIZE) flush();
    }
  }

  void flush() {
    if (p_print == nullptr || idx == 0) {
      idx = 0;
      return;
    }
    const uint8_t* data = (const uint8_t*)buffer;
    size_t open = idx * sizeof(int16_t);
    while (open > 0) {
      size_t written = p_print->write(data, open);
      data += written;
      open -= written;
    }
    idx = 0;
  }
 
 protected:
  static constexpr int BUFFER_SIZE = 128;
  Print* p_print = nullptr;
  int16_t buffer[BUFFER_SIZE];
  int idx = 0;
  int channels = 1;
  int32_t gain = 32767;
};

class PcmInput {
 public:
  static constexpr int MAX_CHANNELS = 8;

  bool begin(int bits_per_sample, int channels, int channel) {
    if (!(bits_per_sample == 8 || bits_per_sample == 16 ||
          bits_per_sample == 32)) {
      LOGE("bits_per_sample must be 8, 16 or 32, not %d", bits_per_sample);
      return false;
    }
    if (channels < 1 || channels > MAX_CHANNELS) {
      LOGE("invalid number of channels: %d", channels);
      return false;
    }
    if (channel >= channels) {
      LOGE("channel %d is not available (channels=%d)", channel, channels);
      return false;
    }
    bits = bits_per_sample;
    int bytes = bits / 8;
    frame_bytes = bytes * channels;
    sample_offset = bytes * channel;
    partial_len = 0;
    return true;
  }

  void reset() { partial_len = 0; }

  template <class F>
  void write(const uint8_t* data, size_t len, F on_sample) {
    size_t j = 0;
    // complete a frame from the previous call
    while (partial_len > 0 && j < len) {
      partial[partial_len++] = data[j++];
      if (partial_len == frame_bytes) {
        on_sample(sampleValue(partial + sample_offset));
        partial_len = 0;
      }
    }
    // complete frames
    for (; len - j >= (size_t)frame_bytes; j += frame_bytes)
      on_sample(sampleValue(data + j + sample_offset));
    // keep the remaining bytes for the next call
    while (j < len) partial[partial_len++] = data[j++];
  }
 
 protected:
  uint8_t partial[MAX_CHANNELS * 4];
  int partial_len = 0;
  int frame_bytes = 2;
  int sample_offset = 0;
  int bits = 16;
 
  float sampleValue(const uint8_t* ptr) {
    switch (bits) {
      case 8:
        return (int8_t)ptr[0] / 128.0f;
      case 16: {
        int16_t v;
        memcpy(&v, ptr, sizeof(v));
        return v / 32768.0f;
      }
      default: {
        int32_t v;
        memcpy(&v, ptr, sizeof(v));
        return v / 2147483648.0f;
      }
    }
  }
};

class ByteQueue : public Print {
 public:
  size_t write(uint8_t ch) override {
    data.push_back(ch);
    return 1;
  }
  int available() { return data.size() - pos; }
  int peek() { return available() > 0 ? data[pos] : -1; }
  int read() {
    int result = peek();
    if (result < 0) return result;
    if (++pos == (size_t)data.size()) {
      clear();
    } else if (pos >= 64 && pos * 2 >= (size_t)data.size()) {
      // drop the consumed data
      size_t n = data.size() - pos;
      memmove(data.data(), data.data() + pos, n);
      data.resize(n);
      pos = 0;
    }
    return result;
  }
  void clear() {
    data.clear();
    pos = 0;
  }
 
 protected:
  audio_tools::Vector<uint8_t> data;
  size_t pos = 0;
};

class ModemEncoder : public audio_tools::AudioEncoder {
 public:
  void setOutput(Print& out) override { tones.setOutput(out); }

  void setGenerator(audio_tools::SoundGenerator<int16_t>& generator) {
    tones.setGenerator(generator);
  }

  audio_tools::SoundGenerator<int16_t>& getGenerator() {
    return tones.generator();
  }

  virtual bool endTransmission(size_t silence_samples = 0) = 0;

  virtual bool isTransmitting() = 0;

  virtual bool writeIdle(size_t nsamples) {
    (void)nsamples;
    return false;
  }

  void writeSilence(size_t nsamples) {
    if (*this) tones.tone(0, nsamples);
  }

  void writeTone(float freq, size_t nsamples) {
    if (*this) tones.tone(freq, nsamples);
  }

  void flush() { endTransmission(); }

  virtual float symbolRate() = 0;
 
  const char* mime() override { return "audio/pcm"; }
 
 protected:
  ToneOutput tones;
};

class ModemDecoder : public audio_tools::AudioDecoder {
 public:
  void setStatusOutput(Print& out) { p_status = &out; }

  void setQuiet(bool quiet) { is_quiet = quiet; }

  virtual bool isCarrier() = 0;

  virtual bool isFinished() = 0;

  virtual void flush() = 0;
 
  const char* mime() override { return "audio/pcm"; }
 
 protected:
  Print* p_status = nullptr;
  bool is_quiet = false;
 
  void status(const char* fmt, ...) {
    if (p_status == nullptr || is_quiet) return;
    char tmp[160];
    va_list args;
    va_start(args, fmt);
    vsnprintf(tmp, sizeof(tmp), fmt, args);
    va_end(args);
    p_status->print(tmp);
  }
};
 
}  // namespace tinymodem