mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-06 18:23:05 +00:00
Code Issues Releases Wiki Activity

GPS L2CM telemetry decoder rewrite, borrowed from the Swift navigator open source libraries

Browse Source
This commit is contained in:
Javier Arribas
2017-03-23 15:45:41 +01:00
parent 9ea8d7d759
commit b96d1707a9
32 changed files with 1783 additions and 503 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/algorithms/telemetry_decoder/libs/CMakeLists.txt
View File

@@ -16,9 +16,11 @@
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
add_subdirectory(libswiftcnav)
set(TELEMETRY_DECODER_LIB_SOURCES
gps_l1_ca_subframe_fsm.cc
viterbi_decoder.cc
viterbi_decoder.cc
)
include_directories(
@@ -33,7 +35,8 @@ include_directories(
)
file(GLOB TELEMETRY_DECODER_LIB_HEADERS "*.h")
list(SORT TELEMETRY_DECODER_LIB_HEADERS)
add_library(telemetry_decoder_lib ${TELEMETRY_DECODER_LIB_SOURCES} ${TELEMETRY_DECODER_LIB_HEADERS})
source_group(Headers FILES ${TELEMETRY_DECODER_LIB_HEADERS})
target_link_libraries(telemetry_decoder_lib gnss_system_parameters)
target_link_libraries(telemetry_decoder_lib gnss_system_parameters)

35
src/algorithms/telemetry_decoder/libs/libswiftcnav/CMakeLists.txt Normal file
View File

@@ -0,0 +1,35 @@
# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors)
#
# This file is part of GNSS-SDR.
#
# GNSS-SDR 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, either version 3 of the License, or
# (at your option) any later version.
#
# GNSS-SDR 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 GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
set(TELEMETRY_DECODER_LIBSWIFTCNAV_SOURCES
cnav_msg.c
bits.c
edc.c
viterbi27.c
)
include_directories(
$(CMAKE_CURRENT_SOURCE_DIR)
)
file(GLOB TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS "*.h")
list(SORT TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS)
add_library(telemetry_decoder_libswiftcnav STATIC ${TELEMETRY_DECODER_LIBSWIFTCNAV_SOURCES} ${TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS})
source_group(Headers FILES ${TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS})
set_target_properties(telemetry_decoder_libswiftcnav PROPERTIES LINKER_LANGUAGE C)

268
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c Normal file
View File

@@ -0,0 +1,268 @@
/*
* Copyright (C) 2013, 2016 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "bits.h"
#include <limits.h>
#include <string.h>
static const u8 bitn[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};
/** \defgroup bits Bit Utils
* Bit field packing, unpacking and utility functions.
* \{ */
/** Computes the parity of a 32-bit word.
*
* References:
* -# https://graphics.stanford.edu/~seander/bithacks.html#ParityParallel
*
* \param x Word for which to compute parity
* \return 1 if there are an odd number of bits set.
* 0 if there are an even number of bits set.
*/
u8 parity(u32 x)
{
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xF;
return (0x6996 >> x) & 1;
}
/** Get bit field from buffer as an unsigned integer.
* Unpacks `len` bits at bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \return Bit field as an unsigned value.
*/
u32 getbitu(const u8 *buff, u32 pos, u8 len)
{
u32 bits = 0;
for (u32 i = pos; i < pos + len; i++) {
bits = (bits << 1) +
((buff[i/8] >> (7 - i%8)) & 1u);
}
return bits;
}
/** Get bit field from buffer as a signed integer.
* Unpacks `len` bits at bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* This function sign extends the `len` bit field to a signed 32 bit integer.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \return Bit field as a signed value.
*/
s32 getbits(const u8 *buff, u32 pos, u8 len)
{
s32 bits = (s32)getbitu(buff, pos, len);
/* Sign extend, taken from:
* http://graphics.stanford.edu/~seander/bithacks.html#VariableSignExtend
*/
s32 m = 1u << (len - 1);
return (bits ^ m) - m;
}
/** Set bit field in buffer from an unsigned integer.
* Packs `len` bits into bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \param data Unsigned integer to be packed into bit field.
*/
void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
{
u32 mask = 1u << (len - 1);
if (len <= 0 || 32 < len)
return;
for (u32 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.
* Packs `len` bits into bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \param data Signed integer to be packed into bit field.
*/
void setbits(u8 *buff, u32 pos, u32 len, s32 data)
{
setbitu(buff, pos, len, (u32)data);
}
/**
* Shift MSB bit buffer contents to the left.
* The method performs in-place shift operation.
*
* \param[in,out] buf Pointer to buffer head.
* \param[in] size Number of bytes in the buffer.
* \param[in] shift Number of bits for left shift operation.
*
* \return None
*/
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;
}
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 */
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++;
for (u32 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);
}
}
}
/**
* Performs block bit copy operation.
*
* The function copies given number of bits from the source to destination.
*
* \param[in,out] dst Pointer to destination buffer.
* \param[in] dst_index Destination bit index.
* \param[in] src Source buffer.
* \param[in] src_index Source bit index.
* \param[in] count Number of bits to copy.
*
* \return None
*
* \todo This function can be optimized for copying aligned data and using
* proper native type like long.
*/
void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index,
u32 count)
{
u32 limit1 = count / 32;
u32 limit2 = count % 32;
for (u32 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);
}
}
/**
* Count number of bits set to certain value in 64 bits word
*
* \param[in] v input 64 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u64(u64 v, u8 bv)
{
u8 r = 0;
for (int i = 0; i < 16; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 64 - r;
}
/**
* Count number of bits set to certain value in 32 bits word
*
* \param[in] v input 32 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u32(u32 v, u8 bv)
{
u8 r = 0;
for (int i = 0; i < 8; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 32 - r;
}
/**
* Count number of bits set to certain value in 16 bits word
*
* \param[in] v input 16 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u16(u16 v, u8 bv)
{
u8 r = 0;
for (int i = 0; i < 4; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 16 - r;
}
/**
* Count number of bits set to certain value in 8 bits word
*
* \param[in] v input 8 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u8(u8 v, u8 bv)
{
u8 r = 0;
for (int i = 0; i < 2; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 8 - r;
}
/** \} */

