fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 04:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Reorganized directory structure for tests and src

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-10 19:25:28 +04:00
parent 21fda96978
commit b86ea67563
43 changed files with 26 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/src/marshal/bit_array_copy.c
-119
View File
@@ -1,119 +0,0 @@
/*
* Fast bit array copy algorithm.
* Source: http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays
*/
#include <limits.h>
#include <string.h>
#include <stddef.h>
#define PREPARE_FIRST_COPY() \
do { \
if (src_len >= (CHAR_BIT - dst_offset_modulo)) { \
*dst &= reverse_mask[dst_offset_modulo]; \
src_len -= CHAR_BIT - dst_offset_modulo; \
} else { \
*dst &= reverse_mask[dst_offset_modulo] \
| reverse_mask_xor[dst_offset_modulo + src_len + 1];\
c &= reverse_mask[dst_offset_modulo + src_len ];\
src_len = 0; \
} } while (0)
void
bitarray_copy(const unsigned char *src_org, int src_offset, int src_len,
unsigned char *dst_org, int dst_offset)
{
static const unsigned char reverse_mask[] =
{ 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
static const unsigned char reverse_mask_xor[] =
{ 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
if (src_len > 0) {
const unsigned char *src;
unsigned char *dst;
int src_offset_modulo,
dst_offset_modulo;
src = src_org + (src_offset / CHAR_BIT);
dst = dst_org + (dst_offset / CHAR_BIT);
src_offset_modulo = src_offset % CHAR_BIT;
dst_offset_modulo = dst_offset % CHAR_BIT;
if (src_offset_modulo == dst_offset_modulo) {
int byte_len;
int src_len_modulo;
if (src_offset_modulo) {
unsigned char c;
c = reverse_mask_xor[dst_offset_modulo] & *src++;
PREPARE_FIRST_COPY();
*dst++ |= c;
}
byte_len = src_len / CHAR_BIT;
src_len_modulo = src_len % CHAR_BIT;
if (byte_len) {
memcpy(dst, src, byte_len);
src += byte_len;
dst += byte_len;
}
if (src_len_modulo) {
*dst &= reverse_mask_xor[src_len_modulo];
*dst |= reverse_mask[src_len_modulo] & *src;
}
} else {
int bit_diff_ls,
bit_diff_rs;
int byte_len;
int src_len_modulo;
unsigned char c;
/*
* Begin: Line things up on destination.
*/
if (src_offset_modulo > dst_offset_modulo) {
bit_diff_ls = src_offset_modulo - dst_offset_modulo;
bit_diff_rs = CHAR_BIT - bit_diff_ls;
c = *src++ << bit_diff_ls;
c |= *src >> bit_diff_rs;
c &= reverse_mask_xor[dst_offset_modulo];
} else {
bit_diff_rs = dst_offset_modulo - src_offset_modulo;
bit_diff_ls = CHAR_BIT - bit_diff_rs;
c = *src >> bit_diff_rs &
reverse_mask_xor[dst_offset_modulo];
}
PREPARE_FIRST_COPY();
*dst++ |= c;
/*
* Middle: copy with only shifting the source.
*/
byte_len = src_len / CHAR_BIT;
while (--byte_len >= 0) {
c = *src++ << bit_diff_ls;
c |= *src >> bit_diff_rs;
*dst++ = c;
}
/*
* End: copy the remaing bits;
*/
src_len_modulo = src_len % CHAR_BIT;
if (src_len_modulo) {
c = *src++ << bit_diff_ls;
c |= *src >> bit_diff_rs;
c &= reverse_mask[src_len_modulo];
*dst &= reverse_mask_xor[src_len_modulo];
*dst |= c;
}
}
}
}
libuavcan/src/marshal/bit_stream.cpp
-85
View File
@@ -1,85 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <sstream>
#include <uavcan/internal/marshal/bit_stream.hpp>
namespace uavcan
{
int BitStream::write(const uint8_t* bytes, const int bitlen)
{
// Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_)
uint8_t tmp[MaxBytesPerRW + 1];
// Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write()
const int bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
assert(MaxBytesPerRW >= bytelen);
tmp[0] = tmp[bytelen - 1] = 0;
std::fill(tmp, tmp + bytelen, 0);
copyBitArray(bytes, 0, bitlen, tmp, bit_offset_ % 8);
const int new_bit_offset = bit_offset_ + bitlen;
// Bitcopy algorithm resets skipped bits in the first byte. Restore them back.
tmp[0] |= byte_cache_;
// (new_bit_offset % 8 == 0) means that this write was perfectly aligned.
byte_cache_ = (new_bit_offset % 8) ? tmp[bytelen - 1] : 0;
/*
* Dump the data into the destination buffer.
* Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value
* within the next write() operation.
*/
const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
if (write_res < 0)
return write_res;
if (write_res < bytelen)
return ResultOutOfBuffer;
bit_offset_ = new_bit_offset;
return ResultOk;
}
int BitStream::read(uint8_t* bytes, const int bitlen)
{
uint8_t tmp[MaxBytesPerRW + 1];
const int bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8));
assert(MaxBytesPerRW >= bytelen);
const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen);
if (read_res < 0)
return read_res;
if (read_res < bytelen)
return ResultOutOfBuffer;
std::fill(bytes, bytes + bitlenToBytelen(bitlen), 0);
copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0);
bit_offset_ += bitlen;
return ResultOk;
}
std::string BitStream::toString() const
{
std::ostringstream os;
for (int offset = 0; true; offset++)
{
uint8_t byte = 0;
if (1 != buf_.read(offset, &byte, 1))
break;
for (int i = 7; i >= 0; i--) // Most significant goes first
os << !!(byte & (1 << i));
os << " ";
}
std::string output = os.str();
if (output.length())
output.erase(output.length() - 1, 1);
return output;
}
}
libuavcan/src/marshal/float_spec.cpp
-64
View File
@@ -1,64 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/internal/marshal/float_spec.hpp>
#include <cmath>
namespace uavcan
{
/*
* IEEE754Converter
* Float16 conversion algorithm: http://half.sourceforge.net/ (MIT License)
* TODO: Use conversion tables (conditional compilation - it would require something like 10Kb+ ROM).
*/
template <typename T>
static inline bool signbit(T arg)
{
return arg < T(0) || (arg == T(0) && T(1) / arg < T(0));
}
uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value)
{
uint16_t hbits = signbit(value) << 15;
if (value == 0.0f)
return hbits;
if (isnanf(value))
return hbits | 0x7FFF;
if (isinff(value))
return hbits | 0x7C00;
int exp;
std::frexp(value, &exp);
if (exp > 16)
return hbits | 0x7C00;
if (exp < -13)
value = std::ldexp(value, 24);
else
{
value = std::ldexp(value, 11 - exp);
hbits |= ((exp + 14) << 10);
}
const int ival = static_cast<int>(value);
hbits |= static_cast<uint16_t>(std::abs(ival) & 0x3FF);
float diff = std::abs(value - static_cast<float>(ival));
hbits += diff >= 0.5f;
return hbits;
}
float IEEE754Converter::halfToNativeNonIeee(uint16_t value)
{
float out;
int abs = value & 0x7FFF;
if (abs > 0x7C00)
out = std::numeric_limits<float>::has_quiet_NaN ? std::numeric_limits<float>::quiet_NaN() : 0.0f;
else if (abs == 0x7C00)
out = std::numeric_limits<float>::has_infinity ?
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
else if (abs > 0x3FF)
out = std::ldexp(static_cast<float>((value & 0x3FF) | 0x400), (abs >> 10) - 25);
else
out = std::ldexp(static_cast<float>(abs), -24);
return (value & 0x8000) ? -out : out;
}
}