From cbf79a0f9cb859d04e8e03d627219cb2bf49611f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 22 Jun 2012 23:12:02 -0700 Subject: [PATCH] altos: Add the simplest possible viterbi decoder I think I understand how it works now. It's not exactly speedy, and it uses a lot of memory. Signed-off-by: Keith Packard --- src/core/ao_fec.h | 9 +++ src/core/ao_viterbi.c | 143 +++++++++++++++++++++++++++++++++ src/test/Makefile | 4 +- src/test/ao_fec_tx_test.c | 163 +++++++++++++++++++++++++++++++++++--- 4 files changed, 307 insertions(+), 12 deletions(-) create mode 100644 src/core/ao_viterbi.c diff --git a/src/core/ao_fec.h b/src/core/ao_fec.h index db5523a3..985352dd 100644 --- a/src/core/ao_fec.h +++ b/src/core/ao_fec.h @@ -57,4 +57,13 @@ ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out); uint8_t ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out); + +/* + * Decode data. 'in' is one byte per bit, soft decision + * 'out' must be len/8 bytes long + */ + +uint8_t +ao_fec_decode(uint8_t *in, int in_len, uint8_t *out); + #endif /* _AO_FEC_H_ */ diff --git a/src/core/ao_viterbi.c b/src/core/ao_viterbi.c new file mode 100644 index 00000000..2d441f4b --- /dev/null +++ b/src/core/ao_viterbi.c @@ -0,0 +1,143 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +/* + * 'input' is 8-bits per symbol soft decision data + * 'len' is output byte length + */ + +static const uint8_t ao_fec_encode_table[16] = { +/* next 0 1 state */ + 0, 3, /* 000 */ + 1, 2, /* 001 */ + 3, 0, /* 010 */ + 2, 1, /* 011 */ + 3, 0, /* 100 */ + 2, 1, /* 101 */ + 0, 3, /* 110 */ + 1, 2 /* 111 */ +}; + +struct ao_soft_sym { + uint8_t a, b; +}; + +struct ao_soft_sym +ao_soft_sym(uint8_t bits) +{ + struct ao_soft_sym s; + + s.a = ((bits & 2) >> 1) * 0xff; + s.b = (bits & 1) * 0xff; + return s; +} + +uint8_t +ao_next_state(uint8_t state, uint8_t bit) +{ + return ((state << 1) | bit) & 0x7; +} + +static inline abs(int x) { return x < 0 ? -x : x; } + +int +ao_cost(struct ao_soft_sym a, struct ao_soft_sym b) +{ + return abs(a.a - b.a) + abs(a.b - b.b); +} + +uint8_t +ao_fec_decode(uint8_t *in, int len, uint8_t *out) +{ + int cost[len/2 + 1][8]; + uint8_t prev[len/2 + 1][8]; + int c; + int i, b; + uint8_t state = 0, min_state; + uint8_t bits[len/2]; + + for (c = 0; c < 8; c++) + cost[0][c] = 10000; + cost[0][0] = 0; + + for (i = 0; i < len; i += 2) { + b = i/2; + struct ao_soft_sym s = { .a = in[i], .b = in[i+1] }; + + for (state = 0; state < 8; state++) + cost[b+1][state] = 10000; + + for (state = 0; state < 8; state++) { + struct ao_soft_sym zero = ao_soft_sym(ao_fec_encode_table[state * 2 + 0]); + struct ao_soft_sym one = ao_soft_sym(ao_fec_encode_table[state * 2 + 1]); + uint8_t zero_state = ao_next_state(state, 0); + uint8_t one_state = ao_next_state(state, 1); + int zero_cost = ao_cost(s, zero); + int one_cost = ao_cost(s, one); + +#if 0 + printf ("saw %02x %02x expected %02x %02x (%d) or %02x %02x (%d)\n", + s.a, s.b, zero.a, zero.b, zero_cost, one.a, one.b, one_cost); +#endif + zero_cost += cost[b][state]; + one_cost += cost[b][state]; + if (zero_cost < cost[b+1][zero_state]) { + prev[b+1][zero_state] = state; + cost[b+1][zero_state] = zero_cost; + } + + if (one_cost < cost[b+1][one_state]) { + prev[b+1][one_state] = state; + cost[b+1][one_state] = one_cost; + } + } + + printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b); + for (state = 0; state < 8; state++) { + printf (" %5d", cost[b+1][state]); + } + printf ("\n"); + } + + b = len / 2; + c = cost[b][0]; + min_state = 0; + for (state = 1; state < 8; state++) { + if (cost[b][state] < c) { + c = cost[b][state]; + min_state = state; + } + } + + for (b = len/2; b > 0; b--) { + bits[b-1] = min_state & 1; + min_state = prev[b][min_state]; + } + + for (i = 0; i < len/2; i += 8) { + uint8_t byte; + + byte = 0; + for (b = 0; b < 8; b++) + byte = (byte << 1) | bits[i + b]; + out[i/8] = byte; + } + return len/16; +} diff --git a/src/test/Makefile b/src/test/Makefile index 3e308dd7..024a54c1 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -40,5 +40,5 @@ ao_convert_pa_test: ao_convert_pa_test.c ao_convert_pa.c altitude-pa.h ao_kalman.h: $(KALMAN) (cd .. && make ao_kalman.h) -ao_fec_tx_test: ao_fec_tx_test.c ao_fec_tx.c - cc $(CFLAGS) -o $@ ao_fec_tx_test.c ../drivers/ao_fec_tx.c +ao_fec_tx_test: ao_fec_tx_test.c ao_fec_tx.c ao_viterbi.c + cc $(CFLAGS) -o $@ ao_fec_tx_test.c ../core/ao_fec_tx.c ../core/ao_viterbi.c -lm diff --git a/src/test/ao_fec_tx_test.c b/src/test/ao_fec_tx_test.c index b8dc9308..d01eadc5 100644 --- a/src/test/ao_fec_tx_test.c +++ b/src/test/ao_fec_tx_test.c @@ -18,21 +18,73 @@ #include #include #include +#include +#include -int -main(int argc, char **argv) +#ifndef RANDOM_MAX +#define RANDOM_MAX 0x7fffffff +#endif + +static double +rand_real(void) { + return (double) random() / (double) RANDOM_MAX; +} + +static double +gaussian_random(double mean, double dev) { - uint8_t input[4] = { 3, 1, 2, 3 }; - uint8_t prepare[sizeof(input) + AO_FEC_PREPARE_EXTRA]; - uint8_t encode[sizeof(prepare) * 2]; - uint8_t interleave[sizeof(encode)]; + static int save_x_valid = 0; + static double save_x; + double x; + + if (save_x_valid) + { + x = save_x; + save_x_valid = 0; + } + else + { + double w; + double normal_x1, normal_x2; + + do { + normal_x1 = 2 * rand_real () - 1; + normal_x2 = 2 * rand_real () - 1; + w = normal_x1*normal_x1 + normal_x2*normal_x2; + } while (w >= 1 || w < 1E-30); + + w = sqrt(log(w)*(-2./w)); + + /* + * normal_x1 and normal_x2 are independent normally + * distributed variates + */ + + x = normal_x1 * w; + /* save normal_x2 for next call */ + save_x = normal_x2 * w; + save_x_valid = 1; + } + return x * dev + mean; +} + +#define PREPARE_LEN(input_len) ((input_len) + AO_FEC_PREPARE_EXTRA) +#define ENCODE_LEN(input_len) (PREPARE_LEN(input_len) * 2) +#define INTERLEAVE_LEN(input_len) ENCODE_LEN(input_len) + +static int +ao_encode(uint8_t *input, int input_len, uint8_t *output) +{ + uint8_t prepare[PREPARE_LEN(input_len)]; + uint8_t encode[ENCODE_LEN(input_len)]; + uint8_t interleave[INTERLEAVE_LEN(input_len)]; uint8_t prepare_len; uint8_t encode_len; uint8_t interleave_len; - ao_fec_dump_bytes(input, sizeof(input), "Input"); + ao_fec_dump_bytes(input, input_len, "Input"); - prepare_len = ao_fec_prepare(input, sizeof (input), prepare); + prepare_len = ao_fec_prepare(input, input_len, prepare); ao_fec_dump_bytes(prepare, prepare_len, "Prepare"); @@ -40,8 +92,99 @@ main(int argc, char **argv) ao_fec_dump_bytes(encode, encode_len, "Encode"); - interleave_len = ao_fec_interleave(encode, encode_len, interleave); + interleave_len = ao_fec_interleave(encode, encode_len, output); + + ao_fec_dump_bytes(output, interleave_len, "Interleave"); + + return interleave_len; +} + +#define RADIO_LEN(input_len) (INTERLEAVE_LEN(input_len) * 8) + +static int +ao_radio(uint8_t *bits, int bits_len, uint8_t *bytes) +{ + uint8_t b, *bytes_orig = bytes; + uint8_t interleave[bits_len]; + int i, bit; + + ao_fec_interleave(bits, bits_len, interleave); + + ao_fec_dump_bytes(interleave, bits_len, "De-interleave"); + + for (i = 0; i < bits_len; i++) { + b = interleave[i]; + for (bit = 7; bit >= 0; bit--) + *bytes++ = ((b >> bit) & 1) * 0xff; + } + + ao_fec_dump_bytes(bytes_orig, bits_len * 8, "Bytes"); - ao_fec_dump_bytes(interleave, interleave_len, "Interleave"); + return bits_len * 8; } +static int +ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev) +{ + int i; + int errors = 0; + + for (i = 0; i < in_len; i++) { + double error = gaussian_random(0, dev); + uint8_t byte = in[i]; + + if (error > 0) { + if (error > 0xff) + error = 0xff; + if (error >= 0x80) + errors++; + if (byte < 0x80) + byte += error; + else + byte -= error; + } + out[i] = byte; + } + + printf ("Introduced %d errors\n", errors); + ao_fec_dump_bytes(out, in_len, "Fuzz"); + return in_len; +} + +static int +ao_decode(uint8_t *bytes, int bytes_len, uint8_t *bits) +{ + int bits_len; + + bits_len = ao_fec_decode(bytes, bytes_len, bits); + + ao_fec_dump_bytes(bits, bits_len, "Decode"); +} + +int +main(int argc, char **argv) +{ + uint8_t original[4] = { 3, 1, 2, 3 }; + uint8_t encode[INTERLEAVE_LEN(sizeof(original))]; + int encode_len; + + uint8_t transmit[RADIO_LEN(sizeof(original))]; + int transmit_len; + + uint8_t receive[RADIO_LEN(sizeof(original))]; + int receive_len; + + uint8_t decode[INTERLEAVE_LEN(sizeof(original))]; + int decode_len; + + encode_len = ao_encode(original, sizeof(original), encode); + + transmit_len = ao_radio(encode, encode_len, transmit); + + /* apply gaussian noise to test viterbi code against errors */ + receive_len = ao_fuzz(transmit, transmit_len, receive, 0x80); + + decode_len = ao_decode(receive, receive_len, decode); +} + + -- 2.30.2