/** perform 1D FFT. iLength, iStrideIn, iStrideOut must be powers of two. */
void fastTwoPowerFourierTransform1D(unsigned int iLength,
const float32* pfRealIn,
const float32* pfImaginaryIn,
float32* pfRealOut,
float32* pfImaginaryOut,
unsigned int iStrideIn,
unsigned int iStrideOut,
bool inverse)
{
unsigned int iStrideShiftIn = log2(iStrideIn);
unsigned int iStrideShiftOut = log2(iStrideOut);
unsigned int iLogLength = log2(iLength);
bitReverse(iLength, pfRealIn, pfRealOut, iStrideShiftIn, iStrideShiftOut);
bitReverse(iLength, pfImaginaryIn, pfImaginaryOut, iStrideShiftIn, iStrideShiftOut);
float32 ca = -1.0;
float32 sa = 0.0;
unsigned int l1 = 1, l2 = 1;
for(unsigned int l=0; l < iLogLength; ++l)
{
l1 = l2;
l2 *= 2;
float32 u1 = 1.0;
float32 u2 = 0.0;
for(unsigned int j = 0; j < l1; j++)
{
for(unsigned int i = j; i < iLength; i += l2)
{
unsigned int i1 = i + l1;
float32 t1 = u1 * pfRealOut[i1<<iStrideShiftOut] - u2 * pfImaginaryOut[i1<<iStrideShiftOut];
float32 t2 = u1 * pfImaginaryOut[i1<<iStrideShiftOut] + u2 * pfRealOut[i1<<iStrideShiftOut];
pfRealOut[i1<<iStrideShiftOut] = pfRealOut[i<<iStrideShiftOut] - t1;
pfImaginaryOut[i1<<iStrideShiftOut] = pfImaginaryOut[i<<iStrideShiftOut] - t2;
pfRealOut[i<<iStrideShiftOut] += t1;
pfImaginaryOut[i<<iStrideShiftOut] += t2;
}
float32 z = u1 * ca - u2 * sa;
u2 = u1 * sa + u2 * ca;
u1 = z;
}
sa = sqrt((1.0 - ca) / 2.0);
if (!inverse)
sa = -sa;
ca = sqrt((1.0 + ca) / 2.0);
}
if (inverse) {
for (unsigned int i = 0; i < iLength; ++i) {
pfRealOut[i<<iStrideShiftOut] /= iLength;
pfImaginaryOut[i<<iStrideShiftOut] /= iLength;
}
}
}
void fftInplace(std::vector<std::complex<_REAL> >& vals, int dir)
{
assert(dir == 1 || dir == -1);
bitReverse(vals);
int n = vals.size();
for (int m = 2; m <= n; m <<= 1)
{
std::complex<_REAL> wm = std::exp(dir*2*M_PI*std::complex<_REAL>(0,1)
/ _REAL(m));
std::complex<_REAL> w = 1;
int m2 = m >> 1;
for (int j = 0; j < m2; ++j)
{
for (int k = j; k < n; k += m)
{
int k2 = k + m2;
std::complex<_REAL> t = w * vals[k2];
std::complex<_REAL> u = vals[k];
vals[k] += t;
vals[k2] = u - t;
}
w *= wm;
}
}
}
// Find the previous position using a uniform random value between 0..1, cube it, and go
// that * position back.
static uint32 findCatenaPos(uint32 pos) {
uint32 rowLength = peMemLength/(peCatenaLambda + 1); // Note: peMemLength/lambda must be power of 2
uint32 row = pos/rowLength;
if(row == 0) {
if(!peCatena3InFirstRow || rowLength < 8) {
return UINT32_MAX;
}
rowLength /= 4; // Build a Catena-3 graph in the first row
row = pos/rowLength;
if(row == 0) {
return UINT32_MAX;
}
uint32 rowPos = pos - row*rowLength;
return (row-1)*rowLength + bitReverse(rowPos, rowLength);
}
uint32 rowPos = pos - row*rowLength;
return (row-1)*rowLength + bitReverse(rowPos, rowLength);
}
uint16_t XN297_UnscramblePayload(uint8_t *data, int len, const uint8_t *rxAddr)
{
uint16_t crc = 0xb5d2;
if (rxAddr) {
for (int ii = 0; ii < RX_TX_ADDR_LEN; ++ii) {
crc = crc16_ccitt(crc, rxAddr[RX_TX_ADDR_LEN - 1 - ii]);
}
}
for (int ii = 0; ii < len; ++ii) {
crc = crc16_ccitt(crc, data[ii]);
data[ii] = bitReverse(data[ii] ^ xn297_data_scramble[ii]);
}
crc ^= xn297_crc_xorout[len];
return crc;
}
// Find the previous position using Alexander's sliding power-of-two window, with Catena
// bit-reversal.
static uint32 findSlidingReversePos(uint32 pos) {
// This is a sliding window which is the largest power of 2 < i.
if(pos < 2) {
return UINT32_MAX;
}
uint32 mask = 1;
while(mask <= pos) {
mask <<= 1;
}
mask = mask >> 1; // Mask is greatest power of 2 <= pos
uint32 reversePos = bitReverse((pos) & (mask-1), mask);
if(reversePos + 1 >= pos - mask) {
return reversePos;
}
return reversePos + mask;
}
int InitOsakanaFpFft(OsakanaFpFftContext_t** pctx, int N, int log2N)
{
int ret = 0;
OsakanaFpFftContext_t* ctx = (OsakanaFpFftContext_t*)malloc(sizeof(OsakanaFpFftContext_t));
if (ctx == NULL) {
return -1;
}
memset(ctx, 0, sizeof(OsakanaFpFftContext_t));
ctx->N = N;
ctx->log2N = log2N;
#if defined(USE_HARDCORD_TABLE)
ctx->twiddles = s_twiddlesFp[log2N-1];
ctx->bitReverseIndexTable = s_bitReverseTable[log2N-1];
ctx->bitReverseIndexTableLen = s_bitReversePairNums[log2N - 1];
#else
ctx->twiddles = (osk_fp_complex_t*)malloc(sizeof(osk_fp_complex_t) * N/2);
if (ctx->twiddles == NULL) {
ret = -3;
goto exit_error;
} printf("malloc items %d\n", sizeof(osk_fp_complex_t) * N / 2);// debug
for (int j = 0; j < N/2; j++) {
ctx->twiddles[j] = twiddle(j, N);
}
ctx->bitReverseIndexTable = (uint16_t*)malloc(sizeof(uint16_t) * N);
if (ctx->bitReverseIndexTable == NULL) {
ret = -4;
goto exit_error;
}printf("malloc reverse %d\n", sizeof(osk_fp_complex_t) * N);// debug
for (int i = 0; i < N; i++) {
ctx->bitReverseIndexTable[i] = (uint16_t)bitReverse(log2N, i);
}
#endif
*pctx = ctx;
return 0;
exit_error:
CleanOsakanaFpFft(ctx);
ctx = NULL;
*pctx = NULL;
return ret;
}
uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr)
{
uint8_t packet[NRF24L01_MAX_PAYLOAD_SIZE];
uint16_t crc = 0xb5d2;
for (int ii = 0; ii < RX_TX_ADDR_LEN; ++ii) {
packet[ii] = rxAddr[RX_TX_ADDR_LEN - 1 - ii];
crc = crc16_ccitt(crc, packet[ii]);
}
for (int ii = 0; ii < len; ++ii) {
const uint8_t bOut = bitReverse(data[ii]);
packet[ii + RX_TX_ADDR_LEN] = bOut ^ xn297_data_scramble[ii];
crc = crc16_ccitt(crc, packet[ii + RX_TX_ADDR_LEN]);
}
crc ^= xn297_crc_xorout[len];
packet[RX_TX_ADDR_LEN + len] = crc >> 8;
packet[RX_TX_ADDR_LEN + len + 1] = crc & 0xff;
return NRF24L01_WritePayload(packet, RX_TX_ADDR_LEN + len + 2);
}
inline unsigned int HuffmanDecoder::Decode(code_t code, /* out */ value_t &value) const
{
assert(m_codeToValue.size > 0);
const LookupEntry &entry = m_cache[code & m_cacheMask];
if (entry.type == 0)
{
value = entry.value;
return entry.len;
}
else
{
code_t normalizedCode = bitReverse(code);
const CodeInfo &codeInfo = (entry.type == 1)
? entry.begin[(normalizedCode << m_cacheBits) >> (MAX_CODE_BITS - (entry.len - m_cacheBits))]
: *(std::upper_bound(entry.begin, entry.end, normalizedCode, CodeLessThan)-1);
value = codeInfo.value;
return codeInfo.len;
}
}
void BitReverseTableForN(int N, int log2N, osk_bitreverse_idx_pair_t** ret_table, uint16_t* ret_count)
{
uint16_t* table = (uint16_t*)malloc(sizeof(uint16_t) * N);
memset(table, 0xFFFF, sizeof(uint16_t)*N);
for (int i = 0; i < N; i++) {
uint32_t r_idx = bitReverse(log2N, i);
if (r_idx == i) {
continue;
}
uint16_t idx_min = std::min(
static_cast<uint16_t>(i),
static_cast<uint16_t>(r_idx));
uint16_t idx_max = std::max(
static_cast<uint16_t>(i),
static_cast<uint16_t>(r_idx));
table[idx_min] = idx_max;
}
int item_num = 0;
for (int i = 0; i < N; i++) {
if (table[i] != 0xFFFF) {
item_num++;
}
}
*ret_table = (osk_bitreverse_idx_pair_t*)malloc(sizeof(osk_bitreverse_idx_pair_t) * (item_num));
int counter = 0;
for (uint16_t i = 0; i < N; i++) {
if (table[i] == 0xFFFF) {
continue;
}
(*ret_table)[counter].first = i;
(*ret_table)[counter].second = table[i];
counter++;
}
*ret_count = counter;
free(table);
}
void fft(complex<double>* a, complex<double>* b, int log2n)
{
//typedef typename std::iterator_traits<Iter_T>::value_type complex;
const complex<double> J(0, 1);
int n = 1 << log2n;
for (unsigned int i=0; i < n; ++i) {
b[bitReverse(i, log2n)] = a[i];
}
for (int s = 1; s <= log2n; ++s) {
int m = 1 << s;
int m2 = m >> 1;
complex<double> w(1, 0);
complex<double> wm = exp(-J * (PI / m2));
for (int j=0; j < m2; ++j) {
for (int k=j; k < n; k += m) {
complex<double> t = w * b[k + m2];
complex<double> u = b[k];
b[k] = u + t;
b[k + m2] = u - t;
}
w *= wm;
}
}
}
void HuffmanDecoder::Initialize(const unsigned int *codeBits, unsigned int nCodes)
{
if (nCodes == 0)
throw Err("null code");
m_maxCodeBits = *std::max_element(codeBits, codeBits+nCodes);
if (m_maxCodeBits == 0)
throw Err("null code");
SecBlock<unsigned int> blCount(m_maxCodeBits+1);
std::fill(blCount.Begin(), blCount.End(), 0);
unsigned int i;
for (i=0; i<nCodes; i++)
blCount[codeBits[i]]++;
code_t code = 0;
SecBlock<code_t> nextCode(m_maxCodeBits+1);
nextCode[1] = 0;
for (i=2; i<=m_maxCodeBits; i++)
{
// compute this while checking for overflow: code = (code + blCount[i-1]) << 1
if (code > code + blCount[i-1])
throw Err("codes oversubscribed");
code += blCount[i-1];
if (code > (code << 1))
throw Err("codes oversubscribed");
code <<= 1;
nextCode[i] = code;
}
if (code > (1 << m_maxCodeBits) - blCount[m_maxCodeBits])
throw Err("codes oversubscribed");
else if (m_maxCodeBits != 1 && code < (1 << m_maxCodeBits) - blCount[m_maxCodeBits])
throw Err("codes incomplete");
m_codeToValue.Resize(nCodes - blCount[0]);
unsigned int j=0;
for (i=0; i<nCodes; i++)
{
unsigned int len = codeBits[i];
if (len != 0)
{
code = NormalizeCode(nextCode[len]++, len);
m_codeToValue[j].code = code;
m_codeToValue[j].len = len;
m_codeToValue[j].value = i;
j++;
}
}
std::sort(m_codeToValue.Begin(), m_codeToValue.End());
m_cacheBits = STDMIN(9U, m_maxCodeBits);
m_cacheMask = (1 << m_cacheBits) - 1;
code_t leftoverMask = ~NormalizeCode(m_cacheMask, m_cacheBits);
m_cache.Resize(1 << m_cacheBits);
for (i=0; i<m_cache.size; i++)
{
code_t normalizedCode = bitReverse(i);
const CodeInfo &codeInfo = *(std::upper_bound(m_codeToValue.Begin(), m_codeToValue.End(), normalizedCode, CodeLessThan)-1);
if (codeInfo.len <= m_cacheBits)
{
m_cache[i].type = 0;
m_cache[i].value = codeInfo.value;
m_cache[i].len = codeInfo.len;
}
else
{
m_cache[i].begin = &codeInfo;
const CodeInfo *last = std::upper_bound(m_codeToValue.Begin(), m_codeToValue.End(), normalizedCode + leftoverMask, CodeLessThan)-1;
if (codeInfo.len == last->len)
{
m_cache[i].type = 1;
m_cache[i].len = codeInfo.len;
}
else
{
m_cache[i].type = 2;
m_cache[i].end = last+1;
}
}
}
}
/**
* calculates the checksum and updates the codes
*/
void Tadiran::setChecksum()
{
// generate the checksum
uint8_t crc = 0;
//for(uint8_t b=0;b<1;b++)
//{
uint8_t block = 0;
for(uint8_t i=3;i<=17;i=i+2)
{
if(codes[i] > CODE_threshold) block++;
if (i <= 15) block <<= 1;
}
uint8_t b1 = block;
//Serial.print(b1, DEC);
b1 = bitReverse(b1);
//Serial.println(b1, DEC);
b1 = b1 & 0x0F;
//Serial.println(b1, DEC);
//Serial.println("");
block = 0;
for(uint8_t i=19;i<=33;i=i+2)
{
if(codes[i] > CODE_threshold) block++;
if (i <= 31) block <<= 1;
}
uint8_t b2 = block;
//Serial.println(b2, BIN);
b2 = bitReverse(b2);
//Serial.println(b2, BIN);
b2 = b2 & 0x0F;
//Serial.print(b2, DEC);
//Serial.println("");
/*
checksum function:
((byte(0) & 0x0F) + (byte(1) & 0x0F) +
(byte(2) & 0x0F) + (byte(3) & 0x0F) +
((byte(5) & 0xF0) >> 4 ) + ((byte(6) & 0xF0) >> 4 ) +
((byte(7) & 0xF0) >> 4 ) + 10) & 0xF0
2 is for byte 2 with light on
*/
uint8_t bitSum = b1 + b2 + 2 + 10;
//Serial.print(bitSum, DEC);
//Serial.println("");
crc = bitSum & 0xF;
//Serial.print(crc, DEC);
//Serial.println("");
//crc = bitReverse(crc);
//Serial.print(crc);
//Serial.println("");
//}
// if (crc & 128) codes[123]=CODE_high;
// else codes[123]=CODE_low;
// if (crc & 64) codes[125]=CODE_high;
// else codes[125]=CODE_low;
// if (crc & 32) codes[127]=CODE_high;
// else codes[127]=CODE_low;
// if (crc & 16) codes[129]=CODE_high;
// else codes[129]=CODE_low;
if (crc & 1) codes[131]=CODE_high;
else codes[131]=CODE_low;
if (crc & 2) codes[133]=CODE_high;
else codes[133]=CODE_low;
if (crc & 4) codes[135]=CODE_high;
else codes[135]=CODE_low;
if (crc & 8) codes[137]=CODE_high;
else codes[137]=CODE_low;
}
