mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 11:06:04 +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:
Nuno Marques
@@ -16,7 +16,8 @@ param set-default SENS_EN_BAROSIM 1
|
||||
param set-default SENS_EN_MAGSIM 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_IXX 0.00402
|
||||
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_FF 0.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 2 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}
|
||||
sim_model=${2:-gazebo-classic_iris}
|
||||
@@ -17,8 +20,17 @@ src_path="$SCRIPT_DIR/../../"
|
||||
|
||||
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"
|
||||
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
|
||||
|
||||
@@ -31,7 +43,7 @@ while [ $n -lt $sitl_num ]; do
|
||||
|
||||
pushd "$working_dir" &>/dev/null
|
||||
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
|
||||
|
||||
n=$(($n + 1))
|
||||
|
||||
@@ -46,7 +46,14 @@
|
||||
#endif
|
||||
#define PX4LOG_REGULAR_FILE DT_REG
|
||||
#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
|
||||
#endif
|
||||
#define PX4_MAX_FILEPATH_SCANF 1023
|
||||
#endif
|
||||
|
||||
|
||||
@@ -383,7 +383,7 @@ Mavlink::destroy_all_instances()
|
||||
|
||||
} else {
|
||||
iterations++;
|
||||
printf(".");
|
||||
PX4_INFO_RAW(".");
|
||||
fflush(stdout);
|
||||
px4_usleep(10000);
|
||||
}
|
||||
@@ -422,7 +422,7 @@ Mavlink::get_status_all_instances(bool show_streams_status)
|
||||
|
||||
for (Mavlink *inst : mavlink_module_instances) {
|
||||
if (inst != nullptr) {
|
||||
printf("\ninstance #%u:\n", iterations);
|
||||
PX4_INFO_RAW("\ninstance #%u:\n", iterations);
|
||||
|
||||
if (show_streams_status) {
|
||||
inst->display_status_streams();
|
||||
@@ -803,7 +803,7 @@ void Mavlink::send_finish()
|
||||
|
||||
if (_src_addr_initialized) {
|
||||
# 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)
|
||||
}
|
||||
|
||||
@@ -818,7 +818,7 @@ void Mavlink::send_finish()
|
||||
|
||||
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 (!_broadcast_failed_warned) {
|
||||
@@ -980,7 +980,7 @@ void Mavlink::find_broadcast_address()
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
@@ -1035,7 +1035,7 @@ void Mavlink::init_udp()
|
||||
.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));
|
||||
}
|
||||
|
||||
@@ -3075,12 +3075,17 @@ Mavlink::start(int argc, char *argv[])
|
||||
// between the starting task and the spawned
|
||||
// task - start_helper() only returns
|
||||
// when the started task exits.
|
||||
px4_task_spawn_cmd("mavlink_main",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
|
||||
(px4_main_t)&Mavlink::start_helper,
|
||||
(char *const *)argv);
|
||||
const px4_task_t task_id = px4_task_spawn_cmd("mavlink_main",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
|
||||
(px4_main_t)&Mavlink::start_helper,
|
||||
(char *const *)argv);
|
||||
|
||||
if (task_id < 0) {
|
||||
PX4_ERR("task spawn failed: %d", task_id);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Ensure that this shell command
|
||||
// does not return before the instance
|
||||
@@ -3089,20 +3094,22 @@ Mavlink::start(int argc, char *argv[])
|
||||
// this is effectively a lock on concurrent
|
||||
// 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;
|
||||
|
||||
// Wait 100 ms max for the startup.
|
||||
const unsigned limit = 100 * 1000 / sleeptime;
|
||||
// Wait 10 s max for the startup.
|
||||
const unsigned limit = 10 * 1000 * 1000 / sleeptime;
|
||||
|
||||
unsigned count = 0;
|
||||
|
||||
while (ic == Mavlink::instance_count() && count < limit) {
|
||||
px4_usleep(sleeptime);
|
||||
system_usleep(sleeptime);
|
||||
count++;
|
||||
}
|
||||
|
||||
if (ic == Mavlink::instance_count()) {
|
||||
PX4_ERR("start timed out");
|
||||
return PX4_ERROR;
|
||||
|
||||
} else {
|
||||
@@ -3118,83 +3125,83 @@ Mavlink::display_status()
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
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) {
|
||||
|
||||
printf("\ttype:\t\t");
|
||||
PX4_INFO_RAW("\ttype:\t\t");
|
||||
|
||||
if (_radio_status_available) {
|
||||
printf("RADIO Link\n");
|
||||
printf("\t rssi:\t\t%" PRIu8 "\n", _rstatus.rssi);
|
||||
printf("\t remote rssi:\t%" PRIu8 "\n", _rstatus.remote_rssi);
|
||||
printf("\t txbuf:\t%" PRIu8 "\n", _rstatus.txbuf);
|
||||
printf("\t noise:\t%" PRIu8 "\n", _rstatus.noise);
|
||||
printf("\t remote noise:\t%" PRIu8 "\n", _rstatus.remote_noise);
|
||||
printf("\t rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
|
||||
printf("\t fixed:\t%" PRIu16 "\n", _rstatus.fix);
|
||||
PX4_INFO_RAW("RADIO Link\n");
|
||||
PX4_INFO_RAW("\t rssi:\t\t%" PRIu8 "\n", _rstatus.rssi);
|
||||
PX4_INFO_RAW("\t remote rssi:\t%" PRIu8 "\n", _rstatus.remote_rssi);
|
||||
PX4_INFO_RAW("\t txbuf:\t%" PRIu8 "\n", _rstatus.txbuf);
|
||||
PX4_INFO_RAW("\t noise:\t%" PRIu8 "\n", _rstatus.noise);
|
||||
PX4_INFO_RAW("\t remote noise:\t%" PRIu8 "\n", _rstatus.remote_noise);
|
||||
PX4_INFO_RAW("\t rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
|
||||
PX4_INFO_RAW("\t fixed:\t%" PRIu16 "\n", _rstatus.fix);
|
||||
|
||||
} else if (_tstatus.type == telemetry_status_s::LINK_TYPE_USB) {
|
||||
printf("USB CDC\n");
|
||||
PX4_INFO_RAW("USB CDC\n");
|
||||
|
||||
} else {
|
||||
printf("GENERIC LINK OR RADIO\n");
|
||||
PX4_INFO_RAW("GENERIC LINK OR RADIO\n");
|
||||
}
|
||||
|
||||
} else {
|
||||
printf("\tno radio status.\n");
|
||||
PX4_INFO_RAW("\tno radio status.\n");
|
||||
}
|
||||
|
||||
printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
|
||||
printf("\trates:\n");
|
||||
printf("\t tx: %.1f B/s\n", (double)_tstatus.tx_rate_avg);
|
||||
printf("\t txerr: %.1f B/s\n", (double)_tstatus.tx_error_rate_avg);
|
||||
printf("\t tx rate mult: %.3f\n", (double)_rate_mult);
|
||||
printf("\t tx rate max: %i B/s\n", _datarate);
|
||||
printf("\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("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
|
||||
PX4_INFO_RAW("\trates:\n");
|
||||
PX4_INFO_RAW("\t tx: %.1f B/s\n", (double)_tstatus.tx_rate_avg);
|
||||
PX4_INFO_RAW("\t txerr: %.1f B/s\n", (double)_tstatus.tx_error_rate_avg);
|
||||
PX4_INFO_RAW("\t tx rate mult: %.3f\n", (double)_rate_mult);
|
||||
PX4_INFO_RAW("\t tx rate max: %i B/s\n", _datarate);
|
||||
PX4_INFO_RAW("\t rx: %.1f B/s\n", (double)_tstatus.rx_rate_avg);
|
||||
PX4_INFO_RAW("\t rx loss: %.1f%%\n", (double)_tstatus.rx_message_lost_rate);
|
||||
|
||||
#if !defined(CONSTRAINED_FLASH)
|
||||
_receiver.print_detailed_rx_stats();
|
||||
#endif // !CONSTRAINED_FLASH
|
||||
|
||||
if (_mavlink_ulog) {
|
||||
printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
|
||||
(double)_mavlink_ulog->maximum_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.);
|
||||
}
|
||||
|
||||
printf("\tFTP enabled: %s, TX enabled: %s\n",
|
||||
_ftp_on ? "YES" : "NO",
|
||||
_transmitting_enabled ? "YES" : "NO");
|
||||
printf("\tmode: %s\n", mavlink_mode_str(_mode));
|
||||
PX4_INFO_RAW("\tFTP enabled: %s, TX enabled: %s\n",
|
||||
_ftp_on ? "YES" : "NO",
|
||||
_transmitting_enabled ? "YES" : "NO");
|
||||
PX4_INFO_RAW("\tmode: %s\n", mavlink_mode_str(_mode));
|
||||
|
||||
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");
|
||||
printf("\tMAVLink version: %" PRId8 "\n", _protocol_version);
|
||||
PX4_INFO_RAW("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off");
|
||||
PX4_INFO_RAW("\tMAVLink version: %" PRId8 "\n", _protocol_version);
|
||||
|
||||
printf("\ttransport protocol: ");
|
||||
PX4_INFO_RAW("\ttransport protocol: ");
|
||||
|
||||
switch (_protocol) {
|
||||
#if defined(MAVLINK_UDP)
|
||||
|
||||
case Protocol::UDP:
|
||||
printf("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port);
|
||||
printf("\tBroadcast enabled: %s\n",
|
||||
broadcast_enabled() ? "YES" : "NO");
|
||||
PX4_INFO_RAW("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port);
|
||||
PX4_INFO_RAW("\tBroadcast enabled: %s\n",
|
||||
broadcast_enabled() ? "YES" : "NO");
|
||||
#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
|
||||
printf("\tMulticast enabled: %s\n",
|
||||
multicast_enabled() ? "YES" : "NO");
|
||||
PX4_INFO_RAW("\tMulticast enabled: %s\n",
|
||||
multicast_enabled() ? "YES" : "NO");
|
||||
#endif
|
||||
#ifdef __PX4_POSIX
|
||||
|
||||
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
|
||||
@@ -3202,24 +3209,24 @@ Mavlink::display_status()
|
||||
#endif // MAVLINK_UDP
|
||||
|
||||
case Protocol::SERIAL:
|
||||
printf("serial (%s @%i)\n", _device_name, _baudrate);
|
||||
PX4_INFO_RAW("serial (%s @%i)\n", _device_name, _baudrate);
|
||||
break;
|
||||
}
|
||||
|
||||
if (_ping_stats.last_ping_time > 0) {
|
||||
printf("\tping statistics:\n");
|
||||
printf("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt);
|
||||
printf("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt);
|
||||
printf("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt);
|
||||
printf("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt);
|
||||
printf("\t dropped packets: %" PRIi32 "\n", _ping_stats.dropped_packets);
|
||||
PX4_INFO_RAW("\tping statistics:\n");
|
||||
PX4_INFO_RAW("\t last: %0.2f ms\n", (double)_ping_stats.last_rtt);
|
||||
PX4_INFO_RAW("\t mean: %0.2f ms\n", (double)_ping_stats.mean_rtt);
|
||||
PX4_INFO_RAW("\t max: %0.2f ms\n", (double)_ping_stats.max_rtt);
|
||||
PX4_INFO_RAW("\t min: %0.2f ms\n", (double)_ping_stats.min_rtt);
|
||||
PX4_INFO_RAW("\t dropped packets: %" PRIi32 "\n", _ping_stats.dropped_packets);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
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;
|
||||
|
||||
@@ -3239,13 +3246,13 @@ Mavlink::display_status_streams()
|
||||
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) {
|
||||
printf(" %3u\n", size);
|
||||
PX4_INFO_RAW(" %3u\n", size);
|
||||
|
||||
} else {
|
||||
printf("\n");
|
||||
PX4_INFO_RAW("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -78,6 +78,9 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) :
|
||||
if (!_dataman_init) {
|
||||
_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;
|
||||
bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast<uint8_t *>(&mission_state),
|
||||
sizeof(mission_s));
|
||||
@@ -91,6 +94,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) :
|
||||
PX4_WARN("offboard mission init failed");
|
||||
}
|
||||
|
||||
#endif
|
||||
update_active_mission(_mission_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq,
|
||||
_crc32[MAV_MISSION_TYPE_MISSION], false);
|
||||
}
|
||||
|
||||
@@ -3411,7 +3411,7 @@ MavlinkReceiver::run()
|
||||
|
||||
else if (_mavlink.get_protocol() == Protocol::UDP) {
|
||||
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();
|
||||
@@ -3678,13 +3678,13 @@ void MavlinkReceiver::print_detailed_rx_stats() const
|
||||
{
|
||||
// TODO: add mutex around shared data.
|
||||
if (_component_states_count > 0) {
|
||||
printf("\tReceived Messages:\n");
|
||||
PX4_INFO_RAW("\tReceived Messages:\n");
|
||||
|
||||
for (const auto &comp_stat : _component_states) {
|
||||
if (comp_stat.received_messages > 0) {
|
||||
printf("\t sysid:%3" PRIu8 ", compid:%3" PRIu8 ", Total: %" PRIu32 " (lost: %" PRIu32 ")\n",
|
||||
comp_stat.system_id, comp_stat.component_id,
|
||||
comp_stat.received_messages, comp_stat.missed_messages);
|
||||
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.received_messages, comp_stat.missed_messages);
|
||||
|
||||
#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;
|
||||
|
||||
printf("\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);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -226,7 +226,7 @@ size_t MavlinkShell::available()
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
if (ioctl(_from_shell_fd, FIONREAD, (unsigned long)&ret) == OK) {
|
||||
if (ioctl(_from_shell_fd, FIONREAD, &ret) == OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -195,7 +195,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
|
||||
}
|
||||
|
||||
// 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) {
|
||||
PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX);
|
||||
return;
|
||||
@@ -238,7 +238,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
|
||||
}
|
||||
|
||||
// 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) {
|
||||
PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX);
|
||||
return;
|
||||
@@ -281,7 +281,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
|
||||
}
|
||||
|
||||
// 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) {
|
||||
PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, MAG_COUNT_MAX);
|
||||
return;
|
||||
@@ -1003,10 +1003,10 @@ void SimulatorMavlink::send_mavlink_message(const mavlink_message_t &aMsg)
|
||||
ssize_t len;
|
||||
|
||||
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 {
|
||||
len = ::send(_fd, buf, bufLen, 0);
|
||||
len = ::send(_fd, (const char *)buf, bufLen, 0);
|
||||
}
|
||||
|
||||
if (len <= 0) {
|
||||
@@ -1134,7 +1134,18 @@ void SimulatorMavlink::run()
|
||||
|
||||
if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
|
||||
PX4_ERR("bind for UDP port %i failed (%i)", _port, errno);
|
||||
#ifdef __PX4_WINDOWS
|
||||
// Winsock SOCKETs are kernel handles, not CRT fds — using ::close()
|
||||
// trips the UCRT _close.cpp assertion `(fh >= 0 && (unsigned)fh < (unsigned)_nhandle)`.
|
||||
closesocket(_fd);
|
||||
#else
|
||||
::close(_fd);
|
||||
#endif
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
// Unblock the lockstep wait in simulator_mavlink_main so the
|
||||
// rest of the startup script (including shutdown) can run.
|
||||
_has_initialized.store(true);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1142,7 +1153,7 @@ void SimulatorMavlink::run()
|
||||
|
||||
while (true) {
|
||||
// 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);
|
||||
|
||||
if (len > 0) {
|
||||
@@ -1181,7 +1192,12 @@ void SimulatorMavlink::run()
|
||||
break;
|
||||
|
||||
} else {
|
||||
#ifdef __PX4_WINDOWS
|
||||
// See note above: Winsock SOCKETs need closesocket(), not ::close().
|
||||
closesocket(_fd);
|
||||
#else
|
||||
::close(_fd);
|
||||
#endif
|
||||
system_usleep(500);
|
||||
}
|
||||
}
|
||||
@@ -1237,7 +1253,7 @@ void SimulatorMavlink::run()
|
||||
|
||||
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) {
|
||||
mavlink_message_t msg;
|
||||
|
||||
@@ -87,9 +87,9 @@ using namespace time_literals;
|
||||
|
||||
//! Enumeration to use on the bitmask in HIL_SENSOR
|
||||
enum class SensorSource {
|
||||
ACCEL = 0b111,
|
||||
GYRO = 0b111000,
|
||||
MAG = 0b111000000,
|
||||
ACCEL_XYZ = 0b111,
|
||||
GYRO_XYZ = 0b111000,
|
||||
MAG_XYZ = 0b111000000,
|
||||
BARO = 0b1101000000000,
|
||||
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.
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -133,6 +133,7 @@ bool UxrceddsClient::init()
|
||||
deinit();
|
||||
|
||||
if (_transport == Transport::Serial) {
|
||||
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||
int fd = open(_device, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
|
||||
if (fd < 0) {
|
||||
@@ -165,6 +166,10 @@ bool UxrceddsClient::init()
|
||||
_transport_serial = nullptr;
|
||||
|
||||
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)
|
||||
@@ -185,6 +190,14 @@ bool UxrceddsClient::init()
|
||||
|
||||
} else {
|
||||
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()
|
||||
{
|
||||
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||
|
||||
if (_transport_serial) {
|
||||
uxr_close_serial_transport(_transport_serial);
|
||||
delete _transport_serial;
|
||||
_transport_serial = nullptr;
|
||||
}
|
||||
|
||||
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||
|
||||
#if defined(UXRCE_DDS_CLIENT_UDP)
|
||||
|
||||
if (_transport_udp) {
|
||||
@@ -417,11 +434,15 @@ UxrceddsClient::~UxrceddsClient()
|
||||
|
||||
delete_repliers();
|
||||
|
||||
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||
|
||||
if (_transport_serial) {
|
||||
uxr_close_serial_transport(_transport_serial);
|
||||
delete _transport_serial;
|
||||
}
|
||||
|
||||
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||
|
||||
perf_free(_loop_perf);
|
||||
perf_free(_loop_interval_perf);
|
||||
|
||||
@@ -710,7 +731,7 @@ void UxrceddsClient::run()
|
||||
int fd_bytes_available = 0;
|
||||
|
||||
if ((_fd < 0)
|
||||
|| (ioctl(_fd, FIONREAD, (unsigned long)&fd_bytes_available) != OK)
|
||||
|| (ioctl(_fd, FIONREAD, &fd_bytes_available) != OK)
|
||||
|| (fd_bytes_available <= 0)) {
|
||||
break;
|
||||
}
|
||||
@@ -1000,10 +1021,14 @@ int UxrceddsClient::print_status()
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||
|
||||
if (_transport_serial != nullptr) {
|
||||
PX4_INFO("Using transport: serial");
|
||||
}
|
||||
|
||||
#endif // UXRCE_DDS_CLIENT_SERIAL
|
||||
|
||||
if (_connected) {
|
||||
PX4_INFO("Payload tx: %i B/s", _last_payload_tx_rate);
|
||||
PX4_INFO("Payload rx: %i B/s", _last_payload_rx_rate);
|
||||
|
||||
@@ -50,11 +50,27 @@
|
||||
# define UXRCE_DDS_CLIENT_UDP 1
|
||||
#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"
|
||||
|
||||
#define MAX_NUM_REPLIERS 5
|
||||
#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
|
||||
{
|
||||
@@ -144,7 +160,9 @@ private:
|
||||
|
||||
Transport _transport{};
|
||||
|
||||
uxrSerialTransport *_transport_serial{nullptr};
|
||||
#if defined(UXRCE_DDS_CLIENT_SERIAL)
|
||||
uxrSerialTransport *_transport_serial {nullptr};
|
||||
#endif
|
||||
char _device[32] {};
|
||||
int _baudrate{};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user