mirror of
https://github.com/steve-m/hsdaoh.git
synced 2025-12-10 07:44:41 +01:00
lib: add IQ converter
This commit is contained in:
parent
de7c63b090
commit
ae0d94eba6
7 changed files with 712 additions and 4 deletions
134
include/filters.h
Normal file
134
include/filters.h
Normal file
|
|
@ -0,0 +1,134 @@
|
|||
/*
|
||||
Copyright (C) 2014, Youssef Touil <youssef@airspy.com>
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#ifndef FILTERS_H
|
||||
#define FILTERS_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define HB_KERNEL_FLOAT_LEN 47
|
||||
|
||||
const float HB_KERNEL_FLOAT[HB_KERNEL_FLOAT_LEN] =
|
||||
{
|
||||
-0.000998606272947510,
|
||||
0.000000000000000000,
|
||||
0.001695637278417295,
|
||||
0.000000000000000000,
|
||||
-0.003054430179754289,
|
||||
0.000000000000000000,
|
||||
0.005055504379767936,
|
||||
0.000000000000000000,
|
||||
-0.007901319195893647,
|
||||
0.000000000000000000,
|
||||
0.011873357051047719,
|
||||
0.000000000000000000,
|
||||
-0.017411159379930066,
|
||||
0.000000000000000000,
|
||||
0.025304817427568772,
|
||||
0.000000000000000000,
|
||||
-0.037225225204559217,
|
||||
0.000000000000000000,
|
||||
0.057533286997004301,
|
||||
0.000000000000000000,
|
||||
-0.102327462004259350,
|
||||
0.000000000000000000,
|
||||
0.317034472508947400,
|
||||
0.500000000000000000,
|
||||
0.317034472508947400,
|
||||
0.000000000000000000,
|
||||
-0.102327462004259350,
|
||||
0.000000000000000000,
|
||||
0.057533286997004301,
|
||||
0.000000000000000000,
|
||||
-0.037225225204559217,
|
||||
0.000000000000000000,
|
||||
0.025304817427568772,
|
||||
0.000000000000000000,
|
||||
-0.017411159379930066,
|
||||
0.000000000000000000,
|
||||
0.011873357051047719,
|
||||
0.000000000000000000,
|
||||
-0.007901319195893647,
|
||||
0.000000000000000000,
|
||||
0.005055504379767936,
|
||||
0.000000000000000000,
|
||||
-0.003054430179754289,
|
||||
0.000000000000000000,
|
||||
0.001695637278417295,
|
||||
0.000000000000000000,
|
||||
-0.000998606272947510
|
||||
};
|
||||
|
||||
#define HB_KERNEL_INT16_LEN 47
|
||||
|
||||
const int16_t HB_KERNEL_INT16[HB_KERNEL_INT16_LEN] =
|
||||
{
|
||||
-33,
|
||||
0,
|
||||
56,
|
||||
0,
|
||||
-100,
|
||||
0,
|
||||
166,
|
||||
0,
|
||||
-259,
|
||||
0,
|
||||
389,
|
||||
0,
|
||||
-571,
|
||||
0,
|
||||
829,
|
||||
0,
|
||||
-1220,
|
||||
0,
|
||||
1885,
|
||||
0,
|
||||
-3353,
|
||||
0,
|
||||
10389,
|
||||
16384,
|
||||
10389,
|
||||
0,
|
||||
-3353,
|
||||
0,
|
||||
1885,
|
||||
0,
|
||||
-1220,
|
||||
0,
|
||||
829,
|
||||
0,
|
||||
-571,
|
||||
0,
|
||||
389,
|
||||
0,
|
||||
-259,
|
||||
0,
|
||||
166,
|
||||
0,
|
||||
-100,
|
||||
0,
|
||||
56,
|
||||
0,
|
||||
-33
|
||||
};
|
||||
|
||||
#endif // FILTERS_H
|
||||
|
|
@ -36,6 +36,7 @@ struct hsdaoh_dev {
|
|||
unsigned int width, height, fps;
|
||||
|
||||
bool output_float;
|
||||
iqconverter_float_t *cnv_f;
|
||||
|
||||
/* status */
|
||||
int dev_lost;
|
||||
|
|
|
|||
47
include/iqconverter_float.h
Normal file
47
include/iqconverter_float.h
Normal file
|
|
@ -0,0 +1,47 @@
|
|||
/*
|
||||
Copyright (C) 2014, Youssef Touil <youssef@airspy.com>
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#ifndef IQCONVERTER_FLOAT_H
|
||||
#define IQCONVERTER_FLOAT_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define IQCONVERTER_NZEROS 2
|
||||
#define IQCONVERTER_NPOLES 2
|
||||
|
||||
typedef struct {
|
||||
float avg;
|
||||
float hbc;
|
||||
int len;
|
||||
int fir_index;
|
||||
int delay_index;
|
||||
float *fir_kernel;
|
||||
float *fir_queue;
|
||||
float *delay_line;
|
||||
} iqconverter_float_t;
|
||||
|
||||
iqconverter_float_t *iqconverter_float_create(const float *hb_kernel, int len);
|
||||
void iqconverter_float_free(iqconverter_float_t *cnv);
|
||||
void iqconverter_float_reset(iqconverter_float_t *cnv);
|
||||
void iqconverter_float_process(iqconverter_float_t *cnv, float *samples, int len);
|
||||
|
||||
#endif // IQCONVERTER_FLOAT_H
|
||||
|
|
@ -18,7 +18,7 @@
|
|||
########################################################################
|
||||
# Setup shared library variant
|
||||
########################################################################
|
||||
add_library(hsdaoh SHARED libhsdaoh.c format_convert.c)
|
||||
add_library(hsdaoh SHARED libhsdaoh.c format_convert.c iqconverter_float.c)
|
||||
target_link_libraries(hsdaoh ${LIBUSB_LIBRARIES} ${LIBUVC_LIBRARIES} ${THREADS_PTHREADS_LIBRARY})
|
||||
target_include_directories(hsdaoh PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
|
||||
|
|
@ -36,7 +36,7 @@ generate_export_header(hsdaoh)
|
|||
########################################################################
|
||||
# Setup static library variant
|
||||
########################################################################
|
||||
add_library(hsdaoh_static STATIC libhsdaoh.c format_convert.c)
|
||||
add_library(hsdaoh_static STATIC libhsdaoh.c format_convert.c iqconverter_float.c)
|
||||
target_link_libraries(hsdaoh m ${LIBUSB_LIBRARIES} ${LIBUVC_LIBRARIES} ${THREADS_PTHREADS_LIBRARY})
|
||||
target_include_directories(hsdaoh_static PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
|
||||
|
|
@ -46,6 +46,7 @@ target_include_directories(hsdaoh_static PUBLIC
|
|||
${THREADS_PTHREADS_INCLUDE_DIR}
|
||||
)
|
||||
set_property(TARGET hsdaoh_static APPEND PROPERTY COMPILE_DEFINITIONS "hsdaoh_STATIC" )
|
||||
set_property(TARGET hsdaoh_static PROPERTY POSITION_INDEPENDENT_CODE ON)
|
||||
if(NOT WIN32)
|
||||
# Force same library filename for static and shared variants of the library
|
||||
set_target_properties(hsdaoh_static PROPERTIES OUTPUT_NAME hsdaoh)
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include <stdlib.h>
|
||||
#include <libusb.h>
|
||||
#include <libuvc/libuvc.h>
|
||||
#include <iqconverter_float.h>
|
||||
#include <hsdaoh.h>
|
||||
#include <hsdaoh_private.h>
|
||||
#include <format_convert.h>
|
||||
|
|
@ -47,6 +48,8 @@ static inline void hsdaoh_16bit_to_float(hsdaoh_dev_t *dev, hsdaoh_data_info_t *
|
|||
floats[j++] = (sample_i - scale) * (1/scale);
|
||||
}
|
||||
|
||||
// iqconverter_float_process(dev->cnv_f, (float *) floats, j);
|
||||
|
||||
data_info->buf = (uint8_t *)floats;
|
||||
data_info->len = j * sizeof(float);
|
||||
dev->cb(data_info);
|
||||
|
|
|
|||
502
src/iqconverter_float.c
Normal file
502
src/iqconverter_float.c
Normal file
|
|
@ -0,0 +1,502 @@
|
|||
/*
|
||||
Copyright (C) 2014, Youssef Touil <youssef@airspy.com>
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "iqconverter_float.h"
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#if defined(__MINGW32__) && !defined(__MINGW64_VERSION_MAJOR)
|
||||
#include <malloc.h>
|
||||
#define _aligned_malloc __mingw_aligned_malloc
|
||||
#define _aligned_free __mingw_aligned_free
|
||||
#define _inline inline
|
||||
#define FIR_STANDARD
|
||||
#elif defined(__APPLE__)
|
||||
#include <malloc/malloc.h>
|
||||
#define _aligned_malloc(size, alignment) malloc(size)
|
||||
#define _aligned_free(mem) free(mem)
|
||||
#define _inline inline
|
||||
#define FIR_STANDARD
|
||||
#elif defined(__FreeBSD__)
|
||||
#define USE_SSE2
|
||||
#include <immintrin.h>
|
||||
#define _inline inline
|
||||
#define _aligned_free(mem) free(mem)
|
||||
void *_aligned_malloc(size_t size, size_t alignment)
|
||||
{
|
||||
void *result;
|
||||
if (posix_memalign(&result, alignment, size) == 0)
|
||||
return result;
|
||||
return 0;
|
||||
}
|
||||
#elif defined(__GNUC__) && !defined(__MINGW64_VERSION_MAJOR)
|
||||
#include <malloc.h>
|
||||
#define _aligned_malloc(size, alignment) memalign(alignment, size)
|
||||
#define _aligned_free(mem) free(mem)
|
||||
#define _inline inline
|
||||
#else
|
||||
#if (_MSC_VER >= 1800)
|
||||
//#define USE_SSE2
|
||||
//#include <immintrin.h>
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#define SIZE_FACTOR 32
|
||||
#define DEFAULT_ALIGNMENT 16
|
||||
#define HPF_COEFF 0.01f
|
||||
|
||||
#if defined(_MSC_VER)
|
||||
#define ALIGNED __declspec(align(DEFAULT_ALIGNMENT))
|
||||
#else
|
||||
#define ALIGNED
|
||||
#endif
|
||||
|
||||
iqconverter_float_t *iqconverter_float_create(const float *hb_kernel, int len)
|
||||
{
|
||||
int i, j;
|
||||
size_t buffer_size;
|
||||
iqconverter_float_t *cnv = (iqconverter_float_t *) _aligned_malloc(sizeof(iqconverter_float_t), DEFAULT_ALIGNMENT);
|
||||
|
||||
cnv->len = len / 2 + 1;
|
||||
cnv->hbc = hb_kernel[len / 2];
|
||||
|
||||
buffer_size = cnv->len * sizeof(float);
|
||||
|
||||
cnv->fir_kernel = (float *) _aligned_malloc(buffer_size, DEFAULT_ALIGNMENT);
|
||||
cnv->fir_queue = (float *) _aligned_malloc(buffer_size * SIZE_FACTOR, DEFAULT_ALIGNMENT);
|
||||
cnv->delay_line = (float *) _aligned_malloc(buffer_size / 2, DEFAULT_ALIGNMENT);
|
||||
|
||||
iqconverter_float_reset(cnv);
|
||||
|
||||
for (i = 0, j = 0; i < cnv->len; i++, j += 2)
|
||||
{
|
||||
cnv->fir_kernel[i] = hb_kernel[j];
|
||||
}
|
||||
|
||||
return cnv;
|
||||
}
|
||||
|
||||
void iqconverter_float_free(iqconverter_float_t *cnv)
|
||||
{
|
||||
_aligned_free(cnv->fir_kernel);
|
||||
_aligned_free(cnv->fir_queue);
|
||||
_aligned_free(cnv->delay_line);
|
||||
_aligned_free(cnv);
|
||||
}
|
||||
|
||||
void iqconverter_float_reset(iqconverter_float_t *cnv)
|
||||
{
|
||||
cnv->avg = 0.0f;
|
||||
cnv->fir_index = 0;
|
||||
cnv->delay_index = 0;
|
||||
memset(cnv->delay_line, 0, cnv->len * sizeof(float) / 2);
|
||||
memset(cnv->fir_queue, 0, cnv->len * sizeof(float) * SIZE_FACTOR);
|
||||
}
|
||||
|
||||
static _inline float process_fir_taps(const float *kernel, const float *queue, int len)
|
||||
{
|
||||
int i;
|
||||
|
||||
#ifdef USE_SSE2
|
||||
|
||||
__m128 acc = _mm_set_ps(0, 0, 0, 0);
|
||||
|
||||
#else
|
||||
|
||||
float sum = 0.0f;
|
||||
|
||||
#endif
|
||||
|
||||
if (len >= 8)
|
||||
{
|
||||
int it = len >> 3;
|
||||
|
||||
#ifdef USE_SSE2
|
||||
|
||||
for (i = 0; i < it; i++)
|
||||
{
|
||||
__m128 head1 = _mm_loadu_ps(queue);
|
||||
__m128 kern1 = _mm_load_ps(kernel);
|
||||
__m128 head2 = _mm_loadu_ps(queue + 4);
|
||||
__m128 kern2 = _mm_load_ps(kernel + 4);
|
||||
|
||||
__m128 mul1 = _mm_mul_ps(kern1, head1);
|
||||
__m128 mul2 = _mm_mul_ps(kern2, head2);
|
||||
|
||||
mul1 = _mm_add_ps(mul1, mul2);
|
||||
|
||||
acc = _mm_add_ps(acc, mul1);
|
||||
|
||||
queue += 8;
|
||||
kernel += 8;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
for (i = 0; i < it; i++)
|
||||
{
|
||||
sum += kernel[0] * queue[0]
|
||||
+ kernel[1] * queue[1]
|
||||
+ kernel[2] * queue[2]
|
||||
+ kernel[3] * queue[3]
|
||||
+ kernel[4] * queue[4]
|
||||
+ kernel[5] * queue[5]
|
||||
+ kernel[6] * queue[6]
|
||||
+ kernel[7] * queue[7];
|
||||
|
||||
queue += 8;
|
||||
kernel += 8;
|
||||
}
|
||||
|
||||
#endif
|
||||
len &= 7;
|
||||
}
|
||||
|
||||
if (len >= 4)
|
||||
{
|
||||
|
||||
#ifdef USE_SSE2
|
||||
|
||||
__m128 head = _mm_loadu_ps(queue);
|
||||
__m128 kern = _mm_load_ps(kernel);
|
||||
__m128 mul = _mm_mul_ps(kern, head);
|
||||
acc = _mm_add_ps(acc, mul);
|
||||
|
||||
#else
|
||||
|
||||
sum += kernel[0] * queue[0]
|
||||
+ kernel[1] * queue[1]
|
||||
+ kernel[2] * queue[2]
|
||||
+ kernel[3] * queue[3];
|
||||
|
||||
#endif
|
||||
|
||||
kernel += 4;
|
||||
queue += 4;
|
||||
len &= 3;
|
||||
}
|
||||
|
||||
#ifdef USE_SSE2
|
||||
|
||||
__m128 t = _mm_add_ps(acc, _mm_movehl_ps(acc, acc));
|
||||
acc = _mm_add_ss(t, _mm_shuffle_ps(t, t, 1));
|
||||
|
||||
#ifdef __FreeBSD__
|
||||
float sum = acc[0];
|
||||
#else
|
||||
float sum = acc.m128_f32[0];
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
if (len >= 2)
|
||||
{
|
||||
sum += kernel[0] * queue[0]
|
||||
+ kernel[1] * queue[1];
|
||||
|
||||
//kernel += 2;
|
||||
//queue += 2;
|
||||
//len &= 1;
|
||||
}
|
||||
|
||||
//if (len >= 1)
|
||||
//{
|
||||
// sum += kernel[0] * queue[0];
|
||||
//}
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
static void fir_interleaved_4(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
int fir_index = cnv->fir_index;
|
||||
int fir_len = cnv->len;
|
||||
float *fir_kernel = cnv->fir_kernel;
|
||||
float *fir_queue = cnv->fir_queue;
|
||||
float *queue;
|
||||
float acc;
|
||||
|
||||
for (i = 0; i < len; i += 2)
|
||||
{
|
||||
queue = fir_queue + fir_index;
|
||||
|
||||
queue[0] = samples[i];
|
||||
|
||||
acc = fir_kernel[0] * (queue[0] + queue[4 - 1])
|
||||
+ fir_kernel[1] * (queue[1] + queue[4 - 2]);
|
||||
|
||||
samples[i] = acc;
|
||||
|
||||
if (--fir_index < 0)
|
||||
{
|
||||
fir_index = fir_len * (SIZE_FACTOR - 1);
|
||||
memcpy(fir_queue + fir_index + 1, fir_queue, (fir_len - 1) * sizeof(float));
|
||||
}
|
||||
}
|
||||
|
||||
cnv->fir_index = fir_index;
|
||||
}
|
||||
|
||||
static void fir_interleaved_8(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
int fir_index = cnv->fir_index;
|
||||
int fir_len = cnv->len;
|
||||
float *fir_kernel = cnv->fir_kernel;
|
||||
float *fir_queue = cnv->fir_queue;
|
||||
float *queue;
|
||||
float acc;
|
||||
|
||||
for (i = 0; i < len; i += 2)
|
||||
{
|
||||
queue = fir_queue + fir_index;
|
||||
|
||||
queue[0] = samples[i];
|
||||
|
||||
acc = fir_kernel[0] * (queue[0] + queue[8 - 1])
|
||||
+ fir_kernel[1] * (queue[1] + queue[8 - 2])
|
||||
+ fir_kernel[2] * (queue[2] + queue[8 - 3])
|
||||
+ fir_kernel[3] * (queue[3] + queue[8 - 4]);
|
||||
|
||||
samples[i] = acc;
|
||||
|
||||
if (--fir_index < 0)
|
||||
{
|
||||
fir_index = fir_len * (SIZE_FACTOR - 1);
|
||||
memcpy(fir_queue + fir_index + 1, fir_queue, (fir_len - 1) * sizeof(float));
|
||||
}
|
||||
}
|
||||
|
||||
cnv->fir_index = fir_index;
|
||||
}
|
||||
|
||||
static void fir_interleaved_12(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
int fir_index = cnv->fir_index;
|
||||
int fir_len = cnv->len;
|
||||
float *fir_kernel = cnv->fir_kernel;
|
||||
float *fir_queue = cnv->fir_queue;
|
||||
float *queue;
|
||||
float acc = 0;
|
||||
|
||||
for (i = 0; i < len; i += 2)
|
||||
{
|
||||
queue = fir_queue + fir_index;
|
||||
|
||||
queue[0] = samples[i];
|
||||
|
||||
acc = fir_kernel[0] * (queue[0] + queue[12 - 1])
|
||||
+ fir_kernel[1] * (queue[1] + queue[12 - 2])
|
||||
+ fir_kernel[2] * (queue[2] + queue[12 - 3])
|
||||
+ fir_kernel[3] * (queue[3] + queue[12 - 4])
|
||||
+ fir_kernel[4] * (queue[4] + queue[12 - 5])
|
||||
+ fir_kernel[5] * (queue[5] + queue[12 - 6]);
|
||||
|
||||
samples[i] = acc;
|
||||
|
||||
if (--fir_index < 0)
|
||||
{
|
||||
fir_index = fir_len * (SIZE_FACTOR - 1);
|
||||
memcpy(fir_queue + fir_index + 1, fir_queue, (fir_len - 1) * sizeof(float));
|
||||
}
|
||||
}
|
||||
|
||||
cnv->fir_index = fir_index;
|
||||
}
|
||||
|
||||
static void fir_interleaved_24(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
int fir_index = cnv->fir_index;
|
||||
int fir_len = cnv->len;
|
||||
float *fir_kernel = cnv->fir_kernel;
|
||||
float *fir_queue = cnv->fir_queue;
|
||||
float *queue;
|
||||
float acc = 0;
|
||||
|
||||
for (i = 0; i < len; i += 2)
|
||||
{
|
||||
queue = fir_queue + fir_index;
|
||||
|
||||
queue[0] = samples[i];
|
||||
|
||||
acc = fir_kernel[0] * (queue[0] + queue[24 - 1])
|
||||
+ fir_kernel[1] * (queue[1] + queue[24 - 2])
|
||||
+ fir_kernel[2] * (queue[2] + queue[24 - 3])
|
||||
+ fir_kernel[3] * (queue[3] + queue[24 - 4])
|
||||
+ fir_kernel[4] * (queue[4] + queue[24 - 5])
|
||||
+ fir_kernel[5] * (queue[5] + queue[24 - 6])
|
||||
+ fir_kernel[6] * (queue[6] + queue[24 - 7])
|
||||
+ fir_kernel[7] * (queue[7] + queue[24 - 8])
|
||||
+ fir_kernel[8] * (queue[8] + queue[24 - 9])
|
||||
+ fir_kernel[9] * (queue[9] + queue[24 - 10])
|
||||
+ fir_kernel[10] * (queue[10] + queue[24 - 11])
|
||||
+ fir_kernel[11] * (queue[11] + queue[24 - 12]);
|
||||
|
||||
samples[i] = acc;
|
||||
|
||||
if (--fir_index < 0)
|
||||
{
|
||||
fir_index = fir_len * (SIZE_FACTOR - 1);
|
||||
memcpy(fir_queue + fir_index + 1, fir_queue, (fir_len - 1) * sizeof(float));
|
||||
}
|
||||
}
|
||||
|
||||
cnv->fir_index = fir_index;
|
||||
}
|
||||
|
||||
static void fir_interleaved_generic(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
int fir_index = cnv->fir_index;
|
||||
int fir_len = cnv->len;
|
||||
float *fir_kernel = cnv->fir_kernel;
|
||||
float *fir_queue = cnv->fir_queue;
|
||||
float *queue;
|
||||
|
||||
for (i = 0; i < len; i += 2)
|
||||
{
|
||||
queue = fir_queue + fir_index;
|
||||
|
||||
queue[0] = samples[i];
|
||||
|
||||
samples[i] = process_fir_taps(fir_kernel, queue, fir_len);
|
||||
|
||||
if (--fir_index < 0)
|
||||
{
|
||||
fir_index = fir_len * (SIZE_FACTOR - 1);
|
||||
memcpy(fir_queue + fir_index + 1, fir_queue, (fir_len - 1) * sizeof(float));
|
||||
}
|
||||
}
|
||||
|
||||
cnv->fir_index = fir_index;
|
||||
}
|
||||
|
||||
static void fir_interleaved(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
switch (cnv->len)
|
||||
{
|
||||
case 4:
|
||||
fir_interleaved_4(cnv, samples, len);
|
||||
break;
|
||||
case 8:
|
||||
fir_interleaved_8(cnv, samples, len);
|
||||
break;
|
||||
case 12:
|
||||
fir_interleaved_12(cnv, samples, len);
|
||||
break;
|
||||
case 24:
|
||||
fir_interleaved_24(cnv, samples, len);
|
||||
break;
|
||||
default:
|
||||
fir_interleaved_generic(cnv, samples, len);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void delay_interleaved(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
ALIGNED int index;
|
||||
ALIGNED int half_len;
|
||||
ALIGNED float res;
|
||||
|
||||
half_len = cnv->len >> 1;
|
||||
index = cnv->delay_index;
|
||||
|
||||
for (i = 0; i < len; i += 2)
|
||||
{
|
||||
res = cnv->delay_line[index];
|
||||
cnv->delay_line[index] = samples[i];
|
||||
samples[i] = res;
|
||||
|
||||
if (++index >= half_len)
|
||||
{
|
||||
index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
cnv->delay_index = index;
|
||||
}
|
||||
|
||||
#define SCALE (0.01f)
|
||||
|
||||
static void remove_dc(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
ALIGNED float avg = cnv->avg;
|
||||
|
||||
for (i = 0; i < len; i++)
|
||||
{
|
||||
samples[i] -= avg;
|
||||
avg += SCALE * samples[i];
|
||||
}
|
||||
|
||||
cnv->avg = avg;
|
||||
}
|
||||
|
||||
static void translate_fs_4(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
int i;
|
||||
ALIGNED float hbc = cnv->hbc;
|
||||
|
||||
#ifdef USE_SSE2
|
||||
|
||||
float *buf = samples;
|
||||
ALIGNED __m128 vec;
|
||||
ALIGNED __m128 rot = _mm_set_ps(hbc, 1.0f, -hbc, -1.0f);
|
||||
|
||||
for (i = 0; i < len / 4; i++, buf +=4)
|
||||
{
|
||||
vec = _mm_loadu_ps(buf);
|
||||
vec = _mm_mul_ps(vec, rot);
|
||||
_mm_storeu_ps(buf, vec);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
int j;
|
||||
|
||||
for (i = 0; i < len / 4; i++)
|
||||
{
|
||||
j = i << 2;
|
||||
samples[j + 0] = -samples[j + 0];
|
||||
samples[j + 1] = -samples[j + 1] * hbc;
|
||||
//samples[j + 2] = samples[j + 2];
|
||||
samples[j + 3] = samples[j + 3] * hbc;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
fir_interleaved(cnv, samples, len);
|
||||
delay_interleaved(cnv, samples + 1, len);
|
||||
}
|
||||
|
||||
void iqconverter_float_process(iqconverter_float_t *cnv, float *samples, int len)
|
||||
{
|
||||
remove_dc(cnv, samples, len);
|
||||
translate_fs_4(cnv, samples, len);
|
||||
}
|
||||
|
|
@ -43,6 +43,9 @@
|
|||
#include <libusb.h>
|
||||
#include <libuvc/libuvc.h>
|
||||
|
||||
#include <iqconverter_float.h>
|
||||
#include <filters.h>
|
||||
|
||||
#include <hsdaoh.h>
|
||||
#include <hsdaoh_private.h>
|
||||
#include <format_convert.h>
|
||||
|
|
@ -473,6 +476,7 @@ int hsdaoh_open(hsdaoh_dev_t **out_dev, uint32_t index)
|
|||
goto err;
|
||||
|
||||
dev->dev_lost = 0;
|
||||
dev->cnv_f = iqconverter_float_create(HB_KERNEL_FLOAT, HB_KERNEL_FLOAT_LEN);
|
||||
|
||||
found:
|
||||
*out_dev = dev;
|
||||
|
|
@ -504,11 +508,23 @@ int hsdaoh_close(hsdaoh_dev_t *dev)
|
|||
uvc_unref_device(dev->uvc_dev);
|
||||
uvc_exit(dev->uvc_ctx);
|
||||
|
||||
iqconverter_float_free(dev->cnv_f);
|
||||
free(dev);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// maybe rename to preferred output format
|
||||
// and add real output format to data_info_t
|
||||
int hsdaoh_set_output_format(hsdaoh_dev_t *dev, hsdaoh_output_format_t format)
|
||||
{
|
||||
if (!dev)
|
||||
return -1;
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* callback for idle/filler data */
|
||||
inline int hsdaoh_check_idle_cnt(hsdaoh_dev_t *dev, uint16_t *buf, size_t length)
|
||||
{
|
||||
|
|
@ -629,8 +645,10 @@ void hsdaoh_process_frame(hsdaoh_dev_t *dev, uint8_t *data, int size)
|
|||
} else if ((meta.crc_config == CRC16_1_LINE) || (meta.crc_config == CRC16_2_LINE)) {
|
||||
uint16_t expected_crc = (meta.crc_config == CRC16_1_LINE) ? dev->last_crc[0] : dev->last_crc[1];
|
||||
|
||||
if ((crc != expected_crc) && dev->stream_synced)
|
||||
if ((crc != expected_crc) && dev->stream_synced) {
|
||||
frame_errors++;
|
||||
fprintf(stderr, "Checksum mismatch in line %d: %04x != %04x\n", i, crc, expected_crc);
|
||||
}
|
||||
|
||||
dev->last_crc[1] = dev->last_crc[0];
|
||||
dev->last_crc[0] = crc16_ccitt(line_dat, dev->width * sizeof(uint16_t));
|
||||
|
|
@ -697,13 +715,15 @@ int hsdaoh_start_stream(hsdaoh_dev_t *dev, hsdaoh_read_cb_t cb, void *ctx)
|
|||
if (HSDAOH_INACTIVE != dev->async_status)
|
||||
return -2;
|
||||
|
||||
iqconverter_float_reset(dev->cnv_f);
|
||||
|
||||
dev->async_status = HSDAOH_RUNNING;
|
||||
dev->async_cancel = 0;
|
||||
|
||||
dev->cb = cb;
|
||||
dev->cb_ctx = ctx;
|
||||
|
||||
// dev->output_float = true;
|
||||
dev->output_float = true;
|
||||
|
||||
uvc_error_t res;
|
||||
uvc_stream_ctrl_t ctrl;
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue