modem.cpp

C++ modem驱动代码

/**
 * FSKFilter
 */
FSKFilter::FSKFilter(int type)
    : m_fskStarted(-1),
    m_lastFiltered(0),
    m_processed(0),
    m_accSin(0),
    m_bitSamples(0)
{
    switch (type) {
	case FSKModem::ETSI:
	    break;
	default:
	    return;
    }

    m_index = 0;
    m_const = s_filterConst + type;
    // Update the sine accumulator from constants (current value after created the header)
    m_accSin = m_const->accSin;
    m_mark.init(1+m_const->spb);
    m_space.init(1+m_const->spb);
    m_lowband.init(1+m_const->spb);
}

// Process data to demodulate a bit
// Return negative if buffer ended, 0/1 if found a bit
inline int FSKFilter::getBit(short*& samples, unsigned int& len)
{
    float ds = m_const->bitLen / 32.;

    bool transition = false;
    while (len) {
	float filtered = filter(samples,len);
	// Check if this a bit transition
	if (filtered * m_lastFiltered < 0 && !transition) {
	    if (m_processed < m_const->halfBitLen)
		m_processed += ds;
	    else
		m_processed -= ds;
	    transition = true;
	}
	m_lastFiltered = filtered;
	m_processed += 1.;
	// Processed a bit: adjust clock (bit length) and return the result
	if (m_processed > m_const->bitLen) {
	    m_processed -= m_const->bitLen;
	    return filtered > 0 ? 1 : 0;;
	}
    }
    return -1;
}

// Filter data until a start bit is found (used to wait for FSK modulation to start)
// Return true if a start bit is found, false if all buffer was processed with no result
inline bool FSKFilter::waitFSK(short*& samples, unsigned int& len)
{
    if (fskStarted())
	return true;
    if (!len)
	return false;

    float tmp = 0;

    if (m_fskStarted == -1) {
	while (tmp >= -0.5) {
	    if (!len)
		return false;
	    tmp = filter(samples,len);
	}
	m_fskStarted = 0;
    }

    // Wait for 0.5 bits before starting the demodulation
    tmp = 1;
    while (tmp > 0) {
	if (len < m_const->halfSpb)
	    return false;
	for(unsigned int i = m_const->halfSpb; i; i--)
	    tmp = filter(samples,len);
    }

    m_fskStarted = 1;
    return true;
}

// Add a modulated bit to a destination buffer
inline void FSKFilter::addBit(short* samples, unsigned int& index, bool bit)
{
    // Get the number of samples nedded for this bit and advance the index
    unsigned int n = m_const->timingSamples(m_bitSamples);
    // Build and store the modulated samples
    if (bit)
	for(; n; n--) {
	    m_accSin += m_const->markCoef;
	    samples[index++] = (short)(MARK_AMPLITUDE * sin(m_accSin));
	}
    else
	for(; n; n--) {
	    m_accSin += m_const->spaceCoef;
	    samples[index++] = (short)(SPACE_AMPLITUDE * sin(m_accSin));
	}
}

// Modulate data to a buffer
// dataBits must not be 0 or greater then 8
// full=true: add data bytes (start bit + data + stop bit)
double FSKFilter::addBuffer(DataBlock& dest, const DataBlock& src,
	unsigned char dataBits, bool full)
{
    // Calculate the destination length. Add 2 more bits if full
    if (m_const)
	if (full)
	    dest.assign(0,m_const->bufLen(src.length() * (dataBits + 2)) * sizeof(short));
	else
	    dest.assign(0,m_const->bufLen(src.length() * dataBits) * sizeof(short));
    else
	dest.clear();
    if (!dest.length())
	return m_accSin;

    // Build modulated buffer
    unsigned char* srcData = (unsigned char*)(src.data());
    short* destData = (short*)(dest.data());
    unsigned int index = 0;
    if (full)
	for (unsigned int i = 0; i < src.length(); i++)
	    addByteFull(destData,index,srcData[i],dataBits);
    else
	for (unsigned int i = 0; i < src.length(); i++)
	    addByte(destData,index,srcData[i],dataBits);
    return m_accSin;
}

// Apply mark/space and low band filter
inline float FSKFilter::filter(short*& samples, unsigned int& len)
{
#define SPB m_const->spb
#define MOD(val) ((val) & SPB)

    short sample = *samples++;
    len--;

    // Mark filter
    m_mark.xbuf[MOD(m_index+6)] = sample * m_const->markGain;
    float mark = m_mark.xbuf[MOD(m_index+6)] - m_mark.xbuf[m_index]
	+ 3 * (m_mark.xbuf[MOD(m_index+2)] - m_mark.xbuf[MOD(m_index+4)]);
    for (unsigned int i = 0; i < 6; i++)
	mark += m_mark.ybuf[MOD(m_index+i)] * m_const->mark[i];
    m_mark.ybuf[MOD(m_index+6)] = mark;

    // Space filter
    m_space.xbuf[MOD(m_index+6)] = sample * m_const->spaceGain;
    float space = m_space.xbuf[MOD(m_index+6)] - m_space.xbuf[m_index]
	+ 3 * (m_space.xbuf[MOD(m_index+2)] - m_space.xbuf[MOD(m_index+4)]);
    for (unsigned int i = 0; i < 6; i++)
	space += m_space.ybuf[MOD(m_index+i)] * m_const->space[i];
    m_space.ybuf[MOD(m_index+6)] = space;

    // Low band filter
    float result = mark * mark - space * space;
    m_lowband.xbuf[MOD(m_index+6)] = result * m_const->lowbandGain;
    result =    (m_lowband.xbuf[m_index]        + m_lowband.xbuf[MOD(m_index+6)])
	 + 6  * (m_lowband.xbuf[MOD(m_index+1)] + m_lowband.xbuf[MOD(m_index+5)])
	 + 15 * (m_lowband.xbuf[MOD(m_index+2)] + m_lowband.xbuf[MOD(m_index+4)])
	 + 20 *  m_lowband.xbuf[MOD(m_index+3)];
    for (unsigned int i = 0; i < 6; i++)
	result += m_lowband.ybuf[MOD(m_index+i)] * m_const->lowband[i];
    m_lowband.ybuf[MOD(m_index+6)] = result;

    // Increase index
    m_index = MOD(m_index+1);

#undef SPB
#undef MOD

    return result;
}


