mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-17 06:51:30 +08:00
Compare commits
No commits in common. "v1.7.2" and "v1.7.0" have entirely different histories.
@ -413,27 +413,6 @@ if (all_posix_cmake_targets)
|
||||
)
|
||||
endif()
|
||||
|
||||
|
||||
#=============================================================================
|
||||
# uORB graph generation: add a custom target 'uorb_graph'
|
||||
#
|
||||
set(uorb_graph_config ${BOARD})
|
||||
|
||||
set(graph_module_list "")
|
||||
foreach(module ${config_module_list})
|
||||
set(graph_module_list "${graph_module_list}" "--src-path" "src/${module}")
|
||||
endforeach()
|
||||
|
||||
add_custom_command(OUTPUT ${uorb_graph_config}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py
|
||||
${module_list}
|
||||
--exclude-path src/examples
|
||||
--file ${PX4_SOURCE_DIR}/Tools/uorb_graph/graph_${uorb_graph_config}
|
||||
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
|
||||
COMMENT "Generating uORB graph"
|
||||
)
|
||||
add_custom_target(uorb_graph DEPENDS ${uorb_graph_config})
|
||||
|
||||
#=============================================================================
|
||||
# packaging
|
||||
#
|
||||
|
||||
BIN
Documentation/arming_state_machine.odg
Normal file
BIN
Documentation/arming_state_machine.odg
Normal file
Binary file not shown.
BIN
Documentation/arming_state_machine.pdf
Normal file
BIN
Documentation/arming_state_machine.pdf
Normal file
Binary file not shown.
BIN
Documentation/code_structure_diagrams.odg
Normal file
BIN
Documentation/code_structure_diagrams.odg
Normal file
Binary file not shown.
BIN
Documentation/code_structure_diagrams.pdf
Normal file
BIN
Documentation/code_structure_diagrams.pdf
Normal file
Binary file not shown.
18434
Documentation/control_flow.graffle
Normal file
18434
Documentation/control_flow.graffle
Normal file
File diff suppressed because it is too large
Load Diff
BIN
Documentation/dsm_bind.odt
Normal file
BIN
Documentation/dsm_bind.odt
Normal file
Binary file not shown.
BIN
Documentation/dsm_bind.pdf
Normal file
BIN
Documentation/dsm_bind.pdf
Normal file
Binary file not shown.
BIN
Documentation/fixed_wing_control.odg
Normal file
BIN
Documentation/fixed_wing_control.odg
Normal file
Binary file not shown.
BIN
Documentation/flight_mode_state_machine.odg
Normal file
BIN
Documentation/flight_mode_state_machine.odg
Normal file
Binary file not shown.
BIN
Documentation/flight_mode_state_machine.pdf
Normal file
BIN
Documentation/flight_mode_state_machine.pdf
Normal file
Binary file not shown.
BIN
Documentation/fw_landing.png
Normal file
BIN
Documentation/fw_landing.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 17 KiB |
4398
Documentation/mixing_architecture.graffle
Normal file
4398
Documentation/mixing_architecture.graffle
Normal file
File diff suppressed because it is too large
Load Diff
BIN
Documentation/px4_block_diagram.odg
Normal file
BIN
Documentation/px4_block_diagram.odg
Normal file
Binary file not shown.
BIN
Documentation/px4_block_diagram.pdf
Normal file
BIN
Documentation/px4_block_diagram.pdf
Normal file
Binary file not shown.
BIN
Documentation/px4_general_structure.png
Normal file
BIN
Documentation/px4_general_structure.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
BIN
Documentation/px4_hil/SITL_Diagram.png
Normal file
BIN
Documentation/px4_hil/SITL_Diagram.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 79 KiB |
BIN
Documentation/px4_hil/SITL_Diagram_QGC.png
Normal file
BIN
Documentation/px4_hil/SITL_Diagram_QGC.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 74 KiB |
98
Documentation/px4_hil/UserGuide.md
Normal file
98
Documentation/px4_hil/UserGuide.md
Normal file
@ -0,0 +1,98 @@
|
||||
[TOC]
|
||||
|
||||
# Introduction
|
||||
|
||||
The HIL architecture allows you to test the flight stack replacing the real physical vehicle and sensors with a simulator of vehicle dynamics and sensor outputs. The flight stack "is not aware" that it is not on a real vehicle. This is a powerful tool for develping and testing code rapidly in a benchtop environment.
|
||||
|
||||
The flight stack can be run anywhere that supports a network connection to the simulator (with sufficient bandwidth and latency to transport the sensor and actuator messages). This can be on a standard linux workstation, an on-target linux image, or the on-target DSP image. These modes can be selected based on the goals of the testing. Workstation is useful for rapid testing in a tool-rich environment. DSP image testing is the closest to the final implementation, so is useful for testing actual HW operation, other than the physical sensing and actuation.
|
||||
|
||||
## Px4 High-level HIL Architecture
|
||||
|
||||
A diagram of the setup described is shown here. Note that UDP port numbers are only displayed on the socket server and are left blank on the socket client.
|
||||
|
||||
(???NOTES: This diagram needs to be updated to use control inputs over UDP, either from QGC or from other)
|
||||
|
||||
|
||||
|
||||
## Requirements
|
||||
The simulator that is currently supported is jMAVSim. The setup described here requires PX4 and jMAVSim installed and running. qGroundControl (QGC) is also required because it is the supported method of providing manual control commands.
|
||||
|
||||
## Assumptions
|
||||
|
||||
# Compiling Code
|
||||
|
||||
## JMAVSim
|
||||
|
||||
### Platform Requirements
|
||||
Linux with java-1.7.x or greater
|
||||
|
||||
### Build Instructions
|
||||
In a clean directory
|
||||
```
|
||||
> git clone https://github.com/PX4/jMAVSim.git
|
||||
> cd jMAVSim
|
||||
> git submodule init
|
||||
> git submodule update
|
||||
> ant
|
||||
```
|
||||
|
||||
## qGroundControl
|
||||
|
||||
### Platform Requirements
|
||||
Windows 7
|
||||
Logitech Gamepad F310 joystick controller
|
||||
|
||||
### Download/Install Instructions
|
||||
Download QGC from http://qgroundcontrol.org/downloads and install using the windows executable.
|
||||
|
||||
|
||||
## PX4
|
||||
|
||||
### Platform Requirements
|
||||
Linux or Eagle with a working IP interface (?? does this need further instructions?)
|
||||
|
||||
### Build Host Requirements
|
||||
(???Notes: Windows?)
|
||||
|
||||
### Download & Build Instructions
|
||||
|
||||
### Installing binaries on the Qualcomm Target
|
||||
|
||||
# Running PX4 in HIL Mode
|
||||
|
||||
## Starting PX4 on Qualcomm Eagle
|
||||
|
||||
```
|
||||
> adb shell
|
||||
# bash
|
||||
root@linaro-developer:/# cd ???
|
||||
root@linaro-developer:/# ./px4
|
||||
App name: px4
|
||||
Enter a command and its args:
|
||||
uorb start
|
||||
muorb start
|
||||
mavlink start -u 14556
|
||||
simulator start -p
|
||||
```
|
||||
|
||||
## Starting jMAVSim
|
||||
In the directory where jMAVSim is installed
|
||||
```
|
||||
java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp <IPADDR>:14560 -n 100
|
||||
```
|
||||
replacing <IPADDR> with the IP address of the machine running PX4 (Eagle). This can be found by running "ifconfig" on that machine.
|
||||
|
||||
## Starting qGroundControl
|
||||
Launch the qGroundControl application
|
||||
1. Set up the communication to the flight stack. In the menu File:Settings:CommLinks, select Add. Enter a Link Name of your choice. Select Link Type: UDP. Set the listening port to an unused port (example: 14561). Select Add. Enter the IP address and port of the PX4 Mavlink app, which is <IPADDR>:14556 with <IPADDR> being the IP address of the Eagle board. Select OK.
|
||||
1. Set up the joystick. Plug in the joystick to your Windows machine. In the menu File:Settings:CommLinks, check Enable Controllers. Select "Gamepad F310". Select "Manual". Set the axes/channel mapping to 0:Yaw, 1:Throttle, 2:unset, 3:Pitch, 4:Roll. Seletct "Inverted" for the throttle axis. Click "Calibrate range". Move the right joystick through its full range of motion. Move the left joystick full left then full right. Move the left joystick full forward (but not full backward). Click "end calibration."
|
||||
1. Connect to the flight stack. Click Analyze. Click the "Connect" button in the upper right, and select the connection that you created in the first step.
|
||||
|
||||
You should now be connected to the flight stack. You can see incoming Mavlink packets using the MAVLink Instpector (from Advanced:Tool Widgets)
|
||||
|
||||
|
||||
## Controlling PX4 flight in HIL Mode
|
||||
The joystick can now be used to fly the simulated vehicle. The jMAVSim world visualization gives a FPV view, and QGC can be used to display instruments such as artificial horizon and maps (if GPS simulation is enabled).
|
||||
|
||||
|
||||
# Debugging/FAQ
|
||||
0
Documentation/px4_hil/docs/readme.txt
Normal file
0
Documentation/px4_hil/docs/readme.txt
Normal file
2403
Documentation/px4_hil/px4_hil.doxyfile
Normal file
2403
Documentation/px4_hil/px4_hil.doxyfile
Normal file
File diff suppressed because it is too large
Load Diff
BIN
Documentation/rc_mode_switch.odg
Normal file
BIN
Documentation/rc_mode_switch.odg
Normal file
Binary file not shown.
BIN
Documentation/rc_mode_switch.pdf
Normal file
BIN
Documentation/rc_mode_switch.pdf
Normal file
Binary file not shown.
2
Documentation/versionfilter.sh
Executable file
2
Documentation/versionfilter.sh
Executable file
@ -0,0 +1,2 @@
|
||||
#!/bin/sh
|
||||
git log --pretty=format:"Last change: commit %h - %aN, %ar : %s" -1 $1 || echo no git
|
||||
145
Jenkinsfile
vendored
145
Jenkinsfile
vendored
@ -1,6 +1,17 @@
|
||||
pipeline {
|
||||
agent none
|
||||
stages {
|
||||
stage('Quality Checks') {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-base:2017-10-23'
|
||||
args '-e CI=true'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
sh 'make check_format'
|
||||
}
|
||||
}
|
||||
|
||||
stage('Build') {
|
||||
steps {
|
||||
@ -44,7 +55,7 @@ pipeline {
|
||||
sh "make clean"
|
||||
sh "ccache -z"
|
||||
sh "git fetch --tags"
|
||||
sh "make px4io-v2_default"
|
||||
sh "make nuttx_px4io-v2_default"
|
||||
sh "make nuttx_px4fmu-v2_default"
|
||||
sh "make nuttx_px4fmu-v2_lpe"
|
||||
sh "make nuttx_px4fmu-v3_default"
|
||||
@ -107,28 +118,6 @@ pipeline {
|
||||
}
|
||||
|
||||
|
||||
// posix_sitl
|
||||
for (def option in ["sitl_default", "sitl_rtps"]) {
|
||||
def node_name = "${option}"
|
||||
|
||||
builds["${node_name}"] = {
|
||||
node {
|
||||
stage("Build Test ${node_name}") {
|
||||
docker.image('px4io/px4-dev-base:2017-10-23').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
|
||||
stage("${node_name}") {
|
||||
checkout scm
|
||||
sh "make clean"
|
||||
sh "ccache -z"
|
||||
sh "make posix_${node_name}"
|
||||
sh "ccache -s"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// raspberry pi and bebop (armhf)
|
||||
for (def option in ["rpi_cross", "bebop_default"]) {
|
||||
def node_name = "${option}"
|
||||
@ -197,8 +186,8 @@ pipeline {
|
||||
}
|
||||
|
||||
|
||||
// GCC7 posix
|
||||
for (def option in ["sitl_default"]) {
|
||||
// GCC7 tests
|
||||
for (def option in ["posix_sitl_default", "nuttx_px4fmu-v5_default"]) {
|
||||
def node_name = "${option}"
|
||||
|
||||
builds["${node_name} (GCC7)"] = {
|
||||
@ -209,28 +198,7 @@ pipeline {
|
||||
checkout scm
|
||||
sh "make clean"
|
||||
sh "ccache -z"
|
||||
sh "make posix_${node_name}"
|
||||
sh "ccache -s"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// GCC7 nuttx
|
||||
for (def option in ["px4fmu-v5_default"]) {
|
||||
def node_name = "${option}"
|
||||
|
||||
builds["${node_name} (GCC7)"] = {
|
||||
node {
|
||||
stage("Build Test ${node_name} (GCC7)") {
|
||||
docker.image('px4io/px4-dev-base-archlinux:2017-12-08').inside('-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw') {
|
||||
stage("${node_name}") {
|
||||
checkout scm
|
||||
sh "make clean"
|
||||
sh "ccache -z"
|
||||
sh "make nuttx_${node_name}"
|
||||
sh "make ${node_name}"
|
||||
sh "ccache -s"
|
||||
}
|
||||
}
|
||||
@ -247,76 +215,19 @@ pipeline {
|
||||
stage('Test') {
|
||||
parallel {
|
||||
|
||||
stage('check style') {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-base:2017-10-23'
|
||||
args '-e CI=true'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
sh 'make check_format'
|
||||
}
|
||||
}
|
||||
|
||||
stage('clang analyzer') {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-clang:2017-10-23'
|
||||
args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
sh 'make clean'
|
||||
sh 'make scan-build'
|
||||
// publish html
|
||||
publishHTML target: [
|
||||
reportTitles: 'clang static analyzer',
|
||||
allowMissing: false,
|
||||
alwaysLinkToLastBuild: true,
|
||||
keepAll: true,
|
||||
reportDir: 'build/scan-build/report_latest',
|
||||
reportFiles: '*',
|
||||
reportName: 'Clang Static Analyzer'
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
stage('clang tidy') {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-clang:2017-10-23'
|
||||
args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
sh 'make clean'
|
||||
sh 'make clang-tidy-quiet'
|
||||
}
|
||||
}
|
||||
|
||||
stage('cppcheck') {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-base:ubuntu17.10'
|
||||
args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
sh 'make clean'
|
||||
sh 'make cppcheck'
|
||||
// publish html
|
||||
publishHTML target: [
|
||||
reportTitles: 'Cppcheck',
|
||||
allowMissing: false,
|
||||
alwaysLinkToLastBuild: true,
|
||||
keepAll: true,
|
||||
reportDir: 'build/cppcheck/',
|
||||
reportFiles: '*',
|
||||
reportName: 'Cppcheck'
|
||||
]
|
||||
}
|
||||
}
|
||||
// temporarily disabled until build resources are available
|
||||
//stage('clang-tidy') {
|
||||
// agent {
|
||||
// docker {
|
||||
// image 'px4io/px4-dev-clang:2017-10-23'
|
||||
// args '-e CI=true -e CCACHE_BASEDIR=$WORKSPACE -e CCACHE_DIR=/tmp/ccache -v /tmp/ccache:/tmp/ccache:rw'
|
||||
// }
|
||||
// }
|
||||
// steps {
|
||||
// sh 'make clean'
|
||||
// sh 'make clang-tidy-quiet'
|
||||
// }
|
||||
//}
|
||||
|
||||
stage('tests') {
|
||||
agent {
|
||||
|
||||
44
Makefile
44
Makefile
@ -218,7 +218,7 @@ check_rtps: \
|
||||
check_posix_sitl_rtps \
|
||||
sizes
|
||||
|
||||
.PHONY: sizes check quick_check check_rtps uorb_graphs
|
||||
.PHONY: sizes check quick_check check_rtps
|
||||
|
||||
sizes:
|
||||
@-find build -name *.elf -type f | xargs size 2> /dev/null || :
|
||||
@ -235,14 +235,6 @@ check_%:
|
||||
@$(MAKE) --no-print-directory $(subst check_,,$@)
|
||||
@echo
|
||||
|
||||
uorb_graphs:
|
||||
@./Tools/uorb_graph/create_from_startupscript.sh
|
||||
@./Tools/uorb_graph/create.py --src-path src --exclude-path src/examples --file Tools/uorb_graph/graph_full
|
||||
@$(MAKE) --no-print-directory px4fmu-v2_default uorb_graph
|
||||
@$(MAKE) --no-print-directory px4fmu-v4_default uorb_graph
|
||||
@$(MAKE) --no-print-directory posix_sitl_default uorb_graph
|
||||
|
||||
|
||||
.PHONY: coverity_scan
|
||||
|
||||
coverity_scan: posix_sitl_default
|
||||
@ -264,6 +256,37 @@ module_documentation:
|
||||
|
||||
px4_metadata: parameters_metadata airframe_metadata module_documentation
|
||||
|
||||
# S3 upload helpers
|
||||
# --------------------------------------------------------------------
|
||||
# s3cmd uses these ENV variables
|
||||
# AWS_ACCESS_KEY_ID
|
||||
# AWS_SECRET_ACCESS_KEY
|
||||
# AWS_S3_BUCKET
|
||||
.PHONY: s3put_firmware s3put_qgc_firmware s3put_px4fmu_firmware s3put_misc_qgc_extra_firmware s3put_metadata s3put_scan-build s3put_cppcheck s3put_coverage
|
||||
|
||||
s3put_qgc_firmware: s3put_px4fmu_firmware s3put_misc_qgc_extra_firmware
|
||||
|
||||
s3put_px4fmu_firmware: px4fmu_firmware
|
||||
@find $(SRC_DIR)/build -name "*.px4" -exec $(SRC_DIR)/Tools/s3put.sh "{}" \;
|
||||
|
||||
s3put_misc_qgc_extra_firmware: misc_qgc_extra_firmware
|
||||
@find $(SRC_DIR)/build -name "*.px4" -exec $(SRC_DIR)/Tools/s3put.sh "{}" \;
|
||||
|
||||
s3put_metadata: px4_metadata
|
||||
@$(SRC_DIR)/Tools/s3put.sh airframes.md
|
||||
@$(SRC_DIR)/Tools/s3put.sh airframes.xml
|
||||
@$(SRC_DIR)/Tools/s3put.sh parameters.xml
|
||||
@$(SRC_DIR)/Tools/s3put.sh parameters.md
|
||||
|
||||
s3put_scan-build: scan-build
|
||||
@$(SRC_DIR)/Tools/s3put.sh `find $(SRC_DIR)/build/scan-build -mindepth 1 -maxdepth 1 -type d`/
|
||||
|
||||
s3put_cppcheck: cppcheck
|
||||
@$(SRC_DIR)/Tools/s3put.sh $(SRC_DIR)/build/cppcheck/
|
||||
|
||||
s3put_coverage: tests_coverage
|
||||
@$(SRC_DIR)/Tools/s3put.sh $(SRC_DIR)/build/posix_sitl_default/coverage-html/
|
||||
|
||||
# Astyle
|
||||
# --------------------------------------------------------------------
|
||||
.PHONY: check_format format
|
||||
@ -297,12 +320,9 @@ tests_coverage:
|
||||
scan-build:
|
||||
@export CCC_CC=clang
|
||||
@export CCC_CXX=clang++
|
||||
@rm -rf $(SRC_DIR)/build/posix_sitl_default-scan-build
|
||||
@rm -rf $(SRC_DIR)/build/scan-build/report_latest
|
||||
@mkdir -p $(SRC_DIR)/build/posix_sitl_default-scan-build
|
||||
@cd $(SRC_DIR)/build/posix_sitl_default-scan-build && scan-build cmake $(SRC_DIR) -GNinja -DCONFIG=posix_sitl_default
|
||||
@scan-build -o $(SRC_DIR)/build/scan-build cmake --build $(SRC_DIR)/build/posix_sitl_default-scan-build
|
||||
@find $(SRC_DIR)/build/scan-build -maxdepth 1 -mindepth 1 -type d -exec cp -r "{}" $(SRC_DIR)/build/scan-build/report_latest \;
|
||||
|
||||
posix_sitl_default-clang:
|
||||
@mkdir -p $(SRC_DIR)/build/posix_sitl_default-clang
|
||||
|
||||
76
README.md
76
README.md
@ -1,42 +1,34 @@
|
||||
# PX4 Pro Drone Autopilot
|
||||
## PX4 Pro Drone Autopilot ##
|
||||
|
||||
[](https://github.com/PX4/Firmware/releases) [](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware)
|
||||
|
||||
[](http://ci.px4.io:8080/blue/organizations/jenkins/Firmware/activity) [](https://scan.coverity.com/projects/3966?tab=overview)
|
||||
[](https://github.com/PX4/Firmware/releases) [](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware) [](https://travis-ci.org/PX4/Firmware) [](https://scan.coverity.com/projects/3966?tab=overview)
|
||||
|
||||
[](http://slack.px4.io)
|
||||
|
||||
This repository holds the [PX4 Pro](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/Firmware/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.
|
||||
|
||||
* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/Firmware/blob/master/LICENSE))
|
||||
* [Supported airframes](https://docs.px4.io/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
|
||||
* [Multicopters](https://docs.px4.io/en/airframes/airframe_reference.html#copter)
|
||||
* [Fixed wing](https://docs.px4.io/en/airframes/airframe_reference.html#plane)
|
||||
* [VTOL](https://docs.px4.io/en/airframes/airframe_reference.html#vtol)
|
||||
* many more experimental types (Rovers, Blimps, Boats, Submarines, etc)
|
||||
* Releases: [Downloads](https://github.com/PX4/Firmware/releases)
|
||||
|
||||
|
||||
## PX4 Users
|
||||
|
||||
The [PX4 User Guide](https://docs.px4.io/en/) explains how to assemble [supported vehicles](https://docs.px4.io/en/airframes/airframe_reference.html) and fly drones with PX4.
|
||||
See the [forum and chat](https://docs.px4.io/en/#support) if you need help!
|
||||
|
||||
|
||||
## PX4 Developers
|
||||
|
||||
This [Developer Guide](https://dev.px4.io/) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, any anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle.
|
||||
|
||||
Developers should read the [Guide for Contributions](https://dev.px4.io/en/contribute/).
|
||||
See the [forum and chat](https://dev.px4.io/en/#support) if you need help!
|
||||
* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md))
|
||||
* Supported airframes:
|
||||
* [Multicopters](http://px4.io/portfolio_category/multicopter/)
|
||||
* [Fixed wing](http://px4.io/portfolio_category/plane/)
|
||||
* [VTOL](http://px4.io/portfolio_category/vtol/)
|
||||
* many more experimental types (Rovers, Blimps, Boats, Submarines, etc)
|
||||
* Releases: [Downloads](https://github.com/PX4/Firmware/releases)
|
||||
|
||||
Please refer to the [user documentation](https://docs.px4.io/en/) and [user forum](http://discuss.px4.io) for flying drones with the PX4 flight stack.
|
||||
|
||||
### Weekly Dev Call
|
||||
|
||||
The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribute/#dev_call).
|
||||
The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribute/).
|
||||
|
||||
> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform.
|
||||
* [Wednesday 17:00 Central European Time, 11:00 Eastern Time, 08:00 Pacific Standard Time](https://www.google.com/calendar/embed?src=bGludXhmb3VuZGF0aW9uLm9yZ19nMjF0dmFtMjRtN3BtN2poZXYwMWJ2bHFoOEBncm91cC5jYWxlbmRhci5nb29nbGUuY29t)
|
||||
* [Uber conference (dial-in or web client)](https://www.uberconference.com/lf-dronecode)
|
||||
* The agenda is announced in advance on the [PX4 Discuss](http://discuss.px4.io/c/weekly-dev-call)
|
||||
* Issues and PRs may be labelled [devcall](https://github.com/PX4/Firmware/issues?q=is%3Aopen+is%3Aissue+label%3Adevcall) to flag them for discussion
|
||||
|
||||
### Developers ###
|
||||
* [Developer Guide](https://dev.px4.io/)
|
||||
* [Build instructions](https://dev.px4.io/en/setup/building_px4.html)
|
||||
* [Guide for Contributions](https://dev.px4.io/en/contribute/)
|
||||
|
||||
## Maintenance Team
|
||||
|
||||
@ -79,32 +71,28 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribut
|
||||
* [Airmind MindPX / MindRacer](https://github.com/PX4/Firmware/labels/mindpx) - [Henry Zhang](https://github.com/iZhangHui)
|
||||
* RTPS/ROS2 Interface - [Vicente Monge](https://github.com/vicenteeprosima)
|
||||
|
||||
See also [About Us](http://px4.io/about-us/#development_team) (px4.io) and the [contributors list](https://github.com/PX4/Firmware/graphs/contributors) (Github).
|
||||
|
||||
## Supported Hardware
|
||||
|
||||
This repository contains code supporting these boards:
|
||||
* [Snapdragon Flight](https://docs.px4.io/en/flight_controller/snapdragon_flight.html)
|
||||
* [Intel Aero](https://docs.px4.io/en/flight_controller/intel_aero.html)
|
||||
* [Raspberry PI with Navio 2](https://docs.px4.io/en/flight_controller/raspberry_pi_navio2.html)
|
||||
* [Snapdragon Flight](https://dev.px4.io/en/flight_controller/snapdragon_flight.html)
|
||||
* [Intel Aero](https://dev.px4.io/en/flight_controller/intel_aero.html)
|
||||
* [Raspberry PI with Navio 2](https://dev.px4.io/en/flight_controller/raspberry_pi.html)
|
||||
* [Parrot Bebop 2](https://dev.px4.io/en/advanced/parrot_bebop.html)
|
||||
* FMUv2.x
|
||||
* [Pixhawk](https://docs.px4.io/en/flight_controller/pixhawk.html)
|
||||
* [Pixhawk Mini](https://docs.px4.io/en/flight_controller/pixhawk_mini.html)
|
||||
* [Pixfalcon](https://docs.px4.io/en/flight_controller/pixfalcon.html)
|
||||
* [Pixhawk](https://dev.px4.io/en/flight_controller/pixhawk.html)
|
||||
* Pixhawk Mini
|
||||
* [Pixfalcon](https://dev.px4.io/en/flight_controller/pixfalcon.html)
|
||||
* FMUv3.x [Pixhawk 2](https://pixhawk.org/modules/pixhawk2)
|
||||
* FMUv4.x
|
||||
* [Pixracer](https://docs.px4.io/en/flight_controller/pixracer.html)
|
||||
* [Pixhawk 3 Pro](https://docs.px4.io/en/flight_controller/pixhawk3_pro.html)
|
||||
* [Pixracer](https://dev.px4.io/en/flight_controller/pixracer.html)
|
||||
* Pixhawk 3 Pro
|
||||
* FMUv5.x (ARM Cortex M7, future Pixhawk)
|
||||
* [STM32F4Discovery](http://www.st.com/en/evaluation-tools/stm32f4discovery.html) (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
|
||||
* [Gumstix AeroCore](https://www.gumstix.com/aerocore-2/) (only v2)
|
||||
* STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
|
||||
* Gumstix AeroCore (v1 and v2)
|
||||
* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
|
||||
* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
|
||||
* [Bitcraze Crazyflie 2.0](https://docs.px4.io/en/flight_controller/crazyflie2.html)
|
||||
* [Bitcraze Crazyflie 2.0](https://dev.px4.io/en/flight_controller/crazyflie2.html)
|
||||
|
||||
Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/en/flight_controller/).
|
||||
## Project Milestones
|
||||
|
||||
## Project Roadmap
|
||||
|
||||
A high level project roadmap is available [here](https://www.dronecode.org/roadmap/).
|
||||
The PX4 software and Pixhawk hardware (which has been designed for it) has been created in 2011 by [Lorenz Meier](https://github.com/LorenzMeier).
|
||||
|
||||
@ -4,17 +4,20 @@ set VEHICLE_TYPE vtol
|
||||
|
||||
if [ $AUTOCNF == yes ]
|
||||
then
|
||||
param set MIS_TAKEOFF_ALT 20
|
||||
param set MIS_YAW_TMT 10
|
||||
|
||||
param set MPC_ACC_HOR_MAX 2.0
|
||||
param set MPC_LAND_SPEED 0.7
|
||||
param set MPC_TKO_SPEED 1.0
|
||||
param set MPC_XY_VEL_MAX 4.0
|
||||
param set MPC_Z_VEL_MAX_DN 1.5
|
||||
param set MC_ROLL_P 6.0
|
||||
param set MC_PITCH_P 6.0
|
||||
param set MC_YAW_P 4
|
||||
|
||||
#
|
||||
# Default parameters for mission and position handling
|
||||
#
|
||||
param set NAV_ACC_RAD 3
|
||||
|
||||
param set MPC_TKO_SPEED 1.0
|
||||
param set MPC_LAND_SPEED 0.7
|
||||
param set MPC_Z_VEL_MAX_DN 1.5
|
||||
param set MPC_XY_VEL_MAX 4.0
|
||||
param set MIS_YAW_TMT 10
|
||||
param set MPC_ACC_HOR_MAX 2.0
|
||||
param set RTL_LAND_DELAY 0
|
||||
fi
|
||||
|
||||
|
||||
@ -3,17 +3,8 @@
|
||||
function check_git_submodule {
|
||||
|
||||
# The .git exists in a submodule if init and update have been done.
|
||||
if [[ -f $1"/.git" || -d $1"/.git" ]];
|
||||
if [ "$CI" != "true" ] && [[ -f $1"/.git" || -d $1"/.git" ]];
|
||||
then
|
||||
|
||||
if [ "$CI" == "true" ];
|
||||
then
|
||||
git submodule sync --recursive -- $1
|
||||
git submodule update --init --recursive --force -- $1 || true
|
||||
git submodule update --init --recursive --force -- $1
|
||||
exit 0
|
||||
fi
|
||||
|
||||
SUBMODULE_STATUS=$(git submodule summary "$1")
|
||||
STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1")
|
||||
if ! [[ -z "$STATUSRETVAL" ]];
|
||||
@ -44,8 +35,8 @@ then
|
||||
elif [ "$user_cmd" == "u" ]
|
||||
then
|
||||
git submodule sync --recursive -- $1
|
||||
git submodule update --init --recursive -- $1 || true
|
||||
git submodule update --init --recursive --force -- $1
|
||||
git submodule update --init --recursive --force --quiet -- $1 || true
|
||||
git submodule update --init --recursive -- $1
|
||||
echo "Submodule fixed, continuing build.."
|
||||
else
|
||||
echo "Build aborted."
|
||||
@ -53,9 +44,9 @@ then
|
||||
fi
|
||||
fi
|
||||
else
|
||||
git submodule sync --recursive --quiet -- $1
|
||||
git submodule update --init --recursive -- $1 || true
|
||||
git submodule update --init --recursive -- $1
|
||||
git submodule sync --recursive -- $1
|
||||
git submodule update --init --recursive --force -- $1 || true
|
||||
git submodule update --init --recursive --force -- $1
|
||||
fi
|
||||
|
||||
}
|
||||
|
||||
23
Tools/s3put.sh
Executable file
23
Tools/s3put.sh
Executable file
@ -0,0 +1,23 @@
|
||||
#!/bin/bash
|
||||
|
||||
filename=${1}
|
||||
|
||||
# Requires these ENV variables
|
||||
# AWS_ACCESS_KEY_ID
|
||||
# AWS_SECRET_ACCESS_KEY
|
||||
# AWS_S3_BUCKET
|
||||
|
||||
[ -z "$AWS_ACCESS_KEY_ID" ] && { echo "ERROR: Need to set AWS_ACCESS_KEY_ID"; exit 1; }
|
||||
[ -z "$AWS_SECRET_ACCESS_KEY" ] && { echo "ERROR: Need to set AWS_SECRET_ACCESS_KEY"; exit 1; }
|
||||
[ -z "$AWS_S3_BUCKET" ] && { echo "ERROR: Need to set AWS_S3_BUCKET"; exit 1; }
|
||||
|
||||
if [ -f ${filename} ]; then
|
||||
base_file_name=`basename $filename`
|
||||
s3cmd --access_key=${AWS_ACCESS_KEY_ID} --secret_key=${AWS_SECRET_ACCESS_KEY} put ${filename} s3://${AWS_S3_BUCKET}/${base_file_name}
|
||||
elif [ -d ${filename} ]; then
|
||||
dir_name=$filename
|
||||
s3cmd --access_key=${AWS_ACCESS_KEY_ID} --secret_key=${AWS_SECRET_ACCESS_KEY} put -r ${dir_name} s3://${AWS_S3_BUCKET}/
|
||||
else
|
||||
echo "ERROR: ${file} doesn't exist"
|
||||
exit 1
|
||||
fi
|
||||
2
Tools/uorb_graph/.gitignore
vendored
2
Tools/uorb_graph/.gitignore
vendored
@ -1,2 +0,0 @@
|
||||
*.json
|
||||
|
||||
@ -1,641 +0,0 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import codecs
|
||||
import re
|
||||
import colorsys
|
||||
import json
|
||||
|
||||
|
||||
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Generate uORB pub/sub dependency graph from source code')
|
||||
|
||||
parser.add_argument('-s', '--src-path', action='append',
|
||||
help='Source path(s) (default=src, can be specified multiple times)',
|
||||
default=[])
|
||||
parser.add_argument('-e', '--exclude-path', action='append',
|
||||
help='Excluded path(s), can be specified multiple times',
|
||||
default=[])
|
||||
parser.add_argument('-f', '--file', metavar='file', action='store',
|
||||
help='output file name prefix',
|
||||
default='graph')
|
||||
parser.add_argument('-o', '--output', metavar='output', action='store',
|
||||
help='output format (json or graphviz)',
|
||||
default='json')
|
||||
parser.add_argument('--use-topic-union', action='store_true',
|
||||
help='''
|
||||
Use the union of all publication and subscription topics (useful for complete
|
||||
graphs or only few/single module(s)). The default is to use the intersection
|
||||
(remove topics that have no subscriber or no publisher)''')
|
||||
parser.add_argument('-m', '--modules', action='store',
|
||||
help='Comma-separated whitelist of modules (the module\'s '+
|
||||
'MAIN, e.g. from a startup script)',
|
||||
default='')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
|
||||
g_debug = False
|
||||
def dbg_print(string):
|
||||
if g_debug:
|
||||
print(string)
|
||||
|
||||
|
||||
def get_N_colors(N, s=0.8, v=0.9):
|
||||
""" get N distinct colors as a list of hex strings """
|
||||
HSV_tuples = [(x*1.0/N, s, v) for x in range(N)]
|
||||
hex_out = []
|
||||
for rgb in HSV_tuples:
|
||||
rgb = map(lambda x: int(x*255), colorsys.hsv_to_rgb(*rgb))
|
||||
hex_out.append("#"+"".join(map(lambda x: format(x, '02x'), rgb)))
|
||||
return hex_out
|
||||
|
||||
|
||||
class PubSub:
|
||||
""" Collects either publication or subscription information for nodes
|
||||
(modules and topics) & edges """
|
||||
|
||||
def __init__(self, is_publication, topic_blacklist, orb_pub_sub_regexes, special_cases):
|
||||
"""
|
||||
:param is_publication: if True, publications, False for
|
||||
subscriptions
|
||||
:param topic_blacklist: list of topics to blacklist
|
||||
:param orb_pub_sub_regexes: list of regexes to extract orb calls
|
||||
(e.g. orb_subscribe). They need to have 2 captures, the second
|
||||
one is the one capturing ORB_ID(<topic>
|
||||
"""
|
||||
self._module_pubsubs = {} # key = module name, value = set of topic names
|
||||
self._special_cases = special_cases
|
||||
self._special_cases_matched = None
|
||||
self._topic_blacklist = topic_blacklist
|
||||
self._orb_pub_sub_regexes = orb_pub_sub_regexes
|
||||
if is_publication:
|
||||
self._method = 'Publication'
|
||||
else:
|
||||
self._method = 'Subscription'
|
||||
|
||||
def reset(self):
|
||||
self._special_cases_matched = [False]*len(self._special_cases)
|
||||
|
||||
def filter_modules(self, module_whitelist):
|
||||
remove = [k for k in self._module_pubsubs if k not in module_whitelist]
|
||||
for k in remove: del self._module_pubsubs[k]
|
||||
|
||||
|
||||
def check_if_match_found(self, modules):
|
||||
""" check if all special cases got a match (if not, it means the source
|
||||
code got changed)
|
||||
"""
|
||||
for i, (module_match, file_match_re, src_match_re, _) in enumerate(self._special_cases):
|
||||
if module_match in modules and src_match_re is not None:
|
||||
if not self._special_cases_matched[i]:
|
||||
raise Exception('Module '+module_match+
|
||||
': no match for '+self._method+' special case'+
|
||||
src_match_re.pattern+'. The case needs to be updated')
|
||||
|
||||
|
||||
def extract(self, file_name, src_str, module, orb_id_vehicle_attitude_controls_topic):
|
||||
""" Extract subscribed/published topics from a source string
|
||||
:param src_str: string of C/C++ code with comments and whitespace removed
|
||||
"""
|
||||
|
||||
orb_pubsub_matches = []
|
||||
for regex in self._orb_pub_sub_regexes:
|
||||
orb_pubsub_matches += re.findall(regex, src_str)
|
||||
|
||||
orb_id = 'ORB_ID('
|
||||
|
||||
for _, match in orb_pubsub_matches:
|
||||
if match == 'ORB_ID_VEHICLE_ATTITUDE_CONTROLS': # special case
|
||||
match = orb_id+orb_id_vehicle_attitude_controls_topic
|
||||
|
||||
# match has the form: '[ORB_ID(]<topic_name>'
|
||||
if match.startswith(orb_id):
|
||||
topic_name = match[len(orb_id):]
|
||||
self._add_topic(topic_name, file_name, module)
|
||||
else:
|
||||
ignore_found = False
|
||||
for module_match, file_match_re, _, ignore_re in self._special_cases:
|
||||
if module == module_match:
|
||||
if file_match_re.search(file_name):
|
||||
if ignore_re.search(match):
|
||||
ignore_found = True
|
||||
if not ignore_found:
|
||||
# If we land here, we need to add another special case
|
||||
raise Exception(self._method+' w/o ORB_ID(): '+match+' in '
|
||||
+file_name+' ('+module+'). You need to add another special case.')
|
||||
|
||||
# handle special cases
|
||||
for i, (module_match, file_match_re, src_match_re, _) in enumerate(self._special_cases):
|
||||
if src_match_re is None:
|
||||
continue
|
||||
if module == module_match:
|
||||
if file_match_re.search(file_name):
|
||||
matches = src_match_re.findall(src_str)
|
||||
for match in matches:
|
||||
# match has the form: '[ORB_ID(]<topic_name>'
|
||||
if match.startswith(orb_id):
|
||||
topic_name = match[len(orb_id):]
|
||||
dbg_print('Found '+self._method+' for special case in '
|
||||
+module+': '+topic_name)
|
||||
self._add_topic(topic_name, file_name, module)
|
||||
self._special_cases_matched[i] = True
|
||||
else:
|
||||
# this is not fatal, as it could be a method delaration/definition
|
||||
dbg_print('Special case '+self._method+' w/o ORB_ID(): '
|
||||
+match+' in '+file_name+' ('+module+')')
|
||||
|
||||
def _add_topic(self, topic_name, file_name, module):
|
||||
""" add a subscription/publication for a module
|
||||
"""
|
||||
if topic_name in self._topic_blacklist:
|
||||
dbg_print('ignoring blacklisted topic '+topic_name)
|
||||
return
|
||||
|
||||
if module is None:
|
||||
if not file_name.endswith('hott/messages.cpp'): # hott has a special module structure. just ignore it
|
||||
print('Warning: found '+self._method+' without associated module: '
|
||||
+topic_name+' in '+file_name)
|
||||
return
|
||||
|
||||
if not module in self._module_pubsubs:
|
||||
self._module_pubsubs[module] = set()
|
||||
self._module_pubsubs[module].add(topic_name)
|
||||
|
||||
|
||||
def get_topics(self, modules):
|
||||
""" get the set of topics
|
||||
:param modules: list of modules to take into account
|
||||
"""
|
||||
topics = set()
|
||||
for module in modules:
|
||||
if module in self._module_pubsubs:
|
||||
topics |= self._module_pubsubs[module]
|
||||
return topics
|
||||
|
||||
@property
|
||||
def pubsubs(self):
|
||||
""" get dict of all publication/subscriptions (key=modules, value=set of
|
||||
topic names"""
|
||||
return self._module_pubsubs
|
||||
|
||||
|
||||
class Graph:
|
||||
""" Collects Node and Edge information by parsing the source tree """
|
||||
def __init__(self, module_whitelist=[], topic_blacklist=[]):
|
||||
self._current_module = [] # stack with current module (they can be nested)
|
||||
self._all_modules = set() # set of all found modules
|
||||
|
||||
self._comment_remove_pattern = re.compile(
|
||||
r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
|
||||
re.DOTALL | re.MULTILINE)
|
||||
self._whitespace_pattern = re.compile(r'\s+')
|
||||
self._module_whitelist = module_whitelist
|
||||
self._excluded_paths = []
|
||||
|
||||
self._orb_id_vehicle_attitude_controls_topic = 'actuator_controls_0'
|
||||
self._orb_id_vehicle_attitude_controls_re = \
|
||||
re.compile(r'\#define\s+ORB_ID_VEHICLE_ATTITUDE_CONTROLS\s+([^,)]+)')
|
||||
|
||||
self._module_subscriptions = {} # key = module name, value = set of topic names
|
||||
self._module_publications = {} # key = module name, value = set of topic names
|
||||
|
||||
self._modules = set() # all modules
|
||||
self._topics = set() # all topics
|
||||
self._topic_colors = {} # key = topic, value = color (html string)
|
||||
|
||||
# handle special cases
|
||||
# format: list of tuples with 4 entries:
|
||||
# - module name to match (module MAIN)
|
||||
# - regex for file name(s) to match within the module (matched against the full path)
|
||||
# - regex to extract the topic name: the match must be ORB_ID(<topic_name>
|
||||
# Note: whitespace is removed from source code, so it does not need to be
|
||||
# accounted for in the regex.
|
||||
# If this is None, it will just be ignored
|
||||
# - regex to ignore matches in the form orb_[subscribe|advertise](<match>
|
||||
# (the expectation is that the previous matching ORB_ID() will be passed
|
||||
# to this, so that we can ignore it)
|
||||
special_cases_sub = [
|
||||
('sensors', r'voted_sensors_update\.cpp$', r'\binit_sensor_class\b\(([^,)]+)', r'^meta$'),
|
||||
('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'),
|
||||
('sdlog2', r'.*', None, r'^topic$'),
|
||||
('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'),
|
||||
|
||||
('uavcan', r'uavcan_main\.cpp$', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('pwm_out_sim', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
|
||||
('fmu', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'),
|
||||
('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'),
|
||||
]
|
||||
special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))
|
||||
for a,b,c,d in special_cases_sub]
|
||||
|
||||
self._subscriptions = PubSub(False, topic_blacklist,
|
||||
[r"\borb_subscribe(_multi|)\b\(([^,)]+)"],
|
||||
special_cases_sub)
|
||||
|
||||
|
||||
special_cases_pub = [
|
||||
('replay', r'replay_main\.cpp$', None, r'^sub\.orb_meta$'),
|
||||
('fw_pos_control_l1', r'FixedwingPositionControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
|
||||
('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
|
||||
('fw_att_control', r'fw_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
|
||||
('fw_att_control', r'fw_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
|
||||
('fw_att_control', r'fw_att_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
|
||||
|
||||
('uavcan', r'sensors/.*\.cpp$', r'\bUavcanCDevSensorBridgeBase\([^{]*DEVICE_PATH,([^,)]+)', r'^_orb_topic$'),
|
||||
('batt_smbus', r'batt_smbus\.cpp$', r'\b_batt_orb_id=([^,)]+)', r'^_batt_orb_id$'),
|
||||
]
|
||||
special_cases_pub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d))
|
||||
for a,b,c,d in special_cases_pub]
|
||||
self._publications = PubSub(True, topic_blacklist,
|
||||
[r"\borb_advertise(_multi|_queue|_multi_queue|)\b\(([^,)]+)",
|
||||
r"\borb_publish_auto()\b\(([^,)]+)"],
|
||||
special_cases_pub)
|
||||
|
||||
|
||||
def _get_current_module(self):
|
||||
if len(self._current_module) == 0:
|
||||
return None
|
||||
return self._current_module[-1]
|
||||
|
||||
def build(self, src_path_list, excluded_paths=[], use_topic_pubsub_union=True):
|
||||
""" parse the source tree & extract pub/sub information.
|
||||
:param use_topic_pubsub_union: if true, use all topics that have a
|
||||
publisher or subscriber. If false, use only topics with at least one
|
||||
publisher and subscriber.
|
||||
|
||||
fill in self._module_subsciptions & self._module_publications
|
||||
"""
|
||||
|
||||
self._subscriptions.reset()
|
||||
self._publications.reset()
|
||||
|
||||
self._excluded_paths = [os.path.normpath(p) for p in excluded_paths]
|
||||
for path in src_path_list:
|
||||
self._build_recursive(path)
|
||||
|
||||
# filter by whitelist
|
||||
if len(self._module_whitelist) > 0:
|
||||
self._subscriptions.filter_modules(self._module_whitelist)
|
||||
self._publications.filter_modules(self._module_whitelist)
|
||||
|
||||
# modules & topics sets
|
||||
self._modules = set(self._publications.pubsubs.keys() +
|
||||
self._subscriptions.pubsubs.keys())
|
||||
print('number of modules: '+str(len(self._modules)))
|
||||
self._topics = self._get_topics(use_topic_pubsub_union=use_topic_pubsub_union)
|
||||
print('number of topics: '+str(len(self._topics)))
|
||||
|
||||
# initialize colors
|
||||
color_list = get_N_colors(len(self._topics), 0.7, 0.85)
|
||||
self._topic_colors = {}
|
||||
for i, topic in enumerate(self._topics):
|
||||
self._topic_colors[topic] = color_list[i]
|
||||
|
||||
|
||||
# validate that all special rules got used
|
||||
self._subscriptions.check_if_match_found(self._all_modules)
|
||||
self._publications.check_if_match_found(self._all_modules)
|
||||
|
||||
|
||||
def _get_topics(self, use_topic_pubsub_union=True):
|
||||
""" get the set of topics
|
||||
"""
|
||||
subscribed_topics = self._subscriptions.get_topics(self._modules)
|
||||
published_topics = self._publications.get_topics(self._modules)
|
||||
if use_topic_pubsub_union:
|
||||
return subscribed_topics | published_topics
|
||||
return subscribed_topics & published_topics
|
||||
|
||||
def _build_recursive(self, path):
|
||||
|
||||
if os.path.normpath(path) in self._excluded_paths:
|
||||
dbg_print('ignoring excluded path '+path)
|
||||
return
|
||||
|
||||
entries = os.listdir(path)
|
||||
|
||||
# check if entering a new module
|
||||
cmake_file = 'CMakeLists.txt'
|
||||
new_module = False
|
||||
if cmake_file in entries:
|
||||
new_module = self._extract_module_name(os.path.join(path, cmake_file))
|
||||
|
||||
# iterate directories recursively
|
||||
for entry in entries:
|
||||
file_name = os.path.join(path, entry)
|
||||
if os.path.isdir(file_name):
|
||||
self._build_recursive(file_name)
|
||||
|
||||
|
||||
# iterate source files
|
||||
# Note: we could skip the entries if we're not in a module, but we don't
|
||||
# so that we get appropriate error messages to know where we miss subs
|
||||
# or pubs
|
||||
for entry in entries:
|
||||
file_name = os.path.join(path, entry)
|
||||
if os.path.isfile(file_name):
|
||||
_, ext = os.path.splitext(file_name)
|
||||
if ext in ['.cpp', '.c', '.h', '.hpp']:
|
||||
self._process_source_file(file_name)
|
||||
|
||||
|
||||
if new_module:
|
||||
self._current_module.pop()
|
||||
|
||||
|
||||
def _extract_module_name(self, file_name):
|
||||
""" extract the module name from a CMakeLists.txt file and store
|
||||
in self._current_module if there is any """
|
||||
datafile = file(file_name)
|
||||
found_module_def = False
|
||||
for line in datafile:
|
||||
if 'px4_add_module' in line: # must contain 'px4_add_module'
|
||||
found_module_def = True
|
||||
|
||||
words = line.split()
|
||||
# get the definition of MAIN
|
||||
if found_module_def and 'MAIN' in words and len(words) >= 2:
|
||||
self._current_module.append(words[1])
|
||||
self._all_modules.add(words[1])
|
||||
dbg_print('Found module name: '+words[1])
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def _process_source_file(self, file_name):
|
||||
""" extract information from a single source file """
|
||||
|
||||
with codecs.open(file_name, 'r', 'utf-8') as f:
|
||||
try:
|
||||
content = f.read()
|
||||
except:
|
||||
print('Failed reading file: %s, skipping content.' % path)
|
||||
return
|
||||
|
||||
|
||||
current_module = self._get_current_module()
|
||||
if current_module == 'uorb_tests': # skip this
|
||||
return
|
||||
if current_module == 'uorb':
|
||||
|
||||
# search and validate the ORB_ID_VEHICLE_ATTITUDE_CONTROLS define
|
||||
matches = self._orb_id_vehicle_attitude_controls_re.findall(content)
|
||||
for match in matches:
|
||||
if match != 'ORB_ID('+self._orb_id_vehicle_attitude_controls_topic:
|
||||
# if we land here, you need to change _orb_id_vehicle_attitude_controls_topic
|
||||
raise Exception(
|
||||
'The extracted define for ORB_ID_VEHICLE_ATTITUDE_CONTROLS '
|
||||
'is '+match+' but expected ORB_ID('+
|
||||
self._orb_id_vehicle_attitude_controls_topic)
|
||||
|
||||
return # skip uorb module for the rest
|
||||
|
||||
|
||||
|
||||
if content.lower().find('orb_') != -1: # approximative filter to quickly
|
||||
# discard files we're not interested in
|
||||
# (speedup the parsing)
|
||||
src = self._comment_remover(content)
|
||||
src = re.sub(self._whitespace_pattern, '', src) # remove all whitespace
|
||||
|
||||
|
||||
# subscriptions
|
||||
self._subscriptions.extract(file_name, src, current_module,
|
||||
self._orb_id_vehicle_attitude_controls_topic)
|
||||
|
||||
# publications
|
||||
self._publications.extract(file_name, src, current_module,
|
||||
self._orb_id_vehicle_attitude_controls_topic)
|
||||
|
||||
# TODO: handle Publication & Subscription template classes
|
||||
|
||||
|
||||
|
||||
def _comment_remover(self, text):
|
||||
""" remove C++ & C style comments.
|
||||
Source: https://stackoverflow.com/a/241506 """
|
||||
def replacer(match):
|
||||
s = match.group(0)
|
||||
if s.startswith('/'):
|
||||
return " " # note: a space and not an empty string
|
||||
else:
|
||||
return s
|
||||
return re.sub(self._comment_remove_pattern, replacer, text)
|
||||
|
||||
|
||||
@property
|
||||
def modules(self):
|
||||
""" get the set of all modules """
|
||||
return self._modules
|
||||
|
||||
@property
|
||||
def topics(self):
|
||||
""" get set set of all topics """
|
||||
return self._topics
|
||||
|
||||
@property
|
||||
def topic_colors(self):
|
||||
""" get a dict of all topic colors with key=topic, value=color """
|
||||
return self._topic_colors
|
||||
|
||||
@property
|
||||
def module_subscriptions(self):
|
||||
""" get a dict of all subscriptions with key=module name, value=set(topic names) """
|
||||
return self._subscriptions.pubsubs
|
||||
|
||||
@property
|
||||
def module_publications(self):
|
||||
""" get a dict of all publications with key=module name, value=set(topic names) """
|
||||
return self._publications.pubsubs
|
||||
|
||||
|
||||
|
||||
class OutputGraphviz:
|
||||
""" write graph using Graphviz """
|
||||
|
||||
def __init__(self, graph):
|
||||
self._graph = graph
|
||||
|
||||
def write(self, file_name, engine='fdp',
|
||||
show_publications=True, show_subscriptions=True):
|
||||
""" write the graph to a file
|
||||
:param engine: graphviz engine
|
||||
- fdp works for large graphs
|
||||
- neato works better for smaller graphs
|
||||
- circo works for single modules
|
||||
CLI: fdp graph.fv -Tpdf -o test.pdf
|
||||
"""
|
||||
|
||||
print('Writing to '+file_name)
|
||||
|
||||
ratio = 1 # aspect ratio
|
||||
|
||||
modules = self._graph.modules
|
||||
topics = self._graph.topics
|
||||
topic_colors = self._graph.topic_colors
|
||||
module_publications = self._graph.module_publications
|
||||
module_subscriptions = self._graph.module_subscriptions
|
||||
|
||||
graph_attr={'splines': 'true', 'ratio': str(ratio), 'overlap': 'false'}
|
||||
graph_attr['sep'] = '"+15,15"' # increase spacing between nodes
|
||||
graph = Digraph(comment='autogenerated graph with graphviz using uorb_graph.py',
|
||||
engine=engine, graph_attr=graph_attr)
|
||||
|
||||
|
||||
# nodes
|
||||
for module in modules:
|
||||
graph.node('m_'+module, module, shape='box', fontcolor='#ffffff',
|
||||
style='filled', color='#666666', fontsize='16')
|
||||
|
||||
for topic in topics:
|
||||
graph.node('t_'+topic, topic, shape='ellipse', fontcolor='#ffffff',
|
||||
style='filled', color=topic_colors[topic])
|
||||
|
||||
|
||||
# edges
|
||||
if show_publications:
|
||||
for module in modules:
|
||||
if module in module_publications:
|
||||
for topic in module_publications[module]:
|
||||
if topic in topics:
|
||||
graph.edge('m_'+module, 't_'+topic,
|
||||
color=topic_colors[topic], style='dashed')
|
||||
|
||||
if show_subscriptions:
|
||||
for module in modules:
|
||||
if module in module_subscriptions:
|
||||
for topic in module_subscriptions[module]:
|
||||
if topic in topics:
|
||||
graph.edge('t_'+topic, 'm_'+module,
|
||||
color=topic_colors[topic])
|
||||
|
||||
graph.render(file_name, view=False)
|
||||
|
||||
|
||||
class OutputJSON:
|
||||
""" write graph to a JSON file (that can be used with D3.js) """
|
||||
|
||||
def __init__(self, graph):
|
||||
self._graph = graph
|
||||
|
||||
def write(self, file_name):
|
||||
|
||||
print('Writing to '+file_name)
|
||||
|
||||
modules = self._graph.modules
|
||||
topics = self._graph.topics
|
||||
topic_colors = self._graph.topic_colors
|
||||
module_publications = self._graph.module_publications
|
||||
module_subscriptions = self._graph.module_subscriptions
|
||||
|
||||
data = {}
|
||||
nodes = []
|
||||
|
||||
# nodes
|
||||
# (sort by length, such that short names are last. The rendering order
|
||||
# will be the same, so that in case of an overlap, the shorter label
|
||||
# will be on top)
|
||||
for module in sorted(modules, key=len, reverse=True):
|
||||
node = {}
|
||||
node['id'] = 'm_'+module
|
||||
node['name'] = module
|
||||
node['type'] = 'module'
|
||||
node['color'] = '#666666'
|
||||
# TODO: add url to open module documentation?
|
||||
nodes.append(node)
|
||||
|
||||
for topic in sorted(topics, key=len, reverse=True):
|
||||
node = {}
|
||||
node['id'] = 't_'+topic
|
||||
node['name'] = topic
|
||||
node['type'] = 'topic'
|
||||
node['color'] = topic_colors[topic]
|
||||
# url is opened when double-clicking on the node
|
||||
# TODO: does not work for multi-topics
|
||||
node['url'] = 'https://github.com/PX4/Firmware/blob/master/msg/'+topic+'.msg'
|
||||
nodes.append(node)
|
||||
|
||||
data['nodes'] = nodes
|
||||
|
||||
edges = []
|
||||
|
||||
# edges
|
||||
for module in modules:
|
||||
if module in module_publications:
|
||||
for topic in module_publications[module]:
|
||||
if topic in topics:
|
||||
edge = {}
|
||||
edge['source'] = 'm_'+module
|
||||
edge['target'] = 't_'+topic
|
||||
edge['color'] = topic_colors[topic]
|
||||
edge['style'] = 'dashed'
|
||||
edges.append(edge)
|
||||
|
||||
for module in modules:
|
||||
if module in module_subscriptions:
|
||||
for topic in module_subscriptions[module]:
|
||||
if topic in topics:
|
||||
edge = {}
|
||||
edge['source'] = 't_'+topic
|
||||
edge['target'] = 'm_'+module
|
||||
edge['color'] = topic_colors[topic]
|
||||
edge['style'] = 'normal'
|
||||
edges.append(edge)
|
||||
|
||||
data['links'] = edges
|
||||
|
||||
with open(file_name, 'w') as outfile:
|
||||
json.dump(data, outfile) # add indent=2 for readable formatting
|
||||
|
||||
|
||||
|
||||
# ignore topics that are subscribed/published by many topics, but are not really
|
||||
# useful to show in the graph
|
||||
topic_blacklist = [ 'parameter_update', 'mavlink_log', 'log_message' ]
|
||||
print('Excluded topics: '+str(topic_blacklist))
|
||||
|
||||
if len(args.modules) == 0:
|
||||
module_whitelist = []
|
||||
else:
|
||||
module_whitelist = [ m.strip() for m in args.modules.split(',')]
|
||||
|
||||
graph = Graph(module_whitelist=module_whitelist, topic_blacklist=topic_blacklist)
|
||||
if len(args.src_path) == 0:
|
||||
args.src_path = ['src']
|
||||
graph.build(args.src_path, args.exclude_path, use_topic_pubsub_union=args.use_topic_union)
|
||||
|
||||
|
||||
if args.output == 'json':
|
||||
output_json = OutputJSON(graph)
|
||||
output_json.write(args.file+'.json')
|
||||
|
||||
elif args.output == 'graphviz':
|
||||
try:
|
||||
from graphviz import Digraph
|
||||
except:
|
||||
print("Failed to import graphviz.")
|
||||
print("You may need to install it with 'pip install graphviz'")
|
||||
print("")
|
||||
raise
|
||||
output_graphviz = OutputGraphviz(graph)
|
||||
engine='fdp' # use neato or fdp
|
||||
output_graphviz.write(args.file+'.fv', engine=engine)
|
||||
output_graphviz.write(args.file+'_subs.fv', show_publications=False, engine=engine)
|
||||
output_graphviz.write(args.file+'_pubs.fv', show_subscriptions=False, engine=engine)
|
||||
else:
|
||||
print('Error: unknown output format '+args.output)
|
||||
|
||||
|
||||
|
||||
@ -1,13 +0,0 @@
|
||||
#! /bin/bash
|
||||
# create the graph from a posix (e.g. SITL) startup script
|
||||
|
||||
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
|
||||
|
||||
startup_file="$SCRIPT_DIR"/../../posix-configs/SITL/init/ekf2/typhoon_h480
|
||||
[ -n "$1" ] && startup_file=$1
|
||||
# get the modules as comma-separated list
|
||||
modules=$(cat "$startup_file"|cut -f1 -d' '|sort|uniq|tr '\n' ,)
|
||||
|
||||
cd "$SCRIPT_DIR/../.."
|
||||
"$SCRIPT_DIR"/create.py --src-path src -m "$modules" -f "$SCRIPT_DIR/graph_runtime_sitl"
|
||||
|
||||
@ -9,8 +9,9 @@ float32 y # Y coordinate in meters
|
||||
float32 z # Z coordinate in meters
|
||||
|
||||
float32 yaw # Yaw angle in radians
|
||||
float32 direction_x # Takeoff direction in NED X
|
||||
float32 direction_y # Takeoff direction in NED Y
|
||||
float32 direction_z # Takeoff direction in NED Z
|
||||
|
||||
bool valid_alt # true when the altitude has been set
|
||||
bool valid_hpos # true when the latitude and longitude have been set
|
||||
|
||||
bool manual_home # true when home position was set manually
|
||||
|
||||
@ -331,7 +331,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch
|
||||
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
|
||||
* time the vehicle is armed with a good GPS fix.
|
||||
**/
|
||||
static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
|
||||
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
|
||||
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
|
||||
const vehicle_attitude_s &attitude,
|
||||
bool set_alt_only_to_lpos_ref);
|
||||
@ -971,8 +971,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
|
||||
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
|
||||
if (cmd_arms && (arming_res == TRANSITION_CHANGED) &&
|
||||
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) &&
|
||||
!home->manual_home) {
|
||||
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
|
||||
|
||||
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false);
|
||||
}
|
||||
@ -1043,7 +1042,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
|
||||
if (use_current) {
|
||||
/* use current position */
|
||||
if (commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude, false)) {
|
||||
if (status_flags.condition_global_position_valid) {
|
||||
home->lat = global_pos->lat;
|
||||
home->lon = global_pos->lon;
|
||||
home->alt = global_pos->alt;
|
||||
|
||||
home->timestamp = hrt_absolute_time();
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
@ -1051,50 +1056,29 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
}
|
||||
|
||||
} else {
|
||||
const double lat = cmd->param5;
|
||||
const double lon = cmd->param6;
|
||||
const float alt = cmd->param7;
|
||||
/* use specified position */
|
||||
home->lat = cmd->param5;
|
||||
home->lon = cmd->param6;
|
||||
home->alt = cmd->param7;
|
||||
|
||||
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
|
||||
home->timestamp = hrt_absolute_time();
|
||||
|
||||
if (local_pos->xy_global && local_pos->z_global) {
|
||||
/* use specified position */
|
||||
home->timestamp = hrt_absolute_time();
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
}
|
||||
|
||||
home->lat = lat;
|
||||
home->lon = lon;
|
||||
home->alt = alt;
|
||||
if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
|
||||
|
||||
home->manual_home = true;
|
||||
home->valid_alt = true;
|
||||
home->valid_hpos = true;
|
||||
|
||||
// update local projection reference including altitude
|
||||
struct map_projection_reference_s ref_pos;
|
||||
map_projection_init(&ref_pos, local_pos->ref_lat, local_pos->ref_lon);
|
||||
map_projection_project(&ref_pos, lat, lon, &home->x, &home->y);
|
||||
home->z = -(alt - local_pos->ref_alt);
|
||||
|
||||
/* announce new home position */
|
||||
if (*home_pub != nullptr) {
|
||||
orb_publish(ORB_ID(home_position), *home_pub, home);
|
||||
|
||||
} else {
|
||||
*home_pub = orb_advertise(ORB_ID(home_position), home);
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
status_flags.condition_home_position_valid = true;
|
||||
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
/* announce new home position */
|
||||
if (*home_pub != nullptr) {
|
||||
orb_publish(ORB_ID(home_position), *home_pub, home);
|
||||
|
||||
} else {
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
*home_pub = orb_advertise(ORB_ID(home_position), home);
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
status_flags.condition_home_position_valid = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -1250,7 +1234,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
* @brief This function initializes the home position an altitude of the vehicle. This happens first time we get a good GPS fix and each
|
||||
* time the vehicle is armed with a good GPS fix.
|
||||
**/
|
||||
static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
|
||||
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
|
||||
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
|
||||
const vehicle_attitude_s &attitude,
|
||||
bool set_alt_only_to_lpos_ref)
|
||||
@ -1258,15 +1242,16 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &
|
||||
if (!set_alt_only_to_lpos_ref) {
|
||||
//Need global and local position fix to be able to set home
|
||||
if (!status_flags.condition_global_position_valid || !status_flags.condition_local_position_valid) {
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
//Ensure that the GPS accuracy is good enough for intializing home
|
||||
if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Set home position
|
||||
//Set home position
|
||||
home.timestamp = hrt_absolute_time();
|
||||
home.lat = globalPosition.lat;
|
||||
home.lon = globalPosition.lon;
|
||||
home.valid_hpos = true;
|
||||
@ -1281,6 +1266,9 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &
|
||||
matrix::Eulerf euler = matrix::Quatf(attitude.q);
|
||||
home.yaw = euler.psi();
|
||||
|
||||
PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
|
||||
|
||||
|
||||
//Play tune first time we initialize HOME
|
||||
if (!status_flags.condition_home_position_valid) {
|
||||
tune_home_set(true);
|
||||
@ -1291,16 +1279,15 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &
|
||||
|
||||
} else if (!home.valid_alt && localPosition.z_global) {
|
||||
// handle special case where we are setting only altitude using local position reference
|
||||
home.timestamp = hrt_absolute_time();
|
||||
home.alt = localPosition.ref_alt;
|
||||
home.valid_alt = true;
|
||||
PX4_INFO("home alt: %.2f", (double)home.alt);
|
||||
|
||||
} else {
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
home.timestamp = hrt_absolute_time();
|
||||
home.manual_home = false;
|
||||
|
||||
/* announce new home position */
|
||||
if (homePub != nullptr) {
|
||||
orb_publish(ORB_ID(home_position), homePub, &home);
|
||||
@ -1308,8 +1295,6 @@ static bool commander_set_home_position(orb_advert_t &homePub, home_position_s &
|
||||
} else {
|
||||
homePub = orb_advertise(ORB_ID(home_position), &home);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int commander_thread_main(int argc, char *argv[])
|
||||
@ -2047,9 +2032,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
// Provide feedback on mission state
|
||||
if ((_mission_result.timestamp > commander_boot_timestamp) && hotplug_timeout &&
|
||||
(_mission_result.instance_count > 0) && !_mission_result.valid) {
|
||||
|
||||
if (!_mission_result.valid && hotplug_timeout && _home.timestamp > 0) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
|
||||
}
|
||||
}
|
||||
@ -3137,43 +3120,24 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* Get current timestamp */
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// automaticcally set or update home position
|
||||
if (!_home.manual_home) {
|
||||
if (armed.armed) {
|
||||
if ((!was_armed || (was_landed && !land_detector.landed)) &&
|
||||
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
|
||||
/* First time home position update - but only if disarmed */
|
||||
if (!status_flags.condition_home_position_valid && !armed.armed) {
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false);
|
||||
}
|
||||
|
||||
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false);
|
||||
}
|
||||
} else {
|
||||
if (status_flags.condition_home_position_valid) {
|
||||
if (land_detector.landed && local_position.xy_valid && local_position.z_valid) {
|
||||
/* distance from home */
|
||||
float home_dist_xy = -1.0f;
|
||||
float home_dist_z = -1.0f;
|
||||
mavlink_wpm_distance_to_point_local(_home.x, _home.y, _home.z, local_position.x, local_position.y,
|
||||
local_position.z, &home_dist_xy, &home_dist_z);
|
||||
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
|
||||
else if (((!was_armed && armed.armed) || (was_landed && !land_detector.landed)) &&
|
||||
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false);
|
||||
|
||||
if (home_dist_xy > local_position.epv * 2 || home_dist_z > local_position.eph * 2) {
|
||||
}
|
||||
|
||||
/* update when disarmed, landed and moved away from current home position */
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
/* First time home position update - but only if disarmed */
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false);
|
||||
}
|
||||
}
|
||||
/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
|
||||
* This allows home atitude to be used in the calculation of height above takeoff location when GPS
|
||||
* use has commenced after takeoff. */
|
||||
if (!_home.valid_alt && local_position.z_global) {
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, true);
|
||||
|
||||
/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
|
||||
* This allows home atitude to be used in the calculation of height above takeoff location when GPS
|
||||
* use has commenced after takeoff. */
|
||||
if (!_home.valid_alt && local_position.z_global) {
|
||||
commander_set_home_position(home_pub, _home, local_position, global_position, attitude, true);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// check for arming state change
|
||||
|
||||
@ -217,7 +217,6 @@ private:
|
||||
BlockParamExtFloat _mag_heading_noise; ///< measurement noise used for simple heading fusion (rad)
|
||||
BlockParamExtFloat _mag_noise; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
|
||||
BlockParamExtFloat _eas_noise; ///< measurement noise used for airspeed fusion (m/sec)
|
||||
BlockParamExtFloat _beta_innov_gate; ///< synthetic sideslip innovation consistency gate size (STD)
|
||||
BlockParamExtFloat _beta_noise; ///< synthetic sideslip noise (rad)
|
||||
BlockParamExtFloat _mag_declination_deg;///< magnetic declination (degrees)
|
||||
BlockParamExtFloat _heading_innov_gate;///< heading fusion innovation consistency gate size (STD)
|
||||
@ -362,7 +361,6 @@ Ekf2::Ekf2():
|
||||
_mag_heading_noise(this, "HEAD_NOISE", true, _params->mag_heading_noise),
|
||||
_mag_noise(this, "MAG_NOISE", true, _params->mag_noise),
|
||||
_eas_noise(this, "EAS_NOISE", true, _params->eas_noise),
|
||||
_beta_innov_gate(this, "BETA_GATE", true, _params->beta_innov_gate),
|
||||
_beta_noise(this, "BETA_NOISE", true, _params->beta_noise),
|
||||
_mag_declination_deg(this, "MAG_DECL", true, _params->mag_declination_deg),
|
||||
_heading_innov_gate(this, "HDG_GATE", true, _params->heading_innov_gate),
|
||||
|
||||
@ -393,18 +393,6 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f);
|
||||
|
||||
/**
|
||||
* Gate size for synthetic sideslip fusion
|
||||
*
|
||||
* Sets the number of standard deviations used by the innovation consistency test.
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 1.0
|
||||
* @unit SD
|
||||
* @decimal 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_BETA_GATE, 5.0f);
|
||||
|
||||
/**
|
||||
* Noise for synthetic sideslip fusion.
|
||||
*
|
||||
@ -564,18 +552,16 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
|
||||
* 3 : Set to true to enable vision position fusion
|
||||
* 4 : Set to true to enable vision yaw fusion
|
||||
* 5 : Set to true to enable multi-rotor drag specific force fusion
|
||||
* 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 127
|
||||
* @max 63
|
||||
* @bit 0 use GPS
|
||||
* @bit 1 use optical flow
|
||||
* @bit 2 inhibit IMU bias estimation
|
||||
* @bit 3 vision position fusion
|
||||
* @bit 4 vision yaw fusion
|
||||
* @bit 5 multi-rotor drag fusion
|
||||
* @bit 6 rotate external vision
|
||||
* @reboot_required true
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
|
||||
|
||||
@ -591,7 +591,6 @@ void Logger::add_default_topics()
|
||||
add_topic("ekf2_innovations", 200);
|
||||
add_topic("esc_status", 250);
|
||||
add_topic("estimator_status", 200);
|
||||
add_topic("home_position");
|
||||
add_topic("input_rc", 200);
|
||||
add_topic("manual_control_setpoint", 200);
|
||||
add_topic("mission_result");
|
||||
|
||||
@ -1406,7 +1406,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_CHANGE_SPEED:
|
||||
case MAV_CMD_DO_SET_HOME:
|
||||
case MAV_CMD_DO_SET_SERVO:
|
||||
case MAV_CMD_DO_LAND_START:
|
||||
case MAV_CMD_DO_TRIGGER_CONTROL:
|
||||
@ -1480,7 +1479,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
|
||||
break;
|
||||
|
||||
case NAV_CMD_DO_CHANGE_SPEED:
|
||||
case NAV_CMD_DO_SET_HOME:
|
||||
case NAV_CMD_DO_SET_SERVO:
|
||||
case NAV_CMD_DO_LAND_START:
|
||||
case NAV_CMD_DO_TRIGGER_CONTROL:
|
||||
|
||||
@ -870,11 +870,11 @@ Mission::do_need_vertical_takeoff()
|
||||
/* force takeoff if landed (additional protection) */
|
||||
_need_takeoff = true;
|
||||
|
||||
} else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) {
|
||||
} else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_acceptance_radius()) {
|
||||
/* if in-air and already above takeoff height, don't do takeoff */
|
||||
_need_takeoff = false;
|
||||
|
||||
} else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius()
|
||||
} else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_acceptance_radius()
|
||||
&& (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|
||||
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
|
||||
/* if in-air but below takeoff height and we have a takeoff item */
|
||||
|
||||
@ -113,7 +113,6 @@ MissionBlock::is_mission_item_reached()
|
||||
}
|
||||
|
||||
case NAV_CMD_DO_CHANGE_SPEED:
|
||||
case NAV_CMD_DO_SET_HOME:
|
||||
return true;
|
||||
|
||||
default:
|
||||
|
||||
@ -64,7 +64,6 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
|
||||
fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
|
||||
const float home_alt = _navigator->get_home_position()->alt;
|
||||
const bool home_valid = _navigator->home_position_valid();
|
||||
const bool home_alt_valid = _navigator->home_alt_valid();
|
||||
|
||||
bool &warning_issued = _navigator->get_mission_result()->warning;
|
||||
const float default_acceptance_rad = _navigator->get_default_acceptance_radius();
|
||||
@ -75,7 +74,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
|
||||
bool warned = false;
|
||||
|
||||
// first check if we have a valid position
|
||||
if (!home_alt_valid) {
|
||||
if (!home_valid) {
|
||||
failed = true;
|
||||
warned = true;
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
|
||||
@ -90,15 +89,15 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
|
||||
failed = failed
|
||||
|| !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
|
||||
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
|
||||
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);
|
||||
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
|
||||
|
||||
if (isRotarywing) {
|
||||
failed = failed
|
||||
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);
|
||||
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_altitude_acceptance_rad);
|
||||
|
||||
} else {
|
||||
failed = failed
|
||||
|| !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
|
||||
|| !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_valid,
|
||||
default_acceptance_rad, land_start_req);
|
||||
}
|
||||
|
||||
@ -107,7 +106,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
|
||||
|
||||
bool
|
||||
MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
|
||||
float home_alt, bool home_alt_valid, float default_altitude_acceptance_rad)
|
||||
float home_alt, bool home_valid, float default_altitude_acceptance_rad)
|
||||
{
|
||||
for (size_t i = 0; i < nMissionItems; i++) {
|
||||
struct mission_item_s missionitem = {};
|
||||
@ -145,11 +144,11 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
|
||||
|
||||
bool
|
||||
MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems,
|
||||
fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_alt_valid,
|
||||
fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_valid,
|
||||
float default_acceptance_rad, bool land_start_req)
|
||||
{
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_alt_valid, land_start_req);
|
||||
bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_valid, land_start_req);
|
||||
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, fw_pos_ctrl_status, land_start_req);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
@ -199,7 +198,7 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
|
||||
|
||||
bool
|
||||
MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
|
||||
float home_alt, bool home_alt_valid, bool &warning_issued, bool throw_error)
|
||||
float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
|
||||
{
|
||||
/* Check if all waypoints are above the home altitude */
|
||||
for (size_t i = 0; i < nMissionItems; i++) {
|
||||
@ -213,7 +212,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
|
||||
}
|
||||
|
||||
/* reject relative alt without home set */
|
||||
if (missionitem.altitude_is_relative && !home_alt_valid && MissionBlock::item_contains_position(missionitem)) {
|
||||
if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(missionitem)) {
|
||||
|
||||
warning_issued = true;
|
||||
|
||||
@ -276,7 +275,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
|
||||
missionitem.nav_cmd != NAV_CMD_DELAY &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
|
||||
@ -329,7 +327,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
|
||||
|
||||
bool
|
||||
MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems,
|
||||
float home_alt, bool home_alt_valid, float default_acceptance_rad)
|
||||
float home_alt, bool home_valid, float default_acceptance_rad)
|
||||
{
|
||||
for (size_t i = 0; i < nMissionItems; i++) {
|
||||
struct mission_item_s missionitem = {};
|
||||
|
||||
@ -57,7 +57,7 @@ private:
|
||||
/* Checks for all airframes */
|
||||
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
|
||||
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
|
||||
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
|
||||
bool &warning_issued, bool throw_error = false);
|
||||
|
||||
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
|
||||
@ -68,16 +68,16 @@ private:
|
||||
|
||||
/* Checks specific to fixedwing airframes */
|
||||
bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
|
||||
float home_alt, bool home_alt_valid, float default_acceptance_rad, bool land_start_req);
|
||||
float home_alt, bool home_valid, float default_acceptance_rad, bool land_start_req);
|
||||
|
||||
bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
|
||||
bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid,
|
||||
float default_acceptance_rad);
|
||||
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
|
||||
bool land_start_req);
|
||||
|
||||
/* Checks specific to rotarywing airframes */
|
||||
bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
|
||||
float home_alt, bool home_alt_valid, float default_altitude_acceptance_rad);
|
||||
float home_alt, bool home_valid, float default_altitude_acceptance_rad);
|
||||
|
||||
public:
|
||||
MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}
|
||||
|
||||
@ -71,7 +71,6 @@ enum NAV_CMD {
|
||||
NAV_CMD_DELAY = 93,
|
||||
NAV_CMD_DO_JUMP = 177,
|
||||
NAV_CMD_DO_CHANGE_SPEED = 178,
|
||||
NAV_CMD_DO_SET_HOME = 179,
|
||||
NAV_CMD_DO_SET_SERVO = 183,
|
||||
NAV_CMD_DO_LAND_START = 189,
|
||||
NAV_CMD_DO_SET_ROI = 201,
|
||||
|
||||
@ -150,8 +150,7 @@ public:
|
||||
|
||||
const vehicle_roi_s &get_vroi() { return _vroi; }
|
||||
|
||||
bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
|
||||
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos); }
|
||||
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_hpos && _home_pos.valid_alt); }
|
||||
|
||||
int get_onboard_mission_sub() { return _onboard_mission_sub; }
|
||||
int get_offboard_mission_sub() { return _offboard_mission_sub; }
|
||||
@ -234,7 +233,7 @@ public:
|
||||
|
||||
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
|
||||
|
||||
void increment_mission_instance_count() { _mission_result.instance_count++; }
|
||||
void increment_mission_instance_count() { _mission_instance_count++; }
|
||||
|
||||
void set_mission_failure(const char *reason);
|
||||
|
||||
@ -291,6 +290,8 @@ private:
|
||||
position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */
|
||||
vehicle_roi_s _vroi{}; /**< vehicle ROI */
|
||||
|
||||
int _mission_instance_count{-1}; /**< instance count for the current mission */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
Geofence _geofence; /**< class that handles the geofence */
|
||||
|
||||
@ -497,7 +497,7 @@ Navigator::task_main()
|
||||
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
|
||||
if (_mission_result.valid &&
|
||||
if (get_mission_result()->valid &&
|
||||
PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
|
||||
|
||||
_mission.set_current_offboard_mission_index(cmd.param1);
|
||||
@ -1114,6 +1114,7 @@ void
|
||||
Navigator::publish_mission_result()
|
||||
{
|
||||
_mission_result.timestamp = hrt_absolute_time();
|
||||
_mission_result.instance_count = _mission_instance_count;
|
||||
|
||||
/* lazily publish the mission result only once available */
|
||||
if (_mission_result_pub != nullptr) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user