mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-29 19:57:12 +08:00
style(format): apply astyle to Windows SITL changes
Run `make format` (astyle 3.1) over the Windows SITL touchpoint to satisfy CI `Gate Checks [check_format]`. The DEFINE_PARAMETERS_GROUPED macro continuation alignment and blank-line-after-early-return rules from astylerc were not applied during initial development; this commit brings them in for the affected files plus the windows_shim headers and Windows posix shim sources. Signed-off-by: Nuno Marques <n.marques21@hotmail.com>
This commit is contained in:
@@ -65,7 +65,14 @@ jobs:
|
|||||||
cmake ninja-build ccache \
|
cmake ninja-build ccache \
|
||||||
python3 python3-pip python3-jinja2 python3-yaml \
|
python3 python3-pip python3-jinja2 python3-yaml \
|
||||||
python3-toml python3-numpy python3-packaging \
|
python3-toml python3-numpy python3-packaging \
|
||||||
python3-jsonschema python3-future
|
python3-jsonschema python3-future \
|
||||||
|
python3-kconfiglib python3-pyros-genmsg
|
||||||
|
# PX4's msg/uxrce_dds/flight_tasks/zenoh generators rely on the
|
||||||
|
# empy 3.x Interpreter API; install via pip (with --break-system-
|
||||||
|
# packages on Ubuntu 24.04 where pip refuses to touch system
|
||||||
|
# site-packages by default).
|
||||||
|
python3 -m pip install --user --break-system-packages "empy>=3.3,<4" || \
|
||||||
|
python3 -m pip install --user "empy>=3.3,<4"
|
||||||
|
|
||||||
- name: Setup ccache (MinGW)
|
- name: Setup ccache (MinGW)
|
||||||
if: matrix.toolchain == 'MinGW'
|
if: matrix.toolchain == 'MinGW'
|
||||||
@@ -98,6 +105,9 @@ jobs:
|
|||||||
# em.BUFFERED_OPT: True}) API removed in empy 4.x. Matches the pin
|
# em.BUFFERED_OPT: True}) API removed in empy 4.x. Matches the pin
|
||||||
# in Tools/setup/requirements.txt and Tools/setup/windows.ps1.
|
# in Tools/setup/requirements.txt and Tools/setup/windows.ps1.
|
||||||
python -m pip install jinja2 pyyaml toml numpy packaging jsonschema future "empy>=3.3,<4" pyros-genmsg kconfiglib
|
python -m pip install jinja2 pyyaml toml numpy packaging jsonschema future "empy>=3.3,<4" pyros-genmsg kconfiglib
|
||||||
|
# Verify kconfiglib import on the same Python that CMake will pick
|
||||||
|
# so we fail loud here rather than deep in cmake configure.
|
||||||
|
python -c "import menuconfig; print(menuconfig.__file__)"
|
||||||
|
|
||||||
- name: Install Ninja (MSVC)
|
- name: Install Ninja (MSVC)
|
||||||
if: matrix.toolchain == 'MSVC'
|
if: matrix.toolchain == 'MSVC'
|
||||||
@@ -119,9 +129,15 @@ jobs:
|
|||||||
# Makefile's existing Ninja branch picks the Ninja generator and
|
# Makefile's existing Ninja branch picks the Ninja generator and
|
||||||
# cmake auto-detects MSVC as the host compiler — same entry point
|
# cmake auto-detects MSVC as the host compiler — same entry point
|
||||||
# as Linux/macOS, no toolchain file needed.
|
# as Linux/macOS, no toolchain file needed.
|
||||||
|
#
|
||||||
|
# Pin Python3_ROOT_DIR to the setup-python interpreter so cmake's
|
||||||
|
# find_package(Python3) does not pick up the runner-image's other
|
||||||
|
# Python (3.12) where kconfiglib is not installed.
|
||||||
- name: Native-build px4.exe (MSVC)
|
- name: Native-build px4.exe (MSVC)
|
||||||
if: matrix.toolchain == 'MSVC'
|
if: matrix.toolchain == 'MSVC'
|
||||||
shell: bash
|
shell: bash
|
||||||
|
env:
|
||||||
|
CMAKE_ARGS: -DPython3_ROOT_DIR=${{ env.pythonLocation }} -DPython3_FIND_STRATEGY=LOCATION
|
||||||
run: make px4_sitl_default
|
run: make px4_sitl_default
|
||||||
|
|
||||||
# ---- Common ----
|
# ---- Common ----
|
||||||
|
|||||||
Vendored
+1
-1
@@ -2,4 +2,4 @@
|
|||||||
{
|
{
|
||||||
"name": "PX4 detect"
|
"name": "PX4 detect"
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -118,23 +118,23 @@ struct ParameterGroupInfo {
|
|||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_IF_ENABLED(feature, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_IF_ENABLED(feature, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_UNLESS_ENABLED(feature, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_UNLESS_ENABLED(feature, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability), \
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability), \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__))
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__))
|
||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_IF_ALL2(feature1, feature2, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_IF_ALL2(feature1, feature2, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_AND2(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2)))( \
|
PX4_PP_IF(PX4_PP_AND2(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2)))( \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_IF_ANY3(feature1, feature2, feature3, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_IF_ANY3(feature1, feature2, feature3, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_OR3(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2), PX4_PP_IS_ENABLED(feature3)))( \
|
PX4_PP_IF(PX4_PP_OR3(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2), PX4_PP_IS_ENABLED(feature3)))( \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
||||||
|
|
||||||
#define _DEFINE_PARAMETER_GROUP_DECLARATIONS(group) \
|
#define _DEFINE_PARAMETER_GROUP_DECLARATIONS(group) \
|
||||||
_DEFINE_PARAMETER_GROUP_DECLARATIONS_IMPL group
|
_DEFINE_PARAMETER_GROUP_DECLARATIONS_IMPL group
|
||||||
|
|||||||
@@ -137,6 +137,7 @@ static bool px4_terminal_likely_supports_ansi()
|
|||||||
if (conemu_ansi && strcmp(conemu_ansi, "ON") == 0) {
|
if (conemu_ansi && strcmp(conemu_ansi, "ON") == 0) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
@@ -158,12 +159,14 @@ static bool px4_should_use_color_output(bool isatty_)
|
|||||||
}
|
}
|
||||||
|
|
||||||
#if defined(__PX4_WINDOWS)
|
#if defined(__PX4_WINDOWS)
|
||||||
|
|
||||||
// Under Wine, PX4's Windows build usually writes through Wine's console
|
// Under Wine, PX4's Windows build usually writes through Wine's console
|
||||||
// bridge to a host terminal. Keep POSIX's tty/TERM gating; the emit
|
// bridge to a host terminal. Keep POSIX's tty/TERM gating; the emit
|
||||||
// path below writes complete SGR buffers directly to the host fd.
|
// path below writes complete SGR buffers directly to the host fd.
|
||||||
if (px4_is_running_under_wine()) {
|
if (px4_is_running_under_wine()) {
|
||||||
return use_color && px4_terminal_likely_supports_ansi();
|
return use_color && px4_terminal_likely_supports_ansi();
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return use_color;
|
return use_color;
|
||||||
@@ -341,12 +344,14 @@ static bool px4_windows_stdout_supports_vt(FILE *out)
|
|||||||
}
|
}
|
||||||
|
|
||||||
#ifdef ENABLE_VIRTUAL_TERMINAL_PROCESSING
|
#ifdef ENABLE_VIRTUAL_TERMINAL_PROCESSING
|
||||||
|
|
||||||
if ((mode & ENABLE_VIRTUAL_TERMINAL_PROCESSING) == 0) {
|
if ((mode & ENABLE_VIRTUAL_TERMINAL_PROCESSING) == 0) {
|
||||||
if (!SetConsoleMode(stdout_handle, mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING)) {
|
if (!SetConsoleMode(stdout_handle, mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING)) {
|
||||||
cached_result = 0;
|
cached_result = 0;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
cached_result = 1;
|
cached_result = 1;
|
||||||
@@ -357,7 +362,7 @@ static bool px4_windows_stdout_supports_vt(FILE *out)
|
|||||||
static long long px4_linux_syscall3(long long num, long long a, long long b, long long c)
|
static long long px4_linux_syscall3(long long num, long long a, long long b, long long c)
|
||||||
{
|
{
|
||||||
long long ret;
|
long long ret;
|
||||||
__asm__ volatile (
|
__asm__ volatile(
|
||||||
"syscall"
|
"syscall"
|
||||||
: "=a"(ret)
|
: "=a"(ret)
|
||||||
: "0"(num), "D"(a), "S"(b), "d"(c)
|
: "0"(num), "D"(a), "S"(b), "d"(c)
|
||||||
@@ -370,6 +375,7 @@ static long long px4_linux_syscall3(long long num, long long a, long long b, lon
|
|||||||
static bool px4_wine_write_stdout_direct(const char *buffer, size_t length)
|
static bool px4_wine_write_stdout_direct(const char *buffer, size_t length)
|
||||||
{
|
{
|
||||||
#if defined(__x86_64__) && defined(__GNUC__)
|
#if defined(__x86_64__) && defined(__GNUC__)
|
||||||
|
|
||||||
if (!px4_is_running_under_wine() || length == 0) {
|
if (!px4_is_running_under_wine() || length == 0) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@@ -628,10 +634,12 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char
|
|||||||
buf[max_length] = 0;
|
buf[max_length] = 0;
|
||||||
|
|
||||||
#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT)
|
#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT)
|
||||||
|
|
||||||
if (!wrote_to_stdout && use_color && px4_is_running_under_wine()) {
|
if (!wrote_to_stdout && use_color && px4_is_running_under_wine()) {
|
||||||
fflush(out);
|
fflush(out);
|
||||||
wrote_to_stdout = px4_wine_write_stdout_direct(buf, strlen(buf));
|
wrote_to_stdout = px4_wine_write_stdout_direct(buf, strlen(buf));
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!wrote_to_stdout) {
|
if (!wrote_to_stdout) {
|
||||||
@@ -769,10 +777,12 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...)
|
|||||||
buf[max_length] = 0;
|
buf[max_length] = 0;
|
||||||
|
|
||||||
#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT)
|
#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT)
|
||||||
|
|
||||||
if (!wrote_to_stdout && use_color && px4_is_running_under_wine()) {
|
if (!wrote_to_stdout && use_color && px4_is_running_under_wine()) {
|
||||||
fflush(out);
|
fflush(out);
|
||||||
wrote_to_stdout = px4_wine_write_stdout_direct(buf, strlen(buf));
|
wrote_to_stdout = px4_wine_write_stdout_direct(buf, strlen(buf));
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!wrote_to_stdout) {
|
if (!wrote_to_stdout) {
|
||||||
|
|||||||
@@ -333,6 +333,7 @@ WorkQueueManagerRun(int, char **)
|
|||||||
int effective_prio = sched_priority;
|
int effective_prio = sched_priority;
|
||||||
|
|
||||||
if (effective_prio > max_prio) { effective_prio = max_prio; }
|
if (effective_prio > max_prio) { effective_prio = max_prio; }
|
||||||
|
|
||||||
if (effective_prio < min_prio) { effective_prio = min_prio; }
|
if (effective_prio < min_prio) { effective_prio = min_prio; }
|
||||||
|
|
||||||
param.sched_priority = effective_prio;
|
param.sched_priority = effective_prio;
|
||||||
|
|||||||
@@ -133,6 +133,7 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
|
|||||||
if (px4_sem_getvalue(&_hrt_work_signal, &sem_val) == 0 && sem_val <= 0) {
|
if (px4_sem_getvalue(&_hrt_work_signal, &sem_val) == 0 && sem_val <= 0) {
|
||||||
px4_sem_post(&_hrt_work_signal);
|
px4_sem_post(&_hrt_work_signal);
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
|
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -111,11 +111,13 @@ static inline DIR *opendir(const char *name)
|
|||||||
if (!name) { errno = EINVAL; return nullptr; }
|
if (!name) { errno = EINVAL; return nullptr; }
|
||||||
|
|
||||||
DIR *d = (DIR *)calloc(1, sizeof(DIR));
|
DIR *d = (DIR *)calloc(1, sizeof(DIR));
|
||||||
|
|
||||||
if (!d) { errno = ENOMEM; return nullptr; }
|
if (!d) { errno = ENOMEM; return nullptr; }
|
||||||
|
|
||||||
/* Translate "foo" -> "foo\*" so FindFirstFile enumerates the
|
/* Translate "foo" -> "foo\*" so FindFirstFile enumerates the
|
||||||
* directory's contents rather than matching the directory itself. */
|
* directory's contents rather than matching the directory itself. */
|
||||||
int n = snprintf(d->pattern, sizeof(d->pattern), "%s\\*", name);
|
int n = snprintf(d->pattern, sizeof(d->pattern), "%s\\*", name);
|
||||||
|
|
||||||
if (n <= 0 || (size_t)n >= sizeof(d->pattern)) {
|
if (n <= 0 || (size_t)n >= sizeof(d->pattern)) {
|
||||||
free(d);
|
free(d);
|
||||||
errno = ENAMETOOLONG;
|
errno = ENAMETOOLONG;
|
||||||
@@ -123,11 +125,13 @@ static inline DIR *opendir(const char *name)
|
|||||||
}
|
}
|
||||||
|
|
||||||
d->handle = FindFirstFileA(d->pattern, &d->find_data);
|
d->handle = FindFirstFileA(d->pattern, &d->find_data);
|
||||||
|
|
||||||
if (d->handle == INVALID_HANDLE_VALUE) {
|
if (d->handle == INVALID_HANDLE_VALUE) {
|
||||||
free(d);
|
free(d);
|
||||||
errno = ENOENT;
|
errno = ENOENT;
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
d->first = 1;
|
d->first = 1;
|
||||||
d->offset = 0;
|
d->offset = 0;
|
||||||
return d;
|
return d;
|
||||||
@@ -145,6 +149,7 @@ static inline struct dirent *readdir(DIR *d)
|
|||||||
|
|
||||||
if (d->first) {
|
if (d->first) {
|
||||||
d->first = 0;
|
d->first = 0;
|
||||||
|
|
||||||
} else if (!FindNextFileA(d->handle, &d->find_data)) {
|
} else if (!FindNextFileA(d->handle, &d->find_data)) {
|
||||||
return nullptr; /* end of directory */
|
return nullptr; /* end of directory */
|
||||||
}
|
}
|
||||||
@@ -157,8 +162,10 @@ static inline struct dirent *readdir(DIR *d)
|
|||||||
|
|
||||||
if (d->find_data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) {
|
if (d->find_data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) {
|
||||||
d->entry.d_type = DT_DIR;
|
d->entry.d_type = DT_DIR;
|
||||||
|
|
||||||
} else if (d->find_data.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) {
|
} else if (d->find_data.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) {
|
||||||
d->entry.d_type = DT_LNK;
|
d->entry.d_type = DT_LNK;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
d->entry.d_type = DT_REG;
|
d->entry.d_type = DT_REG;
|
||||||
}
|
}
|
||||||
@@ -170,9 +177,11 @@ static inline struct dirent *readdir(DIR *d)
|
|||||||
static inline int closedir(DIR *d)
|
static inline int closedir(DIR *d)
|
||||||
{
|
{
|
||||||
if (!d) { errno = EBADF; return -1; }
|
if (!d) { errno = EBADF; return -1; }
|
||||||
|
|
||||||
if (d->handle != INVALID_HANDLE_VALUE) {
|
if (d->handle != INVALID_HANDLE_VALUE) {
|
||||||
FindClose(d->handle);
|
FindClose(d->handle);
|
||||||
}
|
}
|
||||||
|
|
||||||
free(d);
|
free(d);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
@@ -181,9 +190,11 @@ static inline int closedir(DIR *d)
|
|||||||
static inline void rewinddir(DIR *d)
|
static inline void rewinddir(DIR *d)
|
||||||
{
|
{
|
||||||
if (!d) { return; }
|
if (!d) { return; }
|
||||||
|
|
||||||
if (d->handle != INVALID_HANDLE_VALUE) {
|
if (d->handle != INVALID_HANDLE_VALUE) {
|
||||||
FindClose(d->handle);
|
FindClose(d->handle);
|
||||||
}
|
}
|
||||||
|
|
||||||
d->handle = FindFirstFileA(d->pattern, &d->find_data);
|
d->handle = FindFirstFileA(d->pattern, &d->find_data);
|
||||||
d->first = (d->handle != INVALID_HANDLE_VALUE) ? 1 : 0;
|
d->first = (d->handle != INVALID_HANDLE_VALUE) ? 1 : 0;
|
||||||
d->offset = 0;
|
d->offset = 0;
|
||||||
|
|||||||
@@ -240,24 +240,27 @@ static inline int fcntl(int fd, int cmd, ...)
|
|||||||
return 0;
|
return 0;
|
||||||
|
|
||||||
case F_SETFL: {
|
case F_SETFL: {
|
||||||
va_list ap;
|
va_list ap;
|
||||||
va_start(ap, cmd);
|
va_start(ap, cmd);
|
||||||
int flags = va_arg(ap, int);
|
int flags = va_arg(ap, int);
|
||||||
va_end(ap);
|
va_end(ap);
|
||||||
|
|
||||||
if (flags & O_NONBLOCK) {
|
if (flags & O_NONBLOCK) {
|
||||||
/* Socket fds live in a separate namespace on winsock -
|
/* Socket fds live in a separate namespace on winsock -
|
||||||
* ioctlsocket works on SOCKET handles, which the CRT
|
* ioctlsocket works on SOCKET handles, which the CRT
|
||||||
* stores directly in its int fds via _open_osfhandle. */
|
* stores directly in its int fds via _open_osfhandle. */
|
||||||
u_long mode = 1;
|
u_long mode = 1;
|
||||||
if (ioctlsocket((SOCKET)fd, FIONBIO, &mode) == 0) {
|
|
||||||
return 0;
|
if (ioctlsocket((SOCKET)fd, FIONBIO, &mode) == 0) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
errno = EBADF;
|
||||||
|
return -1;
|
||||||
}
|
}
|
||||||
errno = EBADF;
|
|
||||||
return -1;
|
return 0;
|
||||||
}
|
}
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
case F_SETFD:
|
case F_SETFD:
|
||||||
/* FD_CLOEXEC has no Win32 analog - close-on-exec is the
|
/* FD_CLOEXEC has no Win32 analog - close-on-exec is the
|
||||||
@@ -268,59 +271,69 @@ static inline int fcntl(int fd, int cmd, ...)
|
|||||||
case F_GETLK:
|
case F_GETLK:
|
||||||
case F_SETLK:
|
case F_SETLK:
|
||||||
case F_SETLKW: {
|
case F_SETLKW: {
|
||||||
va_list ap;
|
va_list ap;
|
||||||
va_start(ap, cmd);
|
va_start(ap, cmd);
|
||||||
struct flock *fl = va_arg(ap, struct flock *);
|
struct flock *fl = va_arg(ap, struct flock *);
|
||||||
va_end(ap);
|
va_end(ap);
|
||||||
if (!fl) { errno = EINVAL; return -1; }
|
|
||||||
|
|
||||||
HANDLE h = (HANDLE)_get_osfhandle(fd);
|
if (!fl) { errno = EINVAL; return -1; }
|
||||||
if (h == INVALID_HANDLE_VALUE) { errno = EBADF; return -1; }
|
|
||||||
|
|
||||||
/* l_len == 0 means "lock through EOF" in POSIX. Use the max
|
HANDLE h = (HANDLE)_get_osfhandle(fd);
|
||||||
* range LockFileEx accepts (2^63-1) so the byte-range lock
|
|
||||||
* effectively covers the whole file. */
|
if (h == INVALID_HANDLE_VALUE) { errno = EBADF; return -1; }
|
||||||
DWORD off_lo = (DWORD)fl->l_start;
|
|
||||||
DWORD off_hi = 0;
|
/* l_len == 0 means "lock through EOF" in POSIX. Use the max
|
||||||
DWORD len_lo = fl->l_len ? (DWORD)fl->l_len : 0xFFFFFFFFu;
|
* range LockFileEx accepts (2^63-1) so the byte-range lock
|
||||||
DWORD len_hi = fl->l_len ? 0 : 0x7FFFFFFFu;
|
* effectively covers the whole file. */
|
||||||
OVERLAPPED ov;
|
DWORD off_lo = (DWORD)fl->l_start;
|
||||||
memset(&ov, 0, sizeof(ov));
|
DWORD off_hi = 0;
|
||||||
ov.Offset = off_lo;
|
DWORD len_lo = fl->l_len ? (DWORD)fl->l_len : 0xFFFFFFFFu;
|
||||||
ov.OffsetHigh = off_hi;
|
DWORD len_hi = fl->l_len ? 0 : 0x7FFFFFFFu;
|
||||||
|
OVERLAPPED ov;
|
||||||
|
memset(&ov, 0, sizeof(ov));
|
||||||
|
ov.Offset = off_lo;
|
||||||
|
ov.OffsetHigh = off_hi;
|
||||||
|
|
||||||
|
if (cmd == F_GETLK) {
|
||||||
|
/* Try to acquire non-blocking; if it succeeds no conflict
|
||||||
|
* exists, so release and report F_UNLCK. If it fails with
|
||||||
|
* ERROR_LOCK_VIOLATION something else holds a lock. */
|
||||||
|
DWORD flags = LOCKFILE_EXCLUSIVE_LOCK | LOCKFILE_FAIL_IMMEDIATELY;
|
||||||
|
|
||||||
|
if (LockFileEx(h, flags, 0, len_lo, len_hi, &ov)) {
|
||||||
|
UnlockFileEx(h, 0, len_lo, len_hi, &ov);
|
||||||
|
fl->l_type = F_UNLCK;
|
||||||
|
|
||||||
|
} else if (GetLastError() == ERROR_LOCK_VIOLATION
|
||||||
|
|| GetLastError() == ERROR_IO_PENDING) {
|
||||||
|
fl->l_type = F_WRLCK;
|
||||||
|
fl->l_pid = 0;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
errno = EACCES;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (fl->l_type == F_UNLCK) {
|
||||||
|
if (UnlockFileEx(h, 0, len_lo, len_hi, &ov)) { return 0; }
|
||||||
|
|
||||||
if (cmd == F_GETLK) {
|
|
||||||
/* Try to acquire non-blocking; if it succeeds no conflict
|
|
||||||
* exists, so release and report F_UNLCK. If it fails with
|
|
||||||
* ERROR_LOCK_VIOLATION something else holds a lock. */
|
|
||||||
DWORD flags = LOCKFILE_EXCLUSIVE_LOCK | LOCKFILE_FAIL_IMMEDIATELY;
|
|
||||||
if (LockFileEx(h, flags, 0, len_lo, len_hi, &ov)) {
|
|
||||||
UnlockFileEx(h, 0, len_lo, len_hi, &ov);
|
|
||||||
fl->l_type = F_UNLCK;
|
|
||||||
} else if (GetLastError() == ERROR_LOCK_VIOLATION
|
|
||||||
|| GetLastError() == ERROR_IO_PENDING) {
|
|
||||||
fl->l_type = F_WRLCK;
|
|
||||||
fl->l_pid = 0;
|
|
||||||
} else {
|
|
||||||
errno = EACCES;
|
errno = EACCES;
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (fl->l_type == F_UNLCK) {
|
DWORD flags = LOCKFILE_EXCLUSIVE_LOCK;
|
||||||
if (UnlockFileEx(h, 0, len_lo, len_hi, &ov)) { return 0; }
|
|
||||||
errno = EACCES;
|
if (cmd == F_SETLK) { flags |= LOCKFILE_FAIL_IMMEDIATELY; }
|
||||||
|
|
||||||
|
if (LockFileEx(h, flags, 0, len_lo, len_hi, &ov)) { return 0; }
|
||||||
|
|
||||||
|
errno = (GetLastError() == ERROR_LOCK_VIOLATION) ? EAGAIN : EACCES;
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
DWORD flags = LOCKFILE_EXCLUSIVE_LOCK;
|
|
||||||
if (cmd == F_SETLK) { flags |= LOCKFILE_FAIL_IMMEDIATELY; }
|
|
||||||
if (LockFileEx(h, flags, 0, len_lo, len_hi, &ov)) { return 0; }
|
|
||||||
errno = (GetLastError() == ERROR_LOCK_VIOLATION) ? EAGAIN : EACCES;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
default:
|
default:
|
||||||
errno = EINVAL;
|
errno = EINVAL;
|
||||||
return -1;
|
return -1;
|
||||||
|
|||||||
@@ -338,6 +338,7 @@ static inline int poll(struct pollfd *fds, nfds_t nfds, int timeout)
|
|||||||
}
|
}
|
||||||
|
|
||||||
struct pollfd *socket_fds = (struct pollfd *)malloc(sizeof(struct pollfd) * nfds);
|
struct pollfd *socket_fds = (struct pollfd *)malloc(sizeof(struct pollfd) * nfds);
|
||||||
|
|
||||||
nfds_t *socket_indices = (nfds_t *)malloc(sizeof(nfds_t) * nfds);
|
nfds_t *socket_indices = (nfds_t *)malloc(sizeof(nfds_t) * nfds);
|
||||||
|
|
||||||
if (socket_fds == nullptr || socket_indices == nullptr) {
|
if (socket_fds == nullptr || socket_indices == nullptr) {
|
||||||
|
|||||||
@@ -107,6 +107,7 @@ int nanosleep(const struct timespec *req, struct timespec *rem);
|
|||||||
static inline struct tm *gmtime_r(const time_t *timep, struct tm *result)
|
static inline struct tm *gmtime_r(const time_t *timep, struct tm *result)
|
||||||
{
|
{
|
||||||
if (!timep || !result) { return nullptr; }
|
if (!timep || !result) { return nullptr; }
|
||||||
|
|
||||||
return (gmtime_s(result, timep) == 0) ? result : nullptr;
|
return (gmtime_s(result, timep) == 0) ? result : nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -114,6 +115,7 @@ static inline struct tm *gmtime_r(const time_t *timep, struct tm *result)
|
|||||||
static inline struct tm *localtime_r(const time_t *timep, struct tm *result)
|
static inline struct tm *localtime_r(const time_t *timep, struct tm *result)
|
||||||
{
|
{
|
||||||
if (!timep || !result) { return nullptr; }
|
if (!timep || !result) { return nullptr; }
|
||||||
|
|
||||||
return (localtime_s(result, timep) == 0) ? result : nullptr;
|
return (localtime_s(result, timep) == 0) ? result : nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -121,6 +123,7 @@ static inline struct tm *localtime_r(const time_t *timep, struct tm *result)
|
|||||||
static inline char *asctime_r(const struct tm *tm, char *buf)
|
static inline char *asctime_r(const struct tm *tm, char *buf)
|
||||||
{
|
{
|
||||||
if (!tm || !buf) { return nullptr; }
|
if (!tm || !buf) { return nullptr; }
|
||||||
|
|
||||||
/* POSIX requires a 26-byte buffer; match that to asctime_s. */
|
/* POSIX requires a 26-byte buffer; match that to asctime_s. */
|
||||||
return (asctime_s(buf, 26, tm) == 0) ? buf : nullptr;
|
return (asctime_s(buf, 26, tm) == 0) ? buf : nullptr;
|
||||||
}
|
}
|
||||||
@@ -129,6 +132,7 @@ static inline char *asctime_r(const struct tm *tm, char *buf)
|
|||||||
static inline char *ctime_r(const time_t *timep, char *buf)
|
static inline char *ctime_r(const time_t *timep, char *buf)
|
||||||
{
|
{
|
||||||
if (!timep || !buf) { return nullptr; }
|
if (!timep || !buf) { return nullptr; }
|
||||||
|
|
||||||
return (ctime_s(buf, 26, timep) == 0) ? buf : nullptr;
|
return (ctime_s(buf, 26, timep) == 0) ? buf : nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -309,7 +309,9 @@ static inline int usleep(useconds_t usec)
|
|||||||
const LONGLONG bulk_target_qpc = qpc_target
|
const LONGLONG bulk_target_qpc = qpc_target
|
||||||
- ((LONGLONG)tail_us * qpc_freq.QuadPart) / 1000000LL;
|
- ((LONGLONG)tail_us * qpc_freq.QuadPart) / 1000000LL;
|
||||||
LONGLONG overshoot_qpc = wake_now.QuadPart - bulk_target_qpc;
|
LONGLONG overshoot_qpc = wake_now.QuadPart - bulk_target_qpc;
|
||||||
|
|
||||||
if (overshoot_qpc < 0) { overshoot_qpc = 0; }
|
if (overshoot_qpc < 0) { overshoot_qpc = 0; }
|
||||||
|
|
||||||
const long overshoot_us =
|
const long overshoot_us =
|
||||||
(long)((overshoot_qpc * 1000000LL) / qpc_freq.QuadPart);
|
(long)((overshoot_qpc * 1000000LL) / qpc_freq.QuadPart);
|
||||||
|
|
||||||
@@ -495,8 +497,11 @@ static inline int vdprintf(int fd, const char *fmt, va_list ap)
|
|||||||
{
|
{
|
||||||
char buf[1024];
|
char buf[1024];
|
||||||
int n = vsnprintf(buf, sizeof(buf), fmt, ap);
|
int n = vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||||
|
|
||||||
if (n <= 0) { return n; }
|
if (n <= 0) { return n; }
|
||||||
|
|
||||||
if ((size_t)n >= sizeof(buf)) { n = (int)sizeof(buf) - 1; }
|
if ((size_t)n >= sizeof(buf)) { n = (int)sizeof(buf) - 1; }
|
||||||
|
|
||||||
return (int)_write(fd, buf, (unsigned)n);
|
return (int)_write(fd, buf, (unsigned)n);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
+2
-2
@@ -113,9 +113,9 @@ private:
|
|||||||
uint64_t time_us{0};
|
uint64_t time_us{0};
|
||||||
bool timeout{false};
|
bool timeout{false};
|
||||||
#if defined(__PX4_WINDOWS) || defined(_WIN32)
|
#if defined(__PX4_WINDOWS) || defined(_WIN32)
|
||||||
bool signaled{false};
|
bool signaled {false};
|
||||||
#endif
|
#endif
|
||||||
std::atomic<bool> done{false};
|
std::atomic<bool> done {false};
|
||||||
std::atomic<bool> removed{true};
|
std::atomic<bool> removed{true};
|
||||||
|
|
||||||
TimedWait *next{nullptr}; ///< linked list
|
TimedWait *next{nullptr}; ///< linked list
|
||||||
|
|||||||
@@ -56,7 +56,8 @@ static uint64_t lockstep_component_wait_budget_us()
|
|||||||
static const uint64_t wait_budget_us = []() -> uint64_t {
|
static const uint64_t wait_budget_us = []() -> uint64_t {
|
||||||
const char *speed_factor_env = getenv("PX4_SIM_SPEED_FACTOR");
|
const char *speed_factor_env = getenv("PX4_SIM_SPEED_FACTOR");
|
||||||
|
|
||||||
if (speed_factor_env != nullptr) {
|
if (speed_factor_env != nullptr)
|
||||||
|
{
|
||||||
const double speed_factor = strtod(speed_factor_env, nullptr);
|
const double speed_factor = strtod(speed_factor_env, nullptr);
|
||||||
|
|
||||||
if (speed_factor > 0.) {
|
if (speed_factor > 0.) {
|
||||||
@@ -178,7 +179,10 @@ void LockstepComponents::wait_for_components()
|
|||||||
sched_yield();
|
sched_yield();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
while (px4_sem_wait(&_components_sem) != 0) {}
|
while (px4_sem_wait(&_components_sem) != 0) {}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -81,7 +81,8 @@ LockstepScheduler::~LockstepScheduler()
|
|||||||
std::lock_guard<std::mutex> guard(scheduler_registry_mutex());
|
std::lock_guard<std::mutex> guard(scheduler_registry_mutex());
|
||||||
auto &schedulers = scheduler_registry();
|
auto &schedulers = scheduler_registry();
|
||||||
|
|
||||||
for (auto it = schedulers.begin(); it != schedulers.end(); ++it) {
|
for (auto it = schedulers.begin(); it != schedulers.end(); ++it)
|
||||||
|
{
|
||||||
if (*it == this) {
|
if (*it == this) {
|
||||||
schedulers.erase(it);
|
schedulers.erase(it);
|
||||||
break;
|
break;
|
||||||
@@ -297,6 +298,7 @@ int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *loc
|
|||||||
AcquireSRWLockExclusive(&timed_wait.wait_lock);
|
AcquireSRWLockExclusive(&timed_wait.wait_lock);
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Spurious wakeup: loop to re-check.
|
// Spurious wakeup: loop to re-check.
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -304,6 +306,7 @@ int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *loc
|
|||||||
|
|
||||||
pthread_mutex_lock(lock);
|
pthread_mutex_lock(lock);
|
||||||
#else
|
#else
|
||||||
|
|
||||||
while (!timed_wait.timeout) {
|
while (!timed_wait.timeout) {
|
||||||
struct timespec poll_deadline;
|
struct timespec poll_deadline;
|
||||||
clock_gettime(CLOCK_REALTIME, &poll_deadline);
|
clock_gettime(CLOCK_REALTIME, &poll_deadline);
|
||||||
@@ -334,9 +337,11 @@ int LockstepScheduler::cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *loc
|
|||||||
if (result != 0) {
|
if (result != 0) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Spurious wakeup or real signal not reflected in `timeout`
|
// Spurious wakeup or real signal not reflected in `timeout`
|
||||||
// (shouldn't happen normally). Loop to re-check.
|
// (shouldn't happen normally). Loop to re-check.
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const bool timeout = timed_wait.timeout;
|
const bool timeout = timed_wait.timeout;
|
||||||
|
|||||||
@@ -588,6 +588,7 @@ int main(int argc, char **argv)
|
|||||||
|
|
||||||
if (ret != PX4_OK) {
|
if (ret != PX4_OK) {
|
||||||
#ifdef __PX4_WINDOWS
|
#ifdef __PX4_WINDOWS
|
||||||
|
|
||||||
// See create_symlinks_if_needed(): symlink creation may require
|
// See create_symlinks_if_needed(): symlink creation may require
|
||||||
// Developer Mode / admin on Windows. Treat EACCES/EPERM as
|
// Developer Mode / admin on Windows. Treat EACCES/EPERM as
|
||||||
// non-fatal — tests that need test_data must use the absolute
|
// non-fatal — tests that need test_data must use the absolute
|
||||||
@@ -665,6 +666,7 @@ int main(int argc, char **argv)
|
|||||||
std::string cmd("shutdown");
|
std::string cmd("shutdown");
|
||||||
px4_daemon::Pxh::process_line(cmd, true);
|
px4_daemon::Pxh::process_line(cmd, true);
|
||||||
#ifdef __PX4_WINDOWS
|
#ifdef __PX4_WINDOWS
|
||||||
|
|
||||||
// The shutdown command runs asynchronously on the worker queue. While
|
// The shutdown command runs asynchronously on the worker queue. While
|
||||||
// waiting for the worker to terminate the process, keep restoring and
|
// waiting for the worker to terminate the process, keep restoring and
|
||||||
// draining stdin so Enters typed during shutdown do not leak back into
|
// draining stdin so Enters typed during shutdown do not leak back into
|
||||||
@@ -726,6 +728,7 @@ int create_symlinks_if_needed(std::string &data_path)
|
|||||||
|
|
||||||
if (ret != 0) {
|
if (ret != 0) {
|
||||||
#ifdef __PX4_WINDOWS
|
#ifdef __PX4_WINDOWS
|
||||||
|
|
||||||
// On Windows, creating symlinks requires admin rights, Developer Mode
|
// On Windows, creating symlinks requires admin rights, Developer Mode
|
||||||
// (Win10 1703+), or SE_CREATE_SYMBOLIC_LINK_PRIVILEGE. The windows_shim
|
// (Win10 1703+), or SE_CREATE_SYMBOLIC_LINK_PRIVILEGE. The windows_shim
|
||||||
// symlink() already falls back to hard links / recursive copy, but if
|
// symlink() already falls back to hard links / recursive copy, but if
|
||||||
@@ -835,12 +838,15 @@ void sig_int_handler(int sig_num)
|
|||||||
close(_lock_fd_for_signal);
|
close(_lock_fd_for_signal);
|
||||||
_lock_fd_for_signal = -1;
|
_lock_fd_for_signal = -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_lock_path_for_signal[0]) {
|
if (_lock_path_for_signal[0]) {
|
||||||
unlink(_lock_path_for_signal);
|
unlink(_lock_path_for_signal);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_pid_path_for_signal[0]) {
|
if (_pid_path_for_signal[0]) {
|
||||||
unlink(_pid_path_for_signal);
|
unlink(_pid_path_for_signal);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
uorb_shutdown();
|
uorb_shutdown();
|
||||||
px4_daemon::Pxh::stop();
|
px4_daemon::Pxh::stop();
|
||||||
@@ -1164,6 +1170,7 @@ static bool is_absolute_path(const std::string &path)
|
|||||||
if (path.length() >= 2 && is_path_separator(path[0]) && is_path_separator(path[1])) {
|
if (path.length() >= 2 && is_path_separator(path[0]) && is_path_separator(path[1])) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
|
|||||||
@@ -207,6 +207,7 @@ Client::process_args(const int argc, const char **argv)
|
|||||||
PX4_ERR("error connecting to socket: %s", strerror(errno));
|
PX4_ERR("error connecting to socket: %s", strerror(errno));
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
int ret = _send_cmds(argc, argv);
|
int ret = _send_cmds(argc, argv);
|
||||||
@@ -243,12 +244,14 @@ Client::_send_cmds(const int argc, const char **argv)
|
|||||||
std::string arg = argv[i];
|
std::string arg = argv[i];
|
||||||
|
|
||||||
#ifdef __PX4_WINDOWS
|
#ifdef __PX4_WINDOWS
|
||||||
|
|
||||||
// Client executables are real .exe files on Windows, not POSIX
|
// Client executables are real .exe files on Windows, not POSIX
|
||||||
// symlinks. The pxh command namespace remains extensionless
|
// symlinks. The pxh command namespace remains extensionless
|
||||||
// ("commander", "shutdown", ...), so strip only argv[0].
|
// ("commander", "shutdown", ...), so strip only argv[0].
|
||||||
if (i == 0 && ends_with_exe_suffix(arg)) {
|
if (i == 0 && ends_with_exe_suffix(arg)) {
|
||||||
arg.resize(arg.size() - 4);
|
arg.resize(arg.size() - 4);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
cmd_buf += arg;
|
cmd_buf += arg;
|
||||||
|
|||||||
@@ -574,6 +574,7 @@ void Pxh::run_pxh()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -103,6 +103,7 @@ static std::pair<int, int> create_relay_pipe()
|
|||||||
int wfd = _open_osfhandle(reinterpret_cast<intptr_t>(w), 0);
|
int wfd = _open_osfhandle(reinterpret_cast<intptr_t>(w), 0);
|
||||||
|
|
||||||
if (rfd < 0 || wfd < 0) {
|
if (rfd < 0 || wfd < 0) {
|
||||||
|
|
||||||
if (rfd >= 0) { ::close(rfd); } else { CloseHandle(r); }
|
if (rfd >= 0) { ::close(rfd); } else { CloseHandle(r); }
|
||||||
|
|
||||||
if (wfd >= 0) { ::close(wfd); } else { CloseHandle(w); }
|
if (wfd >= 0) { ::close(wfd); } else { CloseHandle(w); }
|
||||||
@@ -166,7 +167,8 @@ static void stdout_relay_loop(int stdout_read_fd, socket_handle_t client_fd)
|
|||||||
static void stdin_relay_loop(socket_handle_t client_fd, int stdin_write_fd, std::string prefill)
|
static void stdin_relay_loop(socket_handle_t client_fd, int stdin_write_fd, std::string prefill)
|
||||||
{
|
{
|
||||||
auto write_all = [&](const char *buf, ssize_t n) -> bool {
|
auto write_all = [&](const char *buf, ssize_t n) -> bool {
|
||||||
while (n > 0) {
|
while (n > 0)
|
||||||
|
{
|
||||||
const int n_written = ::write(stdin_write_fd, buf, (unsigned)n);
|
const int n_written = ::write(stdin_write_fd, buf, (unsigned)n);
|
||||||
|
|
||||||
if (n_written <= 0) {
|
if (n_written <= 0) {
|
||||||
@@ -312,6 +314,7 @@ Server::start()
|
|||||||
PX4_ERR("error binding socket 127.0.0.1:%u, WSA error = %d", port, WSAGetLastError());
|
PX4_ERR("error binding socket 127.0.0.1:%u, WSA error = %d", port, WSAGetLastError());
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
std::string sock_path = get_socket_path(_instance_id);
|
std::string sock_path = get_socket_path(_instance_id);
|
||||||
|
|
||||||
@@ -333,6 +336,7 @@ Server::start()
|
|||||||
PX4_ERR("error binding socket %s, error = %s", sock_path.c_str(), strerror(errno));
|
PX4_ERR("error binding socket %s, error = %s", sock_path.c_str(), strerror(errno));
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (listen(_fd, 10) < 0) {
|
if (listen(_fd, 10) < 0) {
|
||||||
@@ -374,6 +378,7 @@ Server::_server_main()
|
|||||||
}
|
}
|
||||||
|
|
||||||
#ifdef __PX4_WINDOWS
|
#ifdef __PX4_WINDOWS
|
||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
pollfd listen_fd {poll_socket(_fd), POLLIN, 0};
|
pollfd listen_fd {poll_socket(_fd), POLLIN, 0};
|
||||||
int n_ready = poll(&listen_fd, 1, -1);
|
int n_ready = poll(&listen_fd, 1, -1);
|
||||||
|
|||||||
@@ -170,6 +170,7 @@ int px4_sem_timedwait(px4_sem_t *s, const struct timespec *abstime)
|
|||||||
} else {
|
} else {
|
||||||
ret = 0;
|
ret = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
if (s->value < 0) {
|
if (s->value < 0) {
|
||||||
|
|||||||
@@ -49,6 +49,7 @@
|
|||||||
extern "C" int flock(int fd, int operation)
|
extern "C" int flock(int fd, int operation)
|
||||||
{
|
{
|
||||||
HANDLE h = (HANDLE)_get_osfhandle(fd);
|
HANDLE h = (HANDLE)_get_osfhandle(fd);
|
||||||
|
|
||||||
if (h == INVALID_HANDLE_VALUE) { errno = EBADF; return -1; }
|
if (h == INVALID_HANDLE_VALUE) { errno = EBADF; return -1; }
|
||||||
|
|
||||||
if (operation & LOCK_UN) {
|
if (operation & LOCK_UN) {
|
||||||
@@ -58,10 +59,13 @@ extern "C" int flock(int fd, int operation)
|
|||||||
}
|
}
|
||||||
|
|
||||||
DWORD flags = 0;
|
DWORD flags = 0;
|
||||||
|
|
||||||
if (operation & LOCK_EX) { flags |= LOCKFILE_EXCLUSIVE_LOCK; }
|
if (operation & LOCK_EX) { flags |= LOCKFILE_EXCLUSIVE_LOCK; }
|
||||||
|
|
||||||
if (operation & LOCK_NB) { flags |= LOCKFILE_FAIL_IMMEDIATELY; }
|
if (operation & LOCK_NB) { flags |= LOCKFILE_FAIL_IMMEDIATELY; }
|
||||||
|
|
||||||
OVERLAPPED ov{}; ov.Offset = 0; ov.OffsetHigh = 0;
|
OVERLAPPED ov{}; ov.Offset = 0; ov.OffsetHigh = 0;
|
||||||
|
|
||||||
if (LockFileEx(h, flags, 0, MAXDWORD, MAXDWORD, &ov)) { return 0; }
|
if (LockFileEx(h, flags, 0, MAXDWORD, MAXDWORD, &ov)) { return 0; }
|
||||||
|
|
||||||
errno = (GetLastError() == ERROR_LOCK_VIOLATION) ? EWOULDBLOCK : EIO;
|
errno = (GetLastError() == ERROR_LOCK_VIOLATION) ? EWOULDBLOCK : EIO;
|
||||||
|
|||||||
@@ -50,8 +50,11 @@
|
|||||||
static DWORD mman_page_protect(int prot)
|
static DWORD mman_page_protect(int prot)
|
||||||
{
|
{
|
||||||
if ((prot & (PROT_READ | PROT_WRITE)) == (PROT_READ | PROT_WRITE)) { return PAGE_READWRITE; }
|
if ((prot & (PROT_READ | PROT_WRITE)) == (PROT_READ | PROT_WRITE)) { return PAGE_READWRITE; }
|
||||||
|
|
||||||
if (prot & PROT_WRITE) { return PAGE_READWRITE; }
|
if (prot & PROT_WRITE) { return PAGE_READWRITE; }
|
||||||
|
|
||||||
if (prot & PROT_READ) { return PAGE_READONLY; }
|
if (prot & PROT_READ) { return PAGE_READONLY; }
|
||||||
|
|
||||||
return PAGE_NOACCESS;
|
return PAGE_NOACCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -66,12 +69,14 @@ extern "C" void *mmap(void *, size_t length, int prot, int flags, int fd, off_t
|
|||||||
}
|
}
|
||||||
|
|
||||||
HANDLE h = (HANDLE)_get_osfhandle(fd);
|
HANDLE h = (HANDLE)_get_osfhandle(fd);
|
||||||
|
|
||||||
if (h == INVALID_HANDLE_VALUE) { errno = EBADF; return MAP_FAILED; }
|
if (h == INVALID_HANDLE_VALUE) { errno = EBADF; return MAP_FAILED; }
|
||||||
|
|
||||||
DWORD map_access = (prot & PROT_WRITE) ? FILE_MAP_WRITE : FILE_MAP_READ;
|
DWORD map_access = (prot & PROT_WRITE) ? FILE_MAP_WRITE : FILE_MAP_READ;
|
||||||
DWORD create_protect = (prot & PROT_WRITE) ? PAGE_READWRITE : PAGE_READONLY;
|
DWORD create_protect = (prot & PROT_WRITE) ? PAGE_READWRITE : PAGE_READONLY;
|
||||||
|
|
||||||
HANDLE mapping = CreateFileMappingA(h, nullptr, create_protect, 0, 0, nullptr);
|
HANDLE mapping = CreateFileMappingA(h, nullptr, create_protect, 0, 0, nullptr);
|
||||||
|
|
||||||
if (!mapping) { errno = EACCES; return MAP_FAILED; }
|
if (!mapping) { errno = EACCES; return MAP_FAILED; }
|
||||||
|
|
||||||
/* The mapping handle can be closed after MapViewOfFile succeeds; the view
|
/* The mapping handle can be closed after MapViewOfFile succeeds; the view
|
||||||
@@ -91,7 +96,9 @@ extern "C" int munmap(void *addr, size_t length)
|
|||||||
* VirtualFree for anonymous mappings. Both are idempotent enough
|
* VirtualFree for anonymous mappings. Both are idempotent enough
|
||||||
* that trying the wrong one is harmless. */
|
* that trying the wrong one is harmless. */
|
||||||
if (UnmapViewOfFile(addr)) { return 0; }
|
if (UnmapViewOfFile(addr)) { return 0; }
|
||||||
|
|
||||||
if (VirtualFree(addr, 0, MEM_RELEASE)) { return 0; }
|
if (VirtualFree(addr, 0, MEM_RELEASE)) { return 0; }
|
||||||
|
|
||||||
(void)length;
|
(void)length;
|
||||||
errno = EINVAL;
|
errno = EINVAL;
|
||||||
return -1;
|
return -1;
|
||||||
|
|||||||
@@ -53,23 +53,29 @@ extern "C" void *dlopen(const char *filename, int /*flag*/)
|
|||||||
/* POSIX allows dlopen(NULL, ...) to reference the main program. On Windows
|
/* POSIX allows dlopen(NULL, ...) to reference the main program. On Windows
|
||||||
* the equivalent is the current process module. */
|
* the equivalent is the current process module. */
|
||||||
HMODULE h = filename ? LoadLibraryA(filename) : GetModuleHandleA(nullptr);
|
HMODULE h = filename ? LoadLibraryA(filename) : GetModuleHandleA(nullptr);
|
||||||
|
|
||||||
if (!h) { snprintf(_dl_last_error, sizeof(_dl_last_error), "LoadLibrary(%s) failed: 0x%lx", filename ? filename : "(self)", (unsigned long)GetLastError()); }
|
if (!h) { snprintf(_dl_last_error, sizeof(_dl_last_error), "LoadLibrary(%s) failed: 0x%lx", filename ? filename : "(self)", (unsigned long)GetLastError()); }
|
||||||
|
|
||||||
return (void *)h;
|
return (void *)h;
|
||||||
}
|
}
|
||||||
extern "C" int dlclose(void *handle)
|
extern "C" int dlclose(void *handle)
|
||||||
{
|
{
|
||||||
if (!handle) { return 0; }
|
if (!handle) { return 0; }
|
||||||
|
|
||||||
return FreeLibrary((HMODULE)handle) ? 0 : -1;
|
return FreeLibrary((HMODULE)handle) ? 0 : -1;
|
||||||
}
|
}
|
||||||
extern "C" void *dlsym(void *handle, const char *symbol)
|
extern "C" void *dlsym(void *handle, const char *symbol)
|
||||||
{
|
{
|
||||||
FARPROC p = GetProcAddress(handle ? (HMODULE)handle : GetModuleHandleA(nullptr), symbol);
|
FARPROC p = GetProcAddress(handle ? (HMODULE)handle : GetModuleHandleA(nullptr), symbol);
|
||||||
|
|
||||||
if (!p) { snprintf(_dl_last_error, sizeof(_dl_last_error), "GetProcAddress(%s) failed: 0x%lx", symbol, (unsigned long)GetLastError()); }
|
if (!p) { snprintf(_dl_last_error, sizeof(_dl_last_error), "GetProcAddress(%s) failed: 0x%lx", symbol, (unsigned long)GetLastError()); }
|
||||||
|
|
||||||
return (void *)p;
|
return (void *)p;
|
||||||
}
|
}
|
||||||
extern "C" char *dlerror(void)
|
extern "C" char *dlerror(void)
|
||||||
{
|
{
|
||||||
if (_dl_last_error[0] == 0) { return nullptr; }
|
if (_dl_last_error[0] == 0) { return nullptr; }
|
||||||
|
|
||||||
static thread_local char out[sizeof(_dl_last_error)];
|
static thread_local char out[sizeof(_dl_last_error)];
|
||||||
memcpy(out, _dl_last_error, sizeof(out));
|
memcpy(out, _dl_last_error, sizeof(out));
|
||||||
_dl_last_error[0] = 0;
|
_dl_last_error[0] = 0;
|
||||||
|
|||||||
@@ -59,13 +59,16 @@
|
|||||||
extern "C" struct if_nameindex *if_nameindex(void)
|
extern "C" struct if_nameindex *if_nameindex(void)
|
||||||
{
|
{
|
||||||
ULONG flags = GAA_FLAG_SKIP_ANYCAST | GAA_FLAG_SKIP_MULTICAST |
|
ULONG flags = GAA_FLAG_SKIP_ANYCAST | GAA_FLAG_SKIP_MULTICAST |
|
||||||
GAA_FLAG_SKIP_DNS_SERVER;
|
GAA_FLAG_SKIP_DNS_SERVER;
|
||||||
ULONG size = 0;
|
ULONG size = 0;
|
||||||
GetAdaptersAddresses(AF_UNSPEC, flags, nullptr, nullptr, &size);
|
GetAdaptersAddresses(AF_UNSPEC, flags, nullptr, nullptr, &size);
|
||||||
|
|
||||||
if (size == 0) { errno = ENODEV; return nullptr; }
|
if (size == 0) { errno = ENODEV; return nullptr; }
|
||||||
|
|
||||||
IP_ADAPTER_ADDRESSES *adapters = (IP_ADAPTER_ADDRESSES *)malloc(size);
|
IP_ADAPTER_ADDRESSES *adapters = (IP_ADAPTER_ADDRESSES *)malloc(size);
|
||||||
|
|
||||||
if (!adapters) { errno = ENOMEM; return nullptr; }
|
if (!adapters) { errno = ENOMEM; return nullptr; }
|
||||||
|
|
||||||
if (GetAdaptersAddresses(AF_UNSPEC, flags, nullptr, adapters, &size) != NO_ERROR) {
|
if (GetAdaptersAddresses(AF_UNSPEC, flags, nullptr, adapters, &size) != NO_ERROR) {
|
||||||
free(adapters);
|
free(adapters);
|
||||||
errno = ENODEV;
|
errno = ENODEV;
|
||||||
@@ -73,12 +76,15 @@ extern "C" struct if_nameindex *if_nameindex(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
unsigned int count = 0;
|
unsigned int count = 0;
|
||||||
|
|
||||||
for (IP_ADAPTER_ADDRESSES *a = adapters; a; a = a->Next) { count++; }
|
for (IP_ADAPTER_ADDRESSES *a = adapters; a; a = a->Next) { count++; }
|
||||||
|
|
||||||
struct if_nameindex *result = (struct if_nameindex *)calloc(count + 1, sizeof(*result));
|
struct if_nameindex *result = (struct if_nameindex *)calloc(count + 1, sizeof(*result));
|
||||||
|
|
||||||
if (!result) { free(adapters); errno = ENOMEM; return nullptr; }
|
if (!result) { free(adapters); errno = ENOMEM; return nullptr; }
|
||||||
|
|
||||||
unsigned int i = 0;
|
unsigned int i = 0;
|
||||||
|
|
||||||
for (IP_ADAPTER_ADDRESSES *a = adapters; a; a = a->Next, i++) {
|
for (IP_ADAPTER_ADDRESSES *a = adapters; a; a = a->Next, i++) {
|
||||||
char name[IF_NAMESIZE] = {0};
|
char name[IF_NAMESIZE] = {0};
|
||||||
/* FriendlyName is UTF-16. POSIX callers get an IF_NAMESIZE-bounded
|
/* FriendlyName is UTF-16. POSIX callers get an IF_NAMESIZE-bounded
|
||||||
@@ -92,12 +98,14 @@ extern "C" struct if_nameindex *if_nameindex(void)
|
|||||||
* and leak the remaining names; bail out instead so the caller sees ENOMEM. */
|
* and leak the remaining names; bail out instead so the caller sees ENOMEM. */
|
||||||
if (!result[i].if_name) {
|
if (!result[i].if_name) {
|
||||||
for (unsigned int j = 0; j < i; j++) { free(result[j].if_name); }
|
for (unsigned int j = 0; j < i; j++) { free(result[j].if_name); }
|
||||||
|
|
||||||
free(result);
|
free(result);
|
||||||
free(adapters);
|
free(adapters);
|
||||||
errno = ENOMEM;
|
errno = ENOMEM;
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
free(adapters);
|
free(adapters);
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
@@ -105,8 +113,10 @@ extern "C" struct if_nameindex *if_nameindex(void)
|
|||||||
extern "C" void if_freenameindex(struct if_nameindex *ptr)
|
extern "C" void if_freenameindex(struct if_nameindex *ptr)
|
||||||
{
|
{
|
||||||
if (!ptr) { return; }
|
if (!ptr) { return; }
|
||||||
|
|
||||||
for (struct if_nameindex *p = ptr; p->if_name; p++) {
|
for (struct if_nameindex *p = ptr; p->if_name; p++) {
|
||||||
free(p->if_name);
|
free(p->if_name);
|
||||||
}
|
}
|
||||||
|
|
||||||
free(ptr);
|
free(ptr);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -545,7 +545,9 @@ extern "C" struct in_addr inet_makeaddr(in_addr_t net, in_addr_t host)
|
|||||||
}
|
}
|
||||||
|
|
||||||
struct in_addr out {};
|
struct in_addr out {};
|
||||||
|
|
||||||
out.s_addr = htonl(addr);
|
out.s_addr = htonl(addr);
|
||||||
|
|
||||||
return out;
|
return out;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -70,7 +70,7 @@ extern "C" ssize_t sendmsg(int fd, const struct msghdr *msg, int flags)
|
|||||||
DWORD sent = 0;
|
DWORD sent = 0;
|
||||||
const int rc = (msg->msg_name && msg->msg_namelen > 0)
|
const int rc = (msg->msg_name && msg->msg_namelen > 0)
|
||||||
? WSASendTo((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &sent, (DWORD)flags,
|
? WSASendTo((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &sent, (DWORD)flags,
|
||||||
(const sockaddr *)msg->msg_name, msg->msg_namelen, nullptr, nullptr)
|
(const sockaddr *)msg->msg_name, msg->msg_namelen, nullptr, nullptr)
|
||||||
: WSASend((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &sent, (DWORD)flags, nullptr, nullptr);
|
: WSASend((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &sent, (DWORD)flags, nullptr, nullptr);
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
if (rc == SOCKET_ERROR) {
|
||||||
@@ -105,7 +105,7 @@ extern "C" ssize_t recvmsg(int fd, struct msghdr *msg, int flags)
|
|||||||
int namelen = (int)msg->msg_namelen;
|
int namelen = (int)msg->msg_namelen;
|
||||||
const int rc = (msg->msg_name && msg->msg_namelen > 0)
|
const int rc = (msg->msg_name && msg->msg_namelen > 0)
|
||||||
? WSARecvFrom((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &received, &recv_flags,
|
? WSARecvFrom((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &received, &recv_flags,
|
||||||
(sockaddr *)msg->msg_name, &namelen, nullptr, nullptr)
|
(sockaddr *)msg->msg_name, &namelen, nullptr, nullptr)
|
||||||
: WSARecv((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &received, &recv_flags, nullptr, nullptr);
|
: WSARecv((SOCKET)fd, bufs.data(), (DWORD)bufs.size(), &received, &recv_flags, nullptr, nullptr);
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
if (rc == SOCKET_ERROR) {
|
||||||
|
|||||||
@@ -59,156 +59,179 @@ static inline void set_wsa_errno()
|
|||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
||||||
SOCKET WSAAPI px4_windows_socket(int af, int type, int protocol)
|
SOCKET WSAAPI px4_windows_socket(int af, int type, int protocol)
|
||||||
{
|
{
|
||||||
const SOCKET s = socket(af, type, protocol);
|
const SOCKET s = socket(af, type, protocol);
|
||||||
|
|
||||||
if (s == INVALID_SOCKET) {
|
if (s == INVALID_SOCKET) {
|
||||||
set_wsa_errno();
|
set_wsa_errno();
|
||||||
|
}
|
||||||
|
|
||||||
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
return s;
|
int WSAAPI px4_windows_bind(SOCKET s, const struct sockaddr *name, int namelen)
|
||||||
}
|
{
|
||||||
|
const int rc = bind(s, name, namelen);
|
||||||
|
|
||||||
int WSAAPI px4_windows_bind(SOCKET s, const struct sockaddr *name, int namelen)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = bind(s, name, namelen);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_listen(SOCKET s, int backlog)
|
||||||
}
|
{
|
||||||
|
const int rc = listen(s, backlog);
|
||||||
|
|
||||||
int WSAAPI px4_windows_listen(SOCKET s, int backlog)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = listen(s, backlog);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
SOCKET WSAAPI px4_windows_accept(SOCKET s, struct sockaddr *addr, int *addrlen)
|
||||||
}
|
{
|
||||||
|
const SOCKET r = accept(s, addr, addrlen);
|
||||||
|
|
||||||
SOCKET WSAAPI px4_windows_accept(SOCKET s, struct sockaddr *addr, int *addrlen)
|
if (r == INVALID_SOCKET) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const SOCKET r = accept(s, addr, addrlen);
|
}
|
||||||
|
|
||||||
if (r == INVALID_SOCKET) {
|
return r;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return r;
|
int WSAAPI px4_windows_connect(SOCKET s, const struct sockaddr *name, int namelen)
|
||||||
}
|
{
|
||||||
|
const int rc = connect(s, name, namelen);
|
||||||
|
|
||||||
int WSAAPI px4_windows_connect(SOCKET s, const struct sockaddr *name, int namelen)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = connect(s, name, namelen);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_setsockopt(SOCKET s, int level, int optname, const char *optval, int optlen)
|
||||||
}
|
{
|
||||||
|
const int rc = setsockopt(s, level, optname, optval, optlen);
|
||||||
|
|
||||||
int WSAAPI px4_windows_setsockopt(SOCKET s, int level, int optname, const char *optval, int optlen)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = setsockopt(s, level, optname, optval, optlen);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_shutdown(SOCKET s, int how)
|
||||||
}
|
{
|
||||||
|
const int rc = shutdown(s, how);
|
||||||
|
|
||||||
int WSAAPI px4_windows_shutdown(SOCKET s, int how)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = shutdown(s, how);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_recv(SOCKET s, char *buf, int len, int flags)
|
||||||
}
|
{
|
||||||
|
const int rc = recv(s, buf, len, flags);
|
||||||
|
|
||||||
int WSAAPI px4_windows_recv(SOCKET s, char *buf, int len, int flags)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = recv(s, buf, len, flags);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_send(SOCKET s, const char *buf, int len, int flags)
|
||||||
}
|
{
|
||||||
|
const int rc = send(s, buf, len, flags);
|
||||||
|
|
||||||
int WSAAPI px4_windows_send(SOCKET s, const char *buf, int len, int flags)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = send(s, buf, len, flags);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_recvfrom(SOCKET s, char *buf, int len, int flags, struct sockaddr *from, int *fromlen)
|
||||||
}
|
{
|
||||||
|
const int rc = recvfrom(s, buf, len, flags, from, fromlen);
|
||||||
|
|
||||||
int WSAAPI px4_windows_recvfrom(SOCKET s, char *buf, int len, int flags, struct sockaddr *from, int *fromlen)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = recvfrom(s, buf, len, flags, from, fromlen);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
int WSAAPI px4_windows_sendto(SOCKET s, const char *buf, int len, int flags, const struct sockaddr *to, int tolen)
|
||||||
}
|
{
|
||||||
|
const int rc = sendto(s, buf, len, flags, to, tolen);
|
||||||
|
|
||||||
int WSAAPI px4_windows_sendto(SOCKET s, const char *buf, int len, int flags, const struct sockaddr *to, int tolen)
|
if (rc == SOCKET_ERROR) {
|
||||||
{
|
set_wsa_errno();
|
||||||
const int rc = sendto(s, buf, len, flags, to, tolen);
|
}
|
||||||
|
|
||||||
if (rc == SOCKET_ERROR) {
|
return rc;
|
||||||
set_wsa_errno();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return rc;
|
char *px4_windows_strerror(int e)
|
||||||
}
|
{
|
||||||
|
switch (e) {
|
||||||
|
case EADDRINUSE: return const_cast<char *>("Address already in use");
|
||||||
|
|
||||||
char *px4_windows_strerror(int e)
|
case EADDRNOTAVAIL: return const_cast<char *>("Address not available");
|
||||||
{
|
|
||||||
switch (e) {
|
case EAFNOSUPPORT: return const_cast<char *>("Address family not supported");
|
||||||
case EADDRINUSE: return const_cast<char *>("Address already in use");
|
|
||||||
case EADDRNOTAVAIL: return const_cast<char *>("Address not available");
|
case ENOTSOCK: return const_cast<char *>("Not a socket");
|
||||||
case EAFNOSUPPORT: return const_cast<char *>("Address family not supported");
|
|
||||||
case ENOTSOCK: return const_cast<char *>("Not a socket");
|
case EDESTADDRREQ: return const_cast<char *>("Destination address required");
|
||||||
case EDESTADDRREQ: return const_cast<char *>("Destination address required");
|
|
||||||
case EMSGSIZE: return const_cast<char *>("Message too long");
|
case EMSGSIZE: return const_cast<char *>("Message too long");
|
||||||
case EPROTOTYPE: return const_cast<char *>("Protocol wrong type for socket");
|
|
||||||
case ENOPROTOOPT: return const_cast<char *>("Protocol option not available");
|
case EPROTOTYPE: return const_cast<char *>("Protocol wrong type for socket");
|
||||||
case EPROTONOSUPPORT: return const_cast<char *>("Protocol not supported");
|
|
||||||
case EOPNOTSUPP: return const_cast<char *>("Operation not supported");
|
case ENOPROTOOPT: return const_cast<char *>("Protocol option not available");
|
||||||
case ENETDOWN: return const_cast<char *>("Network is down");
|
|
||||||
case ENETUNREACH: return const_cast<char *>("Network unreachable");
|
case EPROTONOSUPPORT: return const_cast<char *>("Protocol not supported");
|
||||||
case ENETRESET: return const_cast<char *>("Connection aborted by network");
|
|
||||||
case ECONNABORTED: return const_cast<char *>("Connection aborted");
|
case EOPNOTSUPP: return const_cast<char *>("Operation not supported");
|
||||||
case ECONNRESET: return const_cast<char *>("Connection reset by peer");
|
|
||||||
case ENOBUFS: return const_cast<char *>("No buffer space available");
|
case ENETDOWN: return const_cast<char *>("Network is down");
|
||||||
case EISCONN: return const_cast<char *>("Already connected");
|
|
||||||
case ENOTCONN: return const_cast<char *>("Not connected");
|
case ENETUNREACH: return const_cast<char *>("Network unreachable");
|
||||||
case ETIMEDOUT: return const_cast<char *>("Connection timed out");
|
|
||||||
case ECONNREFUSED: return const_cast<char *>("Connection refused");
|
case ENETRESET: return const_cast<char *>("Connection aborted by network");
|
||||||
case EALREADY: return const_cast<char *>("Operation already in progress");
|
|
||||||
case EINPROGRESS: return const_cast<char *>("Operation in progress");
|
case ECONNABORTED: return const_cast<char *>("Connection aborted");
|
||||||
case EWOULDBLOCK: return const_cast<char *>("Resource temporarily unavailable");
|
|
||||||
default: return strerror(e);
|
case ECONNRESET: return const_cast<char *>("Connection reset by peer");
|
||||||
|
|
||||||
|
case ENOBUFS: return const_cast<char *>("No buffer space available");
|
||||||
|
|
||||||
|
case EISCONN: return const_cast<char *>("Already connected");
|
||||||
|
|
||||||
|
case ENOTCONN: return const_cast<char *>("Not connected");
|
||||||
|
|
||||||
|
case ETIMEDOUT: return const_cast<char *>("Connection timed out");
|
||||||
|
|
||||||
|
case ECONNREFUSED: return const_cast<char *>("Connection refused");
|
||||||
|
|
||||||
|
case EALREADY: return const_cast<char *>("Operation already in progress");
|
||||||
|
|
||||||
|
case EINPROGRESS: return const_cast<char *>("Operation in progress");
|
||||||
|
|
||||||
|
case EWOULDBLOCK: return const_cast<char *>("Resource temporarily unavailable");
|
||||||
|
|
||||||
|
default: return strerror(e);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
} // extern "C"
|
} // extern "C"
|
||||||
|
|
||||||
|
|||||||
@@ -213,28 +213,51 @@ extern "C" {
|
|||||||
{
|
{
|
||||||
switch (e) {
|
switch (e) {
|
||||||
case EADDRINUSE: return const_cast<char *>("Address already in use");
|
case EADDRINUSE: return const_cast<char *>("Address already in use");
|
||||||
|
|
||||||
case EADDRNOTAVAIL: return const_cast<char *>("Address not available");
|
case EADDRNOTAVAIL: return const_cast<char *>("Address not available");
|
||||||
|
|
||||||
case EAFNOSUPPORT: return const_cast<char *>("Address family not supported");
|
case EAFNOSUPPORT: return const_cast<char *>("Address family not supported");
|
||||||
|
|
||||||
case ENOTSOCK: return const_cast<char *>("Not a socket");
|
case ENOTSOCK: return const_cast<char *>("Not a socket");
|
||||||
|
|
||||||
case EDESTADDRREQ: return const_cast<char *>("Destination address required");
|
case EDESTADDRREQ: return const_cast<char *>("Destination address required");
|
||||||
|
|
||||||
case EMSGSIZE: return const_cast<char *>("Message too long");
|
case EMSGSIZE: return const_cast<char *>("Message too long");
|
||||||
|
|
||||||
case EPROTOTYPE: return const_cast<char *>("Protocol wrong type for socket");
|
case EPROTOTYPE: return const_cast<char *>("Protocol wrong type for socket");
|
||||||
|
|
||||||
case ENOPROTOOPT: return const_cast<char *>("Protocol option not available");
|
case ENOPROTOOPT: return const_cast<char *>("Protocol option not available");
|
||||||
|
|
||||||
case EPROTONOSUPPORT: return const_cast<char *>("Protocol not supported");
|
case EPROTONOSUPPORT: return const_cast<char *>("Protocol not supported");
|
||||||
|
|
||||||
case EOPNOTSUPP: return const_cast<char *>("Operation not supported");
|
case EOPNOTSUPP: return const_cast<char *>("Operation not supported");
|
||||||
|
|
||||||
case ENETDOWN: return const_cast<char *>("Network is down");
|
case ENETDOWN: return const_cast<char *>("Network is down");
|
||||||
|
|
||||||
case ENETUNREACH: return const_cast<char *>("Network unreachable");
|
case ENETUNREACH: return const_cast<char *>("Network unreachable");
|
||||||
|
|
||||||
case ENETRESET: return const_cast<char *>("Connection aborted by network");
|
case ENETRESET: return const_cast<char *>("Connection aborted by network");
|
||||||
|
|
||||||
case ECONNABORTED: return const_cast<char *>("Connection aborted");
|
case ECONNABORTED: return const_cast<char *>("Connection aborted");
|
||||||
|
|
||||||
case ECONNRESET: return const_cast<char *>("Connection reset by peer");
|
case ECONNRESET: return const_cast<char *>("Connection reset by peer");
|
||||||
|
|
||||||
case ENOBUFS: return const_cast<char *>("No buffer space available");
|
case ENOBUFS: return const_cast<char *>("No buffer space available");
|
||||||
|
|
||||||
case EISCONN: return const_cast<char *>("Already connected");
|
case EISCONN: return const_cast<char *>("Already connected");
|
||||||
|
|
||||||
case ENOTCONN: return const_cast<char *>("Not connected");
|
case ENOTCONN: return const_cast<char *>("Not connected");
|
||||||
|
|
||||||
case ETIMEDOUT: return const_cast<char *>("Connection timed out");
|
case ETIMEDOUT: return const_cast<char *>("Connection timed out");
|
||||||
|
|
||||||
case ECONNREFUSED: return const_cast<char *>("Connection refused");
|
case ECONNREFUSED: return const_cast<char *>("Connection refused");
|
||||||
|
|
||||||
case EALREADY: return const_cast<char *>("Operation already in progress");
|
case EALREADY: return const_cast<char *>("Operation already in progress");
|
||||||
|
|
||||||
case EINPROGRESS: return const_cast<char *>("Operation in progress");
|
case EINPROGRESS: return const_cast<char *>("Operation in progress");
|
||||||
|
|
||||||
case EWOULDBLOCK: return const_cast<char *>("Resource temporarily unavailable");
|
case EWOULDBLOCK: return const_cast<char *>("Resource temporarily unavailable");
|
||||||
|
|
||||||
default: return __real_strerror(e);
|
default: return __real_strerror(e);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
@@ -108,6 +108,7 @@ static int align_buf(char **cursor, size_t *remaining, size_t alignment)
|
|||||||
static struct passwd *fill_passwd()
|
static struct passwd *fill_passwd()
|
||||||
{
|
{
|
||||||
DWORD n = sizeof(_pw_name);
|
DWORD n = sizeof(_pw_name);
|
||||||
|
|
||||||
if (!GetUserNameA(_pw_name, &n)) {
|
if (!GetUserNameA(_pw_name, &n)) {
|
||||||
/* GetUserNameA failed; fall back to a stable placeholder. Use a bounded
|
/* GetUserNameA failed; fall back to a stable placeholder. Use a bounded
|
||||||
* copy rather than strcpy() so MSVC does not flag the deprecated CRT
|
* copy rather than strcpy() so MSVC does not flag the deprecated CRT
|
||||||
@@ -115,8 +116,11 @@ static struct passwd *fill_passwd()
|
|||||||
strncpy(_pw_name, "px4", sizeof(_pw_name) - 1);
|
strncpy(_pw_name, "px4", sizeof(_pw_name) - 1);
|
||||||
_pw_name[sizeof(_pw_name) - 1] = '\0';
|
_pw_name[sizeof(_pw_name) - 1] = '\0';
|
||||||
}
|
}
|
||||||
|
|
||||||
const char *home = getenv("USERPROFILE");
|
const char *home = getenv("USERPROFILE");
|
||||||
|
|
||||||
if (!home) { home = "C:\\"; }
|
if (!home) { home = "C:\\"; }
|
||||||
|
|
||||||
strncpy(_pw_dir, home, sizeof(_pw_dir) - 1);
|
strncpy(_pw_dir, home, sizeof(_pw_dir) - 1);
|
||||||
_pw_dir[sizeof(_pw_dir) - 1] = 0;
|
_pw_dir[sizeof(_pw_dir) - 1] = 0;
|
||||||
|
|
||||||
@@ -153,19 +157,31 @@ static int fill_passwd_r(struct passwd *pwd, char *buf, size_t buflen, struct pa
|
|||||||
}
|
}
|
||||||
|
|
||||||
struct passwd *src = fill_passwd();
|
struct passwd *src = fill_passwd();
|
||||||
|
|
||||||
char *cursor = buf;
|
char *cursor = buf;
|
||||||
|
|
||||||
size_t remaining = buflen;
|
size_t remaining = buflen;
|
||||||
|
|
||||||
int rc = 0;
|
int rc = 0;
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->pw_name, &pwd->pw_name);
|
rc = append_buf_string(&cursor, &remaining, src->pw_name, &pwd->pw_name);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->pw_passwd, &pwd->pw_passwd);
|
rc = append_buf_string(&cursor, &remaining, src->pw_passwd, &pwd->pw_passwd);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->pw_gecos, &pwd->pw_gecos);
|
rc = append_buf_string(&cursor, &remaining, src->pw_gecos, &pwd->pw_gecos);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->pw_dir, &pwd->pw_dir);
|
rc = append_buf_string(&cursor, &remaining, src->pw_dir, &pwd->pw_dir);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->pw_shell, &pwd->pw_shell);
|
rc = append_buf_string(&cursor, &remaining, src->pw_shell, &pwd->pw_shell);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
pwd->pw_uid = src->pw_uid;
|
pwd->pw_uid = src->pw_uid;
|
||||||
@@ -182,11 +198,15 @@ static int fill_group_r(struct group *grp, char *buf, size_t buflen, struct grou
|
|||||||
}
|
}
|
||||||
|
|
||||||
struct group *src = fill_group();
|
struct group *src = fill_group();
|
||||||
|
|
||||||
char *cursor = buf;
|
char *cursor = buf;
|
||||||
|
|
||||||
size_t remaining = buflen;
|
size_t remaining = buflen;
|
||||||
|
|
||||||
int rc = 0;
|
int rc = 0;
|
||||||
|
|
||||||
rc = align_buf(&cursor, &remaining, alignof(char *));
|
rc = align_buf(&cursor, &remaining, alignof(char *));
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
if (remaining < sizeof(char *) * 2) {
|
if (remaining < sizeof(char *) * 2) {
|
||||||
@@ -200,10 +220,15 @@ static int fill_group_r(struct group *grp, char *buf, size_t buflen, struct grou
|
|||||||
remaining -= sizeof(char *) * 2;
|
remaining -= sizeof(char *) * 2;
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->gr_name, &grp->gr_name);
|
rc = append_buf_string(&cursor, &remaining, src->gr_name, &grp->gr_name);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->gr_passwd, &grp->gr_passwd);
|
rc = append_buf_string(&cursor, &remaining, src->gr_passwd, &grp->gr_passwd);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
rc = append_buf_string(&cursor, &remaining, src->gr_mem[0], &grp->gr_mem[0]);
|
rc = append_buf_string(&cursor, &remaining, src->gr_mem[0], &grp->gr_mem[0]);
|
||||||
|
|
||||||
if (rc != 0) { *result = nullptr; return rc; }
|
if (rc != 0) { *result = nullptr; return rc; }
|
||||||
|
|
||||||
grp->gr_mem[1] = nullptr;
|
grp->gr_mem[1] = nullptr;
|
||||||
|
|||||||
@@ -52,33 +52,44 @@ int px4_win_error_to_errno(DWORD err)
|
|||||||
case ERROR_INVALID_DRIVE:
|
case ERROR_INVALID_DRIVE:
|
||||||
case ERROR_BAD_PATHNAME:
|
case ERROR_BAD_PATHNAME:
|
||||||
return ENOENT;
|
return ENOENT;
|
||||||
|
|
||||||
case ERROR_ACCESS_DENIED:
|
case ERROR_ACCESS_DENIED:
|
||||||
case ERROR_SHARING_VIOLATION:
|
case ERROR_SHARING_VIOLATION:
|
||||||
case ERROR_LOCK_VIOLATION:
|
case ERROR_LOCK_VIOLATION:
|
||||||
return EACCES;
|
return EACCES;
|
||||||
|
|
||||||
case ERROR_ALREADY_EXISTS:
|
case ERROR_ALREADY_EXISTS:
|
||||||
case ERROR_FILE_EXISTS:
|
case ERROR_FILE_EXISTS:
|
||||||
return EEXIST;
|
return EEXIST;
|
||||||
|
|
||||||
case ERROR_INVALID_HANDLE:
|
case ERROR_INVALID_HANDLE:
|
||||||
return EBADF;
|
return EBADF;
|
||||||
|
|
||||||
case ERROR_NOT_ENOUGH_MEMORY:
|
case ERROR_NOT_ENOUGH_MEMORY:
|
||||||
case ERROR_OUTOFMEMORY:
|
case ERROR_OUTOFMEMORY:
|
||||||
return ENOMEM;
|
return ENOMEM;
|
||||||
|
|
||||||
case ERROR_DIRECTORY:
|
case ERROR_DIRECTORY:
|
||||||
case ERROR_INVALID_NAME:
|
case ERROR_INVALID_NAME:
|
||||||
case ERROR_INVALID_PARAMETER:
|
case ERROR_INVALID_PARAMETER:
|
||||||
return EINVAL;
|
return EINVAL;
|
||||||
|
|
||||||
case ERROR_DIR_NOT_EMPTY:
|
case ERROR_DIR_NOT_EMPTY:
|
||||||
return ENOTEMPTY;
|
return ENOTEMPTY;
|
||||||
|
|
||||||
case ERROR_NOT_SUPPORTED:
|
case ERROR_NOT_SUPPORTED:
|
||||||
case ERROR_CALL_NOT_IMPLEMENTED:
|
case ERROR_CALL_NOT_IMPLEMENTED:
|
||||||
return ENOTSUP;
|
return ENOTSUP;
|
||||||
|
|
||||||
case ERROR_BUSY:
|
case ERROR_BUSY:
|
||||||
return EBUSY;
|
return EBUSY;
|
||||||
|
|
||||||
case ERROR_DISK_FULL:
|
case ERROR_DISK_FULL:
|
||||||
return ENOSPC;
|
return ENOSPC;
|
||||||
|
|
||||||
case ERROR_PROC_NOT_FOUND:
|
case ERROR_PROC_NOT_FOUND:
|
||||||
return ESRCH;
|
return ESRCH;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
/* EIO is the safest "something at the OS boundary failed" fallback. */
|
/* EIO is the safest "something at the OS boundary failed" fallback. */
|
||||||
return EIO;
|
return EIO;
|
||||||
@@ -89,28 +100,51 @@ int px4_wsa_error_to_errno(int err)
|
|||||||
{
|
{
|
||||||
switch (err) {
|
switch (err) {
|
||||||
case WSAEWOULDBLOCK: return EWOULDBLOCK;
|
case WSAEWOULDBLOCK: return EWOULDBLOCK;
|
||||||
|
|
||||||
case WSAEINPROGRESS: return EINPROGRESS;
|
case WSAEINPROGRESS: return EINPROGRESS;
|
||||||
|
|
||||||
case WSAEALREADY: return EALREADY;
|
case WSAEALREADY: return EALREADY;
|
||||||
|
|
||||||
case WSAENOTSOCK: return ENOTSOCK;
|
case WSAENOTSOCK: return ENOTSOCK;
|
||||||
|
|
||||||
case WSAEDESTADDRREQ: return EDESTADDRREQ;
|
case WSAEDESTADDRREQ: return EDESTADDRREQ;
|
||||||
|
|
||||||
case WSAEMSGSIZE: return EMSGSIZE;
|
case WSAEMSGSIZE: return EMSGSIZE;
|
||||||
|
|
||||||
case WSAEPROTOTYPE: return EPROTOTYPE;
|
case WSAEPROTOTYPE: return EPROTOTYPE;
|
||||||
|
|
||||||
case WSAENOPROTOOPT: return ENOPROTOOPT;
|
case WSAENOPROTOOPT: return ENOPROTOOPT;
|
||||||
|
|
||||||
case WSAEPROTONOSUPPORT: return EPROTONOSUPPORT;
|
case WSAEPROTONOSUPPORT: return EPROTONOSUPPORT;
|
||||||
|
|
||||||
case WSAEOPNOTSUPP: return EOPNOTSUPP;
|
case WSAEOPNOTSUPP: return EOPNOTSUPP;
|
||||||
|
|
||||||
case WSAEAFNOSUPPORT: return EAFNOSUPPORT;
|
case WSAEAFNOSUPPORT: return EAFNOSUPPORT;
|
||||||
|
|
||||||
case WSAEADDRINUSE: return EADDRINUSE;
|
case WSAEADDRINUSE: return EADDRINUSE;
|
||||||
|
|
||||||
case WSAEADDRNOTAVAIL: return EADDRNOTAVAIL;
|
case WSAEADDRNOTAVAIL: return EADDRNOTAVAIL;
|
||||||
|
|
||||||
case WSAENETDOWN: return ENETDOWN;
|
case WSAENETDOWN: return ENETDOWN;
|
||||||
|
|
||||||
case WSAENETUNREACH: return ENETUNREACH;
|
case WSAENETUNREACH: return ENETUNREACH;
|
||||||
|
|
||||||
case WSAENETRESET: return ENETRESET;
|
case WSAENETRESET: return ENETRESET;
|
||||||
|
|
||||||
case WSAECONNABORTED: return ECONNABORTED;
|
case WSAECONNABORTED: return ECONNABORTED;
|
||||||
|
|
||||||
case WSAECONNRESET: return ECONNRESET;
|
case WSAECONNRESET: return ECONNRESET;
|
||||||
|
|
||||||
case WSAENOBUFS: return ENOBUFS;
|
case WSAENOBUFS: return ENOBUFS;
|
||||||
|
|
||||||
case WSAEISCONN: return EISCONN;
|
case WSAEISCONN: return EISCONN;
|
||||||
|
|
||||||
case WSAENOTCONN: return ENOTCONN;
|
case WSAENOTCONN: return ENOTCONN;
|
||||||
|
|
||||||
case WSAETIMEDOUT: return ETIMEDOUT;
|
case WSAETIMEDOUT: return ETIMEDOUT;
|
||||||
|
|
||||||
case WSAECONNREFUSED: return ECONNREFUSED;
|
case WSAECONNREFUSED: return ECONNREFUSED;
|
||||||
|
|
||||||
default: return EIO;
|
default: return EIO;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -119,9 +153,13 @@ const char *px4_hstrerror_text(int err)
|
|||||||
{
|
{
|
||||||
switch (err) {
|
switch (err) {
|
||||||
case HOST_NOT_FOUND: return "Unknown host";
|
case HOST_NOT_FOUND: return "Unknown host";
|
||||||
|
|
||||||
case TRY_AGAIN: return "Temporary failure in name resolution";
|
case TRY_AGAIN: return "Temporary failure in name resolution";
|
||||||
|
|
||||||
case NO_RECOVERY: return "Non-recoverable name server error";
|
case NO_RECOVERY: return "Non-recoverable name server error";
|
||||||
|
|
||||||
case NO_DATA: return "No address associated with name";
|
case NO_DATA: return "No address associated with name";
|
||||||
|
|
||||||
default: return "Resolver error";
|
default: return "Resolver error";
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -61,9 +61,12 @@ extern "C" int ioctl(int fd, unsigned long request, ...)
|
|||||||
/* Winsock uses ioctlsocket for the socket readiness/non-blocking calls
|
/* Winsock uses ioctlsocket for the socket readiness/non-blocking calls
|
||||||
* that POSIX code normally sends through ioctl(). */
|
* that POSIX code normally sends through ioctl(). */
|
||||||
int rc = ioctlsocket((SOCKET)fd, (long)request, &v);
|
int rc = ioctlsocket((SOCKET)fd, (long)request, &v);
|
||||||
|
|
||||||
if (rc == 0 && arg) { *(unsigned long *)arg = v; }
|
if (rc == 0 && arg) { *(unsigned long *)arg = v; }
|
||||||
|
|
||||||
return rc == 0 ? 0 : -1;
|
return rc == 0 ? 0 : -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
errno = ENOSYS;
|
errno = ENOSYS;
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -52,9 +52,11 @@
|
|||||||
extern "C" int statfs(const char *path, struct statfs *buf)
|
extern "C" int statfs(const char *path, struct statfs *buf)
|
||||||
{
|
{
|
||||||
if (!path || !buf) { errno = EINVAL; return -1; }
|
if (!path || !buf) { errno = EINVAL; return -1; }
|
||||||
|
|
||||||
memset(buf, 0, sizeof(*buf));
|
memset(buf, 0, sizeof(*buf));
|
||||||
|
|
||||||
ULARGE_INTEGER free_caller, total_bytes, free_total;
|
ULARGE_INTEGER free_caller, total_bytes, free_total;
|
||||||
|
|
||||||
if (!GetDiskFreeSpaceExA(path, &free_caller, &total_bytes, &free_total)) {
|
if (!GetDiskFreeSpaceExA(path, &free_caller, &total_bytes, &free_total)) {
|
||||||
errno = ENOENT;
|
errno = ENOENT;
|
||||||
return -1;
|
return -1;
|
||||||
@@ -87,9 +89,11 @@ static BOOL copy_dir_recursive(const char *src, const char *dst)
|
|||||||
|
|
||||||
WIN32_FIND_DATAA fd;
|
WIN32_FIND_DATAA fd;
|
||||||
HANDLE h = FindFirstFileA(pattern, &fd);
|
HANDLE h = FindFirstFileA(pattern, &fd);
|
||||||
|
|
||||||
if (h == INVALID_HANDLE_VALUE) { return TRUE; /* empty dir is fine */ }
|
if (h == INVALID_HANDLE_VALUE) { return TRUE; /* empty dir is fine */ }
|
||||||
|
|
||||||
BOOL ok = TRUE;
|
BOOL ok = TRUE;
|
||||||
|
|
||||||
do {
|
do {
|
||||||
if (strcmp(fd.cFileName, ".") == 0 || strcmp(fd.cFileName, "..") == 0) { continue; }
|
if (strcmp(fd.cFileName, ".") == 0 || strcmp(fd.cFileName, "..") == 0) { continue; }
|
||||||
|
|
||||||
@@ -99,6 +103,7 @@ static BOOL copy_dir_recursive(const char *src, const char *dst)
|
|||||||
|
|
||||||
if (fd.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) {
|
if (fd.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) {
|
||||||
if (!copy_dir_recursive(s, d)) { ok = FALSE; break; }
|
if (!copy_dir_recursive(s, d)) { ok = FALSE; break; }
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (!CopyFileA(s, d, FALSE)) { ok = FALSE; break; }
|
if (!CopyFileA(s, d, FALSE)) { ok = FALSE; break; }
|
||||||
}
|
}
|
||||||
@@ -119,12 +124,15 @@ extern "C" int symlink(const char *target, const char *linkpath)
|
|||||||
const bool is_dir = (attrs != INVALID_FILE_ATTRIBUTES) && (attrs & FILE_ATTRIBUTE_DIRECTORY);
|
const bool is_dir = (attrs != INVALID_FILE_ATTRIBUTES) && (attrs & FILE_ATTRIBUTE_DIRECTORY);
|
||||||
|
|
||||||
DWORD flags = 0;
|
DWORD flags = 0;
|
||||||
|
|
||||||
if (is_dir) { flags |= 0x1; /* SYMBOLIC_LINK_FLAG_DIRECTORY */ }
|
if (is_dir) { flags |= 0x1; /* SYMBOLIC_LINK_FLAG_DIRECTORY */ }
|
||||||
|
|
||||||
/* Allow creating symlinks without admin rights when Developer Mode
|
/* Allow creating symlinks without admin rights when Developer Mode
|
||||||
* is enabled (Win10 1703+). Harmless on older Windows. */
|
* is enabled (Win10 1703+). Harmless on older Windows. */
|
||||||
flags |= 0x2; /* SYMBOLIC_LINK_FLAG_ALLOW_UNPRIVILEGED_CREATE */
|
flags |= 0x2; /* SYMBOLIC_LINK_FLAG_ALLOW_UNPRIVILEGED_CREATE */
|
||||||
|
|
||||||
BOOL sym_ok = CreateSymbolicLinkA(linkpath, target, flags);
|
BOOL sym_ok = CreateSymbolicLinkA(linkpath, target, flags);
|
||||||
|
|
||||||
/* Verify the link is actually accessible. Wine's CreateSymbolicLinkA
|
/* Verify the link is actually accessible. Wine's CreateSymbolicLinkA
|
||||||
* can report success and leave nothing behind. */
|
* can report success and leave nothing behind. */
|
||||||
if (sym_ok && GetFileAttributesA(linkpath) != INVALID_FILE_ATTRIBUTES) {
|
if (sym_ok && GetFileAttributesA(linkpath) != INVALID_FILE_ATTRIBUTES) {
|
||||||
@@ -137,7 +145,9 @@ extern "C" int symlink(const char *target, const char *linkpath)
|
|||||||
if (is_dir) {
|
if (is_dir) {
|
||||||
return copy_dir_recursive(target, linkpath) ? 0 : (errno = EPERM, -1);
|
return copy_dir_recursive(target, linkpath) ? 0 : (errno = EPERM, -1);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (CreateHardLinkA(linkpath, target, nullptr)) { return 0; }
|
if (CreateHardLinkA(linkpath, target, nullptr)) { return 0; }
|
||||||
|
|
||||||
if (CopyFileA(target, linkpath, FALSE)) { return 0; }
|
if (CopyFileA(target, linkpath, FALSE)) { return 0; }
|
||||||
|
|
||||||
errno = EPERM;
|
errno = EPERM;
|
||||||
@@ -244,47 +254,55 @@ extern "C" long sysconf(int name)
|
|||||||
/* Numeric constants match the values declared in windows_shim/unistd.h.
|
/* Numeric constants match the values declared in windows_shim/unistd.h.
|
||||||
* Keep this switch synchronized with that header. */
|
* Keep this switch synchronized with that header. */
|
||||||
switch (name) {
|
switch (name) {
|
||||||
case 30: /* _SC_PAGESIZE */ {
|
case 30: { /* _SC_PAGESIZE */
|
||||||
SYSTEM_INFO si;
|
SYSTEM_INFO si;
|
||||||
GetSystemInfo(&si);
|
GetSystemInfo(&si);
|
||||||
return (long)si.dwPageSize;
|
return (long)si.dwPageSize;
|
||||||
}
|
}
|
||||||
|
|
||||||
case 2: /* _SC_CLK_TCK: Win32 QueryPerformanceFrequency is Hz */
|
case 2: /* _SC_CLK_TCK: Win32 QueryPerformanceFrequency is Hz */
|
||||||
/* Use 100Hz, matching what Windows' multimedia timers resolve
|
/* Use 100Hz, matching what Windows' multimedia timers resolve
|
||||||
* to by default. Drivers that need higher resolution use
|
* to by default. Drivers that need higher resolution use
|
||||||
* hrt_absolute_time() which goes through QPC directly. */
|
* hrt_absolute_time() which goes through QPC directly. */
|
||||||
return 100;
|
return 100;
|
||||||
case 84: /* _SC_NPROCESSORS_ONLN */ {
|
|
||||||
SYSTEM_INFO si;
|
|
||||||
GetSystemInfo(&si);
|
|
||||||
return (long)si.dwNumberOfProcessors;
|
|
||||||
}
|
|
||||||
case 83: /* _SC_NPROCESSORS_CONF */ {
|
|
||||||
SYSTEM_INFO si;
|
|
||||||
GetSystemInfo(&si);
|
|
||||||
return (long)si.dwNumberOfProcessors;
|
|
||||||
}
|
|
||||||
case 4: /* _SC_OPEN_MAX */
|
|
||||||
return (long)_getmaxstdio();
|
|
||||||
case 180: /* _SC_HOST_NAME_MAX */
|
|
||||||
return (long)(MAX_COMPUTERNAME_LENGTH + 1);
|
|
||||||
case 71: /* _SC_LOGIN_NAME_MAX */
|
|
||||||
return 256;
|
|
||||||
case 85: /* _SC_PHYS_PAGES */
|
|
||||||
case 86: { /* _SC_AVPHYS_PAGES */
|
|
||||||
MEMORYSTATUSEX mem {};
|
|
||||||
mem.dwLength = sizeof(mem);
|
|
||||||
|
|
||||||
if (!GlobalMemoryStatusEx(&mem)) {
|
case 84: { /* _SC_NPROCESSORS_ONLN */
|
||||||
errno = EIO;
|
SYSTEM_INFO si;
|
||||||
return -1;
|
GetSystemInfo(&si);
|
||||||
|
return (long)si.dwNumberOfProcessors;
|
||||||
}
|
}
|
||||||
|
|
||||||
const ULONGLONG page_size = (ULONGLONG)getpagesize();
|
case 83: { /* _SC_NPROCESSORS_CONF */
|
||||||
const ULONGLONG bytes = (name == 85) ? mem.ullTotalPhys : mem.ullAvailPhys;
|
SYSTEM_INFO si;
|
||||||
return (long)(bytes / page_size);
|
GetSystemInfo(&si);
|
||||||
}
|
return (long)si.dwNumberOfProcessors;
|
||||||
|
}
|
||||||
|
|
||||||
|
case 4: /* _SC_OPEN_MAX */
|
||||||
|
return (long)_getmaxstdio();
|
||||||
|
|
||||||
|
case 180: /* _SC_HOST_NAME_MAX */
|
||||||
|
return (long)(MAX_COMPUTERNAME_LENGTH + 1);
|
||||||
|
|
||||||
|
case 71: /* _SC_LOGIN_NAME_MAX */
|
||||||
|
return 256;
|
||||||
|
|
||||||
|
case 85: /* _SC_PHYS_PAGES */
|
||||||
|
case 86: { /* _SC_AVPHYS_PAGES */
|
||||||
|
MEMORYSTATUSEX mem {};
|
||||||
|
mem.dwLength = sizeof(mem);
|
||||||
|
|
||||||
|
if (!GlobalMemoryStatusEx(&mem)) {
|
||||||
|
errno = EIO;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
const ULONGLONG page_size = (ULONGLONG)getpagesize();
|
||||||
|
const ULONGLONG bytes = (name == 85) ? mem.ullTotalPhys : mem.ullAvailPhys;
|
||||||
|
return (long)(bytes / page_size);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
errno = EINVAL;
|
errno = EINVAL;
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -55,6 +55,7 @@
|
|||||||
extern "C" int tcgetattr(int, struct termios *t)
|
extern "C" int tcgetattr(int, struct termios *t)
|
||||||
{
|
{
|
||||||
if (!t) { errno = EINVAL; return -1; }
|
if (!t) { errno = EINVAL; return -1; }
|
||||||
|
|
||||||
memset(t, 0, sizeof(*t));
|
memset(t, 0, sizeof(*t));
|
||||||
t->c_cflag = CS8 | CREAD | CLOCAL;
|
t->c_cflag = CS8 | CREAD | CLOCAL;
|
||||||
t->c_ispeed = t->c_ospeed = B115200;
|
t->c_ispeed = t->c_ospeed = B115200;
|
||||||
@@ -74,6 +75,7 @@ extern "C" speed_t cfgetospeed(const struct termios *t) { return t ?
|
|||||||
extern "C" void cfmakeraw(struct termios *t)
|
extern "C" void cfmakeraw(struct termios *t)
|
||||||
{
|
{
|
||||||
if (!t) { return; }
|
if (!t) { return; }
|
||||||
|
|
||||||
/* Match the traditional POSIX cfmakeraw bit clearing so code inspecting the
|
/* Match the traditional POSIX cfmakeraw bit clearing so code inspecting the
|
||||||
* termios struct sees the same shape it would on Linux. */
|
* termios struct sees the same shape it would on Linux. */
|
||||||
t->c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
|
t->c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
|
||||||
|
|||||||
@@ -52,99 +52,100 @@ static constexpr uint64_t filetime_unix_epoch_100ns = 116444736000000000ULL;
|
|||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
|
||||||
int clock_gettime(clockid_t clk_id, struct timespec *tp)
|
int clock_gettime(clockid_t clk_id, struct timespec *tp)
|
||||||
{
|
{
|
||||||
if (!tp) {
|
if (!tp) {
|
||||||
|
errno = EINVAL;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (clk_id == CLOCK_MONOTONIC) {
|
||||||
|
/* QPC is monotonic and high resolution, but relative to an arbitrary
|
||||||
|
* boot-time counter. That is exactly what CLOCK_MONOTONIC promises.
|
||||||
|
*
|
||||||
|
* Per Microsoft's QueryPerformanceCounter guidance, the QPC frequency
|
||||||
|
* is fixed at system boot and consistent across processors, so we
|
||||||
|
* only need to query it once. clock_gettime is on PX4's hot path
|
||||||
|
* (drv_hrt's hrt_absolute_time, lockstep_scheduler, every uORB
|
||||||
|
* publish/subscribe), and a syscall here adds up quickly.
|
||||||
|
*/
|
||||||
|
static const int64_t frequency = []() {
|
||||||
|
LARGE_INTEGER f {};
|
||||||
|
QueryPerformanceFrequency(&f);
|
||||||
|
return f.QuadPart;
|
||||||
|
}();
|
||||||
|
|
||||||
|
LARGE_INTEGER counter {};
|
||||||
|
QueryPerformanceCounter(&counter);
|
||||||
|
|
||||||
|
const uint64_t seconds = static_cast<uint64_t>(counter.QuadPart / frequency);
|
||||||
|
const uint64_t remainder = static_cast<uint64_t>(counter.QuadPart % frequency);
|
||||||
|
tp->tv_sec = static_cast<time_t>(seconds);
|
||||||
|
tp->tv_nsec = static_cast<long>((remainder * 1000000000ULL) / static_cast<uint64_t>(frequency));
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (clk_id == CLOCK_REALTIME) {
|
||||||
|
/* FILETIME counts 100 ns ticks since 1601-01-01 UTC. Convert to the
|
||||||
|
* Unix epoch expected by timespec. */
|
||||||
|
FILETIME ft {};
|
||||||
|
GetSystemTimePreciseAsFileTime(&ft);
|
||||||
|
const uint64_t ticks = (static_cast<uint64_t>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
|
||||||
|
const uint64_t unix_100ns = ticks - filetime_unix_epoch_100ns;
|
||||||
|
tp->tv_sec = static_cast<time_t>(unix_100ns / 10000000ULL);
|
||||||
|
tp->tv_nsec = static_cast<long>((unix_100ns % 10000000ULL) * 100ULL);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
errno = EINVAL;
|
errno = EINVAL;
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (clk_id == CLOCK_MONOTONIC) {
|
int clock_settime(clockid_t clk_id, const struct timespec *tp)
|
||||||
/* QPC is monotonic and high resolution, but relative to an arbitrary
|
{
|
||||||
* boot-time counter. That is exactly what CLOCK_MONOTONIC promises.
|
(void)clk_id;
|
||||||
*
|
(void)tp;
|
||||||
* Per Microsoft's QueryPerformanceCounter guidance, the QPC frequency
|
errno = ENOTSUP;
|
||||||
* is fixed at system boot and consistent across processors, so we
|
return -1;
|
||||||
* only need to query it once. clock_gettime is on PX4's hot path
|
}
|
||||||
* (drv_hrt's hrt_absolute_time, lockstep_scheduler, every uORB
|
|
||||||
* publish/subscribe), and a syscall here adds up quickly.
|
|
||||||
*/
|
|
||||||
static const int64_t frequency = []() {
|
|
||||||
LARGE_INTEGER f {};
|
|
||||||
QueryPerformanceFrequency(&f);
|
|
||||||
return f.QuadPart;
|
|
||||||
}();
|
|
||||||
|
|
||||||
LARGE_INTEGER counter {};
|
int gettimeofday(struct timeval *tv, void *tz)
|
||||||
QueryPerformanceCounter(&counter);
|
{
|
||||||
|
(void)tz;
|
||||||
|
|
||||||
const uint64_t seconds = static_cast<uint64_t>(counter.QuadPart / frequency);
|
if (!tv) {
|
||||||
const uint64_t remainder = static_cast<uint64_t>(counter.QuadPart % frequency);
|
errno = EINVAL;
|
||||||
tp->tv_sec = static_cast<time_t>(seconds);
|
return -1;
|
||||||
tp->tv_nsec = static_cast<long>((remainder * 1000000000ULL) / static_cast<uint64_t>(frequency));
|
}
|
||||||
|
|
||||||
|
timespec ts {};
|
||||||
|
|
||||||
|
if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
tv->tv_sec = static_cast<long>(ts.tv_sec);
|
||||||
|
tv->tv_usec = ts.tv_nsec / 1000;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (clk_id == CLOCK_REALTIME) {
|
int nanosleep(const struct timespec *req, struct timespec *rem)
|
||||||
/* FILETIME counts 100 ns ticks since 1601-01-01 UTC. Convert to the
|
{
|
||||||
* Unix epoch expected by timespec. */
|
(void)rem;
|
||||||
FILETIME ft {};
|
|
||||||
GetSystemTimePreciseAsFileTime(&ft);
|
if (!req || req->tv_nsec < 0 || req->tv_nsec >= 1000000000L) {
|
||||||
const uint64_t ticks = (static_cast<uint64_t>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
|
errno = EINVAL;
|
||||||
const uint64_t unix_100ns = ticks - filetime_unix_epoch_100ns;
|
return -1;
|
||||||
tp->tv_sec = static_cast<time_t>(unix_100ns / 10000000ULL);
|
}
|
||||||
tp->tv_nsec = static_cast<long>((unix_100ns % 10000000ULL) * 100ULL);
|
|
||||||
|
const uint64_t ms = (static_cast<uint64_t>(req->tv_sec) * 1000ULL) +
|
||||||
|
((static_cast<uint64_t>(req->tv_nsec) + 999999ULL) / 1000000ULL);
|
||||||
|
/* Sleep() is millisecond-granular and cannot report remaining time if it is
|
||||||
|
* interrupted, so rem is intentionally unsupported for now. */
|
||||||
|
Sleep(ms > MAXDWORD ? MAXDWORD : static_cast<DWORD>(ms));
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
errno = EINVAL;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
int clock_settime(clockid_t clk_id, const struct timespec *tp)
|
|
||||||
{
|
|
||||||
(void)clk_id;
|
|
||||||
(void)tp;
|
|
||||||
errno = ENOTSUP;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
int gettimeofday(struct timeval *tv, void *tz)
|
|
||||||
{
|
|
||||||
(void)tz;
|
|
||||||
|
|
||||||
if (!tv) {
|
|
||||||
errno = EINVAL;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
timespec ts {};
|
|
||||||
if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
tv->tv_sec = static_cast<long>(ts.tv_sec);
|
|
||||||
tv->tv_usec = ts.tv_nsec / 1000;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int nanosleep(const struct timespec *req, struct timespec *rem)
|
|
||||||
{
|
|
||||||
(void)rem;
|
|
||||||
|
|
||||||
if (!req || req->tv_nsec < 0 || req->tv_nsec >= 1000000000L) {
|
|
||||||
errno = EINVAL;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
const uint64_t ms = (static_cast<uint64_t>(req->tv_sec) * 1000ULL) +
|
|
||||||
((static_cast<uint64_t>(req->tv_nsec) + 999999ULL) / 1000000ULL);
|
|
||||||
/* Sleep() is millisecond-granular and cannot report remaining time if it is
|
|
||||||
* interrupted, so rem is intentionally unsupported for now. */
|
|
||||||
Sleep(ms > MAXDWORD ? MAXDWORD : static_cast<DWORD>(ms));
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // extern "C"
|
} // extern "C"
|
||||||
|
|
||||||
#endif // defined(_MSC_VER) && !defined(__clang__)
|
#endif // defined(_MSC_VER) && !defined(__clang__)
|
||||||
|
|||||||
@@ -242,16 +242,22 @@ static void px4_windows_calibrate_usleep_threshold()
|
|||||||
// at 700 us (so quiet hosts still cover the typical Win10 1803+
|
// at 700 us (so quiet hosts still cover the typical Win10 1803+
|
||||||
// jitter floor) and capped at 2000 us (the historical safe value).
|
// jitter floor) and capped at 2000 us (the historical safe value).
|
||||||
long sized_tail = p95 + 500;
|
long sized_tail = p95 + 500;
|
||||||
|
|
||||||
if (sized_tail < 700) { sized_tail = 700; }
|
if (sized_tail < 700) { sized_tail = 700; }
|
||||||
|
|
||||||
if (sized_tail > 2000) { sized_tail = 2000; }
|
if (sized_tail > 2000) { sized_tail = 2000; }
|
||||||
|
|
||||||
g_usleep_spin_tail_us = sized_tail;
|
g_usleep_spin_tail_us = sized_tail;
|
||||||
// Set the adaptive tail floor to the host-measured P95 jitter
|
// Set the adaptive tail floor to the host-measured P95 jitter
|
||||||
// (clamped to [200, sized_tail]) so the per-thread controller in
|
// (clamped to [200, sized_tail]) so the per-thread controller in
|
||||||
// usleep() can never trim the spin below the value we already know
|
// usleep() can never trim the spin below the value we already know
|
||||||
// is needed to cover this host's observed long-tail wakes.
|
// is needed to cover this host's observed long-tail wakes.
|
||||||
long adaptive_floor = p95;
|
long adaptive_floor = p95;
|
||||||
|
|
||||||
if (adaptive_floor < 200) { adaptive_floor = 200; }
|
if (adaptive_floor < 200) { adaptive_floor = 200; }
|
||||||
|
|
||||||
if (adaptive_floor > sized_tail) { adaptive_floor = sized_tail; }
|
if (adaptive_floor > sized_tail) { adaptive_floor = sized_tail; }
|
||||||
|
|
||||||
g_usleep_adaptive_min_tail_us = adaptive_floor;
|
g_usleep_adaptive_min_tail_us = adaptive_floor;
|
||||||
std::printf("INFO [px4_windows] usleep spin threshold (auto): %ld us "
|
std::printf("INFO [px4_windows] usleep spin threshold (auto): %ld us "
|
||||||
"(p95 high-res timer jitter: %ld us [N=%d], spin tail: %ld us, "
|
"(p95 high-res timer jitter: %ld us [N=%d], spin tail: %ld us, "
|
||||||
@@ -339,7 +345,7 @@ struct PX4WindowsGlobalInit {
|
|||||||
{
|
{
|
||||||
#if defined(__GNUC__) || defined(__clang__)
|
#if defined(__GNUC__) || defined(__clang__)
|
||||||
long long ret;
|
long long ret;
|
||||||
__asm__ volatile (
|
__asm__ volatile(
|
||||||
"syscall"
|
"syscall"
|
||||||
: "=a"(ret)
|
: "=a"(ret)
|
||||||
: "0"(num), "D"(a)
|
: "0"(num), "D"(a)
|
||||||
@@ -357,7 +363,7 @@ struct PX4WindowsGlobalInit {
|
|||||||
{
|
{
|
||||||
#if defined(__GNUC__) || defined(__clang__)
|
#if defined(__GNUC__) || defined(__clang__)
|
||||||
long long ret;
|
long long ret;
|
||||||
__asm__ volatile (
|
__asm__ volatile(
|
||||||
"syscall"
|
"syscall"
|
||||||
: "=a"(ret)
|
: "=a"(ret)
|
||||||
: "0"(num), "D"(a), "S"(b), "d"(c)
|
: "0"(num), "D"(a), "S"(b), "d"(c)
|
||||||
@@ -418,7 +424,7 @@ struct PX4WindowsGlobalInit {
|
|||||||
term.c_oflag = LX_OPOST | LX_ONLCR;
|
term.c_oflag = LX_OPOST | LX_ONLCR;
|
||||||
term.c_cflag = (term.c_cflag & ~LX_CSIZE) | LX_CS8 | LX_CREAD;
|
term.c_cflag = (term.c_cflag & ~LX_CSIZE) | LX_CS8 | LX_CREAD;
|
||||||
term.c_lflag = LX_ISIG | LX_ICANON | LX_ECHO | LX_ECHOE | LX_ECHOK
|
term.c_lflag = LX_ISIG | LX_ICANON | LX_ECHO | LX_ECHOE | LX_ECHOK
|
||||||
| LX_ECHOCTL | LX_ECHOKE | LX_IEXTEN;
|
| LX_ECHOCTL | LX_ECHOKE | LX_IEXTEN;
|
||||||
|
|
||||||
term.c_cc[0] = 3; // VINTR Ctrl+C
|
term.c_cc[0] = 3; // VINTR Ctrl+C
|
||||||
term.c_cc[1] = 28; // VQUIT Ctrl-backslash
|
term.c_cc[1] = 28; // VQUIT Ctrl-backslash
|
||||||
@@ -469,7 +475,9 @@ struct PX4WindowsGlobalInit {
|
|||||||
void restore_console_modes()
|
void restore_console_modes()
|
||||||
{
|
{
|
||||||
if (saved_stdin.valid) { SetConsoleMode(saved_stdin.handle, running_under_wine ? cooked_stdin_mode() : saved_stdin.mode); }
|
if (saved_stdin.valid) { SetConsoleMode(saved_stdin.handle, running_under_wine ? cooked_stdin_mode() : saved_stdin.mode); }
|
||||||
|
|
||||||
if (saved_stdout.valid) { SetConsoleMode(saved_stdout.handle, saved_stdout.mode); }
|
if (saved_stdout.valid) { SetConsoleMode(saved_stdout.handle, saved_stdout.mode); }
|
||||||
|
|
||||||
if (saved_stderr.valid) { SetConsoleMode(saved_stderr.handle, saved_stderr.mode); }
|
if (saved_stderr.valid) { SetConsoleMode(saved_stderr.handle, saved_stderr.mode); }
|
||||||
|
|
||||||
if (running_under_wine) {
|
if (running_under_wine) {
|
||||||
@@ -482,9 +490,11 @@ struct PX4WindowsGlobalInit {
|
|||||||
PX4WindowsGlobalInit()
|
PX4WindowsGlobalInit()
|
||||||
{
|
{
|
||||||
WSADATA wsaData;
|
WSADATA wsaData;
|
||||||
|
|
||||||
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
|
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
|
||||||
fprintf(stderr, "PX4: WSAStartup failed\n");
|
fprintf(stderr, "PX4: WSAStartup failed\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
SetConsoleOutputCP(CP_UTF8);
|
SetConsoleOutputCP(CP_UTF8);
|
||||||
|
|
||||||
// Raise the global timer resolution to 1 ms. The default
|
// Raise the global timer resolution to 1 ms. The default
|
||||||
@@ -512,6 +522,7 @@ struct PX4WindowsGlobalInit {
|
|||||||
// `__p__fmode()` accessor; setting it is equivalent to linking
|
// `__p__fmode()` accessor; setting it is equivalent to linking
|
||||||
// against binmode.o or compiling the whole image with -D_O_BINARY.
|
// against binmode.o or compiling the whole image with -D_O_BINARY.
|
||||||
if (int *fmode_ptr = __p__fmode()) { *fmode_ptr = _O_BINARY; }
|
if (int *fmode_ptr = __p__fmode()) { *fmode_ptr = _O_BINARY; }
|
||||||
|
|
||||||
_setmode(_fileno(stdin), _O_BINARY);
|
_setmode(_fileno(stdin), _O_BINARY);
|
||||||
_setmode(_fileno(stdout), _O_BINARY);
|
_setmode(_fileno(stdout), _O_BINARY);
|
||||||
_setmode(_fileno(stderr), _O_BINARY);
|
_setmode(_fileno(stderr), _O_BINARY);
|
||||||
@@ -519,7 +530,7 @@ struct PX4WindowsGlobalInit {
|
|||||||
if (running_under_wine) {
|
if (running_under_wine) {
|
||||||
saved_host_tty_valid = tcget_host_tty(saved_host_tty);
|
saved_host_tty_valid = tcget_host_tty(saved_host_tty);
|
||||||
saved_host_tty_was_cooked = saved_host_tty_valid
|
saved_host_tty_was_cooked = saved_host_tty_valid
|
||||||
&& termios_is_interactive_cooked(saved_host_tty);
|
&& termios_is_interactive_cooked(saved_host_tty);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Snapshot console modes so we can restore them on exit. Under
|
// Snapshot console modes so we can restore them on exit. Under
|
||||||
@@ -528,11 +539,15 @@ struct PX4WindowsGlobalInit {
|
|||||||
// back, the shell that launched wine is left with broken
|
// back, the shell that launched wine is left with broken
|
||||||
// arrows / Ctrl+C. On real Windows the restore is a no-op but
|
// arrows / Ctrl+C. On real Windows the restore is a no-op but
|
||||||
// keeps us honest.
|
// keeps us honest.
|
||||||
auto snapshot = [](DWORD which, SavedConsoleMode &slot) {
|
auto snapshot = [](DWORD which, SavedConsoleMode & slot) {
|
||||||
HANDLE h = GetStdHandle(which);
|
HANDLE h = GetStdHandle(which);
|
||||||
|
|
||||||
if (h == INVALID_HANDLE_VALUE || h == nullptr) { return; }
|
if (h == INVALID_HANDLE_VALUE || h == nullptr) { return; }
|
||||||
|
|
||||||
DWORD mode = 0;
|
DWORD mode = 0;
|
||||||
|
|
||||||
if (!GetConsoleMode(h, &mode)) { return; }
|
if (!GetConsoleMode(h, &mode)) { return; }
|
||||||
|
|
||||||
slot.handle = h;
|
slot.handle = h;
|
||||||
slot.mode = mode;
|
slot.mode = mode;
|
||||||
slot.valid = true;
|
slot.valid = true;
|
||||||
@@ -547,7 +562,7 @@ struct PX4WindowsGlobalInit {
|
|||||||
// DISABLE_NEWLINE_AUTO_RETURN prevents the console from inserting an
|
// DISABLE_NEWLINE_AUTO_RETURN prevents the console from inserting an
|
||||||
// extra CR at column 80, which otherwise shows up as spurious line wraps.
|
// extra CR at column 80, which otherwise shows up as spurious line wraps.
|
||||||
const DWORD vt_flags = ENABLE_VIRTUAL_TERMINAL_PROCESSING
|
const DWORD vt_flags = ENABLE_VIRTUAL_TERMINAL_PROCESSING
|
||||||
| DISABLE_NEWLINE_AUTO_RETURN;
|
| DISABLE_NEWLINE_AUTO_RETURN;
|
||||||
|
|
||||||
if (!running_under_wine) {
|
if (!running_under_wine) {
|
||||||
if (saved_stdout.valid) {
|
if (saved_stdout.valid) {
|
||||||
|
|||||||
@@ -121,15 +121,15 @@ static bool looks_like_glob_or_pattern(const std::string &command)
|
|||||||
static std::string ltrim(std::string value)
|
static std::string ltrim(std::string value)
|
||||||
{
|
{
|
||||||
value.erase(value.begin(),
|
value.erase(value.begin(),
|
||||||
std::find_if(value.begin(), value.end(), [](unsigned char ch) { return !std::isspace(ch); }));
|
std::find_if(value.begin(), value.end(), [](unsigned char ch) { return !std::isspace(ch); }));
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
static std::string rtrim(std::string value)
|
static std::string rtrim(std::string value)
|
||||||
{
|
{
|
||||||
value.erase(std::find_if(value.rbegin(), value.rend(),
|
value.erase(std::find_if(value.rbegin(), value.rend(),
|
||||||
[](unsigned char ch) { return !std::isspace(ch); }).base(),
|
[](unsigned char ch) { return !std::isspace(ch); }).base(),
|
||||||
value.end());
|
value.end());
|
||||||
return value;
|
return value;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -170,7 +170,7 @@ static bool is_identifier(const std::string &value)
|
|||||||
static bool is_digits(const std::string &value)
|
static bool is_digits(const std::string &value)
|
||||||
{
|
{
|
||||||
return !value.empty() && std::all_of(value.begin(), value.end(),
|
return !value.empty() && std::all_of(value.begin(), value.end(),
|
||||||
[](unsigned char ch) { return std::isdigit(ch); });
|
[](unsigned char ch) { return std::isdigit(ch); });
|
||||||
}
|
}
|
||||||
|
|
||||||
static std::vector<std::string> split_lines(const std::string &value)
|
static std::vector<std::string> split_lines(const std::string &value)
|
||||||
@@ -428,8 +428,8 @@ private:
|
|||||||
|
|
||||||
parts.push_back(trim(current));
|
parts.push_back(trim(current));
|
||||||
parts.erase(std::remove_if(parts.begin(), parts.end(),
|
parts.erase(std::remove_if(parts.begin(), parts.end(),
|
||||||
[](const std::string &part) { return part.empty(); }),
|
[](const std::string & part) { return part.empty(); }),
|
||||||
parts.end());
|
parts.end());
|
||||||
return parts;
|
return parts;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -929,8 +929,8 @@ private:
|
|||||||
|
|
||||||
parts.emplace_back(pending_operator, trim(current));
|
parts.emplace_back(pending_operator, trim(current));
|
||||||
parts.erase(std::remove_if(parts.begin(), parts.end(),
|
parts.erase(std::remove_if(parts.begin(), parts.end(),
|
||||||
[](const auto &entry) { return entry.second.empty(); }),
|
[](const auto & entry) { return entry.second.empty(); }),
|
||||||
parts.end());
|
parts.end());
|
||||||
return parts;
|
return parts;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -998,8 +998,8 @@ private:
|
|||||||
|
|
||||||
parts.push_back(trim(current));
|
parts.push_back(trim(current));
|
||||||
parts.erase(std::remove_if(parts.begin(), parts.end(),
|
parts.erase(std::remove_if(parts.begin(), parts.end(),
|
||||||
[](const std::string &part) { return part.empty(); }),
|
[](const std::string & part) { return part.empty(); }),
|
||||||
parts.end());
|
parts.end());
|
||||||
return parts;
|
return parts;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1480,7 +1480,7 @@ private:
|
|||||||
std::vector<std::string> lines = split_lines(stdin_data);
|
std::vector<std::string> lines = split_lines(stdin_data);
|
||||||
const bool version_sort = !args.empty() && args[0] == "-V";
|
const bool version_sort = !args.empty() && args[0] == "-V";
|
||||||
|
|
||||||
auto comparator = [version_sort](const std::string &lhs, const std::string &rhs) {
|
auto comparator = [version_sort](const std::string & lhs, const std::string & rhs) {
|
||||||
if (!version_sort) {
|
if (!version_sort) {
|
||||||
return lhs < rhs;
|
return lhs < rhs;
|
||||||
}
|
}
|
||||||
@@ -1764,8 +1764,8 @@ private:
|
|||||||
const bool print = pieces[2].find('p') != std::string::npos;
|
const bool print = pieces[2].find('p') != std::string::npos;
|
||||||
const std::string replacement = convert_sed_replacement(pieces[1]);
|
const std::string replacement = convert_sed_replacement(pieces[1]);
|
||||||
const std::string replaced = global ? std::regex_replace(line, re, replacement)
|
const std::string replaced = global ? std::regex_replace(line, re, replacement)
|
||||||
: std::regex_replace(line, re, replacement,
|
: std::regex_replace(line, re, replacement,
|
||||||
std::regex_constants::format_first_only);
|
std::regex_constants::format_first_only);
|
||||||
|
|
||||||
if (replaced != line) {
|
if (replaced != line) {
|
||||||
line = replaced;
|
line = replaced;
|
||||||
@@ -1847,10 +1847,15 @@ private:
|
|||||||
const long long rvalue = std::atoll(rhs.c_str());
|
const long long rvalue = std::atoll(rhs.c_str());
|
||||||
|
|
||||||
if (op == "-eq") { return lvalue == rvalue; }
|
if (op == "-eq") { return lvalue == rvalue; }
|
||||||
|
|
||||||
if (op == "-ne") { return lvalue != rvalue; }
|
if (op == "-ne") { return lvalue != rvalue; }
|
||||||
|
|
||||||
if (op == "-gt") { return lvalue > rvalue; }
|
if (op == "-gt") { return lvalue > rvalue; }
|
||||||
|
|
||||||
if (op == "-lt") { return lvalue < rvalue; }
|
if (op == "-lt") { return lvalue < rvalue; }
|
||||||
|
|
||||||
if (op == "-ge") { return lvalue >= rvalue; }
|
if (op == "-ge") { return lvalue >= rvalue; }
|
||||||
|
|
||||||
if (op == "-le") { return lvalue <= rvalue; }
|
if (op == "-le") { return lvalue <= rvalue; }
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1873,7 +1878,7 @@ private:
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
auto append_unquoted = [&](const std::string &text) {
|
auto append_unquoted = [&](const std::string & text) {
|
||||||
for (char ch : text) {
|
for (char ch : text) {
|
||||||
if (std::isspace(static_cast<unsigned char>(ch))) {
|
if (std::isspace(static_cast<unsigned char>(ch))) {
|
||||||
flush();
|
flush();
|
||||||
|
|||||||
@@ -109,23 +109,23 @@ struct ParameterGroupInfo {
|
|||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_IF_ENABLED(feature, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_IF_ENABLED(feature, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_UNLESS_ENABLED(feature, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_UNLESS_ENABLED(feature, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
PX4_PP_IF(PX4_PP_IS_ENABLED(feature))( \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability), \
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability), \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__))
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__))
|
||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_IF_ALL2(feature1, feature2, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_IF_ALL2(feature1, feature2, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_AND2(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2)))( \
|
PX4_PP_IF(PX4_PP_AND2(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2)))( \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
||||||
|
|
||||||
#define DEFINE_PARAMETER_GROUP_IF_ANY3(feature1, feature2, feature3, name, availability, ...) \
|
#define DEFINE_PARAMETER_GROUP_IF_ANY3(feature1, feature2, feature3, name, availability, ...) \
|
||||||
PX4_PP_IF(PX4_PP_OR3(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2), PX4_PP_IS_ENABLED(feature3)))( \
|
PX4_PP_IF(PX4_PP_OR3(PX4_PP_IS_ENABLED(feature1), PX4_PP_IS_ENABLED(feature2), PX4_PP_IS_ENABLED(feature3)))( \
|
||||||
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
DEFINE_PARAMETER_GROUP(name, availability, __VA_ARGS__), \
|
||||||
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
_DEFINE_PARAMETER_GROUP_DISABLED(name, availability))
|
||||||
|
|
||||||
#define _DEFINE_PARAMETER_GROUP_DECLARATIONS(group) \
|
#define _DEFINE_PARAMETER_GROUP_DECLARATIONS(group) \
|
||||||
_DEFINE_PARAMETER_GROUP_DECLARATIONS_IMPL group
|
_DEFINE_PARAMETER_GROUP_DECLARATIONS_IMPL group
|
||||||
|
|||||||
@@ -184,11 +184,11 @@ int px4_mavlink_debug_main(int argc, char *argv[])
|
|||||||
// invoking client (`pxh`, `px4-px4_mavlink_debug.exe`, etc.)
|
// invoking client (`pxh`, `px4-px4_mavlink_debug.exe`, etc.)
|
||||||
// would appear to hang.
|
// would appear to hang.
|
||||||
const px4_task_t task = px4_task_spawn_cmd("px4_mavlink_debug",
|
const px4_task_t task = px4_task_spawn_cmd("px4_mavlink_debug",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_DEFAULT,
|
SCHED_PRIORITY_DEFAULT,
|
||||||
2000,
|
2000,
|
||||||
publisher_thread_main,
|
publisher_thread_main,
|
||||||
(argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
|
(argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
|
||||||
|
|
||||||
if (task < 0) {
|
if (task < 0) {
|
||||||
PX4_ERR("task spawn failed");
|
PX4_ERR("task spawn failed");
|
||||||
|
|||||||
@@ -64,10 +64,17 @@ DatamanClient::DatamanClient()
|
|||||||
_fds.fd = _dataman_response_sub;
|
_fds.fd = _dataman_response_sub;
|
||||||
_fds.events = POLLIN;
|
_fds.events = POLLIN;
|
||||||
|
|
||||||
#if !defined(ENABLE_LOCKSTEP_SCHEDULER)
|
#if !(defined(ENABLE_LOCKSTEP_SCHEDULER) && defined(__PX4_WINDOWS))
|
||||||
|
|
||||||
|
// Acquire the client ID eagerly on POSIX (Linux/macOS) and
|
||||||
|
// non-lockstep builds. Only the Windows-lockstep build defers
|
||||||
|
// it, because hrt_absolute_time() is not yet advancing when
|
||||||
|
// the constructor runs there and the GET_ID round-trip would
|
||||||
|
// stall startup.
|
||||||
if (!updateClientId(1000_ms)) {
|
if (!updateClientId(1000_ms)) {
|
||||||
PX4_ERR("Failed to get client ID!");
|
PX4_ERR("Failed to get client ID!");
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -101,6 +108,7 @@ bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_respon
|
|||||||
|
|
||||||
while (!response_received && (time_elapsed < timeout)) {
|
while (!response_received && (time_elapsed < timeout)) {
|
||||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER) && defined(__PX4_WINDOWS)
|
#if defined(ENABLE_LOCKSTEP_SCHEDULER) && defined(__PX4_WINDOWS)
|
||||||
|
|
||||||
if (++iterations > max_iterations) {
|
if (++iterations > max_iterations) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -122,6 +130,7 @@ bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_respon
|
|||||||
if (ret > 0) {
|
if (ret > 0) {
|
||||||
orb_check(_dataman_response_sub, &updated);
|
orb_check(_dataman_response_sub, &updated);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (updated) {
|
if (updated) {
|
||||||
@@ -156,10 +165,12 @@ bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_respon
|
|||||||
|
|
||||||
system_usleep(1000);
|
system_usleep(1000);
|
||||||
#else
|
#else
|
||||||
|
|
||||||
if (ret == 0) {
|
if (ret == 0) {
|
||||||
// No response received, send new request
|
// No response received, send new request
|
||||||
_dataman_request_pub.publish(request);
|
_dataman_request_pub.publish(request);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER) && defined(__PX4_WINDOWS)
|
#if defined(ENABLE_LOCKSTEP_SCHEDULER) && defined(__PX4_WINDOWS)
|
||||||
|
|||||||
@@ -473,7 +473,7 @@ __EXPORT void param_control_autosave(bool enable);
|
|||||||
*/
|
*/
|
||||||
union param_value_u {
|
union param_value_u {
|
||||||
#if defined(__cplusplus)
|
#if defined(__cplusplus)
|
||||||
constexpr param_value_u() : i{} {}
|
constexpr param_value_u() : i {} {}
|
||||||
constexpr explicit param_value_u(int32_t value) : i(value) {}
|
constexpr explicit param_value_u(int32_t value) : i(value) {}
|
||||||
constexpr explicit param_value_u(float value) : f(value) {}
|
constexpr explicit param_value_u(float value) : f(value) {}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -204,6 +204,7 @@ static bool wait_for_vehicle_command_reply(const uint32_t cmd,
|
|||||||
|
|
||||||
system_usleep(wait_interval_us);
|
system_usleep(wait_interval_us);
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
hrt_abstime start = hrt_absolute_time();
|
hrt_abstime start = hrt_absolute_time();
|
||||||
|
|
||||||
@@ -216,6 +217,7 @@ static bool wait_for_vehicle_command_reply(const uint32_t cmd,
|
|||||||
|
|
||||||
px4_usleep(10000);
|
px4_usleep(10000);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
|
|||||||
+206
-206
@@ -522,264 +522,264 @@ private:
|
|||||||
|
|
||||||
DEFINE_PARAMETERS_GROUPED(
|
DEFINE_PARAMETERS_GROUPED(
|
||||||
DEFINE_PARAMETER_GROUP(core, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(core, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamBool<px4::params::EKF2_LOG_VERBOSE>) _param_ekf2_log_verbose,
|
(ParamBool<px4::params::EKF2_LOG_VERBOSE>) _param_ekf2_log_verbose,
|
||||||
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
|
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
|
||||||
(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
|
(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
|
||||||
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
|
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
|
||||||
(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
|
(ParamExtFloat<px4::params::EKF2_VEL_LIM>) _param_ekf2_vel_lim,
|
||||||
(ParamBool<px4::params::EKF2_POS_LOCK>) _param_ekf2_pos_lock,
|
(ParamBool<px4::params::EKF2_POS_LOCK>) _param_ekf2_pos_lock,
|
||||||
(ParamExtInt<px4::params::EKF2_SENS_EN>) _param_ekf2_sens_en
|
(ParamExtInt<px4::params::EKF2_SENS_EN>) _param_ekf2_sens_en
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_AUXVEL, auxiliary_velocity, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_AUXVEL),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_AUXVEL, auxiliary_velocity, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_AUXVEL),
|
||||||
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
|
(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
|
||||||
_param_ekf2_avel_delay ///< auxiliary velocity measurement delay relative to the IMU (mSec)
|
_param_ekf2_avel_delay ///< auxiliary velocity measurement delay relative to the IMU (mSec)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP(imu_noise, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(imu_noise, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
|
||||||
_param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
|
_param_ekf2_gyr_noise, ///< IMU angular rate noise used for covariance prediction (rad/sec)
|
||||||
(ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
|
||||||
_param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
|
_param_ekf2_acc_noise, ///< IMU acceleration noise use for covariance prediction (m/sec**2)
|
||||||
|
|
||||||
// process noise
|
// process noise
|
||||||
(ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
|
||||||
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
|
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
|
||||||
_param_ekf2_acc_b_noise///< process noise for IMU accelerometer bias prediction (m/sec**3)
|
_param_ekf2_acc_b_noise///< process noise for IMU accelerometer bias prediction (m/sec**3)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_WIND, wind, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_WIND),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_WIND, wind, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_WIND),
|
||||||
(ParamExtFloat<px4::params::EKF2_WIND_NSD>) _param_ekf2_wind_nsd
|
(ParamExtFloat<px4::params::EKF2_WIND_NSD>) _param_ekf2_wind_nsd
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP(no_aid, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(no_aid, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>) _param_ekf2_noaid_noise
|
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>) _param_ekf2_noaid_noise
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_GNSS, gnss, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_GNSS),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_GNSS, gnss, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_GNSS),
|
||||||
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
|
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
|
||||||
(ParamExtInt<px4::params::EKF2_GPS_MODE>) _param_ekf2_gps_mode,
|
(ParamExtInt<px4::params::EKF2_GPS_MODE>) _param_ekf2_gps_mode,
|
||||||
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
|
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>) _param_ekf2_gps_v_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,
|
(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>) _param_ekf2_gps_p_noise,
|
||||||
|
|
||||||
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>) _param_ekf2_gps_p_gate,
|
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>) _param_ekf2_gps_p_gate,
|
||||||
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>) _param_ekf2_gps_v_gate,
|
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>) _param_ekf2_gps_v_gate,
|
||||||
|
|
||||||
(ParamExtInt<px4::params::EKF2_GPS_CHECK>) _param_ekf2_gps_check,
|
(ParamExtInt<px4::params::EKF2_GPS_CHECK>) _param_ekf2_gps_check,
|
||||||
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph,
|
(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph,
|
||||||
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv,
|
(ParamExtFloat<px4::params::EKF2_REQ_EPV>) _param_ekf2_req_epv,
|
||||||
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc,
|
(ParamExtFloat<px4::params::EKF2_REQ_SACC>) _param_ekf2_req_sacc,
|
||||||
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats,
|
(ParamExtInt<px4::params::EKF2_REQ_NSATS>) _param_ekf2_req_nsats,
|
||||||
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>) _param_ekf2_req_pdop,
|
(ParamExtFloat<px4::params::EKF2_REQ_PDOP>) _param_ekf2_req_pdop,
|
||||||
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _param_ekf2_req_hdrift,
|
(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>) _param_ekf2_req_hdrift,
|
||||||
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,
|
(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,
|
||||||
(ParamExtInt<px4::params::EKF2_REQ_FIX>) _param_ekf2_req_fix,
|
(ParamExtInt<px4::params::EKF2_REQ_FIX>) _param_ekf2_req_fix,
|
||||||
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h,
|
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h,
|
||||||
|
|
||||||
// Used by EKF-GSF experimental yaw estimator
|
// Used by EKF-GSF experimental yaw estimator
|
||||||
(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas,
|
(ParamExtFloat<px4::params::EKF2_GSF_TAS>) _param_ekf2_gsf_tas,
|
||||||
(ParamFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off
|
(ParamFloat<px4::params::EKF2_GPS_YAW_OFF>) _param_ekf2_gps_yaw_off
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_BAROMETER, barometer, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_BAROMETER),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_BAROMETER, barometer, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_BAROMETER),
|
||||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,
|
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,
|
||||||
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>) _param_ekf2_baro_delay,
|
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>) _param_ekf2_baro_delay,
|
||||||
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _param_ekf2_baro_noise,
|
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _param_ekf2_baro_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_BARO_GATE>) _param_ekf2_baro_gate,
|
(ParamExtFloat<px4::params::EKF2_BARO_GATE>) _param_ekf2_baro_gate,
|
||||||
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>) _param_ekf2_gnd_eff_dz,
|
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>) _param_ekf2_gnd_eff_dz,
|
||||||
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>) _param_ekf2_gnd_max_hgt
|
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>) _param_ekf2_gnd_max_hgt
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ALL2(CONFIG_EKF2_BAROMETER, CONFIG_EKF2_BARO_COMPENSATION,
|
DEFINE_PARAMETER_GROUP_IF_ALL2(CONFIG_EKF2_BAROMETER, CONFIG_EKF2_BARO_COMPENSATION,
|
||||||
barometer_static_pressure_compensation,
|
barometer_static_pressure_compensation,
|
||||||
PARAMETER_GROUP_CONDITION(defined(CONFIG_EKF2_BAROMETER) && defined(CONFIG_EKF2_BARO_COMPENSATION)),
|
PARAMETER_GROUP_CONDITION(defined(CONFIG_EKF2_BAROMETER) &&defined(CONFIG_EKF2_BARO_COMPENSATION)),
|
||||||
// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
|
// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
|
||||||
(ParamExtFloat<px4::params::EKF2_ASPD_MAX>) _param_ekf2_aspd_max,
|
(ParamExtFloat<px4::params::EKF2_ASPD_MAX>) _param_ekf2_aspd_max,
|
||||||
(ParamExtFloat<px4::params::EKF2_PCOEF_XP>) _param_ekf2_pcoef_xp,
|
(ParamExtFloat<px4::params::EKF2_PCOEF_XP>) _param_ekf2_pcoef_xp,
|
||||||
(ParamExtFloat<px4::params::EKF2_PCOEF_XN>) _param_ekf2_pcoef_xn,
|
(ParamExtFloat<px4::params::EKF2_PCOEF_XN>) _param_ekf2_pcoef_xn,
|
||||||
(ParamExtFloat<px4::params::EKF2_PCOEF_YP>) _param_ekf2_pcoef_yp,
|
(ParamExtFloat<px4::params::EKF2_PCOEF_YP>) _param_ekf2_pcoef_yp,
|
||||||
(ParamExtFloat<px4::params::EKF2_PCOEF_YN>) _param_ekf2_pcoef_yn,
|
(ParamExtFloat<px4::params::EKF2_PCOEF_YN>) _param_ekf2_pcoef_yn,
|
||||||
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>) _param_ekf2_pcoef_z
|
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>) _param_ekf2_pcoef_z
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_AIRSPEED, airspeed, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_AIRSPEED),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_AIRSPEED, airspeed, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_AIRSPEED),
|
||||||
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
|
(ParamExtFloat<px4::params::EKF2_ASP_DELAY>)
|
||||||
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
|
_param_ekf2_asp_delay, ///< airspeed measurement delay relative to the IMU (mSec)
|
||||||
(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
|
(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
|
||||||
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
|
_param_ekf2_tas_gate, ///< True Airspeed innovation consistency gate size (STD)
|
||||||
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_EAS_NOISE>)
|
||||||
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
|
_param_ekf2_eas_noise, ///< measurement noise used for airspeed fusion (m/sec)
|
||||||
|
|
||||||
// control of airspeed fusion
|
// control of airspeed fusion
|
||||||
(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
|
(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
|
||||||
_param_ekf2_arsp_thr ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
|
_param_ekf2_arsp_thr ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_SIDESLIP, sideslip, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_SIDESLIP),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_SIDESLIP, sideslip, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_SIDESLIP),
|
||||||
(ParamExtFloat<px4::params::EKF2_BETA_GATE>) _param_ekf2_beta_gate,
|
(ParamExtFloat<px4::params::EKF2_BETA_GATE>) _param_ekf2_beta_gate,
|
||||||
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise,
|
(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise,
|
||||||
(ParamExtInt<px4::params::EKF2_FUSE_BETA>) _param_ekf2_fuse_beta
|
(ParamExtInt<px4::params::EKF2_FUSE_BETA>) _param_ekf2_fuse_beta
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_MAGNETOMETER, magnetometer, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_MAGNETOMETER),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_MAGNETOMETER, magnetometer, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_MAGNETOMETER),
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>) _param_ekf2_mag_delay,
|
(ParamExtFloat<px4::params::EKF2_MAG_DELAY>) _param_ekf2_mag_delay,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>) _param_ekf2_mag_e_noise,
|
(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>) _param_ekf2_mag_e_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>) _param_ekf2_mag_b_noise,
|
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>) _param_ekf2_mag_b_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>) _param_ekf2_head_noise,
|
(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>) _param_ekf2_head_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>) _param_ekf2_mag_noise,
|
(ParamExtFloat<px4::params::EKF2_MAG_NOISE>) _param_ekf2_mag_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,
|
(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,
|
||||||
(ParamExtFloat<px4::params::EKF2_HDG_GATE>) _param_ekf2_hdg_gate,
|
(ParamExtFloat<px4::params::EKF2_HDG_GATE>) _param_ekf2_hdg_gate,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_GATE>) _param_ekf2_mag_gate,
|
(ParamExtFloat<px4::params::EKF2_MAG_GATE>) _param_ekf2_mag_gate,
|
||||||
(ParamExtInt<px4::params::EKF2_DECL_TYPE>) _param_ekf2_decl_type,
|
(ParamExtInt<px4::params::EKF2_DECL_TYPE>) _param_ekf2_decl_type,
|
||||||
(ParamExtInt<px4::params::EKF2_MAG_TYPE>) _param_ekf2_mag_type,
|
(ParamExtInt<px4::params::EKF2_MAG_TYPE>) _param_ekf2_mag_type,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>) _param_ekf2_mag_acclim,
|
(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>) _param_ekf2_mag_acclim,
|
||||||
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
|
(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
|
(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
|
||||||
(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
|
(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
|
||||||
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synt_mag_z
|
(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synt_mag_z
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP(height_reference, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(height_reference, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
|
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
|
||||||
|
|
||||||
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
|
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
|
||||||
_param_ekf2_noaid_tout ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
|
_param_ekf2_noaid_tout ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ANY3(CONFIG_EKF2_TERRAIN, CONFIG_EKF2_OPTICAL_FLOW, CONFIG_EKF2_RANGE_FINDER,
|
DEFINE_PARAMETER_GROUP_IF_ANY3(CONFIG_EKF2_TERRAIN, CONFIG_EKF2_OPTICAL_FLOW, CONFIG_EKF2_RANGE_FINDER,
|
||||||
minimum_range,
|
minimum_range,
|
||||||
PARAMETER_GROUP_CONDITION(defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)),
|
PARAMETER_GROUP_CONDITION(defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)),
|
||||||
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng
|
(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_TERRAIN, terrain, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_TERRAIN),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_TERRAIN, terrain, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_TERRAIN),
|
||||||
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,
|
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>) _param_ekf2_terr_grad
|
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>) _param_ekf2_terr_grad
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_RANGE_FINDER, range_finder, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_RANGE_FINDER),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_RANGE_FINDER, range_finder, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_RANGE_FINDER),
|
||||||
// range finder fusion
|
// range finder fusion
|
||||||
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl,
|
(ParamExtInt<px4::params::EKF2_RNG_CTRL>) _param_ekf2_rng_ctrl,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>) _param_ekf2_rng_delay,
|
(ParamExtFloat<px4::params::EKF2_RNG_DELAY>) _param_ekf2_rng_delay,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
|
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
|
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
|
(ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
|
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
|
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
|
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
|
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
|
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
|
(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
|
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
|
(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z
|
(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_RANGING_BEACON, ranging_beacon, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_RANGING_BEACON),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_RANGING_BEACON, ranging_beacon, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_RANGING_BEACON),
|
||||||
// ranging beacon fusion
|
// ranging beacon fusion
|
||||||
(ParamExtInt<px4::params::EKF2_RNGBC_CTRL>) _param_ekf2_rngbc_ctrl,
|
(ParamExtInt<px4::params::EKF2_RNGBC_CTRL>) _param_ekf2_rngbc_ctrl,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNGBC_DELAY>) _param_ekf2_rngbc_delay,
|
(ParamExtFloat<px4::params::EKF2_RNGBC_DELAY>) _param_ekf2_rngbc_delay,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNGBC_NOISE>) _param_ekf2_rngbc_noise,
|
(ParamExtFloat<px4::params::EKF2_RNGBC_NOISE>) _param_ekf2_rngbc_noise,
|
||||||
(ParamExtFloat<px4::params::EKF2_RNGBC_GATE>) _param_ekf2_rngbc_gate
|
(ParamExtFloat<px4::params::EKF2_RNGBC_GATE>) _param_ekf2_rngbc_gate
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_EXTERNAL_VISION, external_vision, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_EXTERNAL_VISION),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_EXTERNAL_VISION, external_vision, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_EXTERNAL_VISION),
|
||||||
// vision estimate fusion
|
// vision estimate fusion
|
||||||
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
|
(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
|
||||||
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
|
_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
|
||||||
|
|
||||||
(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection
|
(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection
|
||||||
(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
|
(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
|
||||||
(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
|
(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
|
||||||
(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)
|
||||||
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
|
_param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_EVV_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_EVV_NOISE>)
|
||||||
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
|
_param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s)
|
||||||
(ParamExtFloat<px4::params::EKF2_EVA_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_EVA_NOISE>)
|
||||||
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
|
_param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad)
|
||||||
(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
|
(ParamExtFloat<px4::params::EKF2_EVV_GATE>)
|
||||||
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
|
_param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD)
|
||||||
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
|
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
|
||||||
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
|
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
|
||||||
|
|
||||||
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
|
(ParamExtFloat<px4::params::EKF2_EV_POS_X>)
|
||||||
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
|
_param_ekf2_ev_pos_x, ///< X position of VI sensor focal point in body frame (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
|
(ParamExtFloat<px4::params::EKF2_EV_POS_Y>)
|
||||||
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
|
_param_ekf2_ev_pos_y, ///< Y position of VI sensor focal point in body frame (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
|
(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
|
||||||
_param_ekf2_ev_pos_z ///< Z position of VI sensor focal point in body frame (m)
|
_param_ekf2_ev_pos_z ///< Z position of VI sensor focal point in body frame (m)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_OPTICAL_FLOW, optical_flow, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_OPTICAL_FLOW),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_OPTICAL_FLOW, optical_flow, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_OPTICAL_FLOW),
|
||||||
// optical flow fusion
|
// optical flow fusion
|
||||||
(ParamExtInt<px4::params::EKF2_OF_CTRL>)
|
(ParamExtInt<px4::params::EKF2_OF_CTRL>)
|
||||||
_param_ekf2_of_ctrl,
|
_param_ekf2_of_ctrl,
|
||||||
(ParamExtInt<px4::params::EKF2_OF_GYR_SRC>)
|
(ParamExtInt<px4::params::EKF2_OF_GYR_SRC>)
|
||||||
_param_ekf2_of_gyr_src,
|
_param_ekf2_of_gyr_src,
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
|
(ParamExtFloat<px4::params::EKF2_OF_DELAY>)
|
||||||
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
_param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
|
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
|
||||||
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
|
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
|
(ParamExtFloat<px4::params::EKF2_OF_N_MAX>)
|
||||||
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
|
_param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||||
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
|
(ParamExtInt<px4::params::EKF2_OF_QMIN>)
|
||||||
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air
|
_param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air
|
||||||
(ParamExtInt<px4::params::EKF2_OF_QMIN_GND>)
|
(ParamExtInt<px4::params::EKF2_OF_QMIN_GND>)
|
||||||
_param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground
|
_param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
|
(ParamExtFloat<px4::params::EKF2_OF_GATE>)
|
||||||
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
|
_param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD)
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
|
(ParamExtFloat<px4::params::EKF2_OF_POS_X>)
|
||||||
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
|
_param_ekf2_of_pos_x, ///< X position of optical flow sensor focal point in body frame (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
|
(ParamExtFloat<px4::params::EKF2_OF_POS_Y>)
|
||||||
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
|
_param_ekf2_of_pos_y, ///< Y position of optical flow sensor focal point in body frame (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
|
(ParamExtFloat<px4::params::EKF2_OF_POS_Z>)
|
||||||
_param_ekf2_of_pos_z ///< Z position of optical flow sensor focal point in body frame (m)
|
_param_ekf2_of_pos_z ///< Z position of optical flow sensor focal point in body frame (m)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_DRAG_FUSION, drag_fusion, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_DRAG_FUSION),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_DRAG_FUSION, drag_fusion, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_DRAG_FUSION),
|
||||||
(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl, ///< drag fusion selection
|
(ParamExtInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl, ///< drag fusion selection
|
||||||
// Multi-rotor drag specific force fusion
|
// Multi-rotor drag specific force fusion
|
||||||
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
|
(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
|
||||||
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
|
_param_ekf2_drag_noise, ///< observation noise variance for drag specific force measurements (m/sec**2)**2
|
||||||
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
|
(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, ///< ballistic coefficient along the X-axis (kg/m**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
|
(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y, ///< ballistic coefficient along the Y-axis (kg/m**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef ///< propeller momentum drag coefficient (1/s)
|
(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef ///< propeller momentum drag coefficient (1/s)
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_GRAVITY_FUSION, gravity_fusion, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_GRAVITY_FUSION),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_EKF2_GRAVITY_FUSION, gravity_fusion, PARAMETER_GROUP_FEATURE(CONFIG_EKF2_GRAVITY_FUSION),
|
||||||
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise
|
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>) _param_ekf2_grav_noise
|
||||||
),
|
),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP(output_predictor, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(output_predictor, PARAMETER_GROUP_ALWAYS,
|
||||||
// sensor positions in body frame
|
// sensor positions in body frame
|
||||||
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
|
(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x, ///< X position of IMU in body frame (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
|
(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y, ///< Y position of IMU in body frame (m)
|
||||||
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
|
(ParamExtFloat<px4::params::EKF2_IMU_POS_Z>) _param_ekf2_imu_pos_z, ///< Z position of IMU in body frame (m)
|
||||||
|
|
||||||
// IMU switch on bias parameters
|
// IMU switch on bias parameters
|
||||||
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
|
(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
|
||||||
_param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
|
_param_ekf2_gbias_init, ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
|
||||||
(ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
|
(ParamExtFloat<px4::params::EKF2_ABIAS_INIT>)
|
||||||
_param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
|
_param_ekf2_abias_init, ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
|
(ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
|
||||||
_param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
|
_param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
|
||||||
|
|
||||||
// EKF accel bias learning control
|
// EKF accel bias learning control
|
||||||
(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2)
|
(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
|
(ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
|
||||||
_param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
|
_param_ekf2_abl_acclim, ///< Maximum IMU accel magnitude that allows IMU bias learning (m/s**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_ABL_GYRLIM>)
|
(ParamExtFloat<px4::params::EKF2_ABL_GYRLIM>)
|
||||||
_param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
|
_param_ekf2_abl_gyrlim, ///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
|
||||||
(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
|
(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
|
||||||
_param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
|
_param_ekf2_abl_tau, ///< Time constant used to inhibit IMU delta velocity bias learning (sec)
|
||||||
|
|
||||||
(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s)
|
(ParamExtFloat<px4::params::EKF2_GYR_B_LIM>) _param_ekf2_gyr_b_lim, ///< Gyro bias learning limit (rad/s)
|
||||||
|
|
||||||
// output predictor filter time constants
|
// output predictor filter time constants
|
||||||
(ParamFloat<px4::params::EKF2_TAU_VEL>) _param_ekf2_tau_vel,
|
(ParamFloat<px4::params::EKF2_TAU_VEL>) _param_ekf2_tau_vel,
|
||||||
(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos
|
(ParamFloat<px4::params::EKF2_TAU_POS>) _param_ekf2_tau_pos
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
};
|
};
|
||||||
#endif // !EKF2_HPP
|
#endif // !EKF2_HPP
|
||||||
|
|||||||
+11
-11
@@ -401,19 +401,19 @@ private:
|
|||||||
|
|
||||||
DEFINE_PARAMETERS_GROUPED(
|
DEFINE_PARAMETERS_GROUPED(
|
||||||
DEFINE_PARAMETER_GROUP(logging, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(logging, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamInt<px4::params::SDLOG_UTC_OFFSET>) _param_sdlog_utc_offset,
|
(ParamInt<px4::params::SDLOG_UTC_OFFSET>) _param_sdlog_utc_offset,
|
||||||
(ParamInt<px4::params::SDLOG_MAX_SIZE>) _param_sdlog_max_size,
|
(ParamInt<px4::params::SDLOG_MAX_SIZE>) _param_sdlog_max_size,
|
||||||
(ParamInt<px4::params::SDLOG_ROTATE>) _param_sdlog_rotate,
|
(ParamInt<px4::params::SDLOG_ROTATE>) _param_sdlog_rotate,
|
||||||
(ParamInt<px4::params::SDLOG_DIRS_MAX>) _param_sdlog_dirs_max,
|
(ParamInt<px4::params::SDLOG_DIRS_MAX>) _param_sdlog_dirs_max,
|
||||||
(ParamInt<px4::params::SDLOG_PROFILE>) _param_sdlog_profile,
|
(ParamInt<px4::params::SDLOG_PROFILE>) _param_sdlog_profile,
|
||||||
(ParamInt<px4::params::SDLOG_MISSION>) _param_sdlog_mission,
|
(ParamInt<px4::params::SDLOG_MISSION>) _param_sdlog_mission,
|
||||||
(ParamBool<px4::params::SDLOG_BOOT_BAT>) _param_sdlog_boot_bat,
|
(ParamBool<px4::params::SDLOG_BOOT_BAT>) _param_sdlog_boot_bat,
|
||||||
(ParamBool<px4::params::SDLOG_UUID>) _param_sdlog_uuid),
|
(ParamBool<px4::params::SDLOG_UUID>) _param_sdlog_uuid),
|
||||||
|
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(PX4_CRYPTO, encrypted_logging, PARAMETER_GROUP_FEATURE(PX4_CRYPTO),
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(PX4_CRYPTO, encrypted_logging, PARAMETER_GROUP_FEATURE(PX4_CRYPTO),
|
||||||
(ParamInt<px4::params::SDLOG_ALGORITHM>) _param_sdlog_crypto_algorithm,
|
(ParamInt<px4::params::SDLOG_ALGORITHM>) _param_sdlog_crypto_algorithm,
|
||||||
(ParamInt<px4::params::SDLOG_KEY>) _param_sdlog_crypto_key,
|
(ParamInt<px4::params::SDLOG_KEY>) _param_sdlog_crypto_key,
|
||||||
(ParamInt<px4::params::SDLOG_EXCH_KEY>) _param_sdlog_crypto_exchange_key)
|
(ParamInt<px4::params::SDLOG_EXCH_KEY>) _param_sdlog_crypto_exchange_key)
|
||||||
)
|
)
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -251,19 +251,19 @@ private:
|
|||||||
|
|
||||||
DEFINE_PARAMETERS_GROUPED(
|
DEFINE_PARAMETERS_GROUPED(
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_SENSORS_VEHICLE_AIR_DATA, vehicle_air_data,
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_SENSORS_VEHICLE_AIR_DATA, vehicle_air_data,
|
||||||
PARAMETER_GROUP_FEATURE(CONFIG_SENSORS_VEHICLE_AIR_DATA),
|
PARAMETER_GROUP_FEATURE(CONFIG_SENSORS_VEHICLE_AIR_DATA),
|
||||||
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
|
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
|
||||||
),
|
),
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_SENSORS_VEHICLE_GPS_POSITION, vehicle_gps_position,
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_SENSORS_VEHICLE_GPS_POSITION, vehicle_gps_position,
|
||||||
PARAMETER_GROUP_FEATURE(CONFIG_SENSORS_VEHICLE_GPS_POSITION),
|
PARAMETER_GROUP_FEATURE(CONFIG_SENSORS_VEHICLE_GPS_POSITION),
|
||||||
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps
|
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps
|
||||||
),
|
),
|
||||||
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_SENSORS_VEHICLE_MAGNETOMETER, vehicle_magnetometer,
|
DEFINE_PARAMETER_GROUP_IF_ENABLED(CONFIG_SENSORS_VEHICLE_MAGNETOMETER, vehicle_magnetometer,
|
||||||
PARAMETER_GROUP_FEATURE(CONFIG_SENSORS_VEHICLE_MAGNETOMETER),
|
PARAMETER_GROUP_FEATURE(CONFIG_SENSORS_VEHICLE_MAGNETOMETER),
|
||||||
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
|
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
|
||||||
),
|
),
|
||||||
DEFINE_PARAMETER_GROUP(imu_mode, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(imu_mode, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
|
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -183,21 +183,21 @@ private:
|
|||||||
|
|
||||||
DEFINE_PARAMETERS_GROUPED(
|
DEFINE_PARAMETERS_GROUPED(
|
||||||
DEFINE_PARAMETER_GROUP_UNLESS_ENABLED(CONSTRAINED_FLASH, dynamic_notch_filter,
|
DEFINE_PARAMETER_GROUP_UNLESS_ENABLED(CONSTRAINED_FLASH, dynamic_notch_filter,
|
||||||
PARAMETER_GROUP_CONDITION(!defined(CONSTRAINED_FLASH)),
|
PARAMETER_GROUP_CONDITION(!defined(CONSTRAINED_FLASH)),
|
||||||
(ParamInt<px4::params::IMU_GYRO_DNF_EN>) _param_imu_gyro_dnf_en,
|
(ParamInt<px4::params::IMU_GYRO_DNF_EN>) _param_imu_gyro_dnf_en,
|
||||||
(ParamInt<px4::params::IMU_GYRO_DNF_HMC>) _param_imu_gyro_dnf_hmc,
|
(ParamInt<px4::params::IMU_GYRO_DNF_HMC>) _param_imu_gyro_dnf_hmc,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
|
(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_DNF_MIN>) _param_imu_gyro_dnf_min
|
(ParamFloat<px4::params::IMU_GYRO_DNF_MIN>) _param_imu_gyro_dnf_min
|
||||||
),
|
),
|
||||||
DEFINE_PARAMETER_GROUP(gyro_filtering, PARAMETER_GROUP_ALWAYS,
|
DEFINE_PARAMETER_GROUP(gyro_filtering, PARAMETER_GROUP_ALWAYS,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
|
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_NF0_FRQ>) _param_imu_gyro_nf0_frq,
|
(ParamFloat<px4::params::IMU_GYRO_NF0_FRQ>) _param_imu_gyro_nf0_frq,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_NF0_BW>) _param_imu_gyro_nf0_bw,
|
(ParamFloat<px4::params::IMU_GYRO_NF0_BW>) _param_imu_gyro_nf0_bw,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_NF1_FRQ>) _param_imu_gyro_nf1_frq,
|
(ParamFloat<px4::params::IMU_GYRO_NF1_FRQ>) _param_imu_gyro_nf1_frq,
|
||||||
(ParamFloat<px4::params::IMU_GYRO_NF1_BW>) _param_imu_gyro_nf1_bw,
|
(ParamFloat<px4::params::IMU_GYRO_NF1_BW>) _param_imu_gyro_nf1_bw,
|
||||||
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
|
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
|
||||||
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
|
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@@ -118,7 +118,7 @@ static uint64_t wall_time_us()
|
|||||||
gettimeofday(&t, nullptr);
|
gettimeofday(&t, nullptr);
|
||||||
return t.tv_sec * static_cast<uint64_t>(1000000) + t.tv_usec;
|
return t.tv_sec * static_cast<uint64_t>(1000000) + t.tv_usec;
|
||||||
#else
|
#else
|
||||||
timespec ts{};
|
timespec ts {};
|
||||||
system_clock_gettime(CLOCK_MONOTONIC, &ts);
|
system_clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||||
return (static_cast<uint64_t>(ts.tv_sec) * 1000000ULL) + (static_cast<uint64_t>(ts.tv_nsec) / 1000ULL);
|
return (static_cast<uint64_t>(ts.tv_sec) * 1000000ULL) + (static_cast<uint64_t>(ts.tv_nsec) / 1000ULL);
|
||||||
#endif
|
#endif
|
||||||
@@ -135,10 +135,13 @@ static void lockstep_wall_sleep(int sleep_time_us)
|
|||||||
const uint64_t sleep_until_us = wall_time_us() + sleep_time_us;
|
const uint64_t sleep_until_us = wall_time_us() + sleep_time_us;
|
||||||
|
|
||||||
#if defined(__PX4_WINDOWS)
|
#if defined(__PX4_WINDOWS)
|
||||||
|
|
||||||
while (wall_time_us() < sleep_until_us) {
|
while (wall_time_us() < sleep_until_us) {
|
||||||
sched_yield();
|
sched_yield();
|
||||||
}
|
}
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
const uint64_t now_us = wall_time_us();
|
const uint64_t now_us = wall_time_us();
|
||||||
|
|
||||||
@@ -150,6 +153,7 @@ static void lockstep_wall_sleep(int sleep_time_us)
|
|||||||
sched_yield();
|
sched_yield();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|||||||
@@ -161,7 +161,7 @@ private:
|
|||||||
Transport _transport{};
|
Transport _transport{};
|
||||||
|
|
||||||
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
uxrSerialTransport *_transport_serial{nullptr};
|
uxrSerialTransport *_transport_serial {nullptr};
|
||||||
#endif
|
#endif
|
||||||
char _device[32] {};
|
char _device[32] {};
|
||||||
int _baudrate{};
|
int _baudrate{};
|
||||||
|
|||||||
Reference in New Issue
Block a user