/**
 * FSKModem
 */
TokenDict FSKModem::s_typeName[] = {
    {"etsi", FSKModem::ETSI},
    {0,0}
};

FSKModem::FSKModem(const NamedList& params, UART* uart)
    : m_type(ETSI),
    m_terminated(false),
    m_filter(0),
    m_uart(uart),
    m_bits(0)
{
    if (!m_uart) {
	Debug(DebugWarn,"Request to create FSK modem without UART");
	m_terminated = true;
	return;
    }

    const char* typeName = params.getValue("modemtype");
    if (typeName && *typeName)
	m_type = lookup(typeName,s_typeName);

    switch (m_type) {
	case ETSI:
	    break;
	default:
	    Debug(m_uart,DebugWarn,"Unknown modem type='%s' [%p]",typeName,m_uart);
	    m_terminated = true;
	    return;
    }

#ifdef YMODEM_BUFFER_BITS
    if (params.getBoolValue("bufferbits",false))
	m_bits = new BitBuffer;
#endif

    reset();
    XDebug(m_uart,DebugAll,"Modem created type='%s' [%p]",lookup(m_type,s_typeName),this);
}

FSKModem::~FSKModem()
{
    if (m_filter)
	delete m_filter;
    if (m_bits) {
	m_bits->printBits(m_uart);
	delete m_bits;
    }
    XDebug(m_uart,DebugAll,"Modem destroyed [%p]",this);
}

// Reset state. Clear buffer
void FSKModem::reset()
{
    m_terminated = false;
    m_buffer.clear();
    if (m_filter)
	delete m_filter;
    m_filter = 0;
    if (m_type < TypeCount)
	m_filter = new FSKFilter(m_type);
    if (m_bits)
	m_bits->reset();
}

// Data processor. Feed the collector
// Return false to stop processing
bool FSKModem::demodulate(const DataBlock& data)
{
    if (m_terminated)
	return false;
    if (!data.length())
	return true;

    // Prepare buffer to process
    void* buffer = 0;                    // Original buffer
    unsigned int len = 0;                // Data length in bytes
    if (m_buffer.length()) {
	m_buffer += data;
	buffer = m_buffer.data();
	len = m_buffer.length();
    }
    else {
	buffer = data.data();
	len = data.length();
    }
    short* samples = (short*)buffer;          // Data to process
    unsigned int count = len / sizeof(short); // The number of available samples

    XDebug(m_uart,DebugAll,"Demodulating %u bytes [%p]",len,m_uart);

    // Wait at least 6 samples to process
    while (count > 6) {
	// Check if FSK modulation was detected
	if (!m_filter->fskStarted()) {
	    if (!m_filter->waitFSK(samples,count))
		break;
	    DDebug(m_uart,DebugInfo,"FSK modulation started [%p]",m_uart);
	    m_terminated = !m_uart->fskStarted();
	    if (m_terminated)
		break;
#ifdef YMODEM_BUFFER_BITS
	    if (m_bits)
		m_bits->accumulate(false);
#endif
	    m_terminated = !m_uart->recvBit(false);
	}

	// FSK started: get bits and send them to the UART
	for (int bit = 1; bit >= 0 && !m_terminated; ) {
	    bit = m_filter->getBit(samples,count);
	    if (bit >= 0) {
#ifdef YMODEM_BUFFER_BITS
		if (m_bits)
		    m_bits->accumulate(bit);
#endif
		m_terminated = !m_uart->recvBit(bit);
	    }
	}
	break;
    }
    // Keep the unprocessed bytes
    unsigned int rest = count * sizeof(short) + len % sizeof(short);

    if (rest) {
	DataBlock tmp;
	if (m_buffer.data()) {
	   tmp.assign(buffer,len,false);
	   m_buffer.clear(false);
	}
	m_buffer.assign(samples,rest);
    }
    else
	m_buffer.clear();

    return !m_terminated;
}

// Create a buffer containing the modulated representation of a message
void FSKModem::modulate(DataBlock& dest, const DataBlock& data)
{
#ifdef DEBUG
    String tmp;
    tmp.hexify(data.data(),data.length(),' ');
    Debug(m_uart,DebugAll,"Modulating '%s' [%p]",tmp.safe(),m_uart);
#endif

    if (!(data.length() && m_filter && m_filter->constants()))
	return;

    DataBlock tmpData;
    m_filter->addBuffer(tmpData,data,m_uart->accumulator().dataBits(),true);
    dest += m_filter->constants()->header;
    dest += tmpData;
    // Build pattern after
    unsigned int nbits = (unsigned int)(m_filter->constants()->baudRate / 1000 * PATTERN_AFTER);
    DataBlock p(0,(nbits+7)/8);
    DataBlock tmpAfter;
    m_filter->addBuffer(tmpAfter,p,m_uart->accumulator().dataBits(),false);
    dest += tmpAfter;
    DDebug(m_uart,DebugAll,"Modulated header=%u data=%u pattern=%u [%p]",
	m_filter->constants()->header.length(),
	tmpData.length(),tmpAfter.length(),m_uart);
}

/* vi: set ts=8 sw=4 sts=4 noet: */