mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Adapt to gnss-sdr coding style

Browse Source
This commit is contained in:
Carles Fernandez
2017-03-28 19:32:42 +02:00
parent fa4eb25920
commit cff63b378a
6 changed files with 561 additions and 504 deletions
Download Patch File Download Diff File Expand all files Collapse all files

175
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c
View File

@@ -52,11 +52,11 @@ static const u8 bitn[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};
*/
u8 parity(u32 x)
{
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xF;
return (0x6996 >> x) & 1;
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xF;
return (0x6996 >> x) & 1;
}
@@ -72,11 +72,12 @@ u8 parity(u32 x)
u32 getbitu(const u8 *buff, u32 pos, u8 len)
{
u32 bits = 0;
u32 i=0;
for ( i= pos; i < pos + len; i++) {
bits = (bits << 1) +
((buff[i/8] >> (7 - i%8)) & 1u);
}
u32 i=0;
for (i = pos; i < pos + len; i++)
{
bits = (bits << 1) +
((buff[i/8] >> (7 - i%8)) & 1u);
}
return bits;
}
@@ -114,17 +115,18 @@ s32 getbits(const u8 *buff, u32 pos, u8 len)
*/
void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
{
u32 mask = 1u << (len - 1);
u32 mask = 1u << (len - 1);
if (len <= 0 || 32 < len)
return;
u32 i=0;
for (i = pos; i < pos + len; i++, mask >>= 1) {
if (data & mask)
buff[i/8] |= 1u << (7 - i % 8);
else
buff[i/8] &= ~(1u << (7 - i % 8));
}
if (len <= 0 || 32 < len)
return;
u32 i = 0;
for (i = pos; i < pos + len; i++, mask >>= 1)
{
if (data & mask)
buff[i/8] |= 1u << (7 - i % 8);
else
buff[i/8] &= ~(1u << (7 - i % 8));
}
}
/** Set bit field in buffer from a signed integer.
@@ -138,7 +140,7 @@ void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
*/
void setbits(u8 *buff, u32 pos, u32 len, s32 data)
{
setbitu(buff, pos, len, (u32)data);
setbitu(buff, pos, len, (u32)data);
}
/**
@@ -153,38 +155,43 @@ void setbits(u8 *buff, u32 pos, u32 len, s32 data)
*/
void bitshl(void *buf, u32 size, u32 shift)
{
if (shift > size * CHAR_BIT) {
/* Quick check: if the shift is larger, than the buffer, zero the data */
memset(buf, 0, size);
return;
}
if (shift > size * CHAR_BIT)
{
/* Quick check: if the shift is larger, than the buffer, zero the data */
memset(buf, 0, size);
return;
}
unsigned char *dst = buf; /* Destination byte. */
const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte,
* possibly incomplete. */
unsigned char *dst = buf; /* Destination byte. */
const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, possibly incomplete. */
u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */
u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */
u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */
u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */
u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */
u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */
if (0 == byte_shift) {
/* When moving data in character boundaries, use built-in functions: move
* data, and then zero the tail. */
memmove(dst, src, full_bytes);
memset(dst + full_bytes, 0, size - full_bytes);
} else {
/* Create an accumulator: it will hold a value of two consecutive bytes */
u32 acc = *src++;
u32 i=0;
for (i = 0; i < full_bytes; ++i) {
acc = (acc << CHAR_BIT) | *src++;
*dst++ = acc >> byte_shift;
}
*dst++ = acc << CHAR_BIT >> byte_shift;
if (full_bytes + 1 < size) {
memset(dst, 0, size - full_bytes - 1);
}
}
if (0 == byte_shift)
{
/* When moving data in character boundaries, use built-in functions: move
* data, and then zero the tail. */
memmove(dst, src, full_bytes);
memset(dst + full_bytes, 0, size - full_bytes);
}
else
{
/* Create an accumulator: it will hold a value of two consecutive bytes */
u32 acc = *src++;
u32 i = 0;
for (i = 0; i < full_bytes; ++i)
{
acc = (acc << CHAR_BIT) | *src++;
*dst++ = acc >> byte_shift;
}
*dst++ = acc << CHAR_BIT >> byte_shift;
if (full_bytes + 1 < size)
{
memset(dst, 0, size - full_bytes - 1);
}
}
}
/**
@@ -206,19 +213,21 @@ void bitshl(void *buf, u32 size, u32 shift)
void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index,
u32 count)
{
u32 limit1 = count / 32;
u32 limit2 = count % 32;
u32 idx=0;
for (idx = 0; idx < limit1; ++idx) {
u32 tmp = getbitu(src, src_index, 32);
setbitu(dst, dst_index, 32, tmp);
src_index += 32;
dst_index += 32;
}
if (0 != limit2) {
u32 tmp = getbitu(src, src_index, limit2);
setbitu(dst, dst_index, limit2, tmp);
}
u32 limit1 = count / 32;
u32 limit2 = count % 32;
u32 idx = 0;
for (idx = 0; idx < limit1; ++idx)
{
u32 tmp = getbitu(src, src_index, 32);
setbitu(dst, dst_index, 32, tmp);
src_index += 32;
dst_index += 32;
}
if (0 != limit2)
{
u32 tmp = getbitu(src, src_index, limit2);
setbitu(dst, dst_index, limit2, tmp);
}
}
/**
@@ -231,11 +240,11 @@ void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index,
*/
u8 count_bits_u64(u64 v, u8 bv)
{
u8 r = 0;
int i =0;
for (i = 0; i < 16; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 64 - r;
u8 r = 0;
int i = 0;
for (i = 0; i < 16; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 64 - r;
}
/**
@@ -248,11 +257,11 @@ int i =0;
*/
u8 count_bits_u32(u32 v, u8 bv)
{
u8 r = 0;
int i=0;
for (i = 0; i < 8; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 32 - r;
u8 r = 0;
int i = 0;
for (i = 0; i < 8; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 32 - r;
}
/**
@@ -265,11 +274,11 @@ u8 count_bits_u32(u32 v, u8 bv)
*/
u8 count_bits_u16(u16 v, u8 bv)
{
u8 r = 0;
int i =0;
for (i= 0; i < 4; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 16 - r;
u8 r = 0;
int i = 0;
for (i= 0; i < 4; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 16 - r;
}
/**
@@ -282,11 +291,11 @@ int i =0;
*/
u8 count_bits_u8(u8 v, u8 bv)
{
u8 r = 0;
int i=0;
for (i = 0; i < 2; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 8 - r;
u8 r = 0;
int i = 0;
for (i = 0; i < 2; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 8 - r;
}
/** \} */

457
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c
View File

@@ -84,12 +84,13 @@
*/
static u32 _cnav_compute_crc(cnav_v27_part_t *part)
{
u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
part->invert);
u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
part->invert);
return crc;
return crc;
}
/**
* Extracts CRC-24Q from a CNAV message buffer.
* CRC-24Q value is the last 24 bits from 300 bits message buffer.
@@ -102,14 +103,16 @@ static u32 _cnav_compute_crc(cnav_v27_part_t *part)
*/
static u32 _cnav_extract_crc(const cnav_v27_part_t *part)
{
u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
GPS_CNAV_MSG_CRC_LENGTH);
if (part->invert) {
crc ^= 0xFFFFFF;
}
return crc;
u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
GPS_CNAV_MSG_CRC_LENGTH);
if (part->invert)
{
crc ^= 0xFFFFFF;
}
return crc;
}
/**
* Helper to rescan for preamble in the received buffer.
* Occasionally there could be a false lock on message contents if it the
@@ -126,31 +129,35 @@ static u32 _cnav_extract_crc(const cnav_v27_part_t *part)
*/
static void _cnav_rescan_preamble(cnav_v27_part_t *part)
{
part->preamble_seen = false;
part->preamble_seen = false;
if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) {
size_t i=0;
size_t j=0;
for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH;
i < j; ++i) {
u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH);
if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c) {
part->preamble_seen = true;
part->invert = (GPS_CNAV_PREAMBLE2 == c);
/* We shift the accumulated bits to the beginning of the buffer */
bitshl(part->decoded, sizeof(part->decoded), i);
part->n_decoded -= i;
break;
}
}
}
if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH) {
bitshl(part->decoded, sizeof(part->decoded),
part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1;
}
if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1)
{
size_t i = 0;
size_t j = 0;
for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; i < j; ++i)
{
u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH);
if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c)
{
part->preamble_seen = true;
part->invert = (GPS_CNAV_PREAMBLE2 == c);
/* We shift the accumulated bits to the beginning of the buffer */
bitshl(part->decoded, sizeof(part->decoded), i);
part->n_decoded -= i;
break;
}
}
}
if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH)
{
bitshl(part->decoded, sizeof(part->decoded),
part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1;
}
}
/**
* Feed a symbol into Viterbi decoder instance.
*
@@ -167,110 +174,126 @@ static void _cnav_rescan_preamble(cnav_v27_part_t *part)
*/
static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
{
part->symbols[part->n_symbols++] = ch;
part->symbols[part->n_symbols++] = ch;
if (part->init) {
/* Initial step - load more symbols without decoding. */
if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2) {
return;
}
part->init = false;
}
else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) {
/* Wait until decoding block is accumulated */
return;
}
/* Feed accumulated symbols into the buffer, reset the number of accumulated
* symbols. */
v27_update(&part->dec, part->symbols, part->n_symbols / 2);
part->n_symbols = 0;
/* Decode N+M bits, where:
* - N - Number of bits to put into decoded buffer
* - M - Number of bits in the tail to ignore.
*/
unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
CHAR_BIT - 1) / CHAR_BIT];
v27_chainback_likely(&part->dec, tmp_bits,
GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
/* Read decoded bits and add them to the decoded buffer */
bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
part->n_decoded += GPS_L2C_V27_DECODE_BITS;
/* Depending on the decoder state, one of the following actions are
* possible:
* - If no message lock
* - If no preamble seen - look for preamble
* - If preamble seen - collect 300 bits
* - If 300 bits are collected - verify CRC
* - If CRC is OK - message lock is acquired
* - If CRC fails - rescan for preamble
* - If found - continue collecting 300 bits
* - If not found - continue preamble wait
* - If message lock
* - If 300 bits collected, compute CRC
* - If CRC is OK, message can be decoded
* - If CRC is not OK, discard data
*/
bool retry = true;
while (retry) {
retry = false;
if (!part->preamble_seen) {
/* Rescan for preamble if possible. The first bit is ignored. */
_cnav_rescan_preamble(part);
}
if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded) {
/* We have collected 300 bits starting from message preamble. Now try
* to compute CRC-24Q */
u32 crc = _cnav_compute_crc(part);
u32 crc2 = _cnav_extract_crc(part);
if (part->message_lock) {
/* We have message lock */
part->crc_ok = (crc == crc2);
if (part->crc_ok) {
/* Reset message lock counter */
part->n_crc_fail = 0;
} else {
/* Increment message lock counter */
part->n_crc_fail++;
if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS) {
/* CRC has failed too many times - drop the lock. */
part->n_crc_fail = 0;
part->message_lock = false;
part->preamble_seen = false;
/* Try to find a new preamble, reuse data from buffer. */
retry = true;
}
if (part->init)
{
/* Initial step - load more symbols without decoding. */
if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2)
{
return;
}
part->init = false;
}
else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2)
{
/* Wait until decoding block is accumulated */
return;
}
} else if (crc == crc2) {
/* CRC match - message can be decoded */
part->message_lock = true;
part->crc_ok = true;
part->n_crc_fail = 0;
} else {
/* There is no message lock and the CRC check fails. Assume there is
* false positive lock - rescan for preamble. */
part->crc_ok = false;
part->preamble_seen = false;
/* CRC mismatch - try to re-scan for preamble */
retry = true;
}
}
else
{
/* No preamble or preamble and less than 300 bits decoded */
}
}
/* Feed accumulated symbols into the buffer, reset the number of accumulated
* symbols. */
v27_update(&part->dec, part->symbols, part->n_symbols / 2);
part->n_symbols = 0;
/* Decode N+M bits, where:
* - N - Number of bits to put into decoded buffer
* - M - Number of bits in the tail to ignore.
*/
unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
CHAR_BIT - 1) / CHAR_BIT];
v27_chainback_likely(&part->dec, tmp_bits,
GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
/* Read decoded bits and add them to the decoded buffer */
bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
part->n_decoded += GPS_L2C_V27_DECODE_BITS;
/* Depending on the decoder state, one of the following actions are
* possible:
* - If no message lock
* - If no preamble seen - look for preamble
* - If preamble seen - collect 300 bits
* - If 300 bits are collected - verify CRC
* - If CRC is OK - message lock is acquired
* - If CRC fails - rescan for preamble
* - If found - continue collecting 300 bits
* - If not found - continue preamble wait
* - If message lock
* - If 300 bits collected, compute CRC
* - If CRC is OK, message can be decoded
* - If CRC is not OK, discard data
*/
bool retry = true;
while (retry)
{
retry = false;
if (!part->preamble_seen)
{
/* Rescan for preamble if possible. The first bit is ignored. */
_cnav_rescan_preamble(part);
}
if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded)
{
/* We have collected 300 bits starting from message preamble. Now try
* to compute CRC-24Q */
u32 crc = _cnav_compute_crc(part);
u32 crc2 = _cnav_extract_crc(part);
if (part->message_lock)
{
/* We have message lock */
part->crc_ok = (crc == crc2);
if (part->crc_ok)
{
/* Reset message lock counter */
part->n_crc_fail = 0;
}
else
{
/* Increment message lock counter */
part->n_crc_fail++;
if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS)
{
/* CRC has failed too many times - drop the lock. */
part->n_crc_fail = 0;
part->message_lock = false;
part->preamble_seen = false;
/* Try to find a new preamble, reuse data from buffer. */
retry = true;
}
}
}
else if (crc == crc2)
{
/* CRC match - message can be decoded */
part->message_lock = true;
part->crc_ok = true;
part->n_crc_fail = 0;
}
else
{
/* There is no message lock and the CRC check fails. Assume there is
* false positive lock - rescan for preamble. */
part->crc_ok = false;
part->preamble_seen = false;
/* CRC mismatch - try to re-scan for preamble */
retry = true;
}
}
else
{
/* No preamble or preamble and less than 300 bits decoded */
}
}
}
/**
* Invert message bits in the buffer.
*
@@ -282,12 +305,14 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
*/
static void _cnav_msg_invert(cnav_v27_part_t *part)
{
size_t i = 0;
for (i = 0; i < sizeof(part->decoded); i++) {
part->decoded[i] ^= 0xFFu;
}
size_t i = 0;
for (i = 0; i < sizeof(part->decoded); i++)
{
part->decoded[i] ^= 0xFFu;
}
}
/**
* Performs CNAV message decoding.
*
@@ -310,39 +335,45 @@ static void _cnav_msg_invert(cnav_v27_part_t *part)
*/
static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay)
{
bool res = false;
if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) {
if (part->crc_ok) {
/* CRC is OK */
if (part->invert) {
_cnav_msg_invert(part);
}
bool res = false;
if (GPS_CNAV_MSG_LENGTH <= part->n_decoded)
{
if (part->crc_ok)
{
/* CRC is OK */
if (part->invert)
{
_cnav_msg_invert(part);
}
msg->prn = getbitu(part->decoded, 8, 6);
msg->msg_id = getbitu(part->decoded, 14, 6);
msg->tow = getbitu(part->decoded, 20, 17);
msg->alert = getbitu(part->decoded, 37, 1) ? true : false;
msg->prn = getbitu(part->decoded, 8, 6);
msg->msg_id = getbitu(part->decoded, 14, 6);
msg->tow = getbitu(part->decoded, 20, 17);
msg->alert = getbitu(part->decoded, 37, 1) ? true : false;
/* copy RAW message for GNSS-SDR */
memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
/* copy RAW message for GNSS-SDR */
memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
*delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS)
* 2 + part->n_symbols;
*delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols;
if (part->invert) {
_cnav_msg_invert(part);
}
res = true;
} else {
/* CRC mismatch - no decoding */
}
bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH);
part->n_decoded -= GPS_CNAV_MSG_LENGTH;
}
if (part->invert)
{
_cnav_msg_invert(part);
}
res = true;
}
else
{
/* CRC mismatch - no decoding */
}
bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH);
part->n_decoded -= GPS_CNAV_MSG_LENGTH;
}
return res;
return res;
}
/**
* Initialize CNAV decoder.
*
@@ -355,20 +386,20 @@ static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay)
*/
void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
{
memset(dec, 0, sizeof(*dec));
v27_init(&dec->part1.dec,
dec->part1.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(),
0);
v27_init(&dec->part2.dec,
dec->part2.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(),
0);
dec->part1.init = true;
dec->part2.init = true;
_cnav_add_symbol(&dec->part2, 0x80);
memset(dec, 0, sizeof(*dec));
v27_init(&dec->part1.dec,
dec->part1.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(),
0);
v27_init(&dec->part2.dec,
dec->part2.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(),
0);
dec->part1.init = true;
dec->part2.init = true;
_cnav_add_symbol(&dec->part2, 0x80);
}
/**
@@ -394,29 +425,32 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
* \retval false More data is required.
*/
bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
u8 symbol,
cnav_msg_t *msg,
u32 *pdelay)
u8 symbol,
cnav_msg_t *msg,
u32 *pdelay)
{
_cnav_add_symbol(&dec->part1, symbol);
_cnav_add_symbol(&dec->part2, symbol);
_cnav_add_symbol(&dec->part1, symbol);
_cnav_add_symbol(&dec->part2, symbol);
if (dec->part1.message_lock) {
/* Flush data in decoder. */
dec->part2.n_decoded = 0;
dec->part2.n_symbols = 0;
return _cnav_msg_decode(&dec->part1, msg, pdelay);
}
if (dec->part2.message_lock) {
/* Flush data in decoder. */
dec->part1.n_decoded = 0;
dec->part1.n_symbols = 0;
return _cnav_msg_decode(&dec->part2, msg, pdelay);
}
if (dec->part1.message_lock)
{
/* Flush data in decoder. */
dec->part2.n_decoded = 0;
dec->part2.n_symbols = 0;
return _cnav_msg_decode(&dec->part1, msg, pdelay);
}
if (dec->part2.message_lock)
{
/* Flush data in decoder. */
dec->part1.n_decoded = 0;
dec->part1.n_symbols = 0;
return _cnav_msg_decode(&dec->part2, msg, pdelay);
}
return false;
return false;
}
/**
* Provides a singleton polynomial object.
*
@@ -429,27 +463,28 @@ bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
*/
const v27_poly_t *cnav_msg_decoder_get_poly(void)
{
static v27_poly_t instance;
static bool initialized = false;
static v27_poly_t instance;
static bool initialized = false;
if (!initialized) {
/* Coefficients for polynomial object */
const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B };
if (!initialized)
{
/* Coefficients for polynomial object */
const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B };
/* Racing condition handling: the data can be potential initialized more
* than once in case multiple threads request concurrent access. However,
* nature of the v27_poly_init() function and data alignment ensure that
* the data returned from the earlier finished call is consistent and can
* be used even when re-initialization is happening.
*
* Other possible approaches are:
* - Replace late initialization with an explicit call.
* - Use POSIX synchronization objects like pthread_once_t.
*/
v27_poly_init(&instance, coeffs);
initialized = true;
}
return &instance;
/* Racing condition handling: the data can be potential initialized more
* than once in case multiple threads request concurrent access. However,
* nature of the v27_poly_init() function and data alignment ensure that
* the data returned from the earlier finished call is consistent and can
* be used even when re-initialization is happening.
*
* Other possible approaches are:
* - Replace late initialization with an explicit call.
* - Use POSIX synchronization objects like pthread_once_t.
*/
v27_poly_init(&instance, coeffs);
initialized = true;
}
return &instance;
}
/** \} */

