Compare commits

..

41 Commits

Author SHA1 Message Date
Pedro-Roque bbd6d9794f fix: add whitelist on dds em 2026-03-12 22:50:46 -07:00
Pedro-Roque 695e2c7caa Merge branch 'feat/safe_dds' of github.com:PX4/PX4-Autopilot into feat/safe_dds 2026-03-12 15:25:42 -07:00
Pedro-Roque 627072b811 feat: added module name 2026-03-12 15:25:31 -07:00
Pedro Roque d651d9e8e2 Merge branch 'main' into feat/safe_dds 2026-03-12 15:08:41 -07:00
Pedro-Roque 574295998b Merge branch 'feat/safe_dds' of github.com:PX4/PX4-Autopilot into feat/safe_dds 2026-03-12 15:07:21 -07:00
Pedro-Roque 71807c93d8 fix: schema failure 2026-03-12 15:07:09 -07:00
PX4BuildBot 59ded6affd docs: auto-sync metadata [skip ci]
Co-Authored-By: PX4 BuildBot <bot@px4.io>
2026-03-12 20:51:32 +00:00
Ege Kural 4a33fb169f fix(ci): enable clang-tidy bugprone-macro-parentheses (#26722)
Signed-off-by: kuralme <kuralme@protonmail.com>
2026-03-12 12:42:07 -08:00
Ramon Roche 11700382f6 docs(contributing): add coding standards and test policy
Add explicit coding standards section referencing astyle and
clang-tidy enforcement. Add formal test policy requiring tests
where practical and types of tests table.

Signed-off-by: Ramon Roche <mrpollo@gmail.com>
2026-03-12 13:15:45 -07:00
Ramon Roche 3f0ddf9793 docs(security): update policy for OpenSSF badge
Update supported versions to 1.16.x, add response process with
7-day acknowledgment timeline, reporter credit policy, and secure
development practices section.

Signed-off-by: Ramon Roche <mrpollo@gmail.com>
2026-03-12 13:15:45 -07:00
Ramon Roche 400bb253bd docs(mavlink): security hardening guide for production deployments (#26730)
* docs(mavlink): add security hardening guide for production deployments

Add a dedicated security hardening page covering MAVLink authentication
risks, a hardening checklist (enable signing, secure physical access,
secure network links), and integrator responsibility for deployment
security. Add a warning block to the main MAVLink page linking to the
new guide.

---------

Signed-off-by: Ramon Roche <mrpollo@gmail.com>
2026-03-12 12:53:29 -07:00
PX4BuildBot d6e31f59cf docs: auto-sync metadata [skip ci]
Co-Authored-By: PX4 BuildBot <bot@px4.io>
2026-03-12 19:52:00 +00:00
Ramon Roche 3ed2f23d9c fix(build): resolve Dependabot security alerts (#26729)
Fix 4 Dependabot alerts:
- CVE-2021-34141: remove duplicate vulnerable numpy==1.21.5 pin
- markdown-it ReDoS (>= 13.0.0, < 14.1.1): add yarn resolution to 14.1.1
- preact JSON VNode injection: resolved by yarn upgrade to 10.29.0
- esbuild dev server request leak (<= 0.24.2): add yarn resolution to 0.25.0

Signed-off-by: Ramon Roche <mrpollo@gmail.com>
2026-03-12 12:40:35 -07:00
Balduin ab6c9b7909 docs(ekf2): clarify EKF2_HGT_REF param description (#26725)
* docs(ekf2): clarify EKF2_HGT_REF param description

To me it was not obvious that with EKF2_GPS_CTRL=0 this altitude
initialisation based on GPS again does not apply.

* docs(ekf2): separate paragraph
2026-03-12 11:30:22 -08:00
Pedro Roque b0d061b0b7 Merge branch 'main' into feat/safe_dds 2026-03-12 11:15:05 -07:00
Pedro-Roque 23613e7e4a rft(dds): reduce the number of conditional checks 2026-03-12 11:13:59 -07:00
PX4BuildBot eeb251aa52 docs: auto-sync metadata [skip ci]
Co-Authored-By: PX4 BuildBot <bot@px4.io>
2026-03-12 17:47:04 +00:00
Matthias Grob 7b3fe3478b ESC check cleanup 2026-03-12 18:30:51 +01:00
ttechnick 7aa28de922 ESC check: use constants for ESC timeout 2026-03-12 18:30:51 +01:00
Matthias Grob a9461c4d1a escCheck: Change MOTFAIL_TIME unit to seconds for better UX 2026-03-12 18:30:51 +01:00
Matthias Grob fb9f8d1835 escCheck: remove thrust threshold above which current model applies
The newer upper lower bound offset current model should apply more accurately and not require a lower bount for thrust where there's no detection.
2026-03-12 18:30:51 +01:00
Matthias Grob 6361b4cd7e Unify motor function mapping checks to only depend on the interface 2026-03-12 18:30:51 +01:00
Matthias Grob 8bb82c70ee escCheck: structure suggestions 2026-03-12 18:30:51 +01:00
Matthias Grob 0071699348 HealthChecks: correct indentation for EVENT metadata 2026-03-12 18:30:51 +01:00
Matthias Grob 54df6d64a6 Commander: move FD_ACT_EN to esc check 2026-03-12 18:30:51 +01:00
Matthias Grob 7207c34c5b Commander: avoid leaking health checks into failure detector 2026-03-12 18:30:51 +01:00
Matthias Grob 270ad06e5f Remove traces of FD_ESCS_EN 2026-03-12 18:30:51 +01:00
Matthias Grob 8bafcfbac7 Rename parameters file for ESC checks 2026-03-12 18:30:51 +01:00
Matthias Grob 2ff83e7e7c escCheck: rename MOTFAIL_TOUT -> MOTFAIL_TIME and further cleanup 2026-03-12 18:30:51 +01:00
Matthias Grob 035ccc8395 FailureDetector: disarm again with ESC failures during spoolup 2026-03-12 18:30:51 +01:00
Matthias Grob 7d84911668 FailureDetector: remove obsolete subscriptions 2026-03-12 18:30:51 +01:00
ttechnick 4e279b16c2 uavcan: optimization and edge cases 2026-03-12 18:30:51 +01:00
ttechnick c5652b2084 escChecks: param reorg
Reorganise parameters
fix esc & motor indices
set failsafe flags
2026-03-12 18:30:51 +01:00
ttechnick 03fc051c29 uavcan:fix check 2026-03-12 18:30:51 +01:00
ttechnick 96c5c7ba02 work on: feed back checks to commander 2026-03-12 18:30:51 +01:00
ttechnick e9874b6f05 ensure motor faults are cleared 2026-03-12 18:30:51 +01:00
ttechnick 15f5a18629 uavcan: cleanup 2026-03-12 18:30:51 +01:00
ttechnick b2ea7ffab6 fd: reorganise motor & esc failures 2026-03-12 18:30:51 +01:00
ttechnick 9f978b05f3 uavcan: unify timeout handling 2026-03-12 18:30:51 +01:00
Pedro-Roque 71ac74827a feat: add whitelisting 2026-03-12 10:00:05 -07:00
Pedro-Roque 41e1ee6023 feat: ensure safe DDS interface by default 2026-03-11 15:58:47 -07:00
106 changed files with 1669 additions and 1980 deletions
-1
View File
@@ -150,7 +150,6 @@ Checks: '*,
-readability-convert-member-functions-to-static,
-readability-make-member-function-const,
-bugprone-implicit-widening-of-multiplication-result,
-bugprone-macro-parentheses,
-bugprone-multi-level-implicit-pointer-conversion,
-bugprone-signed-char-misuse,
-cppcoreguidelines-avoid-non-const-global-variables,
+1 -3
View File
@@ -265,7 +265,5 @@ jobs:
with:
draft: true
prerelease: ${{ steps.upload-location.outputs.is_prerelease == 'true' }}
files: |
artifacts/*.px4
artifacts/*.deb
files: artifacts/*.px4
name: ${{ steps.upload-location.outputs.uploadlocation }}
+21 -1
View File
@@ -16,7 +16,13 @@ git checkout -b mydescriptivebranchname
## Edit and build the code
The [developer guide](https://docs.px4.io/main/en/development/development.html) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](https://docs.px4.io/main/en/contribute/code.html) when editing files.
The [developer guide](https://docs.px4.io/main/en/development/development.html) explains how to set up the development environment on Mac OS, Linux or Windows.
### Coding standards
All C/C++ code must follow the [PX4 coding style](https://docs.px4.io/main/en/contribute/code.html). Formatting is enforced by [astyle](http://astyle.sourceforge.net/) in CI (`make check_format`). Code quality checks run via [clang-tidy](https://clang.llvm.org/extra/clang-tidy/). Pull requests that fail either check will not be merged.
Python code is checked with [mypy](https://mypy-lang.org/) and [flake8](https://flake8.pycqa.org/).
## Commit message convention
@@ -141,6 +147,20 @@ git push --force-with-lease
## Test your changes
PX4 is safety-critical software. All contributions must include adequate testing where practical:
- **New features** must include unit tests and/or integration tests that exercise the new functionality, where practical. Hardware-dependent changes that cannot be tested in SITL should include bench test or flight test evidence.
- **Bug fixes** must include a regression test where practical. When automated testing is not feasible (hardware-specific issues, race conditions, etc.), provide a link to a flight log demonstrating the fix and the reproduction steps for the original bug.
- **Reviewers** will verify that tests or test evidence exist before approving a pull request.
### Types of tests
| Test type | When to use | How to run |
|-----------|-------------|------------|
| **Unit tests** (gtest) | Module-level logic, math, parsing | `make tests` |
| **SITL integration tests** (MAVSDK) | Flight behavior, failsafes, missions | `test/mavsdk_tests/` |
| **Bench tests / flight logs** | Hardware-dependent changes | Upload logs to [Flight Review](https://logs.px4.io) |
Since we care about safety, we will regularly ask you for test results. Best is to do a test flight (or bench test where it applies) and upload the log file from it (on the microSD card in the logs directory) to Google Drive or Dropbox and share the link.
## Push your changes
-13
View File
@@ -226,22 +226,9 @@ CONFIG_TARGETS_DEFAULT := $(patsubst %_default,%,$(filter %_default,$(ALL_CONFIG
$(CONFIG_TARGETS_DEFAULT):
@$(call cmake-build,$@_default$(BUILD_DIR_SUFFIX))
# Multi-processor boards: build all processor targets together
# VOXL2 apps processor (default) depends on SLPI DSP being built first
modalai_voxl2_default: modalai_voxl2_slpi
modalai_voxl2: modalai_voxl2_slpi
modalai_voxl2_deb: modalai_voxl2_slpi
all_config_targets: $(ALL_CONFIG_TARGETS)
all_default_targets: $(CONFIG_TARGETS_DEFAULT)
# DEB package targets: builds _default config, then runs cpack.
# Multi-processor boards (e.g. VOXL2) chain companion builds automatically
# via existing cmake prerequisites.
%_deb:
@$(call cmake-build,$(subst _deb,_default,$@)$(BUILD_DIR_SUFFIX))
@cd "$(SRC_DIR)/build/$(subst _deb,_default,$@)" && cpack -G DEB
updateconfig:
@./Tools/kconfig/updateconfig.py
@@ -44,8 +44,6 @@ param set-default FW_T_SINK_MIN 3
param set-default FW_W_EN 1
param set-default FD_ESCS_EN 0
param set-default MIS_TAKEOFF_ALT 30
param set-default NAV_ACC_RAD 15
@@ -104,4 +104,3 @@ param set-default VT_FWD_THRUST_EN 4
param set-default VT_PITCH_MIN -5
param set-default VT_F_TRANS_THR 1
param set-default VT_TYPE 2
param set-default FD_ESCS_EN 0
@@ -26,7 +26,6 @@ param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 1
param set-default SENS_EN_MAGSIM 1
param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs
param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?
# Set proper failsafes
param set-default COM_ACT_FAIL_ACT 0
@@ -28,7 +28,6 @@ param set-default SIM_GZ_EN 1
param set-default SENS_EN_MAGSIM 1
param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs
param set-default FD_ESCS_EN 0
param set-default CA_AIRFRAME 14
param set-default MAV_TYPE 45
@@ -28,7 +28,6 @@ param set-default SIM_GZ_EN 1
param set-default SENS_EN_MAGSIM 1
param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs
param set-default FD_ESCS_EN 0
param set-default CA_AIRFRAME 14
param set-default MAV_TYPE 45
+28 -12
View File
@@ -2,24 +2,40 @@
## Supported Versions
The following is a list of versions the development team is currently supporting.
The following versions receive security updates:
| Version | Supported |
| ------- | ------------------ |
| 1.4.x | :white_check_mark: |
| 1.3.3 | :white_check_mark: |
| < 1.3 | :x: |
| 1.16.x | :white_check_mark: |
| < 1.16 | :x: |
## Reporting a Vulnerability
We currently only receive security vulnerability reports through GitHub.
We receive security vulnerability reports through GitHub Security Advisories.
To begin a report, please go to the top-level repository, for example, PX4/PX4-Autopilot,
and click on the Security tab. If you are on mobile, click the ... dropdown menu, and then click Security.
To begin a report, go to the [PX4/PX4-Autopilot](https://github.com/PX4/PX4-Autopilot) repository
and click on the **Security** tab. If you are on mobile, click the **...** dropdown menu, then click **Security**.
Click Report a Vulnerability to open the advisory form. Fill in the advisory details form.
Make sure your title is descriptive, and the development team can find all of the relevant details needed
to verify on the description box. We recommend you add as much data as possible. We welcome logs,
screenshots, photos, and videos, anything that can help us verify and identify the issues being reported.
Click **Report a Vulnerability** to open the advisory form. Fill in the advisory details form.
Make sure your title is descriptive and the description contains all relevant details needed
to verify the issue. We welcome logs, screenshots, photos, and videos.
At the bottom of the form, click Submit report. The maintainer team will be notified and will get back to you ASAP.
At the bottom of the form, click **Submit report**.
## Response Process
1. **Acknowledgment**: The maintainer team will acknowledge your report within **7 days**.
2. **Triage**: We will assess severity and impact and communicate next steps.
3. **Disclosure**: We coordinate disclosure with the reporter. We follow responsible disclosure practices and will credit reporters in the advisory unless they request anonymity.
If you do not receive acknowledgment within 7 days, please follow up by emailing the [release managers](MAINTAINERS.md).
## Secure Development Practices
The PX4 development team applies the following practices to reduce security risk:
- **Code review**: All changes require peer review before merging.
- **Static analysis**: [clang-tidy](https://clang.llvm.org/extra/clang-tidy/) runs on every pull request with warnings treated as errors.
- **Fuzzing**: A daily fuzzing pipeline using [Google fuzztest](https://github.com/google/fuzztest) tests MAVLink message handling and GNSS driver protocol parsing.
- **Input validation**: All external inputs (MAVLink messages, RC signals, sensor data) are validated against expected ranges before use.
- **Compiler hardening**: Builds use `-Wall -Werror`, stack protectors, and other hardening flags where supported by the target platform.
-59
View File
@@ -189,65 +189,6 @@ for manufacturer in sorted(os.scandir(os.path.join(source_dir, '../boards')), ke
if target is not None:
build_configs.append(target)
# Remove companion targets from CI groups (parent target builds them via Make prerequisite)
for manufacturer in sorted(os.scandir(os.path.join(source_dir, '../boards')), key=lambda e: e.name):
if not manufacturer.is_dir():
continue
for board in sorted(os.scandir(manufacturer.path), key=lambda e: e.name):
if not board.is_dir():
continue
companion_file = os.path.join(board.path, 'companion_targets')
if os.path.exists(companion_file):
with open(companion_file) as f:
companions = {l.strip() for l in f if l.strip() and not l.startswith('#')}
for arch in grouped_targets:
for man in grouped_targets[arch]['manufacturers']:
grouped_targets[arch]['manufacturers'][man] = [
t for t in grouped_targets[arch]['manufacturers'][man]
if t not in companions
]
# Append _deb targets for boards that have cmake/package.cmake
for manufacturer in sorted(os.scandir(os.path.join(source_dir, '../boards')), key=lambda e: e.name):
if not manufacturer.is_dir():
continue
if manufacturer.name in excluded_manufacturers:
continue
for board in sorted(os.scandir(manufacturer.path), key=lambda e: e.name):
if not board.is_dir():
continue
board_name = manufacturer.name + '_' + board.name
if board_name in excluded_boards:
continue
package_cmake = os.path.join(board.path, 'cmake', 'package.cmake')
if os.path.exists(package_cmake):
deb_target = board_name + '_deb'
if target_filter and not any(deb_target.startswith(f) for f in target_filter):
continue
# Determine the container and group for this board
container = default_container
if board_name in board_container_overrides:
container = board_container_overrides[board_name]
target_entry = {'target': deb_target, 'container': container}
if args.group:
# Find the group where this board's _default target already lives
default_target = board_name + '_default'
group = None
for g in grouped_targets:
targets_in_group = grouped_targets[g].get('manufacturers', {}).get(manufacturer.name, [])
if default_target in targets_in_group:
group = g
break
if group is None:
group = 'base'
target_entry['arch'] = group
if group not in grouped_targets:
grouped_targets[group] = {'container': container, 'manufacturers': {}}
if manufacturer.name not in grouped_targets[group]['manufacturers']:
grouped_targets[group]['manufacturers'][manufacturer.name] = []
grouped_targets[group]['manufacturers'][manufacturer.name].append(deb_target)
build_configs.append(target_entry)
if(verbose):
import pprint
print("============================")
-1
View File
@@ -3,7 +3,6 @@
mkdir artifacts
cp **/**/*.px4 artifacts/ 2>/dev/null || true
cp **/**/*.elf artifacts/ 2>/dev/null || true
cp **/**/*.deb artifacts/ 2>/dev/null || true
for build_dir_path in build/*/ ; do
build_dir_path=${build_dir_path::${#build_dir_path}-1}
build_dir=${build_dir_path#*/}
@@ -1,23 +1,5 @@
CONFIG_PLATFORM_QURT=y
CONFIG_BOARD_TOOLCHAIN="qurt"
# Disable modules from default.px4board that are apps-only
CONFIG_BOARD_LINUX_TARGET=n
CONFIG_DRIVERS_OSD_MSP_OSD=n
CONFIG_DRIVERS_QSHELL_POSIX=n
CONFIG_DRIVERS_RC_INPUT=n
CONFIG_MODULES_DATAMAN=n
CONFIG_MODULES_LOGGER=n
CONFIG_MODULES_MAVLINK=n
CONFIG_MODULES_MUORB_APPS=n
CONFIG_MODULES_NAVIGATOR=n
CONFIG_MODULES_UXRCE_DDS_CLIENT=n
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n
CONFIG_SYSTEMCMDS_BSONDUMP=n
CONFIG_SYSTEMCMDS_PERF=n
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n
CONFIG_SYSTEMCMDS_VER=n
CONFIG_SYSTEMCMDS_REBOOT=n
CONFIG_PARAM_PRIMARY=n
CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -0,0 +1,84 @@
############################################################################
#
# Copyright (c) 2022 ModalAI, Inc. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# Need to make sure that the DSP processor on VOXL2
# knows about all parameters since some modules need parameters
# from other modules that are not running on the DSP.
set(DISABLE_PARAMS_MODULE_SCOPING TRUE PARENT_SCOPE)
add_library(drivers_board
board_config.h
i2c.cpp
init.c
spi.cpp
)
# Generate MAVLink common headers for SLPI drivers (dsp_hitl, mavlink_rc_in)
# Replicates the generation from src/modules/mavlink/CMakeLists.txt so the
# SLPI build is self-contained and does not depend on voxl2-default.
set(MAVLINK_GIT_DIR "${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink")
set(MAVLINK_LIBRARY_DIR "${CMAKE_BINARY_DIR}/mavlink")
px4_add_git_submodule(TARGET git_mavlink_v2 PATH "${MAVLINK_GIT_DIR}")
add_custom_command(
OUTPUT ${MAVLINK_LIBRARY_DIR}/common/common.h
COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
--lang C --wire-protocol 2.0
--output ${MAVLINK_LIBRARY_DIR}
${MAVLINK_GIT_DIR}/message_definitions/v1.0/common.xml
> ${CMAKE_BINARY_DIR}/mavgen_common.log
DEPENDS
git_mavlink_v2
${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
${MAVLINK_GIT_DIR}/message_definitions/v1.0/common.xml
COMMENT "Generating MAVLink common headers for SLPI"
)
add_custom_target(mavlink_common_generate DEPENDS ${MAVLINK_LIBRARY_DIR}/common/common.h)
add_library(mavlink_common_headers INTERFACE)
add_dependencies(mavlink_common_headers mavlink_common_generate)
target_compile_options(mavlink_common_headers INTERFACE -Wno-address-of-packed-member -Wno-cast-align)
target_include_directories(mavlink_common_headers INTERFACE
${MAVLINK_LIBRARY_DIR}
${MAVLINK_LIBRARY_DIR}/common
)
# Add custom drivers for SLPI
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
@@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (c) 2022-2026 ModalAI, Inc. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file board_config.h
*
* VOXL2 internal definitions
*/
#pragma once
#define CONFIG_BOARDCTL_RESET
#define BOARD_HAS_NO_BOOTLOADER
/*
* I2C buses
*/
#define CONFIG_I2C 1
#define PX4_NUMBER_I2C_BUSES 4
/*
* SPI buses
*/
#define CONFIG_SPI 1
#define BOARD_SPI_BUS_MAX_BUS_ITEMS 1
/*
* Include these last to make use of the definitions above
*/
#include <system_config.h>
#include <px4_platform_common/board_common.h>
/*
* Default port for the ESC
*/
#define VOXL_ESC_DEFAULT_PORT "2"
/*
* Default port for the GHST RC
*/
#define GHST_RC_DEFAULT_PORT "7"
/*
* Default port for M0065
*/
#define VOXL2_IO_DEFAULT_PORT "2"
/*
* M0065 PWM
*/
#define DIRECT_PWM_OUTPUT_CHANNELS 4
#define MAX_IO_TIMERS 3
+35
View File
@@ -0,0 +1,35 @@
/****************************************************************************
*
* Copyright (C) 2022 ModalAI, Inc. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "board_config.h"
// Place holder for VOXL2-specific early startup code
+2 -10
View File
@@ -13,10 +13,6 @@ critical applications such as Mavlink, and logging are running on the
ARM CPU cluster (aka apps proc). The DSP and ARM CPU cluster communicate via a
Qualcomm proprietary shared memory interface.
Both processors are built from this single board directory:
- `default.px4board` - POSIX apps processor (ARM64)
- `slpi.px4board` - QURT DSP (Hexagon)
## Build environment
In order to build for this platform both the Qualcomm Hexagon (DSP) toolchain and the Linaro ARM64 toolchain need to be installed. The (nearly) complete setup including the ARM64 toolchain is provided in the base Docker image provided by ModalAI, but since ModalAI is not allowed to redistribute the Qualcomm Hexagon DSP SDK this must be added by the end user.
@@ -26,21 +22,17 @@ The full instructions are available here:
## Build overview
A single `make modalai_voxl2` command builds both the DSP and apps processor
firmware. The Makefile chains the SLPI build as a prerequisite of the default
(apps) build.
- Clone the repo (Don't forget to update and initialize all submodules)
- In the top level directory
```
px4$ boards/modalai/voxl2/scripts/run-docker.sh
root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh
root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-deps.sh
root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh
root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh
root@9373fa1401b8:/usr/local/workspace# exit
```
For DSP-only rebuilds: `make modalai_voxl2_slpi`
## Install and run on VOXL 2
Once the DSP and Linux images have been built they can be installed on a VOXL 2
-4
View File
@@ -31,10 +31,6 @@
#
############################################################################
if(NOT "${PX4_PLATFORM}" STREQUAL "posix")
return()
endif()
# Initialize libfc-sensor-api submodule (fetches from GitLab if not present)
execute_process(
COMMAND Tools/check_submodules.sh boards/modalai/voxl2/libfc-sensor-api
-83
View File
@@ -1,83 +0,0 @@
############################################################################
#
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# VOXL2 board-specific install rules for .deb packaging
# Included from platforms/posix/CMakeLists.txt where the px4 target exists
# SLPI companion build output directory
set(VOXL2_SLPI_BUILD_DIR "${PX4_SOURCE_DIR}/build/modalai_voxl2_slpi")
# Apps processor binary
install(TARGETS px4 RUNTIME DESTINATION bin)
# px4-alias.sh (generated during build into bin/ subdirectory)
install(PROGRAMS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/px4-alias.sh DESTINATION bin)
# Startup scripts from board target directory
install(PROGRAMS
${PX4_BOARD_DIR}/target/voxl-px4
${PX4_BOARD_DIR}/target/voxl-px4-start
${PX4_BOARD_DIR}/target/voxl-px4-hitl
${PX4_BOARD_DIR}/target/voxl-px4-hitl-start
DESTINATION bin
)
# DSP firmware blob from companion SLPI build
install(FILES ${VOXL2_SLPI_BUILD_DIR}/platforms/qurt/libpx4.so
DESTINATION lib/rfsa/adsp
)
# Configuration files
install(FILES
${PX4_BOARD_DIR}/target/voxl-px4-fake-imu-calibration.config
${PX4_BOARD_DIR}/target/voxl-px4-hitl-set-default-parameters.config
DESTINATION ../etc/modalai
)
# Systemd service file
install(FILES ${PX4_BOARD_DIR}/debian/voxl-px4.service
DESTINATION ../etc/systemd/system
)
# Component metadata JSON files
install(FILES
${PX4_BINARY_DIR}/actuators.json.xz
${PX4_BINARY_DIR}/component_general.json.xz
${PX4_BINARY_DIR}/parameters.json.xz
DESTINATION ../data/px4/etc/extras
OPTIONAL
)
install(FILES ${PX4_BINARY_DIR}/events/all_events.json.xz
DESTINATION ../data/px4/etc/extras
OPTIONAL
)
@@ -1,6 +1,3 @@
if(NOT "${PX4_PLATFORM}" STREQUAL "posix")
return()
endif()
# Link against the public stub version of the proprietary fc sensor library
target_link_libraries(px4 PRIVATE
-63
View File
@@ -1,63 +0,0 @@
############################################################################
#
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# VOXL2 board-specific CPack overrides
# Loaded after cmake/package.cmake sets up CPack defaults
# Derive Debian-compatible version from git tag (e.g. v1.17.0-alpha1-42-gabcdef -> 1.17.0~alpha1.42.gabcdef)
string(REGEX REPLACE "^v" "" _deb_ver "${PX4_GIT_TAG}")
string(REGEX REPLACE "-" "~" _deb_ver "${_deb_ver}" )
string(REGEX REPLACE "~([0-9]+)~" ".\\1." _deb_ver "${_deb_ver}")
# VOXL2 is always aarch64 regardless of build host
set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE "arm64")
set(CPACK_DEBIAN_PACKAGE_NAME "voxl-px4")
set(CPACK_DEBIAN_FILE_NAME "voxl-px4_${_deb_ver}_arm64.deb")
set(CPACK_PACKAGING_INSTALL_PREFIX "/usr")
set(CPACK_INSTALL_PREFIX "/usr")
set(CPACK_SET_DESTDIR true)
set(CPACK_DEBIAN_PACKAGE_DEPENDS "libfc-sensor (>=1.0.10), voxl-px4-params (>=0.3.10), voxl3-system-image(>=0.0.2) | voxl2-system-image(>=1.5.4) | rb5-system-image(>=1.6.2), modalai-slpi(>=1.1.16) | modalai-adsp(>=1.0.2)")
set(CPACK_DEBIAN_PACKAGE_CONFLICTS "px4-rb5-flight")
set(CPACK_DEBIAN_PACKAGE_REPLACES "px4-rb5-flight")
set(CPACK_DEBIAN_PACKAGE_DESCRIPTION "PX4 Autopilot for ModalAI VOXL2")
set(CPACK_DEBIAN_PACKAGE_MAINTAINER "ModalAI <support@modalai.com>")
# Disable shlibdeps for cross-compiled boards
set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS OFF)
set(CPACK_DEBIAN_PACKAGE_CONTROL_EXTRA
"${PX4_BOARD_DIR}/debian/postinst;${PX4_BOARD_DIR}/debian/prerm")
# Install rules are in boards/modalai/voxl2/cmake/install.cmake,
# included from platforms/posix/CMakeLists.txt where the px4 target exists.
-3
View File
@@ -1,3 +0,0 @@
# Companion processor targets - built automatically by the parent (default) target
# These are excluded from CI target lists to avoid redundant builds
modalai_voxl2_slpi
-36
View File
@@ -1,36 +0,0 @@
#!/bin/bash
set -e
# Create px4-* symlinks from px4-alias.sh
# The alias format is: alias <module>='px4-<module> --instance $px4_instance'
# We extract the px4-<module> command name and symlink it to the px4 binary
if [ -f /usr/bin/px4-alias.sh ]; then
grep "^alias " /usr/bin/px4-alias.sh | \
sed -n "s/.*'\(px4-[a-zA-Z0-9_]*\).*/\1/p" | while read cmd; do
ln -sf px4 "/usr/bin/${cmd}"
done
fi
# Detect platform and generate DSP test signature if needed
if ! /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then
echo "[INFO] Generating DSP test signature..."
if [ -f /share/modalai/qcs6490-slpi-test-sig/generate-test-sig.sh ]; then
/share/modalai/qcs6490-slpi-test-sig/generate-test-sig.sh || true
elif [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then
/share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh || true
else
echo "[WARNING] Could not find DSP signature generation script"
fi
fi
# Create required data directories
mkdir -p /data/px4/param
mkdir -p /data/px4/etc/extras
chown -R root:root /data/px4
# Reload systemd if available
if command -v systemctl > /dev/null 2>&1; then
systemctl daemon-reload
fi
echo "voxl-px4 installed successfully"
-14
View File
@@ -1,14 +0,0 @@
#!/bin/bash
set -e
# Stop voxl-px4 service if running
if command -v systemctl > /dev/null 2>&1; then
systemctl stop voxl-px4 2>/dev/null || true
fi
# Remove px4-* symlinks
for f in /usr/bin/px4-*; do
if [ -L "$f" ] && [ "$(readlink "$f")" = "px4" ]; then
rm -f "$f"
fi
done
@@ -1,14 +0,0 @@
[Unit]
Description=PX4 Autopilot for VOXL2
After=sscrpcd.service
Requires=sscrpcd.service
[Service]
Type=simple
ExecStart=/usr/bin/voxl-px4
ExecStopPost=/usr/bin/voxl-reset-slpi
Restart=on-failure
RestartSec=5
[Install]
WantedBy=multi-user.target
+2 -2
View File
@@ -1,6 +1,6 @@
#!/bin/bash
echo "*** Starting unified VOXL2 build (apps + SLPI) ***"
echo "*** Starting apps processor build ***"
source /home/build-env.sh
@@ -8,4 +8,4 @@ make modalai_voxl2
cat build/modalai_voxl2_default/src/lib/version/build_git_version.h
echo "*** End of unified VOXL2 build ***"
echo "*** End of apps processor build ***"
-11
View File
@@ -1,11 +0,0 @@
#!/bin/bash
echo "*** Starting unified VOXL2 build (apps + SLPI) ***"
source /home/build-env.sh
make modalai_voxl2_deb
cat build/modalai_voxl2_default/src/lib/version/build_git_version.h
echo "*** End of unified VOXL2 build ***"
+1 -1
View File
@@ -4,6 +4,6 @@ echo "*** Starting qurt slpi build ***"
source /home/build-env.sh
make modalai_voxl2_slpi
make modalai_voxl2-slpi
echo "*** End of qurt slpi build ***"
@@ -1,7 +1,7 @@
#!/bin/bash
# Push slpi image to voxl2
adb push build/modalai_voxl2_slpi/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
# Push apps processor image to voxl2
adb push build/modalai_voxl2_default/bin/px4 /usr/bin
+1 -1
View File
@@ -1,7 +1,7 @@
#!/bin/bash
# Push slpi image to voxl2
adb push build/modalai_voxl2_slpi/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
adb push build/modalai_voxl2-slpi_default/platforms/qurt/libpx4.so /usr/lib/rfsa/adsp
# Push apps processor image to voxl2
adb push build/modalai_voxl2_default/bin/px4 /usr/bin
+18 -71
View File
@@ -31,81 +31,28 @@
#
############################################################################
# Both processors need to know about all parameters since modules are
# split across processors and may reference parameters from the other side.
# Need to make sure that the Linux processor on VOXL2
# knows about all parameters since it is acting as the
# parameter server for other processors that may define
# parameters that it doesn't normally know about.
set(DISABLE_PARAMS_MODULE_SCOPING TRUE PARENT_SCOPE)
set(SRCS
add_library(drivers_board
board_config.h
i2c.cpp
init.c
)
boardctl.c
spi.cpp
)
if("${PX4_PLATFORM}" STREQUAL "qurt")
list(APPEND SRCS
i2c_qurt.cpp
spi_qurt.cpp
)
elseif("${PX4_PLATFORM}" STREQUAL "posix")
list(APPEND SRCS
boardctl.c
i2c_posix.cpp
spi_posix.cpp
)
endif()
# Add custom drivers
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus)
add_library(drivers_board ${SRCS})
# Add custom libraries
add_subdirectory(${PX4_BOARD_DIR}/src/lib/mpa)
if("${PX4_PLATFORM}" STREQUAL "qurt")
# Generate MAVLink common headers for SLPI drivers (dsp_hitl, mavlink_rc_in)
# Replicates the generation from src/modules/mavlink/CMakeLists.txt so the
# SLPI build is self-contained and does not depend on voxl2-default.
set(MAVLINK_GIT_DIR "${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink")
set(MAVLINK_LIBRARY_DIR "${CMAKE_BINARY_DIR}/mavlink")
px4_add_git_submodule(TARGET git_mavlink_v2_slpi PATH "${MAVLINK_GIT_DIR}")
add_custom_command(
OUTPUT ${MAVLINK_LIBRARY_DIR}/common/common.h
COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
--lang C --wire-protocol 2.0
--output ${MAVLINK_LIBRARY_DIR}
${MAVLINK_GIT_DIR}/message_definitions/v1.0/common.xml
> ${CMAKE_BINARY_DIR}/mavgen_common.log
DEPENDS
git_mavlink_v2_slpi
${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
${MAVLINK_GIT_DIR}/message_definitions/v1.0/common.xml
COMMENT "Generating MAVLink common headers for SLPI"
)
add_custom_target(mavlink_common_generate DEPENDS ${MAVLINK_LIBRARY_DIR}/common/common.h)
add_library(mavlink_common_headers INTERFACE)
add_dependencies(mavlink_common_headers mavlink_common_generate)
target_compile_options(mavlink_common_headers INTERFACE -Wno-address-of-packed-member -Wno-cast-align)
target_include_directories(mavlink_common_headers INTERFACE
${MAVLINK_LIBRARY_DIR}
${MAVLINK_LIBRARY_DIR}/common
)
# Add custom drivers for SLPI
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/rc_controller rc_controller)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/mavlink_rc_in mavlink_rc_in)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/spektrum_rc spektrum_rc)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/ghst_rc ghst_rc)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/dsp_hitl dsp_hitl)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/dsp_sbus dsp_sbus)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/qurt/elrs_led elrs_led)
elseif("${PX4_PLATFORM}" STREQUAL "posix")
# Add custom drivers
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/posix/apps_sbus apps_sbus)
# Add custom libraries
add_subdirectory(${PX4_BOARD_DIR}/src/lib/mpa mpa)
# Add custom modules
add_subdirectory(${PX4_BOARD_DIR}/src/modules/voxl_save_cal_params voxl_save_cal_params)
add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_air_data_bridge vehicle_air_data_bridge)
add_subdirectory(${PX4_BOARD_DIR}/src/modules/sensor_baro_bridge sensor_baro_bridge)
add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_local_position_bridge vehicle_local_position_bridge)
endif()
# Add custom modules
add_subdirectory(${PX4_BOARD_DIR}/src/modules/voxl_save_cal_params)
add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_air_data_bridge)
add_subdirectory(${PX4_BOARD_DIR}/src/modules/sensor_baro_bridge)
add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_local_position_bridge)
+10 -35
View File
@@ -42,44 +42,21 @@
#define CONFIG_BOARDCTL_RESET
#define BOARD_HAS_NO_BOOTLOADER
// Define this as empty since i2c clock init isn't required
#define BOARD_I2C_BUS_CLOCK_INIT
/*
* SPI buses (shared)
* I2C buses
*/
#define CONFIG_I2C 1
#define PX4_NUMBER_I2C_BUSES 1
/*
* SPI buses
*/
#define CONFIG_SPI 1
#define BOARD_SPI_BUS_MAX_BUS_ITEMS 1
#ifdef __PX4_QURT
/*
* QURT (DSP) specific defines
*/
#define CONFIG_I2C 1
#define PX4_NUMBER_I2C_BUSES 4
#include <system_config.h>
#include <px4_platform_common/board_common.h>
#define VOXL_ESC_DEFAULT_PORT "2"
#define GHST_RC_DEFAULT_PORT "7"
#define VOXL2_IO_DEFAULT_PORT "2"
/* M0065 PWM */
#define DIRECT_PWM_OUTPUT_CHANNELS 4
#define MAX_IO_TIMERS 3
#endif /* __PX4_QURT */
#if defined(__PX4_POSIX) && !defined(__PX4_QURT)
/*
* POSIX (apps processor) specific defines
*/
/* I2C clock init not required on Linux */
#define BOARD_I2C_BUS_CLOCK_INIT
#define CONFIG_I2C 1
#define PX4_NUMBER_I2C_BUSES 1
#include <system_config.h>
#include <px4_platform_common/board_common.h>
@@ -88,5 +65,3 @@
#define VOXL_ESC_DEFAULT_PORT "2"
#define VOXL2_IO_DEFAULT_PORT "2"
#endif /* __PX4_POSIX && !__PX4_QURT */
+1 -6
View File
@@ -86,7 +86,7 @@ if("${CMAKE_SYSTEM}" MATCHES "Linux")
set(CPACK_DEBIAN_PACKAGE_DESCRIPTION "PX4 autopilot")
set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional")
set(CPACK_DEBIAN_PACKAGE_SECTION "misc")
set(CPACK_DEBIAN_PACKAGE_ARCHITECTURE ${CMAKE_SYSTEM_PROCESSOR})
set(CPACK_DEBIAN_ARCHITECTURE ${CMAKE_SYSTEM_PROCESSOR})
# autogenerate dependency information
set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON)
@@ -97,9 +97,4 @@ else()
set(CPACK_GENERATOR "ZIP")
endif()
# Board-specific overrides (loaded after defaults are set)
if(EXISTS "${PX4_BOARD_DIR}/cmake/package.cmake")
include(${PX4_BOARD_DIR}/cmake/package.cmake)
endif()
include(CPack)
@@ -2,7 +2,7 @@
#
# Stack: PX4 Pro
# Vehicle: Amovlab F410
# Version: 1.15.4
# Version: 1.15.4
# Git Revision: 99c40407ff000000
#
# Vehicle-Id Component-Id Name Value Type
@@ -479,11 +479,6 @@
1 1 EKF2_WIND_NSD 0.050000000745058060 9
1 1 EV_TSK_RC_LOSS 0 6
1 1 EV_TSK_STAT_DIS 0 6
1 1 FD_ACT_EN 1 6
1 1 FD_ACT_MOT_C2T 2.000000000000000000 9
1 1 FD_ACT_MOT_THR 0.200000002980232239 9
1 1 FD_ACT_MOT_TOUT 100 6
1 1 FD_ESCS_EN 1 6
1 1 FD_EXT_ATS_EN 0 6
1 1 FD_EXT_ATS_TRIG 1900 6
1 1 FD_FAIL_P 60 6
+1 -1
View File
@@ -476,7 +476,6 @@
- [Flight Controller Porting Guide](hardware/porting_guide.md)
- [PX4 Board Configuration (kconfig)](hardware/porting_guide_config.md)
- [NuttX Board Porting Guide](hardware/porting_guide_nuttx.md)
- [Board Firmware Packaging (.deb)](hardware/board_packaging.md)
- [Serial Port Mapping](hardware/serial_port_mapping.md)
- [Airframes](dev_airframes/index.md)
- [Adding a New Airframe](dev_airframes/adding_a_new_frame.md)
@@ -760,6 +759,7 @@
- [Receiving Messages](mavlink/receiving_messages.md)
- [Custom MAVLink Messages](mavlink/custom_messages.md)
- [Message Signing](mavlink/message_signing.md)
- [Security Hardening](mavlink/security_hardening.md)
- [Protocols/Microservices](mavlink/protocols.md)
- [Standard Modes Protocol](mavlink/standard_modes.md)
- [uXRCE-DDS (PX4-ROS 2/DDS Bridge)](middleware/uxrce_dds.md)
+59 -77
View File
@@ -20502,7 +20502,8 @@ Measurement noise for magnetic heading fusion.
Determines the reference source of height data used by the EKF.
When multiple height sources are enabled at the same time, the height estimate will always converge towards the reference height source selected by this parameter. The range sensor and vision options should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level. If GPS is set as reference but altitude fusion is disabled in EKF2_GPS_CTRL, the GPS altitude is still used to initiaize the bias of the other height sensors.
When multiple height sources are enabled at the same time, the height estimate will always converge towards the reference height source selected by this parameter. The range sensor and vision options should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level.
If GPS is set as reference and EKF2_GPS_CTRL is not 0, the GPS altitude is still used to initiaize the bias of the other height sensors, regardless of the altitude fusion bit in EKF2_GPS_CTRL.
**Values:**
@@ -22729,82 +22730,6 @@ Yaw rate proportional gain.
## Failure Detector
### FD_ACT_EN (`INT32`) {#FD_ACT_EN}
Enable Actuator Failure check.
If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
level is being consumed.
Otherwise this indicates an motor failure.
| Reboot | minValue | maxValue | increment | default | unit |
| ------- | -------- | -------- | --------- | ------------ | ---- |
| &check; | | | | Disabled (0) | |
### FD_ACT_HIGH_OFF (`FLOAT`) {#FD_ACT_HIGH_OFF}
Overcurrent motor failure limit offset.
threshold = FD_ACT_MOT_C2T \* thrust + FD_ACT_HIGH_OFF
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0 | 30 | 1 | 10. | A |
### FD_ACT_LOW_OFF (`FLOAT`) {#FD_ACT_LOW_OFF}
Undercurrent motor failure limit offset.
threshold = FD_ACT_MOT_C2T \* thrust - FD_ACT_LOW_OFF
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0 | 30 | 1 | 10. | A |
### FD_ACT_MOT_C2T (`FLOAT`) {#FD_ACT_MOT_C2T}
Motor Failure Current/Throttle Scale.
Determines the slope between expected steady state current and linearized, normalized thrust command.
E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0.0 | 50.0 | 1 | 35. | A/% |
### FD_ACT_MOT_THR (`FLOAT`) {#FD_ACT_MOT_THR}
Motor Failure Thrust Threshold.
Failure detection per motor only triggers above this thrust value.
Set to 1 to disable the detection.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0.0 | 1.0 | 0.01 | 0.2 | norm |
### FD_ACT_MOT_TOUT (`INT32`) {#FD_ACT_MOT_TOUT}
Motor Failure Hysteresis Time.
Motor failure only triggers after current thresholds are exceeded for this time.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 10 | 10000 | 100 | 1000 | ms |
### FD_ESCS_EN (`INT32`) {#FD_ESCS_EN}
Enable checks on ESCs that report their arming state.
If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ----------- | ---- |
| &nbsp; | | | | Enabled (1) | |
### FD_EXT_ATS_EN (`INT32`) {#FD_EXT_ATS_EN}
Enable PWM input on for engaging failsafe from an external automatic trigger system (ATS).
@@ -28378,6 +28303,63 @@ needs to be changed to match a custom setting
| ------- | -------- | -------- | --------- | ------- | ---- |
| &check; | | | | 0 | |
## Motor Failure
### FD_ACT_EN (`INT32`) {#FD_ACT_EN}
Enable Actuator Failure check.
If enabled, the HealthAndArmingChecks will verify that for motors, a minimum amount of ESC current per throttle
level is being consumed.
Otherwise this indicates an motor failure.
This check only works for ESCs that report current consumption.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------------ | ---- |
| &nbsp; | | | | Disabled (0) | |
### MOTFAIL_C2T (`FLOAT`) {#MOTFAIL_C2T}
Motor Failure Current/Throttle Scale.
Determines the slope between expected steady state current and linearized, normalized thrust command.
E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0.0 | 50.0 | 1 | 35. | A/% |
### MOTFAIL_HIGH_OFF (`FLOAT`) {#MOTFAIL_HIGH_OFF}
Overcurrent motor failure limit offset.
threshold = FD_ACT_MOT_C2T \* thrust + FD_ACT_HIGH_OFF
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0 | 30 | 1 | 10. | A |
### MOTFAIL_LOW_OFF (`FLOAT`) {#MOTFAIL_LOW_OFF}
Undercurrent motor failure limit offset.
threshold = FD_ACT_MOT_C2T \* thrust - FD_ACT_LOW_OFF
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0 | 30 | 1 | 10. | A |
### MOTFAIL_TIME (`FLOAT`) {#MOTFAIL_TIME}
Motor Failure Hysteresis Time.
Motor failure only triggers after current thresholds are exceeded for this time.
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| &nbsp; | 0.01 | 10 | 1 | 1. | s |
## Mount
### MNT_DO_STAB (`INT32`) {#MNT_DO_STAB}
-298
View File
@@ -1,298 +0,0 @@
# Board Firmware Packaging
PX4 supports building distributable firmware packages for Linux-based (POSIX) boards.
While NuttX boards produce `.px4` firmware files that are flashed via QGroundControl, POSIX boards can produce `.deb` (Debian) packages that are installed using standard Linux package management tools (`dpkg`, `apt`).
This page covers how manufacturers can add `.deb` packaging to their boards, with examples for both single-processor and multi-processor architectures.
## Overview
The packaging framework uses [CMake CPack](https://cmake.org/cmake/help/latest/module/CPack.html) with the DEB generator.
It is built on two extension points in the PX4 build system:
- **`boards/<vendor>/<board>/cmake/package.cmake`**: CPack variable overrides (package name, version, dependencies, architecture, maintainer info). Loaded during CMake configure.
- **`boards/<vendor>/<board>/cmake/install.cmake`**: `install()` rules that define what goes into the package (binaries, scripts, config files, service files). Loaded from `platforms/posix/CMakeLists.txt` where build targets are available.
When a board provides these files, CI automatically discovers and builds the `_deb` target alongside the normal firmware build.
## Build Command
For any board with packaging support:
```sh
make <vendor>_<board>_deb
```
For example:
```sh
make modalai_voxl2_deb
```
This builds the `_default` configuration (and any companion builds for multi-processor boards), then runs `cpack -G DEB` in the build directory.
The resulting `.deb` file is placed in `build/<vendor>_<board>_default/`.
## Adding Packaging to a Board
### File Structure
```
boards/<vendor>/<board>/
cmake/
package.cmake # CPack configuration (required)
install.cmake # Install rules (required)
debian/
postinst # Post-install script (optional)
prerm # Pre-remove script (optional)
<name>.service # Systemd unit file (optional)
```
### Step 1: CPack Configuration (package.cmake)
This file sets CPack variables that control the `.deb` metadata.
It is included from `cmake/package.cmake` after the base CPack defaults are configured.
```cmake
# boards/<vendor>/<board>/cmake/package.cmake
# Derive Debian-compatible version from git tag
# v1.17.0-alpha1-42-gabcdef -> 1.17.0~alpha1.42.gabcdef
# v1.17.0 -> 1.17.0
string(REGEX REPLACE "^v" "" _deb_ver "${PX4_GIT_TAG}")
string(REGEX REPLACE "-" "~" _deb_ver "${_deb_ver}")
string(REGEX REPLACE "~([0-9]+)~" ".\\1." _deb_ver "${_deb_ver}")
# Target architecture (use the target arch, not the build host)
set(CPACK_DEBIAN_ARCHITECTURE "arm64")
# Package identity
set(CPACK_DEBIAN_PACKAGE_NAME "my-px4-board")
set(CPACK_DEBIAN_FILE_NAME "my-px4-board_${_deb_ver}_arm64.deb")
# Install prefix
set(CPACK_PACKAGING_INSTALL_PREFIX "/usr")
set(CPACK_INSTALL_PREFIX "/usr")
set(CPACK_SET_DESTDIR true)
# Package metadata
set(CPACK_DEBIAN_PACKAGE_DESCRIPTION "PX4 Autopilot for My Board")
set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Vendor <support@vendor.com>")
set(CPACK_DEBIAN_PACKAGE_DEPENDS "some-dependency (>= 1.0)")
set(CPACK_DEBIAN_PACKAGE_CONFLICTS "")
set(CPACK_DEBIAN_PACKAGE_REPLACES "")
# Disable shlibdeps for cross-compiled boards
set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS OFF)
# Include post-install and pre-remove scripts (optional)
set(CPACK_DEBIAN_PACKAGE_CONTROL_EXTRA
"${PX4_BOARD_DIR}/debian/postinst;${PX4_BOARD_DIR}/debian/prerm")
```
**Key variables:**
| Variable | Purpose |
|---|---|
| `CPACK_DEBIAN_ARCHITECTURE` | Target architecture. Set explicitly for cross-compiled boards since `dpkg --print-architecture` reports the build host, not the target. |
| `CPACK_DEBIAN_PACKAGE_NAME` | Package name as it appears in `dpkg -l`. |
| `CPACK_DEBIAN_FILE_NAME` | Output `.deb` filename. |
| `CPACK_DEBIAN_PACKAGE_DEPENDS` | Runtime dependencies (comma-separated, Debian format). |
| `CPACK_DEBIAN_PACKAGE_SHLIBDEPS` | Set to `OFF` for cross-compiled boards where `dpkg-shlibdeps` cannot inspect target binaries. |
### Step 2: Install Rules (install.cmake)
This file defines what files are packaged in the `.deb`.
It is included from `platforms/posix/CMakeLists.txt` where the `px4` build target is available.
All paths are relative to `CPACK_PACKAGING_INSTALL_PREFIX` (typically `/usr`). Use `../` to install outside the prefix (e.g., `../etc/` installs to `/etc/`).
**Minimal example (single-processor board):**
```cmake
# boards/<vendor>/<board>/cmake/install.cmake
# PX4 binary
install(TARGETS px4 RUNTIME DESTINATION bin)
# Module alias script (generated during build)
install(PROGRAMS ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/px4-alias.sh DESTINATION bin)
# Startup scripts
install(PROGRAMS
${PX4_BOARD_DIR}/target/my-px4-start
DESTINATION bin
)
# Configuration files
install(FILES
${PX4_BOARD_DIR}/target/my-config.conf
DESTINATION ../etc/my-board
)
# Systemd service
install(FILES ${PX4_BOARD_DIR}/debian/my-px4.service
DESTINATION ../etc/systemd/system
)
# Component metadata
install(FILES
${PX4_BINARY_DIR}/actuators.json.xz
${PX4_BINARY_DIR}/parameters.json.xz
DESTINATION ../data/px4/etc/extras
OPTIONAL
)
install(FILES ${PX4_BINARY_DIR}/events/all_events.json.xz
DESTINATION ../data/px4/etc/extras
OPTIONAL
)
```
### Step 3: Debian Scripts (optional)
#### postinst
Runs after the package is installed. Common tasks:
- Create `px4-*` module symlinks from `px4-alias.sh`
- Set up required directories with correct ownership
- Run `systemctl daemon-reload` to pick up the service file
- Board-specific setup (e.g., DSP signature generation)
```bash
#!/bin/bash
set -e
# Create px4-* symlinks
if [ -f /usr/bin/px4-alias.sh ]; then
grep "^alias " /usr/bin/px4-alias.sh | \
sed "s/alias \(px4-[a-zA-Z0-9_]*\)=.*/\1/" | while read cmd; do
ln -sf px4 "/usr/bin/${cmd}"
done
fi
# Create data directories
mkdir -p /data/px4/param
mkdir -p /data/px4/etc/extras
# Reload systemd
if command -v systemctl > /dev/null 2>&1; then
systemctl daemon-reload
fi
```
#### prerm
Runs before the package is removed:
```bash
#!/bin/bash
set -e
# Stop the service
if command -v systemctl > /dev/null 2>&1; then
systemctl stop my-px4 2>/dev/null || true
fi
# Remove px4-* symlinks
for f in /usr/bin/px4-*; do
if [ -L "$f" ] && [ "$(readlink "$f")" = "px4" ]; then
rm -f "$f"
fi
done
```
Both scripts must be executable (`chmod +x`).
## Multi-Processor Boards
Some boards run PX4 across multiple processors, for example the ModalAI VOXL2 which has a POSIX apps processor (ARM) and a Hexagon DSP (SLPI).
These produce two separate CMake builds, but the `.deb` must contain artifacts from both.
### How It Works
1. The `_default` build (POSIX/apps processor) owns the `.deb`.
2. The Makefile `%_deb` target builds `_default`, which chains any companion builds as CMake prerequisites.
3. The `install.cmake` pulls companion build artifacts via absolute path to the sibling build directory.
4. CPack runs in the `_default` build tree and produces a single `.deb`.
### Companion Build Artifacts
In `install.cmake`, reference the companion build output by absolute path:
```cmake
# DSP firmware blob from companion SLPI build
set(SLPI_BUILD_DIR "${PX4_SOURCE_DIR}/build/<vendor>_<board>-slpi_default")
install(FILES ${SLPI_BUILD_DIR}/platforms/qurt/libpx4.so
DESTINATION lib/rfsa/adsp
OPTIONAL
)
```
The `OPTIONAL` keyword allows the `.deb` to build even when the companion build hasn't run (useful for development/testing of just the apps-processor side).
### VOXL2 Reference
The VOXL2 board is a complete working example of multi-processor packaging:
```
boards/modalai/voxl2/
cmake/
package.cmake # CPack config: voxl-px4, arm64, deps, shlibdeps off
install.cmake # px4 binary, SLPI libpx4.so, scripts, configs, metadata
debian/
postinst # Symlinks, DSP signature, directory setup
prerm # Stop service, remove symlinks
voxl-px4.service # Systemd unit (after sscrpcd, restart on-failure)
target/
voxl-px4 # Main startup wrapper
voxl-px4-start # PX4 module startup script
```
The resulting `.deb` installs:
| Path | Contents |
|---|---|
| `/usr/bin/px4` | Apps processor PX4 binary |
| `/usr/bin/px4-alias.sh` | Module alias script |
| `/usr/bin/voxl-px4` | Startup wrapper |
| `/usr/bin/voxl-px4-start` | Module startup script |
| `/usr/lib/rfsa/adsp/libpx4.so` | DSP firmware (from SLPI build) |
| `/etc/modalai/*.config` | Board configuration files |
| `/etc/systemd/system/voxl-px4.service` | Systemd service |
| `/data/px4/etc/extras/*.json.xz` | Component metadata |
## CI Integration
### Automatic Discovery
The CI system (`Tools/ci/generate_board_targets_json.py`) automatically discovers boards with `cmake/package.cmake` and adds a `<vendor>_<board>_deb` target to the board's existing CI group.
No manual CI configuration is needed.
### Artifact Collection
The `Tools/ci/package_build_artifacts.sh` script collects `.deb` files alongside `.px4` and `.elf` artifacts.
On tagged releases, `.deb` files are uploaded to both S3 and GitHub Releases.
## Version Format
The `.deb` version is derived from `PX4_GIT_TAG` using Debian-compatible formatting:
| Git Tag | Debian Version | Notes |
|---|---|---|
| `v1.17.0` | `1.17.0` | Stable release |
| `v1.17.0-beta1` | `1.17.0~beta1` | Pre-release (`~` sorts before release) |
| `v1.17.0-alpha1-42-gabcdef` | `1.17.0~alpha1.42.gabcdef` | Development build |
The `~` prefix in Debian versioning ensures pre-releases sort lower than the final release: `1.17.0~beta1 < 1.17.0`.
## Checklist for New Boards
1. Create `boards/<vendor>/<board>/cmake/package.cmake` with CPack variables
2. Create `boards/<vendor>/<board>/cmake/install.cmake` with install rules
3. (Optional) Create `boards/<vendor>/<board>/debian/postinst` and `prerm`
4. (Optional) Create `boards/<vendor>/<board>/debian/<name>.service`
5. Test locally: `make <vendor>_<board>_deb`
6. Verify: `dpkg-deb --info build/<vendor>_<board>_default/<name>_*.deb`
7. Verify: `dpkg-deb --contents build/<vendor>_<board>_default/<name>_*.deb`
8. CI picks it up automatically on the next push
+4
View File
@@ -14,6 +14,10 @@ It also links instructions for how you can add PX4 support for:
- [Message Signing](../mavlink/message_signing.md)
- [Protocols/Microservices](../mavlink/protocols.md)
::: warning
MAVLink messages are unauthenticated by default. Without [message signing](../mavlink/message_signing.md) enabled, any device that can send MAVLink messages to the vehicle can execute commands including shell access, file operations, and flight termination. Production deployments must enable signing and follow the [Security Hardening](../mavlink/security_hardening.md) guide.
:::
::: info
We do not yet cover _command_ handling and sending, or how to implement your own microservices.
:::
+84
View File
@@ -0,0 +1,84 @@
# MAVLink Security Hardening for Production Deployments
<Badge type="tip" text="PX4 v1.17" />
MAVLink is an open communication protocol designed for lightweight, low-latency communication between drones and ground stations.
By default, all MAVLink messages are unauthenticated.
This is intentional for development and testing, but **production deployments must enable [message signing](message_signing.md)** to prevent unauthorized access.
::: warning
Without message signing enabled, any device that can send MAVLink messages to the vehicle (via radio, network, or serial) can execute any command, including shell access, file operations, parameter changes, mission uploads, arming, and flight termination.
:::
## What Is at Risk
When MAVLink signing is not enabled, an attacker within communication range can:
| Capability | MAVLink mechanism |
| ---------------------------- | ------------------------------------------------ |
| Execute shell commands | `SERIAL_CONTROL` with `SERIAL_CONTROL_DEV_SHELL` |
| Read, write, or delete files | MAVLink FTP protocol |
| Change any flight parameter | `PARAM_SET` / `PARAM_EXT_SET` |
| Upload or overwrite missions | Mission protocol |
| Arm or disarm motors | `MAV_CMD_COMPONENT_ARM_DISARM` |
| Terminate flight (crash) | `MAV_CMD_DO_FLIGHTTERMINATION` |
| Trigger emergency landing | Spoofed `BATTERY_STATUS` |
| Reboot the vehicle | `MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN` |
All of these are standard MAVLink capabilities used by ground control stations.
Without signing, there is no distinction between a legitimate GCS and an unauthorized sender.
## Hardening Checklist
### 1. Enable Message Signing
Message signing provides cryptographic authentication for all MAVLink communication.
See [Message Signing](message_signing.md) for full details.
Steps:
1. Connect the vehicle via **USB** (key provisioning only works over USB).
2. Provision a 32-byte secret key using the [SETUP_SIGNING](https://mavlink.io/en/messages/common.html#SETUP_SIGNING) message.
3. Set [MAV_SIGN_CFG](../advanced_config/parameter_reference.md#MAV_SIGN_CFG) to **1** (signing enabled on all links except USB) or **2** (signing on all links including USB).
4. Provision the same key on all ground control stations and companion computers that need to communicate with the vehicle.
5. Verify that unsigned messages from unknown sources are rejected.
::: info
`MAV_SIGN_CFG=1` is recommended for most deployments.
This enforces signing on telemetry radios and network links while allowing unsigned access over USB for maintenance.
USB connections require physical access to the vehicle, which provides equivalent security to physical key access.
:::
### 2. Secure Physical Access
- Protect access to the SD card. The signing key is stored at `/mavlink/mavlink-signing-key.bin` and can be read or removed by anyone with physical access.
- USB connections bypass signing when `MAV_SIGN_CFG=1`. Ensure USB ports are not exposed in deployed configurations.
### 3. Secure Network Links
- Do not expose MAVLink UDP/TCP ports to untrusted networks or the internet.
- Place MAVLink communication links behind firewalls or VPNs.
- Segment MAVLink networks from business or public networks.
- When using companion computers, audit which network interfaces MAVLink is bound to.
### 4. Understand the Limitations
- **No encryption**: Message signing provides authentication and integrity, but messages are sent in plaintext. An eavesdropper can read telemetry and commands but cannot forge them.
- **Allowlisted messages**: A small set of [safety-critical messages](message_signing.md#unsigned-message-allowlist) (RADIO_STATUS, ADSB_VEHICLE, COLLISION) are always accepted unsigned.
- **Key management**: There is no automatic key rotation. Keys must be reprovisioned manually via USB if compromised.
## Integrator Responsibility
PX4 is open-source flight controller firmware used by manufacturers and system integrators to build commercial and custom drone platforms.
Securing the communication links for a specific deployment is the responsibility of the system integrator.
This includes:
- Choosing appropriate radio hardware and link security
- Enabling and managing MAVLink message signing
- Restricting network access to MAVLink interfaces
- Applying firmware updates that address security issues
- Evaluating whether the default configuration meets the security requirements of the target application
PX4 provides the tools for securing MAVLink communication.
Integrators must enable and configure them for their deployment context.
+189 -189
View File
@@ -95,202 +95,202 @@ They are not build into the module, and hence are neither published or subscribe
::: details See messages
- [GpioRequest](../msg_docs/GpioRequest.md)
- [VehicleImuStatus](../msg_docs/VehicleImuStatus.md)
- [EstimatorBias](../msg_docs/EstimatorBias.md)
- [SatelliteInfo](../msg_docs/SatelliteInfo.md)
- [GimbalDeviceSetAttitude](../msg_docs/GimbalDeviceSetAttitude.md)
- [DebugArray](../msg_docs/DebugArray.md)
- [VehicleGlobalPositionV0](../msg_docs/VehicleGlobalPositionV0.md)
- [OrbTestLarge](../msg_docs/OrbTestLarge.md)
- [IridiumsbdStatus](../msg_docs/IridiumsbdStatus.md)
- [TrajectorySetpoint6dof](../msg_docs/TrajectorySetpoint6dof.md)
- [ActionRequest](../msg_docs/ActionRequest.md)
- [DatamanRequest](../msg_docs/DatamanRequest.md)
- [EventV0](../msg_docs/EventV0.md)
- [BatteryStatusV0](../msg_docs/BatteryStatusV0.md)
- [DronecanNodeStatus](../msg_docs/DronecanNodeStatus.md)
- [MissionResult](../msg_docs/MissionResult.md)
- [SensorsStatusImu](../msg_docs/SensorsStatusImu.md)
- [AutotuneAttitudeControlStatus](../msg_docs/AutotuneAttitudeControlStatus.md)
- [SensorBaro](../msg_docs/SensorBaro.md)
- [EstimatorGpsStatus](../msg_docs/EstimatorGpsStatus.md)
- [SensorGyroFft](../msg_docs/SensorGyroFft.md)
- [VehicleCommandAckV0](../msg_docs/VehicleCommandAckV0.md)
- [DebugValue](../msg_docs/DebugValue.md)
- [VehicleAngularAccelerationSetpoint](../msg_docs/VehicleAngularAccelerationSetpoint.md)
- [GimbalDeviceInformation](../msg_docs/GimbalDeviceInformation.md)
- [ConfigOverridesV0](../msg_docs/ConfigOverridesV0.md)
- [RcParameterMap](../msg_docs/RcParameterMap.md)
- [SensorPreflightMag](../msg_docs/SensorPreflightMag.md)
- [ParameterResetRequest](../msg_docs/ParameterResetRequest.md)
- [CameraCapture](../msg_docs/CameraCapture.md)
- [EstimatorInnovations](../msg_docs/EstimatorInnovations.md)
- [EstimatorStates](../msg_docs/EstimatorStates.md)
- [PowerButtonState](../msg_docs/PowerButtonState.md)
- [GeofenceResult](../msg_docs/GeofenceResult.md)
- [LandingTargetInnovations](../msg_docs/LandingTargetInnovations.md)
- [LaunchDetectionStatus](../msg_docs/LaunchDetectionStatus.md)
- [OrbitStatus](../msg_docs/OrbitStatus.md)
- [CanInterfaceStatus](../msg_docs/CanInterfaceStatus.md)
- [TiltrotorExtraControls](../msg_docs/TiltrotorExtraControls.md)
- [ParameterSetUsedRequest](../msg_docs/ParameterSetUsedRequest.md)
- [VehicleLocalPositionV0](../msg_docs/VehicleLocalPositionV0.md)
- [ArmingCheckReplyV0](../msg_docs/ArmingCheckReplyV0.md)
- [EstimatorAidSource1d](../msg_docs/EstimatorAidSource1d.md)
- [RegisterExtComponentReplyV0](../msg_docs/RegisterExtComponentReplyV0.md)
- [Airspeed](../msg_docs/Airspeed.md)
- [CameraTrigger](../msg_docs/CameraTrigger.md)
- [RtlStatus](../msg_docs/RtlStatus.md)
- [VehicleRoi](../msg_docs/VehicleRoi.md)
- [TaskStackInfo](../msg_docs/TaskStackInfo.md)
- [LandingTargetPose](../msg_docs/LandingTargetPose.md)
- [Gripper](../msg_docs/Gripper.md)
- [ParameterSetValueResponse](../msg_docs/ParameterSetValueResponse.md)
- [ParameterUpdate](../msg_docs/ParameterUpdate.md)
- [VehicleAirData](../msg_docs/VehicleAirData.md)
- [OpenDroneIdArmStatus](../msg_docs/OpenDroneIdArmStatus.md)
- [ActuatorTest](../msg_docs/ActuatorTest.md)
- [Ekf2Timestamps](../msg_docs/Ekf2Timestamps.md)
- [CellularStatus](../msg_docs/CellularStatus.md)
- [LogMessage](../msg_docs/LogMessage.md)
- [SensorsStatus](../msg_docs/SensorsStatus.md)
- [SensorHygrometer](../msg_docs/SensorHygrometer.md)
- [SensorAccelFifo](../msg_docs/SensorAccelFifo.md)
- [Rpm](../msg_docs/Rpm.md)
- [GimbalManagerSetManualControl](../msg_docs/GimbalManagerSetManualControl.md)
- [EstimatorStatus](../msg_docs/EstimatorStatus.md)
- [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md)
- [MavlinkTunnel](../msg_docs/MavlinkTunnel.md)
- [FollowTarget](../msg_docs/FollowTarget.md)
- [PositionControllerLandingStatus](../msg_docs/PositionControllerLandingStatus.md)
- [MagWorkerData](../msg_docs/MagWorkerData.md)
- [RadioStatus](../msg_docs/RadioStatus.md)
- [YawEstimatorStatus](../msg_docs/YawEstimatorStatus.md)
- [SensorTemp](../msg_docs/SensorTemp.md)
- [EscReport](../msg_docs/EscReport.md)
- [ActuatorOutputs](../msg_docs/ActuatorOutputs.md)
- [OpenDroneIdSystem](../msg_docs/OpenDroneIdSystem.md)
- [SensorCorrection](../msg_docs/SensorCorrection.md)
- [ControlAllocatorStatus](../msg_docs/ControlAllocatorStatus.md)
- [NormalizedUnsignedSetpoint](../msg_docs/NormalizedUnsignedSetpoint.md)
- [EstimatorBias3d](../msg_docs/EstimatorBias3d.md)
- [Cpuload](../msg_docs/Cpuload.md)
- [PowerMonitor](../msg_docs/PowerMonitor.md)
- [WheelEncoders](../msg_docs/WheelEncoders.md)
- [InternalCombustionEngineStatus](../msg_docs/InternalCombustionEngineStatus.md)
- [EstimatorAidSource2d](../msg_docs/EstimatorAidSource2d.md)
- [RaptorInput](../msg_docs/RaptorInput.md)
- [GimbalManagerInformation](../msg_docs/GimbalManagerInformation.md)
- [QshellReq](../msg_docs/QshellReq.md)
- [EstimatorEventFlags](../msg_docs/EstimatorEventFlags.md)
- [TuneControl](../msg_docs/TuneControl.md)
- [ManualControlSwitches](../msg_docs/ManualControlSwitches.md)
- [Px4ioStatus](../msg_docs/Px4ioStatus.md)
- [FixedWingRunwayControl](../msg_docs/FixedWingRunwayControl.md)
- [Mission](../msg_docs/Mission.md)
- [HomePositionV0](../msg_docs/HomePositionV0.md)
- [InternalCombustionEngineControl](../msg_docs/InternalCombustionEngineControl.md)
- [VehicleAttitudeSetpointV0](../msg_docs/VehicleAttitudeSetpointV0.md)
- [VehicleConstraints](../msg_docs/VehicleConstraints.md)
- [NeuralControl](../msg_docs/NeuralControl.md)
- [MountOrientation](../msg_docs/MountOrientation.md)
- [VehicleStatusV1](../msg_docs/VehicleStatusV1.md)
- [GpioConfig](../msg_docs/GpioConfig.md)
- [FixedWingLateralStatus](../msg_docs/FixedWingLateralStatus.md)
- [ButtonEvent](../msg_docs/ButtonEvent.md)
- [BatteryInfo](../msg_docs/BatteryInfo.md)
- [VehicleOpticalFlowVel](../msg_docs/VehicleOpticalFlowVel.md)
- [DistanceSensorModeChangeRequest](../msg_docs/DistanceSensorModeChangeRequest.md)
- [RaptorStatus](../msg_docs/RaptorStatus.md)
- [GpsInjectData](../msg_docs/GpsInjectData.md)
- [EstimatorSelectorStatus](../msg_docs/EstimatorSelectorStatus.md)
- [DebugVect](../msg_docs/DebugVect.md)
- [VehicleOpticalFlow](../msg_docs/VehicleOpticalFlow.md)
- [SensorGyro](../msg_docs/SensorGyro.md)
- [PositionControllerStatus](../msg_docs/PositionControllerStatus.md)
- [DeviceInformation](../msg_docs/DeviceInformation.md)
- [PositionSetpoint](../msg_docs/PositionSetpoint.md)
- [RtlTimeEstimate](../msg_docs/RtlTimeEstimate.md)
- [NavigatorMissionItem](../msg_docs/NavigatorMissionItem.md)
- [OpenDroneIdOperatorId](../msg_docs/OpenDroneIdOperatorId.md)
- [ActuatorControlsStatus](../msg_docs/ActuatorControlsStatus.md)
- [AirspeedWind](../msg_docs/AirspeedWind.md)
- [HeaterStatus](../msg_docs/HeaterStatus.md)
- [RoverSpeedStatus](../msg_docs/RoverSpeedStatus.md)
- [IrlockReport](../msg_docs/IrlockReport.md)
- [LedControl](../msg_docs/LedControl.md)
- [SensorAccel](../msg_docs/SensorAccel.md)
- [CameraStatus](../msg_docs/CameraStatus.md)
- [QshellRetval](../msg_docs/QshellRetval.md)
- [EscStatus](../msg_docs/EscStatus.md)
- [Vtx](../msg_docs/Vtx.md)
- [AirspeedValidatedV0](../msg_docs/AirspeedValidatedV0.md)
- [MavlinkLog](../msg_docs/MavlinkLog.md)
- [GimbalManagerStatus](../msg_docs/GimbalManagerStatus.md)
- [PurePursuitStatus](../msg_docs/PurePursuitStatus.md)
- [RateCtrlStatus](../msg_docs/RateCtrlStatus.md)
- [SystemPower](../msg_docs/SystemPower.md)
- [SensorAirflow](../msg_docs/SensorAirflow.md)
- [GpioIn](../msg_docs/GpioIn.md)
- [NavigatorStatus](../msg_docs/NavigatorStatus.md)
- [FuelTankStatus](../msg_docs/FuelTankStatus.md)
- [LoggerStatus](../msg_docs/LoggerStatus.md)
- [VehicleAngularVelocity](../msg_docs/VehicleAngularVelocity.md)
- [Ping](../msg_docs/Ping.md)
- [GeofenceStatus](../msg_docs/GeofenceStatus.md)
- [DatamanResponse](../msg_docs/DatamanResponse.md)
- [PpsCapture](../msg_docs/PpsCapture.md)
- [UavcanParameterRequest](../msg_docs/UavcanParameterRequest.md)
- [TecsStatus](../msg_docs/TecsStatus.md)
- [AdcReport](../msg_docs/AdcReport.md)
- [VelocityLimits](../msg_docs/VelocityLimits.md)
- [GpioOut](../msg_docs/GpioOut.md)
- [VehicleLocalPositionSetpoint](../msg_docs/VehicleLocalPositionSetpoint.md)
- [ParameterSetUsedRequest](../msg_docs/ParameterSetUsedRequest.md)
- [Mission](../msg_docs/Mission.md)
- [VehicleMagnetometer](../msg_docs/VehicleMagnetometer.md)
- [GimbalManagerSetAttitude](../msg_docs/GimbalManagerSetAttitude.md)
- [FollowTargetStatus](../msg_docs/FollowTargetStatus.md)
- [SensorUwb](../msg_docs/SensorUwb.md)
- [SensorGnssStatus](../msg_docs/SensorGnssStatus.md)
- [RoverAttitudeStatus](../msg_docs/RoverAttitudeStatus.md)
- [OpenDroneIdSelfId](../msg_docs/OpenDroneIdSelfId.md)
- [EstimatorAidSource3d](../msg_docs/EstimatorAidSource3d.md)
- [GimbalControls](../msg_docs/GimbalControls.md)
- [InternalCombustionEngineControl](../msg_docs/InternalCombustionEngineControl.md)
- [VehicleLocalPositionSetpoint](../msg_docs/VehicleLocalPositionSetpoint.md)
- [LedControl](../msg_docs/LedControl.md)
- [GimbalDeviceInformation](../msg_docs/GimbalDeviceInformation.md)
- [LoggerStatus](../msg_docs/LoggerStatus.md)
- [VehicleAcceleration](../msg_docs/VehicleAcceleration.md)
- [UlogStream](../msg_docs/UlogStream.md)
- [RcChannels](../msg_docs/RcChannels.md)
- [FigureEightStatus](../msg_docs/FigureEightStatus.md)
- [RoverRateStatus](../msg_docs/RoverRateStatus.md)
- [UlogStreamAck](../msg_docs/UlogStreamAck.md)
- [DebugVect](../msg_docs/DebugVect.md)
- [PositionControllerLandingStatus](../msg_docs/PositionControllerLandingStatus.md)
- [EscReport](../msg_docs/EscReport.md)
- [AirspeedWind](../msg_docs/AirspeedWind.md)
- [GimbalManagerSetAttitude](../msg_docs/GimbalManagerSetAttitude.md)
- [EstimatorSensorBias](../msg_docs/EstimatorSensorBias.md)
- [TakeoffStatus](../msg_docs/TakeoffStatus.md)
- [DifferentialPressure](../msg_docs/DifferentialPressure.md)
- [FixedWingLateralGuidanceStatus](../msg_docs/FixedWingLateralGuidanceStatus.md)
- [RegisterExtComponentRequestV0](../msg_docs/RegisterExtComponentRequestV0.md)
- [GeneratorStatus](../msg_docs/GeneratorStatus.md)
- [VehicleImu](../msg_docs/VehicleImu.md)
- [FollowTargetEstimator](../msg_docs/FollowTargetEstimator.md)
- [Event](../msg_docs/Event.md)
- [ArmingCheckRequestV0](../msg_docs/ArmingCheckRequestV0.md)
- [OrbTest](../msg_docs/OrbTest.md)
- [HoverThrustEstimate](../msg_docs/HoverThrustEstimate.md)
- [PwmInput](../msg_docs/PwmInput.md)
- [SensorGyroFifo](../msg_docs/SensorGyroFifo.md)
- [LandingGearWheel](../msg_docs/LandingGearWheel.md)
- [UavcanParameterValue](../msg_docs/UavcanParameterValue.md)
- [ParameterSetValueRequest](../msg_docs/ParameterSetValueRequest.md)
- [FlightPhaseEstimation](../msg_docs/FlightPhaseEstimation.md)
- [SensorSelection](../msg_docs/SensorSelection.md)
- [EstimatorBias3d](../msg_docs/EstimatorBias3d.md)
- [EscStatus](../msg_docs/EscStatus.md)
- [TaskStackInfo](../msg_docs/TaskStackInfo.md)
- [MountOrientation](../msg_docs/MountOrientation.md)
- [Cpuload](../msg_docs/Cpuload.md)
- [RaptorStatus](../msg_docs/RaptorStatus.md)
- [ActuatorOutputs](../msg_docs/ActuatorOutputs.md)
- [MavlinkLog](../msg_docs/MavlinkLog.md)
- [DronecanNodeStatus](../msg_docs/DronecanNodeStatus.md)
- [GainCompression](../msg_docs/GainCompression.md)
- [ActuatorArmed](../msg_docs/ActuatorArmed.md)
- [VehicleStatusV0](../msg_docs/VehicleStatusV0.md)
- [GpsDump](../msg_docs/GpsDump.md)
- [HealthReport](../msg_docs/HealthReport.md)
- [ActuatorTest](../msg_docs/ActuatorTest.md)
- [ButtonEvent](../msg_docs/ButtonEvent.md)
- [HeaterStatus](../msg_docs/HeaterStatus.md)
- [CanInterfaceStatus](../msg_docs/CanInterfaceStatus.md)
- [NavigatorMissionItem](../msg_docs/NavigatorMissionItem.md)
- [GeneratorStatus](../msg_docs/GeneratorStatus.md)
- [OrbTest](../msg_docs/OrbTest.md)
- [NormalizedUnsignedSetpoint](../msg_docs/NormalizedUnsignedSetpoint.md)
- [FollowTarget](../msg_docs/FollowTarget.md)
- [ActuatorServosTrim](../msg_docs/ActuatorServosTrim.md)
- [DebugKeyValue](../msg_docs/DebugKeyValue.md)
- [InputRc](../msg_docs/InputRc.md)
- [SensorGnssRelative](../msg_docs/SensorGnssRelative.md)
- [OrbTestMedium](../msg_docs/OrbTestMedium.md)
- [QshellReq](../msg_docs/QshellReq.md)
- [UavcanParameterValue](../msg_docs/UavcanParameterValue.md)
- [TecsStatus](../msg_docs/TecsStatus.md)
- [Vtx](../msg_docs/Vtx.md)
- [SensorGyro](../msg_docs/SensorGyro.md)
- [VehicleAngularVelocity](../msg_docs/VehicleAngularVelocity.md)
- [FollowTargetStatus](../msg_docs/FollowTargetStatus.md)
- [GpioIn](../msg_docs/GpioIn.md)
- [DatamanResponse](../msg_docs/DatamanResponse.md)
- [FollowTargetEstimator](../msg_docs/FollowTargetEstimator.md)
- [GimbalControls](../msg_docs/GimbalControls.md)
- [TakeoffStatus](../msg_docs/TakeoffStatus.md)
- [OpenDroneIdSystem](../msg_docs/OpenDroneIdSystem.md)
- [RtlTimeEstimate](../msg_docs/RtlTimeEstimate.md)
- [SensorMag](../msg_docs/SensorMag.md)
- [VehicleImuStatus](../msg_docs/VehicleImuStatus.md)
- [VehicleCommandAckV0](../msg_docs/VehicleCommandAckV0.md)
- [QshellRetval](../msg_docs/QshellRetval.md)
- [SensorGnssStatus](../msg_docs/SensorGnssStatus.md)
- [RcChannels](../msg_docs/RcChannels.md)
- [InternalCombustionEngineStatus](../msg_docs/InternalCombustionEngineStatus.md)
- [RtlStatus](../msg_docs/RtlStatus.md)
- [OrbTestMedium](../msg_docs/OrbTestMedium.md)
- [Rpm](../msg_docs/Rpm.md)
- [PwmInput](../msg_docs/PwmInput.md)
- [OpenDroneIdSelfId](../msg_docs/OpenDroneIdSelfId.md)
- [SensorUwb](../msg_docs/SensorUwb.md)
- [AirspeedValidatedV0](../msg_docs/AirspeedValidatedV0.md)
- [SensorGnssRelative](../msg_docs/SensorGnssRelative.md)
- [ParameterSetValueRequest](../msg_docs/ParameterSetValueRequest.md)
- [ManualControlSwitches](../msg_docs/ManualControlSwitches.md)
- [RegisterExtComponentReplyV0](../msg_docs/RegisterExtComponentReplyV0.md)
- [OrbTestLarge](../msg_docs/OrbTestLarge.md)
- [EstimatorAidSource3d](../msg_docs/EstimatorAidSource3d.md)
- [ArmingCheckReplyV0](../msg_docs/ArmingCheckReplyV0.md)
- [GpioRequest](../msg_docs/GpioRequest.md)
- [DebugValue](../msg_docs/DebugValue.md)
- [FlightPhaseEstimation](../msg_docs/FlightPhaseEstimation.md)
- [VelocityLimits](../msg_docs/VelocityLimits.md)
- [SensorCorrection](../msg_docs/SensorCorrection.md)
- [IridiumsbdStatus](../msg_docs/IridiumsbdStatus.md)
- [BatteryInfo](../msg_docs/BatteryInfo.md)
- [FixedWingLateralGuidanceStatus](../msg_docs/FixedWingLateralGuidanceStatus.md)
- [GpioOut](../msg_docs/GpioOut.md)
- [TiltrotorExtraControls](../msg_docs/TiltrotorExtraControls.md)
- [GimbalManagerInformation](../msg_docs/GimbalManagerInformation.md)
- [EstimatorInnovations](../msg_docs/EstimatorInnovations.md)
- [YawEstimatorStatus](../msg_docs/YawEstimatorStatus.md)
- [VehicleLocalPositionV0](../msg_docs/VehicleLocalPositionV0.md)
- [GimbalManagerSetManualControl](../msg_docs/GimbalManagerSetManualControl.md)
- [MissionResult](../msg_docs/MissionResult.md)
- [TuneControl](../msg_docs/TuneControl.md)
- [WheelEncoders](../msg_docs/WheelEncoders.md)
- [FailureDetectorStatus](../msg_docs/FailureDetectorStatus.md)
- [RegisterExtComponentRequestV0](../msg_docs/RegisterExtComponentRequestV0.md)
- [EstimatorGpsStatus](../msg_docs/EstimatorGpsStatus.md)
- [HealthReport](../msg_docs/HealthReport.md)
- [NavigatorStatus](../msg_docs/NavigatorStatus.md)
- [DistanceSensorModeChangeRequest](../msg_docs/DistanceSensorModeChangeRequest.md)
- [CellularStatus](../msg_docs/CellularStatus.md)
- [LandingGearWheel](../msg_docs/LandingGearWheel.md)
- [ActuatorControlsStatus](../msg_docs/ActuatorControlsStatus.md)
- [SensorGyroFft](../msg_docs/SensorGyroFft.md)
- [VehicleRoi](../msg_docs/VehicleRoi.md)
- [OpenDroneIdOperatorId](../msg_docs/OpenDroneIdOperatorId.md)
- [PpsCapture](../msg_docs/PpsCapture.md)
- [GimbalManagerStatus](../msg_docs/GimbalManagerStatus.md)
- [VehicleStatusV0](../msg_docs/VehicleStatusV0.md)
- [ActuatorArmed](../msg_docs/ActuatorArmed.md)
- [DifferentialPressure](../msg_docs/DifferentialPressure.md)
- [ParameterUpdate](../msg_docs/ParameterUpdate.md)
- [ActionRequest](../msg_docs/ActionRequest.md)
- [LaunchDetectionStatus](../msg_docs/LaunchDetectionStatus.md)
- [AutotuneAttitudeControlStatus](../msg_docs/AutotuneAttitudeControlStatus.md)
- [DebugArray](../msg_docs/DebugArray.md)
- [ControlAllocatorStatus](../msg_docs/ControlAllocatorStatus.md)
- [RoverRateStatus](../msg_docs/RoverRateStatus.md)
- [SatelliteInfo](../msg_docs/SatelliteInfo.md)
- [CameraStatus](../msg_docs/CameraStatus.md)
- [LogMessage](../msg_docs/LogMessage.md)
- [PositionControllerStatus](../msg_docs/PositionControllerStatus.md)
- [SensorBaro](../msg_docs/SensorBaro.md)
- [SensorAccelFifo](../msg_docs/SensorAccelFifo.md)
- [SensorHygrometer](../msg_docs/SensorHygrometer.md)
- [PositionSetpoint](../msg_docs/PositionSetpoint.md)
- [VehicleAngularAccelerationSetpoint](../msg_docs/VehicleAngularAccelerationSetpoint.md)
- [GpsDump](../msg_docs/GpsDump.md)
- [GpioConfig](../msg_docs/GpioConfig.md)
- [GeofenceStatus](../msg_docs/GeofenceStatus.md)
- [EstimatorEventFlags](../msg_docs/EstimatorEventFlags.md)
- [SensorPreflightMag](../msg_docs/SensorPreflightMag.md)
- [VehicleOpticalFlow](../msg_docs/VehicleOpticalFlow.md)
- [PurePursuitStatus](../msg_docs/PurePursuitStatus.md)
- [GeofenceResult](../msg_docs/GeofenceResult.md)
- [EstimatorBias](../msg_docs/EstimatorBias.md)
- [SystemPower](../msg_docs/SystemPower.md)
- [EventV0](../msg_docs/EventV0.md)
- [CameraCapture](../msg_docs/CameraCapture.md)
- [RoverSpeedStatus](../msg_docs/RoverSpeedStatus.md)
- [HomePositionV0](../msg_docs/HomePositionV0.md)
- [VehicleGlobalPositionV0](../msg_docs/VehicleGlobalPositionV0.md)
- [Px4ioStatus](../msg_docs/Px4ioStatus.md)
- [EstimatorStates](../msg_docs/EstimatorStates.md)
- [FixedWingRunwayControl](../msg_docs/FixedWingRunwayControl.md)
- [FigureEightStatus](../msg_docs/FigureEightStatus.md)
- [InputRc](../msg_docs/InputRc.md)
- [Ekf2Timestamps](../msg_docs/Ekf2Timestamps.md)
- [LandingTargetInnovations](../msg_docs/LandingTargetInnovations.md)
- [UavcanParameterRequest](../msg_docs/UavcanParameterRequest.md)
- [DatamanRequest](../msg_docs/DatamanRequest.md)
- [ParameterResetRequest](../msg_docs/ParameterResetRequest.md)
- [VehicleConstraints](../msg_docs/VehicleConstraints.md)
- [VehicleAttitudeSetpointV0](../msg_docs/VehicleAttitudeSetpointV0.md)
- [FixedWingLateralStatus](../msg_docs/FixedWingLateralStatus.md)
- [RoverAttitudeStatus](../msg_docs/RoverAttitudeStatus.md)
- [NeuralControl](../msg_docs/NeuralControl.md)
- [DebugKeyValue](../msg_docs/DebugKeyValue.md)
- [GimbalDeviceSetAttitude](../msg_docs/GimbalDeviceSetAttitude.md)
- [EstimatorStatus](../msg_docs/EstimatorStatus.md)
- [ArmingCheckRequestV0](../msg_docs/ArmingCheckRequestV0.md)
- [TrajectorySetpoint6dof](../msg_docs/TrajectorySetpoint6dof.md)
- [ParameterSetValueResponse](../msg_docs/ParameterSetValueResponse.md)
- [MagnetometerBiasEstimate](../msg_docs/MagnetometerBiasEstimate.md)
- [CameraTrigger](../msg_docs/CameraTrigger.md)
- [PowerButtonState](../msg_docs/PowerButtonState.md)
- [ConfigOverridesV0](../msg_docs/ConfigOverridesV0.md)
- [VehicleStatusV1](../msg_docs/VehicleStatusV1.md)
- [AdcReport](../msg_docs/AdcReport.md)
- [SensorsStatus](../msg_docs/SensorsStatus.md)
- [UlogStreamAck](../msg_docs/UlogStreamAck.md)
- [UlogStream](../msg_docs/UlogStream.md)
- [SensorGyroFifo](../msg_docs/SensorGyroFifo.md)
- [BatteryStatusV0](../msg_docs/BatteryStatusV0.md)
- [SensorAccel](../msg_docs/SensorAccel.md)
- [PowerMonitor](../msg_docs/PowerMonitor.md)
- [OrbitStatus](../msg_docs/OrbitStatus.md)
- [RateCtrlStatus](../msg_docs/RateCtrlStatus.md)
- [MavlinkTunnel](../msg_docs/MavlinkTunnel.md)
- [Airspeed](../msg_docs/Airspeed.md)
- [EstimatorAidSource2d](../msg_docs/EstimatorAidSource2d.md)
- [EstimatorAidSource1d](../msg_docs/EstimatorAidSource1d.md)
- [RadioStatus](../msg_docs/RadioStatus.md)
- [VehicleOpticalFlowVel](../msg_docs/VehicleOpticalFlowVel.md)
- [VehicleAirData](../msg_docs/VehicleAirData.md)
- [SensorAirflow](../msg_docs/SensorAirflow.md)
- [Ping](../msg_docs/Ping.md)
- [MagWorkerData](../msg_docs/MagWorkerData.md)
- [FuelTankStatus](../msg_docs/FuelTankStatus.md)
- [GpsInjectData](../msg_docs/GpsInjectData.md)
- [Event](../msg_docs/Event.md)
- [OpenDroneIdArmStatus](../msg_docs/OpenDroneIdArmStatus.md)
- [HoverThrustEstimate](../msg_docs/HoverThrustEstimate.md)
- [RcParameterMap](../msg_docs/RcParameterMap.md)
- [Gripper](../msg_docs/Gripper.md)
- [SensorsStatusImu](../msg_docs/SensorsStatusImu.md)
- [VehicleImu](../msg_docs/VehicleImu.md)
- [SensorSelection](../msg_docs/SensorSelection.md)
- [EstimatorSelectorStatus](../msg_docs/EstimatorSelectorStatus.md)
- [IrlockReport](../msg_docs/IrlockReport.md)
- [SensorTemp](../msg_docs/SensorTemp.md)
- [RaptorInput](../msg_docs/RaptorInput.md)
:::
+6 -6
View File
@@ -22,11 +22,11 @@ Published by the vehicle's allocation and consumed by the ESC protocol drivers e
## Constants
| Name | Type | Value | Description |
| --------------------------------------------------------------- | -------- | ----- | ----------- |
| Name | Type | Value | Description |
| --------------------------------------------------------------- | -------- | ----- | --------------------------------- |
| <a id="#MESSAGE_VERSION"></a> MESSAGE_VERSION | `uint32` | 0 |
| <a id="#ACTUATOR_FUNCTION_MOTOR1"></a> ACTUATOR_FUNCTION_MOTOR1 | `uint8` | 101 |
| <a id="#NUM_CONTROLS"></a> NUM_CONTROLS | `uint8` | 12 |
| <a id="#ACTUATOR_FUNCTION_MOTOR1"></a> ACTUATOR_FUNCTION_MOTOR1 | `uint8` | 101 | output_functions.yaml Motor.start |
| <a id="#NUM_CONTROLS"></a> NUM_CONTROLS | `uint8` | 12 | output_functions.yaml Motor.count |
## Source Message
@@ -47,9 +47,9 @@ uint64 timestamp_sample # [us] Sampling timestamp of the data this control respo
uint16 reversible_flags # [-] Bitset indicating which motors are configured to be reversible
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 # output_functions.yaml Motor.start
uint8 NUM_CONTROLS = 12
uint8 NUM_CONTROLS = 12 # output_functions.yaml Motor.count
float32[12] control # [@range -1, 1] Normalized thrust. Where 1 means maximum positive thrust, -1 maximum negative (if not supported by the output, <0 maps to NaN). NaN maps to disarmed (stop the motors)
```
+2 -23
View File
@@ -29,17 +29,7 @@ pageClass: is-wide-page
| Name | Type | Value | Description |
| --------------------------------------------------------------------------- | ------- | ----- | ---------------------------------------------------------------------------- |
| <a id="#ACTUATOR_FUNCTION_MOTOR1"></a> ACTUATOR_FUNCTION_MOTOR1 | `uint8` | 101 |
| <a id="#ACTUATOR_FUNCTION_MOTOR2"></a> ACTUATOR_FUNCTION_MOTOR2 | `uint8` | 102 |
| <a id="#ACTUATOR_FUNCTION_MOTOR3"></a> ACTUATOR_FUNCTION_MOTOR3 | `uint8` | 103 |
| <a id="#ACTUATOR_FUNCTION_MOTOR4"></a> ACTUATOR_FUNCTION_MOTOR4 | `uint8` | 104 |
| <a id="#ACTUATOR_FUNCTION_MOTOR5"></a> ACTUATOR_FUNCTION_MOTOR5 | `uint8` | 105 |
| <a id="#ACTUATOR_FUNCTION_MOTOR6"></a> ACTUATOR_FUNCTION_MOTOR6 | `uint8` | 106 |
| <a id="#ACTUATOR_FUNCTION_MOTOR7"></a> ACTUATOR_FUNCTION_MOTOR7 | `uint8` | 107 |
| <a id="#ACTUATOR_FUNCTION_MOTOR8"></a> ACTUATOR_FUNCTION_MOTOR8 | `uint8` | 108 |
| <a id="#ACTUATOR_FUNCTION_MOTOR9"></a> ACTUATOR_FUNCTION_MOTOR9 | `uint8` | 109 |
| <a id="#ACTUATOR_FUNCTION_MOTOR10"></a> ACTUATOR_FUNCTION_MOTOR10 | `uint8` | 110 |
| <a id="#ACTUATOR_FUNCTION_MOTOR11"></a> ACTUATOR_FUNCTION_MOTOR11 | `uint8` | 111 |
| <a id="#ACTUATOR_FUNCTION_MOTOR12"></a> ACTUATOR_FUNCTION_MOTOR12 | `uint8` | 112 |
| <a id="#ACTUATOR_FUNCTION_MOTOR_MAX"></a> ACTUATOR_FUNCTION_MOTOR_MAX | `uint8` | 112 | output_functions.yaml Motor.start + Motor.count - 1 |
| <a id="#FAILURE_OVER_CURRENT"></a> FAILURE_OVER_CURRENT | `uint8` | 0 | (1 << 0) |
| <a id="#FAILURE_OVER_VOLTAGE"></a> FAILURE_OVER_VOLTAGE | `uint8` | 1 | (1 << 1) |
| <a id="#FAILURE_MOTOR_OVER_TEMPERATURE"></a> FAILURE_MOTOR_OVER_TEMPERATURE | `uint8` | 2 | (1 << 2) |
@@ -72,19 +62,8 @@ uint8 esc_cmdcount # Counter of number of commands
uint8 esc_state # State of ESC - depend on Vendor
uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR2 = 102
uint8 ACTUATOR_FUNCTION_MOTOR3 = 103
uint8 ACTUATOR_FUNCTION_MOTOR4 = 104
uint8 ACTUATOR_FUNCTION_MOTOR5 = 105
uint8 ACTUATOR_FUNCTION_MOTOR6 = 106
uint8 ACTUATOR_FUNCTION_MOTOR7 = 107
uint8 ACTUATOR_FUNCTION_MOTOR8 = 108
uint8 ACTUATOR_FUNCTION_MOTOR9 = 109
uint8 ACTUATOR_FUNCTION_MOTOR10 = 110
uint8 ACTUATOR_FUNCTION_MOTOR11 = 111
uint8 ACTUATOR_FUNCTION_MOTOR12 = 112
uint8 ACTUATOR_FUNCTION_MOTOR_MAX = 112 # output_functions.yaml Motor.start + Motor.count - 1
uint16 failures # Bitmask to indicate the internal ESC faults
int8 esc_power # Applied power 0-100 in % (negative values reserved)
+4
View File
@@ -27,5 +27,9 @@
},
"devDependencies": {
"prettier": "^3.2.0"
},
"resolutions": {
"markdown-it": "^14.1.1",
"esbuild": "^0.25.0"
}
}
File diff suppressed because one or more lines are too long
Binary file not shown.
File diff suppressed because one or more lines are too long
+487 -445
View File
File diff suppressed because it is too large Load Diff
+1 -12
View File
@@ -11,19 +11,8 @@ uint8 esc_cmdcount # Counter of number of commands
uint8 esc_state # State of ESC - depend on Vendor
uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR2 = 102
uint8 ACTUATOR_FUNCTION_MOTOR3 = 103
uint8 ACTUATOR_FUNCTION_MOTOR4 = 104
uint8 ACTUATOR_FUNCTION_MOTOR5 = 105
uint8 ACTUATOR_FUNCTION_MOTOR6 = 106
uint8 ACTUATOR_FUNCTION_MOTOR7 = 107
uint8 ACTUATOR_FUNCTION_MOTOR8 = 108
uint8 ACTUATOR_FUNCTION_MOTOR9 = 109
uint8 ACTUATOR_FUNCTION_MOTOR10 = 110
uint8 ACTUATOR_FUNCTION_MOTOR11 = 111
uint8 ACTUATOR_FUNCTION_MOTOR12 = 112
uint8 ACTUATOR_FUNCTION_MOTOR_MAX = 112 # output_functions.yaml Motor.start + Motor.count - 1
uint16 failures # Bitmask to indicate the internal ESC faults
int8 esc_power # Applied power 0-100 in % (negative values reserved)
+2 -2
View File
@@ -10,7 +10,7 @@ uint64 timestamp_sample # [us] Sampling timestamp of the data this control respo
uint16 reversible_flags # [-] Bitset indicating which motors are configured to be reversible
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 # output_functions.yaml Motor.start
uint8 NUM_CONTROLS = 12
uint8 NUM_CONTROLS = 12 # output_functions.yaml Motor.count
float32[12] control # [@range -1, 1] Normalized thrust. Where 1 means maximum positive thrust, -1 maximum negative (if not supported by the output, <0 maps to NaN). NaN maps to disarmed (stop the motors)
@@ -645,7 +645,7 @@ int uORBTest::UnitTest::test_wrap_around()
}
#define CHECK_COPY(i_got, i_correct) \
orb_copy(ORB_ID(orb_test_medium_wrap_around), sfd, &u); \
if (i_got != i_correct) { \
if ((i_got) != (i_correct)) { \
return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \
}
@@ -875,7 +875,7 @@ int uORBTest::UnitTest::test_queue()
}
#define CHECK_COPY(i_got, i_correct) \
orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \
if (i_got != i_correct) { \
if ((i_got) != (i_correct)) { \
return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \
}
+1 -9
View File
@@ -80,8 +80,7 @@ target_link_libraries(px4 PRIVATE uORB)
# install
#
# Generic install rules (skipped when board provides its own install.cmake)
if(NOT EXISTS "${PX4_BOARD_DIR}/cmake/install.cmake")
# TODO: extend to snapdragon
# px4 dirs
install(
@@ -95,8 +94,6 @@ install(
USE_SOURCE_PERMISSIONS
)
endif() # NOT board install.cmake
# Module Symlinks
px4_posix_generate_symlinks(
MODULE_LIST ${module_libraries}
@@ -115,11 +112,6 @@ if(EXISTS "${PX4_BOARD_DIR}/cmake/upload.cmake")
include(${PX4_BOARD_DIR}/cmake/upload.cmake)
endif()
# board defined install rules for .deb packaging
if(EXISTS "${PX4_BOARD_DIR}/cmake/install.cmake")
include(${PX4_BOARD_DIR}/cmake/install.cmake)
endif()
# board defined link libraries
if(EXISTS "${PX4_BOARD_DIR}/cmake/link_libraries.cmake")
include(${PX4_BOARD_DIR}/cmake/link_libraries.cmake)
@@ -88,7 +88,7 @@ typedef enum : int32_t {
TRIGGER_MODE_DISTANCE_ON_CMD
} trigger_mode_t;
#define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
#define commandParamToInt(n) static_cast<int>((n) >= 0 ? (n) + 0.5f : (n) - 0.5f)
class CameraTrigger final : public px4::ScheduledWorkItem
{
+2 -2
View File
@@ -51,8 +51,8 @@
# define debug(fmt, args...) do { } while(0)
#endif
#define CODER_CHECK(_c) do { if (_c->dead) { PX4_ERR("coder dead"); return -1; }} while(0)
#define CODER_KILL(_c, _reason) do { PX4_ERR("killed: %s", _reason); _c->dead = true; return -1; } while(0)
#define CODER_CHECK(_c) do { if ((_c)->dead) { PX4_ERR("coder dead"); return -1; }} while(0)
#define CODER_KILL(_c, _reason) do { PX4_ERR("killed: %s", _reason); (_c)->dead = true; return -1; } while(0)
static int
read_x(bson_decoder_t decoder, void *p, size_t s)
+1 -1
View File
@@ -46,7 +46,7 @@
#include <errno.h>
#define BEAT_TIME_CONVERSION_MS (60 * 1000 * 4)
#define BEAT_TIME_CONVERSION_US BEAT_TIME_CONVERSION_MS * 1000
#define BEAT_TIME_CONVERSION_US (BEAT_TIME_CONVERSION_MS * 1000)
#define BEAT_TIME_CONVERSION BEAT_TIME_CONVERSION_US
// semitone offsets from C for the characters 'A'-'G'
+1 -1
View File
@@ -2016,7 +2016,7 @@ void Commander::run()
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
_failure_detector.publishStatus();
_failure_detector.publishStatus(_health_and_arming_checks.getEscArmStatus(), _health_and_arming_checks.getMotorFailureMask());
}
checkWorkerThread();
@@ -109,6 +109,9 @@ public:
const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
uint16_t getMotorFailureMask() const {return _esc_checks.getMotorFailureMask(); }
bool getEscArmStatus() const { return _esc_checks.getEscArmStatus(); }
#ifndef CONSTRAINED_FLASH
ExternalChecks &externalChecks() { return _external_checks; }
#endif
@@ -37,13 +37,13 @@ void ArmPermissionChecks::checkAndReport(const Context &context, Report &reporte
{
if (_param_com_armable.get() < 1) {
/* EVENT
* @description
* Vehicle is in safety configuration and denies arming.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARMABLE</param> parameter.
* </profile>
*/
* @description
* Vehicle is in safety configuration and denies arming.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARMABLE</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_armable_configuration"),
events::Log::Error, "Vehicle is in safety configuration");
@@ -211,13 +211,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
// This is declared critical so QGC displays a yellow box and reads "low battery" out loud making the user aware
/* EVENT
* @description
* The lowest battery state of charge is below the low threshold.
*
* <profile name="dev">
* Can be configured with <param>BAT_LOW_THR</param>.
* </profile>
*/
* @description
* The lowest battery state of charge is below the low threshold.
*
* <profile name="dev">
* Can be configured with <param>BAT_LOW_THR</param>.
* </profile>
*/
reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"),
events::Log::Critical, "Low battery");
@@ -229,13 +229,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
case battery_status_s::WARNING_CRITICAL:
/* EVENT
* @description
* The lowest battery state of charge is below the critical threshold.
*
* <profile name="dev">
* Can be configured with <param>BAT_CRIT_THR</param> and from when to disalow arming with <param>COM_ARM_BAT_MIN</param>.
* </profile>
*/
* @description
* The lowest battery state of charge is below the critical threshold.
*
* <profile name="dev">
* Can be configured with <param>BAT_CRIT_THR</param> and from when to disalow arming with <param>COM_ARM_BAT_MIN</param>.
* </profile>
*/
reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_critical"),
events::Log::Critical, "Critical battery");
@@ -247,13 +247,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
case battery_status_s::WARNING_EMERGENCY:
/* EVENT
* @description
* The lowest battery state of charge is below the emergency threshold.
*
* <profile name="dev">
* Can be configured with <param>BAT_EMERGEN_THR</param>.
* </profile>
*/
* @description
* The lowest battery state of charge is below the emergency threshold.
*
* <profile name="dev">
* Can be configured with <param>BAT_EMERGEN_THR</param>.
* </profile>
*/
reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_emergency"),
events::Log::Emergency, "Emergency battery level");
@@ -62,7 +62,6 @@ static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_r
case esc_fault_reason_t::esc_warn_temp: return "over temperature";
case esc_fault_reason_t::esc_over_temp: return "critical temperature";
}
return "";
@@ -72,19 +71,31 @@ static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_r
void EscChecks::checkAndReport(const Context &context, Report &reporter)
{
const hrt_abstime now = hrt_absolute_time();
const hrt_abstime esc_telemetry_timeout =
700_ms; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
esc_status_s esc_status;
if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) {
// Run motor status checks even when no new ESC data arrives (to detect timeouts)
if (_esc_status_sub.copy(&esc_status)) {
if (esc_status.esc_count <= 0) {
return;
}
checkEscStatus(context, reporter, esc_status);
uint16_t mask = 0;
if (_param_com_arm_chk_escs.get() > 0) {
mask |= checkEscOnline(context, reporter, esc_status, now);
mask |= checkEscStatus(context, reporter, esc_status);
}
if (_param_fd_act_en.get() > 0) {
updateEscsStatus(context, reporter, esc_status, now);
mask |= checkMotorStatus(context, reporter, esc_status, now);
}
_motor_failure_mask = mask;
reporter.setIsPresent(health_component_t::motors_escs);
reporter.failsafeFlags().fd_motor_failure = (mask != 0);
} else if (_param_com_arm_chk_escs.get()
&& now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init
} else if ((_param_com_arm_chk_escs.get() > 0) && now > _start_time + 3_s) { // Allow ESCs to init
/* EVENT
* @description
* <profile name="dev">
@@ -100,83 +111,240 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
}
}
void EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now)
{
const NavModes required_modes = _param_com_arm_chk_escs.get() ? NavModes::All : NavModes::None;
// Check if one or more the ESCs are offline
uint16_t mask = 0;
char esc_fail_msg[esc_status_s::CONNECTED_ESC_MAX * 6 + 1] = "";
if (esc_status.esc_count > 0) {
// Check if one or more the ESCs are offline
char esc_fail_msg[50];
esc_fail_msg[0] = '\0';
for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) {
if (!math::isInRange(esc_status.esc[esc_index].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) {
continue; // Skip unmapped ESC status entries
}
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1));
const bool offline = (esc_status.esc_online_flags & (1 << i)) == 0;
const bool timeout = now > esc_status.esc[esc_index].timestamp + ESC_TIMEOUT_US;
const bool is_offline = (esc_status.esc_online_flags & (1 << esc_index)) == 0;
if (mapped && offline) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"),
events::Log::Critical, "ESC {1} offline", i + 1);
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", i + 1);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
// Set failure bits for this motor
if (timeout || is_offline) {
mask |= (1u << esc_index);
uint8_t esc_nr = esc_index + 1;
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs, events::ID("check_escs_offline"),
events::Log::Critical, "ESC {1} offline", esc_nr);
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", esc_nr);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
}
}
if (reporter.mavlink_log_pub() && esc_fail_msg[0] != '\0') {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
return mask;
}
uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
uint16_t mask = 0;
for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) {
if (!math::isInRange(esc_status.esc[esc_index].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) {
continue; // Skip unmapped ESC status entries
}
const uint8_t actuator_function_index = esc_status.esc[esc_index].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
if (esc_status.esc[esc_index].failures == 0) {
continue;
} else {
mask |= (1u << actuator_function_index); // Set bit in mask
}
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
if (!(esc_status.esc[esc_index].failures & (1 << fault_index))) {
continue;
}
}
if ((esc_fail_msg[0] != '\0') && reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
const char *user_action = "";
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {
if (context.isArmed()) {
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp
|| fault_reason_index == esc_fault_reason_t::over_rpm) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
if (esc_status.esc[index].failures != 0) {
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
if (esc_status.esc[index].failures & (1 << fault_index)) {
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
const char *user_action = "";
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
if (context.isArmed()) {
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
}
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
/* EVENT
* @description
* {3}
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
required_modes, health_component_t::motors_escs, events::ID("check_escs_fault"),
events::Log::Critical, "ESC {1}: {2}", motor_index, fault_reason_index, action);
if (reporter.mavlink_log_pub()) {
mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", motor_index,
esc_fault_reason_str(fault_reason_index), user_action);
}
}
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
}
/* EVENT
* @description
* {3}
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
events::Log::Critical, "ESC {1}: {2}", esc_index + 1, fault_reason_index, action);
if (reporter.mavlink_log_pub()) {
mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", esc_index + 1,
esc_fault_reason_str(fault_reason_index), user_action);
}
}
}
return mask;
}
uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now)
{
uint16_t mask = 0;
// Only check while armed
if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);
// Check individual ESC reports
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
if (!math::isInRange(esc_status.esc[i].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) {
continue; // Skip unmapped ESC status entries
}
const uint8_t actuator_function_index = esc_status.esc[i].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
const float current = esc_status.esc[i].esc_current;
// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
if (current > FLT_EPSILON) {
_esc_has_reported_current[i] = true;
}
if (!_esc_has_reported_current[i]) {
continue;
}
// Current limits
float thrust = 0.f;
if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
thrust = fabsf(actuator_motors.control[actuator_function_index]);
}
bool current_too_low = current < (thrust * _param_motfail_c2t.get()) - _param_motfail_low_off.get();
bool current_too_high = current > (thrust * _param_motfail_c2t.get()) + _param_motfail_high_off.get();
_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_motfail_time.get() * 1_s);
_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_motfail_time.get() * 1_s);
if (!_esc_undercurrent_hysteresis[i].get_state()) {
// Only set, never clear mid-air: stopping the motor in response could make it appear healthy again
_esc_undercurrent_hysteresis[i].set_state_and_update(current_too_low, now);
}
if (!_esc_overcurrent_hysteresis[i].get_state()) {
// Only set, never clear mid-air: stopping the motor in response could make it appear healthy again
_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high, now);
}
mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
if (_esc_undercurrent_hysteresis[i].get_state()) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_ACT_EN</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
events::ID("check_motor_undercurrent"),
events::Log::Critical, "Motor {1} undercurrent detected", actuator_function_index + 1);
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d undercurrent detected",
actuator_function_index + 1);
}
}
if (_esc_overcurrent_hysteresis[i].get_state()) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_ACT_EN</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
events::ID("check_motor_overcurrent"),
events::Log::Critical, "Motor {1} overcurrent detected", actuator_function_index + 1);
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Motor failure: Motor %d overcurrent detected",
actuator_function_index + 1);
}
}
}
} else { // Disarmed
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
}
}
return mask;
}
void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now)
{
if (context.status().arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);
_esc_arm_hysteresis.set_hysteresis_time_from(false, ESC_TIMEOUT_US);
_esc_arm_hysteresis.set_state_and_update(!is_all_escs_armed, now);
if (_esc_arm_hysteresis.get_state()) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs,
events::ID("check_escs_not_all_armed"),
events::Log::Critical, "Not all ESCs are armed");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "ESC failure: Not all ESCs are armed. Land now!");
}
reporter.failsafeFlags().fd_esc_arming_failure = true;
}
} else {
// reset ESC bitfield
_esc_arm_hysteresis.set_state_and_update(false, now);
reporter.failsafeFlags().fd_esc_arming_failure = false;
}
}
@@ -35,8 +35,11 @@
#include "../Common.hpp"
#include <lib/hysteresis/hysteresis.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_detector_status.h>
class EscChecks : public HealthAndArmingCheckBase
{
@@ -46,14 +49,33 @@ public:
void checkAndReport(const Context &context, Report &reporter) override;
uint16_t getMotorFailureMask() const { return _motor_failure_mask; }
bool getEscArmStatus() const { return _esc_arm_hysteresis.get_state(); }
private:
void checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
uint16_t checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now);
uint16_t checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
uint16_t checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now);
void updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status, hrt_abstime now);
static constexpr hrt_abstime ESC_TIMEOUT_US = 300_ms;
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
const hrt_abstime _start_time{hrt_absolute_time()};
uint16_t _motor_failure_mask = 0;
bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {};
systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
systemlib::Hysteresis _esc_arm_hysteresis;
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs
)
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
(ParamFloat<px4::params::MOTFAIL_C2T>) _param_motfail_c2t,
(ParamFloat<px4::params::MOTFAIL_TIME>) _param_motfail_time,
(ParamFloat<px4::params::MOTFAIL_LOW_OFF>) _param_motfail_low_off,
(ParamFloat<px4::params::MOTFAIL_HIGH_OFF>) _param_motfail_high_off);
};
@@ -98,27 +98,6 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
(vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT |
vehicle_status_s::FAILURE_EXT);
reporter.failsafeFlags().fd_esc_arming_failure = context.status().failure_detector_status &
vehicle_status_s::FAILURE_ARM_ESC;
if (reporter.failsafeFlags().fd_esc_arming_failure) {
/* EVENT
* @description
* One or more ESCs failed to arm.
*
* <profile name="dev">
* This check can be configured via <param>FD_ESCS_EN</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
events::Log::Critical, "ESC failure");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
}
}
reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status &
vehicle_status_s::FAILURE_IMBALANCED_PROP;
@@ -138,22 +117,4 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Imbalanced propeller detected");
}
}
reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
if (reporter.failsafeFlags().fd_motor_failure) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_ACT_EN</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_motor"),
events::Log::Critical, "Motor failure detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor failure detected");
}
}
}
@@ -80,9 +80,9 @@ void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter)
}
/* EVENT
* @description
* Maximal flight time warning (less than 1min remaining)
*/
* @description
* Maximal flight time warning (less than 1min remaining)
*/
events::send<int16_t>(events::ID("commander_max_flight_time_warning_seconds"), events::Log::Warning,
"Approaching max flight time (system will RTL in {1} seconds)", floored_remaining_flight_time_sec);
@@ -97,9 +97,9 @@ void FlightTimeChecks::checkAndReport(const Context &context, Report &reporter)
}
/* EVENT
* @description
* Maximal flight time warning (more than 1min remaining)
*/
* @description
* Maximal flight time warning (more than 1min remaining)
*/
events::send<int16_t>(events::ID("commander_max_flight_time_warning_minutes"), events::Log::Warning,
"Approaching max flight time (system will RTL in {1} minutes)", floored_remaining_flight_time_min);
}
@@ -51,11 +51,11 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
if (geofence_result.geofence_max_dist_triggered) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> and <param>GF_MAX_HOR_DIST</param> parameters.
* </profile>
*/
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> and <param>GF_MAX_HOR_DIST</param> parameters.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_gf_violation_max_hor_dist"),
events::Log::Error, "Geofence violation: exceeding maximum distance to Home");
@@ -67,11 +67,11 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
if (geofence_result.geofence_max_alt_triggered) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> and <param>GF_MAX_VER_DIST</param> parameters.
* </profile>
*/
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> and <param>GF_MAX_VER_DIST</param> parameters.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_gf_violation_max_alt"),
events::Log::Error, "Geofence violation: exceeding maximum altitude above Home");
@@ -83,11 +83,11 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
if (geofence_result.geofence_custom_fence_triggered) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
* @description
* <profile name="dev">
* This check can be configured via <param>GF_ACTION</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system,
events::ID("check_gf_violation_custom_gf"),
events::Log::Error, "Geofence violation: approaching or outside geofence");
@@ -0,0 +1,104 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* Enable Actuator Failure check
*
* If enabled, the HealthAndArmingChecks will verify that for motors, a minimum amount of ESC current per throttle
* level is being consumed.
* Otherwise this indicates an motor failure.
* This check only works for ESCs that report current consumption.
*
* @boolean
*
* @group Motor Failure
*/
PARAM_DEFINE_INT32(FD_ACT_EN, 0);
/**
* Motor Failure Current/Throttle Scale
*
* Determines the slope between expected steady state current and linearized, normalized thrust command.
* E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
* FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
*
* @group Motor Failure
* @min 0.0
* @max 50.0
* @unit A/%
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(MOTFAIL_C2T, 35.f);
/**
* Undercurrent motor failure limit offset
*
* threshold = FD_ACT_MOT_C2T * thrust - FD_ACT_LOW_OFF
*
* @group Motor Failure
* @min 0
* @max 30
* @unit A
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(MOTFAIL_LOW_OFF, 10.f);
/**
* Overcurrent motor failure limit offset
*
* threshold = FD_ACT_MOT_C2T * thrust + FD_ACT_HIGH_OFF
*
* @group Motor Failure
* @min 0
* @max 30
* @unit A
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(MOTFAIL_HIGH_OFF, 10.f);
/**
* Motor Failure Hysteresis Time
*
* Motor failure only triggers after current thresholds are exceeded for this time.
*
* @group Motor Failure
* @unit s
* @min 0.01
* @max 10
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(MOTFAIL_TIME, 1.f);
@@ -39,6 +39,7 @@
*/
#include "FailureDetector.hpp"
#include "../HealthAndArmingChecks/HealthAndArmingChecks.hpp"
using namespace time_literals;
@@ -67,21 +68,6 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
_failure_detector_status.flags.ext = false;
}
// esc_status subscriber is shared between subroutines
esc_status_s esc_status;
if (_esc_status_sub.update(&esc_status)) {
_failure_injector.manipulateEscStatus(esc_status);
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status, esc_status);
}
if (_param_fd_act_en.get()) {
updateMotorStatus(vehicle_status, esc_status);
}
}
if (_param_fd_imb_prop_thr.get() > 0) {
updateImbalancedPropStatus();
}
@@ -89,19 +75,19 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
return _failure_detector_status.value != status_prev.value;
}
void FailureDetector::publishStatus()
void FailureDetector::publishStatus(bool esc_arm_status, uint16_t motor_failure_mask)
{
failure_detector_status_s failure_detector_status{};
failure_detector_status.fd_roll = _failure_detector_status.flags.roll;
failure_detector_status.fd_pitch = _failure_detector_status.flags.pitch;
failure_detector_status.fd_alt = _failure_detector_status.flags.alt;
failure_detector_status.fd_ext = _failure_detector_status.flags.ext;
failure_detector_status.fd_arm_escs = _failure_detector_status.flags.arm_escs;
failure_detector_status.fd_arm_escs = esc_arm_status || (motor_failure_mask != 0);
failure_detector_status.fd_battery = _failure_detector_status.flags.battery;
failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop;
failure_detector_status.fd_motor = _failure_detector_status.flags.motor;
failure_detector_status.fd_motor = (motor_failure_mask != 0);
failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState();
failure_detector_status.motor_failure_mask = _motor_failure_mask;
failure_detector_status.motor_failure_mask = motor_failure_mask;
failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask();
failure_detector_status.timestamp = hrt_absolute_time();
_failure_detector_status_pub.publish(failure_detector_status);
@@ -172,34 +158,6 @@ void FailureDetector::updateExternalAtsStatus()
}
}
void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
hrt_abstime now = hrt_absolute_time();
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);
bool is_esc_failure = !is_all_escs_armed;
for (int i = 0; i < limited_esc_count; i++) {
is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, now);
if (_esc_failure_hysteresis.get_state()) {
_failure_detector_status.flags.arm_escs = true;
}
} else {
// reset ESC bitfield
_esc_failure_hysteresis.set_state_and_update(false, now);
_failure_detector_status.flags.arm_escs = false;
}
}
void FailureDetector::updateImbalancedPropStatus()
{
@@ -258,82 +216,3 @@ void FailureDetector::updateImbalancedPropStatus()
}
}
}
void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
// 1. Telemetry times out -> communication or power lost on that ESC
// 2. Too low current draw compared to commanded thrust
// Overvoltage, overcurrent do not have checks yet esc_report.failures are handled separately
const hrt_abstime now = hrt_absolute_time();
// Only check while armed
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);
// Check individual ESC reports
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
// Map the esc status index to the actuator function index
const uint8_t actuator_function_index =
esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
continue; // Invalid mapping
}
const bool timeout = now > esc_status.esc[i].timestamp + 300_ms;
const float current = esc_status.esc[i].esc_current;
// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
if (current > FLT_EPSILON) {
_esc_has_reported_current[i] = true;
}
if (!_esc_has_reported_current[i]) {
continue;
}
_motor_failure_mask &= ~(1u << actuator_function_index); // Reset bit in mask to accumulate failures again
_motor_failure_mask |= (static_cast<uint16_t>(timeout) << actuator_function_index); // Telemetry timeout
// Current limits
float thrust = 0.f;
if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
thrust = fabsf(actuator_motors.control[actuator_function_index]);
}
bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get();
bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get();
bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get();
_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
if (!_esc_undercurrent_hysteresis[i].get_state()) {
// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
_esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low && !timeout, now);
}
if (!_esc_overcurrent_hysteresis[i].get_state()) {
// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high && !timeout, now);
}
_motor_failure_mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
_motor_failure_mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
}
_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);
} else { // Disarmed
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
}
_failure_detector_status.flags.motor = false;
}
}
@@ -53,8 +53,6 @@
// subscriptions
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/sensor_selection.h>
@@ -70,10 +68,8 @@ union failure_detector_status_u {
uint16_t pitch : 1;
uint16_t alt : 1;
uint16_t ext : 1;
uint16_t arm_escs : 1;
uint16_t battery : 1;
uint16_t imbalanced_prop : 1;
uint16_t motor : 1;
} flags;
uint16_t value {0};
};
@@ -89,13 +85,11 @@ public:
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
const failure_detector_status_u &getStatus() const { return _failure_detector_status; }
void publishStatus();
void publishStatus(bool esc_arm_status, uint16_t motor_failure_mask);
private:
void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateImbalancedPropStatus();
failure_detector_status_u _failure_detector_status{};
@@ -103,25 +97,17 @@ private:
systemlib::Hysteresis _roll_failure_hysteresis{false};
systemlib::Hysteresis _pitch_failure_hysteresis{false};
systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
systemlib::Hysteresis _esc_failure_hysteresis{false};
static constexpr float _imbalanced_prop_lpf_time_constant{5.f};
AlphaFilter<float> _imbalanced_prop_lpf{};
uint32_t _selected_accel_device_id{0};
hrt_abstime _imu_status_timestamp_prev{0};
// Motor failure check
bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if ESC reported non-zero current before (some never report any)
systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
uint16_t _motor_failure_mask = 0; // actuator function indexed
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
@@ -134,15 +120,6 @@ private:
(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
// Actuator failure
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_act_mot_thr,
(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_act_mot_c2t,
(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_act_mot_tout,
(ParamFloat<px4::params::FD_ACT_LOW_OFF>) _param_fd_act_low_off,
(ParamFloat<px4::params::FD_ACT_HIGH_OFF>) _param_fd_act_high_off
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
)
};
@@ -39,9 +39,6 @@
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*/
#include <px4_platform_common/px4_config.h>
#include <parameters/param.h>
/**
* FailureDetector Max Roll
*
@@ -130,18 +127,6 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
*/
PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
/**
* Enable checks on ESCs that report their arming state.
*
* If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
* Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
*
* @boolean
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
/**
* Imbalanced propeller check threshold
*
@@ -156,89 +141,3 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
/**
* Enable Actuator Failure check
*
* If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
* level is being consumed.
* Otherwise this indicates an motor failure.
*
* @boolean
* @reboot_required true
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ACT_EN, 0);
/**
* Motor Failure Thrust Threshold
*
* Failure detection per motor only triggers above this thrust value.
* Set to 1 to disable the detection.
*
* @group Failure Detector
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
/**
* Motor Failure Current/Throttle Scale
*
* Determines the slope between expected steady state current and linearized, normalized thrust command.
* E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
* FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
*
* @group Failure Detector
* @min 0.0
* @max 50.0
* @unit A/%
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 35.f);
/**
* Motor Failure Hysteresis Time
*
* Motor failure only triggers after current thresholds are exceeded for this time.
*
* @group Failure Detector
* @unit ms
* @min 10
* @max 10000
* @increment 100
*/
PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 1000);
/**
* Undercurrent motor failure limit offset
*
* threshold = FD_ACT_MOT_C2T * thrust - FD_ACT_LOW_OFF
*
* @group Failure Detector
* @min 0
* @max 30
* @unit A
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(FD_ACT_LOW_OFF, 10.f);
/**
* Overcurrent motor failure limit offset
*
* threshold = FD_ACT_MOT_C2T * thrust + FD_ACT_HIGH_OFF
*
* @group Failure Detector
* @min 0
* @max 30
* @unit A
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(FD_ACT_HIGH_OFF, 10.f);
@@ -1,6 +1,5 @@
matplotlib==3.5.1
numpy==1.22.2
numpy==1.21.5
numpy_quaternion==2022.4.3
pyulog==0.9.0
scipy==1.8.0
+4 -2
View File
@@ -92,8 +92,10 @@ parameters:
by this parameter. The range sensor and vision options should only be used
when for operation over a flat surface as the local NED origin will move
up and down with ground level.
If GPS is set as reference but altitude fusion is disabled in EKF2_GPS_CTRL,
the GPS altitude is still used to initiaize the bias of the other height sensors.
If GPS is set as reference and EKF2_GPS_CTRL is not 0, the GPS altitude is
still used to initiaize the bias of the other height sensors, regardless of
the altitude fusion bit in EKF2_GPS_CTRL.
type: enum
values:
0: Barometric pressure
+2 -2
View File
@@ -102,8 +102,8 @@ private:
_interface[i].esc_online_flags = 0;
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) &&
((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12);
bool is_motor = math::isInRange(esc.esc[j].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX);
if (is_motor) {
// Map OutputFunction number to index
+2 -2
View File
@@ -84,8 +84,8 @@ private:
if (_esc_status_subs[i].update(&esc)) {
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) {
bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) &&
((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12);
const bool is_motor = math::isInRange(esc.esc[j].actuator_function,
esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX);
if (is_motor) {
// Map OutputFunction number to index
@@ -118,6 +118,7 @@ else()
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py
--client-outdir ${CMAKE_CURRENT_BINARY_DIR}
--dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml
--whitelist-file ${CMAKE_CURRENT_SOURCE_DIR}/safety_whitelist.yaml
--template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em
DEPENDS
${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py

Some files were not shown because too many files have changed in this diff Show More