31
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h Normal file
View File

@@ -0,0 +1,31 @@
/*
* Copyright (C) 2013, 2016 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_BITS_H
#define LIBSWIFTNAV_BITS_H
#include "swift_common.h"
u8 parity(u32 x);
u32 getbitu(const u8 *buff, u32 pos, u8 len);
s32 getbits(const u8 *buff, u32 pos, u8 len);
void setbitu(u8 *buff, u32 pos, u32 len, u32 data);
void setbits(u8 *buff, u32 pos, u32 len, s32 data);
void bitcopy(void *dst, u32 dst_index,
const void *src, u32 src_index, u32 count);
void bitshl(void *buf, u32 size, u32 shift);
u8 count_bits_u64(u64 v, u8 bv);
u8 count_bits_u32(u32 v, u8 bv);
u8 count_bits_u16(u16 v, u8 bv);
u8 count_bits_u8(u8 v, u8 bv);
#endif /* LIBSWIFTNAV_BITS_H */

432
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c Normal file
View File

@@ -0,0 +1,432 @@
/*
* Copyright (C) 2016 Swift Navigation Inc.
* Contact: Valeri Atamaniouk <valeri@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
#include "bits.h"
#include "cnav_msg.h"
#include <limits.h>
#include <string.h>
/** \defgroup GPS_L2 GPS L2
* GPS L2 operations
* \{ */
/** \defgroup gps_cnav_decoder Decoder
* GPS L2C CNAV message decoding.
* \{ */
/*
* Block Viterbi decoding parameters.
*/
/** Viterbi decoder reversed polynomial A */
#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/
/** Viterbi decoder reversed polynomial B */
#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */
/*
* GPS L2C message constants.
*/
/** GPS L2C preamble */
#define GPS_CNAV_PREAMBLE1 (0b10001011u)
/** Inverted GPS L2C preamble */
#define GPS_CNAV_PREAMBLE2 (0b01110100u)
/** GPS L2C preamble length in bits */
#define GPS_CNAV_PREAMBLE_LENGTH (8)
/** GPS L2C CNAV message length in bits */
#define GPS_CNAV_MSG_LENGTH (300)
/** GPS LC2 CNAV CRC length in bits */
#define GPS_CNAV_MSG_CRC_LENGTH (24)
/** GPS L2C CNAV message payload length in bits */
#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH-GPS_CNAV_MSG_CRC_LENGTH)
/** GPS L2C CNAV message lock detector threshold */
#define GPS_CNAV_LOCK_MAX_CRC_FAILS (10)
/**
* Computes CRC-24Q from a CNAV message buffer.
* CRC-24Q is computed for 274 bits. For a purpose of 8-bit alignment, the
* message is assumed to be prepended with four zero bits.
*
* \param[in] part Decoder component with payload
*
* \return CRC-24Q value.
*
* \private
*/
static u32 _cnav_compute_crc(cnav_v27_part_t *part)
{
u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
part->invert);
return crc;
}
/**
* Extracts CRC-24Q from a CNAV message buffer.
* CRC-24Q value is the last 24 bits from 300 bits message buffer.
*
* \param[in] part Decoded component with payload.
*
* \return CRC24-Q value.
*
* \private
*/
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;
}
/**
* Helper to rescan for preamble in the received buffer.
* Occasionally there could be a false lock on message contents if it the
* preamble sequence is encountered in the message body. For this case, the
* function performs for a scan for a preamble with a different offset:
* - When found, the preamble octet is moved into the head of the buffer.
* - When not found, only 7 bits are left in the buffer.
*
* \param[in,out] part Decoded component.
*
* \return None
*
* \private
*/
static void _cnav_rescan_preamble(cnav_v27_part_t *part)
{
part->preamble_seen = false;
if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) {
for (size_t 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.
*
* The method uses block Viterbi decoder. It first accumulates initial number of
* symbols, and after that runs decoding every time the buffer is full. Only
* some of the decoded symbols are used.
*
* \param[in,out] part Decoder object
* \param[in] s Symbol (0x00 - Hard 0, 0xFF - Hard 1)
*
* \return None
*
* \private
*/
static void _cnav_add_symbol(cnav_v27_part_t *part, u8 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;
}
}
} 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.
*
* The method inverts bits of the decoded data.
*
* \param[in,out] part Decoder component with a message buffer.
*
* \return None
*/
static void _cnav_msg_invert(cnav_v27_part_t *part)
{
for (size_t i = 0; i < sizeof(part->decoded); i++) {
part->decoded[i] ^= 0xFFu;
}
}
/**
* Performs CNAV message decoding.
*
* This function decoded CNAV message, if the following conditions are met:
* - Buffer contains 300 bits.
* - First 8 bits are matching direct or inverse preamble.
* - Message data CRC matches one in the buffer.
*
* In case the message starts with inverted preamble, the data is inverted
* before parsing.
*
* \param[in,out] part Decoder component.
* \param[out] msg Container for a decoded message.
* \param[out] delay Delay of the message in symbols.
*
* \retval true The message has been decoded, and \a msg container is populated.
* \retval false Not enough data or CRC is not correct.
*
* \private
*/
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);
}
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);
*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;
}
return res;
}
/**
* Initialize CNAV decoder.
*
* CNAV decoder contains two Viterbi decoders that are used to estimate bit and
* message boundary.
*
* \param[out] dec Decoder structure.
*
* \return None
*/
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);
}
/**
* Adds a received symbol to decoder.
*
* The method feeds the symbol into the decoder. In case there is a sufficient
* information to produce a message, the message is decoded and symbol delay is
* reported.
* The time of the last input symbol can be computed from the message ToW and
* delay by the formulae:
* \code
* symbolTime_ms = msg->tow * 6000 + *pdelay * 20
* \endcode
*
* \param[in,out] dec Decoder object.
* \param[in] symbol Symbol value probability, where 0x00 - 100% of 0,
* 0xFF - 100% of 1.
* \param[out] msg Buffer for decoded message. The message is available
* only when message lock is acquired and CRC is correct.
* \param[out] pdelay Delay of message generation in symbols.
*
* \retval true The message has been decoded. ToW parameter is available.
* \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)
{
_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);
}
return false;
}
/**
* Provides a singleton polynomial object.
*
* The method constructs and returns polynomial object for CNAV message
* decoding. The same polynomial can be used also for other message handling.
*
* The object is initialized on the first call. The method is thread-safe.
*
* @return Pointer to polynomial object for CNAV message decoding.
*/
const v27_poly_t *cnav_msg_decoder_get_poly(void)
{
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 };
/* 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;
}
/** \} */
/** \} */

