MLECO-2489: Minor addition for PlatformMath
Adding sine function and allowing FFTs to be computed for cases where
the FFT len is not a power of 2. In this case, the naive implementation
is used. Option for computing FFT for a complex vector has also been
added, although, the CMSIS-DSP flow needs to be tested.
Change-Id: Iad9902b946f3088de91a5f67acfb4cb3d0b00457
diff --git a/source/application/main/PlatformMath.cc b/source/application/main/PlatformMath.cc
index 0043635..505d357 100644
--- a/source/application/main/PlatformMath.cc
+++ b/source/application/main/PlatformMath.cc
@@ -34,6 +34,15 @@
#endif /* ARM_DSP_AVAILABLE */
}
+ float MathUtils::SineF32(float radians)
+ {
+#if ARM_DSP_AVAILABLE
+ return arm_sin_f32(radians);
+#else /* ARM_DSP_AVAILABLE */
+ return sin(radians);
+#endif /* ARM_DSP_AVAILABLE */
+ }
+
float MathUtils::SqrtF32(float input)
{
#if ARM_DSP_AVAILABLE
@@ -89,54 +98,144 @@
#endif /* ARM_DSP_AVAILABLE */
}
- bool MathUtils::FftInitF32(const uint16_t fftLen, arm::app::math::FftInstance& fftInstance)
+ void MathUtils::FftInitF32(const uint16_t fftLen,
+ FftInstance& fftInstance,
+ const FftType type)
{
-#if ARM_DSP_AVAILABLE
- if (!fftInstance.initialised) {
- arm_status status = arm_rfft_fast_init_f32(&fftInstance.instance, fftLen);
+ fftInstance.m_fftLen = fftLen;
+ fftInstance.m_initialised = false;
+ fftInstance.m_optimisedOptionAvailable = false;
+ fftInstance.m_type = type;
- if (ARM_MATH_SUCCESS != status) {
- return false;
- }
- fftInstance.initialised = true;
+#if ARM_DSP_AVAILABLE
+ arm_status status = ARM_MATH_ARGUMENT_ERROR;
+ switch (fftInstance.m_type) {
+ case FftType::real:
+ status = arm_rfft_fast_init_f32(&fftInstance.m_instanceReal, fftLen);
+ break;
+
+ case FftType::complex:
+ status = arm_cfft_init_f32(&fftInstance.m_instanceComplex, fftLen);
+ break;
+
+ default:
+ printf_err("Invalid FFT type\n");
+ return;
}
-#else
- UNUSED(fftLen);
- UNUSED(fftInstance);
+
+ if (ARM_MATH_SUCCESS != status) {
+ printf_err("Failed to initialise FFT for len %d\n", fftLen);
+ } else {
+ fftInstance.m_optimisedOptionAvailable = true;
+ }
#endif /* ARM_DSP_AVAILABLE */
- return true;
+
+ if (!fftInstance.m_optimisedOptionAvailable) {
+ debug("Non optimised FFT will be used\n.");
+ }
+
+ fftInstance.m_initialised = true;
+ }
+
+ static void FftRealF32(std::vector<float>& input,
+ std::vector<float>& fftOutput)
+ {
+ const size_t inputLength = input.size();
+ const size_t halfLength = input.size() / 2;
+
+ fftOutput[0] = 0;
+ fftOutput[1] = 0;
+ for (size_t t = 0; t < inputLength; t++) {
+ fftOutput[0] += input[t];
+ fftOutput[1] += input[t] *
+ MathUtils::CosineF32(2 * M_PI * halfLength * t / inputLength);
+ }
+
+ for (size_t k = 1, j = 2; k < halfLength; ++k, j += 2) {
+ float sumReal = 0;
+ float sumImag = 0;
+
+ const float theta = static_cast<float>(2 * M_PI * k / inputLength);
+
+ for (size_t t = 0; t < inputLength; t++) {
+ const auto angle = static_cast<float>(t * theta);
+ sumReal += input[t] * MathUtils::CosineF32(angle);
+ sumImag += -input[t]* MathUtils::SineF32(angle);
+ }
+
+ /* Arrange output to [real0, realN/2, real1, im1, real2, im2, ...] */
+ fftOutput[j] = sumReal;
+ fftOutput[j + 1] = sumImag;
+ }
+ }
+
+ static void FftComplexF32(std::vector<float>& input,
+ std::vector<float>& fftOutput)
+ {
+ const size_t fftLen = input.size() / 2;
+ for (size_t k = 0; k < fftLen; k++) {
+ float sumReal = 0;
+ float sumImag = 0;
+ const auto theta = static_cast<float>(2 * M_PI * k / fftLen);
+ for (size_t t = 0; t < fftLen; t++) {
+ const auto angle = theta * t;
+ const auto cosine = MathUtils::CosineF32(angle);
+ const auto sine = MathUtils::SineF32(angle);
+ sumReal += input[t*2] * cosine + input[t*2 + 1] * sine;
+ sumImag += -input[t*2] * sine + input[t*2 + 1] * cosine;
+ }
+ fftOutput[k*2] = sumReal;
+ fftOutput[k*2 + 1] = sumImag;
+ }
}
void MathUtils::FftF32(std::vector<float>& input,
std::vector<float>& fftOutput,
arm::app::math::FftInstance& fftInstance)
{
-#if ARM_DSP_AVAILABLE
- arm_rfft_fast_f32(&fftInstance.instance, input.data(), fftOutput.data(), 0);
-#else
- UNUSED(fftInstance);
- const int inputLength = input.size();
-
- for (int k = 0; k <= inputLength / 2; k++) {
- float sumReal = 0, sumImag = 0;
-
- for (int t = 0; t < inputLength; t++) {
- auto angle = static_cast<float>(2 * M_PI * t * k / inputLength);
- sumReal += input[t] * cosf(angle);
- sumImag += -input[t] * sinf(angle);
- }
-
- /* Arrange output to [real0, realN/2, real1, im1, real2, im2, ...] */
- if (k == 0) {
- fftOutput[0] = sumReal;
- } else if (k == inputLength / 2) {
- fftOutput[1] = sumReal;
- } else {
- fftOutput[k*2] = sumReal;
- fftOutput[k*2 + 1] = sumImag;
- };
+ if (!fftInstance.m_initialised) {
+ printf_err("FFT uninitialised\n");
+ return;
+ } else if (input.size() < fftInstance.m_fftLen) {
+ printf_err("FFT len: %" PRIu16 "; input len: %zu\n",
+ fftInstance.m_fftLen, input.size());
+ return;
+ } else if (fftOutput.size() < input.size()) {
+ printf_err("Output vector len insufficient to hold FFTs\n");
+ return;
}
+
+ switch (fftInstance.m_type) {
+ case FftType::real:
+
+#if ARM_DSP_AVAILABLE
+ if (fftInstance.m_optimisedOptionAvailable) {
+ arm_rfft_fast_f32(&fftInstance.m_instanceReal, input.data(), fftOutput.data(), 0);
+ return;
+ }
#endif /* ARM_DSP_AVAILABLE */
+ FftRealF32(input, fftOutput);
+ return;
+
+ case FftType::complex:
+ if (input.size() < fftInstance.m_fftLen * 2) {
+ printf_err("Complex FFT instance should have input size >= (FFT len x 2)");
+ return;
+ }
+#if ARM_DSP_AVAILABLE
+ if (fftInstance.m_optimisedOptionAvailable) {
+ fftOutput = input; /* Complex function works in-place */
+ arm_cfft_f32(&fftInstance.m_instanceComplex, fftOutput.data(), 0, 0);
+ return;
+ }
+#endif /* ARM_DSP_AVAILABLE */
+ FftComplexF32(input, fftOutput);
+ return;
+
+ default:
+ printf_err("Invalid FFT type\n");
+ return;
+ }
}
void MathUtils::VecLogarithmF32(std::vector <float>& input,
@@ -193,4 +292,4 @@
} /* namespace math */
} /* namespace app */
-} /* namespace arm */
\ No newline at end of file
+} /* namespace arm */
diff --git a/source/application/main/Profiler.cc b/source/application/main/Profiler.cc
index 5d2c23f..fe3aeaf 100644
--- a/source/application/main/Profiler.cc
+++ b/source/application/main/Profiler.cc
@@ -242,6 +242,11 @@
void Profiler::AddProfilingUnit(time_counter start, time_counter end,
const std::string& name)
{
+ if (!this->m_pPlatform) {
+ printf_err("Invalid platform\n");
+ return;
+ }
+
platform_timer * timer = this->m_pPlatform->timer;
struct ProfilingUnit unit;
@@ -273,4 +278,4 @@
}
} /* namespace app */
-} /* namespace arm */
\ No newline at end of file
+} /* namespace arm */
diff --git a/source/application/main/include/PlatformMath.hpp b/source/application/main/include/PlatformMath.hpp
index 45e6a9e..6804025 100644
--- a/source/application/main/include/PlatformMath.hpp
+++ b/source/application/main/include/PlatformMath.hpp
@@ -36,11 +36,20 @@
namespace app {
namespace math {
+ enum class FftType {
+ real = 0,
+ complex = 1
+ };
+
struct FftInstance {
#if ARM_DSP_AVAILABLE
- arm_rfft_fast_instance_f32 instance;
+ arm_rfft_fast_instance_f32 m_instanceReal;
+ arm_cfft_instance_f32 m_instanceComplex;
#endif
- bool initialised = false;
+ uint16_t m_fftLen{0};
+ FftType m_type;
+ bool m_optimisedOptionAvailable{false};
+ bool m_initialised{false};
};
/* Class to provide Math functions like FFT, mean, stddev etc.
@@ -59,6 +68,13 @@
static float CosineF32(float radians);
/**
+ * @brief Get the sine value of the argument in floating point.
+ * @param[in] radians Angle in radians.
+ * @return Sine value (floating point).
+ */
+ static float SineF32(float radians);
+
+ /**
* @brief Get the square root of the argument in floating point.
* @param[in] input Value to compute square root of.
* @return Square root (floating point) value.
@@ -90,9 +106,11 @@
* prior to Fft32 function call if built with ARM DSP functions.
* @param[in] fftLen Requested length of the FFT.
* @param[in] fftInstance FFT instance struct to use.
- * @return true if successful, false otherwise.
+ * @param[in] type FFT type (real or complex)
*/
- static bool FftInitF32(const uint16_t fftLen, arm::app::math::FftInstance& fftInstance);
+ static void FftInitF32(const uint16_t fftLen,
+ FftInstance& fftInstance,
+ const FftType type = FftType::real);
/**
* @brief Computes the FFT for the input vector.
@@ -102,7 +120,7 @@
*/
static void FftF32(std::vector<float>& input,
std::vector<float>& fftOutput,
- arm::app::math::FftInstance& fftInstance);
+ FftInstance& fftInstance);
/**
* @brief Computes the natural logarithms of input floating point