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:
Nuno Marques
2026-05-07 16:55:19 -07:00
parent 6dc436811e
commit 0bb318cec5
47 changed files with 1452 additions and 1165 deletions
+17 -1
View File
@@ -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 ----
+1 -1
View File
@@ -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
+11 -1
View File
@@ -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;
+1
View File
@@ -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;
+71 -58
View File
@@ -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);
} }
@@ -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;
+7
View File
@@ -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");
+12 -1
View File
@@ -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)
+1 -1
View File
@@ -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
+2
View File
@@ -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
View File
@@ -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
View File
@@ -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)
) )
}; };
+11 -11
View File
@@ -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
) )
) )
}; };
+5 -1
View File
@@ -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{};