101
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h Normal file
View File

@@ -0,0 +1,101 @@
/*
* Copyright (C) 2016 Swift Navigation Inc.
* Contact: Valeri Atamaniouk <valeri@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_CNAV_MSG_H
#define LIBSWIFTNAV_CNAV_MSG_H
#include "fec.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <limits.h>
#include "swift_common.h"
/** \addtogroup GPS_L2
* \{ */
/** \addtogroup gps_cnav_decoder
* \{ */
/** Size of the Viterbi decoder history. */
#define GPS_L2_V27_HISTORY_LENGTH_BITS 64
/** Bits to accumulate before decoding starts. */
#define GPS_L2C_V27_INIT_BITS (32)
/** Bits to decode at a time. */
#define GPS_L2C_V27_DECODE_BITS (32)
/** Bits in decoder tail. We ignore them. */
#define GPS_L2C_V27_DELAY_BITS (32)
/**
* GPS CNAV message container.
*
* @sa cnav_msg_decoder_add_symbol
*/
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 */
} cnav_msg_t;
/**
* GPS CNAV decoder component.
* This component controls symbol decoding string.
*
* @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. */
} cnav_v27_part_t;
/**
* GPS CNAV message lock and decoder object.
*
* Decoder uses two Viterbi decoder objects to ensure the lock is acquired when
* the input symbol phase is not known.
*/
typedef struct
{
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);
void cnav_msg_decoder_init(cnav_msg_decoder_t *dec);
bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
unsigned char symbol,
cnav_msg_t *msg,
u32 *delay);
/** \} */
/** \} */
#endif /* LIBSWIFTNAV_CNAV_MSG_H */

