rtl_fm: arbitrary translation

src/rtl_fm.c

@@ -133,6 +133,14 @@ struct agc_state
 	int     error;
 };
 
+struct translate_state
+{
+	double angle;  /* radians */
+	int16_t *sincos;  /* pairs */
+	int len;
+	int i;
+};
+
 struct demod_state
 {
 	int      exit_flag;
@@ -161,11 +169,7 @@ struct demod_state
 	int      prev_lpr_index;
 	int      dc_block, dc_avg;
 	int      rotate_enable;
-	double   rotate_angle;
+	struct   translate_state rotate;
-	int32_t  angle_a;  /* constant, cos(rotate_angle) * ONE_INT */
-	int32_t  angle_b;  /* constant, sin(rotate_angle) * ONE_INT */
-	int32_t  angle_sin;  /* iterative */
-	int32_t  angle_cos;
 	int      agc_enable;
 	struct   agc_state *agc;
 	void     (*mode_demod)(struct demod_state*);
@@ -348,59 +352,76 @@ void rotate_90(unsigned char *buf, uint32_t len)
 	}
 }
 
-void translate_init(struct demod_state *d, double angle)
+int translate_init(struct translate_state *ts)
-/* angle in radians */
+/* two pass: first to find optimal length, second to alloc/fill */
 {
-	d->rotate_enable = 1;
+	int max_length = 100000;
-	d->rotate_angle = angle;
+	int i, s, c, best_i;
-	d->angle_a = (int32_t)(cos(angle) * ONE_INT);
+	double a, a2, err, best_360;
-	d->angle_b = (int32_t)(sin(angle) * ONE_INT);
+	if (fabs(ts->angle) < 2*M_PI/max_length) {
-	d->angle_sin = 0;
+		fprintf(stderr, "angle too small or array too short\n");
-	d->angle_cos = ONE_INT;
+		return 1;
+	}
+	ts->i = 0;
+	ts->sincos = NULL;
+	if (ts->len) {
+		max_length = ts->len;
+		ts->sincos = malloc(max_length * sizeof(int16_t));
+	}
+	a = 0.0;
+	err = 0.0;
+	best_i = 0;
+	best_360 = 4.0;
+	for (i=0; i < max_length; i+=2) {
+		s = (int)round(sin(a + err) * (1<<14));
+		c = (int)round(cos(a + err) * (1<<14));
+		a2 = atan2(s, c);
+		err = fmod(a, 2*M_PI) - a2;
+		a += ts->angle;
+		while (a > M_PI) {
+			a -= 2*M_PI;}
+		while (a < -M_PI) {
+			a += 2*M_PI;}
+		if (i != 0 && fabs(a) < best_360) {
+			best_i = i;
+			best_360 = fabs(a);
+		}
+		if (i != 0 && fabs(a) < 0.0000001) {
+			break;}
+		if (ts->sincos) {
+			ts->sincos[i] = s;
+			ts->sincos[i+1] = c;
+			//fprintf(stderr, "%i   %i %i\n", i, s, c);
+		}
+	}
+	if (ts->sincos) {
+		return 0;
+	}
+	ts->len = best_i + 2;
+	return translate_init(ts);
 }
 
 void translate(struct demod_state *d)
 {
-	int i, len;
+	int i, len, sc_i, sc_len;
-	int32_t old_s, old_c, new_s, new_c, angle_a, angle_b, tmp;
+	int32_t tmp, ar, aj, br, bj;
 	int16_t *buf = d->lowpassed;
+	int16_t *sincos = d->rotate.sincos;
 	len = d->lp_len;
-	old_s = d->angle_sin;
+	sc_i = d->rotate.i;
-	old_c = d->angle_cos;
+	sc_len = d->rotate.len;
-	angle_a = d->angle_a;
+	for (i=0; i<len; i+=2, sc_i+=2) {
-	angle_b = d->angle_b;
+		sc_i = sc_i % sc_len;
-	for (i=0; i<len; i+=2) {
+		ar = (int32_t)buf[i];
-		//buf[i]   = (int16_t)(((int32_t)buf[i] * old_c) >> 14);
+		aj = (int32_t)buf[i+1];
-		//buf[i+1] = (int16_t)(((int32_t)buf[i+1] * old_s) >> 14);
+		br = (int32_t)sincos[sc_i];
-		//new_s = (angle_b * old_c + angle_a * old_s) >> 14;
+		bj = (int32_t)sincos[sc_i+1];
-		//new_c = (angle_a * old_c + angle_b * old_s) >> 14;
+		tmp = ar*br - aj*bj;
-		tmp = (int32_t)buf[i] * old_c;
-		if (tmp % (1<<14) > (1<<13)) {
-			tmp += (1<<13);}
 		buf[i]   = (int16_t)(tmp >> 14);
-
+		tmp = aj*br + ar*bj;
-		tmp = (int32_t)buf[i+1] * old_s;
-		if (tmp % (1<<14) > (1<<13)) {
-			tmp += (1<<13);}
 		buf[i+1]   = (int16_t)(tmp >> 14);
-
-		tmp = angle_b * old_c + angle_a * old_s;
-		if (tmp % (1<<14) > (1<<13)) {
-			tmp += (1<<13);}
-		new_s = (int16_t)(tmp >> 14);
-
-		tmp = angle_a * old_c + angle_b * old_s;
-		if (tmp % (1<<14) > (1<<13)) {
-			tmp += (1<<13);}
-		new_c = (int16_t)(tmp >> 14);
-
-		old_s = new_s;
-		old_c = new_c;
-
-		//fprintf(stderr, "%i %i\n", new_s, new_c);
 	}
-	d->angle_sin = old_s;
+	d->rotate.i = sc_i;
-	d->angle_cos = old_c;
 }
 
 void low_pass(struct demod_state *d)
@@ -535,7 +556,7 @@ int polar_discriminant(int ar, int aj, int br, int bj)
 	double angle;
 	multiply(ar, aj, br, -bj, &cr, &cj);
 	angle = atan2((double)cj, (double)cr);
-	return (int)(angle / 3.14159 * (1<<14));
+	return (int)(angle / M_PI * (1<<14));
 }
 
 int fast_atan2(int y, int x)
