mirror of
https://github.com/portapack-mayhem/mayhem-firmware.git
synced 2026-09-10 16:49:36 +00:00
wf3
This commit is contained in:
HTotoo
@@ -0,0 +1,163 @@
|
||||
/*
|
||||
* Copyright (C) 2024 HTotoo
|
||||
*
|
||||
* This file is part of PortaPack.
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, Inc., 51 Franklin Street,
|
||||
* Boston, MA 02110-1301, USA.
|
||||
*/
|
||||
|
||||
#include "proc_wefaxrx.hpp"
|
||||
#include "sine_table_int8.hpp"
|
||||
#include "portapack_shared_memory.hpp"
|
||||
|
||||
#include "audio_dma.hpp"
|
||||
#include "math.h"
|
||||
#include "event_m4.hpp"
|
||||
#include "fxpt_atan2.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
|
||||
// updates the per pixel timers
|
||||
void WeFaxRx::update_params() {
|
||||
switch (ioc_mode) {
|
||||
case 1:
|
||||
freq_start_tone = 675;
|
||||
break;
|
||||
default:
|
||||
case 0:
|
||||
freq_start_tone = 300;
|
||||
break;
|
||||
}
|
||||
// 840 px / line with line start
|
||||
time_per_pixel = 60000000 / lpm * 840; // micros (595,2380952 at 120 lpm)
|
||||
pxRem = (double)baseband_fs / 8.0 / 8.0 / 4.0 / ((int)lpm * 14.0); // 840/60 = 228.57 sample / px
|
||||
samples_per_pixel = pxRem;
|
||||
pxRem -= samples_per_pixel;
|
||||
pxRoll = 0;
|
||||
}
|
||||
|
||||
double WeFaxRx::calculatePhaseAngle(int16_t i, int16_t q) {
|
||||
// return std::atan2(static_cast<double>(q), static_cast<double>(i));
|
||||
double ang = fxpt_atan2(q, i);
|
||||
return ang / 32768.0 * M_PI;
|
||||
}
|
||||
|
||||
double WeFaxRx::calculateFrequencyDeviation(complex16_t& iq, complex16_t& iqlast) {
|
||||
// Calculate phase difference between successive samples
|
||||
double phaseDiff = calculatePhaseAngle(iq.imag(), iq.real()) - calculatePhaseAngle(iqlast.imag(), iqlast.real());
|
||||
// Ensure phase difference is within -pi to pi range
|
||||
if (phaseDiff > M_PI) {
|
||||
phaseDiff -= 2.0 * M_PI;
|
||||
} else if (phaseDiff < -M_PI) {
|
||||
phaseDiff += 2.0 * M_PI;
|
||||
}
|
||||
// Calculate frequency deviation
|
||||
return (phaseDiff / (2.0 * M_PI)) * 12000.0; // (sample rate)
|
||||
}
|
||||
|
||||
void WeFaxRx::execute(const buffer_c8_t& buffer) {
|
||||
// This is called at 3072000 / 2048 = 1500Hz
|
||||
if (!configured) return;
|
||||
|
||||
const auto decim_0_out = decim_0.execute(buffer, dst_buffer); // /8 = 256
|
||||
const auto decim_1_out = decim_1.execute(decim_0_out, dst_buffer); // /8 = 32
|
||||
const auto decim_2_out = decim_2.execute(decim_1_out, dst_buffer); // /4 = 8
|
||||
// const auto channel_out = channel_filter.execute(decim_2_out, dst_buffer); // /1 = 8
|
||||
|
||||
// feed_channel_stats(channel_out);
|
||||
// auto audio = demod.execute(channel_out, audio_buffer);
|
||||
// audio_output.write(audio);
|
||||
|
||||
// todo process
|
||||
for (size_t c = 0; c < decim_2_out.count; c++) {
|
||||
cnt++;
|
||||
double freqq = calculateFrequencyDeviation(decim_2_out.p[c], iqlast);
|
||||
if (status_message.freqmin > freqq) status_message.freqmin = freqq;
|
||||
if (status_message.freqmax < freqq) status_message.freqmax = freqq;
|
||||
status_message.freqavg += (freqq - status_message.freqavg) / cnt;
|
||||
iqlast = decim_2_out.p[c];
|
||||
if (cnt >= (samples_per_pixel + (uint32_t)pxRoll)) { // got a pixel
|
||||
cnt = 0;
|
||||
if (pxRoll >= 1) pxRoll -= 1;
|
||||
pxRoll += pxRem;
|
||||
status_message.freq = freqq;
|
||||
image_message.cnt++; // saves the pixel
|
||||
if (image_message.cnt < 480) {
|
||||
image_message.image[image_message.cnt] = status_message.freqavg < 2500 ? 0 : 255; // todo remove limit, send in multiple
|
||||
/*if (status_message.freqavg >= 3000)
|
||||
image_message.image[image_message.cnt] = 255;
|
||||
else if (status_message.freqavg <= 2200)
|
||||
image_message.image[image_message.cnt] = 0;
|
||||
else {
|
||||
image_message.image[image_message.cnt] = 256 - ((3000 - status_message.freqavg) / 3.1);
|
||||
}*/
|
||||
}
|
||||
if (image_message.cnt >= 840) {
|
||||
shared_memory.application_queue.push(image_message);
|
||||
image_message.cnt = 0;
|
||||
shared_memory.application_queue.push(status_message);
|
||||
status_message.freqmin = INT32_MAX;
|
||||
status_message.freqmax = INT32_MIN;
|
||||
}
|
||||
status_message.freqavg = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void WeFaxRx::on_message(const Message* const message) {
|
||||
switch (message->id) {
|
||||
case Message::ID::WeFaxRxConfigure:
|
||||
default:
|
||||
configure(*reinterpret_cast<const WeFaxRxConfigureMessage*>(message));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void WeFaxRx::configure(const WeFaxRxConfigureMessage& message) {
|
||||
update_params();
|
||||
constexpr size_t decim_0_input_fs = baseband_fs;
|
||||
constexpr size_t decim_0_output_fs = decim_0_input_fs / decim_0.decimation_factor;
|
||||
|
||||
constexpr size_t decim_1_input_fs = decim_0_output_fs;
|
||||
constexpr size_t decim_1_output_fs = decim_1_input_fs / decim_1.decimation_factor;
|
||||
|
||||
constexpr size_t decim_2_input_fs = decim_1_output_fs;
|
||||
constexpr size_t decim_2_output_fs = decim_2_input_fs / 4;
|
||||
|
||||
constexpr size_t channel_filter_input_fs = decim_2_output_fs;
|
||||
const size_t channel_filter_output_fs = channel_filter_input_fs / 1; // 12000ul
|
||||
|
||||
lpm = message.lpm;
|
||||
ioc_mode = message.ioc;
|
||||
|
||||
decim_0.configure(taps_6k0_decim_0.taps);
|
||||
decim_1.configure(taps_6k0_decim_1.taps);
|
||||
decim_2.configure(taps_6k0_decim_2.taps, 4);
|
||||
channel_filter.configure(taps_2k8_usb_channel.taps, 1);
|
||||
demod.configure(channel_filter_output_fs, 3600);
|
||||
audio_output.configure(audio_24k_hpf_300hz_config, audio_24k_deemph_300_6_config, 0);
|
||||
configured = true;
|
||||
}
|
||||
|
||||
int main() {
|
||||
audio::dma::init_audio_out();
|
||||
EventDispatcher event_dispatcher{std::make_unique<WeFaxRx>()};
|
||||
event_dispatcher.run();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user