fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-11 07:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into lpc11c24

Browse Source
This commit is contained in:
Pavel Kirienko
2015-08-24 14:21:08 +03:00
parent 16800376af be7f68763e
commit b334dbbfe5
10 changed files with 224 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dsdl
+1 -1
Submodule dsdl updated: fd12483ddd...1abd370556
libuavcan/include/uavcan/build_config.hpp
+15
View File
@@ -100,6 +100,13 @@
# define UAVCAN_TINY 0
#endif
/**
* Disable the global data type registry, which can save some space on embedded systems.
*/
#ifndef UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY
# define UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY 0
#endif
/**
* toString() methods will be disabled by default, unless the library is built for a general-purpose target like Linux.
* It is not recommended to enable toString() on embedded targets as code size will explode.
@@ -136,6 +143,14 @@
# define UAVCAN_USE_EXTERNAL_SNPRINTF 0
#endif
/**
* Allows the user's application to provide a custom implementation of IEEE754Converter::nativeIeeeToHalf and
* IEEE754Converter::halfToNativeIeee.
*/
#ifndef UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
# define UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 0
#endif
/**
* Run time checks.
* Resolves to the standard assert() by default.
libuavcan/include/uavcan/marshal/float_spec.hpp
+20 -5
View File
@@ -36,12 +36,25 @@ struct NativeFloatSelector
class UAVCAN_EXPORT IEEE754Converter
{
// TODO: Non-IEEE float support for float32 and float64
static uint16_t nativeNonIeeeToHalf(float value);
static float halfToNativeNonIeee(uint16_t value);
// TODO: Non-IEEE float support
static uint16_t nativeIeeeToHalf(float value);
static float halfToNativeIeee(uint16_t value);
IEEE754Converter();
template <unsigned BitLen>
static void enforceIeee()
{
/*
* Some compilers may have is_iec559 to be defined false despite the fact that IEEE754 is supported.
* An acceptable workaround would be to put an #ifdef here.
*/
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
StaticAssert<std::numeric_limits<typename NativeFloatSelector<BitLen>::Type>::is_iec559>::check();
#endif
}
public:
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
/// UAVCAN requires rounding to nearest for all float conversions
@@ -52,6 +65,7 @@ public:
static typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType
toIeee(typename NativeFloatSelector<BitLen>::Type value)
{
enforceIeee<BitLen>();
union
{
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType i;
@@ -66,6 +80,7 @@ public:
static typename NativeFloatSelector<BitLen>::Type
toNative(typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType value)
{
enforceIeee<BitLen>();
union
{
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType i;
@@ -80,13 +95,13 @@ template <>
inline typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType
IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value)
{
return nativeNonIeeeToHalf(value);
return nativeIeeeToHalf(value);
}
template <>
inline typename NativeFloatSelector<16>::Type
IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value)
{
return halfToNativeNonIeee(value);
return halfToNativeIeee(value);
}
libuavcan/include/uavcan/node/global_data_type_registry.hpp
+2
View File
@@ -187,6 +187,7 @@ struct UAVCAN_EXPORT DefaultDataTypeRegistrator
{
DefaultDataTypeRegistrator()
{
#if !UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY
const GlobalDataTypeRegistry::RegistrationResult res =
GlobalDataTypeRegistry::instance().registerDataType<Type>(Type::DefaultDataTypeID);
@@ -194,6 +195,7 @@ struct UAVCAN_EXPORT DefaultDataTypeRegistrator
{
handleFatalError("Type reg failed");
}
#endif
}
};
libuavcan/src/marshal/uc_float_spec.cpp
+63 -66
View File
@@ -6,87 +6,84 @@
#include <uavcan/build_config.hpp>
#include <cmath>
#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
# error UAVCAN_CPP_VERSION
#endif
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
# include <limits>
#endif
namespace uavcan
{
#if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
union Fp32
{
uint32_t u;
float f;
};
/*
* IEEE754Converter
* Float16 conversion algorithm: http://half.sourceforge.net/ (MIT License)
*/
uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value)
uint16_t IEEE754Converter::nativeIeeeToHalf(float value)
{
uint16_t hbits = uint16_t(getSignBit(value) ? 0x8000U : 0);
if (areFloatsExactlyEqual(value, 0.0F))
/*
* https://gist.github.com/rygorous/2156668
* Public domain, by Fabian "ryg" Giesen
*/
const Fp32 f32infty = { 255U << 23 };
const Fp32 f16infty = { 31U << 23 };
const Fp32 magic = { 15U << 23 };
const uint32_t sign_mask = 0x80000000U;
const uint32_t round_mask = ~0xFFFU;
Fp32 in;
uint16_t out;
in.f = value;
uint32_t sign = in.u & sign_mask;
in.u ^= sign;
if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */
{
return hbits;
/* NaN->sNaN and Inf->Inf */
out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U;
}
if (isNaN(value))
else /* (De)normalized number or zero */
{
return hbits | 0x7FFFU;
in.u &= round_mask;
in.f *= magic.f;
in.u -= round_mask;
if (in.u > f16infty.u)
{
in.u = f16infty.u; /* Clamp to signed infinity if overflowed */
}
out = uint16_t(in.u >> 13); /* Take the bits! */
}
if (isInfinity(value))
{
return hbits | 0x7C00U;
}
int exp;
(void)std::frexp(value, &exp);
if (exp > 16)
{
return hbits | 0x7C00U;
}
if (exp < -13)
{
value = std::ldexp(value, 24);
}
else
{
value = std::ldexp(value, 11 - exp);
hbits |= uint16_t((exp + 14) << 10);
}
const int32_t ival = static_cast<int32_t>(value);
hbits = uint16_t(hbits | (uint32_t((ival < 0) ? (-ival) : ival) & 0x3FFU));
float diff = std::fabs(value - static_cast<float>(ival));
hbits = uint16_t(hbits + (diff >= 0.5F));
return hbits;
out |= uint16_t(sign >> 16);
return out;
}
float IEEE754Converter::halfToNativeNonIeee(uint16_t value)
float IEEE754Converter::halfToNativeIeee(uint16_t value)
{
float out;
unsigned abs = value & 0x7FFFU;
if (abs > 0x7C00U)
/*
* https://gist.github.com/rygorous/2144712
* Public domain, by Fabian "ryg" Giesen
*/
const Fp32 magic = { (254U - 15U) << 23 };
const Fp32 was_infnan = { (127U + 16U) << 23 };
Fp32 out;
out.u = (value & 0x7FFFU) << 13; /* exponent/mantissa bits */
out.f *= magic.f; /* exponent adjust */
if (out.f >= was_infnan.f) /* make sure Inf/NaN survive */
{
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
out = std::numeric_limits<float>::has_quiet_NaN ? std::numeric_limits<float>::quiet_NaN() : 0.0F;
#else
out = nanf("");
#endif
out.u |= 255U << 23;
}
else if (abs == 0x7C00U)
{
#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
out = std::numeric_limits<float>::has_infinity ?
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
#else
out = NumericTraits<float>::infinity();
#endif
}
else if (abs > 0x3FFU)
{
out = std::ldexp(static_cast<float>((value & 0x3FFU) | 0x400U), int(abs >> 10) - 25);
}
else
{
out = std::ldexp(static_cast<float>(abs), -24);
}
return (value & 0x8000U) ? -out : out;
out.u |= (value & 0x8000U) << 16; /* sign bit */
return out.f;
}
#endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION
}
libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp
+32 -21
View File
@@ -75,7 +75,7 @@ protected:
friend FDCache;
FDCacheItem* next_;
time_t last_access_;
std::time_t last_access_;
const int fd_;
const int oflags_;
const char* const path_;
@@ -101,6 +101,7 @@ protected:
~FDCacheItem()
{
using namespace std;
if (valid())
{
::free(const_cast<char*>(path_));
@@ -117,13 +118,14 @@ protected:
return fd_;
}
time_t getAccess() const
std::time_t getAccess() const
{
return last_access_;
}
time_t acessed()
std::time_t acessed()
{
using namespace std;
last_access_ = time(NULL);
return getAccess();
}
@@ -135,11 +137,13 @@ protected:
bool expired() const
{
using namespace std;
return 0 == last_access_ || (time(NULL) - last_access_) > MaxAgeSeconds;
}
bool equals(const char* path, int oflags) const
{
using namespace std;
return oflags_ == oflags && 0 == ::strcmp(path, path_);
}
@@ -275,7 +279,6 @@ protected:
if (pi && !pi->valid())
{
/* Allocation worked but clone or path failed */
delete pi;
pi = NULL;
}
@@ -286,7 +289,6 @@ protected:
* If allocation fails no harm just can not cache it
* return open fd
*/
return fd;
}
/* add new */
@@ -334,7 +336,7 @@ protected:
* Implementation of this method is required.
* On success the method must return zero.
*/
virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type)
virtual uavcan::int16_t getInfo(const Path& path, uavcan::uint64_t& out_size, EntryType& out_type)
{
int rv = uavcan::protocol::file::Error::INVALID_VALUE;
@@ -376,12 +378,15 @@ protected:
* if the end of file is reached.
* On success the method must return zero.
*/
virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size)
virtual uavcan::int16_t read(const Path& path, const uavcan::uint64_t offset, uavcan::uint8_t* out_buffer,
uavcan::uint16_t& inout_size)
{
int rv = uavcan::protocol::file::Error::INVALID_VALUE;
if (path.size() > 0)
if (path.size() > 0 && inout_size != 0)
{
using namespace std;
FDCacheBase& cache = getFDCache();
int fd = cache.open(path.c_str(), O_RDONLY);
@@ -391,9 +396,9 @@ protected:
}
else
{
rv = ::lseek(fd, offset, SEEK_SET);
ssize_t total_read = 0;
ssize_t len = 0;
rv = ::lseek(fd, offset, SEEK_SET);
if (rv < 0)
{
@@ -401,21 +406,27 @@ protected:
}
else
{
// TODO use a read at offset to fill on EAGAIN
len = ::read(fd, out_buffer, inout_size);
if (len < 0)
rv = 0;
ssize_t remaining = inout_size;
ssize_t nread = 0;
do
{
rv = errno;
}
else
{
rv = 0;
nread = ::read(fd, &out_buffer[total_read], remaining);
if (nread < 0)
{
rv = errno;
}
else
{
remaining -= nread,
total_read += nread;
}
}
while (nread > 0 && remaining > 0);
}
(void)cache.close(fd, rv != 0 || len != inout_size);
inout_size = len;
(void)cache.close(fd, rv != 0 || total_read != inout_size);
inout_size = total_read;
}
}
return rv;
libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp
+16 -5
View File
@@ -51,10 +51,22 @@ protected:
{
const int FormatBufferLength = 63;
char buffer[FormatBufferLength + 1];
int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n",
static_cast<long>(ts.tv_sec), static_cast<long>(ts.tv_nsec / 1000L),
static_cast<int>(code), static_cast<long long>(argument));
(void)write(fd, buffer, n); // TODO FIXME Write loop
ssize_t remaining = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n",
static_cast<long>(ts.tv_sec), static_cast<long>(ts.tv_nsec / 1000L),
static_cast<int>(code), static_cast<long long>(argument));
ssize_t total_written = 0;
ssize_t written = 0;
do
{
written = write(fd, &buffer[total_written], remaining);
if (written > 0)
{
total_written += written;
remaining -= written;
}
}
while (written > 0 && remaining > 0);
(void)close(fd);
}
}
@@ -82,7 +94,6 @@ public:
return rv;
}
};
}
}
libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp
+32 -8
View File
@@ -55,13 +55,25 @@ protected:
{
char buffer[MaxStringLength + 1];
(void)memset(buffer, 0, sizeof(buffer));
int len = read(fd, buffer, MaxStringLength);
(void)close(fd);
if (len > 0)
ssize_t remaining = MaxStringLength;
ssize_t total_read = 0;
ssize_t nread = 0;
do
{
for (int i = 0; i < len; i++)
nread = ::read(fd, &buffer[total_read], remaining);
if (nread > 0)
{
if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r' )
remaining -= nread,
total_read += nread;
}
}
while (nread > 0 && remaining > 0);
(void)close(fd);
if (total_read > 0)
{
for (int i = 0; i < total_read; i++)
{
if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r')
{
buffer[i] = '\0';
break;
@@ -81,7 +93,20 @@ protected:
int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions);
if (fd >= 0)
{
(void)write(fd, value.c_str(), value.size()); // TODO FIXME Write loop
ssize_t remaining = value.size();
ssize_t total_written = 0;
ssize_t written = 0;
do
{
written = write(fd, &value.c_str()[total_written], remaining);
if (written > 0)
{
total_written += written;
remaining -= written;
}
}
while (written > 0 && remaining > 0);
(void)fsync(fd);
(void)close(fd);
}
@@ -117,7 +142,7 @@ public:
// coverity[toctou]
rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
}
if (rv >= 0 )
if (rv >= 0)
{
base_path.push_back('/');
if ((base_path.size() + MaxStringLength) > MaxPathLength)
@@ -129,7 +154,6 @@ public:
return rv;
}
};
}
}
libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp
+28 -12
View File
@@ -86,6 +86,8 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
int copyIfNot(const char* srcpath, const char* destpath)
{
using namespace std;
// Does the file exist
int rv = 0;
int dfd = open(destpath, O_RDONLY, 0);
@@ -113,7 +115,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
}
else
{
ssize_t size;
ssize_t size = 0;
do
{
size = ::read(sfd, buffer, sizeof(buffer));
@@ -125,14 +127,28 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
}
else
{
if (size != write(dfd, buffer, size))
rv = 0;
ssize_t remaining = size;
ssize_t total_written = 0;
ssize_t written = 0;
do
{
rv = -errno;
written = write(dfd, &buffer[total_written], remaining);
if (written < 0)
{
rv = -errno;
}
else
{
total_written += written;
remaining -= written;
}
}
while (written > 0 && remaining > 0);
}
}
}
while (rv == 0 && size);
while (rv == 0 && size != 0);
(void)close(sfd);
}
@@ -144,13 +160,13 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
struct AppDescriptor
{
uint8_t signature[sizeof(uavcan::uint64_t)];
uint64_t image_crc;
uint32_t image_size;
uint32_t vcs_commit;
uint8_t major_version;
uint8_t minor_version;
uint8_t reserved[6];
uavcan::uint8_t signature[sizeof(uavcan::uint64_t)];
uavcan::uint64_t image_crc;
uavcan::uint32_t image_size;
uavcan::uint32_t vcs_commit;
uavcan::uint8_t major_version;
uavcan::uint8_t minor_version;
uavcan::uint8_t reserved[6];
};
static int getFileInfo(const char* path, AppDescriptor& descriptor)
@@ -251,7 +267,7 @@ protected:
node_info.hardware_version.major,
node_info.hardware_version.minor);
if (n > 0 && n < (int) sizeof(fname_root) - 2)
if (n > 0 && n < (int)sizeof(fname_root) - 2)
{
DIR* const fwdir = opendir(fname_root);
libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
+15 -15
View File
@@ -32,22 +32,26 @@ void ledSet(bool state)
palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state);
}
int init()
void init()
{
int res = 0;
halInit();
chibios_rt::System::init();
sdStart(&STDOUT_SD, NULL);
res = can.init(1000000);
if (res < 0)
int res = 0;
do
{
goto leave;
}
::sleep(1);
::lowsyslog("CAN auto bit rate detection...\n");
leave:
return res;
std::uint32_t bitrate = 0;
res = can.init([]() { ::usleep(can.getRecommendedListeningDelay().toUSec()); }, bitrate);
if (res >= 0)
{
::lowsyslog("CAN inited at %u bps\n", unsigned(bitrate));
}
}
while (res < 0);
}
#if __GNUC__
@@ -55,7 +59,7 @@ __attribute__((noreturn))
#endif
void die(int status)
{
lowsyslog("Now I am dead x_x %i\n", status);
lowsyslog("Initialization failure %i\n", status);
while (1)
{
ledSet(false);
@@ -166,11 +170,7 @@ public:
int main()
{
const int init_res = app::init();
if (init_res != 0)
{
app::die(init_res);
}
app::init();
lowsyslog("Starting the UAVCAN thread\n");
app::uavcan_node_thread.start(LOWPRIO);