@@ -1108,8 +1129,10 @@ static void optimal_settings(int freq, int rate)
 		dm->output_scale = 1;}
 	d->freq = (uint32_t)capture_freq;
 	d->rate = (uint32_t)capture_rate;
-	//translate_init(&demod, 0.25 * 2 * 3.14159);
 	//d->pre_rotate = 0;
+	//demod.rotate_enable = 1;
+	//demod.rotate.angle = -0.25 * 2 * M_PI;
+	//translate_init(&demod.rotate);
 }
 
 void optimal_lrmix(void)
@@ -1145,10 +1168,12 @@ void optimal_lrmix(void)
 	}
 	angle1 = ((double)freq1 - (double)freq) / (double)dongle_bw;
 	angle1 *= 2 * 3.14159;
+	demod.rotate.angle = angle1;
 	angle2 = ((double)freq2 - (double)freq) / (double)dongle_bw;
 	angle2 *= 2 * 3.14159;
-	translate_init(&demod, angle1);
+	demod2.rotate.angle = angle2;
-	translate_init(&demod2, angle2);
+	translate_init(&demod.rotate);
+	translate_init(&demod2.rotate);
 }
 
 static void *controller_thread_fn(void *arg)
@@ -1250,6 +1275,9 @@ void demod_init(struct demod_state *s)
 	s->custom_atan = 0;
 	s->deemph = 0;
 	s->agc_enable = 0;
+	s->rotate_enable = 0;
+	s->rotate.len = 0;
+	s->rotate.sincos = NULL;
 	s->rate_out2 = -1;  // flag for disabled
 	s->mode_demod = &fm_demod;
 	s->pre_j = s->pre_r = s->now_r = s->now_j = 0;
@@ -1396,6 +1424,7 @@ int main(int argc, char **argv)
 	int r, opt;
 	int dev_given = 0;
 	int custom_ppm = 0;
+
 	dongle_init(&dongle);
 	demod_init(&demod);
 	agc_init(&demod);