50
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h
View File

@@ -62,11 +62,11 @@
*/
typedef struct
{
u8 prn; /**< SV PRN. 0..31 */
u8 msg_id; /**< Message id. 0..31 */
u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */
bool alert; /**< CNAV message alert flag */
u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */
u8 prn; /**< SV PRN. 0..31 */
u8 msg_id; /**< Message id. 0..31 */
u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */
bool alert; /**< CNAV message alert flag */
u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */
} cnav_msg_t;
/**
@@ -76,24 +76,24 @@ typedef struct
* @sa cnav_msg_decoder_t
*/
typedef struct {
v27_t dec; /**< Viterbi block decoder object */
v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS];
/**< Decision graph */
unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2];
/**< Symbol buffer */
size_t n_symbols; /**< Count of symbols in the symbol buffer */
unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS];
/**< Decode buffer */
size_t n_decoded; /**< Number of bits in the decode buffer */
bool preamble_seen; /**< When true, the decode buffer is aligned on
* preamble. */
bool invert; /**< When true, indicates the bits are inverted */
bool message_lock; /**< When true, indicates the message boundary
* is found. */
bool crc_ok; /**< Flag that the last message had good CRC */
size_t n_crc_fail; /**< Counter for CRC failures */
bool init; /**< Initial state flag. When true, initial bits
* do not produce output. */
v27_t dec; /**< Viterbi block decoder object */
v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS];
/**< Decision graph */
unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2];
/**< Symbol buffer */
size_t n_symbols; /**< Count of symbols in the symbol buffer */
unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS];
/**< Decode buffer */
size_t n_decoded; /**< Number of bits in the decode buffer */
bool preamble_seen; /**< When true, the decode buffer is aligned on
* preamble. */
bool invert; /**< When true, indicates the bits are inverted */
bool message_lock; /**< When true, indicates the message boundary
* is found. */
bool crc_ok; /**< Flag that the last message had good CRC */
size_t n_crc_fail; /**< Counter for CRC failures */
bool init; /**< Initial state flag. When true, initial bits
* do not produce output. */
} cnav_v27_part_t;
/**
@@ -104,8 +104,8 @@ typedef struct {
*/
typedef struct
{
cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */
cnav_v27_part_t part2; /**< Decoder for even symbol pairs */
cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */
cnav_v27_part_t part2; /**< Decoder for even symbol pairs */
} cnav_msg_decoder_t;
const v27_poly_t *cnav_msg_decoder_get_poly(void);

