1 /*
2  * Copyright (c) 2023 Huawei Device Co., Ltd.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15 
16 #include "conversion_fft.h"
17 
18 #include "sensor_log.h"
19 #include "sensors_errors.h"
20 #include "utils.h"
21 
22 #undef LOG_TAG
23 #define LOG_TAG "ConversionFFT"
24 
25 namespace OHOS {
26 namespace Sensors {
27 namespace {
28 constexpr int32_t SPECTRUM_COUNT_MAX { 8192 };
29 constexpr int32_t MAX_FFT_SIZE { 10240 };
30 }  // namespace
31 
Init(const FFTInputPara & fftPara)32 int32_t ConversionFFT::Init(const FFTInputPara &fftPara)
33 {
34     if ((fftPara.sampleRate <= 0) ||
35         (fftPara.fftSize <= 0 || !(IsPowerOfTwo(static_cast<uint32_t>(fftPara.fftSize)))) ||
36         (fftPara.hopSize <= 0 || !(IsPowerOfTwo(static_cast<uint32_t>(fftPara.hopSize))))) {
37         SEN_HILOGE("sampleRate:%{public}d, fftSize:%{public}d, hopSize:%{public}d", fftPara.sampleRate,
38             fftPara.fftSize, fftPara.hopSize);
39         return Sensors::PARAMETER_ERROR;
40     }
41     para_ = fftPara;
42     fft_.Init(para_.fftSize);
43     para_.windowSize = (fftPara.windowSize > para_.fftSize) ? fftPara.windowSize : para_.fftSize;
44     pos_ = para_.windowSize - para_.hopSize;
45     if (pos_ < 0) {
46         pos_ = 0;
47     }
48     isFrameFull_ = false;
49     isFftCalcFinish_ = false;
50     bins_ = para_.fftSize / 2;
51     fftResult_.buffer.resize(para_.fftSize, 0.0F);
52     fftResult_.window.resize(para_.fftSize, 0.0F);
53     fftResult_.magnitudes.resize(bins_, 0.0F);
54     fftResult_.magnitudesDB.resize(bins_, 0.0F);
55     fftResult_.phases.resize(bins_, 0.0F);
56     fft_.GenWindow(WND_TYPE_HANNING, para_.windowSize, fftResult_.window);
57     return Sensors::SUCCESS;
58 }
59 
ProcessSingle(float value,FFTModes mode)60 bool ConversionFFT::ProcessSingle(float value, FFTModes mode)
61 {
62     int32_t bufferSize = static_cast<int32_t>(fftResult_.buffer.size());
63     // add value to buffer at current pos
64     if (pos_ < bufferSize) {
65         fftResult_.buffer[pos_++] = value;
66     }
67     // if buffer full, run fft
68     isFrameFull_ = (pos_ == para_.windowSize);
69     if (!isFrameFull_) {
70         return isFrameFull_;
71     }
72     if (mode == ConversionFFT::WITH_POLAR_CONVERSION) {
73         fft_.CalculatePowerSpectrum(fftResult_.buffer, fftResult_.window, fftResult_.magnitudes, fftResult_.phases);
74     } else {
75         fft_.CalcFFT(fftResult_.buffer, fftResult_.window);
76     }
77     // reset pos to start of hop
78     pos_ = para_.windowSize - para_.hopSize;
79     if ((pos_ + para_.hopSize) >= bufferSize) {
80             pos_ = bufferSize - para_.hopSize;
81             SEN_HILOGW("modify pos data, pos:%{public}d", pos_);
82         }
83     /** shift buffer back by one hop size. */
84     for (int32_t i = 0; i < pos_; i++) {
85         fftResult_.buffer[i] = fftResult_.buffer[i + para_.hopSize];
86     }
87     isFftCalcFinish_ = true;
88     return isFrameFull_;
89 }
90 
Process(const std::vector<double> & values,int32_t & frameCount,std::vector<float> & frameMagsArr)91 int32_t ConversionFFT::Process(const std::vector<double> &values, int32_t &frameCount,
92     std::vector<float> &frameMagsArr)
93 {
94     FFTModes mode = ConversionFFT::WITH_POLAR_CONVERSION;
95     pos_ = para_.windowSize - para_.hopSize;
96     frameCount = 0;
97     bool isFrameFull = false;
98     size_t valuesSize = values.size();
99     for (size_t j = 0; j < valuesSize; j++) {
100         // add value to buffer at current pos
101         if (pos_ < static_cast<int32_t>(fftResult_.buffer.size())) {
102             fftResult_.buffer[pos_++] = static_cast<float>(values[j]);
103         }
104         // if buffer full, run fft
105         isFrameFull = (pos_ == para_.windowSize);
106         if (!isFrameFull) {
107             continue;
108         }
109         if (mode == ConversionFFT::WITH_POLAR_CONVERSION) {
110             fft_.CalculatePowerSpectrum(fftResult_.buffer, fftResult_.window,
111                 fftResult_.magnitudes, fftResult_.phases);
112         } else {
113             fft_.CalcFFT(fftResult_.buffer, fftResult_.window);
114         }
115         // reset pos to start of hop
116         pos_ = para_.windowSize - para_.hopSize;
117         /** shift buffer back by one hop size. */
118         for (int32_t i = 0; i < pos_; i++) {
119             fftResult_.buffer[i] = fftResult_.buffer[i + para_.hopSize];
120         }
121         isFftCalcFinish_ = true;
122         frameMagsArr.insert(frameMagsArr.end(), fftResult_.magnitudes.begin(), fftResult_.magnitudes.end());
123         ++frameCount;
124     }
125     return Sensors::SUCCESS;
126 }
127 
ConvertDB()128 std::vector<float> &ConversionFFT::ConvertDB()
129 {
130     if (isFftCalcFinish_) {
131         fft_.ConvertDB(fftResult_.magnitudes, fftResult_.magnitudesDB);
132         isFftCalcFinish_ = false;
133     }
134     return fftResult_.magnitudesDB;
135 }
136 
GetSpectralFlatness() const137 float ConversionFFT::GetSpectralFlatness() const
138 {
139     if (bins_ == 0) {
140         SEN_HILOGE("bins_ should not be 0");
141         return 0.0F;
142     }
143     float geometricMean = 0.0F;
144     float arithmaticMean = 0.0F;
145     for (int32_t i = 0; i < bins_; i++) {
146         if (fftResult_.magnitudes[i] != 0) {
147             geometricMean += logf(fftResult_.magnitudes[i]);
148         }
149         arithmaticMean += fftResult_.magnitudes[i];
150     }
151     geometricMean = expf(geometricMean / bins_);
152     arithmaticMean /= bins_;
153     bool isEqual = IsEqual(arithmaticMean, 0.0F);
154     return isEqual ? 0.0F : (geometricMean / arithmaticMean);
155 }
156 
GetSpectralCentroid() const157 float ConversionFFT::GetSpectralCentroid() const
158 {
159     float x = 0.0F;
160     float y = 0.0F;
161     for (int32_t i = 0; i < bins_; i++) {
162         x += fabs(fftResult_.magnitudes[i]) * i;
163         y += fabs(fftResult_.magnitudes[i]);
164     }
165     if (IsEqual(y, 0.0F) || (para_.fftSize == 0)) {
166         return 0.0F;
167     }
168     return ((x / y) * (static_cast<float>(para_.sampleRate) / para_.fftSize));
169 }
170 
171 // INVERSE FFT
Init(int32_t fftSize,int32_t hopSize,int32_t windowSize)172 int32_t ConversionIFFT::Init(int32_t fftSize, int32_t hopSize, int32_t windowSize)
173 {
174     if (fftSize > MAX_FFT_SIZE) {
175         SEN_HILOGE("fftSize is greater than MAX_FFT_SIZE,fftSize:%{public}d", fftSize);
176         return Sensors::ERROR;
177     }
178     fft_.Init(fftSize);
179     para_.fftSize = fftSize;
180     para_.windowSize = (windowSize > 0) ? windowSize : para_.fftSize;
181     bins_ = para_.fftSize / 2;
182     para_.hopSize = hopSize;
183     fftResult_.buffer.resize(para_.fftSize, 0.0F);
184     ifftOut_.resize(para_.fftSize, 0.0F);
185     pos_ = 0;
186     fftResult_.window.resize(para_.fftSize, 0.0F);
187     fft_.GenWindow(WND_TYPE_HANNING, para_.windowSize, fftResult_.window);
188     return Sensors::SUCCESS;
189 }
190 
Process(const std::vector<float> & mags,const std::vector<float> & phases,const FFTModes mode)191 float ConversionIFFT::Process(const std::vector<float> &mags, const std::vector<float> &phases, const FFTModes mode)
192 {
193     if (pos_ == 0) {
194         ifftOut_.clear();
195         if (mode == ConversionIFFT::SPECTRUM) {
196             fft_.InversePowerSpectrum(fftResult_.window, mags, phases, ifftOut_);
197         } else {
198             fft_.InverseFFTComplex(fftResult_.window, mags, phases, ifftOut_);
199         }
200         // add to output
201         // shift back by one hop
202         for (int32_t i = 0; i < (para_.fftSize - para_.hopSize); i++) {
203             fftResult_.buffer[i] = fftResult_.buffer[i + para_.hopSize];
204         }
205         // clear the end chunk
206         for (int32_t i = 0; i < para_.hopSize; i++) {
207             fftResult_.buffer[i + para_.fftSize - para_.hopSize] = 0.0F;
208         }
209         // merge new output
210         for (int32_t i = 0; i < para_.fftSize; i++) {
211             fftResult_.buffer[i] += ifftOut_[i];
212         }
213     }
214     nextValue_ = fftResult_.buffer[pos_];
215     // limit the values, this alg seems to spike occasionally (and break the audio drivers)
216     if (para_.hopSize == ++pos_) {
217         pos_ = 0;
218     }
219     return nextValue_;
220 }
221 
Init(float samplingRate,int32_t nBandsInTheFFT,int32_t nAveragesPerOctave)222 int32_t ConversionOctave::Init(float samplingRate, int32_t nBandsInTheFFT, int32_t nAveragesPerOctave)
223 {
224     samplingRate_ = samplingRate;
225     nSpectrum_ = nBandsInTheFFT;
226     if ((nSpectrum_ <= 0) || (nSpectrum_ > SPECTRUM_COUNT_MAX)) {
227         SEN_HILOGE("The nSpectrum_ value is invalid");
228         return Sensors::PARAMETER_ERROR;
229     }
230     spectrumFrequencySpan_ = (samplingRate / 2.0) / nSpectrum_;
231     nAverages_ = nBandsInTheFFT;
232     // fe:  2f for octave bands, sqrt(2) for half-octave bands, cuberoot(2) for third-octave bands, etc
233     // um, wtf
234     if (nAveragesPerOctave == 0) {
235         nAveragesPerOctave = 1;
236     }
237     averageFrequencyIncrement_ = pow(2.0, 1.0F / nAveragesPerOctave);
238     // this isn't currently configurable (used once here then no effect), but here's some reasoning
239     firstOctaveFrequency_ = 55.0F;
240     // for each spectrum[] bin, calculate the mapping into the appropriate average[] bin.
241     auto spe2avg = new (std::nothrow) int32_t[nSpectrum_];
242     CHKPR(spe2avg, Sensors::ERROR);
243     std::shared_ptr<int32_t> spe2avgShared(spe2avg, std::default_delete<int32_t[]>());
244     spe2avg_ = std::move(spe2avgShared);
245     int32_t avgIdx = 0;
246     float averageFreq = firstOctaveFrequency_; // the "top" of the first averaging bin
247     // we're looking for the "top" of the first spectrum bin, and i'm just sort of
248     // guessing that this is where it is (or possibly spectrumFrequencySpan/2?)
249     // ... either way it's probably close enough for these purposes
250     float spectrumFreq = spectrumFrequencySpan_;
251     for (int32_t speIdx = 0; speIdx < nSpectrum_; speIdx++) {
252         while (spectrumFreq > averageFreq) {
253             ++avgIdx;
254             averageFreq *= averageFrequencyIncrement_;
255         }
256         *(spe2avg_.get() + speIdx) = avgIdx;
257         spectrumFreq += spectrumFrequencySpan_;
258     }
259     nAverages_ = avgIdx;
260     auto averages = new (std::nothrow) float[nAverages_];
261     CHKPR(averages, Sensors::ERROR);
262     std::shared_ptr<float> averagesShared(averages, std::default_delete<float[]>());
263     averages_ = std::move(averagesShared);
264     auto peaks = new (std::nothrow) float[nAverages_];
265     CHKPR(peaks, Sensors::ERROR);
266     std::shared_ptr<float> peaksShared(peaks, std::default_delete<float[]>());
267     peaks_ = std::move(peaksShared);
268     auto peakHoldTimes = new (std::nothrow) int32_t[nAverages_];
269     CHKPR(peakHoldTimes, Sensors::ERROR);
270     std::shared_ptr<int32_t> peakHoldTimesShared(peakHoldTimes, std::default_delete<int32_t[]>());
271     peakHoldTimes_ = std::move(peakHoldTimesShared);
272 
273     peakHoldTime_ = 0;
274     peakDecayRate_ = 0.9F;
275     linearEQIntercept_ = 1.0F;
276     linearEQSlope_ = 0.0F;
277     return Sensors::SUCCESS;
278 }
279 
Calculate(const std::vector<float> & fftData)280 int32_t ConversionOctave::Calculate(const std::vector<float> &fftData)
281 {
282     if (fftData.empty()) {
283         SEN_HILOGE("fftData is empty");
284         return Sensors::PARAMETER_ERROR;
285     }
286     CHKPR(spe2avg_, Sensors::PARAMETER_ERROR);
287     CHKPR(averages_, Sensors::PARAMETER_ERROR);
288     CHKPR(peaks_, Sensors::PARAMETER_ERROR);
289     CHKPR(peakHoldTimes_, Sensors::PARAMETER_ERROR);
290     int32_t lastAvgIdx = 0; // tracks when we've crossed into a new averaging bin, so store current average
291     float sum = 0.0F;        // running total of spectrum data
292     int32_t count = 0;       // count of spectrums accumulated (for averaging)
293     for (int32_t speIdx = 0; speIdx < nSpectrum_; speIdx++) {
294         ++count;
295         sum += (fftData[speIdx] * (linearEQIntercept_ + speIdx * linearEQSlope_));
296         int32_t avgIdx = *(spe2avg_.get() + speIdx);
297         if (avgIdx != lastAvgIdx) {
298             for (int32_t j = lastAvgIdx; j < avgIdx; j++) {
299                 *(averages_.get() + j) = sum / static_cast<float>(count);
300             }
301             count = 0;
302             sum = 0.0F;
303         }
304         lastAvgIdx = avgIdx;
305     }
306     // the last average was probably not calculated...
307     if ((count > 0) && (lastAvgIdx < nAverages_)) {
308         *(averages_.get() + lastAvgIdx) = sum / count;
309     }
310     // update the peaks separately
311     for (int32_t i = 0; i < nAverages_; i++) {
312         if (IsGreatOrEqual(*(averages_.get() + i), *(peaks_.get() + i))) {
313             // save new peak level, also reset the hold timer
314             *(peaks_.get() + i) = *(averages_.get() + i);
315             *(peakHoldTimes_.get() + i) = peakHoldTime_;
316         } else {
317             // current average does not exceed peak, so hold or decay the peak
318             if (*(peakHoldTimes_.get() + i) > 0) {
319                 --*(peakHoldTimes_.get() + i);
320             } else {
321                 *(peaks_.get() + i) *= peakDecayRate_;
322             }
323         }
324     }
325     return Sensors::SUCCESS;
326 }
327 }  // namespace Sensors
328 }  // namespace OHOS