mirror of https://github.com/g4klx/MMDVM.git
128 lines
2.9 KiB
C++
128 lines
2.9 KiB
C++
/*
|
|
* Copyright (C) 2009-2015,2020 by Jonathan Naylor G4KLX
|
|
* Copyright (C) 2016 by Colin Durbridge G4EML
|
|
* Copyright (C) 2020 by Phil Taylor M0VSE
|
|
*
|
|
* 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 of the License, 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; if not, write to the Free Software
|
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
*/
|
|
|
|
#include "Config.h"
|
|
#include "Globals.h"
|
|
#include "CalFM.h"
|
|
|
|
|
|
const struct TONE_TABLE {
|
|
uint16_t frequency;
|
|
uint16_t length;
|
|
q31_t increment;
|
|
} TONE_TABLE_DATA[] = {
|
|
{2495U, 10U, 223248821},
|
|
{2079U, 12U, 186025772},
|
|
{1633U, 15U, 146118367},
|
|
{1247U, 19U, 111579672},
|
|
{1039U, 23U, 93012886},
|
|
{956U, 25U, 85541432}};
|
|
|
|
const uint8_t TONE_TABLE_DATA_LEN = 6U;
|
|
|
|
CCalFM::CCalFM() :
|
|
m_frequency(0),
|
|
m_length(0),
|
|
m_tone(NULL),
|
|
m_level(128 * 12),
|
|
m_transmit(false),
|
|
m_audioSeq(0),
|
|
m_lastState(STATE_IDLE)
|
|
{
|
|
}
|
|
|
|
void CCalFM::process()
|
|
{
|
|
const TONE_TABLE* entry = NULL;
|
|
|
|
if (m_modemState!=m_lastState)
|
|
{
|
|
switch (m_modemState) {
|
|
case STATE_FMCAL10K:
|
|
m_frequency = 956U;
|
|
break;
|
|
case STATE_FMCAL12K:
|
|
m_frequency = 1039U;
|
|
break;
|
|
case STATE_FMCAL15K:
|
|
m_frequency = 1247U;
|
|
break;
|
|
case STATE_FMCAL20K:
|
|
m_frequency = 1633U;
|
|
break;
|
|
case STATE_FMCAL25K:
|
|
m_frequency = 2079U;
|
|
break;
|
|
case STATE_FMCAL30K:
|
|
m_frequency = 2495U;
|
|
break;
|
|
default:
|
|
m_frequency = 0U;
|
|
break;
|
|
}
|
|
|
|
for (uint8_t i = 0U; i < TONE_TABLE_DATA_LEN; i++) {
|
|
if (TONE_TABLE_DATA[i].frequency == m_frequency) {
|
|
entry = TONE_TABLE_DATA + i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (entry == NULL)
|
|
return;
|
|
|
|
m_length = entry->length;
|
|
|
|
delete[] m_tone;
|
|
m_tone = new q15_t[m_length];
|
|
|
|
q31_t arg = 0;
|
|
for (uint16_t i = 0U; i < m_length; i++) {
|
|
q63_t value = ::arm_sin_q31(arg) * q63_t(m_level);
|
|
m_tone[i] = q15_t(__SSAT((value >> 31), 16));
|
|
|
|
arg += entry->increment;
|
|
}
|
|
|
|
m_lastState=m_modemState;
|
|
}
|
|
|
|
if (m_transmit)
|
|
{
|
|
uint16_t space = io.getSpace();
|
|
while (space > m_length)
|
|
{
|
|
io.write(m_modemState,m_tone,m_length);
|
|
space -= m_length;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
uint8_t CCalFM::write(const uint8_t* data, uint16_t length)
|
|
{
|
|
if (length != 1U)
|
|
return 4U;
|
|
|
|
m_transmit = data[0U] == 1U;
|
|
|
|
return 0U;
|
|
}
|