You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

168 lines
4.6 KiB
C++

/*
* Project 25 IMBE Encoder/Decoder Fixed-Point implementation
* Developed by Pavel Yazev E-mail: pyazev@gmail.com
* Version 1.0 (c) Copyright 2009
*
* This 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 3, or (at your option)
* any later version.
*
* The software 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; see the file COPYING. If not, write to the Free
* Software Foundation, Inc., 51 Franklin Street, Boston, MA
* 02110-1301, USA.
*/
#include "typedef.h"
#include "globals.h"
#include "imbe.h"
#include "basic_op.h"
#include "aux_sub.h"
#include "math_sub.h"
#include "dsp_sub.h"
#include "tbls.h"
#include "pitch_ref.h"
#include <cstdio>
#include <cstdlib>
#include <cmath>
#define PITCH_REF_FRAME 221
#define MIN_INDEX 50
void pitch_ref(IMBE_PARAM *imbe_param, Cmplx16 *fft_buf)
{
Word16 i, j, index_a_save, pitch_est, tmp, shift, index_wr, up_lim;
Cmplx16 sp_rec[FFTLENGTH/2];
Word32 fund_freq, fund_freq_2, fund_freq_acc_a, fund_freq_acc_b, fund_freq_acc, L_tmp, amp_re_acc, amp_im_acc, L_sum, L_diff_min;
Word16 ha, hb, index_a, index_b, index_tbl[20], it_ind, re_tmp, im_tmp, pitch_cand=0;
Word32 fund_freq_cand=0;
pitch_est = shl(imbe_param->pitch, 7); // Convert to Q8.8
pitch_est = sub(pitch_est, CNST_1_125_Q8_8); // Sub 1.125 = 9/8
L_diff_min = MAX_32;
for(i = 0; i < 19; i++)
{
shift = norm_s(pitch_est);
tmp = shl(pitch_est, shift);
tmp = div_s(0x4000, tmp);
fund_freq = L_shl(tmp, shift + 11);
fund_freq_acc = fund_freq;
fund_freq_2 = L_shr(fund_freq, 1);
fund_freq_acc_a = L_sub(fund_freq, fund_freq_2);
fund_freq_acc_b = L_add(fund_freq, fund_freq_2);
// Calculate upper limit for spectrum reconstruction
up_lim = extract_h(L_shr((UWord32)CNST_0_9254_Q0_16 * pitch_est, 1)); // 0.9254/fund_freq
up_lim = sub(up_lim, CNST_0_5_Q8_8); // sub 0.5
up_lim = up_lim & 0xFF00; // extract fixed part
up_lim = mult(up_lim, extract_h(fund_freq));
up_lim = shr(up_lim, 1);
index_b = 0;
while(index_b <= up_lim)
{
ha = extract_h(fund_freq_acc_a);
hb = extract_h(fund_freq_acc_b);
index_a = (ha >> 8) + ((ha & 0xFF)?1:0);
index_b = (hb >> 8) + ((hb & 0xFF)?1:0);
if(index_b >= MIN_INDEX)
{
L_tmp = L_shl(L_deposit_h(index_a), 8);
L_tmp = L_sub(L_tmp, fund_freq_acc);
L_tmp = L_add(L_tmp, 0x00020000); // for rounding purpose
L_tmp = L_shr(L_tmp, 2);
index_a_save = index_a;
it_ind = 0;
amp_re_acc = amp_im_acc = 0;
while(index_a < index_b)
{
index_wr = extract_h(L_tmp);
if(index_wr < 0 && (L_tmp & 0xFFFF)) // truncating for negative number
index_wr = add(index_wr, 1);
index_wr = add(index_wr, 160);
index_tbl[it_ind++] = index_wr;
if(index_wr >= 0 && index_wr <= 320)
{
amp_re_acc = L_mac(amp_re_acc, fft_buf[index_a].re, wr_sp[index_wr]);
amp_im_acc = L_mac(amp_im_acc, fft_buf[index_a].im, wr_sp[index_wr]);
}
index_a++;
L_tmp = L_add(L_tmp, 0x400000);
}
it_ind = 0;
index_a = index_a_save;
while(index_a < index_b)
{
index_wr = index_tbl[it_ind++];
if(index_wr < 0 || index_wr > 320)
{
sp_rec[index_a].im = sp_rec[index_a].re = 0;
}
else
{
sp_rec[index_a].im = mult(mult(extract_h(amp_im_acc), wr_sp[index_wr]), 0x6666);
sp_rec[index_a].re = mult(mult(extract_h(amp_re_acc), wr_sp[index_wr]), 0x6666);
}
index_a++;
}
}
fund_freq_acc_a = L_add(fund_freq_acc_a, fund_freq);
fund_freq_acc_b = L_add(fund_freq_acc_b, fund_freq);
fund_freq_acc = L_add(fund_freq_acc, fund_freq);
}
L_sum = 0;
for(j = MIN_INDEX; j <= up_lim; j++)
{
re_tmp = sub(fft_buf[j].re, sp_rec[j].re);
im_tmp = sub(fft_buf[j].im, sp_rec[j].im);
L_sum = L_mac(L_sum, re_tmp, re_tmp);
L_sum = L_mac(L_sum, im_tmp, im_tmp);
}
if(L_sum < L_diff_min)
{
L_diff_min = L_sum;
pitch_cand = pitch_est;
fund_freq_cand = fund_freq;
}
pitch_est = add(pitch_est, CNST_0_125_Q8_8); // Add 0.125 = 1/8
}
imbe_param->ref_pitch = pitch_cand;
imbe_param->fund_freq = fund_freq_cand;
}