mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
fix(simulation): support Windows SITL transports
Make MAVLink, uXRCE-DDS, and simulator_mavlink tolerate Windows socket and daemon semantics while preserving the existing POSIX transport behavior. Update Gazebo Classic integration, SIH defaults, and multi-instance simulation scripts needed by the native Windows SITL workflow.
This commit is contained in:
@@ -16,7 +16,8 @@ param set-default SENS_EN_BAROSIM 1
|
|||||||
param set-default SENS_EN_MAGSIM 1
|
param set-default SENS_EN_MAGSIM 1
|
||||||
param set-default SENS_EN_ARSPDSIM 1
|
param set-default SENS_EN_ARSPDSIM 1
|
||||||
|
|
||||||
param set-default SIH_T_MAX 6
|
param set-default SIH_F_T_MAX 8
|
||||||
|
param set-default SIH_F_CT0 0.16
|
||||||
param set-default SIH_MASS 0.3
|
param set-default SIH_MASS 0.3
|
||||||
param set-default SIH_IXX 0.00402
|
param set-default SIH_IXX 0.00402
|
||||||
param set-default SIH_IYY 0.0144
|
param set-default SIH_IYY 0.0144
|
||||||
@@ -65,3 +66,23 @@ param set-default FW_YR_I 3.0000
|
|||||||
param set-default FW_YR_D 0.0000
|
param set-default FW_YR_D 0.0000
|
||||||
param set-default FW_YR_FF 0.0000
|
param set-default FW_YR_FF 0.0000
|
||||||
param set-default FW_YR_IMAX 1.0000
|
param set-default FW_YR_IMAX 1.0000
|
||||||
|
|
||||||
|
# FW controller / TECS tuning for this 0.3 kg low-mass airframe.
|
||||||
|
# Default PX4 FW tuning is calibrated for 1-5 kg planes; this airframe needs
|
||||||
|
# specific tuning for the takeoff -> cruise handoff at 10 m/s trim with 8 N
|
||||||
|
# max thrust. Values verified by iterating against SIH dynamics.
|
||||||
|
param set-default FW_AIRSPD_STALL 6
|
||||||
|
param set-default FW_AIRSPD_MIN 7
|
||||||
|
param set-default FW_AIRSPD_TRIM 10
|
||||||
|
param set-default FW_AIRSPD_MAX 12
|
||||||
|
param set-default FW_PSP_OFF 2.0
|
||||||
|
param set-default FW_THR_TRIM 0.9
|
||||||
|
param set-default FW_THR_MIN 0.5
|
||||||
|
param set-default FW_T_CLMB_MAX 2
|
||||||
|
param set-default FW_T_SINK_MIN 2
|
||||||
|
param set-default FW_T_SPDWEIGHT 2.0
|
||||||
|
param set-default FW_T_ALT_TC 3.0
|
||||||
|
param set-default FW_T_TAS_TC 3.0
|
||||||
|
param set-default FW_T_HRATE_FF 0.6
|
||||||
|
param set-default FW_T_THR_DAMPING 0.3
|
||||||
|
param set-default NAV_LOITER_RAD 30
|
||||||
|
|||||||
Submodule Tools/simulation/gazebo-classic/sitl_gazebo-classic updated: f835e077d0...00ac441b27
@@ -0,0 +1,105 @@
|
|||||||
|
<#
|
||||||
|
.SYNOPSIS
|
||||||
|
Run multiple instances of the 'px4' binary on Windows, without starting
|
||||||
|
an external simulator. Mirrors Tools/simulation/sitl_multiple_run.sh -
|
||||||
|
same args, same per-instance working directories, same logfile names.
|
||||||
|
|
||||||
|
.DESCRIPTION
|
||||||
|
For developers on PowerShell or CMD (the .sh script also works under
|
||||||
|
Git Bash on Windows). Assumes px4 is already built with the specified
|
||||||
|
build target (e.g., px4_sitl_default).
|
||||||
|
|
||||||
|
Pass 0 for SitlNum to just stop everything currently running:
|
||||||
|
.\Tools\simulation\sitl_multiple_run.ps1 0
|
||||||
|
|
||||||
|
.PARAMETER SitlNum
|
||||||
|
Number of instances to start. Defaults to 2.
|
||||||
|
|
||||||
|
.PARAMETER SimModel
|
||||||
|
Value to export as PX4_SIM_MODEL. Defaults to 'gazebo-classic_iris' to
|
||||||
|
match the .sh script; set to 'sihsim_quadx' for a Windows-friendly run
|
||||||
|
that needs no external simulator.
|
||||||
|
|
||||||
|
.PARAMETER BuildTarget
|
||||||
|
Name of the cmake build directory under build/. Defaults to
|
||||||
|
'px4_sitl_default'.
|
||||||
|
|
||||||
|
.EXAMPLE
|
||||||
|
.\Tools\simulation\sitl_multiple_run.ps1 3 sihsim_quadx px4_sitl_default
|
||||||
|
|
||||||
|
.EXAMPLE
|
||||||
|
# From CMD:
|
||||||
|
powershell -ExecutionPolicy Bypass -File Tools\simulation\sitl_multiple_run.ps1 2 sihsim_quadx
|
||||||
|
#>
|
||||||
|
param(
|
||||||
|
[int]$SitlNum = 2,
|
||||||
|
[string]$SimModel = 'gazebo-classic_iris',
|
||||||
|
[string]$BuildTarget = 'px4_sitl_default'
|
||||||
|
)
|
||||||
|
|
||||||
|
$ErrorActionPreference = 'Stop'
|
||||||
|
|
||||||
|
$ScriptDir = Split-Path -Parent $MyInvocation.MyCommand.Path
|
||||||
|
$SrcPath = (Resolve-Path (Join-Path $ScriptDir '..\..')).Path
|
||||||
|
$BuildPath = Join-Path $SrcPath "build\$BuildTarget"
|
||||||
|
|
||||||
|
# Prefer px4.exe; fall back to px4 so this also runs against a WSL/MinGW
|
||||||
|
# layout that drops the suffix.
|
||||||
|
$Px4Bin = Join-Path $BuildPath 'bin\px4.exe'
|
||||||
|
if (-not (Test-Path $Px4Bin)) { $Px4Bin = Join-Path $BuildPath 'bin\px4' }
|
||||||
|
if ($SitlNum -gt 0 -and -not (Test-Path $Px4Bin)) {
|
||||||
|
Write-Error "px4 binary not found in $BuildPath\bin"
|
||||||
|
}
|
||||||
|
|
||||||
|
Write-Host 'killing running instances'
|
||||||
|
Get-Process -Name 'px4' -ErrorAction SilentlyContinue | Stop-Process -Force -ErrorAction SilentlyContinue
|
||||||
|
Start-Sleep -Seconds 1
|
||||||
|
|
||||||
|
# Clean up stale lock files left by force-killed instances. Each instance
|
||||||
|
# uses %TEMP%\px4_lock-<N>; if the previous owner was killed before it could
|
||||||
|
# unlink, the next start sees "already running" and aborts.
|
||||||
|
Get-ChildItem -Path "$env:TEMP" -Filter 'px4_lock-*' -ErrorAction SilentlyContinue | Remove-Item -Force -ErrorAction SilentlyContinue
|
||||||
|
|
||||||
|
# Clear stale per-instance log files from previous runs. These are not
|
||||||
|
# truncated by the new launch (Start-Process -RedirectStandardOutput appends),
|
||||||
|
# so leftover content from a different rcS / different model can mislead
|
||||||
|
# debugging by appearing alongside fresh output.
|
||||||
|
if ($SitlNum -gt 0) {
|
||||||
|
for ($n = 0; $n -lt $SitlNum; $n++) {
|
||||||
|
$stale = Join-Path $BuildPath "instance_$n"
|
||||||
|
if (Test-Path $stale) {
|
||||||
|
Remove-Item -Path (Join-Path $stale 'out.log') -Force -ErrorAction SilentlyContinue
|
||||||
|
Remove-Item -Path (Join-Path $stale 'err.log') -Force -ErrorAction SilentlyContinue
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
$env:PX4_SIM_MODEL = $SimModel
|
||||||
|
|
||||||
|
for ($n = 0; $n -lt $SitlNum; $n++) {
|
||||||
|
$WorkingDir = Join-Path $BuildPath "instance_$n"
|
||||||
|
if (-not (Test-Path $WorkingDir)) { New-Item -ItemType Directory -Path $WorkingDir | Out-Null }
|
||||||
|
Write-Host "starting instance $n in $WorkingDir"
|
||||||
|
# Spawn px4.exe directly with redirected stdout/stderr.
|
||||||
|
#
|
||||||
|
# Earlier revisions wrapped each launch in `cmd.exe /c "px4.exe ... > out.log"`,
|
||||||
|
# which had a fatal flaw at higher instance counts: every cmd.exe child
|
||||||
|
# inherits the launcher's console, and when that console is torn down
|
||||||
|
# (cmd.exe exiting after spawning px4, or the launcher PowerShell window
|
||||||
|
# closing) Windows broadcasts CTRL_CLOSE_EVENT to every attached process.
|
||||||
|
# PX4's SetConsoleCtrlHandler maps CTRL_CLOSE_EVENT to SIGINT and shuts
|
||||||
|
# down cleanly, so multi-instance launches died silently a few seconds
|
||||||
|
# after rcS finished -- no error, empty stderr, just gone.
|
||||||
|
#
|
||||||
|
# Start-Process with -RedirectStandard* writes both streams to per-instance
|
||||||
|
# log files without involving cmd.exe, and -WindowStyle Hidden gives each
|
||||||
|
# child its own (hidden) console so it survives the launcher exiting.
|
||||||
|
$stdoutPath = Join-Path $WorkingDir 'out.log'
|
||||||
|
$stderrPath = Join-Path $WorkingDir 'err.log'
|
||||||
|
Start-Process -FilePath $Px4Bin `
|
||||||
|
-ArgumentList @('-i', $n, '-d', "$BuildPath\etc") `
|
||||||
|
-WorkingDirectory $WorkingDir `
|
||||||
|
-RedirectStandardOutput $stdoutPath `
|
||||||
|
-RedirectStandardError $stderrPath `
|
||||||
|
-WindowStyle Hidden | Out-Null
|
||||||
|
}
|
||||||
@@ -7,6 +7,9 @@
|
|||||||
# ./Tools/simulation/sitl_multiple_run.sh 3 sihsim_quadx px4_sitl_sih
|
# ./Tools/simulation/sitl_multiple_run.sh 3 sihsim_quadx px4_sitl_sih
|
||||||
# ./Tools/simulation/sitl_multiple_run.sh 2 gazebo-classic_iris px4_sitl_default
|
# ./Tools/simulation/sitl_multiple_run.sh 2 gazebo-classic_iris px4_sitl_default
|
||||||
# ./Tools/simulation/sitl_multiple_run.sh # defaults: 2 instances, gazebo-classic_iris, px4_sitl_default
|
# ./Tools/simulation/sitl_multiple_run.sh # defaults: 2 instances, gazebo-classic_iris, px4_sitl_default
|
||||||
|
#
|
||||||
|
# Pass 0 instances to just stop everything currently running:
|
||||||
|
# ./Tools/simulation/sitl_multiple_run.sh 0
|
||||||
|
|
||||||
sitl_num=${1:-2}
|
sitl_num=${1:-2}
|
||||||
sim_model=${2:-gazebo-classic_iris}
|
sim_model=${2:-gazebo-classic_iris}
|
||||||
@@ -17,8 +20,17 @@ src_path="$SCRIPT_DIR/../../"
|
|||||||
|
|
||||||
build_path=${src_path}/build/${build_target}
|
build_path=${src_path}/build/${build_target}
|
||||||
|
|
||||||
|
# Pick px4 vs px4.exe so this script also runs from Git Bash on Windows.
|
||||||
|
px4_bin="$build_path/bin/px4"
|
||||||
|
[ -x "$px4_bin.exe" ] && px4_bin="$px4_bin.exe"
|
||||||
|
|
||||||
echo "killing running instances"
|
echo "killing running instances"
|
||||||
pkill -x px4 || true
|
if command -v pkill >/dev/null 2>&1; then
|
||||||
|
pkill -x px4 || true
|
||||||
|
fi
|
||||||
|
if command -v taskkill >/dev/null 2>&1; then
|
||||||
|
taskkill //IM px4.exe //F >/dev/null 2>&1 || true
|
||||||
|
fi
|
||||||
|
|
||||||
sleep 1
|
sleep 1
|
||||||
|
|
||||||
@@ -31,7 +43,7 @@ while [ $n -lt $sitl_num ]; do
|
|||||||
|
|
||||||
pushd "$working_dir" &>/dev/null
|
pushd "$working_dir" &>/dev/null
|
||||||
echo "starting instance $n in $(pwd)"
|
echo "starting instance $n in $(pwd)"
|
||||||
$build_path/bin/px4 -i $n -d "$build_path/etc" >out.log 2>err.log &
|
"$px4_bin" -i $n -d "$build_path/etc" >out.log 2>err.log &
|
||||||
popd &>/dev/null
|
popd &>/dev/null
|
||||||
|
|
||||||
n=$(($n + 1))
|
n=$(($n + 1))
|
||||||
|
|||||||
@@ -46,7 +46,14 @@
|
|||||||
#endif
|
#endif
|
||||||
#define PX4LOG_REGULAR_FILE DT_REG
|
#define PX4LOG_REGULAR_FILE DT_REG
|
||||||
#define PX4LOG_DIRECTORY DT_DIR
|
#define PX4LOG_DIRECTORY DT_DIR
|
||||||
|
// Windows (MinGW) sets PATH_MAX to MAX_PATH=260. Use the larger of the
|
||||||
|
// platform PATH_MAX and 1024 so the buffer always fits the 1023-char
|
||||||
|
// scanf conversion target that the logger expects.
|
||||||
|
#if defined(__PX4_WINDOWS) && PATH_MAX < 1024
|
||||||
|
#define PX4_MAX_FILEPATH 1024
|
||||||
|
#else
|
||||||
#define PX4_MAX_FILEPATH PATH_MAX
|
#define PX4_MAX_FILEPATH PATH_MAX
|
||||||
|
#endif
|
||||||
#define PX4_MAX_FILEPATH_SCANF 1023
|
#define PX4_MAX_FILEPATH_SCANF 1023
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|||||||
@@ -383,7 +383,7 @@ Mavlink::destroy_all_instances()
|
|||||||
|
|
||||||
} else {
|
} else {
|
||||||
iterations++;
|
iterations++;
|
||||||
printf(".");
|
PX4_INFO_RAW(".");
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
px4_usleep(10000);
|
px4_usleep(10000);
|
||||||
}
|
}
|
||||||
@@ -422,7 +422,7 @@ Mavlink::get_status_all_instances(bool show_streams_status)
|
|||||||
|
|
||||||
for (Mavlink *inst : mavlink_module_instances) {
|
for (Mavlink *inst : mavlink_module_instances) {
|
||||||
if (inst != nullptr) {
|
if (inst != nullptr) {
|
||||||
printf("\ninstance #%u:\n", iterations);
|
PX4_INFO_RAW("\ninstance #%u:\n", iterations);
|
||||||
|
|
||||||
if (show_streams_status) {
|
if (show_streams_status) {
|
||||||
inst->display_status_streams();
|
inst->display_status_streams();
|
||||||
@@ -803,7 +803,7 @@ void Mavlink::send_finish()
|
|||||||
|
|
||||||
if (_src_addr_initialized) {
|
if (_src_addr_initialized) {
|
||||||
# endif // CONFIG_NET
|
# endif // CONFIG_NET
|
||||||
ret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
|
ret = sendto(_socket_fd, (const char *)_buf, _buf_fill, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
|
||||||
# if defined(CONFIG_NET)
|
# if defined(CONFIG_NET)
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -818,7 +818,7 @@ void Mavlink::send_finish()
|
|||||||
|
|
||||||
if (_broadcast_address_found && _buf_fill > 0) {
|
if (_broadcast_address_found && _buf_fill > 0) {
|
||||||
|
|
||||||
int bret = sendto(_socket_fd, _buf, _buf_fill, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
|
int bret = sendto(_socket_fd, (const char *)_buf, _buf_fill, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
|
||||||
|
|
||||||
if (bret <= 0) {
|
if (bret <= 0) {
|
||||||
if (!_broadcast_failed_warned) {
|
if (!_broadcast_failed_warned) {
|
||||||
@@ -980,7 +980,7 @@ void Mavlink::find_broadcast_address()
|
|||||||
|
|
||||||
int broadcast_opt = 1;
|
int broadcast_opt = 1;
|
||||||
|
|
||||||
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
|
if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, (const char *)&broadcast_opt, sizeof(broadcast_opt)) < 0) {
|
||||||
PX4_WARN("setting broadcast permission failed");
|
PX4_WARN("setting broadcast permission failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1035,7 +1035,7 @@ void Mavlink::init_udp()
|
|||||||
.tv_usec = UDP_SEND_TIMEOUT_US
|
.tv_usec = UDP_SEND_TIMEOUT_US
|
||||||
};
|
};
|
||||||
|
|
||||||
if (setsockopt(_socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)) < 0) {
|
if (setsockopt(_socket_fd, SOL_SOCKET, SO_SNDTIMEO, (const char *)&tv, sizeof(tv)) < 0) {
|
||||||
PX4_WARN("setting socket timeout failed: %s", strerror(errno));
|
PX4_WARN("setting socket timeout failed: %s", strerror(errno));
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -3075,12 +3075,17 @@ Mavlink::start(int argc, char *argv[])
|
|||||||
// between the starting task and the spawned
|
// between the starting task and the spawned
|
||||||
// task - start_helper() only returns
|
// task - start_helper() only returns
|
||||||
// when the started task exits.
|
// when the started task exits.
|
||||||
px4_task_spawn_cmd("mavlink_main",
|
const px4_task_t task_id = px4_task_spawn_cmd("mavlink_main",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_DEFAULT,
|
SCHED_PRIORITY_DEFAULT,
|
||||||
PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
|
PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
|
||||||
(px4_main_t)&Mavlink::start_helper,
|
(px4_main_t)&Mavlink::start_helper,
|
||||||
(char *const *)argv);
|
(char *const *)argv);
|
||||||
|
|
||||||
|
if (task_id < 0) {
|
||||||
|
PX4_ERR("task spawn failed: %d", task_id);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
// Ensure that this shell command
|
// Ensure that this shell command
|
||||||
// does not return before the instance
|
// does not return before the instance
|
||||||
@@ -3089,20 +3094,22 @@ Mavlink::start(int argc, char *argv[])
|
|||||||
// this is effectively a lock on concurrent
|
// this is effectively a lock on concurrent
|
||||||
// instance starting. XXX do a real lock.
|
// instance starting. XXX do a real lock.
|
||||||
|
|
||||||
// Sleep 500 us between each attempt
|
// Sleep 500 us between each attempt. This is a shell-side startup wait, so
|
||||||
|
// it must use wall time rather than simulated lockstep time.
|
||||||
const unsigned sleeptime = 500;
|
const unsigned sleeptime = 500;
|
||||||
|
|
||||||
// Wait 100 ms max for the startup.
|
// Wait 10 s max for the startup.
|
||||||
const unsigned limit = 100 * 1000 / sleeptime;
|
const unsigned limit = 10 * 1000 * 1000 / sleeptime;
|
||||||
|
|
||||||
unsigned count = 0;
|
unsigned count = 0;
|
||||||
|
|
||||||
while (ic == Mavlink::instance_count() && count < limit) {
|
while (ic == Mavlink::instance_count() && count < limit) {
|
||||||
px4_usleep(sleeptime);
|
system_usleep(sleeptime);
|
||||||
count++;
|
count++;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (ic == Mavlink::instance_count()) {
|
if (ic == Mavlink::instance_count()) {
|
||||||
|
PX4_ERR("start timed out");
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
@@ -3118,83 +3125,83 @@ Mavlink::display_status()
|
|||||||
#endif // !CONSTRAINED_FLASH
|
#endif // !CONSTRAINED_FLASH
|
||||||
|
|
||||||
if (_tstatus.heartbeat_type_gcs) {
|
if (_tstatus.heartbeat_type_gcs) {
|
||||||
printf("\tGCS heartbeat valid\n");
|
PX4_INFO_RAW("\tGCS heartbeat valid\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("\tmavlink chan: #%u\n", static_cast<unsigned>(_channel));
|
PX4_INFO_RAW("\tmavlink chan: #%u\n", static_cast<unsigned>(_channel));
|
||||||
|
|
||||||
if (_tstatus.timestamp > 0) {
|
if (_tstatus.timestamp > 0) {
|
||||||
|
|
||||||
printf("\ttype:\t\t");
|
PX4_INFO_RAW("\ttype:\t\t");
|
||||||
|
|
||||||
if (_radio_status_available) {
|
if (_radio_status_available) {
|
||||||
printf("RADIO Link\n");
|
PX4_INFO_RAW("RADIO Link\n");
|
||||||
printf("\t rssi:\t\t%" PRIu8 "\n", _rstatus.rssi);
|
PX4_INFO_RAW("\t rssi:\t\t%" PRIu8 "\n", _rstatus.rssi);
|
||||||
printf("\t remote rssi:\t%" PRIu8 "\n", _rstatus.remote_rssi);
|
PX4_INFO_RAW("\t remote rssi:\t%" PRIu8 "\n", _rstatus.remote_rssi);
|
||||||
printf("\t txbuf:\t%" PRIu8 "\n", _rstatus.txbuf);
|
PX4_INFO_RAW("\t txbuf:\t%" PRIu8 "\n", _rstatus.txbuf);
|
||||||
printf("\t noise:\t%" PRIu8 "\n", _rstatus.noise);
|
PX4_INFO_RAW("\t noise:\t%" PRIu8 "\n", _rstatus.noise);
|
||||||
printf("\t remote noise:\t%" PRIu8 "\n", _rstatus.remote_noise);
|
PX4_INFO_RAW("\t remote noise:\t%" PRIu8 "\n", _rstatus.remote_noise);
|
||||||
printf("\t rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
|
PX4_INFO_RAW("\t rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
|
||||||
printf("\t fixed:\t%" PRIu16 "\n", _rstatus.fix);
|
PX4_INFO_RAW("\t fixed:\t%" PRIu16 "\n", _rstatus.fix);
|
||||||
|
|
||||||
} else if (_tstatus.type == telemetry_status_s::LINK_TYPE_USB) {
|
} else if (_tstatus.type == telemetry_status_s::LINK_TYPE_USB) {
|
||||||
printf("USB CDC\n");
|
PX4_INFO_RAW("USB CDC\n");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("GENERIC LINK OR RADIO\n");
|
PX4_INFO_RAW("GENERIC LINK OR RADIO\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("\tno radio status.\n");
|
PX4_INFO_RAW("\tno radio status.\n");
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
|
PX4_INFO_RAW("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
|
||||||
printf("\trates:\n");
|
PX4_INFO_RAW("\trates:\n");
|
||||||
printf("\t tx: %.1f B/s\n", (double)_tstatus.tx_rate_avg);
|
PX4_INFO_RAW("\t tx: %.1f B/s\n", (double)_tstatus.tx_rate_avg);
|
||||||
printf("\t txerr: %.1f B/s\n", (double)_tstatus.tx_error_rate_avg);
|
PX4_INFO_RAW("\t txerr: %.1f B/s\n", (double)_tstatus.tx_error_rate_avg);
|
||||||
printf("\t tx rate mult: %.3f\n", (double)_rate_mult);
|
PX4_INFO_RAW("\t tx rate mult: %.3f\n", (double)_rate_mult);
|
||||||
printf("\t tx rate max: %i B/s\n", _datarate);
|
PX4_INFO_RAW("\t tx rate max: %i B/s\n", _datarate);
|
||||||
printf("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg);
|
PX4_INFO_RAW("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg);
|
||||||
printf("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate);
|
PX4_INFO_RAW("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate);
|
||||||
|
|
||||||
#if !defined(CONSTRAINED_FLASH)
|
#if !defined(CONSTRAINED_FLASH)
|
||||||
_receiver.print_detailed_rx_stats();
|
_receiver.print_detailed_rx_stats();
|
||||||
#endif // !CONSTRAINED_FLASH
|
#endif // !CONSTRAINED_FLASH
|
||||||
|
|
||||||
if (_mavlink_ulog) {
|
if (_mavlink_ulog) {
|
||||||
printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
|
PX4_INFO_RAW("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
|
||||||
(double)_mavlink_ulog->maximum_data_rate() * 100.);
|
(double)_mavlink_ulog->maximum_data_rate() * 100.);
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("\tFTP enabled: %s, TX enabled: %s\n",
|
PX4_INFO_RAW("\tFTP enabled: %s, TX enabled: %s\n",
|
||||||
_ftp_on ? "YES" : "NO",
|
_ftp_on ? "YES" : "NO",
|
||||||
_transmitting_enabled ? "YES" : "NO");
|
_transmitting_enabled ? "YES" : "NO");
|
||||||
printf("\tmode: %s\n", mavlink_mode_str(_mode));
|
PX4_INFO_RAW("\tmode: %s\n", mavlink_mode_str(_mode));
|
||||||
|
|
||||||
if (_mode == MAVLINK_MODE_IRIDIUM) {
|
if (_mode == MAVLINK_MODE_IRIDIUM) {
|
||||||
printf("\t iridium tx freq: %.3f\n", (double)(_high_latency_freq));
|
PX4_INFO_RAW("\t iridium tx freq: %.3f\n", (double)(_high_latency_freq));
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off");
|
PX4_INFO_RAW("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off");
|
||||||
printf("\tMAVLink version: %" PRId8 "\n", _protocol_version);
|
PX4_INFO_RAW("\tMAVLink version: %" PRId8 "\n", _protocol_version);
|
||||||
|
|
||||||
printf("\ttransport protocol: ");
|
PX4_INFO_RAW("\ttransport protocol: ");
|
||||||
|
|
||||||
switch (_protocol) {
|
switch (_protocol) {
|
||||||
#if defined(MAVLINK_UDP)
|
#if defined(MAVLINK_UDP)
|
||||||
|
|
||||||
case Protocol::UDP:
|
case Protocol::UDP:
|
||||||
printf("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port);
|
PX4_INFO_RAW("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port);
|
||||||
printf("\tBroadcast enabled: %s\n",
|
PX4_INFO_RAW("\tBroadcast enabled: %s\n",
|
||||||
broadcast_enabled() ? "YES" : "NO");
|
broadcast_enabled() ? "YES" : "NO");
|
||||||
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
|
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
|
||||||
printf("\tMulticast enabled: %s\n",
|
PX4_INFO_RAW("\tMulticast enabled: %s\n",
|
||||||
multicast_enabled() ? "YES" : "NO");
|
multicast_enabled() ? "YES" : "NO");
|
||||||
#endif
|
#endif
|
||||||
#ifdef __PX4_POSIX
|
#ifdef __PX4_POSIX
|
||||||
|
|
||||||
if (get_client_source_initialized()) {
|
if (get_client_source_initialized()) {
|
||||||
printf("\tpartner IP: %s\n", inet_ntoa(get_client_source_address().sin_addr));
|
PX4_INFO_RAW("\tpartner IP: %s\n", inet_ntoa(get_client_source_address().sin_addr));
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
@@ -3202,24 +3209,24 @@ Mavlink::display_status()
|
|||||||
#endif // MAVLINK_UDP
|
#endif // MAVLINK_UDP
|
||||||
|
|
||||||
case Protocol::SERIAL:
|
case Protocol::SERIAL:
|
||||||
printf("serial (%s @%i)\n", _device_name, _baudrate);
|
PX4_INFO_RAW("serial (%s @%i)\n", _device_name, _baudrate);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_ping_stats.last_ping_time > 0) {
|
if (_ping_stats.last_ping_time > 0) {
|
||||||
printf("\tping statistics:\n");
|
PX4_INFO_RAW("\tping statistics:\n");
|
||||||
printf("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt);
|
PX4_INFO_RAW("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt);
|
||||||
printf("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt);
|
PX4_INFO_RAW("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt);
|
||||||
printf("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt);
|
PX4_INFO_RAW("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt);
|
||||||
printf("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt);
|
PX4_INFO_RAW("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt);
|
||||||
printf("\t dropped packets: %" PRIi32 "\n", _ping_stats.dropped_packets);
|
PX4_INFO_RAW("\t dropped packets: %" PRIi32 "\n", _ping_stats.dropped_packets);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Mavlink::display_status_streams()
|
Mavlink::display_status_streams()
|
||||||
{
|
{
|
||||||
printf("\t%-20s%-16s %s\n", "Name", "Rate Config (current) [Hz]", "Message Size (if active) [B]");
|
PX4_INFO_RAW("\t%-20s%-16s %s\n", "Name", "Rate Config (current) [Hz]", "Message Size (if active) [B]");
|
||||||
|
|
||||||
const float rate_mult = _rate_mult;
|
const float rate_mult = _rate_mult;
|
||||||
|
|
||||||
@@ -3239,13 +3246,13 @@ Mavlink::display_status_streams()
|
|||||||
snprintf(rate_str, sizeof(rate_str), "%6.2f (%.3f)", (double)rate, (double)rate_current);
|
snprintf(rate_str, sizeof(rate_str), "%6.2f (%.3f)", (double)rate, (double)rate_current);
|
||||||
}
|
}
|
||||||
|
|
||||||
printf("\t%-30s%-16s", stream->get_name(), rate_str);
|
PX4_INFO_RAW("\t%-30s%-16s", stream->get_name(), rate_str);
|
||||||
|
|
||||||
if (size > 0) {
|
if (size > 0) {
|
||||||
printf(" %3u\n", size);
|
PX4_INFO_RAW(" %3u\n", size);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
printf("\n");
|
PX4_INFO_RAW("\n");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -78,6 +78,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) :
|
|||||||
if (!_dataman_init) {
|
if (!_dataman_init) {
|
||||||
_dataman_init = true;
|
_dataman_init = true;
|
||||||
|
|
||||||
|
#if defined(ENABLE_LOCKSTEP_SCHEDULER) && defined(__PX4_WINDOWS)
|
||||||
|
// Avoid blocking MAVLink startup on dataman during Windows lockstep initialization.
|
||||||
|
#else
|
||||||
mission_s mission_state;
|
mission_s mission_state;
|
||||||
bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
|
bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
|
||||||
sizeof(mission_s));
|
sizeof(mission_s));
|
||||||
@@ -91,6 +94,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) :
|
|||||||
PX4_WARN("offboard mission init failed");
|
PX4_WARN("offboard mission init failed");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq,
|
update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq,
|
||||||
_crc32[MAV_MISSION_TYPE_MISSION], false);
|
_crc32[MAV_MISSION_TYPE_MISSION], false);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -3411,7 +3411,7 @@ MavlinkReceiver::run()
|
|||||||
|
|
||||||
else if (_mavlink.get_protocol() == Protocol::UDP) {
|
else if (_mavlink.get_protocol() == Protocol::UDP) {
|
||||||
if (fds[0].revents & POLLIN) {
|
if (fds[0].revents & POLLIN) {
|
||||||
nread = recvfrom(_mavlink.get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
|
nread = recvfrom(_mavlink.get_socket_fd(), (char *)buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
|
||||||
}
|
}
|
||||||
|
|
||||||
struct sockaddr_in &srcaddr_last = _mavlink.get_client_source_address();
|
struct sockaddr_in &srcaddr_last = _mavlink.get_client_source_address();
|
||||||
@@ -3678,13 +3678,13 @@ void MavlinkReceiver::print_detailed_rx_stats() const
|
|||||||
{
|
{
|
||||||
// TODO: add mutex around shared data.
|
// TODO: add mutex around shared data.
|
||||||
if (_component_states_count > 0) {
|
if (_component_states_count > 0) {
|
||||||
printf("\tReceived Messages:\n");
|
PX4_INFO_RAW("\tReceived Messages:\n");
|
||||||
|
|
||||||
for (const auto &comp_stat : _component_states) {
|
for (const auto &comp_stat : _component_states) {
|
||||||
if (comp_stat.received_messages > 0) {
|
if (comp_stat.received_messages > 0) {
|
||||||
printf("\t sysid:%3" PRIu8 ", compid:%3" PRIu8 ", Total: %" PRIu32 " (lost: %" PRIu32 ")\n",
|
PX4_INFO_RAW("\t sysid:%3" PRIu8 ", compid:%3" PRIu8 ", Total: %" PRIu32 " (lost: %" PRIu32 ")\n",
|
||||||
comp_stat.system_id, comp_stat.component_id,
|
comp_stat.system_id, comp_stat.component_id,
|
||||||
comp_stat.received_messages, comp_stat.missed_messages);
|
comp_stat.received_messages, comp_stat.missed_messages);
|
||||||
|
|
||||||
#if !defined(CONSTRAINED_FLASH)
|
#if !defined(CONSTRAINED_FLASH)
|
||||||
|
|
||||||
@@ -3702,8 +3702,8 @@ void MavlinkReceiver::print_detailed_rx_stats() const
|
|||||||
|
|
||||||
const float elapsed_s = (now_ms - msg_stat.last_time_received_ms) / 1000.f;
|
const float elapsed_s = (now_ms - msg_stat.last_time_received_ms) / 1000.f;
|
||||||
|
|
||||||
printf("\t msgid:%5" PRIu16 ", Rate:%5.1f Hz, last %.2fs ago\n",
|
PX4_INFO_RAW("\t msgid:%5" PRIu16 ", Rate:%5.1f Hz, last %.2fs ago\n",
|
||||||
msg_stat.msg_id, (double)msg_stat.avg_rate_hz, (double)elapsed_s);
|
msg_stat.msg_id, (double)msg_stat.avg_rate_hz, (double)elapsed_s);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -226,7 +226,7 @@ size_t MavlinkShell::available()
|
|||||||
{
|
{
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
||||||
if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
|
if (ioctl(_from_shell_fd, FIONREAD, &ret) == OK) {
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -195,7 +195,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
|
|||||||
}
|
}
|
||||||
|
|
||||||
// accel
|
// accel
|
||||||
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
|
if ((sensors.fields_updated & SensorSource::ACCEL_XYZ) == SensorSource::ACCEL_XYZ) {
|
||||||
if (sensors.id >= ACCEL_COUNT_MAX) {
|
if (sensors.id >= ACCEL_COUNT_MAX) {
|
||||||
PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX);
|
PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX);
|
||||||
return;
|
return;
|
||||||
@@ -238,7 +238,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
|
|||||||
}
|
}
|
||||||
|
|
||||||
// gyro
|
// gyro
|
||||||
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
|
if ((sensors.fields_updated & SensorSource::GYRO_XYZ) == SensorSource::GYRO_XYZ) {
|
||||||
if (sensors.id >= GYRO_COUNT_MAX) {
|
if (sensors.id >= GYRO_COUNT_MAX) {
|
||||||
PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX);
|
PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX);
|
||||||
return;
|
return;
|
||||||
@@ -281,7 +281,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
|
|||||||
}
|
}
|
||||||
|
|
||||||
// magnetometer
|
// magnetometer
|
||||||
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG) {
|
if ((sensors.fields_updated & SensorSource::MAG_XYZ) == SensorSource::MAG_XYZ) {
|
||||||
if (sensors.id >= MAG_COUNT_MAX) {
|
if (sensors.id >= MAG_COUNT_MAX) {
|
||||||
PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, MAG_COUNT_MAX);
|
PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, MAG_COUNT_MAX);
|
||||||
return;
|
return;
|
||||||
@@ -1003,10 +1003,10 @@ void SimulatorMavlink::send_mavlink_message(const mavlink_message_t &aMsg)
|
|||||||
ssize_t len;
|
ssize_t len;
|
||||||
|
|
||||||
if (_ip == InternetProtocol::UDP) {
|
if (_ip == InternetProtocol::UDP) {
|
||||||
len = ::sendto(_fd, buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
|
len = ::sendto(_fd, (const char *)buf, bufLen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr));
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
len = ::send(_fd, buf, bufLen, 0);
|
len = ::send(_fd, (const char *)buf, bufLen, 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (len <= 0) {
|
if (len <= 0) {
|
||||||
@@ -1134,7 +1134,18 @@ void SimulatorMavlink::run()
|
|||||||
|
|
||||||
if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
|
if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
|
||||||
PX4_ERR("bind for UDP port %i failed (%i)", _port, errno);
|
PX4_ERR("bind for UDP port %i failed (%i)", _port, errno);
|
||||||
|
#ifdef __PX4_WINDOWS
|
||||||
|
// Winsock SOCKETs are kernel handles, not CRT fds — using ::close()
|
||||||
|
// trips the UCRT _close.cpp assertion `(fh >= 0 && (unsigned)fh < (unsigned)_nhandle)`.
|
||||||
|
closesocket(_fd);
|
||||||
|
#else
|
||||||
::close(_fd);
|
::close(_fd);
|
||||||
|
#endif
|
||||||
|
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||||
|
// Unblock the lockstep wait in simulator_mavlink_main so the
|
||||||
|
// rest of the startup script (including shutdown) can run.
|
||||||
|
_has_initialized.store(true);
|
||||||
|
#endif
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1142,7 +1153,7 @@ void SimulatorMavlink::run()
|
|||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
// Once we receive something, we're most probably good and can carry on.
|
// Once we receive something, we're most probably good and can carry on.
|
||||||
int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0,
|
int len = ::recvfrom(_fd, (char *)_buf, sizeof(_buf), 0,
|
||||||
(struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen);
|
(struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen);
|
||||||
|
|
||||||
if (len > 0) {
|
if (len > 0) {
|
||||||
@@ -1181,7 +1192,12 @@ void SimulatorMavlink::run()
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
#ifdef __PX4_WINDOWS
|
||||||
|
// See note above: Winsock SOCKETs need closesocket(), not ::close().
|
||||||
|
closesocket(_fd);
|
||||||
|
#else
|
||||||
::close(_fd);
|
::close(_fd);
|
||||||
|
#endif
|
||||||
system_usleep(500);
|
system_usleep(500);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1237,7 +1253,7 @@ void SimulatorMavlink::run()
|
|||||||
|
|
||||||
if (fds[0].revents & POLLIN) {
|
if (fds[0].revents & POLLIN) {
|
||||||
|
|
||||||
int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen);
|
int len = ::recvfrom(_fd, (char *)_buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen);
|
||||||
|
|
||||||
if (len > 0) {
|
if (len > 0) {
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
|
|||||||
@@ -87,9 +87,9 @@ using namespace time_literals;
|
|||||||
|
|
||||||
//! Enumeration to use on the bitmask in HIL_SENSOR
|
//! Enumeration to use on the bitmask in HIL_SENSOR
|
||||||
enum class SensorSource {
|
enum class SensorSource {
|
||||||
ACCEL = 0b111,
|
ACCEL_XYZ = 0b111,
|
||||||
GYRO = 0b111000,
|
GYRO_XYZ = 0b111000,
|
||||||
MAG = 0b111000000,
|
MAG_XYZ = 0b111000000,
|
||||||
BARO = 0b1101000000000,
|
BARO = 0b1101000000000,
|
||||||
DIFF_PRESS = 0b10000000000
|
DIFF_PRESS = 0b10000000000
|
||||||
};
|
};
|
||||||
|
|||||||
Submodule src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client updated: 711aef423e...2884794316
@@ -65,7 +65,7 @@ static void configure_udp_socket_nonblocking(int fd)
|
|||||||
// Prevent deadlocks in send() under RX buffer starvation by making the socket non-blocking.
|
// Prevent deadlocks in send() under RX buffer starvation by making the socket non-blocking.
|
||||||
int nonblock = 1;
|
int nonblock = 1;
|
||||||
|
|
||||||
if (ioctl(fd, FIONBIO, (unsigned long)&nonblock) != OK) {
|
if (ioctl(fd, FIONBIO, &nonblock) != OK) {
|
||||||
PX4_WARN("failed to set non-blocking (%i)", errno);
|
PX4_WARN("failed to set non-blocking (%i)", errno);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -133,6 +133,7 @@ bool UxrceddsClient::init()
|
|||||||
deinit();
|
deinit();
|
||||||
|
|
||||||
if (_transport == Transport::Serial) {
|
if (_transport == Transport::Serial) {
|
||||||
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
int fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
int fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||||
|
|
||||||
if (fd < 0) {
|
if (fd < 0) {
|
||||||
@@ -165,6 +166,10 @@ bool UxrceddsClient::init()
|
|||||||
_transport_serial = nullptr;
|
_transport_serial = nullptr;
|
||||||
|
|
||||||
return false;
|
return false;
|
||||||
|
#else
|
||||||
|
PX4_ERR("serial transport not supported on this platform");
|
||||||
|
return false;
|
||||||
|
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(UXRCE_DDS_CLIENT_UDP)
|
#if defined(UXRCE_DDS_CLIENT_UDP)
|
||||||
@@ -185,6 +190,14 @@ bool UxrceddsClient::init()
|
|||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_ERR("init UDP agent IP:%s, port:%s failed", _agent_ip, _port);
|
PX4_ERR("init UDP agent IP:%s, port:%s failed", _agent_ip, _port);
|
||||||
|
|
||||||
|
// Symmetric with the serial path above: free the half-initialised
|
||||||
|
// transport here rather than leaving it dangling for the next
|
||||||
|
// init() call to clean up. uxr_init_udp_platform() now releases
|
||||||
|
// its socket / WSAStartup ref on its own failure paths, so
|
||||||
|
// dropping the C++ wrapper here does not leak Winsock state.
|
||||||
|
delete _transport_udp;
|
||||||
|
_transport_udp = nullptr;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -195,12 +208,16 @@ bool UxrceddsClient::init()
|
|||||||
|
|
||||||
void UxrceddsClient::deinit()
|
void UxrceddsClient::deinit()
|
||||||
{
|
{
|
||||||
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
|
|
||||||
if (_transport_serial) {
|
if (_transport_serial) {
|
||||||
uxr_close_serial_transport(_transport_serial);
|
uxr_close_serial_transport(_transport_serial);
|
||||||
delete _transport_serial;
|
delete _transport_serial;
|
||||||
_transport_serial = nullptr;
|
_transport_serial = nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||||
|
|
||||||
#if defined(UXRCE_DDS_CLIENT_UDP)
|
#if defined(UXRCE_DDS_CLIENT_UDP)
|
||||||
|
|
||||||
if (_transport_udp) {
|
if (_transport_udp) {
|
||||||
@@ -417,11 +434,15 @@ UxrceddsClient::~UxrceddsClient()
|
|||||||
|
|
||||||
delete_repliers();
|
delete_repliers();
|
||||||
|
|
||||||
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
|
|
||||||
if (_transport_serial) {
|
if (_transport_serial) {
|
||||||
uxr_close_serial_transport(_transport_serial);
|
uxr_close_serial_transport(_transport_serial);
|
||||||
delete _transport_serial;
|
delete _transport_serial;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||||
|
|
||||||
perf_free(_loop_perf);
|
perf_free(_loop_perf);
|
||||||
perf_free(_loop_interval_perf);
|
perf_free(_loop_interval_perf);
|
||||||
|
|
||||||
@@ -710,7 +731,7 @@ void UxrceddsClient::run()
|
|||||||
int fd_bytes_available = 0;
|
int fd_bytes_available = 0;
|
||||||
|
|
||||||
if ((_fd < 0)
|
if ((_fd < 0)
|
||||||
|| (ioctl(_fd, FIONREAD, (unsigned long)&fd_bytes_available) != OK)
|
|| (ioctl(_fd, FIONREAD, &fd_bytes_available) != OK)
|
||||||
|| (fd_bytes_available <= 0)) {
|
|| (fd_bytes_available <= 0)) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@@ -1000,10 +1021,14 @@ int UxrceddsClient::print_status()
|
|||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
|
|
||||||
if (_transport_serial != nullptr) {
|
if (_transport_serial != nullptr) {
|
||||||
PX4_INFO("Using transport: serial");
|
PX4_INFO("Using transport: serial");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||||
|
|
||||||
if (_connected) {
|
if (_connected) {
|
||||||
PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
|
PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
|
||||||
PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
|
PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
|
||||||
|
|||||||
@@ -50,11 +50,27 @@
|
|||||||
# define UXRCE_DDS_CLIENT_UDP 1
|
# define UXRCE_DDS_CLIENT_UDP 1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// Micro-XRCE-DDS-Client's Windows platform has no serial transport — its
|
||||||
|
// CMakeLists forces UCLIENT_PROFILE_SERIAL OFF under UCLIENT_PLATFORM_WINDOWS,
|
||||||
|
// so the `uxrSerialTransport` type simply isn't emitted. Gate PX4's
|
||||||
|
// serial wiring on the same build-time knob so the whole module keeps
|
||||||
|
// compiling on Windows while serial stays available on NuttX/Linux.
|
||||||
|
#if !defined(__PX4_WINDOWS)
|
||||||
|
# define UXRCE_DDS_CLIENT_SERIAL 1
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "srv_base.h"
|
#include "srv_base.h"
|
||||||
|
|
||||||
#define MAX_NUM_REPLIERS 5
|
#define MAX_NUM_REPLIERS 5
|
||||||
#define STREAM_HISTORY 4
|
#define STREAM_HISTORY 4
|
||||||
#define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY) // MTU==512 by default
|
// Pick the largest MTU across the profiles enabled for this build. The
|
||||||
|
// serial transport's MTU is the historical cap, so on platforms where
|
||||||
|
// serial is compiled out (Windows) we fall back to the UDP MTU.
|
||||||
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
|
# define BUFFER_SIZE (UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY)
|
||||||
|
#else
|
||||||
|
# define BUFFER_SIZE (UXR_CONFIG_UDP_TRANSPORT_MTU * STREAM_HISTORY)
|
||||||
|
#endif
|
||||||
|
|
||||||
class UxrceddsClient : public ModuleBase, public ModuleParams
|
class UxrceddsClient : public ModuleBase, public ModuleParams
|
||||||
{
|
{
|
||||||
@@ -144,7 +160,9 @@ private:
|
|||||||
|
|
||||||
Transport _transport{};
|
Transport _transport{};
|
||||||
|
|
||||||
uxrSerialTransport *_transport_serial{nullptr};
|
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||||
|
uxrSerialTransport *_transport_serial {nullptr};
|
||||||
|
#endif
|
||||||
char _device[32] {};
|
char _device[32] {};
|
||||||
int _baudrate{};
|
int _baudrate{};
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user