0
src/algorithms/telemetry_decoder/libs/convolutional.h → src/algorithms/telemetry_decoder/libs/libswiftcnav/convolutional.h
View File

122
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c Normal file
View File

@@ -0,0 +1,122 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
/** \defgroup edc Error Detection and Correction
* Error detection and correction functions.
* \{ */
/** \defgroup crc CRC
* Cyclic redundancy checks.
* \{ */
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
};
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
*
* The CRC polynomial used is:
* \f[
* x^{24} + x^{23} + x^{18} + x^{17} + x^{14} + x^{11} + x^{10} +
* x^7 + x^6 + x^5 + x^4 + x^3 + x+1
* \f]
* Mask 0x1864CFB, not reversed, not XOR'd
*
* \param buf Array of data to calculate CRC for
* \param len Length of data array
* \param crc Initial CRC value
*
* \return CRC-24Q value
*/
u32 crc24q(const u8 *buf, u32 len, u32 crc)
{
for (u32 i = 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff];
return crc;
}
/**
* Computes CRC-24Q for left-aligned bit message.
* This function is used for left-aligned bit messages, for example SBAS and
* GPS CNAV.
* GPS message is 300 bits total, but 276 bits without CRC. It takes 34.5
* 8-bit bytes, and when computing CRC the message has to be padded with zero
* bits.
*
* \param[in] crc Initial CRC value
* \param[in] buf Pointer to MSB-aligned data.
* \param[in] n_bits Number of bits in the data buffer.
* \param[in] invert Flag to compute inverted CRC.
*
* \return CRC-24Q value
*/
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;
for (u32 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];
return crc;
}
/** \} */
/** \} */

21
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h Normal file
View File

@@ -0,0 +1,21 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_EDC_H
#define LIBSWIFTNAV_EDC_H
#include "swift_common.h"
u32 crc24q(const u8 *buf, u32 len, u32 crc);
u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert);
#endif /* LIBSWIFTNAV_EDC_H */

47
src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h Normal file
View File

@@ -0,0 +1,47 @@
/* User include file for libfec
* Copyright 2004, Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#ifndef _FEC_H_
#define _FEC_H_
/* r=1/2 k=7 convolutional encoder polynomials
* The NASA-DSN convention is to use V27POLYA inverted, then V27POLYB
* The CCSDS/NASA-GSFC convention is to use V27POLYB, then V27POLYA inverted
*/
#define V27POLYA 0x4f
#define V27POLYB 0x6d
typedef struct {
unsigned char c0[32];
unsigned char c1[32];
} v27_poly_t;
typedef struct {
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 */
} v27_t;
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);
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);
void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits);
#endif

