1 /* 2 * Copyright (C) 2018 3 * Matthias P. Braendli (matthias.braendli@mpb.li) 4 * 5 * Copyright (C) 2017 6 * Albrecht Lohofener (albrechtloh@gmx.de) 7 * 8 * This file is based on SDR-J 9 * Copyright (C) 2010, 2011, 2012 10 * Jan van Katwijk (J.vanKatwijk@gmail.com) 11 * 12 * This file is part of the welle.io. 13 * Many of the ideas as implemented in welle.io are derived from 14 * other work, made available through the GNU general Public License. 15 * All copyrights of the original authors are recognized. 16 * 17 * welle.io is free software; you can redistribute it and/or modify 18 * it under the terms of the GNU General Public License as published by 19 * the Free Software Foundation; either version 2 of the License, or 20 * (at your option) any later version. 21 * 22 * welle.io is distributed in the hope that it will be useful, 23 * but WITHOUT ANY WARRANTY; without even the implied warranty of 24 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 * GNU General Public License for more details. 26 * 27 * You should have received a copy of the GNU General Public License 28 * along with welle.io; if not, write to the Free Software 29 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 30 * 31 */ 32 33 #ifndef MATHHELPER_H 34 #define MATHHELPER_H 35 36 #include <complex> 37 #include <cstring> 38 39 #define Hz(x) (x) 40 #define kHz(x) (x * 1000) 41 #define MHz(x) (kHz(x) * 1000) 42 43 static inline float get_db_over_256(float x) 44 { 45 return 20 * log10((x + 1.0f) / 256.0f); 46 } 47 48 static inline float l1_norm(const std::complex<float>& z) 49 { 50 return std::abs(z.real()) + std::abs(z.imag()); 51 } 52 53 static inline bool check_CRC_bits(const uint8_t *in, int size) 54 { 55 static const uint8_t crcPolynome[] = { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }; // MSB .. LSB 56 uint8_t b[16]; 57 memset(b, 1, 16); 58 59 for (int i = 0; i < size; i++) { 60 uint8_t d = in[i]; 61 if (i >= size - 16) { 62 d ^= 1; 63 } 64 65 if ((b[0] ^ d) == 1) { 66 for (int f = 0; f < 15; f++) 67 b[f] = crcPolynome[f] ^ b[f + 1]; 68 b[15] = 1; 69 } 70 else { 71 memmove(&b[0], &b[1], sizeof(uint8_t) * 15); // Shift 72 b[15] = 0; 73 } 74 } 75 76 uint16_t crc = 0; 77 for (int i = 0; i < 16; i++) 78 crc |= b[i] << i; 79 return crc == 0; 80 } 81 82 static inline bool check_crc_bytes(const uint8_t *msg, int len) 83 { 84 uint16_t accumulator = 0xFFFF; 85 const uint16_t genpoly = 0x1021; 86 87 for (int i = 0; i < len; i++) { 88 int16_t data = msg[i] << 8; 89 for (int j = 8; j > 0; j--) { 90 if ((data ^ accumulator) & 0x8000) 91 accumulator = ((accumulator << 1) ^ genpoly) & 0xFFFF; 92 else 93 accumulator = (accumulator << 1) & 0xFFFF; 94 data = (data << 1) & 0xFFFF; 95 } 96 } 97 // 98 // ok, now check with the crc that is contained 99 // in the au 100 uint16_t crc = ~((msg[len] << 8) | msg[len + 1]) & 0xFFFF; 101 return (crc ^ accumulator) == 0; 102 } 103 104 static inline uint32_t getBits(const uint8_t* d, int16_t offset, uint8_t size) 105 { 106 if (size > 32) { 107 throw std::logic_error("getBits called with size>32"); 108 } 109 110 uint32_t res = 0; 111 112 for (int i = 0; i < size; i++) { 113 res <<= 1; 114 res |= d[offset + i]; 115 } 116 return res; 117 } 118 119 static inline uint16_t getBits_1(const uint8_t* d, int16_t offset) 120 { 121 return (d[offset] & 0x01); 122 } 123 124 static inline uint16_t getBits_2(const uint8_t* d, int16_t offset) 125 { 126 uint16_t res = d[offset]; 127 res <<= 1; 128 res |= d[offset + 1]; 129 return res; 130 } 131 132 static inline uint16_t getBits_3(const uint8_t* d, int16_t offset) 133 { 134 uint16_t res = d[offset]; 135 res <<= 1; 136 res |= d[offset + 1]; 137 res <<= 1; 138 res |= d[offset + 2]; 139 return res; 140 } 141 142 static inline uint16_t getBits_4(const uint8_t* d, int16_t offset) 143 { 144 uint16_t res = d[offset]; 145 res <<= 1; 146 res |= d[offset + 1]; 147 res <<= 1; 148 res |= d[offset + 2]; 149 res <<= 1; 150 res |= d[offset + 3]; 151 return res; 152 } 153 154 static inline uint16_t getBits_5(const uint8_t* d, int16_t offset) 155 { 156 uint16_t res = d[offset]; 157 res <<= 1; 158 res |= d[offset + 1]; 159 res <<= 1; 160 res |= d[offset + 2]; 161 res <<= 1; 162 res |= d[offset + 3]; 163 res <<= 1; 164 res |= d[offset + 4]; 165 return res; 166 } 167 168 static inline uint16_t getBits_6(const uint8_t* d, int16_t offset) 169 { 170 uint16_t res = d[offset]; 171 res <<= 1; 172 res |= d[offset + 1]; 173 res <<= 1; 174 res |= d[offset + 2]; 175 res <<= 1; 176 res |= d[offset + 3]; 177 res <<= 1; 178 res |= d[offset + 4]; 179 res <<= 1; 180 res |= d[offset + 5]; 181 return res; 182 } 183 184 static inline uint16_t getBits_7(const uint8_t* d, int16_t offset) 185 { 186 uint16_t res = d[offset]; 187 res <<= 1; 188 res |= d[offset + 1]; 189 res <<= 1; 190 res |= d[offset + 2]; 191 res <<= 1; 192 res |= d[offset + 3]; 193 res <<= 1; 194 res |= d[offset + 4]; 195 res <<= 1; 196 res |= d[offset + 5]; 197 res <<= 1; 198 res |= d[offset + 6]; 199 return res; 200 } 201 202 static inline uint16_t getBits_8(const uint8_t* d, int16_t offset) 203 { 204 uint16_t res = d[offset]; 205 res <<= 1; 206 res |= d[offset + 1]; 207 res <<= 1; 208 res |= d[offset + 2]; 209 res <<= 1; 210 res |= d[offset + 3]; 211 res <<= 1; 212 res |= d[offset + 4]; 213 res <<= 1; 214 res |= d[offset + 5]; 215 res <<= 1; 216 res |= d[offset + 6]; 217 res <<= 1; 218 res |= d[offset + 7]; 219 return res; 220 } 221 222 #endif // MATHHELPER_H 223