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         prepare_len;
  81         uint8_t         encode_len;
  82         uint8_t         interleave_len;
  83
  84         ao_fec_dump_bytes(input, input_len, "Input");
  85
  86         prepare_len = ao_fec_prepare(input, input_len, prepare);
  87
  88         ao_fec_dump_bytes(prepare, prepare_len, "Prepare");
  89
  90         encode_len = ao_fec_encode(prepare, prepare_len, encode);
  91
  92         ao_fec_dump_bytes(encode, encode_len, "Encode");
  93
  94         interleave_len = ao_fec_interleave(encode, encode_len, output);
  95
  96         ao_fec_dump_bytes(output, interleave_len, "Interleave");
  97
  98         return interleave_len;
  99 }
 100
 101 #define RADIO_LEN(input_len)    (INTERLEAVE_LEN(input_len) * 8)
 102
 103 static int
 104 ao_radio(uint8_t *bits, int bits_len, uint8_t *bytes)
 105 {
 106         uint8_t b, *bytes_orig = bytes;
 107         uint8_t interleave[bits_len];
 108         int     i, bit;
 109
 110         ao_fec_interleave(bits, bits_len, interleave);
 111
 112         ao_fec_dump_bytes(interleave, bits_len, "De-interleave");
 113
 114         for (i = 0; i < bits_len; i++) {
 115                 b = interleave[i];
 116                 for (bit = 7; bit >= 0; bit--)
 117                         *bytes++ = ((b >> bit) & 1) * 0xff;
 118         }
 119
 120         ao_fec_dump_bytes(bytes_orig, bits_len * 8, "Bytes");
 121
 122         return bits_len * 8;
 123 }
 124
 125 static int
 126 ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev)
 127 {
 128         int     i;
 129         int     errors = 0;
 130
 131         for (i = 0; i < in_len; i++) {
 132                 double  error = gaussian_random(0, dev);
 133                 uint8_t byte = in[i];
 134
 135                 if (error > 0) {
 136                         if (error > 0xff)
 137                                 error = 0xff;
 138                         if (error >= 0x80)
 139                                 errors++;
 140                         if (byte < 0x80)
 141                                 byte += error;
 142                         else
 143                                 byte -= error;
 144                 }
 145                 out[i] = byte;
 146         }
 147
 148         printf ("Introduced %d errors\n", errors);
 149         ao_fec_dump_bytes(out, in_len, "Fuzz");
 150         return in_len;
 151 }
 152
 153 static int
 154 ao_decode(uint8_t *bytes, int bytes_len, uint8_t *bits)
 155 {
 156         int     bits_len;
 157
 158         bits_len = ao_fec_decode(bytes, bytes_len, bits);
 159
 160         ao_fec_dump_bytes(bits, bits_len, "Decode");
 161         return bits_len;
 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, 0x70);
 186
 187         decode_len = ao_decode(receive, receive_len, decode);
 188
 189         return decode_len >= sizeof(original);
 190 }
 191
 192