-STATIC void _lowpass_demod (void *ctx) {
- demod_ctx *demod = (demod_ctx *)ctx;
- int i=0, i2=0;
-
- while (i < demod->buf_len) {
- demod->now_r += demod->buf[i];
- demod->now_j += demod->buf[i+1];
- i += 2;
- demod->index++;
- if (demod->index < ((1000000 / 200000) + 1))
- continue;
- demod->buf[i2] = demod->now_r;
- demod->buf[i2+1] = demod->now_j;
- demod->index = 0;
- demod->now_r = demod->now_j = 0;
- i2 += 2;
- }
- demod->buf_len = i2;
-}
- /**/
-STATIC void _lowpassreal_demod (void *ctx) {
- demod_ctx *demod = (demod_ctx *)ctx;
- int i=0, i2=0;
- int fast = 200000;
- int slow = 48000;
-
- while (i < demod->res_len) {
- demod->now_index += demod->res[i];
- i++;
- demod->pre_index += slow;
- if (demod->pre_index < fast)
- continue;
- demod->res[i2] = (int16_t)(demod->now_index / (fast/slow));
- demod->pre_index -= fast;
- demod->now_index = 0;
- i2 += 1;
- }
- demod->res_len = i2;
-}
- /**/
-STATIC void _multiply (int ar, int aj, int br, int bj, int *cr, int *cj) {
- *cr = ar*br - aj*bj;
- *cj = aj*br + ar*bj;
-}
- /**/
-STATIC int _polar_discriminant (int ar, int aj, int br, int bj) {
- int cr, cj;
- double angle;
- _multiply(ar, aj, br, -bj, &cr, &cj);
- angle = atan2((double)cj, (double)cr);
- return (int)(angle / 3.14159 * (1<<14));
-}
- /**/
-STATIC void _fm_demod (void *ctx) {
- demod_ctx *demod = (demod_ctx *)ctx;
- int16_t *buf = demod->buf;
- int buf_len = demod->buf_len;
- int pcm, i;
-
- pcm = _polar_discriminant(buf[0], buf[1], demod->pre_r, demod->pre_j);
- demod->res[0] = (int16_t)pcm;
-
- for (i = 2; i < (buf_len-1); i += 2) {
- pcm = _polar_discriminant(buf[i], buf[i+1], buf[i-2], buf[i-1]);
- demod->res[i/2] = (int16_t)pcm;
- }
- demod->pre_r = buf[buf_len - 2];
- demod->pre_j = buf[buf_len - 1];
- demod->res_len = buf_len/2;
-}
- /**/
-STATIC void _am_demod (void *ctx) {
- demod_ctx *demod = (demod_ctx *)ctx;
- int16_t *buf = demod->buf;
- int buf_len = demod->buf_len;
- int pcm, i;
-
- for (i = 0; i < buf_len; i += 2) {
- pcm = buf[i] * buf[i];
- pcm += buf[i+1] * buf[i+1];
- demod->res[i/2] = (int16_t)sqrt(pcm);
- }
- demod->res_len = buf_len/2;
-}
- /**/
-STATIC void* _demod_thread_fn (void *ctx) {
- dev_ctx_T *dev_ctx = (dev_ctx_T *)ctx;
- demod_ctx *demod = dev_ctx->demod;
- output_ctx *output = dev_ctx->output;
-
- while(dev_ctx->should_run) {
- pthread_wait(&demod->ok, &demod->ok_m);
- pthread_rwlock_wrlock(&demod->lck);
- _lowpass_demod(demod);
- if (dev_ctx->mode == FM)
- _fm_demod(demod);
- else
- _am_demod(demod);
- _lowpassreal_demod(demod);
- pthread_rwlock_unlock(&demod->lck);
-
- /* lock demod thread, write to it, unlock */
- pthread_rwlock_wrlock(&output->lck);
- memcpy(output->buf, demod->res, 2 * demod->res_len);
- output->buf_len = demod->res_len;
- pthread_rwlock_unlock(&output->lck);
- pthread_signal(&output->ok, &output->ok_m);