109
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c
View File

@@ -40,38 +40,38 @@
* \{ */
static const u32 crc24qtab[256] = {
0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
};
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
@@ -91,10 +91,10 @@ static const u32 crc24qtab[256] = {
*/
u32 crc24q(const u8 *buf, u32 len, u32 crc)
{
u32 i=0;
for ( i= 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff];
return crc;
u32 i = 0;
for (i = 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff];
return crc;
}
/**
@@ -114,27 +114,30 @@ u32 crc24q(const u8 *buf, u32 len, u32 crc)
*/
u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert)
{
u16 acc = 0;
u8 b = 0;
u32 shift = 8 - n_bits % 8;
u16 acc = 0;
u8 b = 0;
u32 shift = 8 - n_bits % 8;
u32 i=0;
for ( i= 0; i < n_bits / 8; ++i) {
acc = (acc << 8) | *buf++;
if (invert) {
acc ^= 0xFFu;
}
u32 i = 0;
for (i = 0; i < n_bits / 8; ++i)
{
acc = (acc << 8) | *buf++;
if (invert)
{
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
}
acc = (acc << 8) | *buf;
if (invert)
{
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
}
acc = (acc << 8) | *buf;
if (invert) {
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
return crc;
return crc;
}

22
src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h
View File

@@ -40,25 +40,25 @@
#define V27POLYB 0x6d
typedef struct {
unsigned char c0[32];
unsigned char c1[32];
unsigned char c0[32];
unsigned char c1[32];
} v27_poly_t;
typedef struct {
unsigned int w[2];
unsigned int w[2];
} v27_decision_t;
/* State info for instance of r=1/2 k=7 Viterbi decoder
*/
typedef struct {
unsigned int metrics1[64]; /* Path metric buffer 1 */
unsigned int metrics2[64]; /* Path metric buffer 2 */
/* Pointers to path metrics, swapped on every bit */
unsigned int *old_metrics, *new_metrics;
const v27_poly_t *poly; /* Polynomial to use */
v27_decision_t *decisions; /* Beginning of decisions for block */
unsigned int decisions_index; /* Index of current decision */
unsigned int decisions_count; /* Number of decisions in history */
unsigned int metrics1[64]; /* Path metric buffer 1 */
unsigned int metrics2[64]; /* Path metric buffer 2 */
/* Pointers to path metrics, swapped on every bit */
unsigned int *old_metrics, *new_metrics;
const v27_poly_t *poly; /* Polynomial to use */
v27_decision_t *decisions; /* Beginning of decisions for block */
unsigned int decisions_index; /* Index of current decision */
unsigned int decisions_count; /* Number of decisions in history */
} v27_t;
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]);

252
src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c
View File

@@ -35,13 +35,14 @@
static inline int parity(int x)
{
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xf;
return (0x6996 >> x) & 1;
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xf;
return (0x6996 >> x) & 1;
}
/** Initialize a v27_poly_t struct for use with a v27_t decoder.
*
* \param poly Structure to initialize.
@@ -49,14 +50,16 @@ static inline int parity(int x)
*/
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
{
int state;
int state;
for(state = 0; state < 32; state++) {
poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0;
poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0;
}
for(state = 0; state < 32; state++)
{
poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0;
poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0;
}
}
/** Initialize a v27_t struct for Viterbi decoding.
*
* \param v Structure to initialize
@@ -71,35 +74,36 @@ void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count,
const v27_poly_t *poly, unsigned char initial_state)
{
int i;
int i;
v->old_metrics = v->metrics1;
v->new_metrics = v->metrics2;
v->poly = poly;
v->decisions = decisions;
v->decisions_index = 0;
v->decisions_count = decisions_count;
v->old_metrics = v->metrics1;
v->new_metrics = v->metrics2;
v->poly = poly;
v->decisions = decisions;
v->decisions_index = 0;
v->decisions_count = decisions_count;
for(i = 0; i < 64; i++)
v->old_metrics[i] = 63;
for(i = 0; i < 64; i++)
v->old_metrics[i] = 63;
v->old_metrics[initial_state & 63] = 0; /* Bias known start state */
v->old_metrics[initial_state & 63] = 0; /* Bias known start state */
}
/* C-language butterfly */
#define BFLY(i) {\
unsigned int metric,m0,m1,decision;\
metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\
m0 = v->old_metrics[i] + metric;\
m1 = v->old_metrics[i+32] + (510 - metric);\
decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i)&31);\
m0 -= (metric+metric-510);\
m1 += (metric+metric-510);\
decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i+1] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i+1)&31);\
unsigned int metric,m0,m1,decision;\
metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\
m0 = v->old_metrics[i] + metric;\
m1 = v->old_metrics[i+32] + (510 - metric);\
decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i)&31);\
m0 -= (metric+metric-510);\
m1 += (metric+metric-510);\
decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i+1] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i+1)&31);\
}
/** Update a v27_t decoder with a block of symbols.
@@ -111,77 +115,81 @@ unsigned int metric,m0,m1,decision;\
*/
void v27_update(v27_t *v, const unsigned char *syms, int nbits)
{
unsigned char sym0, sym1;
unsigned int *tmp;
int normalize = 0;
unsigned char sym0, sym1;
unsigned int *tmp;
int normalize = 0;
while(nbits--) {
v27_decision_t *d = &v->decisions[v->decisions_index];
while(nbits--)
{
v27_decision_t *d = &v->decisions[v->decisions_index];
d->w[0] = d->w[1] = 0;
sym0 = *syms++;
sym1 = *syms++;
d->w[0] = d->w[1] = 0;
sym0 = *syms++;
sym1 = *syms++;
BFLY(0);
BFLY(1);
BFLY(2);
BFLY(3);
BFLY(4);
BFLY(5);
BFLY(6);
BFLY(7);
BFLY(8);
BFLY(9);
BFLY(10);
BFLY(11);
BFLY(12);
BFLY(13);
BFLY(14);
BFLY(15);
BFLY(16);
BFLY(17);
BFLY(18);
BFLY(19);
BFLY(20);
BFLY(21);
BFLY(22);
BFLY(23);
BFLY(24);
BFLY(25);
BFLY(26);
BFLY(27);
BFLY(28);
BFLY(29);
BFLY(30);
BFLY(31);
BFLY(0);
BFLY(1);
BFLY(2);
BFLY(3);
BFLY(4);
BFLY(5);
BFLY(6);
BFLY(7);
BFLY(8);
BFLY(9);
BFLY(10);
BFLY(11);
BFLY(12);
BFLY(13);
BFLY(14);
BFLY(15);
BFLY(16);
BFLY(17);
BFLY(18);
BFLY(19);
BFLY(20);
BFLY(21);
BFLY(22);
BFLY(23);
BFLY(24);
BFLY(25);
BFLY(26);
BFLY(27);
BFLY(28);
BFLY(29);
BFLY(30);
BFLY(31);
/* Normalize metrics if they are nearing overflow */
if(v->new_metrics[0] > (1<<30)) {
int i;
unsigned int minmetric = 1<<31;
/* Normalize metrics if they are nearing overflow */
if(v->new_metrics[0] > (1 << 30))
{
int i;
unsigned int minmetric = 1 << 31;
for(i=0; i<64; i++) {
if(v->new_metrics[i] < minmetric)
minmetric = v->new_metrics[i];
}
for(i = 0; i < 64; i++)
{
if(v->new_metrics[i] < minmetric)
minmetric = v->new_metrics[i];
}
for(i=0; i<64; i++)
v->new_metrics[i] -= minmetric;
for(i = 0; i < 64; i++)
v->new_metrics[i] -= minmetric;
normalize += minmetric;
}
normalize += minmetric;
}
/* Advance decision index */
if(++v->decisions_index >= v->decisions_count)
v->decisions_index = 0;
/* Advance decision index */
if(++v->decisions_index >= v->decisions_count)
v->decisions_index = 0;
/* Swap pointers to old and new metrics */
tmp = v->old_metrics;
v->old_metrics = v->new_metrics;
v->new_metrics = tmp;
}
/* Swap pointers to old and new metrics */
tmp = v->old_metrics;
v->old_metrics = v->new_metrics;
v->new_metrics = tmp;
}
}
/** Retrieve the most likely output bit sequence with known final state from
* a v27_t decoder.
*
@@ -193,28 +201,30 @@ void v27_update(v27_t *v, const unsigned char *syms, int nbits)
void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
unsigned char final_state)
{
int k;
unsigned int decisions_index = v->decisions_index;
int k;
unsigned int decisions_index = v->decisions_index;
final_state %= 64;
final_state <<= 2;
final_state %= 64;
final_state <<= 2;
while(nbits-- != 0) {
while(nbits-- != 0)
{
/* Decrement decision index */
decisions_index = (decisions_index == 0) ?
v->decisions_count-1 : decisions_index-1;
/* Decrement decision index */
decisions_index = (decisions_index == 0) ?
v->decisions_count-1 : decisions_index-1;
v27_decision_t *d = &v->decisions[decisions_index];
k = (d->w[(final_state>>2)/32] >> ((final_state>>2)%32)) & 1;
/* The store into data[] only needs to be done every 8 bits.
* But this avoids a conditional branch, and the writes will
* combine in the cache anyway
*/
data[nbits>>3] = final_state = (final_state >> 1) | (k << 7);
}
v27_decision_t *d = &v->decisions[decisions_index];
k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1;
/* The store into data[] only needs to be done every 8 bits.
* But this avoids a conditional branch, and the writes will
* combine in the cache anyway
*/
data[nbits >> 3] = final_state = (final_state >> 1) | (k << 7);
}
}
/** Retrieve the most likely output bit sequence with unknown final state from
* a v27_t decoder.
*
@@ -224,19 +234,19 @@ void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
*/
void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits)
{
/* Determine state with minimum metric */
/* Determine state with minimum metric */
int i;
unsigned int best_metric = 0xffffffff;
unsigned char best_state = 0;
for(i=0; i<64; i++)
{
if(v->new_metrics[i] < best_metric)
{
best_metric = v->new_metrics[i];
best_state = i;
}
}
int i;
unsigned int best_metric = 0xffffffff;
unsigned char best_state = 0;
for(i = 0; i < 64; i++)
{
if(v->new_metrics[i] < best_metric)
{
best_metric = v->new_metrics[i];
best_state = i;
}
}
v27_chainback_fixed(v, data, nbits, best_state);
v27_chainback_fixed(v, data, nbits, best_state);
}