git.gag.com Git - fw/altos/blob - src/test/ao_fec_tx_test.c

   1 /*
   2  * Copyright © 2012 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <ao_fec.h>
  19 #include <stdlib.h>
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <math.h>
  23
  24 #ifndef RANDOM_MAX
  25 #define RANDOM_MAX 0x7fffffff
  26 #endif
  27
  28 static double
  29 rand_real(void) {
  30         return (double) random() / (double) RANDOM_MAX;
  31 }
  32
  33 static double
  34 gaussian_random(double mean, double dev)
  35 {
  36         static int      save_x_valid = 0;
  37         static double   save_x;
  38         double          x;
  39
  40         if (save_x_valid)
  41         {
  42                 x = save_x;
  43                 save_x_valid = 0;
  44         }
  45         else
  46         {
  47                 double    w;
  48                 double    normal_x1, normal_x2;
  49
  50                 do {
  51                         normal_x1 = 2 * rand_real () - 1;
  52                         normal_x2 = 2 * rand_real () - 1;
  53                         w = normal_x1*normal_x1 + normal_x2*normal_x2;
  54                 } while (w >= 1 || w < 1E-30);
  55
  56                 w = sqrt(log(w)*(-2./w));
  57
  58                 /*
  59                  * normal_x1 and normal_x2 are independent normally
  60                  * distributed variates
  61                  */
  62
  63                 x = normal_x1 * w;
  64                 /* save normal_x2 for next call */
  65                 save_x = normal_x2 * w;
  66                 save_x_valid = 1;
  67         }
  68         return x * dev + mean;
  69 }
  70
  71 #define PREPARE_LEN(input_len)          ((input_len) + AO_FEC_PREPARE_EXTRA)
  72 #define ENCODE_LEN(input_len)           (PREPARE_LEN(input_len) * 2)
  73 #define INTERLEAVE_LEN(input_len)       ENCODE_LEN(input_len)
  74
  75 static int
  76 ao_encode(uint8_t *input, int input_len, uint8_t *output)
  77 {
  78         uint8_t         prepare[PREPARE_LEN(input_len)];
  79         uint8_t         encode[ENCODE_LEN(input_len)];
  80         uint8_t         interleave[INTERLEAVE_LEN(input_len)];
  81         uint8_t         prepare_len;
  82         uint8_t         encode_len;
  83         uint8_t         interleave_len;
  84
  85         ao_fec_dump_bytes(input, input_len, "Input");
  86
  87         prepare_len = ao_fec_prepare(input, input_len, prepare);
  88
  89         ao_fec_dump_bytes(prepare, prepare_len, "Prepare");
  90
  91         encode_len = ao_fec_encode(prepare, prepare_len, encode);
  92
  93         ao_fec_dump_bytes(encode, encode_len, "Encode");
  94
  95         interleave_len = ao_fec_interleave(encode, encode_len, output);
  96
  97         ao_fec_dump_bytes(output, interleave_len, "Interleave");
  98
  99         return interleave_len;
 100 }
 101
 102 #define RADIO_LEN(input_len)    (INTERLEAVE_LEN(input_len) * 8)
 103
 104 static int
 105 ao_radio(uint8_t *bits, int bits_len, uint8_t *bytes)
 106 {
 107         uint8_t b, *bytes_orig = bytes;
 108         uint8_t interleave[bits_len];
 109         int     i, bit;
 110
 111         ao_fec_interleave(bits, bits_len, interleave);
 112
 113         ao_fec_dump_bytes(interleave, bits_len, "De-interleave");
 114
 115         for (i = 0; i < bits_len; i++) {
 116                 b = interleave[i];
 117                 for (bit = 7; bit >= 0; bit--)
 118                         *bytes++ = ((b >> bit) & 1) * 0xff;
 119         }
 120
 121         ao_fec_dump_bytes(bytes_orig, bits_len * 8, "Bytes");
 122
 123         return bits_len * 8;
 124 }
 125
 126 static int
 127 ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev)
 128 {
 129         int     i;
 130         int     errors = 0;
 131
 132         for (i = 0; i < in_len; i++) {
 133                 double  error = gaussian_random(0, dev);
 134                 uint8_t byte = in[i];
 135
 136                 if (error > 0) {
 137                         if (error > 0xff)
 138                                 error = 0xff;
 139                         if (error >= 0x80)
 140                                 errors++;
 141                         if (byte < 0x80)
 142                                 byte += error;
 143                         else
 144                                 byte -= error;
 145                 }
 146                 out[i] = byte;
 147         }
 148
 149         printf ("Introduced %d errors\n", errors);
 150         ao_fec_dump_bytes(out, in_len, "Fuzz");
 151         return in_len;
 152 }
 153
 154 static int
 155 ao_decode(uint8_t *bytes, int bytes_len, uint8_t *bits)
 156 {
 157         int     bits_len;
 158
 159         bits_len = ao_fec_decode(bytes, bytes_len, bits);
 160
 161         ao_fec_dump_bytes(bits, bits_len, "Decode");
 162 }
 163
 164 int
 165 main(int argc, char **argv)
 166 {
 167         uint8_t         original[4] = { 3, 1, 2, 3 };
 168         uint8_t         encode[INTERLEAVE_LEN(sizeof(original))];
 169         int             encode_len;
 170
 171         uint8_t         transmit[RADIO_LEN(sizeof(original))];
 172         int             transmit_len;
 173
 174         uint8_t         receive[RADIO_LEN(sizeof(original))];
 175         int             receive_len;
 176
 177         uint8_t         decode[INTERLEAVE_LEN(sizeof(original))];
 178         int             decode_len;
 179
 180         encode_len = ao_encode(original, sizeof(original), encode);
 181
 182         transmit_len = ao_radio(encode, encode_len, transmit);
 183
 184         /* apply gaussian noise to test viterbi code against errors */
 185         receive_len = ao_fuzz(transmit, transmit_len, receive, 0x80);
 186
 187         decode_len = ao_decode(receive, receive_len, decode);
 188 }
 189
 190