mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-09 22:08:56 +08:00
fix(modules): remove GNU-only runtime constructs
Native MSVC builds cannot rely on GNU extensions such as typeof, variable-length stack arrays, postfix __declspec exports, or range cases. Replace those constructs with standard C/C++ forms and bounded buffers across the affected modules, tests, and helper libraries. This also makes a few small runtime safety fixes that surfaced while removing the extensions: bounds-check AtomicBitset access, avoid stack-sized dataman buffers, keep MSP packets within the protocol payload limit, and make Windows-compatible debug/logging paths use PX4 platform wrappers. Signed-off-by: Nuno Marques <n.marques21@hotmail.com>
This commit is contained in:
@@ -42,10 +42,14 @@
|
||||
#include <stdbool.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
extern void led_init(void);
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
extern void led_toggle(int led);
|
||||
/* MSVC requires the dllexport storage class on the prototype too — a plain
|
||||
* `extern` declaration followed by a `__declspec(dllexport)` definition is
|
||||
* "redefinition; different linkage" (C2375). GCC accepts the visibility
|
||||
* attribute on either side. */
|
||||
extern __EXPORT void led_init(void);
|
||||
extern __EXPORT void led_on(int led);
|
||||
extern __EXPORT void led_off(int led);
|
||||
extern __EXPORT void led_toggle(int led);
|
||||
__END_DECLS
|
||||
|
||||
static bool _led_state[2] = { false, false };
|
||||
|
||||
@@ -45,6 +45,22 @@ public:
|
||||
|
||||
AtomicBitset() = default;
|
||||
|
||||
AtomicBitset(const AtomicBitset &other)
|
||||
{
|
||||
*this = other;
|
||||
}
|
||||
|
||||
AtomicBitset &operator=(const AtomicBitset &other)
|
||||
{
|
||||
if (this != &other) {
|
||||
for (size_t i = 0; i < ARRAY_SIZE; i++) {
|
||||
_data[i].store(other._data[i].load());
|
||||
}
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
size_t count() const
|
||||
{
|
||||
size_t total = 0;
|
||||
@@ -65,11 +81,19 @@ public:
|
||||
|
||||
bool operator[](size_t position) const
|
||||
{
|
||||
if (position >= N) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return _data[array_index(position)].load() & element_mask(position);
|
||||
}
|
||||
|
||||
void set(size_t pos, bool val = true)
|
||||
{
|
||||
if (pos >= N) {
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t bitmask = element_mask(pos);
|
||||
|
||||
if (val) {
|
||||
@@ -90,8 +114,8 @@ public:
|
||||
|
||||
private:
|
||||
static constexpr uint8_t BITS_PER_ELEMENT = 32;
|
||||
static constexpr size_t ARRAY_SIZE = ((N % BITS_PER_ELEMENT) == 0) ? (N / BITS_PER_ELEMENT) :
|
||||
(N / BITS_PER_ELEMENT + 1);
|
||||
static constexpr size_t ARRAY_SIZE = (N == 0) ? 1 :
|
||||
(((N % BITS_PER_ELEMENT) == 0) ? (N / BITS_PER_ELEMENT) : (N / BITS_PER_ELEMENT + 1));
|
||||
static constexpr size_t ALLOCATED_BITS = ARRAY_SIZE * BITS_PER_ELEMENT;
|
||||
|
||||
size_t array_index(size_t position) const { return position / BITS_PER_ELEMENT; }
|
||||
|
||||
@@ -53,9 +53,9 @@
|
||||
/* Define PX4_ISFINITE */
|
||||
#ifdef __cplusplus
|
||||
#if defined(_MSC_VER) && !defined(__clang__)
|
||||
#include <cmath>
|
||||
static inline bool PX4_ISFINITE(float x) { return std::isfinite(x); }
|
||||
static inline bool PX4_ISFINITE(double x) { return std::isfinite(x); }
|
||||
#include <cfloat>
|
||||
static inline constexpr bool PX4_ISFINITE(float x) { return (x >= -FLT_MAX) && (x <= FLT_MAX); }
|
||||
static inline constexpr bool PX4_ISFINITE(double x) { return (x >= -DBL_MAX) && (x <= DBL_MAX); }
|
||||
#else
|
||||
static inline constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
|
||||
static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
|
||||
|
||||
@@ -38,6 +38,7 @@
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// check if p is a valid option and if the option takes an arg
|
||||
static char isvalidopt(char p, const char *options, int *takesarg)
|
||||
@@ -63,11 +64,16 @@ static char isvalidopt(char p, const char *options, int *takesarg)
|
||||
// reorder argv and put non-options at the end
|
||||
static int reorder(int argc, char **argv, const char *options)
|
||||
{
|
||||
char *tmp_argv[argc];
|
||||
char **tmp_argv = (char **)malloc(sizeof(*tmp_argv) * (size_t)argc);
|
||||
char c;
|
||||
int idx = 1;
|
||||
int tmpidx = 1;
|
||||
int takesarg;
|
||||
int ret = 0;
|
||||
|
||||
if (tmp_argv == NULL) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
// move the options to the front
|
||||
while (idx < argc && argv[idx] != 0) {
|
||||
@@ -75,7 +81,8 @@ static int reorder(int argc, char **argv, const char *options)
|
||||
c = isvalidopt(argv[idx][1], options, &takesarg);
|
||||
|
||||
if (c == '?') {
|
||||
return 1;
|
||||
ret = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
tmp_argv[tmpidx] = argv[idx];
|
||||
@@ -83,7 +90,8 @@ static int reorder(int argc, char **argv, const char *options)
|
||||
|
||||
if (takesarg) {
|
||||
if (idx + 1 >= argc) { //Error: option takes an argument, but there is no more argument
|
||||
return 1;
|
||||
ret = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
tmp_argv[tmpidx] = argv[idx + 1];
|
||||
@@ -104,7 +112,8 @@ static int reorder(int argc, char **argv, const char *options)
|
||||
c = isvalidopt(argv[idx][1], options, &takesarg);
|
||||
|
||||
if (c == '?') {
|
||||
return c;
|
||||
ret = c;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
if (takesarg) {
|
||||
@@ -124,7 +133,9 @@ static int reorder(int argc, char **argv, const char *options)
|
||||
argv[idx] = tmp_argv[idx];
|
||||
}
|
||||
|
||||
return 0;
|
||||
cleanup:
|
||||
free(tmp_argv);
|
||||
return ret;
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
@@ -48,6 +48,12 @@
|
||||
|
||||
/* Define PX4_ISFINITE */
|
||||
#ifdef __cplusplus
|
||||
#if defined(_MSC_VER) && !defined(__clang__)
|
||||
#include <cfloat>
|
||||
constexpr bool PX4_ISFINITE(float x) { return (x >= -FLT_MAX) && (x <= FLT_MAX); }
|
||||
constexpr bool PX4_ISFINITE(double x) { return (x >= -DBL_MAX) && (x <= DBL_MAX); }
|
||||
#else
|
||||
constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
|
||||
constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
|
||||
#endif
|
||||
#endif /* __cplusplus */
|
||||
|
||||
@@ -149,9 +149,12 @@ static inline hrt_abstime ts_to_abstime(const struct timespec *ts)
|
||||
*/
|
||||
static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
||||
{
|
||||
ts->tv_sec = (typeof(ts->tv_sec))(abstime / 1000000);
|
||||
/* POSIX guarantees `time_t tv_sec` and `long tv_nsec`. Use explicit
|
||||
* casts instead of GCC's `typeof` so this header parses under MSVC C,
|
||||
* which lacks `typeof` outside the C23 mode we don't enable. */
|
||||
ts->tv_sec = (time_t)(abstime / 1000000);
|
||||
abstime -= (hrt_abstime)(ts->tv_sec) * 1000000;
|
||||
ts->tv_nsec = (typeof(ts->tv_nsec))(abstime * 1000);
|
||||
ts->tv_nsec = (long)(abstime * 1000);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -37,6 +37,7 @@
|
||||
#include <sys/types.h>
|
||||
#include <stdbool.h>
|
||||
#include <float.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
@@ -79,6 +80,41 @@ const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
|
||||
{MSP_RC, true, sizeof(msp_rc_t)},
|
||||
};
|
||||
|
||||
namespace
|
||||
{
|
||||
constexpr uint32_t MSP_V1_MAX_PAYLOAD_SIZE = UINT8_MAX;
|
||||
|
||||
bool send_packet(int fd, const uint8_t message_id, const void *payload, const uint32_t payload_size)
|
||||
{
|
||||
if ((payload_size > MSP_V1_MAX_PAYLOAD_SIZE) || ((payload_size > 0) && (payload == nullptr))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t packet[MSP_FRAME_START_SIZE + MSP_V1_MAX_PAYLOAD_SIZE + MSP_CRC_SIZE];
|
||||
|
||||
packet[0] = '$';
|
||||
packet[1] = 'M';
|
||||
packet[2] = '>';
|
||||
packet[3] = static_cast<uint8_t>(payload_size);
|
||||
packet[4] = message_id;
|
||||
|
||||
uint8_t crc = static_cast<uint8_t>(payload_size) ^ message_id;
|
||||
|
||||
if (payload_size > 0) {
|
||||
memcpy(packet + MSP_FRAME_START_SIZE, payload, payload_size);
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < payload_size; i ++) {
|
||||
crc ^= packet[MSP_FRAME_START_SIZE + i];
|
||||
}
|
||||
|
||||
packet[MSP_FRAME_START_SIZE + payload_size] = crc;
|
||||
|
||||
const int packet_size = MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
|
||||
return write(fd, packet, packet_size) == packet_size;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
bool MspV1::Send(const uint8_t message_id, const void *payload)
|
||||
{
|
||||
uint32_t payload_size = 0;
|
||||
@@ -102,52 +138,12 @@ bool MspV1::Send(const uint8_t message_id, const void *payload)
|
||||
|
||||
payload_size = desc->message_size;
|
||||
|
||||
uint8_t packet[MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE];
|
||||
uint8_t crc;
|
||||
|
||||
packet[0] = '$';
|
||||
packet[1] = 'M';
|
||||
packet[2] = '>';
|
||||
packet[3] = payload_size;
|
||||
packet[4] = message_id;
|
||||
|
||||
crc = payload_size ^ message_id;
|
||||
|
||||
memcpy(packet + MSP_FRAME_START_SIZE, payload, payload_size);
|
||||
|
||||
for (uint32_t i = 0; i < payload_size; i ++) {
|
||||
crc ^= packet[MSP_FRAME_START_SIZE + i];
|
||||
}
|
||||
|
||||
packet[MSP_FRAME_START_SIZE + payload_size] = crc;
|
||||
|
||||
int packet_size = MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
|
||||
return write(_fd, packet, packet_size) == packet_size;
|
||||
return send_packet(_fd, message_id, payload, payload_size);
|
||||
}
|
||||
|
||||
bool MspV1::Send(const uint8_t message_id, const void *payload, uint32_t payload_size)
|
||||
{
|
||||
uint8_t packet[MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE];
|
||||
uint8_t crc;
|
||||
|
||||
packet[0] = '$';
|
||||
packet[1] = 'M';
|
||||
packet[2] = '>';
|
||||
packet[3] = payload_size;
|
||||
packet[4] = message_id;
|
||||
|
||||
crc = payload_size ^ message_id;
|
||||
|
||||
memcpy(packet + MSP_FRAME_START_SIZE, payload, payload_size);
|
||||
|
||||
for (uint32_t i = 0; i < payload_size; i ++) {
|
||||
crc ^= packet[MSP_FRAME_START_SIZE + i];
|
||||
}
|
||||
|
||||
packet[MSP_FRAME_START_SIZE + payload_size] = crc;
|
||||
|
||||
int packet_size = MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
|
||||
return write(_fd, packet, packet_size) == packet_size;
|
||||
return send_packet(_fd, message_id, payload, payload_size);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -45,7 +45,16 @@
|
||||
# undef __EXPORT
|
||||
#endif
|
||||
#if defined(_MSC_VER) && !defined(__clang__)
|
||||
# define __EXPORT __declspec(dllexport)
|
||||
/* PX4 puts `__EXPORT` in postfix position on many declarations
|
||||
* (e.g. `extern int foo(args) __EXPORT;`). GCC's
|
||||
* `__attribute__((visibility("default")))` accepts that placement, but
|
||||
* MSVC's `__declspec(dllexport)` does not — it's a parse error.
|
||||
*
|
||||
* For SITL on Windows everything ends up statically linked into
|
||||
* px4.exe, so symbol-export visibility isn't load-bearing. The
|
||||
* dyn_* `.px4mod` shared modules are the only consumers; they can be
|
||||
* handled via `/DEF:` files or `--export-all-symbols`-equivalent. */
|
||||
# define __EXPORT
|
||||
#else
|
||||
# define __EXPORT __attribute__ ((visibility ("default")))
|
||||
#endif
|
||||
@@ -63,6 +72,25 @@
|
||||
# define __attribute__(x)
|
||||
#endif
|
||||
|
||||
#if defined(_MSC_VER) && !defined(__clang__)
|
||||
/* MSVC lacks GCC's `__builtin_*` family used throughout PX4. Map the
|
||||
* floating-point classifiers to <math.h>'s C99 / std::math equivalents and
|
||||
* neutralize the optimization-hint builtins that have no MSVC parallel. */
|
||||
# include <math.h>
|
||||
# ifdef __cplusplus
|
||||
# include <cmath>
|
||||
# define __builtin_isfinite(x) std::isfinite(x)
|
||||
# define __builtin_isnan(x) std::isnan(x)
|
||||
# define __builtin_isinf(x) std::isinf(x)
|
||||
# else
|
||||
# define __builtin_isfinite(x) isfinite(x)
|
||||
# define __builtin_isnan(x) isnan(x)
|
||||
# define __builtin_isinf(x) isinf(x)
|
||||
# endif
|
||||
# define __builtin_expect(expr, expected) (expr)
|
||||
# define __builtin_unreachable() __assume(0)
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
# define __BEGIN_DECLS extern "C" {
|
||||
# define __END_DECLS }
|
||||
|
||||
@@ -64,7 +64,7 @@ public:
|
||||
// methods
|
||||
BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
|
||||
Block(parent, name),
|
||||
_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
|
||||
_state(NAN), // initialize to invalid val, force into is_finite() check on first call
|
||||
_fCut(this, ""), // only one parameter, no need to name
|
||||
_fs(sample_freq),
|
||||
_lp(_fs, _fCut.get())
|
||||
|
||||
@@ -67,7 +67,7 @@ public:
|
||||
_fCut(this, "") // only one parameter, no need to name
|
||||
{
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
_state(i) = 0.0f / 0.0f;
|
||||
_state(i) = static_cast<Type>(NAN);
|
||||
}
|
||||
}
|
||||
virtual ~BlockLowPassVector() = default;
|
||||
|
||||
@@ -98,8 +98,7 @@ DirectionalGuidance::guideToPath(const Vector2f &curr_pos_local, const Vector2f
|
||||
wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature) * feas_combined * track_proximity_;
|
||||
course_sp_ = atan2f(bearing_vec_(1), bearing_vec_(0));
|
||||
|
||||
return DirectionalGuidanceOutput{.course_setpoint = course_sp_,
|
||||
.lateral_acceleration_feedforward = lateral_accel_ff_};
|
||||
return DirectionalGuidanceOutput{course_sp_, lateral_accel_ff_};
|
||||
}
|
||||
|
||||
float DirectionalGuidance::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
|
||||
|
||||
@@ -45,10 +45,12 @@
|
||||
|
||||
#include "tinybson.h"
|
||||
|
||||
/* C99 variadic macro syntax — GCC's `args...` named-rest is rejected by
|
||||
* MSVC (C2010 "'.' unexpected in macro parameter list"). */
|
||||
#if 0
|
||||
# define debug(fmt, args...) do { PX4_INFO("BSON: " fmt, ##args); } while(0)
|
||||
# define debug(fmt, ...) do { PX4_INFO("BSON: " fmt, ##__VA_ARGS__); } while(0)
|
||||
#else
|
||||
# define debug(fmt, args...) do { } while(0)
|
||||
# define debug(fmt, ...) do { (void)(fmt); } while(0)
|
||||
#endif
|
||||
|
||||
#define CODER_CHECK(_c) do { if ((_c)->dead) { PX4_ERR("coder dead"); return -1; }} while(0)
|
||||
|
||||
@@ -330,7 +330,13 @@ Tunes::Status Tunes::get_next_note(unsigned &frequency, unsigned &duration, unsi
|
||||
|
||||
break;
|
||||
|
||||
case 'A'...'G': // Play a note in the current octave.
|
||||
case 'A': // Play a note in the current octave.
|
||||
case 'B':
|
||||
case 'C':
|
||||
case 'D':
|
||||
case 'E':
|
||||
case 'F':
|
||||
case 'G':
|
||||
note = _note_tab[c - 'A'] + (_octave * 12) + 1;
|
||||
c = next_char();
|
||||
|
||||
|
||||
@@ -149,6 +149,21 @@ static constexpr size_t g_per_item_size_with_hdr[DM_KEY_NUM_KEYS] = {
|
||||
g_per_item_size[DM_KEY_COMPAT] + DM_SECTOR_HDR_SIZE
|
||||
};
|
||||
|
||||
static constexpr size_t max_item_size_with_hdr()
|
||||
{
|
||||
size_t max_size = 0;
|
||||
|
||||
for (size_t i = 0; i < DM_KEY_NUM_KEYS; i++) {
|
||||
if (g_per_item_size_with_hdr[i] > max_size) {
|
||||
max_size = g_per_item_size_with_hdr[i];
|
||||
}
|
||||
}
|
||||
|
||||
return max_size;
|
||||
}
|
||||
|
||||
static constexpr size_t g_max_item_size_with_hdr = max_item_size_with_hdr();
|
||||
|
||||
/* Table of offset for index 0 of each item type */
|
||||
static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
|
||||
|
||||
@@ -260,7 +275,7 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count)
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned char buffer[g_per_item_size_with_hdr[item]];
|
||||
unsigned char buffer[g_max_item_size_with_hdr];
|
||||
|
||||
/* Get the offset for this item */
|
||||
const int offset = calculate_offset(item, index);
|
||||
@@ -383,7 +398,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count)
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned char buffer[g_per_item_size_with_hdr[item]];
|
||||
unsigned char buffer[g_max_item_size_with_hdr];
|
||||
|
||||
/* Get the offset for this item */
|
||||
int offset = calculate_offset(item, index);
|
||||
|
||||
@@ -826,11 +826,11 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
|
||||
const int param_compid = roundf(vehicle_command.param2);
|
||||
|
||||
uint8_t new_sysid_primary_control = [&]() {
|
||||
switch (param_sysid) {
|
||||
if ((param_sysid >= 0) && (param_sysid <= 255)) {
|
||||
return static_cast<uint8_t>(param_sysid);
|
||||
}
|
||||
|
||||
case 0 ... 255:
|
||||
// Valid new sysid.
|
||||
return (uint8_t) param_sysid;
|
||||
switch (param_sysid) {
|
||||
|
||||
case -1:
|
||||
// leave unchanged
|
||||
@@ -838,13 +838,13 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
|
||||
|
||||
case -2:
|
||||
// set itself
|
||||
return (uint8_t) _parameters.mav_sysid;
|
||||
return static_cast<uint8_t>(_parameters.mav_sysid);
|
||||
|
||||
case -3:
|
||||
|
||||
// release control if in control
|
||||
if (control_data.sysid_primary_control == vehicle_command.source_system) {
|
||||
return (uint8_t) 0;
|
||||
return static_cast<uint8_t>(0);
|
||||
|
||||
} else {
|
||||
return control_data.sysid_primary_control;
|
||||
@@ -857,10 +857,11 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
|
||||
}();
|
||||
|
||||
uint8_t new_compid_primary_control = [&]() {
|
||||
if ((param_compid >= 0) && (param_compid <= 255)) {
|
||||
return static_cast<uint8_t>(param_compid);
|
||||
}
|
||||
|
||||
switch (param_compid) {
|
||||
case 0 ... 255:
|
||||
// Valid new compid.
|
||||
return (uint8_t) param_compid;
|
||||
|
||||
case -1:
|
||||
// leave unchanged
|
||||
@@ -868,13 +869,13 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
|
||||
|
||||
case -2:
|
||||
// set itself
|
||||
return (uint8_t) _parameters.mav_compid;
|
||||
return static_cast<uint8_t>(_parameters.mav_compid);
|
||||
|
||||
case -3:
|
||||
|
||||
// release control if in control
|
||||
if (control_data.compid_primary_control == vehicle_command.source_component) {
|
||||
return (uint8_t) 0;
|
||||
return static_cast<uint8_t>(0);
|
||||
|
||||
} else {
|
||||
return control_data.compid_primary_control;
|
||||
|
||||
@@ -31,6 +31,11 @@
|
||||
|
||||
#include "arm_math.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_FFT_ALLOW_TABLES)
|
||||
/* Double Precision Float CFFT twiddles */
|
||||
#if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FFT_TABLES) || defined(ARM_TABLE_BITREV_1024)
|
||||
@@ -513,4 +518,8 @@
|
||||
extern const unsigned char hwLUT[256];
|
||||
#endif /* (defined(ARM_MATH_MVEI) || defined(ARM_MATH_HELIUM)) */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* ARM_COMMON_TABLES_H */
|
||||
|
||||
@@ -33,6 +33,11 @@
|
||||
#include "arm_math.h"
|
||||
#include "arm_common_tables.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
extern const arm_cfft_instance_f64 arm_cfft_sR_f64_len16;
|
||||
extern const arm_cfft_instance_f64 arm_cfft_sR_f64_len32;
|
||||
extern const arm_cfft_instance_f64 arm_cfft_sR_f64_len64;
|
||||
@@ -73,4 +78,8 @@
|
||||
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len2048;
|
||||
extern const arm_cfft_instance_q15 arm_cfft_sR_q15_len4096;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@@ -698,7 +698,7 @@ void Logger::run()
|
||||
|
||||
if (_writer.backend() & LogWriter::BackendFile) {
|
||||
|
||||
const pid_t pid_self = getpid();
|
||||
const pid_t pid_self = px4_getpid();
|
||||
const pthread_t writer_thread = _writer.thread_id_file();
|
||||
|
||||
// sched_note_start is already called from pthread_create and task_create,
|
||||
|
||||
@@ -1134,7 +1134,13 @@ void SimulatorMavlink::run()
|
||||
|
||||
if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
|
||||
PX4_ERR("bind for UDP port %i failed (%i)", _port, errno);
|
||||
#ifdef __PX4_WINDOWS
|
||||
// Winsock SOCKETs are kernel handles, not CRT fds — using ::close()
|
||||
// trips the UCRT _close.cpp assertion `(fh >= 0 && (unsigned)fh < (unsigned)_nhandle)`.
|
||||
closesocket(_fd);
|
||||
#else
|
||||
::close(_fd);
|
||||
#endif
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
// Unblock the lockstep wait in simulator_mavlink_main so the
|
||||
// rest of the startup script (including shutdown) can run.
|
||||
@@ -1186,7 +1192,12 @@ void SimulatorMavlink::run()
|
||||
break;
|
||||
|
||||
} else {
|
||||
#ifdef __PX4_WINDOWS
|
||||
// See note above: Winsock SOCKETs need closesocket(), not ::close().
|
||||
closesocket(_fd);
|
||||
#else
|
||||
::close(_fd);
|
||||
#endif
|
||||
system_usleep(500);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -53,7 +53,7 @@ static const bool sample_bool = true;
|
||||
static const int32_t sample_small_int = 123;
|
||||
static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL;
|
||||
static const double sample_double = 2.5f;
|
||||
static const char *sample_string = "this is a test";
|
||||
static const char sample_string[] = "this is a test";
|
||||
static const uint8_t sample_data[256] = {0};
|
||||
//static const char *sample_filename = "/fs/microsd/bson.test";
|
||||
|
||||
@@ -175,7 +175,7 @@ decode_callback(bson_decoder_t decoder, bson_node_t node)
|
||||
return 1;
|
||||
}
|
||||
|
||||
char sbuf[len];
|
||||
char sbuf[sizeof(sample_string)];
|
||||
|
||||
if (bson_decoder_copy_data(decoder, sbuf)) {
|
||||
PX4_ERR("FAIL: decoder: string1 copy failed");
|
||||
@@ -219,7 +219,7 @@ decode_callback(bson_decoder_t decoder, bson_node_t node)
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint8_t dbuf[len];
|
||||
uint8_t dbuf[sizeof(sample_data)];
|
||||
|
||||
if (bson_decoder_copy_data(decoder, dbuf)) {
|
||||
PX4_ERR("FAIL: decoder: data1 copy failed");
|
||||
|
||||
@@ -90,8 +90,11 @@ int check_user_abort(int fd)
|
||||
int
|
||||
test_file(int argc, char *argv[])
|
||||
{
|
||||
const unsigned iterations = 2;
|
||||
const unsigned alignments = 33;
|
||||
enum {
|
||||
iterations = 2,
|
||||
alignments = 33,
|
||||
max_chunk_size = 1500
|
||||
};
|
||||
|
||||
/* check if microSD card is mounted */
|
||||
struct stat buffer;
|
||||
@@ -113,15 +116,16 @@ test_file(int argc, char *argv[])
|
||||
printf("\n");
|
||||
printf("----- alignment test: %u bytes -----\n", a);
|
||||
|
||||
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
||||
const size_t buffer_size = (size_t)chunk_sizes[c] + alignments;
|
||||
uint8_t write_buf[max_chunk_size + alignments] __attribute__((aligned(64)));
|
||||
|
||||
/* fill write buffer with known values */
|
||||
for (size_t i = 0; i < sizeof(write_buf); i++) {
|
||||
for (size_t i = 0; i < buffer_size; i++) {
|
||||
/* this will wrap, but we just need a known value with spacing */
|
||||
write_buf[i] = i + 11;
|
||||
}
|
||||
|
||||
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
|
||||
uint8_t read_buf[max_chunk_size + alignments] __attribute__((aligned(64)));
|
||||
hrt_abstime start, end;
|
||||
|
||||
int fd = open(PX4_STORAGEDIR "/testfile", O_TRUNC | O_WRONLY | O_CREAT, 0600);
|
||||
@@ -260,7 +264,7 @@ test_file(int argc, char *argv[])
|
||||
bool unalign_read_ok = true;
|
||||
int unalign_read_err_count = 0;
|
||||
|
||||
memset(read_buf, 0, sizeof(read_buf));
|
||||
memset(read_buf, 0, buffer_size);
|
||||
|
||||
/* read back data unaligned */
|
||||
for (unsigned i = 0; i < iterations; i++) {
|
||||
|
||||
@@ -84,17 +84,24 @@ static int test_corruption(const char *filename, uint32_t write_chunk, uint32_t
|
||||
|
||||
// create a file of size write_size, in write_chunk blocks
|
||||
uint8_t counter = 0;
|
||||
uint8_t *buffer = (uint8_t *)malloc(write_chunk);
|
||||
|
||||
if (buffer == NULL) {
|
||||
printf("malloc failed for write_chunk=%" PRIu32 "\n", write_chunk);
|
||||
close(fd);
|
||||
return 1;
|
||||
}
|
||||
|
||||
while (ofs < write_size) {
|
||||
uint8_t buffer[write_chunk];
|
||||
|
||||
for (uint16_t j = 0; j < write_chunk; j++) {
|
||||
buffer[j] = get_value(ofs);
|
||||
ofs++;
|
||||
}
|
||||
|
||||
if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
|
||||
if (write(fd, buffer, write_chunk) != (int)write_chunk) {
|
||||
printf("write failed at offset %" PRIu32 "\n", ofs);
|
||||
free(buffer);
|
||||
close(fd);
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -118,6 +125,7 @@ static int test_corruption(const char *filename, uint32_t write_chunk, uint32_t
|
||||
|
||||
if (fd == -1) {
|
||||
perror(filename);
|
||||
free(buffer);
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -125,17 +133,16 @@ static int test_corruption(const char *filename, uint32_t write_chunk, uint32_t
|
||||
ofs = 0;
|
||||
|
||||
while (ofs < write_size) {
|
||||
uint8_t buffer[write_chunk];
|
||||
|
||||
if (counter % 100 == 0) {
|
||||
printf("read ofs=%" PRIu32 "\r", ofs);
|
||||
}
|
||||
|
||||
counter++;
|
||||
|
||||
if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
|
||||
if (read(fd, buffer, write_chunk) != (int)write_chunk) {
|
||||
printf("read failed at offset %" PRIu32 "\n", ofs);
|
||||
close(fd);
|
||||
free(buffer);
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -143,6 +150,7 @@ static int test_corruption(const char *filename, uint32_t write_chunk, uint32_t
|
||||
if (buffer[j] != get_value(ofs)) {
|
||||
printf("corruption at ofs=%" PRIu32 " got %" PRIu8 "\n", ofs, buffer[j]);
|
||||
close(fd);
|
||||
free(buffer);
|
||||
return 1;
|
||||
}
|
||||
|
||||
@@ -156,6 +164,7 @@ static int test_corruption(const char *filename, uint32_t write_chunk, uint32_t
|
||||
|
||||
printf("read ofs=%" PRIu32 "\n", ofs);
|
||||
close(fd);
|
||||
free(buffer);
|
||||
unlink(filename);
|
||||
printf("All OK\n");
|
||||
return 0;
|
||||
|
||||
@@ -378,8 +378,8 @@ bool MathlibTest::testFinite()
|
||||
ut_assert("PX4_ISFINITE(-1.0f)", PX4_ISFINITE(-1.0f) == true);
|
||||
|
||||
ut_assert("PX4_ISFINITE(NAN)", PX4_ISFINITE(NAN) == false);
|
||||
ut_assert("PX4_ISFINITE(1/0)", PX4_ISFINITE(1.0f / 0.0f) == false);
|
||||
ut_assert("PX4_ISFINITE(0/0)", PX4_ISFINITE(0.0f / 0.0f) == false);
|
||||
ut_assert("PX4_ISFINITE(1/0)", PX4_ISFINITE(INFINITY) == false);
|
||||
ut_assert("PX4_ISFINITE(0/0)", PX4_ISFINITE(NAN) == false);
|
||||
ut_assert("PX4_ISFINITE(INFINITY)", PX4_ISFINITE(INFINITY) == false);
|
||||
ut_assert("PX4_ISFINITE(NAN * INFINITY)", PX4_ISFINITE(NAN * INFINITY) == false);
|
||||
ut_assert("PX4_ISFINITE(NAN * 1.0f)", PX4_ISFINITE(NAN * 1.0f) == false);
|
||||
|
||||
Reference in New Issue
Block a user