64
src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h Normal file
View File

@@ -0,0 +1,64 @@
/*
* Copyright (C) 2012 Swift Navigation Inc.
* Contact: Henry Hallam <henry@swift-nav.com>
* Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#ifndef LIBSWIFTNAV_COMMON_H
#define LIBSWIFTNAV_COMMON_H
/** \defgroup common Common definitions
* Common definitions used throughout the library.
* \{ */
#define ABS(x) ((x) < 0 ? -(x) : (x))
#define MIN(x, y) (((x) < (y)) ? (x) : (y))
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#define CLAMP_DIFF(a,b) (MAX((a),(b)) - (b))
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
#ifndef COMMON_INT_TYPES
#define COMMON_INT_TYPES
/** \defgroup common_inttypes Integer types
* Specified-width integer type definitions for shorter and nicer code.
*
* These should be used in preference to unspecified width types such as
* `int` which can lead to portability issues between different platforms.
* \{ */
/** Signed 8-bit integer. */
typedef int8_t s8;
/** Signed 16-bit integer. */
typedef int16_t s16;
/** Signed 32-bit integer. */
typedef int32_t s32;
/** Signed 64-bit integer. */
typedef int64_t s64;
/** Unsigned 8-bit integer. */
typedef uint8_t u8;
/** Unsigned 16-bit integer. */
typedef uint16_t u16;
/** Unsigned 32-bit integer. */
typedef uint32_t u32;
/** Unsigned 64-bit integer. */
typedef uint64_t u64;
#endif
/** \} */
/** \} */
#endif /* LIBSWIFTNAV_COMMON_H */

216
src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c Normal file
View File

@@ -0,0 +1,216 @@
/* K=7 r=1/2 Viterbi decoder in portable C
* Copyright Feb 2004, Phil Karn, KA9Q
* May be used under the terms of the GNU Lesser General Public License (LGPL)
*/
#include <stdlib.h>
#include "fec.h"
static inline int parity(int x)
{
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.
* \param polynomial Byte array representing the desired polynomials.
*/
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
{
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;
}
}
/** Initialize a v27_t struct for Viterbi decoding.
*
* \param v Structure to initialize
* \param decisions Array of v27_decision_t structs, capacity = decisions_count.
* Must remain valid as long as v is used.
* \param decisions_count Size of decisions array. Equal to the number of bit
* decisions kept in history.
* \param poly Struct describing the polynomials to use. Must remain valid as
* long as v is used. May be shared between multiple decoders.
* \param initial_state Initial state of the decoder shift register. Usually zero.
*/
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;
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;
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);\
}
/** Update a v27_t decoder with a block of symbols.
*
* \param v Structure to update.
* \param syms Array of symbols to use. Must contain two symbols per bit.
* 0xff = strong 1, 0x00 = strong 0.
* \param nbits Number of bits corresponding to the provided symbols.
*/
void v27_update(v27_t *v, const unsigned char *syms, int nbits)
{
unsigned char sym0, sym1;
unsigned int *tmp;
int normalize = 0;
while(nbits--) {
v27_decision_t *d = &v->decisions[v->decisions_index];
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);
/* 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++)
v->new_metrics[i] -= minmetric;
normalize += minmetric;
}
/* 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;
}
}
/** Retrieve the most likely output bit sequence with known final state from
* a v27_t decoder.
*
* \param v Structure to use.
* \param data Array used to store output bits, capacity = nbits.
* \param nbits Number of bits to retrieve.
* \param final_state Known final state of the decoder shift register.
*/
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;
final_state %= 64;
final_state <<= 2;
while(nbits-- != 0) {
/* 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);
}
}
/** Retrieve the most likely output bit sequence with unknown final state from
* a v27_t decoder.
*
* \param v Structure to use.
* \param data Array used to store output bits, capacity = nbits.
* \param nbits Number of bits to retrieve.
*/
void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits)
{
/* 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;
}
}
v27_chainback_fixed(v, data, nbits, best_state);
}