mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-30 13:40:34 +08:00
Compare commits
75 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Daniel Agar
|
64a3f0545f | ||
|
Ramon Roche
|
7462e98e16 | ||
|
Ramon Roche
|
3240cf4dc7 | ||
|
Ramon Roche
|
f2bd3105ad | ||
|
Ramon Roche
|
fb42770131 | ||
|
Ramon Roche
|
14468d49c1 | ||
|
bresch
|
14fe6c2167 | ||
|
bresch
|
93c690f133 | ||
|
bresch
|
a224d38e52 | ||
|
bresch
|
aeb182a8ed | ||
|
bresch
|
30d98885b7 | ||
|
bresch
|
814a2706f5 | ||
|
bresch
|
842212df6c | ||
|
bresch
|
6becccb217 | ||
|
bresch
|
a492457322 | ||
|
bresch
|
13c5bd4412 | ||
|
bresch
|
b19a6ee3b5 | ||
|
Ramon Roche
|
cf34b9d574 | ||
|
Ramon Roche
|
0fca8e3a85 | ||
|
Ramon Roche
|
884dee35a5 | ||
|
Daniel Agar
|
233cf17fa7 | ||
|
Silvan Fuhrer
|
2f65644aeb | ||
|
Julian Oes
|
746e26bf9e | ||
|
PX4 BuildBot
|
10590fc597 | ||
|
Beat Küng
|
8ccd43dc1b | ||
|
Beat Küng
|
be300b767d | ||
|
Matthias Grob
|
044d13635d | ||
|
Claudio Chies
|
db13b9cb50 | ||
|
Matthias Grob
|
bbc59dcde7 | ||
|
Matthias Grob
|
1410325c62 | ||
|
Matthias Grob
|
30eec33e09 | ||
|
Matthias Grob
|
399a8ad5b7 | ||
|
Matthias Grob
|
61d999073b | ||
|
Claudio Chies
|
f41a08aea8 | ||
|
Matthias Grob
|
2ef2911c36 | ||
|
Matthias Grob
|
b74dd57e7c | ||
|
Matthias Grob
|
4c8c5fbb37 | ||
|
Matthias Grob
|
001d722abd | ||
|
Matthias Grob
|
1fa76ac71d | ||
|
Matthias Grob
|
c879ca531d | ||
|
Matthias Grob
|
84dbbb4351 | ||
|
Matthias Grob
|
50ee5bd1b4 | ||
|
Matthias Grob
|
eb06ace8e4 | ||
|
Matthias Grob
|
079f3ca67c | ||
|
Claudio Chies
|
0cd6a553b9 | ||
|
Matthias Grob
|
b8c2805263 | ||
|
PX4 BuildBot
|
a49f034757 | ||
|
PX4 BuildBot
|
2f8460da91 | ||
|
Ramon Roche
|
dbc2e56b2e | ||
|
Ramon Roche
|
fb0f9b8aa7 | ||
|
Ramon Roche
|
74447a3ae2 | ||
|
Daniel Agar
|
cad2de228c | ||
|
Ramon Roche
|
abb5048cdb | ||
|
Jacob Dahl
|
1cfefc4c63 | ||
|
Daniel Agar
|
7288d16855 | ||
|
Daniel Agar
|
ada2bb4c42 | ||
|
Ramon Roche
|
8e288629d0 | ||
|
Julian Oes
|
7c507f136c | ||
|
Matthias Grob
|
3f83a8fa7a | ||
|
Federico Ciresola
|
ddc604d0de | ||
|
Daniel Agar
|
1d2ed28966 | ||
|
Daniel Agar
|
635e162a0c | ||
|
Matthias Grob
|
f8b34fd79d | ||
|
Matthias Grob
|
dcfecb4e16 | ||
|
Marco Hauswirth
|
a191e1e14e | ||
|
Matthias Grob
|
53d920ea33 | ||
|
somebody-once-told-me
|
3943025f32 | ||
|
Jacob Dahl
|
b891961a0c | ||
|
Matthias Grob
|
1aadbf85af | ||
|
Khalil L
|
129c0f188a | ||
|
Jacob Dahl
|
3bce6988ab | ||
|
Jacob Dahl
|
7a7240c3a2 | ||
|
Stefano Colli
|
b492947459 | ||
|
jmackay2
|
5b66a1eff1 | ||
|
Jacob Dahl
|
5f57b23477 |
@@ -1,233 +0,0 @@
|
||||
#!/usr/bin/env groovy
|
||||
|
||||
pipeline {
|
||||
agent none
|
||||
stages {
|
||||
|
||||
stage('Build') {
|
||||
steps {
|
||||
script {
|
||||
def build_nodes = [:]
|
||||
def docker_images = [
|
||||
armhf: "px4io/px4-dev-armhf:2023-06-26",
|
||||
arm64: "px4io/px4-dev-aarch64:2022-08-12",
|
||||
base: "px4io/px4-dev-ros2-foxy:2022-08-12",
|
||||
nuttx: "px4io/px4-dev-nuttx-focal:2022-08-12",
|
||||
]
|
||||
|
||||
def armhf_builds = [
|
||||
target: ["beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default", "scumaker_pilotpi_default"],
|
||||
image: docker_images.armhf,
|
||||
archive: false
|
||||
]
|
||||
|
||||
def arm64_builds = [
|
||||
target: ["scumaker_pilotpi_arm64"],
|
||||
image: docker_images.arm64,
|
||||
archive: false
|
||||
]
|
||||
|
||||
def base_builds = [
|
||||
target: ["px4_sitl_default"],
|
||||
image: docker_images.base,
|
||||
archive: false
|
||||
]
|
||||
|
||||
def nuttx_builds_archive = [
|
||||
target: [
|
||||
"3dr_ctrl-zero-h7-oem-revg_default",
|
||||
"airmind_mindpx-v2_default",
|
||||
"ark_can-flow_canbootloader",
|
||||
"ark_can-flow_default",
|
||||
"ark_can-gps_canbootloader",
|
||||
"ark_can-gps_default",
|
||||
"ark_can-rtk-gps_canbootloader",
|
||||
"ark_can-rtk-gps_default",
|
||||
"ark_cannode_canbootloader",
|
||||
"ark_cannode_default",
|
||||
"ark_fmu-v6x_bootloader",
|
||||
"ark_fmu-v6x_default",
|
||||
"ark_fpv_bootloader",
|
||||
"ark_fpv_default",
|
||||
"ark_pi6x_bootloader",
|
||||
"ark_pi6x_default",
|
||||
"atl_mantis-edu_default",
|
||||
"av_x-v1_default",
|
||||
"bitcraze_crazyflie21_default",
|
||||
"bitcraze_crazyflie_default",
|
||||
"cuav_7-nano_default",
|
||||
"cuav_can-gps-v1_canbootloader",
|
||||
"cuav_can-gps-v1_default",
|
||||
"cuav_nora_default",
|
||||
"cuav_x7pro_default",
|
||||
"cubepilot_cubeorange_default",
|
||||
"cubepilot_cubeorangeplus_default",
|
||||
"cubepilot_cubeyellow_default",
|
||||
"diatone_mamba-f405-mk2_default",
|
||||
"flywoo_gn-f405_default",
|
||||
"freefly_can-rtk-gps_canbootloader",
|
||||
"freefly_can-rtk-gps_default",
|
||||
"holybro_can-gps-v1_canbootloader",
|
||||
"holybro_can-gps-v1_default",
|
||||
"holybro_durandal-v1_default",
|
||||
"holybro_kakutef7_default",
|
||||
"holybro_kakuteh7_default",
|
||||
"holybro_kakuteh7mini_default",
|
||||
"holybro_kakuteh7v2_default",
|
||||
"holybro_pix32v5_default",
|
||||
"matek_gnss-m9n-f4_canbootloader",
|
||||
"matek_gnss-m9n-f4_default",
|
||||
"matek_h743-mini_default",
|
||||
"matek_h743-slim_default",
|
||||
"matek_h743_default",
|
||||
"micoair_h743_default",
|
||||
"micoair_h743-aio_default",
|
||||
"modalai_fc-v1_default",
|
||||
"modalai_fc-v2_default",
|
||||
"mro_ctrl-zero-classic_default",
|
||||
"mro_ctrl-zero-f7-oem_default",
|
||||
"mro_ctrl-zero-f7_default",
|
||||
"mro_ctrl-zero-h7-oem_default",
|
||||
"mro_ctrl-zero-h7_default",
|
||||
"mro_pixracerpro_default",
|
||||
"mro_x21-777_default",
|
||||
"mro_x21_default",
|
||||
"nxp_fmuk66-e_default",
|
||||
"nxp_fmuk66-e_socketcan",
|
||||
"nxp_fmuk66-v3_default",
|
||||
"nxp_fmuk66-v3_socketcan",
|
||||
"nxp_mr-canhubk3_default",
|
||||
"nxp_mr-canhubk3_fmu",
|
||||
"nxp_ucans32k146_canbootloader",
|
||||
"nxp_ucans32k146_default",
|
||||
"nxp_tropic-community_default",
|
||||
"omnibus_f4sd_default",
|
||||
"px4_fmu-v2_default",
|
||||
"px4_fmu-v2_fixedwing",
|
||||
"px4_fmu-v2_lto",
|
||||
"px4_fmu-v2_multicopter",
|
||||
"px4_fmu-v2_rover",
|
||||
"px4_fmu-v3_default",
|
||||
"px4_fmu-v4_default",
|
||||
"px4_fmu-v4pro_default",
|
||||
"px4_fmu-v5_cyphal",
|
||||
"px4_fmu-v5_debug",
|
||||
"px4_fmu-v5_default",
|
||||
"px4_fmu-v5_lto",
|
||||
"px4_fmu-v5_rover",
|
||||
"px4_fmu-v5_stackcheck",
|
||||
"px4_fmu-v5_uavcanv0periph",
|
||||
"px4_fmu-v5x_default",
|
||||
"px4_fmu-v5x_rover",
|
||||
"px4_fmu-v6c_default",
|
||||
"px4_fmu-v6c_rover",
|
||||
"px4_fmu-v6u_default",
|
||||
"px4_fmu-v6u_rover",
|
||||
"px4_fmu-v6x_default",
|
||||
"px4_fmu-v6x_rover",
|
||||
"px4_fmu-v6xrt_bootloader",
|
||||
"px4_fmu-v6xrt_default",
|
||||
"px4_fmu-v6xrt_rover",
|
||||
"px4_io-v2_default",
|
||||
"raspberrypi_pico_default",
|
||||
"siyi_n7_default",
|
||||
"sky-drones_smartap-airlink_default",
|
||||
"spracing_h7extreme_default",
|
||||
"thepeach_k1_default",
|
||||
"thepeach_r1_default",
|
||||
"uvify_core_default",
|
||||
"x-mav_ap-h743v2_default",
|
||||
"zeroone_x6_bootloader",
|
||||
"zeroone_x6_default",
|
||||
],
|
||||
image: docker_images.nuttx,
|
||||
archive: true
|
||||
]
|
||||
|
||||
def docker_builds = [
|
||||
armhf_builds, base_builds, nuttx_builds_archive
|
||||
]
|
||||
|
||||
for (def build_type = 0; build_type < docker_builds.size(); build_type++) {
|
||||
for (def build_target = 0; build_target < docker_builds[build_type].target.size(); build_target++) {
|
||||
build_nodes.put(docker_builds[build_type].target[build_target],
|
||||
createBuildNode(docker_builds[build_type].archive, docker_builds[build_type].image, docker_builds[build_type].target[build_target])
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
parallel build_nodes
|
||||
|
||||
} // script
|
||||
} // steps
|
||||
} // stage Build
|
||||
|
||||
// TODO: actually upload artifacts to S3
|
||||
// stage('S3 Upload') {
|
||||
// agent {
|
||||
// docker { image 'px4io/px4-dev-base-focal:2021-09-08' }
|
||||
// }
|
||||
// options {
|
||||
// skipDefaultCheckout()
|
||||
// }
|
||||
// when {
|
||||
// anyOf {
|
||||
// branch 'master'
|
||||
// branch 'beta'
|
||||
// branch 'stable'
|
||||
// branch 'pr-jenkins' // for testing
|
||||
// }
|
||||
// }
|
||||
// steps {
|
||||
// sh 'echo "uploading to S3"'
|
||||
// }
|
||||
// }
|
||||
|
||||
} // stages
|
||||
environment {
|
||||
CCACHE_DIR = '/tmp/ccache'
|
||||
CI = true
|
||||
}
|
||||
options {
|
||||
buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14'))
|
||||
timeout(time: 120, unit: 'MINUTES')
|
||||
}
|
||||
}
|
||||
|
||||
def createBuildNode(Boolean archive, String docker_image, String target) {
|
||||
return {
|
||||
|
||||
bypass_entrypoint = ''
|
||||
|
||||
node {
|
||||
docker.withRegistry('https://registry.hub.docker.com', 'docker_hub_dagar') {
|
||||
docker.image(docker_image).inside('-e CCACHE_BASEDIR=${WORKSPACE} -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + bypass_entrypoint) {
|
||||
stage(target) {
|
||||
try {
|
||||
sh('export')
|
||||
checkout(scm)
|
||||
sh('make distclean; git clean -ff -x -d .')
|
||||
sh('git fetch --tags')
|
||||
sh('ccache -s')
|
||||
sh('make ' + target)
|
||||
sh('ccache -s')
|
||||
sh('make sizes')
|
||||
if (archive) {
|
||||
archiveArtifacts(allowEmptyArchive: false, artifacts: 'build/*/*.px4, build/*/*.elf, build/*/*.bin', fingerprint: true, onlyIfSuccessful: true)
|
||||
}
|
||||
sh('make ' + target + ' package')
|
||||
archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/*/*.tar.bz2', fingerprint: true, onlyIfSuccessful: true)
|
||||
archiveArtifacts(allowEmptyArchive: true, artifacts: 'build/*/*.deb', fingerprint: true, onlyIfSuccessful: true)
|
||||
}
|
||||
catch (exc) {
|
||||
throw (exc)
|
||||
}
|
||||
finally {
|
||||
sh('make distclean; git clean -ff -x -d .')
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,904 +0,0 @@
|
||||
#!/usr/bin/env groovy
|
||||
|
||||
pipeline {
|
||||
agent none
|
||||
stages {
|
||||
stage('Hardware Test') {
|
||||
|
||||
parallel {
|
||||
|
||||
stage("cubepilot_cubeorange_test") {
|
||||
stages {
|
||||
stage("build cubepilot_cubeorange_test") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make cubepilot_cubeorange_bootloader'
|
||||
sh 'make cubepilot_cubeorange_test'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*, build/cubepilot_cubeorange_test/etc/init.d/airframes/*', name: 'cubepilot_cubeorange_test'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'cubepilot_cubeorange'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'cubepilot_cubeorange_test'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_bootloader/cubepilot_cubeorange_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
// run tests
|
||||
runTests()
|
||||
|
||||
// load all airframes
|
||||
// sh("./Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` `cd build/cubepilot_cubeorange_test/etc/init.d/airframes/; find . -regex '.*/[0-9].*' -exec basename {} \\; | cut -d '_' -f 1` || true") // test loading all airframes\
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"'
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
|
||||
checkStatus()
|
||||
quickCalibrate()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("cuav_x7pro_test") {
|
||||
stages {
|
||||
stage("build cuav_x7pro_test") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make cuav_x7pro_bootloader'
|
||||
sh 'make cuav_x7pro_test'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'cuav_x7pro_test'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'cuav_x7pro'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'cuav_x7pro_test'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_bootloader/cuav_x7pro_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
runTests()
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"'
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
|
||||
checkStatus()
|
||||
quickCalibrate()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("px4_fmu-v4_test") {
|
||||
stages {
|
||||
stage("build px4_fmu-v4_test") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make px4_fmu-v4_test'
|
||||
sh 'make px4_fmu-v4_test bootloader_elf'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4_test'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'px4_fmu-v4'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'px4_fmu-v4_test'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
runTests()
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
|
||||
checkStatus()
|
||||
quickCalibrate()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("px4_fmu-v4pro_test") {
|
||||
stages {
|
||||
stage("build px4_fmu-v4pro_test") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make px4_fmu-v4pro_test'
|
||||
sh 'make px4_fmu-v4pro_test bootloader_elf'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4pro_test'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'px4_fmu-v4pro'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'px4_fmu-v4pro_test'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
runTests()
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
|
||||
checkStatus()
|
||||
quickCalibrate()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("px4_fmu-v5_debug") {
|
||||
stages {
|
||||
stage("build px4_fmu-v5_debug") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make px4_fmu-v5_debug'
|
||||
sh 'make px4_fmu-v5_debug bootloader_elf'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_debug'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'px4_fmu-v5'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'px4_fmu-v5_debug'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600 || true'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"'
|
||||
|
||||
// test dataman
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic quadcopter
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
checkStatus()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("px4_fmu-v5_stackcheck") {
|
||||
stages {
|
||||
stage("build px4_fmu-v5_stackcheck") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make px4_fmu-v5_stackcheck'
|
||||
sh 'make px4_fmu-v5_stackcheck bootloader_elf'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_stackcheck'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'px4_fmu-v5'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'px4_fmu-v5_stackcheck'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
|
||||
|
||||
// test dataman
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors'
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic quadcopter
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
checkStatus()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("px4_fmu-v5_test") {
|
||||
stages {
|
||||
stage("build px4_fmu-v5_test") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make px4_fmu-v5_test'
|
||||
sh 'make px4_fmu-v5_test bootloader_elf'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_test'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'px4_fmu-v5'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'px4_fmu-v5_test'
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
runTests()
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
|
||||
checkStatus()
|
||||
quickCalibrate()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
stage("nxp_fmuk66-v3_test") {
|
||||
stages {
|
||||
stage("build nxp_fmuk66-v3_test") {
|
||||
agent {
|
||||
docker {
|
||||
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
|
||||
args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
|
||||
}
|
||||
}
|
||||
steps {
|
||||
checkoutSCM()
|
||||
sh 'make nxp_fmuk66-v3_test'
|
||||
//sh 'make nxp_fmuk66-v3_test bootloader_elf'
|
||||
sh 'ccache -s'
|
||||
stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'nxp_fmuk66-v3_test'
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
}
|
||||
}
|
||||
} // stage build
|
||||
stage("hardware") {
|
||||
agent {
|
||||
label 'nxp_fmuk66-v3'
|
||||
}
|
||||
stages {
|
||||
stage("flash") {
|
||||
steps {
|
||||
sh 'export'
|
||||
sh 'find /dev/serial'
|
||||
unstash 'nxp_fmuk66-v3_test'
|
||||
//sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_bootloader.elf'
|
||||
// flash board and watch bootup
|
||||
sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
|
||||
resetBoard()
|
||||
}
|
||||
}
|
||||
stage("tests") {
|
||||
steps {
|
||||
runTests()
|
||||
}
|
||||
}
|
||||
stage("status") {
|
||||
steps {
|
||||
// configure
|
||||
resetParameters()
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "400"'
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger
|
||||
checkStatus()
|
||||
quickCalibrate()
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger
|
||||
}
|
||||
}
|
||||
stage("print topics") {
|
||||
steps {
|
||||
printTopics()
|
||||
}
|
||||
}
|
||||
}
|
||||
post {
|
||||
always {
|
||||
sh 'cat /tmp/pyserial_spy_file.txt || true'
|
||||
sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
|
||||
}
|
||||
}
|
||||
} // stage test
|
||||
}
|
||||
}
|
||||
|
||||
} // parallel
|
||||
} // stage Hardware Test
|
||||
} // stages
|
||||
environment {
|
||||
CCACHE_DIR = '/tmp/ccache'
|
||||
CCACHE_NOHASHDIR = 1
|
||||
CI = true
|
||||
}
|
||||
options {
|
||||
buildDiscarder(logRotator(numToKeepStr: '30', artifactDaysToKeepStr: '60'))
|
||||
timeout(time: 180, unit: 'MINUTES')
|
||||
skipDefaultCheckout()
|
||||
}
|
||||
}
|
||||
|
||||
void checkoutSCM() {
|
||||
retry(3) {
|
||||
checkout scm
|
||||
sh 'export'
|
||||
sh 'make distclean; git clean -ff -x -d .'
|
||||
sh 'git fetch --tags'
|
||||
sh 'ccache -z'
|
||||
}
|
||||
}
|
||||
|
||||
void quickCalibrate() {
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters before
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
|
||||
}
|
||||
|
||||
void checkStatus() {
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
|
||||
|
||||
// status commands
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/meminfo"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/uptime"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander check" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ekf2 status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload; top once; listener cpuload"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /bin"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /dev"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /etc"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status streams" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf latency"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ps"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm_out status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
|
||||
}
|
||||
|
||||
void resetParameters() {
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param reset_all"'
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "CBRK_BUZZER" --value "782097"'
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SDLOG_DIRS_MAX" --value "1"'
|
||||
}
|
||||
|
||||
void runTests() {
|
||||
|
||||
// test loading a range of airframes
|
||||
sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001'
|
||||
|
||||
resetParameters()
|
||||
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_IMU_AUTOCAL" --value "0" || true' // disable during testing
|
||||
sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_MAG_AUTOCAL" --value "0" || true' // disable during testing
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ostest"'
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot after ostest
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander_tests" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "controllib_test"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "lightware_laser_test"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink_tests" || true' // TODO
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd readtest"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params" || true' // expected to fail after erase
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v"'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"'
|
||||
|
||||
// tests (stop modules first)
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander stop"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink stop-all"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "navigator stop"'
|
||||
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during microbenchmarks
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "microbench all"'
|
||||
|
||||
//sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "calib_udelay"'
|
||||
}
|
||||
|
||||
void printTopics() {
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
|
||||
|
||||
// these are for casually inspecting the system, output failure doesn't matter
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_wind" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_optical_flow_vel" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_states" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_wind" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener event" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener heater_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener input_rc" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener led_control" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener log_message" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener manual_control_setpoint" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mavlink_log" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mission" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener multirotor_motor_limits" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener optical_flow" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_controller_landing_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_setpoint_triplet" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener radio_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command_ack" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_control_mode" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_global_position" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_land_detected" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position_setpoint" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true'
|
||||
}
|
||||
|
||||
void resetBoard() {
|
||||
resetParameters()
|
||||
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true' // limit cpu usage
|
||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "echo > /fs/microsd/.format" || true'
|
||||
sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
|
||||
}
|
||||
@@ -6,11 +6,6 @@
|
||||
name: Build all targets
|
||||
|
||||
on:
|
||||
workflow_dispatch:
|
||||
inputs:
|
||||
tag:
|
||||
required: true
|
||||
description: release version
|
||||
push:
|
||||
tags:
|
||||
- 'v*'
|
||||
@@ -27,7 +22,7 @@ jobs:
|
||||
group_targets:
|
||||
name: Scan for Board Targets
|
||||
# runs-on: ubuntu-latest
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
outputs:
|
||||
matrix: ${{ steps.set-matrix.outputs.matrix }}
|
||||
timestamp: ${{ steps.set-timestamp.outputs.timestamp }}
|
||||
@@ -36,6 +31,10 @@ jobs:
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
|
||||
- name: Update python packaging to avoid canonicalize_version() error
|
||||
run: |
|
||||
pip3 install -U packaging
|
||||
|
||||
- name: Install Python Dependencies
|
||||
uses: py-actions/py-dependency-install@v4
|
||||
with:
|
||||
@@ -47,31 +46,20 @@ jobs:
|
||||
- id: set-timestamp
|
||||
run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")"
|
||||
|
||||
# This job is also triggered with versioned tags
|
||||
# Creating a and pushing a tag starting with "v" just as "v1.0.0"
|
||||
# will trigger this workflow and when all builds are done create a Github Release
|
||||
# then it will upload all binaries built as assets
|
||||
# Additionally, we can also trigger this step manually
|
||||
# From the Github Actions tab for this repository:
|
||||
# https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml
|
||||
# You can now click a "Run Workflow" button that will prompt you for a tag name
|
||||
# This tag name has to match an existing tag otherwise the new release will be detached
|
||||
# Note: Only developers with "write" permission to the repository can use this feature
|
||||
- id: set-tag
|
||||
if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch'
|
||||
run: echo "::set-output name=tagname::${{ github.event_name == 'workflow_dispatch' && inputs.tag || github.ref_name }}"
|
||||
|
||||
- id: set-branch
|
||||
run: echo "::set-output name=branchname::${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}"
|
||||
|
||||
- name: Debug Matrix Output
|
||||
if: runner.debug == '1'
|
||||
run: echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)"
|
||||
run: |
|
||||
echo "${{ steps.set-timestamp.outputs.timestamp }}"
|
||||
echo "${{ steps.set-branch.outputs.branchname }}"
|
||||
echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)"
|
||||
|
||||
setup:
|
||||
name: Build Group [${{ matrix.group }}]
|
||||
# runs-on: ubuntu-latest
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
needs: group_targets
|
||||
strategy:
|
||||
matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }}
|
||||
@@ -124,7 +112,7 @@ jobs:
|
||||
artifacts:
|
||||
name: Upload Artifacts to S3
|
||||
# runs-on: ubuntu-latest
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
needs: [setup, group_targets]
|
||||
if: contains(fromJSON('["main", "stable", "beta"]'), needs.group_targets.outputs.branchname)
|
||||
steps:
|
||||
@@ -153,22 +141,18 @@ jobs:
|
||||
release:
|
||||
name: Create Release and Upload Artifacts
|
||||
# runs-on: ubuntu-latest
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
needs: [setup, group_targets]
|
||||
if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch'
|
||||
if: startsWith(github.ref, 'refs/tags/v')
|
||||
steps:
|
||||
- name: Download Artifacts
|
||||
uses: actions/download-artifact@v4
|
||||
|
||||
- name: Arrange Binaries
|
||||
run: |
|
||||
mkdir artifacts
|
||||
cp **/**/*.px4 artifacts/
|
||||
with:
|
||||
path: artifacts/
|
||||
merge-multiple: true
|
||||
|
||||
- name: Upload Binaries to Release
|
||||
uses: softprops/action-gh-release@v2
|
||||
with:
|
||||
name: ${{ needs.group_targets.outputs.tagname }}
|
||||
tag_name: ${{ needs.group_targets.outputs.tagname }}
|
||||
draft: true
|
||||
files: artifacts/*.px4
|
||||
|
||||
@@ -17,5 +17,7 @@ jobs:
|
||||
with:
|
||||
token: ${{secrets.ACCESS_TOKEN}}
|
||||
|
||||
- name: make clang-tidy-quiet
|
||||
run: make clang-tidy-quiet
|
||||
- uses: corrupt952/actions-retry-command@v1.0.7
|
||||
with:
|
||||
command: make clang-tidy-quiet
|
||||
max_attempts: 3
|
||||
|
||||
@@ -18,7 +18,7 @@ jobs:
|
||||
fail-fast: false
|
||||
matrix:
|
||||
version: ['ubuntu:22.04', 'ubuntu:24.04']
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}"]
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}",spot=false]
|
||||
container:
|
||||
image: ${{ matrix.version }}
|
||||
volumes:
|
||||
|
||||
@@ -8,38 +8,95 @@ on:
|
||||
- 'stable'
|
||||
- 'beta'
|
||||
- 'release/**'
|
||||
tags:
|
||||
- 'v*'
|
||||
pull_request:
|
||||
branches:
|
||||
- '*'
|
||||
|
||||
jobs:
|
||||
build:
|
||||
name: Build Container
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}"]
|
||||
name: Build and Push Container
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
steps:
|
||||
|
||||
- uses: actions/checkout@v4
|
||||
with:
|
||||
fetch-tags: true
|
||||
submodules: false
|
||||
fetch-depth: 0
|
||||
|
||||
- name: Login to Docker Hub
|
||||
uses: docker/login-action@v3
|
||||
with:
|
||||
username: ${{ secrets.DOCKERHUB_USERNAME }}
|
||||
password: ${{ secrets.DOCKERHUB_TOKEN }}
|
||||
|
||||
- name: Login to GitHub Container Registry
|
||||
uses: docker/login-action@v3
|
||||
with:
|
||||
registry: ghcr.io
|
||||
username: ${{ github.actor }}
|
||||
password: ${{ secrets.GITHUB_TOKEN }}
|
||||
|
||||
- name: Extract metadata (tags, labels) for Docker
|
||||
id: meta
|
||||
uses: docker/metadata-action@v5
|
||||
with:
|
||||
images: |
|
||||
ghcr.io/PX4/px4-dev
|
||||
px4io/px4-dev
|
||||
tags: |
|
||||
type=schedule
|
||||
type=semver,pattern={{version}}
|
||||
type=semver,pattern={{major}}.{{minor}}
|
||||
type=semver,pattern={{major}}
|
||||
type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600
|
||||
type=ref,event=branch,suffix=,priority=500
|
||||
type=ref,event=pr
|
||||
type=sha
|
||||
|
||||
- name: Set up Docker Buildx
|
||||
uses: docker/setup-buildx-action@v3
|
||||
|
||||
- name: Build and load container image
|
||||
uses: docker/build-push-action@v6
|
||||
id: docker
|
||||
with:
|
||||
context: Tools/setup
|
||||
tags: px4-dev:latest
|
||||
tags: ${{ steps.meta.outputs.tags }}
|
||||
labels: ${{ steps.meta.outputs.labels }}
|
||||
platforms: |
|
||||
linux/amd64
|
||||
load: true
|
||||
push: false
|
||||
cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }}
|
||||
cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max
|
||||
|
||||
- name: Get Tag Name
|
||||
id: tag_name
|
||||
run: |
|
||||
echo "::set-output name=tag_name::$(echo '${{ fromJSON(steps.docker.outputs.metadata)['image.name'] }}' | sed 's/,.*$//')"
|
||||
|
||||
- name: make quick_check
|
||||
uses: addnab/docker-run-action@v3
|
||||
with:
|
||||
image: px4-dev:latest
|
||||
image: ${{ steps.tag_name.outputs.tag_name }}
|
||||
options: -v ${{ github.workspace }}:/workspace
|
||||
run: |
|
||||
cd /workspace
|
||||
git config --global --add safe.directory /workspace
|
||||
make px4_sitl_default
|
||||
make px4_fmu-v6x_default
|
||||
|
||||
- name: Push container image
|
||||
uses: docker/build-push-action@v6
|
||||
with:
|
||||
context: Tools/setup
|
||||
tags: ${{ steps.meta.outputs.tags }}
|
||||
labels: ${{ steps.meta.outputs.labels }}
|
||||
platforms: |
|
||||
linux/amd64
|
||||
provenance: mode=max
|
||||
push: true
|
||||
cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }}
|
||||
cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max
|
||||
|
||||
@@ -10,8 +10,8 @@ on:
|
||||
|
||||
jobs:
|
||||
analyze_flash:
|
||||
name: FLASH usage analysis
|
||||
runs-on: ubuntu-latest
|
||||
name: Analyzing ${{ matrix.target }}
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
container:
|
||||
image: px4io/px4-dev-nuttx-focal
|
||||
strategy:
|
||||
@@ -29,7 +29,7 @@ jobs:
|
||||
- name: Git ownership workaround
|
||||
run: git config --system --add safe.directory '*'
|
||||
|
||||
- name: Build
|
||||
- name: Build Target
|
||||
run: make ${{ matrix.target }}
|
||||
|
||||
- name: Store the ELF with the change
|
||||
@@ -73,36 +73,46 @@ jobs:
|
||||
echo "$EOF" >> $GITHUB_OUTPUT
|
||||
|
||||
post_pr_comment:
|
||||
name: Post PR comment
|
||||
runs-on: ubuntu-latest
|
||||
name: Publish Results
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
needs: [analyze_flash]
|
||||
if: ${{ github.event.pull_request }}
|
||||
steps:
|
||||
- name: If it's a PR add a comment with the bloaty output
|
||||
if: ${{ github.event.pull_request }}
|
||||
uses: actions/github-script@v6
|
||||
- name: Find Comment
|
||||
uses: peter-evans/find-comment@v3
|
||||
id: fc
|
||||
with:
|
||||
script: |
|
||||
const comment = [
|
||||
'## FLASH Analysis',
|
||||
'<details>',
|
||||
'<summary>px4_fmu-v5x</summary>',
|
||||
'',
|
||||
'```',
|
||||
`${{ needs.analyze_flash.outputs.px4_fmu-v5x }}`,
|
||||
'```',
|
||||
'</details>',
|
||||
'',
|
||||
'<details>',
|
||||
'<summary>px4_fmu-v6x</summary>',
|
||||
'',
|
||||
'```',
|
||||
`${{ needs.analyze_flash.outputs.px4_fmu-v6x }}`,
|
||||
'```',
|
||||
'</details>'
|
||||
]
|
||||
github.rest.issues.createComment({
|
||||
issue_number: context.issue.number,
|
||||
owner: context.repo.owner,
|
||||
repo: context.repo.repo,
|
||||
body: comment.join('\n')
|
||||
})
|
||||
issue-number: ${{ github.event.pull_request.number }}
|
||||
comment-author: 'github-actions[bot]'
|
||||
body-includes: FLASH Analysis
|
||||
|
||||
- name: Set Build Time
|
||||
id: bt
|
||||
run: |
|
||||
echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT
|
||||
|
||||
- name: Create or update comment
|
||||
uses: peter-evans/create-or-update-comment@v4
|
||||
with:
|
||||
comment-id: ${{ steps.fc.outputs.comment-id }}
|
||||
issue-number: ${{ github.event.pull_request.number }}
|
||||
body: |
|
||||
## FLASH Analysis
|
||||
<details>
|
||||
<summary>px4_fmu-v5x</summary>
|
||||
|
||||
```
|
||||
${{ needs.analyze_flash.outputs.px4_fmu-v5x }}
|
||||
```
|
||||
</details>
|
||||
|
||||
<details>
|
||||
<summary>px4_fmu-v6x</summary>
|
||||
|
||||
```
|
||||
${{ needs.analyze_flash.outputs.px4_fmu-v6x }}
|
||||
```
|
||||
</details>
|
||||
|
||||
**Updated: _${{ steps.bt.outputs.timestamp }}_**
|
||||
edit-mode: replace
|
||||
|
||||
@@ -16,7 +16,7 @@ on:
|
||||
jobs:
|
||||
build:
|
||||
name: Testing PX4 ${{ matrix.config.model }}
|
||||
runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
|
||||
runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
container:
|
||||
image: px4io/px4-dev-simulation-focal:2021-09-08
|
||||
options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
|
||||
|
||||
+11
-9
@@ -173,9 +173,10 @@ set(config_module_list)
|
||||
set(config_kernel_list)
|
||||
|
||||
# Find Python
|
||||
find_package(PythonInterp 3)
|
||||
find_package(Python COMPONENTS Interpreter)
|
||||
|
||||
# We have a custom error message to tell users how to install python3.
|
||||
if(NOT PYTHONINTERP_FOUND)
|
||||
if(NOT Python_FOUND)
|
||||
message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n"
|
||||
" Ubuntu: sudo apt install python3 python3-dev python3-pip\n"
|
||||
" macOS: brew install python")
|
||||
@@ -184,7 +185,7 @@ endif()
|
||||
option(PYTHON_COVERAGE "Python code coverage" OFF)
|
||||
if(PYTHON_COVERAGE)
|
||||
message(STATUS "python coverage enabled")
|
||||
set(PYTHON_EXECUTABLE coverage run -p)
|
||||
set(Python_EXECUTABLE coverage run -p)
|
||||
endif()
|
||||
|
||||
include(px4_config)
|
||||
@@ -408,14 +409,15 @@ include(px4_add_gtest)
|
||||
if(BUILD_TESTING)
|
||||
include(gtest)
|
||||
|
||||
set(TESTFILTERARG "")
|
||||
|
||||
# Ensure there's no -R without any filter expression since that trips newer ctest versions
|
||||
if(TESTFILTER)
|
||||
set(TESTFILTERARG "-R ${TESTFILTER}")
|
||||
set(TESTFILTERARG "-R")
|
||||
else()
|
||||
set(TESTFILTERARG "")
|
||||
endif()
|
||||
|
||||
add_custom_target(test_results
|
||||
COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test ${TESTFILTERARG}
|
||||
COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test ${TESTFILTERARG} ${TESTFILTER}
|
||||
DEPENDS
|
||||
px4
|
||||
examples__dyn_hello
|
||||
@@ -480,7 +482,7 @@ foreach(module ${config_module_list})
|
||||
endforeach()
|
||||
|
||||
add_custom_command(OUTPUT ${uorb_graph_config}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py
|
||||
${graph_module_list} --src-path src/lib
|
||||
--merge-depends
|
||||
--exclude-path src/examples
|
||||
@@ -501,7 +503,7 @@ include(package)
|
||||
|
||||
# install python requirements using configured python
|
||||
add_custom_target(install_python_requirements
|
||||
COMMAND ${PYTHON_EXECUTABLE} -m pip install --break-system-packages --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
|
||||
COMMAND ${Python_EXECUTABLE} -m pip install --break-system-packages --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
|
||||
DEPENDS ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
@@ -169,7 +169,7 @@ endif
|
||||
|
||||
# Pick up specific Python path if set
|
||||
ifdef PYTHON_EXECUTABLE
|
||||
override CMAKE_ARGS += -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
|
||||
override CMAKE_ARGS += -DPython_EXECUTABLE=${PYTHON_EXECUTABLE}
|
||||
endif
|
||||
|
||||
# Functions
|
||||
|
||||
@@ -104,7 +104,7 @@ These boards fully comply with Pixhawk Standard, and are maintained by the PX4-A
|
||||
|
||||
These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.
|
||||
|
||||
* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html)
|
||||
* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/ark_v6x.html)
|
||||
* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html)
|
||||
* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
|
||||
* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
|
||||
|
||||
@@ -124,15 +124,15 @@ add_custom_command(
|
||||
${romfs_copy_stamp}
|
||||
COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/*
|
||||
COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
--airframes-path ${romfs_gen_root_dir}/init.d
|
||||
--start-script ${romfs_gen_root_dir}/init.d/rc.autostart
|
||||
--board ${PX4_BOARD}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
|
||||
--rc-dir ${romfs_gen_root_dir}/init.d
|
||||
--serial-ports ${board_serial_ports} ${added_arguments}
|
||||
--config-files ${module_config_files} #--verbose
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/filepaths/generate_config.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/filepaths/generate_config.py
|
||||
--rc-dir ${romfs_gen_root_dir}/init.d
|
||||
--params-file ${CONFIG_BOARD_PARAM_FILE}
|
||||
COMMAND ${CMAKE_COMMAND} -E touch ${romfs_copy_stamp}
|
||||
@@ -330,7 +330,7 @@ add_custom_target(romfs_gen_files_target
|
||||
|
||||
add_custom_command(
|
||||
OUTPUT romfs_pruned.stamp
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_romfs_pruner.py --folder ${romfs_gen_root_dir} --board ${PX4_BOARD}
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_romfs_pruner.py --folder ${romfs_gen_root_dir} --board ${PX4_BOARD}
|
||||
COMMAND ${CMAKE_COMMAND} -E touch romfs_pruned.stamp
|
||||
DEPENDS
|
||||
romfs_copy.stamp
|
||||
|
||||
@@ -24,33 +24,33 @@ param set-default FW_LND_ANG 8
|
||||
param set-default NPFG_PERIOD 12
|
||||
|
||||
param set-default FW_PR_P 0.9
|
||||
param set-default FW_PR_FF 0.5
|
||||
param set-default FW_PR_FF 0.2
|
||||
param set-default FW_PR_I 0.5
|
||||
param set-default TRIM_PITCH -0.15
|
||||
|
||||
param set-default FW_PSP_OFF 2
|
||||
param set-default FW_P_LIM_MIN -15
|
||||
|
||||
param set-default FW_RR_FF 0.5
|
||||
param set-default FW_RR_P 0.3
|
||||
param set-default FW_RR_I 0.5
|
||||
param set-default FW_RR_P 0.4
|
||||
param set-default FW_RR_I 0.7
|
||||
|
||||
param set-default FW_YR_FF 0.5
|
||||
param set-default FW_YR_P 0.6
|
||||
param set-default FW_YR_I 0.5
|
||||
param set-default FW_R_RMAX 56.15
|
||||
|
||||
param set-default FW_YR_FF 0.3
|
||||
param set-default FW_YR_P 1.3
|
||||
param set-default FW_YR_I 0.7
|
||||
|
||||
param set-default FW_PSP_OFF 0
|
||||
param set-default FW_P_LIM_MIN -15
|
||||
|
||||
param set-default FW_SPOILERS_LND 0.4
|
||||
|
||||
param set-default FW_THR_MAX 0.6
|
||||
param set-default FW_THR_MIN 0.05
|
||||
param set-default FW_THR_TRIM 0.25
|
||||
|
||||
param set-default FW_T_CLMB_MAX 8
|
||||
param set-default FW_T_SINK_MAX 2.7
|
||||
param set-default FW_T_SINK_MIN 2.2
|
||||
param set-default FW_T_CLMB_MAX 5
|
||||
param set-default FW_T_SINK_MAX 3.5
|
||||
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
|
||||
|
||||
@@ -75,7 +75,6 @@ param set-default MPC_Z_VEL_P_ACC 5
|
||||
param set-default MPC_Z_VEL_I_ACC 3
|
||||
param set-default MPC_LAND_ALT1 3
|
||||
param set-default MPC_LAND_ALT2 1
|
||||
param set-default MPC_POS_MODE 3
|
||||
param set-default CP_GO_NO_DATA 1
|
||||
|
||||
# Navigator Parameters
|
||||
|
||||
@@ -190,11 +190,11 @@ if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
|
||||
echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
|
||||
param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
|
||||
|
||||
COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
|
||||
COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc)
|
||||
echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
|
||||
param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
|
||||
|
||||
COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc)
|
||||
COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc)
|
||||
echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
|
||||
param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
|
||||
|
||||
|
||||
@@ -67,7 +67,6 @@ param set-default MPC_Z_VEL_P_ACC 5
|
||||
param set-default MPC_Z_VEL_I_ACC 3
|
||||
param set-default MPC_LAND_ALT1 3
|
||||
param set-default MPC_LAND_ALT2 1
|
||||
param set-default MPC_POS_MODE 3
|
||||
param set-default CP_GO_NO_DATA 1
|
||||
|
||||
# Navigator Parameters
|
||||
|
||||
@@ -68,7 +68,6 @@ param set-default MPC_Z_VEL_P_ACC 5
|
||||
param set-default MPC_Z_VEL_I_ACC 3
|
||||
param set-default MPC_LAND_ALT1 3
|
||||
param set-default MPC_LAND_ALT2 1
|
||||
param set-default MPC_POS_MODE 3
|
||||
param set-default CP_GO_NO_DATA 1
|
||||
|
||||
# Navigator Parameters
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
argcomplete
|
||||
cerberus
|
||||
coverage
|
||||
empy==3.3.4
|
||||
empy>=3.3,<4
|
||||
future
|
||||
jinja2>=2.8
|
||||
jsonschema
|
||||
|
||||
@@ -64,6 +64,7 @@ sudo apt-get update -y --quiet
|
||||
sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
|
||||
astyle \
|
||||
build-essential \
|
||||
ccache \
|
||||
cmake \
|
||||
cppcheck \
|
||||
file \
|
||||
|
||||
@@ -1,29 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
EXEDIR=`pwd`
|
||||
BASEDIR=$(dirname $0)
|
||||
|
||||
SYSTYPE=`uname -s`
|
||||
|
||||
#
|
||||
# Serial port defaults.
|
||||
#
|
||||
# XXX The uploader should be smarter than this.
|
||||
#
|
||||
if [ $SYSTYPE = "Darwin" ]; then
|
||||
SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
|
||||
fi
|
||||
|
||||
if [ $SYSTYPE = "Linux" ]; then
|
||||
SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/usb-ARK*,"
|
||||
fi
|
||||
|
||||
if [[ $SYSTYPE = *"CYGWIN"* ]]; then
|
||||
SERIAL_PORTS="/dev/ttyS*"
|
||||
fi
|
||||
|
||||
if [ $SYSTYPE = "" ]; then
|
||||
SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"
|
||||
fi
|
||||
|
||||
python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1
|
||||
Binary file not shown.
@@ -31,6 +31,10 @@ CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_FW_POS_CONTROL=y
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
|
||||
Binary file not shown.
@@ -69,7 +69,7 @@
|
||||
|
||||
//#define USE_VBUS_PULL_DOWN
|
||||
#define INTERFACE_USART 1
|
||||
#define INTERFACE_USART_CONFIG "/dev/ttyS6,921600"
|
||||
#define INTERFACE_USART_CONFIG "/dev/ttyS0,921600"
|
||||
#define BOOT_DELAY_ADDRESS 0x000001a0
|
||||
#define BOARD_TYPE 59
|
||||
#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880)
|
||||
|
||||
@@ -95,7 +95,7 @@
|
||||
/* Define GPIO pins used as ADC N.B. Channel numbers must match below */
|
||||
/* Define Channel numbers must match above GPIO pin IN(n)*/
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL ADC12_CH(4)
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(5)
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(8)
|
||||
|
||||
#define ADC_CHANNELS \
|
||||
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
|
||||
|
||||
@@ -20,10 +20,10 @@ CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_COMMON_RC=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_TAP_ESC=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_INTERFACES=1
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
|
||||
Submodule boards/modalai/voxl2/libfc-sensor-api updated: ca16e99074...85151aaf6b
@@ -35,7 +35,7 @@
|
||||
set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.hex)
|
||||
|
||||
add_custom_target(upload_teensy
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/teensy_uploader.py --port ${serial_ports} ${PX4_FW_NAME} --vendor-id 0x1FC9 --product-id 0x0024
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/teensy_uploader.py --port ${serial_ports} ${PX4_FW_NAME} --vendor-id 0x1FC9 --product-id 0x0024
|
||||
DEPENDS ${PX4_FW_NAME}
|
||||
COMMENT "uploading px4"
|
||||
VERBATIM
|
||||
|
||||
+12
-12
@@ -1,20 +1,20 @@
|
||||
set(BOARD_DEFCONFIG ${PX4_CONFIG_FILE} CACHE FILEPATH "path to defconfig" FORCE)
|
||||
set(BOARD_CONFIG ${PX4_BINARY_DIR}/boardconfig CACHE FILEPATH "path to config" FORCE)
|
||||
|
||||
execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import menuconfig" RESULT_VARIABLE ret)
|
||||
execute_process(COMMAND ${Python_EXECUTABLE} -c "import menuconfig" RESULT_VARIABLE ret)
|
||||
if(ret EQUAL "1")
|
||||
message(FATAL_ERROR "kconfiglib is not installed or not in PATH\n"
|
||||
"please install using \"pip3 install kconfiglib\"\n")
|
||||
endif()
|
||||
|
||||
set(MENUCONFIG_PATH ${PYTHON_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfig program" FORCE)
|
||||
set(GUICONFIG_PATH ${PYTHON_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE)
|
||||
set(DEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE)
|
||||
set(SAVEDEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE)
|
||||
set(GENCONFIG_PATH ${PYTHON_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE)
|
||||
set(MENUCONFIG_PATH ${Python_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfig program" FORCE)
|
||||
set(GUICONFIG_PATH ${Python_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE)
|
||||
set(DEFCONFIG_PATH ${Python_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE)
|
||||
set(SAVEDEFCONFIG_PATH ${Python_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE)
|
||||
set(GENCONFIG_PATH ${Python_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE)
|
||||
|
||||
set(COMMON_KCONFIG_ENV_SETTINGS
|
||||
PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
|
||||
Python_EXECUTABLE=${Python_EXECUTABLE}
|
||||
KCONFIG_CONFIG=${BOARD_CONFIG}
|
||||
# Set environment variables so that Kconfig can prune Kconfig source
|
||||
# files for other architectures
|
||||
@@ -47,7 +47,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
|
||||
# Generate boardconfig from default.px4board and {label}.px4board
|
||||
execute_process(
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
|
||||
${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/merge_config.py Kconfig ${BOARD_CONFIG} ${PX4_BOARD_DIR}/default.px4board ${BOARD_DEFCONFIG}
|
||||
${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/merge_config.py Kconfig ${BOARD_CONFIG} ${PX4_BOARD_DIR}/default.px4board ${BOARD_DEFCONFIG}
|
||||
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
|
||||
OUTPUT_VARIABLE DUMMY_RESULTS
|
||||
)
|
||||
@@ -57,7 +57,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
|
||||
message(AUTHOR_WARNING "allyes build: allyes is for CI coverage and not for use in production")
|
||||
execute_process(
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
|
||||
${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/allyesconfig.py
|
||||
${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/allyesconfig.py
|
||||
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
|
||||
)
|
||||
endif()
|
||||
@@ -452,7 +452,7 @@ elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which is
|
||||
add_custom_target(boardconfig
|
||||
${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${MENUCONFIG_PATH} Kconfig
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH}
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
|
||||
COMMAND ${CMAKE_COMMAND} -E remove defconfig
|
||||
COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp
|
||||
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
|
||||
@@ -463,7 +463,7 @@ elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which is
|
||||
add_custom_target(boardguiconfig
|
||||
${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${GUICONFIG_PATH} Kconfig
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH}
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
|
||||
COMMAND ${CMAKE_COMMAND} -E remove defconfig
|
||||
COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp
|
||||
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
|
||||
@@ -473,7 +473,7 @@ elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which is
|
||||
|
||||
add_custom_target(px4_savedefconfig
|
||||
${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH}
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
|
||||
COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG}
|
||||
COMMAND ${CMAKE_COMMAND} -E remove defconfig
|
||||
COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp
|
||||
WORKING_DIRECTORY ${PX4_SOURCE_DIR}
|
||||
|
||||
+12
-12
@@ -35,10 +35,10 @@
|
||||
|
||||
add_custom_target(metadata_airframes
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
-v -a ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
|
||||
--markdown ${PX4_BINARY_DIR}/docs/airframes.md
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
-v -a ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d
|
||||
--xml ${PX4_BINARY_DIR}/docs/airframes.xml
|
||||
COMMENT "Generating full airframe metadata (markdown and xml)"
|
||||
@@ -58,28 +58,28 @@ add_custom_target(metadata_parameters
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_params_dir}
|
||||
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
|
||||
--all-ports --ethernet --params-file ${generated_params_dir}/serial_params.c --config-files ${yaml_config_files}
|
||||
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
|
||||
--params-file ${generated_params_dir}/module_params.c
|
||||
--timer-config ${PX4_SOURCE_DIR}/boards/px4/fmu-v5/src/timer_config.cpp # select a typical board
|
||||
--board-with-io
|
||||
--ethernet
|
||||
--config-files ${yaml_config_files} #--verbose
|
||||
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
|
||||
--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
|
||||
--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
|
||||
--markdown ${PX4_BINARY_DIR}/docs/parameters.md
|
||||
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
|
||||
--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
|
||||
--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
|
||||
--json ${PX4_BINARY_DIR}/docs/parameters.json
|
||||
--compress
|
||||
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py
|
||||
--src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir}
|
||||
--inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml
|
||||
--xml ${PX4_BINARY_DIR}/docs/parameters.xml
|
||||
@@ -90,7 +90,7 @@ add_custom_target(metadata_parameters
|
||||
|
||||
add_custom_target(metadata_module_documentation
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_module_doc.py -v --src-path ${PX4_SOURCE_DIR}/src
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_module_doc.py -v --src-path ${PX4_SOURCE_DIR}/src
|
||||
--markdown ${PX4_BINARY_DIR}/docs/modules
|
||||
COMMENT "Generating module documentation"
|
||||
USES_TERMINAL
|
||||
@@ -99,17 +99,17 @@ add_custom_target(metadata_module_documentation
|
||||
set(events_src_path "${PX4_SOURCE_DIR}/src/lib/events")
|
||||
add_custom_target(metadata_extract_events
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/events
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
|
||||
--src-path ${PX4_SOURCE_DIR}/src
|
||||
--json ${PX4_BINARY_DIR}/events/px4_full.json #--verbose
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${events_src_path}/libevents/scripts/combine.py
|
||||
COMMAND ${Python_EXECUTABLE} ${events_src_path}/libevents/scripts/combine.py
|
||||
${PX4_BINARY_DIR}/events/px4_full.json
|
||||
${events_src_path}/libevents/events/common.json
|
||||
${events_src_path}/enums.json
|
||||
--output ${PX4_BINARY_DIR}/events/all_events_full.json
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${events_src_path}/libevents/scripts/validate.py
|
||||
COMMAND ${Python_EXECUTABLE} ${events_src_path}/libevents/scripts/validate.py
|
||||
${PX4_BINARY_DIR}/events/all_events_full.json
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
|
||||
${PX4_BINARY_DIR}/events/all_events_full.json
|
||||
COMMENT "Extracting events from full source"
|
||||
USES_TERMINAL
|
||||
|
||||
@@ -67,7 +67,7 @@ function(px4_generate_airframes_xml)
|
||||
ARGN ${ARGN})
|
||||
|
||||
add_custom_command(OUTPUT ${PX4_BINARY_DIR}/airframes.xml
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_airframes.py
|
||||
--airframes-path ${PX4_SOURCE_DIR}/ROMFS/${config_romfs_root}/init.d
|
||||
--board CONFIG_ARCH_BOARD_${PX4_BOARD}
|
||||
--xml ${PX4_BINARY_DIR}/airframes.xml
|
||||
|
||||
+9
-9
@@ -312,7 +312,7 @@ add_custom_command(
|
||||
OUTPUT
|
||||
${uorb_headers}
|
||||
${msg_out_path}/uORBTopics.hpp
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
--headers
|
||||
-f ${msg_files}
|
||||
-i ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
@@ -333,7 +333,7 @@ add_custom_target(uorb_headers DEPENDS ${uorb_headers})
|
||||
add_custom_command(
|
||||
OUTPUT
|
||||
${uorb_json_files}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
--json
|
||||
-f ${msg_files}
|
||||
-i ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
@@ -356,7 +356,7 @@ add_custom_command(
|
||||
OUTPUT
|
||||
${uorb_message_fields_cpp_file}
|
||||
${uorb_message_fields_header_file}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_compressed_fields.py
|
||||
-f ${uorb_json_files}
|
||||
--source-output-file ${uorb_message_fields_cpp_file}
|
||||
--header-output-file ${uorb_message_fields_header_file}
|
||||
@@ -371,7 +371,7 @@ add_custom_command(
|
||||
# Generate microcdr headers
|
||||
add_custom_command(
|
||||
OUTPUT ${uorb_ucdr_headers}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
--headers
|
||||
-f ${msg_files}
|
||||
-i ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
@@ -393,7 +393,7 @@ add_custom_command(
|
||||
OUTPUT
|
||||
${uorb_sources}
|
||||
${msg_source_out_path}/uORBTopics.cpp
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
--sources
|
||||
-f ${msg_files}
|
||||
-i ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
@@ -459,7 +459,7 @@ if(CONFIG_LIB_CDRSTREAM)
|
||||
OUTPUT ${uorb_cdr_idl}
|
||||
COMMAND ${CMAKE_COMMAND}
|
||||
-E env "PYTHONPATH=${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_adapter:${PX4_SOURCE_DIR}/src/lib/cdrstream/rosidl/rosidl_cli"
|
||||
${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py
|
||||
${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/cdrstream/msg2idl.py
|
||||
${uorb_cdr_msg}
|
||||
DEPENDS
|
||||
${uorb_cdr_msg}
|
||||
@@ -489,7 +489,7 @@ if(CONFIG_LIB_CDRSTREAM)
|
||||
|
||||
add_custom_target(
|
||||
uorb_idl_header
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py
|
||||
--uorb-idl-header
|
||||
-f ${msg_files}
|
||||
-i ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
@@ -513,7 +513,7 @@ endif()
|
||||
|
||||
if(CONFIG_MODULES_ZENOH)
|
||||
# Update kconfig file for topics
|
||||
execute_process(COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
|
||||
execute_process(COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
|
||||
--zenoh-config
|
||||
-f ${msg_files}
|
||||
-o ${PX4_SOURCE_DIR}/src/modules/zenoh/
|
||||
@@ -522,7 +522,7 @@ if(CONFIG_MODULES_ZENOH)
|
||||
add_custom_command(
|
||||
OUTPUT
|
||||
${PX4_BINARY_DIR}/src/modules/zenoh/uorb_pubsub_factory.hpp
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/zenoh/px_generate_zenoh_topic_files.py
|
||||
--zenoh-pub-sub
|
||||
-f ${msg_files}
|
||||
-o ${PX4_BINARY_DIR}/src/modules/zenoh/
|
||||
|
||||
@@ -7,7 +7,7 @@ uint32 device_id
|
||||
|
||||
uint64 time_last_fuse
|
||||
|
||||
float32[2] observation
|
||||
float64[2] observation
|
||||
float32[2] observation_variance
|
||||
|
||||
float32[2] innovation
|
||||
|
||||
@@ -87,7 +87,12 @@ const px4_spi_bus_t *px4_spi_buses{nullptr};
|
||||
|
||||
int px4_find_spi_bus(uint32_t devid)
|
||||
{
|
||||
// px4_spi_buses is only NULL on certain targets depending on defines
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Waddress"
|
||||
|
||||
for (int i = 0; ((px4_spi_bus_t *) px4_spi_buses) != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
|
||||
#pragma GCC diagnostic pop
|
||||
const px4_spi_bus_t &bus_data = px4_spi_buses[i];
|
||||
|
||||
if (bus_data.bus == -1) {
|
||||
|
||||
@@ -146,7 +146,14 @@ int uORBTest::UnitTest::pubsublatency_main()
|
||||
|
||||
pubsubtest_passed = true;
|
||||
|
||||
if (mean > 150.0f) {
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
|
||||
// relaxed on SITL (non-realtime)
|
||||
const float kMaxMeanUs = 1000.f; // 1000 microseconds
|
||||
#else
|
||||
const float kMaxMeanUs = 150.f; // 150 microseconds
|
||||
#endif
|
||||
|
||||
if (mean > kMaxMeanUs) {
|
||||
pubsubtest_res = PX4_ERROR;
|
||||
|
||||
} else {
|
||||
|
||||
@@ -328,7 +328,7 @@ if (TARGET parameters_xml AND TARGET airframes_xml)
|
||||
add_custom_command(
|
||||
OUTPUT ${fw_package}
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
|
||||
${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_mkfw.py
|
||||
--prototype ${PX4_SOURCE_DIR}/boards/${PX4_BOARD_VENDOR}/${PX4_BOARD_MODEL}/firmware.prototype
|
||||
--git_identity ${PX4_SOURCE_DIR}
|
||||
--parameter_xml ${PX4_BINARY_DIR}/parameters.xml
|
||||
|
||||
@@ -6,13 +6,13 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
|
||||
export APPSDIR="`pwd`/../apps"
|
||||
|
||||
if [ "command -v python3" ]; then
|
||||
PYTHON_EXECUTABLE=python3
|
||||
Python_EXECUTABLE=python3
|
||||
else
|
||||
PYTHON_EXECUTABLE=python
|
||||
Python_EXECUTABLE=python
|
||||
fi
|
||||
|
||||
if [ "${1}" = "--olddefconfig" ]; then
|
||||
PYTHONPATH=${DIR} ${PYTHON_EXECUTABLE} ${DIR}/olddefconfig.py > /dev/null
|
||||
PYTHONPATH=${DIR} ${Python_EXECUTABLE} ${DIR}/olddefconfig.py > /dev/null
|
||||
else
|
||||
echo "ERROR: ${@} unsupported"
|
||||
exit 1
|
||||
|
||||
@@ -42,12 +42,14 @@ set(serial_ports)
|
||||
if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux")
|
||||
|
||||
set(px4_usb_path "${vendorstr_underscore}_${productstr_underscore}")
|
||||
set(px4_bl_usb_path "${vendorstr_underscore}_BL")
|
||||
|
||||
list(APPEND serial_ports
|
||||
# NuttX vendor + product string
|
||||
/dev/serial/by-id/*-${px4_usb_path}*
|
||||
|
||||
# Bootloader
|
||||
/dev/serial/by-id/*_${px4_bl_usb_path}*
|
||||
/dev/serial/by-id/*PX4_BL* # typical bootloader USB device string
|
||||
/dev/serial/by-id/*BL_FMU*
|
||||
|
||||
@@ -79,7 +81,7 @@ endif()
|
||||
string(REPLACE ";" "," serial_ports "${serial_ports}")
|
||||
|
||||
add_custom_target(upload
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${fw_package}
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${fw_package}
|
||||
DEPENDS ${fw_package}
|
||||
COMMENT "uploading px4"
|
||||
VERBATIM
|
||||
@@ -88,7 +90,7 @@ add_custom_target(upload
|
||||
)
|
||||
|
||||
add_custom_target(force-upload
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --force --port ${serial_ports} ${fw_package}
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_uploader.py --force --port ${serial_ports} ${fw_package}
|
||||
DEPENDS ${fw_package}
|
||||
COMMENT "uploading px4 with --force"
|
||||
VERBATIM
|
||||
|
||||
@@ -67,7 +67,7 @@ BMP280::init()
|
||||
|
||||
// set config, recommended settings
|
||||
_interface->set_reg(_curr_ctrl, BMP280_ADDR_CTRL);
|
||||
_interface->set_reg(BMP280_CONFIG_F16, BMP280_ADDR_CONFIG);
|
||||
_interface->set_reg(BMP280_CONFIG_F0, BMP280_ADDR_CONFIG);
|
||||
|
||||
// get calibration and pre process them
|
||||
_cal = _interface->get_calibration(BMP280_ADDR_CAL);
|
||||
|
||||
@@ -71,7 +71,7 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES "canbootloader")
|
||||
${PX4_BINARY_DIR}/${uavcan_bl_image_name}
|
||||
${PX4_BINARY_DIR}/deploy/${HWBOARD_ID}.bin
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --padding ${uavcanbl_padding} --use-git-hash ${PX4_CONFIG}.bin ${uavcan_bl_image_name}
|
||||
${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --padding ${uavcanbl_padding} --use-git-hash ${PX4_CONFIG}.bin ${uavcan_bl_image_name}
|
||||
COMMAND
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory deploy
|
||||
COMMAND
|
||||
|
||||
@@ -120,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
|
||||
endforeach(DSDLC_INPUT)
|
||||
add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
|
||||
${Python_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
|
||||
--outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS}
|
||||
#--verbose
|
||||
COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
|
||||
|
||||
@@ -42,7 +42,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
|
||||
set(DSDLC_INPUT_FILES ${DSDLC_INPUT_FILES} ${DSDLC_NEW_INPUT_FILES})
|
||||
endforeach(DSDLC_INPUT)
|
||||
add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
|
||||
COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp
|
||||
DEPENDS ${DSDLC_INPUT_FILES}
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
|
||||
@@ -120,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
|
||||
endforeach(DSDLC_INPUT)
|
||||
add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
|
||||
${Python_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc
|
||||
--outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS}
|
||||
#--verbose
|
||||
COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
|
||||
|
||||
@@ -41,51 +41,19 @@
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
|
||||
namespace
|
||||
{
|
||||
static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly
|
||||
static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
|
||||
|
||||
static float wrap_360(float f)
|
||||
{
|
||||
return wrap(f, 0.f, 360.f);
|
||||
}
|
||||
|
||||
static int wrap_bin(int i)
|
||||
{
|
||||
i = i % INTERNAL_MAP_USED_BINS;
|
||||
|
||||
while (i < 0) {
|
||||
i += INTERNAL_MAP_USED_BINS;
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
CollisionPrevention::CollisionPrevention(ModuleParams *parent) :
|
||||
ModuleParams(parent)
|
||||
{
|
||||
static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5");
|
||||
static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly");
|
||||
static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5");
|
||||
static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly");
|
||||
|
||||
// initialize internal obstacle map
|
||||
_obstacle_map_body_frame.timestamp = getTime();
|
||||
_obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
|
||||
_obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG;
|
||||
_obstacle_map_body_frame.increment = BIN_SIZE;
|
||||
_obstacle_map_body_frame.min_distance = UINT16_MAX;
|
||||
_obstacle_map_body_frame.max_distance = 0;
|
||||
_obstacle_map_body_frame.angle_offset = 0.f;
|
||||
uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]);
|
||||
uint64_t current_time = getTime();
|
||||
|
||||
for (uint32_t i = 0 ; i < internal_bins; i++) {
|
||||
_data_timestamps[i] = current_time;
|
||||
_data_maxranges[i] = 0;
|
||||
_data_fov[i] = 0;
|
||||
for (uint32_t i = 0 ; i < BIN_COUNT; i++) {
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
}
|
||||
@@ -112,48 +80,234 @@ bool CollisionPrevention::is_active()
|
||||
return activated;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
|
||||
void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
int msg_index = 0;
|
||||
float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
|
||||
float increment_factor = 1.f / obstacle.increment;
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude;
|
||||
|
||||
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
|
||||
_vehicle_attitude = Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = Eulerf(_vehicle_attitude).psi();
|
||||
}
|
||||
}
|
||||
|
||||
//calculate movement constraints based on range data
|
||||
const Vector2f original_setpoint = setpoint_accel;
|
||||
_updateObstacleMap();
|
||||
_updateObstacleData();
|
||||
_calculateConstrainedSetpoint(setpoint_accel, setpoint_vel);
|
||||
|
||||
// publish constraints
|
||||
collision_constraints_s constraints{};
|
||||
original_setpoint.copyTo(constraints.original_setpoint);
|
||||
setpoint_accel.copyTo(constraints.adapted_setpoint);
|
||||
constraints.timestamp = getTime();
|
||||
_constraints_pub.publish(constraints);
|
||||
}
|
||||
|
||||
void CollisionPrevention::_updateObstacleMap()
|
||||
{
|
||||
// add distance sensor data
|
||||
for (auto &dist_sens_sub : _distance_sensor_subs) {
|
||||
distance_sensor_s distance_sensor;
|
||||
|
||||
if (dist_sens_sub.update(&distance_sensor)) {
|
||||
// consider only instances with valid data and orientations useful for collision prevention
|
||||
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
|
||||
|
||||
// update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
(uint16_t)(distance_sensor.max_distance * 100.0f));
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
(uint16_t)(distance_sensor.min_distance * 100.0f));
|
||||
|
||||
_addDistanceSensorData(distance_sensor, _vehicle_attitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add obstacle distance data
|
||||
if (_sub_obstacle_distance.update()) {
|
||||
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
|
||||
|
||||
// Update map with obstacle data if the data is not stale
|
||||
if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
|
||||
//update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
obstacle_distance.max_distance);
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
obstacle_distance.min_distance);
|
||||
_addObstacleSensorData(obstacle_distance, _vehicle_yaw);
|
||||
}
|
||||
}
|
||||
|
||||
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
|
||||
_obstacle_distance_fused_pub.publish(_obstacle_map_body_frame);
|
||||
}
|
||||
|
||||
void CollisionPrevention::_updateObstacleData()
|
||||
{
|
||||
_obstacle_data_present = false;
|
||||
_closest_dist = UINT16_MAX;
|
||||
_closest_dist_dir.setZero();
|
||||
|
||||
for (int i = 0; i < BIN_COUNT; i++) {
|
||||
// if the data is stale, reset the bin
|
||||
if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) {
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE +
|
||||
_obstacle_map_body_frame.angle_offset));
|
||||
const Vector2f bin_direction = {cosf(angle), sinf(angle)};
|
||||
const uint16_t bin_distance = _obstacle_map_body_frame.distances[i];
|
||||
|
||||
// check if there is avaliable data and the data of the map is not stale
|
||||
if (bin_distance < UINT16_MAX
|
||||
&& (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
|
||||
_obstacle_data_present = true;
|
||||
}
|
||||
|
||||
if (bin_distance * 0.01f < _closest_dist) {
|
||||
_closest_dist = bin_distance * 0.01f;
|
||||
_closest_dist_dir = bin_direction;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
{
|
||||
const float setpoint_length = setpoint_accel.norm();
|
||||
_min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get());
|
||||
|
||||
const hrt_abstime now = getTime();
|
||||
|
||||
const bool is_stick_deflected = setpoint_length > 0.001f;
|
||||
|
||||
if (_obstacle_data_present && is_stick_deflected) {
|
||||
|
||||
_transformSetpoint(setpoint_accel);
|
||||
|
||||
float vel_comp_accel = INFINITY;
|
||||
Vector2f vel_comp_accel_dir{};
|
||||
|
||||
_getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now,
|
||||
vel_comp_accel, vel_comp_accel_dir);
|
||||
|
||||
Vector2f constr_accel_setpoint{};
|
||||
|
||||
if (_checkSetpointDirectionFeasability()) {
|
||||
constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length);
|
||||
}
|
||||
|
||||
setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir;
|
||||
|
||||
} else if (!_obstacle_data_present) {
|
||||
// allow no movement
|
||||
setpoint_accel.setZero();
|
||||
|
||||
// if distance data is stale, switch to Loiter
|
||||
if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
|
||||
if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US &&
|
||||
getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
|
||||
_publishVehicleCmdDoLoiter();
|
||||
}
|
||||
|
||||
PX4_WARN("No obstacle data, not moving...");
|
||||
_last_timeout_warning = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed...
|
||||
void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw)
|
||||
{
|
||||
|
||||
float vehicle_orientation_deg = math::degrees(vehicle_yaw);
|
||||
|
||||
|
||||
if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
|
||||
// Obstacle message arrives in local_origin frame (north aligned)
|
||||
// corresponding data index (convert to world frame and shift by msg offset)
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
|
||||
float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset;
|
||||
msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
|
||||
for (int i = 0; i < BIN_COUNT; i++) {
|
||||
for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) {
|
||||
float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
|
||||
- (float)_obstacle_map_body_frame.increment / 2.f);
|
||||
float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
|
||||
+ (float)_obstacle_map_body_frame.increment / 2.f);
|
||||
float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
|
||||
obstacle.increment / 2.f);
|
||||
float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
|
||||
obstacle.increment / 2.f);
|
||||
|
||||
//add all data points inside to FOV
|
||||
if (obstacle.distances[msg_index] != UINT16_MAX) {
|
||||
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
|
||||
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
|
||||
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
|
||||
_data_maxranges[i] = obstacle.max_distance;
|
||||
_data_fov[i] = 1;
|
||||
// if a bin stretches over the 0/360 degree line, adjust the angles
|
||||
if (bin_lower_angle > bin_upper_angle) {
|
||||
bin_lower_angle -= 360;
|
||||
}
|
||||
|
||||
if (msg_lower_angle > msg_upper_angle) {
|
||||
msg_lower_angle -= 360;
|
||||
}
|
||||
|
||||
// Check for overlaps.
|
||||
if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
|
||||
(msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
|
||||
(msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
|
||||
(msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
|
||||
if (obstacle.distances[j] != UINT16_MAX) {
|
||||
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
|
||||
_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
|
||||
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
|
||||
_data_maxranges[i] = obstacle.max_distance;
|
||||
_data_fov[i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
} else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) {
|
||||
// Obstacle message arrives in body frame (front aligned)
|
||||
// corresponding data index (shift by msg offset)
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
|
||||
float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG +
|
||||
_obstacle_map_body_frame.angle_offset;
|
||||
msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
|
||||
for (int i = 0; i < BIN_COUNT; i++) {
|
||||
for (int j = 0; j < 360 / obstacle.increment; j++) {
|
||||
float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
|
||||
- (float)_obstacle_map_body_frame.increment / 2.f);
|
||||
float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
|
||||
+ (float)_obstacle_map_body_frame.increment / 2.f);
|
||||
float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
|
||||
float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
|
||||
|
||||
//add all data points inside to FOV
|
||||
if (obstacle.distances[msg_index] != UINT16_MAX) {
|
||||
|
||||
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
|
||||
_obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
|
||||
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
|
||||
_data_maxranges[i] = obstacle.max_distance;
|
||||
_data_fov[i] = 1;
|
||||
// if a bin stretches over the 0/360 degree line, adjust the angles
|
||||
if (bin_lower_angle > bin_upper_angle) {
|
||||
bin_lower_angle -= 360;
|
||||
}
|
||||
|
||||
if (msg_lower_angle > msg_upper_angle) {
|
||||
msg_lower_angle -= 360;
|
||||
}
|
||||
|
||||
// Check for overlaps.
|
||||
if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) ||
|
||||
(msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) ||
|
||||
(msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) ||
|
||||
(msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) {
|
||||
if (obstacle.distances[j] != UINT16_MAX) {
|
||||
|
||||
if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) {
|
||||
_obstacle_map_body_frame.distances[i] = obstacle.distances[j];
|
||||
_data_timestamps[i] = _obstacle_map_body_frame.timestamp;
|
||||
_data_maxranges[i] = obstacle.max_distance;
|
||||
_data_fov[i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -197,80 +351,60 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_updateObstacleMap()
|
||||
bool
|
||||
CollisionPrevention::_checkSetpointDirectionFeasability()
|
||||
{
|
||||
_sub_vehicle_attitude.update();
|
||||
bool setpoint_feasible = true;
|
||||
|
||||
// add distance sensor data
|
||||
for (auto &dist_sens_sub : _distance_sensor_subs) {
|
||||
distance_sensor_s distance_sensor;
|
||||
for (int i = 0; i < BIN_COUNT; i++) {
|
||||
// check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data
|
||||
if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get()
|
||||
|| (_param_cp_go_no_data.get() && _data_fov[i]))) {
|
||||
setpoint_feasible = false;
|
||||
|
||||
if (dist_sens_sub.update(&distance_sensor)) {
|
||||
// consider only instances with valid data and orientations useful for collision prevention
|
||||
if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
|
||||
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
|
||||
|
||||
// update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
(uint16_t)(distance_sensor.max_distance * 100.0f));
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
(uint16_t)(distance_sensor.min_distance * 100.0f));
|
||||
|
||||
_addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// add obstacle distance data
|
||||
if (_sub_obstacle_distance.update()) {
|
||||
const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
|
||||
|
||||
// Update map with obstacle data if the data is not stale
|
||||
if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
|
||||
//update message description
|
||||
_obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp);
|
||||
_obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance,
|
||||
obstacle_distance.max_distance);
|
||||
_obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance,
|
||||
obstacle_distance.min_distance);
|
||||
_addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
|
||||
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
|
||||
_obstacle_distance_pub.publish(_obstacle_map_body_frame);
|
||||
return setpoint_feasible;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
|
||||
CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
|
||||
{
|
||||
const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw;
|
||||
const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
|
||||
_obstacle_map_body_frame.angle_offset);
|
||||
_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
|
||||
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
|
||||
_setpoint_dir = setpoint.unit_or_zero();
|
||||
_adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw);
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude)
|
||||
{
|
||||
// clamp at maximum sensor range
|
||||
float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
|
||||
|
||||
// discard values below min range
|
||||
if ((distance_reading > distance_sensor.min_distance)) {
|
||||
// negative values indicate out of range but valid measurements.
|
||||
if (fabsf(distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) {
|
||||
distance_reading = distance_sensor.max_distance;
|
||||
}
|
||||
|
||||
// discard values below min range
|
||||
if (distance_reading > distance_sensor.min_distance) {
|
||||
float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
|
||||
float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
|
||||
|
||||
// calculate the field of view boundary bin indices
|
||||
int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) /
|
||||
INTERNAL_MAP_INCREMENT_DEG);
|
||||
int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) /
|
||||
INTERNAL_MAP_INCREMENT_DEG);
|
||||
int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
|
||||
int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
|
||||
|
||||
// floor values above zero, ceil values below zero
|
||||
if (lower_bound < 0) { lower_bound++; }
|
||||
|
||||
if (upper_bound < 0) { upper_bound++; }
|
||||
|
||||
// rotate vehicle attitude into the sensor body frame
|
||||
matrix::Quatf attitude_sensor_frame = vehicle_attitude;
|
||||
Quatf attitude_sensor_frame = vehicle_attitude;
|
||||
attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
|
||||
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
|
||||
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify
|
||||
|
||||
if (distance_reading < distance_sensor.max_distance) {
|
||||
distance_reading = distance_reading * sensor_dist_scale;
|
||||
@@ -279,7 +413,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
|
||||
uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm
|
||||
|
||||
for (int bin = lower_bound; bin <= upper_bound; ++bin) {
|
||||
int wrapped_bin = wrap_bin(bin);
|
||||
int wrapped_bin = _wrap_bin(bin);
|
||||
|
||||
if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
|
||||
_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
|
||||
@@ -294,8 +428,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
|
||||
void
|
||||
CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
|
||||
{
|
||||
const float col_prev_d = _param_cp_dist.get();
|
||||
const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
|
||||
const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE);
|
||||
const int sp_index_original = setpoint_index;
|
||||
float best_cost = 9999.f;
|
||||
int new_sp_index = setpoint_index;
|
||||
@@ -307,19 +440,19 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
|
||||
float mean_dist = 0;
|
||||
|
||||
for (int j = i - filter_size; j <= i + filter_size; j++) {
|
||||
int bin = wrap_bin(j);
|
||||
int bin = _wrap_bin(j);
|
||||
|
||||
if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
|
||||
mean_dist += col_prev_d * 100.f;
|
||||
mean_dist += _param_cp_dist.get() * 100.f;
|
||||
|
||||
} else {
|
||||
mean_dist += _obstacle_map_body_frame.distances[bin];
|
||||
}
|
||||
}
|
||||
|
||||
const int bin = wrap_bin(i);
|
||||
const int bin = _wrap_bin(i);
|
||||
mean_dist = mean_dist / (2.f * filter_size + 1.f);
|
||||
const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original);
|
||||
const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
|
||||
const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
|
||||
|
||||
if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
|
||||
@@ -330,7 +463,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
|
||||
|
||||
//only change setpoint direction if it was moved to a different bin
|
||||
if (new_sp_index != setpoint_index) {
|
||||
float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
|
||||
float angle = math::radians((float)new_sp_index * BIN_SIZE + _obstacle_map_body_frame.angle_offset);
|
||||
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
|
||||
setpoint_dir = {cosf(angle), sinf(angle)};
|
||||
setpoint_index = new_sp_index;
|
||||
@@ -340,7 +473,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
|
||||
float
|
||||
CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
|
||||
{
|
||||
float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
|
||||
float offset = math::max(math::radians(angle_offset), 0.f);
|
||||
|
||||
switch (distance_sensor.orientation) {
|
||||
case distance_sensor_s::ROTATION_YAW_0:
|
||||
@@ -376,180 +509,147 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
|
||||
break;
|
||||
|
||||
case distance_sensor_s::ROTATION_CUSTOM:
|
||||
offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
|
||||
offset = Eulerf(Quatf(distance_sensor.q)).psi();
|
||||
break;
|
||||
}
|
||||
|
||||
return offset;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel)
|
||||
float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
|
||||
{
|
||||
_updateObstacleMap();
|
||||
|
||||
// read parameters
|
||||
const float col_prev_d = _param_cp_dist.get();
|
||||
const float col_prev_dly = _param_cp_delay.get();
|
||||
const bool move_no_data = _param_cp_go_nodata.get();
|
||||
const float xy_p = _param_mpc_xy_p.get();
|
||||
const float max_jerk = _param_mpc_jerk_max.get();
|
||||
const float max_accel = _param_mpc_acc_hor.get();
|
||||
const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
|
||||
const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
|
||||
|
||||
const float setpoint_length = setpoint.norm();
|
||||
|
||||
const hrt_abstime constrain_time = getTime();
|
||||
int num_fov_bins = 0;
|
||||
|
||||
if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
|
||||
if (setpoint_length > 0.001f) {
|
||||
|
||||
Vector2f setpoint_dir = setpoint / setpoint_length;
|
||||
float vel_max = setpoint_length;
|
||||
const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
|
||||
|
||||
const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
|
||||
const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
|
||||
_obstacle_map_body_frame.angle_offset);
|
||||
int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
|
||||
|
||||
// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
|
||||
_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
|
||||
|
||||
// limit speed for safe flight
|
||||
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
|
||||
|
||||
// delete stale values
|
||||
const hrt_abstime data_age = constrain_time - _data_timestamps[i];
|
||||
|
||||
if (data_age > RANGE_STREAM_TIMEOUT_US) {
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
|
||||
const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters
|
||||
const float max_range = _data_maxranges[i] * 0.01f; // convert to meters
|
||||
float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
|
||||
|
||||
// convert from body to local frame in the range [0, 2*pi]
|
||||
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
|
||||
|
||||
// get direction of current bin
|
||||
const Vector2f bin_direction = {cosf(angle), sinf(angle)};
|
||||
|
||||
//count number of bins in the field of valid_new
|
||||
if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
|
||||
num_fov_bins ++;
|
||||
}
|
||||
|
||||
if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
|
||||
&& _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
|
||||
|
||||
if (setpoint_dir.dot(bin_direction) > 0) {
|
||||
// calculate max allowed velocity with a P-controller (same gain as in the position controller)
|
||||
const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
|
||||
float delay_distance = curr_vel_parallel * col_prev_dly;
|
||||
|
||||
if (distance < max_range) {
|
||||
delay_distance += curr_vel_parallel * (data_age * 1e-6f);
|
||||
}
|
||||
|
||||
const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
|
||||
const float vel_max_posctrl = xy_p * stop_distance;
|
||||
|
||||
const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
|
||||
const float projection = bin_direction.dot(setpoint_dir);
|
||||
float vel_max_bin = vel_max;
|
||||
|
||||
if (projection > 0.01f) {
|
||||
vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection;
|
||||
}
|
||||
|
||||
// constrain the velocity
|
||||
if (vel_max_bin >= 0) {
|
||||
vel_max = math::min(vel_max, vel_max_bin);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
|
||||
if (!move_no_data || (move_no_data && _data_fov[i])) {
|
||||
vel_max = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//if the sensor field of view is zero, never allow to move (even if move_no_data=1)
|
||||
if (num_fov_bins == 0) {
|
||||
vel_max = 0.f;
|
||||
}
|
||||
|
||||
setpoint = setpoint_dir * vel_max;
|
||||
}
|
||||
|
||||
} else {
|
||||
//allow no movement
|
||||
float vel_max = 0.f;
|
||||
setpoint = setpoint * vel_max;
|
||||
|
||||
// if distance data is stale, switch to Loiter
|
||||
if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
|
||||
|
||||
if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
|
||||
&& getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
|
||||
_publishVehicleCmdDoLoiter();
|
||||
}
|
||||
|
||||
_last_timeout_warning = getTime();
|
||||
}
|
||||
|
||||
float obstacle_distance = 0.f;
|
||||
const float direction_norm = direction.norm();
|
||||
|
||||
if (direction_norm > FLT_EPSILON) {
|
||||
Vector2f dir = direction / direction_norm;
|
||||
const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw;
|
||||
const float sp_angle_with_offset_deg =
|
||||
_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
|
||||
int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
|
||||
dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1);
|
||||
obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
|
||||
}
|
||||
|
||||
return obstacle_distance;
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel)
|
||||
Vector2f
|
||||
CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length)
|
||||
{
|
||||
//calculate movement constraints based on range data
|
||||
Vector2f new_setpoint = original_setpoint;
|
||||
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
|
||||
Vector2f new_setpoint{};
|
||||
const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir));
|
||||
const Vector2f tangential_component = _setpoint_dir - normal_component;
|
||||
|
||||
//warn user if collision prevention starts to interfere
|
||||
bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
|
||||
|| new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
|
||||
|| new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
|
||||
|| new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
|
||||
const float normal_scale = _getScale(_closest_dist);
|
||||
|
||||
_interfering = currently_interfering;
|
||||
|
||||
// publish constraints
|
||||
collision_constraints_s constraints{};
|
||||
constraints.timestamp = getTime();
|
||||
original_setpoint.copyTo(constraints.original_setpoint);
|
||||
new_setpoint.copyTo(constraints.adapted_setpoint);
|
||||
_constraints_pub.publish(constraints);
|
||||
const float closest_dist_tangential = _getObstacleDistance(tangential_component);
|
||||
const float tangential_scale = _getScale(closest_dist_tangential);
|
||||
|
||||
original_setpoint = new_setpoint;
|
||||
|
||||
// only scale accelerations towards the obstacle
|
||||
if (_closest_dist_dir.dot(_setpoint_dir) > 0) {
|
||||
new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length;
|
||||
|
||||
} else {
|
||||
new_setpoint = _setpoint_dir * setpoint_length;
|
||||
}
|
||||
|
||||
return new_setpoint;
|
||||
}
|
||||
|
||||
float
|
||||
CollisionPrevention::_getScale(const float &reference_distance)
|
||||
{
|
||||
float scale = (reference_distance - _min_dist_to_keep);
|
||||
const float scale_distance = math::max(_min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get());
|
||||
|
||||
// if scale is positive, square it and scale it with the scale_distance
|
||||
scale = scale > 0 ? powf(scale / scale_distance, 2) : scale;
|
||||
scale = math::min(scale, 1.0f);
|
||||
return scale;
|
||||
}
|
||||
|
||||
void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad,
|
||||
const Vector2f &setpoint_vel,
|
||||
const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir)
|
||||
{
|
||||
for (int i = 0; i < BIN_COUNT; i++) {
|
||||
const float max_range = _data_maxranges[i] * 0.01f;
|
||||
|
||||
// get the vector pointing into the direction of current bin
|
||||
float bin_angle = wrap_2pi(vehicle_yaw_angle_rad
|
||||
+ math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset));
|
||||
|
||||
const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) };
|
||||
float bin_distance = _obstacle_map_body_frame.distances[i];
|
||||
|
||||
// only consider bins which are between min and max values
|
||||
if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) {
|
||||
const float distance = bin_distance * 0.01f;
|
||||
|
||||
// Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high
|
||||
// acceleration maneuvers
|
||||
const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction));
|
||||
float delay_distance = curr_vel_parallel * _param_cp_delay.get();
|
||||
|
||||
const hrt_abstime data_age = now - _data_timestamps[i];
|
||||
|
||||
if (distance < max_range) {
|
||||
delay_distance += curr_vel_parallel * (data_age * 1e-6f);
|
||||
}
|
||||
|
||||
const float stop_distance = distance - _min_dist_to_keep - delay_distance;
|
||||
|
||||
float curr_acc_vel_constraint;
|
||||
|
||||
if (stop_distance >= 0.f) {
|
||||
const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(),
|
||||
_param_mpc_acc_hor.get(), stop_distance, 0.f);
|
||||
curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f);
|
||||
|
||||
} else {
|
||||
curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel;
|
||||
}
|
||||
|
||||
if (curr_acc_vel_constraint < vel_comp_accel) {
|
||||
vel_comp_accel = curr_acc_vel_constraint;
|
||||
vel_comp_accel_dir = bin_direction;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CollisionPrevention::_publishVehicleCmdDoLoiter()
|
||||
{
|
||||
vehicle_command_s command{};
|
||||
command.timestamp = getTime();
|
||||
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
|
||||
command.param1 = (float)1; // base mode
|
||||
command.param3 = (float)0; // sub mode
|
||||
command.param1 = 1.f; // base mode VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED
|
||||
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
|
||||
command.target_system = 1;
|
||||
command.target_component = 1;
|
||||
command.source_system = 1;
|
||||
command.source_component = 1;
|
||||
command.confirmation = false;
|
||||
command.from_external = false;
|
||||
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
|
||||
|
||||
// publish the vehicle command
|
||||
command.timestamp = getTime();
|
||||
_vehicle_command_pub.publish(command);
|
||||
}
|
||||
|
||||
float CollisionPrevention::_wrap_360(const float f)
|
||||
{
|
||||
return wrap(f, 0.f, 360.f);
|
||||
}
|
||||
|
||||
int CollisionPrevention::_wrap_bin(int i)
|
||||
{
|
||||
i = i % BIN_COUNT;
|
||||
|
||||
while (i < 0) {
|
||||
i += BIN_COUNT;
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
@@ -34,6 +34,7 @@
|
||||
/**
|
||||
* @file CollisionPrevention.hpp
|
||||
* @author Tanja Baumann <tanja@auterion.com>
|
||||
* @author Claudio Chies <claudio@chies.com>
|
||||
*
|
||||
* CollisionPrevention controller.
|
||||
*
|
||||
@@ -74,21 +75,29 @@ public:
|
||||
|
||||
/**
|
||||
* Computes collision free setpoints
|
||||
* @param original_setpoint, setpoint before collision prevention intervention
|
||||
* @param max_speed, maximum xy speed
|
||||
* @param curr_pos, current vehicle position
|
||||
* @param curr_vel, current vehicle velocity
|
||||
* @param setpoint_accel setpoint purely based on sticks, to be modified
|
||||
* @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed
|
||||
*/
|
||||
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
|
||||
const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
|
||||
void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
|
||||
|
||||
static constexpr int BIN_COUNT =
|
||||
sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); // 72
|
||||
static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly
|
||||
|
||||
protected:
|
||||
/** Aggregates the sensor data into an internal obstacle map in body frame */
|
||||
void _updateObstacleMap();
|
||||
|
||||
obstacle_distance_s _obstacle_map_body_frame {};
|
||||
bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
|
||||
uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
|
||||
uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
|
||||
_obstacle_map_body_frame.distances[0])]; /**< in cm */
|
||||
/** Updates the obstacle data based on stale data and calculates values from the map */
|
||||
void _updateObstacleData();
|
||||
|
||||
/** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */
|
||||
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel);
|
||||
|
||||
obstacle_distance_s _obstacle_map_body_frame{};
|
||||
bool _data_fov[BIN_COUNT] {};
|
||||
uint64_t _data_timestamps[BIN_COUNT] {};
|
||||
uint16_t _data_maxranges[BIN_COUNT] {}; /**< in cm */
|
||||
|
||||
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
|
||||
|
||||
@@ -96,7 +105,7 @@ protected:
|
||||
* Updates obstacle distance message with measurement from offboard
|
||||
* @param obstacle, obstacle_distance message to be updated
|
||||
*/
|
||||
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
|
||||
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw);
|
||||
|
||||
/**
|
||||
* Computes an adaption to the setpoint direction to guide towards free space
|
||||
@@ -106,6 +115,34 @@ protected:
|
||||
*/
|
||||
void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
|
||||
|
||||
/**
|
||||
* Constrain the acceleration setpoint based on the distance to the obstacle
|
||||
* The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above
|
||||
* +1 ________ _ _
|
||||
* ┌─┐ │ //
|
||||
* │X│ │ //
|
||||
* │X│ │ //
|
||||
* │X│ │ ///
|
||||
* │X│ │ //
|
||||
* │X│ │/////
|
||||
* │X│──────┼─────────────┬─────────────
|
||||
* │X│ /│ scale_distance
|
||||
* │X│ / │
|
||||
* │X│ / │
|
||||
* │X│ / │
|
||||
* │X│ / │
|
||||
* └─┘/ │
|
||||
* -1
|
||||
*/
|
||||
matrix::Vector2f _constrainAccelerationSetpoint(const float &setpoint_length);
|
||||
|
||||
void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel,
|
||||
const hrt_abstime now, float &vel_comp_accel, matrix::Vector2f &vel_comp_accel_dir);
|
||||
|
||||
float _getObstacleDistance(const matrix::Vector2f &direction);
|
||||
|
||||
float _getScale(const float &reference_distance);
|
||||
|
||||
/**
|
||||
* Determines whether a new sensor measurement is used
|
||||
* @param map_index, index of the bin in the internal map the measurement belongs in
|
||||
@@ -114,25 +151,39 @@ protected:
|
||||
*/
|
||||
bool _enterData(int map_index, float sensor_range, float sensor_reading);
|
||||
|
||||
bool _checkSetpointDirectionFeasability();
|
||||
|
||||
void _transformSetpoint(const matrix::Vector2f &setpoint);
|
||||
|
||||
|
||||
//Timing functions. Necessary to mock time in the tests
|
||||
virtual hrt_abstime getTime();
|
||||
virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
|
||||
|
||||
|
||||
private:
|
||||
|
||||
bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
|
||||
bool _data_stale{true}; /**< states if the data is stale */
|
||||
bool _was_active{false}; /**< states if the collision prevention interferes with the user input */
|
||||
bool _obstacle_data_present{false}; /**< states if obstacle data is present */
|
||||
|
||||
int _setpoint_index{}; /**< index of the setpoint*/
|
||||
matrix::Vector2f _setpoint_dir{}; /**< direction of the setpoint*/
|
||||
|
||||
float _closest_dist{}; /**< closest distance to an obstacle */
|
||||
matrix::Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */
|
||||
|
||||
float _min_dist_to_keep{};
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
|
||||
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
matrix::Quatf _vehicle_attitude{};
|
||||
float _vehicle_yaw{0.f};
|
||||
|
||||
uORB::Publication<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
|
||||
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
|
||||
|
||||
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
|
||||
uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
|
||||
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
|
||||
@@ -142,13 +193,15 @@ private:
|
||||
hrt_abstime _time_activated{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
|
||||
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
|
||||
(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
|
||||
(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
|
||||
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
|
||||
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
|
||||
(ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
|
||||
(ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
|
||||
(ParamBool<px4::params::CP_GO_NO_DATA>) _param_cp_go_no_data, /**< movement allowed where no data*/
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
|
||||
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_xy_vel_p_acc, /**< p gain from velocity controller*/
|
||||
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/
|
||||
)
|
||||
|
||||
/**
|
||||
@@ -180,14 +233,11 @@ private:
|
||||
*/
|
||||
void _publishObstacleDistance(obstacle_distance_s &obstacle);
|
||||
|
||||
/**
|
||||
* Aggregates the sensor data into a internal obstacle map in body frame
|
||||
*/
|
||||
void _updateObstacleMap();
|
||||
|
||||
/**
|
||||
* Publishes vehicle command.
|
||||
*/
|
||||
void _publishVehicleCmdDoLoiter();
|
||||
|
||||
static float _wrap_360(const float f);
|
||||
static int _wrap_bin(int i);
|
||||
};
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -83,12 +83,12 @@ list(APPEND comp_metadata_types "--type" "5,${PX4_BINARY_DIR}/actuators.json.xz,
|
||||
set(component_general_json ${PX4_BINARY_DIR}/component_general.json)
|
||||
set(component_information_header ${CMAKE_CURRENT_BINARY_DIR}/checksums.h)
|
||||
add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz ${component_information_header}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_component_general.py
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_component_general.py
|
||||
${component_general_json}
|
||||
--compress
|
||||
${comp_metadata_types}
|
||||
--version-file ${PX4_BINARY_DIR}/src/lib/version/build_git_version.h
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_crc.py
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_crc.py
|
||||
${component_general_json}
|
||||
${PX4_BINARY_DIR}/events/all_events.json.xz
|
||||
--output ${component_information_header}
|
||||
|
||||
@@ -20,7 +20,7 @@ version_file = args.version_file
|
||||
version_dir = ''
|
||||
if version_file is not None:
|
||||
for line in open(version_file, "r"):
|
||||
version_search = re.search('PX4_GIT_TAG_OR_BRANCH_NAME\s+"(.+)"', line)
|
||||
version_search = re.search(r'PX4_GIT_TAG_OR_BRANCH_NAME\s+"(.+)"', line)
|
||||
if version_search:
|
||||
version_dir = version_search.group(1)
|
||||
break
|
||||
|
||||
@@ -47,7 +47,7 @@ set(generated_events_px4_file ${generated_events_dir}/px4.json)
|
||||
set(generated_events_common_enums_file ${generated_events_dir}/common_with_enums.json)
|
||||
add_custom_command(OUTPUT ${generated_events_px4_file}
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py
|
||||
--base-path ${PX4_SOURCE_DIR}/src
|
||||
--src-path ${all_px4_src_files_relative}
|
||||
--json ${generated_events_px4_file} #--verbose
|
||||
@@ -64,13 +64,13 @@ add_custom_target(events_px4_json DEPENDS ${generated_events_px4_file})
|
||||
|
||||
set(generated_events_file ${generated_events_dir}/all_events.json)
|
||||
add_custom_command(OUTPUT ${generated_events_file} ${generated_events_file}.xz
|
||||
COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/combine.py
|
||||
COMMAND ${Python_EXECUTABLE} libevents/scripts/combine.py
|
||||
${generated_events_px4_file}
|
||||
${generated_events_common_enums_file}
|
||||
--output ${generated_events_file}
|
||||
COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/validate.py
|
||||
COMMAND ${Python_EXECUTABLE} libevents/scripts/validate.py
|
||||
${generated_events_file}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py
|
||||
${generated_events_file}
|
||||
DEPENDS
|
||||
${generated_events_px4_file}
|
||||
@@ -86,7 +86,7 @@ add_custom_target(events_json DEPENDS ${generated_events_file})
|
||||
# combine common.json with our enums for the code generation
|
||||
add_custom_command(OUTPUT ${generated_events_common_enums_file}
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir}
|
||||
COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/combine.py
|
||||
COMMAND ${Python_EXECUTABLE} libevents/scripts/combine.py
|
||||
enums.json
|
||||
libevents/events/common.json
|
||||
--output ${generated_events_common_enums_file}
|
||||
@@ -105,7 +105,7 @@ add_custom_command(OUTPUT ${generated_events_common_enums_file}
|
||||
set(generated_events_header ${generated_events_dir}/events_generated.h)
|
||||
add_custom_command(OUTPUT ${generated_events_header}
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir}
|
||||
COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/generate.py
|
||||
COMMAND ${Python_EXECUTABLE} libevents/scripts/generate.py
|
||||
--template libevents/libs/cpp/templates/events_generated.h.jinja
|
||||
--output ${generated_events_dir}
|
||||
${generated_events_common_enums_file}
|
||||
|
||||
@@ -39,14 +39,14 @@ endif()
|
||||
set(generated_actuators_metadata_file ${PX4_BINARY_DIR}/actuators.json)
|
||||
add_custom_command(OUTPUT ${generated_actuators_metadata_file}
|
||||
${generated_actuators_metadata_file}.xz
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_actuators_metadata.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_actuators_metadata.py
|
||||
${board_with_io_arg}
|
||||
--timer-config ${PX4_BOARD_DIR}/src/timer_config.cpp
|
||||
--config-files ${module_config_files} #--verbose
|
||||
--compress
|
||||
--board ${PX4_BOARD}
|
||||
--output-file ${generated_actuators_metadata_file}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
|
||||
--schema-file ${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink/component_information/actuators.schema.json
|
||||
${generated_actuators_metadata_file}
|
||||
--skip-if-no-schema # mavlink submodule might not exist for current target if built in CI
|
||||
|
||||
@@ -33,7 +33,7 @@
|
||||
|
||||
set(functions_header ${CMAKE_CURRENT_BINARY_DIR}/output_functions.hpp)
|
||||
add_custom_command(OUTPUT ${functions_header}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_function_header.py
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/output_functions.yaml
|
||||
${functions_header}
|
||||
DEPENDS
|
||||
|
||||
@@ -170,8 +170,6 @@ public:
|
||||
void setAllMinValues(uint16_t value);
|
||||
void setAllMaxValues(uint16_t value);
|
||||
|
||||
uint16_t &reverseOutputMask() { return _reverse_output_mask; }
|
||||
uint16_t &failsafeValue(int index) { return _failsafe_value[index]; }
|
||||
/** Disarmed values: disarmedValue < minValue needs to hold */
|
||||
uint16_t &disarmedValue(int index) { return _disarmed_value[index]; }
|
||||
uint16_t &minValue(int index) { return _min_value[index]; }
|
||||
|
||||
@@ -90,12 +90,12 @@ endif()
|
||||
|
||||
add_custom_command(OUTPUT ${generated_serial_params_file} ${generated_module_params_file}
|
||||
COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_params_dir}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py
|
||||
--params-file ${generated_serial_params_file}
|
||||
--serial-ports ${board_serial_ports} ${added_arguments} ${constrained_flash_arg}
|
||||
--config-files ${module_config_files}
|
||||
#--verbose
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py
|
||||
--params-file ${generated_module_params_file}
|
||||
${added_arguments} ${board_with_io_arg}
|
||||
--timer-config ${PX4_BOARD_DIR}/src/timer_config.cpp
|
||||
@@ -114,7 +114,7 @@ set(parameters_xml ${PX4_BINARY_DIR}/parameters.xml)
|
||||
set(parameters_json ${PX4_BINARY_DIR}/parameters.json)
|
||||
file(GLOB_RECURSE param_src_files ${PX4_SOURCE_DIR}/src/*params.c ${PX4_SOURCE_DIR}/src/*parameters.c)
|
||||
add_custom_command(OUTPUT ${parameters_xml} ${parameters_json} ${parameters_json}.xz
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_process_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_process_params.py
|
||||
--src-path ${module_list} ${generated_params_dir}
|
||||
--xml ${parameters_xml}
|
||||
--json ${parameters_json}
|
||||
@@ -123,7 +123,7 @@ add_custom_command(OUTPUT ${parameters_xml} ${parameters_json} ${parameters_json
|
||||
--overrides ${PARAM_DEFAULT_OVERRIDES}
|
||||
--board ${PX4_BOARD}
|
||||
#--verbose
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
|
||||
COMMAND ${Python_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/validate_json.py
|
||||
--schema-file ${PX4_SOURCE_DIR}/src/modules/mavlink/mavlink/component_information/parameter.schema.json
|
||||
${parameters_json}
|
||||
--skip-if-no-schema # mavlink submodule might not exist for current target if built in CI
|
||||
@@ -145,7 +145,7 @@ add_custom_target(parameters_xml DEPENDS ${parameters_xml})
|
||||
|
||||
# generate px4_parameters.hpp
|
||||
add_custom_command(OUTPUT px4_parameters.hpp
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_generate_params.py
|
||||
--xml ${parameters_xml} --dest ${CMAKE_CURRENT_BINARY_DIR}
|
||||
DEPENDS
|
||||
${PX4_BINARY_DIR}/parameters.xml
|
||||
@@ -157,7 +157,7 @@ add_custom_target(parameters_header DEPENDS px4_parameters.hpp)
|
||||
set(SRCS)
|
||||
|
||||
list(APPEND SRCS
|
||||
parameters.cpp
|
||||
parameters.cpp
|
||||
atomic_transaction.cpp
|
||||
autosave.cpp
|
||||
)
|
||||
|
||||
@@ -54,7 +54,7 @@ endif()
|
||||
set(px4_git_ver_header ${CMAKE_CURRENT_BINARY_DIR}/build_git_version.h)
|
||||
add_custom_command(OUTPUT ${px4_git_ver_header}
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
|
||||
${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
|
||||
DEPENDS
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py
|
||||
${git_dir_path}/HEAD
|
||||
|
||||
@@ -40,7 +40,7 @@ add_dependencies(wind_estimator prebuild_targets)
|
||||
add_subdirectory(test)
|
||||
|
||||
add_custom_target(wind_estimator_generate_airspeed_fusion
|
||||
COMMAND ${PYTHON_EXECUTABLE} derivation.py
|
||||
COMMAND ${Python_EXECUTABLE} derivation.py
|
||||
#DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/python/derivation.py
|
||||
#BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/python/generated/fuse_airspeed.h
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/python
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
add_custom_target(world_magnetic_model_update
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
|
||||
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
USES_TERMINAL
|
||||
@@ -40,7 +40,7 @@ add_custom_target(world_magnetic_model_update
|
||||
)
|
||||
|
||||
add_custom_target(world_magnetic_model_tests_update
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
|
||||
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
USES_TERMINAL
|
||||
|
||||
@@ -716,6 +716,8 @@ Commander::Commander() :
|
||||
}
|
||||
|
||||
updateParameters();
|
||||
|
||||
_failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this);
|
||||
}
|
||||
|
||||
Commander::~Commander()
|
||||
@@ -2799,8 +2801,8 @@ void Commander::dataLinkCheck()
|
||||
_open_drone_id_system_lost = false;
|
||||
|
||||
if (_datalink_last_heartbeat_open_drone_id_system != 0) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t");
|
||||
events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained");
|
||||
mavlink_log_info(&_mavlink_log_pub, "Remote ID system regained\t");
|
||||
events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "Remote ID system regained");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2861,11 +2863,11 @@ void Commander::dataLinkCheck()
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
// OpenDroneID system
|
||||
// Remote ID system
|
||||
if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s)
|
||||
&& !_open_drone_id_system_lost) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost");
|
||||
events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost");
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Remote ID system lost");
|
||||
events::send(events::ID("commander_remote_id_lost"), events::Log::Critical, "Remote ID system lost");
|
||||
_vehicle_status.open_drone_id_system_present = false;
|
||||
_vehicle_status.open_drone_id_system_healthy = false;
|
||||
_open_drone_id_system_lost = true;
|
||||
@@ -3000,6 +3002,20 @@ void Commander::send_parachute_command()
|
||||
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
|
||||
}
|
||||
|
||||
void Commander::onFailsafeNotifyUserTrampoline(void *arg)
|
||||
{
|
||||
Commander *commander = static_cast<Commander *>(arg);
|
||||
commander->onFailsafeNotifyUser();
|
||||
}
|
||||
|
||||
void Commander::onFailsafeNotifyUser()
|
||||
{
|
||||
// If we are about to inform about a failsafe, we need to ensure any pending health report is sent out first,
|
||||
// as the failsafe message might reference that. This is only needed in case the report is currently rate-limited,
|
||||
// i.e. it had a recent previous change already.
|
||||
_health_and_arming_checks.reportIfUnreportedDifferences();
|
||||
}
|
||||
|
||||
int Commander::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
|
||||
@@ -200,6 +200,9 @@ private:
|
||||
|
||||
void modeManagementUpdate();
|
||||
|
||||
static void onFailsafeNotifyUserTrampoline(void *arg);
|
||||
void onFailsafeNotifyUser();
|
||||
|
||||
enum class PrearmedMode {
|
||||
DISABLED = 0,
|
||||
SAFETY_BUTTON = 1,
|
||||
|
||||
@@ -219,7 +219,7 @@ bool Report::finalize()
|
||||
return _results_changed;
|
||||
}
|
||||
|
||||
bool Report::report(bool is_armed, bool force)
|
||||
bool Report::report(bool force)
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
const bool has_difference = _had_unreported_difference || _results_changed;
|
||||
@@ -317,3 +317,12 @@ bool Report::report(bool is_armed, bool force)
|
||||
current_results.health.error, current_results.health.warning);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Report::reportIfUnreportedDifferences()
|
||||
{
|
||||
if (_had_unreported_difference) {
|
||||
return report(true);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -337,7 +337,12 @@ private:
|
||||
*/
|
||||
bool finalize();
|
||||
|
||||
bool report(bool is_armed, bool force);
|
||||
bool report(bool force);
|
||||
|
||||
/**
|
||||
* Send out any unreported changes if there are any
|
||||
*/
|
||||
bool reportIfUnreportedDifferences();
|
||||
|
||||
const hrt_abstime _min_reporting_interval;
|
||||
|
||||
|
||||
@@ -67,7 +67,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
|
||||
}
|
||||
|
||||
const bool results_changed = _reporter.finalize();
|
||||
const bool reported = _reporter.report(_context.isArmed(), force_reporting);
|
||||
const bool reported = _reporter.report(force_reporting);
|
||||
|
||||
if (reported) {
|
||||
|
||||
@@ -89,7 +89,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
|
||||
}
|
||||
|
||||
_reporter.finalize();
|
||||
_reporter.report(_context.isArmed(), false);
|
||||
_reporter.report(false);
|
||||
_reporter._mavlink_log_pub = nullptr;
|
||||
// LEGACY end
|
||||
|
||||
@@ -120,3 +120,8 @@ void HealthAndArmingChecks::updateParams()
|
||||
_checks[i]->updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
bool HealthAndArmingChecks::reportIfUnreportedDifferences()
|
||||
{
|
||||
return _reporter.reportIfUnreportedDifferences();
|
||||
}
|
||||
|
||||
@@ -88,6 +88,8 @@ public:
|
||||
*/
|
||||
bool update(bool force_reporting = false, bool is_arming_request = false);
|
||||
|
||||
bool reportIfUnreportedDifferences();
|
||||
|
||||
/**
|
||||
* Whether arming is possible for a given navigation mode
|
||||
*/
|
||||
|
||||
@@ -69,7 +69,7 @@ TEST_F(ReporterTest, basic_no_checks)
|
||||
|
||||
reporter.reset();
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
|
||||
ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
|
||||
ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, 0xff);
|
||||
@@ -92,7 +92,7 @@ TEST_F(ReporterTest, basic_fail_all_modes)
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control,
|
||||
events::ID("arming_test_basic_fail_all_modes_fail1"), events::Log::Info, "");
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
|
||||
ASSERT_FALSE(reporter.canArm(nav_state));
|
||||
ASSERT_TRUE(reporter.canRun(nav_state));
|
||||
@@ -113,7 +113,7 @@ TEST_F(ReporterTest, arming_checks_mode_category)
|
||||
events::ID("arming_test_arming_checks_mode_category_fail2"), events::Log::Info, "");
|
||||
reporter.setIsPresent(health_component_t::battery);
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
|
||||
ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
|
||||
ASSERT_TRUE(reporter.canRun(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
|
||||
@@ -138,7 +138,7 @@ TEST_F(ReporterTest, arming_checks_mode_category2)
|
||||
reporter.healthFailure(NavModes::Mission, health_component_t::remote_control,
|
||||
events::ID("arming_test_arming_checks_mode_category2_fail1"), events::Log::Warning, "");
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
|
||||
ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
|
||||
|
||||
@@ -178,7 +178,7 @@ TEST_F(ReporterTest, reporting)
|
||||
}
|
||||
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
|
||||
|
||||
if (i == 0) {
|
||||
@@ -219,7 +219,7 @@ TEST_F(ReporterTest, reporting)
|
||||
}
|
||||
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
|
||||
|
||||
if (i == 0) {
|
||||
@@ -265,7 +265,7 @@ TEST_F(ReporterTest, reporting_multiple)
|
||||
reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::remote_control,
|
||||
events::ID("arming_test_reporting_multiple_fail3"), events::Log::Warning, "", 55);
|
||||
reporter.finalize();
|
||||
reporter.report(false, false);
|
||||
reporter.report(false);
|
||||
ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
|
||||
|
||||
if (i == 0) {
|
||||
|
||||
@@ -75,6 +75,10 @@ static constexpr const char *battery_fault_reason_str(battery_fault_reason_t bat
|
||||
void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) {
|
||||
// Reset related failsafe flags otherwise failures from before disabling the check cause failsafes without reported reason
|
||||
reporter.failsafeFlags().battery_unhealthy = false;
|
||||
reporter.failsafeFlags().battery_low_remaining_time = false;
|
||||
reporter.failsafeFlags().battery_warning = battery_status_s::BATTERY_WARNING_NONE;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -37,7 +37,7 @@ set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_
|
||||
set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html)
|
||||
set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html)
|
||||
add_custom_command(OUTPUT ${generated_uorb_struct_field_mapping_header} ${html_output_file}
|
||||
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
|
||||
COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
|
||||
${failsafe_flags_msg_file} ${generated_uorb_struct_field_mapping_header}
|
||||
${html_template_file} ${html_output_file}
|
||||
DEPENDS
|
||||
|
||||
@@ -170,6 +170,10 @@ void FailsafeBase::removeActions(ClearCondition condition)
|
||||
|
||||
void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause)
|
||||
{
|
||||
if (_on_notify_user_cb) {
|
||||
_on_notify_user_cb(_on_notify_user_arg);
|
||||
}
|
||||
|
||||
int delay_s = (_current_delay + 500_ms) / 1_s;
|
||||
PX4_DEBUG("User notification: failsafe triggered (action=%i, delayed_action=%i, cause=%i, delay=%is)", (int)action,
|
||||
(int)delayed_action, (int)cause, delay_s);
|
||||
|
||||
@@ -165,6 +165,17 @@ public:
|
||||
bool getDeferFailsafes() const { return _defer_failsafes; }
|
||||
bool failsafeDeferred() const { return _failsafe_defer_started != 0; }
|
||||
|
||||
using UserCallback = void(*)(void *);
|
||||
|
||||
/**
|
||||
* Register a callback that is called before notifying the user.
|
||||
*/
|
||||
void setOnNotifyUserCallback(UserCallback callback, void *arg)
|
||||
{
|
||||
_on_notify_user_cb = callback;
|
||||
_on_notify_user_arg = arg;
|
||||
}
|
||||
|
||||
protected:
|
||||
enum class UserTakeoverAllowed {
|
||||
Always, ///< allow takeover (immediately)
|
||||
@@ -278,6 +289,9 @@ private:
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
UserCallback _on_notify_user_cb{nullptr};
|
||||
void *_on_notify_user_arg{nullptr};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
|
||||
(ParamFloat<px4::params::COM_FAIL_ACT_T>) _param_com_fail_act_t
|
||||
);
|
||||
|
||||
+1
-1
@@ -219,7 +219,7 @@ ControlAllocationSequentialDesaturation::mixYaw()
|
||||
// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
|
||||
// and allow some yaw response at maximum thrust
|
||||
ActuatorVector max_prev = _actuator_max;
|
||||
_actuator_max += (_actuator_max - _actuator_min) * 0.15f;
|
||||
_actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN;
|
||||
desaturateActuators(_actuator_sp, yaw);
|
||||
_actuator_max = max_prev;
|
||||
|
||||
|
||||
+4
@@ -57,6 +57,10 @@ public:
|
||||
void allocate() override;
|
||||
|
||||
void updateParameters() override;
|
||||
|
||||
// This is the minimum actuator yaw granted when the controller is saturated.
|
||||
// In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount.
|
||||
static constexpr float MINIMUM_YAW_MARGIN{0.15f};
|
||||
private:
|
||||
|
||||
/**
|
||||
|
||||
@@ -35,7 +35,7 @@ option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF)
|
||||
|
||||
# Symforce code generation TODO:fixme
|
||||
execute_process(
|
||||
COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
|
||||
COMMAND ${Python_EXECUTABLE} -m symforce.symbolic
|
||||
RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
|
||||
OUTPUT_QUIET
|
||||
)
|
||||
@@ -58,7 +58,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
|
||||
${EKF_DERIVATION_SRC_DIR}/generated/predict_covariance.h
|
||||
${EKF_DERIVATION_SRC_DIR}/generated/state.h
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py
|
||||
${Python_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py
|
||||
DEPENDS
|
||||
${EKF_DERIVATION_SRC_DIR}/derivation.py
|
||||
${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py
|
||||
@@ -92,7 +92,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
|
||||
${EKF_DERIVATION_DST_DIR}/generated/predict_covariance.h
|
||||
${EKF_DERIVATION_DST_DIR}/generated/state.h
|
||||
COMMAND
|
||||
${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
|
||||
${Python_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
|
||||
DEPENDS
|
||||
${EKF_DERIVATION_SRC_DIR}/derivation.py
|
||||
${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py
|
||||
@@ -277,6 +277,7 @@ px4_add_module(
|
||||
world_magnetic_model
|
||||
|
||||
${EKF_LIBS}
|
||||
lat_lon_alt
|
||||
bias_estimator
|
||||
output_predictor
|
||||
UNITY_BUILD
|
||||
|
||||
@@ -33,6 +33,7 @@
|
||||
|
||||
add_subdirectory(bias_estimator)
|
||||
add_subdirectory(output_predictor)
|
||||
add_subdirectory(lat_lon_alt)
|
||||
|
||||
set(EKF_LIBS)
|
||||
set(EKF_SRCS)
|
||||
@@ -154,6 +155,7 @@ target_link_libraries(ecl_EKF
|
||||
PRIVATE
|
||||
bias_estimator
|
||||
geo
|
||||
lat_lon_alt
|
||||
output_predictor
|
||||
world_magnetic_model
|
||||
${EKF_LIBS}
|
||||
|
||||
@@ -74,24 +74,21 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
}
|
||||
|
||||
estimator_aid_source2d_s aid_src{};
|
||||
Vector2f position;
|
||||
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
|
||||
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
|
||||
|
||||
if (ekf.global_origin_valid()) {
|
||||
position = ekf.global_origin().project(sample.latitude, sample.longitude);
|
||||
//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
|
||||
// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
|
||||
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
|
||||
const float pos_var = sq(pos_noise);
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
|
||||
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
|
||||
const float pos_var = sq(pos_noise);
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
|
||||
ekf.updateAidSourceStatus(aid_src,
|
||||
sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(ekf.state().pos) - position, // innovation
|
||||
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
|
||||
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
|
||||
}
|
||||
ekf.updateAidSourceStatus(aid_src,
|
||||
sample.time_us, // sample timestamp
|
||||
matrix::Vector2d(sample.latitude, sample.longitude), // observation
|
||||
pos_obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
|
||||
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
|
||||
|
||||
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
|
||||
&& ekf.control_status_flags().yaw_align;
|
||||
@@ -113,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
|
||||
} else {
|
||||
// Try to initialize using measurement
|
||||
if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
|
||||
sample.epv)) {
|
||||
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
|
||||
sample.epv)) {
|
||||
ekf.enableControlStatusAuxGpos();
|
||||
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
|
||||
_state = State::active;
|
||||
@@ -131,7 +128,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
|
||||
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
|
||||
|
||||
ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
|
||||
ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
|
||||
|
||||
ekf.resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
|
||||
@@ -73,7 +73,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
|
||||
|
||||
if (_baro_counter <= _obs_buffer_length) {
|
||||
// Initialize the pressure offset (included in the baro bias)
|
||||
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
|
||||
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -106,7 +106,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
|
||||
if (measurement_valid) {
|
||||
bias_est.setMaxStateNoise(sqrtf(measurement_var));
|
||||
bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
}
|
||||
|
||||
// determine if we should use height aiding
|
||||
@@ -131,8 +131,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
|
||||
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_hgt_to_baro = true;
|
||||
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
|
||||
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
|
||||
resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
|
||||
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
|
||||
|
||||
// reset vertical velocity if no valid sources available
|
||||
if (!isVerticalVelocityAidingActive()) {
|
||||
@@ -163,12 +163,12 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
|
||||
_height_sensor_ref = HeightSensor::BARO;
|
||||
|
||||
_information_events.flags.reset_hgt_to_baro = true;
|
||||
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
|
||||
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
|
||||
initialiseAltitudeTo(measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
|
||||
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = imu_sample.time_us;
|
||||
|
||||
@@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
|
||||
if (measurement_valid && quality_sufficient) {
|
||||
bias_est.setMaxStateNoise(sqrtf(measurement_var));
|
||||
bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
|
||||
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
}
|
||||
|
||||
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
|
||||
@@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
|
||||
|
||||
if (_height_sensor_ref == HeightSensor::EV) {
|
||||
_information_events.flags.reset_hgt_to_ev = true;
|
||||
resetVerticalPositionTo(measurement, measurement_var);
|
||||
resetAltitudeTo(-measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
bias_est.setBias(-_state.pos(2) + measurement);
|
||||
bias_est.setBias(_gpos.altitude() + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
@@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
|
||||
// All height sources are failing
|
||||
ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME);
|
||||
_information_events.flags.reset_hgt_to_ev = true;
|
||||
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
|
||||
bias_est.setBias(-_state.pos(2) + measurement);
|
||||
resetAltitudeTo(-measurement - bias_est.getBias(), measurement_var);
|
||||
bias_est.setBias(_gpos.altitude() + measurement);
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
|
||||
if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::EV)) {
|
||||
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
|
||||
_information_events.flags.reset_hgt_to_ev = true;
|
||||
resetVerticalPositionTo(measurement, measurement_var);
|
||||
resetAltitudeTo(-measurement, measurement_var);
|
||||
|
||||
_height_sensor_ref = HeightSensor::EV;
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s fusion", AID_SRC_NAME);
|
||||
bias_est.setBias(-_state.pos(2) + measurement);
|
||||
bias_est.setBias(_gpos.altitude() + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
const Vector2f position_estimate = getLocalHorizontalPosition();
|
||||
|
||||
const Vector2f measurement{pos(0), pos(1)};
|
||||
|
||||
const Vector2f measurement_var{
|
||||
@@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|
||||
if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) {
|
||||
if (quality_sufficient) {
|
||||
// reset the bias estimator
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
_ev_pos_b_est.setBias(-position_estimate + measurement);
|
||||
|
||||
} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
|
||||
// otherwise stop EV position, when quality is good again it will restart with reset bias
|
||||
@@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|
||||
ev_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
position_estimate - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
|
||||
|
||||
@@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|
||||
if (measurement_valid && quality_sufficient) {
|
||||
_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
|
||||
_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
|
||||
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
|
||||
_ev_pos_b_est.fuseBias(measurement - position_estimate,
|
||||
measurement_var + Vector2f(getStateVariance<State::pos>()));
|
||||
}
|
||||
|
||||
@@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
|
||||
// TODO: (_params.position_sensor_ref == PositionSensor::EV)
|
||||
if (_control_status.flags.gps) {
|
||||
ECL_INFO("starting %s fusion", EV_AID_SRC_NAME);
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
|
||||
_ev_pos_b_est.setFusionActive();
|
||||
|
||||
} else {
|
||||
@@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
|
||||
_ev_pos_b_est.reset();
|
||||
|
||||
} else {
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
@@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
|
||||
|
||||
if (_control_status.flags.gps && !pos_xy_fusion_failing) {
|
||||
// reset EV position bias
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
_ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement);
|
||||
|
||||
} else {
|
||||
_information_events.flags.reset_pos_to_vision = true;
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar());
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
|
||||
|
||||
} else {
|
||||
resetHorizontalPositionTo(measurement, measurement_var);
|
||||
|
||||
@@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion()
|
||||
const float obs_var = sq(_params.pos_noaid_noise);
|
||||
const float innov_gate = 3.f;
|
||||
|
||||
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate);
|
||||
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
|
||||
|
||||
const bool continuing_conditions_passing = !isVerticalAidingActive();
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
@@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion()
|
||||
void Ekf::resetFakeHgtFusion()
|
||||
{
|
||||
ECL_INFO("reset fake height fusion");
|
||||
_last_known_pos(2) = _state.pos(2);
|
||||
_last_known_gpos.setAltitude(_gpos.altitude());
|
||||
|
||||
resetVerticalVelocityToZero();
|
||||
resetHeightToLastKnown();
|
||||
@@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion()
|
||||
void Ekf::resetHeightToLastKnown()
|
||||
{
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
|
||||
resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
|
||||
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
|
||||
resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
void Ekf::stopFakeHgtFusion()
|
||||
|
||||
@@ -63,17 +63,17 @@ void Ekf::controlFakePosFusion()
|
||||
obs_var(0) = obs_var(1) = sq(0.5f);
|
||||
}
|
||||
|
||||
const Vector2f position(_last_known_pos);
|
||||
const Vector2f innovation = (_gpos - _last_known_gpos).xy();
|
||||
|
||||
const float innov_gate = 3.f;
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
_time_delayed_us,
|
||||
position, // observation
|
||||
obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
|
||||
innov_gate); // innovation gate
|
||||
Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation
|
||||
obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
|
||||
innov_gate); // innovation gate
|
||||
|
||||
const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest;
|
||||
const bool enable_fake_pos = !enable_valid_fake_pos
|
||||
@@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion()
|
||||
void Ekf::resetFakePosFusion()
|
||||
{
|
||||
ECL_INFO("reset fake position fusion");
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
_last_known_gpos.setLatLon(_gpos);
|
||||
|
||||
resetHorizontalPositionToLastKnown();
|
||||
resetHorizontalVelocityToZero();
|
||||
|
||||
@@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
|
||||
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2);
|
||||
const float gnss_alt = gps_sample.alt + pos_offset_earth(2);
|
||||
|
||||
const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
|
||||
const float measurement = gnss_alt;
|
||||
const float measurement_var = sq(noise);
|
||||
|
||||
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
|
||||
@@ -81,13 +81,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
|
||||
if (measurement_valid) {
|
||||
bias_est.setMaxStateNoise(sqrtf(measurement_var));
|
||||
bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
}
|
||||
|
||||
// determine if we should use height aiding
|
||||
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
|
||||
&& measurement_valid
|
||||
&& _pos_ref.isInitialized()
|
||||
&& _local_origin_lat_lon.isInitialized()
|
||||
&& _gps_checks_passed;
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
@@ -105,8 +105,8 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
|
||||
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_hgt_to_gps = true;
|
||||
resetVerticalPositionTo(aid_src.observation, measurement_var);
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
resetAltitudeTo(measurement, measurement_var);
|
||||
bias_est.setBias(-_gpos.altitude() + measurement);
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@@ -128,13 +128,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
|
||||
_height_sensor_ref = HeightSensor::GNSS;
|
||||
|
||||
_information_events.flags.reset_hgt_to_gps = true;
|
||||
resetVerticalPositionTo(-measurement, measurement_var);
|
||||
_gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
|
||||
|
||||
initialiseAltitudeTo(measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
bias_est.setBias(-_gpos.altitude() + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@@ -55,32 +55,6 @@
|
||||
#define MASK_GPS_VSPD (1<<8)
|
||||
#define MASK_GPS_SPOOFED (1<<9)
|
||||
|
||||
void Ekf::collect_gps(const gnssSample &gps)
|
||||
{
|
||||
if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) {
|
||||
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
|
||||
setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
|
||||
|
||||
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
|
||||
if (!PX4_ISFINITE(_gps_alt_ref)) {
|
||||
setAltOriginFromCurrentPos(gps.alt, gps.vacc);
|
||||
}
|
||||
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
|
||||
ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
|
||||
_pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
|
||||
|
||||
} else {
|
||||
// a rough 2D fix is sufficient to lookup earth spin rate
|
||||
const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
|
||||
|
||||
if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) {
|
||||
_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::runGnssChecks(const gnssSample &gps)
|
||||
{
|
||||
_gps_check_fail_status.flags.spoofed = gps.spoofed;
|
||||
|
||||
@@ -67,11 +67,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
&& isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
|
||||
if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
|
||||
// First time checks are passing, latching.
|
||||
if (!_gps_checks_passed) {
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
}
|
||||
|
||||
_gps_checks_passed = true;
|
||||
}
|
||||
|
||||
collect_gps(gnss_sample);
|
||||
|
||||
} else {
|
||||
// Skip this sample
|
||||
_gps_data_ready = false;
|
||||
@@ -82,10 +84,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
}
|
||||
}
|
||||
|
||||
if (_pos_ref.isInitialized()) {
|
||||
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
|
||||
}
|
||||
|
||||
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
|
||||
updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
|
||||
|
||||
} else if (_control_status.flags.gps) {
|
||||
@@ -108,9 +107,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
|
||||
const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled)
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& _pos_ref.isInitialized();
|
||||
&& _control_status.flags.yaw_align;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
|
||||
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
if (continuing_conditions_passing) {
|
||||
@@ -174,6 +173,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
}
|
||||
|
||||
_control_status.flags.gps = true;
|
||||
|
||||
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
|
||||
resetHorizontalPositionToGnss(_aid_src_gnss_pos);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -221,8 +223,10 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
|
||||
{
|
||||
// correct position and height for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy();
|
||||
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
|
||||
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
|
||||
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
|
||||
const Vector2f innovation = (_gpos - measurement_corrected).xy();
|
||||
|
||||
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
|
||||
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
|
||||
@@ -237,12 +241,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
|
||||
|
||||
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
observation, // observation
|
||||
pos_obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
innovation, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
@@ -322,8 +327,9 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
|
||||
void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
|
||||
_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
|
||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||
aid_src.observation_variance[0] +
|
||||
aid_src.observation_variance[1]);
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
}
|
||||
|
||||
@@ -90,12 +90,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
|
||||
if (global_origin_valid()
|
||||
&& (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
|
||||
) {
|
||||
// position of local NED origin in GPS / WGS84 frame
|
||||
double latitude_deg;
|
||||
double longitude_deg;
|
||||
global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
|
||||
|
||||
if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
|
||||
if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) {
|
||||
wmm_updated = true;
|
||||
}
|
||||
|
||||
@@ -368,7 +363,7 @@ bool Ekf::checkHaglYawResetReq() const
|
||||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() + _gpos.altitude()) > mag_anomalies_max_hagl;
|
||||
return above_mag_anomalies;
|
||||
}
|
||||
|
||||
|
||||
@@ -230,12 +230,6 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample)
|
||||
const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
|
||||
resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);
|
||||
|
||||
// reset position, estimate is relative to initial position in this mode, so we start with zero error
|
||||
if (!_control_status.flags.in_air) {
|
||||
ECL_INFO("reset position to zero");
|
||||
resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f);
|
||||
}
|
||||
|
||||
resetAidSourceStatusZeroInnovation(_aid_src_optical_flow);
|
||||
|
||||
_innov_check_fail_status.flags.reject_optflow_X = false;
|
||||
@@ -247,7 +241,7 @@ void Ekf::resetTerrainToFlow()
|
||||
ECL_INFO("reset hagl to flow");
|
||||
|
||||
// TODO: use the flow data
|
||||
const float new_terrain = fmaxf(0.0f, _state.pos(2));
|
||||
const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
|
||||
const float delta_terrain = new_terrain - _state.terrain;
|
||||
_state.terrain = new_terrain;
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
|
||||
|
||||
@@ -67,7 +67,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
|
||||
|
||||
// recalculate the innovation using the updated state
|
||||
const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
|
||||
_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1];
|
||||
_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
|
||||
(_aid_src_optical_flow.observation[1]);
|
||||
}
|
||||
|
||||
if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {
|
||||
|
||||
@@ -148,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance);
|
||||
resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
|
||||
_state.terrain = 0.f;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
stopRngTerrFusion();
|
||||
@@ -180,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
|
||||
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
|
||||
resetAltitudeTo(aid_src.observation - _state.terrain);
|
||||
|
||||
// reset vertical velocity if no valid sources available
|
||||
if (!isVerticalVelocityAidingActive()) {
|
||||
|
||||
@@ -74,7 +74,7 @@ void Ekf::reset()
|
||||
//
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
// assume a ground clearance
|
||||
_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
@@ -97,7 +97,7 @@ void Ekf::reset()
|
||||
resetGpsDriftCheckFilters();
|
||||
_gps_checks_passed = false;
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
_gps_alt_ref = NAN;
|
||||
_local_origin_alt = NAN;
|
||||
|
||||
_output_predictor.reset();
|
||||
|
||||
@@ -113,7 +113,7 @@ void Ekf::reset()
|
||||
_time_last_heading_fuse = 0;
|
||||
_time_last_terrain_fuse = 0;
|
||||
|
||||
_last_known_pos.setZero();
|
||||
_last_known_gpos.setZero();
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_counter = 0;
|
||||
@@ -168,7 +168,7 @@ bool Ekf::update()
|
||||
// control fusion of observation data
|
||||
controlFusionModes(imu_sample_delayed);
|
||||
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos,
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos,
|
||||
_state.gyro_bias, _state.accel_bias);
|
||||
|
||||
return true;
|
||||
@@ -205,7 +205,7 @@ bool Ekf::initialiseFilter()
|
||||
initialiseCovariance();
|
||||
|
||||
// reset the output predictor state history to match the EKF initial values
|
||||
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
|
||||
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -230,6 +230,11 @@ bool Ekf::initialiseTilt()
|
||||
|
||||
void Ekf::predictState(const imuSample &imu_delayed)
|
||||
{
|
||||
if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) {
|
||||
_earth_rate_lat_ref_rad = _gpos.latitude_rad();
|
||||
_earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad);
|
||||
}
|
||||
|
||||
// apply imu bias corrections
|
||||
const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt;
|
||||
Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled;
|
||||
@@ -254,15 +259,18 @@ void Ekf::predictState(const imuSample &imu_delayed)
|
||||
// calculate the increment in velocity using the current orientation
|
||||
_state.vel += corrected_delta_vel_ef;
|
||||
|
||||
// compensate for acceleration due to gravity
|
||||
_state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt;
|
||||
// compensate for acceleration due to gravity, Coriolis and transport rate
|
||||
const Vector3f gravity_acceleration(0.f, 0.f, CONSTANTS_ONE_G); // simplistic model
|
||||
const Vector3f coriolis_acceleration = -2.f * _earth_rate_NED.cross(vel_last);
|
||||
const Vector3f transport_rate = -_gpos.computeAngularRateNavFrame(vel_last).cross(vel_last);
|
||||
_state.vel += (gravity_acceleration + coriolis_acceleration + transport_rate) * imu_delayed.delta_vel_dt;
|
||||
|
||||
// predict position states via trapezoidal integration of velocity
|
||||
_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
|
||||
_gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
|
||||
_state.pos(2) = -_gpos.altitude();
|
||||
|
||||
// constrain states
|
||||
_state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit);
|
||||
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
|
||||
|
||||
|
||||
// calculate a filtered horizontal acceleration with a 1 sec time constant
|
||||
@@ -283,14 +291,12 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_pos_ref.isInitialized()) {
|
||||
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
|
||||
if (!_local_origin_lat_lon.isInitialized()) {
|
||||
if (!resetLatLonTo(latitude, longitude, sq(eph))) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_gps_alt_ref)) {
|
||||
setAltOriginFromCurrentPos(altitude, epv);
|
||||
}
|
||||
initialiseAltitudeTo(altitude, sq(epv));
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -315,12 +321,20 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
pos_correction = _state.vel * dt_s;
|
||||
}
|
||||
|
||||
{
|
||||
const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
|
||||
LatLonAlt gpos(latitude, longitude, altitude);
|
||||
bool alt_valid = true;
|
||||
|
||||
if (!checkAltitudeValidity(gpos.altitude())) {
|
||||
gpos.setAltitude(_gpos.altitude());
|
||||
alt_valid = false;
|
||||
}
|
||||
|
||||
const LatLonAlt gpos_corrected = gpos + pos_correction;
|
||||
|
||||
{
|
||||
const float obs_var = math::max(sq(eph), sq(0.01f));
|
||||
|
||||
const Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
|
||||
const Vector2f innov = (_gpos - gpos_corrected).xy();
|
||||
const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
|
||||
|
||||
const float sq_gate = sq(5.f); // magic hardcoded gate
|
||||
@@ -334,8 +348,8 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
ECL_INFO("reset position to external observation");
|
||||
_information_events.flags.reset_pos_to_ext_obs = true;
|
||||
|
||||
resetHorizontalPositionTo(hpos, obs_var);
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
resetHorizontalPositionTo(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var);
|
||||
_last_known_gpos.setLatLon(gpos_corrected);
|
||||
|
||||
} else {
|
||||
ECL_INFO("fuse external observation as position measurement");
|
||||
@@ -348,24 +362,16 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
_state_reset_status.posNE_change.zero();
|
||||
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
_last_known_gpos.setLatLon(gpos_corrected);
|
||||
}
|
||||
}
|
||||
|
||||
if (checkAltitudeValidity(altitude)) {
|
||||
const float altitude_corrected = altitude - pos_correction(2);
|
||||
if (alt_valid) {
|
||||
const float obs_var = math::max(sq(epv), sq(0.01f));
|
||||
|
||||
if (!PX4_ISFINITE(_gps_alt_ref)) {
|
||||
setAltOriginFromCurrentPos(altitude_corrected, epv);
|
||||
|
||||
} else {
|
||||
const float vpos = -(altitude_corrected - _gps_alt_ref);
|
||||
const float obs_var = math::max(sq(epv), sq(0.01f));
|
||||
|
||||
ECL_INFO("reset height to external observation");
|
||||
resetVerticalPositionTo(vpos, obs_var);
|
||||
_last_known_pos(2) = _state.pos(2);
|
||||
}
|
||||
ECL_INFO("reset height to external observation");
|
||||
initialiseAltitudeTo(gpos_corrected.altitude(), obs_var);
|
||||
_last_known_gpos.setAltitude(gpos_corrected.altitude());
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -425,9 +431,10 @@ void Ekf::print_status()
|
||||
(double)getStateVariance<State::vel>()(2)
|
||||
);
|
||||
|
||||
const Vector3f position = getPosition();
|
||||
printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
|
||||
State::pos.idx, State::pos.idx + State::pos.dof - 1,
|
||||
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
|
||||
(double)position(0), (double)position(1), (double) position(2),
|
||||
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1),
|
||||
(double)getStateVariance<State::pos>()(2)
|
||||
);
|
||||
|
||||
+33
-23
@@ -66,6 +66,8 @@
|
||||
# include "aid_sources/aux_global_position/aux_global_position.hpp"
|
||||
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
|
||||
|
||||
#include "lat_lon_alt/lat_lon_alt.hpp"
|
||||
|
||||
enum class Likelihood { LOW, MEDIUM, HIGH };
|
||||
class ExternalVisionVel;
|
||||
|
||||
@@ -102,8 +104,8 @@ public:
|
||||
bool isTerrainEstimateValid() const { return _terrain_valid; }
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
float getTerrainVertPos() const { return _state.terrain; };
|
||||
float getHagl() const { return _state.terrain - _state.pos(2); }
|
||||
float getTerrainVertPos() const { return _state.terrain + getEkfGlobalOriginAltitude(); };
|
||||
float getHagl() const { return _state.terrain + _gpos.altitude(); }
|
||||
|
||||
// get the terrain variance
|
||||
float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
|
||||
@@ -192,8 +194,8 @@ public:
|
||||
bool checkLatLonValidity(double latitude, double longitude);
|
||||
bool checkAltitudeValidity(float altitude);
|
||||
bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
|
||||
bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN,
|
||||
float epv = NAN);
|
||||
bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN,
|
||||
float epv = NAN);
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
|
||||
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
|
||||
@@ -217,17 +219,14 @@ public:
|
||||
void resetAccelBias();
|
||||
void resetAccelBiasCov();
|
||||
|
||||
// return true if the global position estimate is valid
|
||||
// return true if the origin is set we are not doing unconstrained free inertial navigation
|
||||
// and have not started using synthetic position observations to constrain drift
|
||||
bool isGlobalHorizontalPositionValid() const
|
||||
{
|
||||
return _pos_ref.isInitialized() && isLocalHorizontalPositionValid();
|
||||
return _local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid();
|
||||
}
|
||||
|
||||
bool isGlobalVerticalPositionValid() const
|
||||
{
|
||||
return _pos_ref.isInitialized() && isLocalVerticalPositionValid();
|
||||
return PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid();
|
||||
}
|
||||
|
||||
bool isLocalHorizontalPositionValid() const
|
||||
@@ -375,8 +374,6 @@ public:
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
void collect_gps(const gnssSample &gps);
|
||||
|
||||
// set minimum continuous period without GPS fail required to mark a healthy GPS status
|
||||
void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
|
||||
|
||||
@@ -473,6 +470,8 @@ private:
|
||||
|
||||
StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
|
||||
|
||||
LatLonAlt _gpos{0.0, 0.0, 0.f};
|
||||
|
||||
bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
|
||||
|
||||
uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
|
||||
@@ -486,9 +485,10 @@ private:
|
||||
uint64_t _time_last_heading_fuse{0};
|
||||
uint64_t _time_last_terrain_fuse{0};
|
||||
|
||||
Vector3f _last_known_pos{}; ///< last known local position vector (m)
|
||||
LatLonAlt _last_known_gpos{};
|
||||
|
||||
Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
|
||||
Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
|
||||
double _earth_rate_lat_ref_rad{0.0}; ///< latitude at which the earth rate was evaluated (radians)
|
||||
|
||||
Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction
|
||||
|
||||
@@ -645,11 +645,11 @@ private:
|
||||
P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
|
||||
}
|
||||
|
||||
bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
|
||||
bool setAltOrigin(float altitude, float epv = NAN);
|
||||
bool setLatLonOrigin(double latitude, double longitude, float hpos_var = NAN);
|
||||
bool setAltOrigin(float altitude, float vpos_var = NAN);
|
||||
|
||||
bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
|
||||
bool setAltOriginFromCurrentPos(float altitude, float epv = NAN);
|
||||
bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN);
|
||||
bool initialiseAltitudeTo(float altitude, float vpos_var = NAN);
|
||||
|
||||
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
|
||||
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
|
||||
@@ -711,14 +711,22 @@ private:
|
||||
|
||||
void resetHorizontalPositionToLastKnown();
|
||||
|
||||
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
|
||||
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
|
||||
void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude,
|
||||
const Vector2f &new_horz_pos_var);
|
||||
void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, const float pos_var = NAN) { resetHorizontalPositionTo(new_latitude, new_longitude, Vector2f(pos_var, pos_var)); }
|
||||
void resetHorizontalPositionTo(const Vector2f &new_pos, const Vector2f &new_horz_pos_var);
|
||||
|
||||
Vector2f getLocalHorizontalPosition() const;
|
||||
|
||||
Vector2f computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const;
|
||||
void updateHorizontalPositionResetStatus(const Vector2f &delta);
|
||||
|
||||
void resetWindTo(const Vector2f &wind, const Vector2f &wind_var);
|
||||
|
||||
bool isHeightResetRequired() const;
|
||||
|
||||
void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);
|
||||
void resetAltitudeTo(float new_altitude, float new_vert_pos_var = NAN);
|
||||
void updateVerticalPositionResetStatus(const float delta_z);
|
||||
|
||||
void resetVerticalVelocityToZero();
|
||||
|
||||
@@ -740,6 +748,7 @@ private:
|
||||
void controlTerrainFakeFusion();
|
||||
|
||||
void updateTerrainValidity();
|
||||
void updateTerrainResetStatus(const float delta_z);
|
||||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// update the terrain vertical position estimate using a height above ground measurement from the range finder
|
||||
@@ -832,6 +841,7 @@ private:
|
||||
void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
|
||||
void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient,
|
||||
bool reset, estimator_aid_source2d_s &aid_src);
|
||||
|
||||
void stopEvPosFusion();
|
||||
void stopEvHgtFusion();
|
||||
void stopEvVelFusion();
|
||||
@@ -1049,9 +1059,9 @@ private:
|
||||
}
|
||||
|
||||
// helper used for populating and filtering estimator aid source struct for logging
|
||||
template <typename T, typename S>
|
||||
template <typename T, typename S, typename D>
|
||||
void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample,
|
||||
const S &observation, const S &observation_variance,
|
||||
const D &observation, const S &observation_variance,
|
||||
const S &innovation, const S &innovation_variance,
|
||||
float innovation_gate = 1.f) const
|
||||
{
|
||||
@@ -1106,7 +1116,7 @@ private:
|
||||
|
||||
status.test_ratio[i] = test_ratio;
|
||||
|
||||
status.observation[i] = observation(i);
|
||||
status.observation[i] = static_cast<double>(observation(i));
|
||||
status.observation_variance[i] = observation_variance(i);
|
||||
|
||||
status.innovation[i] = innovation(i);
|
||||
|
||||
@@ -65,9 +65,9 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
|
||||
|
||||
void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
|
||||
{
|
||||
origin_time = _pos_ref.getProjectionReferenceTimestamp();
|
||||
latitude = _pos_ref.getProjectionReferenceLat();
|
||||
longitude = _pos_ref.getProjectionReferenceLon();
|
||||
origin_time = _local_origin_lat_lon.getProjectionReferenceTimestamp();
|
||||
latitude = _local_origin_lat_lon.getProjectionReferenceLat();
|
||||
longitude = _local_origin_lat_lon.getProjectionReferenceLon();
|
||||
origin_alt = getEkfGlobalOriginAltitude();
|
||||
}
|
||||
|
||||
@@ -85,156 +85,170 @@ bool Ekf::checkAltitudeValidity(const float altitude)
|
||||
return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
|
||||
}
|
||||
|
||||
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
|
||||
const float epv)
|
||||
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float hpos_var,
|
||||
const float vpos_var)
|
||||
{
|
||||
if (!setLatLonOrigin(latitude, longitude, eph)) {
|
||||
if (!setLatLonOrigin(latitude, longitude, hpos_var)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// altitude is optional
|
||||
setAltOrigin(altitude, epv);
|
||||
setAltOrigin(altitude, vpos_var);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
|
||||
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var)
|
||||
{
|
||||
if (!checkLatLonValidity(latitude, longitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool current_pos_available = false;
|
||||
double current_lat = static_cast<double>(NAN);
|
||||
double current_lon = static_cast<double>(NAN);
|
||||
if (!_local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid()) {
|
||||
// Already navigating in a local frame, use the origin to initialize global position
|
||||
const Vector2f pos_prev = getLocalHorizontalPosition();
|
||||
_local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us);
|
||||
double new_latitude;
|
||||
double new_longitude;
|
||||
_local_origin_lat_lon.reproject(pos_prev(0), pos_prev(1), new_latitude, new_longitude);
|
||||
resetHorizontalPositionTo(new_latitude, new_longitude, hpos_var);
|
||||
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigating
|
||||
if (_pos_ref.isInitialized() && isLocalHorizontalPositionValid()) {
|
||||
_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
|
||||
current_pos_available = true;
|
||||
}
|
||||
|
||||
// reinitialize map projection to latitude, longitude, altitude, and reset position
|
||||
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
|
||||
|
||||
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
|
||||
_gpos_origin_eph = eph;
|
||||
}
|
||||
|
||||
if (current_pos_available) {
|
||||
// reset horizontal position if we already have a global origin
|
||||
Vector2f position = _pos_ref.project(current_lat, current_lon);
|
||||
resetHorizontalPositionTo(position);
|
||||
} else {
|
||||
// Simply move the origin and compute the change in local position
|
||||
const Vector2f pos_prev = getLocalHorizontalPosition();
|
||||
_local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us);
|
||||
const Vector2f pos_new = getLocalHorizontalPosition();
|
||||
const Vector2f delta_pos = pos_new - pos_prev;
|
||||
updateHorizontalPositionResetStatus(delta_pos);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setAltOrigin(const float altitude, const float epv)
|
||||
bool Ekf::setAltOrigin(const float altitude, const float vpos_var)
|
||||
{
|
||||
if (!checkAltitudeValidity(altitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float gps_alt_ref_prev = _gps_alt_ref;
|
||||
_gps_alt_ref = altitude;
|
||||
ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt,
|
||||
(double)altitude);
|
||||
|
||||
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
|
||||
_gpos_origin_epv = epv;
|
||||
}
|
||||
if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) {
|
||||
const float local_alt_prev = _gpos.altitude();
|
||||
_local_origin_alt = altitude;
|
||||
resetAltitudeTo(local_alt_prev + _local_origin_alt);
|
||||
|
||||
if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) {
|
||||
// determine current z
|
||||
const float z_prev = _state.pos(2);
|
||||
const float current_alt = -z_prev + gps_alt_ref_prev;
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
resetVerticalPositionTo(_gps_alt_ref - current_alt);
|
||||
ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
|
||||
(double)_state.pos(2));
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
// adjust existing GPS height bias
|
||||
_gps_hgt_b_est.setBias(gps_hgt_bias);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
} else {
|
||||
const float delta_origin_alt = altitude - _local_origin_alt;
|
||||
_local_origin_alt = altitude;
|
||||
updateVerticalPositionResetStatus(-delta_origin_alt);
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
updateTerrainResetStatus(-delta_origin_alt);
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
|
||||
const float eph, const float epv)
|
||||
bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude,
|
||||
const float hpos_var, const float vpos_var)
|
||||
{
|
||||
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
|
||||
if (!resetLatLonTo(latitude, longitude, hpos_var)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// altitude is optional
|
||||
setAltOriginFromCurrentPos(altitude, epv);
|
||||
initialiseAltitudeTo(altitude, vpos_var);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
|
||||
bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var)
|
||||
{
|
||||
if (!checkLatLonValidity(latitude, longitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_pos_ref.initReference(latitude, longitude, _time_delayed_us);
|
||||
Vector2f pos_prev;
|
||||
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigating
|
||||
if (isLocalHorizontalPositionValid()) {
|
||||
double est_lat;
|
||||
double est_lon;
|
||||
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
|
||||
_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
|
||||
if (!_local_origin_lat_lon.isInitialized()) {
|
||||
MapProjection zero_ref;
|
||||
zero_ref.initReference(0.0, 0.0);
|
||||
pos_prev = zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg());
|
||||
|
||||
_local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us);
|
||||
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigating
|
||||
if (isLocalHorizontalPositionValid()) {
|
||||
double est_lat;
|
||||
double est_lon;
|
||||
_local_origin_lat_lon.reproject(-pos_prev(0), -pos_prev(1), est_lat, est_lon);
|
||||
_local_origin_lat_lon.initReference(est_lat, est_lon, _time_delayed_us);
|
||||
}
|
||||
|
||||
ECL_INFO("Origin set to lat=%.6f, lon=%.6f",
|
||||
_local_origin_lat_lon.getProjectionReferenceLat(), _local_origin_lat_lon.getProjectionReferenceLon());
|
||||
|
||||
} else {
|
||||
pos_prev = _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg());
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
|
||||
_gpos_origin_eph = eph;
|
||||
_gpos.setLatLonDeg(latitude, longitude);
|
||||
_output_predictor.resetLatLonTo(latitude, longitude);
|
||||
|
||||
const Vector2f delta_horz_pos = getLocalHorizontalPosition() - pos_prev;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
updateHorizontalPositionResetStatus(delta_horz_pos);
|
||||
|
||||
if (PX4_ISFINITE(hpos_var)) {
|
||||
P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, math::max(sq(0.01f), hpos_var));
|
||||
}
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
|
||||
bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var)
|
||||
{
|
||||
if (!checkAltitudeValidity(altitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_gps_alt_ref = altitude + _state.pos(2);
|
||||
if (!PX4_ISFINITE(_local_origin_alt)) {
|
||||
const float local_alt_prev = _gpos.altitude();
|
||||
|
||||
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
|
||||
_gpos_origin_epv = epv;
|
||||
if (isLocalVerticalPositionValid()) {
|
||||
_local_origin_alt = altitude - local_alt_prev;
|
||||
|
||||
} else {
|
||||
_local_origin_alt = altitude;
|
||||
}
|
||||
|
||||
ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt);
|
||||
}
|
||||
|
||||
resetAltitudeTo(altitude, vpos_var);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
{
|
||||
float eph = INFINITY;
|
||||
float epv = INFINITY;
|
||||
|
||||
if (global_origin_valid()) {
|
||||
// report absolute accuracy taking into account the uncertainty in location of the origin
|
||||
eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph));
|
||||
epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv));
|
||||
get_ekf_lpos_accuracy(ekf_eph, ekf_epv);
|
||||
|
||||
if (_horizontal_deadreckon_time_exceeded) {
|
||||
float lpos_eph = 0.f;
|
||||
float lpos_epv = 0.f;
|
||||
get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv);
|
||||
|
||||
eph = math::max(eph, lpos_eph);
|
||||
epv = math::max(epv, lpos_epv);
|
||||
}
|
||||
} else {
|
||||
*ekf_eph = INFINITY;
|
||||
*ekf_epv = INFINITY;
|
||||
}
|
||||
|
||||
*ekf_eph = eph;
|
||||
*ekf_epv = epv;
|
||||
}
|
||||
|
||||
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
@@ -726,7 +740,13 @@ void Ekf::fuse(const VectorState &K, float innovation)
|
||||
_state.vel = matrix::constrain(_state.vel - K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f);
|
||||
|
||||
// pos
|
||||
_state.pos = matrix::constrain(_state.pos - K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f);
|
||||
const Vector3f pos_correction = K.slice<State::pos.dof, 1>(State::pos.idx, 0) * (-innovation);
|
||||
|
||||
// Accumulate position in global coordinates
|
||||
_gpos += pos_correction;
|
||||
_state.pos.zero();
|
||||
// Also store altitude in the state vector as this is used for optical flow fusion
|
||||
_state.pos(2) = -_gpos.altitude();
|
||||
|
||||
// gyro_bias
|
||||
_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx,
|
||||
|
||||
@@ -568,6 +568,26 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
return true;
|
||||
}
|
||||
|
||||
Vector3f EstimatorInterface::getPosition() const
|
||||
{
|
||||
LatLonAlt lla = _output_predictor.getLatLonAlt();
|
||||
float x;
|
||||
float y;
|
||||
|
||||
if (_local_origin_lat_lon.isInitialized()) {
|
||||
_local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
|
||||
|
||||
} else {
|
||||
MapProjection zero_ref;
|
||||
zero_ref.initReference(0.0, 0.0);
|
||||
zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y);
|
||||
}
|
||||
|
||||
const float z = -(lla.altitude() - getEkfGlobalOriginAltitude());
|
||||
|
||||
return Vector3f(x, y, z);
|
||||
}
|
||||
|
||||
bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
|
||||
{
|
||||
return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag);
|
||||
|
||||
@@ -42,6 +42,7 @@
|
||||
#ifndef EKF_ESTIMATOR_INTERFACE_H
|
||||
#define EKF_ESTIMATOR_INTERFACE_H
|
||||
|
||||
#include "lat_lon_alt/lat_lon_alt.hpp"
|
||||
#if defined(MODULE_NAME)
|
||||
#include <px4_platform_common/log.h>
|
||||
# define ECL_INFO PX4_DEBUG
|
||||
@@ -241,7 +242,8 @@ public:
|
||||
Vector3f getVelocity() const { return _output_predictor.getVelocity(); }
|
||||
const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); }
|
||||
float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); }
|
||||
Vector3f getPosition() const { return _output_predictor.getPosition(); }
|
||||
Vector3f getPosition() const;
|
||||
LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }
|
||||
const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
@@ -307,9 +309,9 @@ public:
|
||||
const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); }
|
||||
const uint64_t &time_delayed_us() const { return _time_delayed_us; }
|
||||
|
||||
bool global_origin_valid() const { return _pos_ref.isInitialized(); }
|
||||
const MapProjection &global_origin() const { return _pos_ref; }
|
||||
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
|
||||
bool global_origin_valid() const { return _local_origin_lat_lon.isInitialized(); }
|
||||
const MapProjection &global_origin() const { return _local_origin_lat_lon; }
|
||||
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_local_origin_alt) ? _local_origin_alt : 0.f; }
|
||||
|
||||
OutputPredictor &output_predictor() { return _output_predictor; };
|
||||
|
||||
@@ -379,10 +381,8 @@ protected:
|
||||
bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
|
||||
|
||||
// Variables used to publish the WGS-84 location of the EKF local NED origin
|
||||
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
|
||||
float _gps_alt_ref{NAN}; ///< WGS-84 height (m)
|
||||
float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin
|
||||
float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
|
||||
MapProjection _local_origin_lat_lon{};
|
||||
float _local_origin_alt{NAN};
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
RingBuffer<gnssSample> *_gps_buffer {nullptr};
|
||||
|
||||
+9
-5
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
# 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
|
||||
@@ -31,9 +31,13 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(FlightTaskManualPositionSmoothVel
|
||||
FlightTaskManualPositionSmoothVel.cpp
|
||||
add_library(lat_lon_alt
|
||||
lat_lon_alt.cpp
|
||||
lat_lon_alt.hpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility)
|
||||
target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
add_dependencies(lat_lon_alt prebuild_targets)
|
||||
target_include_directories(lat_lon_alt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo)
|
||||
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "lat_lon_alt.hpp"
|
||||
|
||||
using matrix::Vector3f;
|
||||
using matrix::Vector2d;
|
||||
|
||||
Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const
|
||||
{
|
||||
double r_n;
|
||||
double r_e;
|
||||
computeRadiiOfCurvature(_latitude_rad, r_n, r_e);
|
||||
return Vector3f(
|
||||
v_ned(1) / (static_cast<float>(r_e) + _altitude),
|
||||
-v_ned(0) / (static_cast<float>(r_n) + _altitude),
|
||||
-v_ned(1) * tanf(_latitude_rad) / (static_cast<float>(r_e) + _altitude));
|
||||
}
|
||||
|
||||
Vector2d LatLonAlt::deltaLatLonToDeltaXY(const double latitude, const float altitude)
|
||||
{
|
||||
double r_n;
|
||||
double r_e;
|
||||
computeRadiiOfCurvature(latitude, r_n, r_e);
|
||||
const double dn_dlat = r_n + static_cast<double>(altitude);
|
||||
const double de_dlon = (r_e + static_cast<double>(altitude)) * cos(latitude);
|
||||
|
||||
return Vector2d(dn_dlat, de_dlon);
|
||||
}
|
||||
|
||||
void LatLonAlt::computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
|
||||
double &transverse_radius_of_curvature)
|
||||
{
|
||||
const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2);
|
||||
const double sqrt_tmp = std::sqrt(tmp);
|
||||
meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp);
|
||||
transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp;
|
||||
}
|
||||
|
||||
LatLonAlt LatLonAlt::operator+(const matrix::Vector3f &delta_pos) const
|
||||
{
|
||||
const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude());
|
||||
const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
|
||||
const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
|
||||
const float altitude = _altitude - delta_pos(2);
|
||||
|
||||
LatLonAlt lla_new;
|
||||
lla_new.setLatLonRad(latitude_rad, longitude_rad);
|
||||
lla_new.setAltitude(altitude);
|
||||
return lla_new;
|
||||
}
|
||||
|
||||
void LatLonAlt::operator+=(const matrix::Vector3f &delta_pos)
|
||||
{
|
||||
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
|
||||
_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
|
||||
_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
|
||||
_altitude -= delta_pos(2);
|
||||
}
|
||||
|
||||
void LatLonAlt::operator+=(const matrix::Vector2f &delta_pos)
|
||||
{
|
||||
matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
|
||||
_latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast<double>(delta_pos(0)) / d_lat_lon_to_d_xy(0));
|
||||
_longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast<double>(delta_pos(1)) / d_lat_lon_to_d_xy(1));
|
||||
}
|
||||
|
||||
matrix::Vector3f LatLonAlt::operator-(const LatLonAlt &lla) const
|
||||
{
|
||||
const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad());
|
||||
const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad());
|
||||
const float delta_alt = _altitude - lla.altitude();
|
||||
|
||||
const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude);
|
||||
return matrix::Vector3f(static_cast<float>(delta_lat * d_lat_lon_to_d_xy(0)),
|
||||
static_cast<float>(delta_lon * d_lat_lon_to_d_xy(1)),
|
||||
-delta_alt);
|
||||
}
|
||||
@@ -0,0 +1,113 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "mathlib/math/Limits.hpp"
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
class LatLonAlt
|
||||
{
|
||||
public:
|
||||
LatLonAlt() = default;
|
||||
LatLonAlt(const LatLonAlt &lla)
|
||||
{
|
||||
_latitude_rad = lla.latitude_rad();
|
||||
_longitude_rad = lla.longitude_rad();
|
||||
_altitude = lla.altitude();
|
||||
}
|
||||
|
||||
LatLonAlt(const double latitude_deg, const double longitude_deg, const float altitude_m)
|
||||
{
|
||||
_latitude_rad = math::radians(latitude_deg);
|
||||
_longitude_rad = math::radians(longitude_deg);
|
||||
_altitude = altitude_m;
|
||||
}
|
||||
|
||||
void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; }
|
||||
|
||||
double latitude_deg() const { return math::degrees(latitude_rad()); }
|
||||
double longitude_deg() const { return math::degrees(longitude_rad()); }
|
||||
|
||||
const double &latitude_rad() const { return _latitude_rad; }
|
||||
const double &longitude_rad() const { return _longitude_rad; }
|
||||
float altitude() const { return _altitude; }
|
||||
|
||||
void setLatitudeDeg(const double &latitude_deg) { _latitude_rad = math::radians(latitude_deg); }
|
||||
void setLongitudeDeg(const double &longitude_deg) { _longitude_rad = math::radians(longitude_deg); }
|
||||
void setAltitude(const float altitude) { _altitude = altitude; }
|
||||
|
||||
void setLatLon(const LatLonAlt &lla) { _latitude_rad = lla.latitude_rad(); _longitude_rad = lla.longitude_rad(); }
|
||||
void setLatLonDeg(const double latitude, const double longitude) { _latitude_rad = math::radians(latitude); _longitude_rad = math::radians(longitude); }
|
||||
void setLatLonRad(const double latitude, const double longitude) { _latitude_rad = latitude; _longitude_rad = longitude; }
|
||||
|
||||
void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); }
|
||||
|
||||
/*
|
||||
* The plus and minus operators below use approximations and should only be used when the Cartesian component is small
|
||||
*/
|
||||
LatLonAlt operator+(const matrix::Vector3f &delta_pos) const;
|
||||
void operator+=(const matrix::Vector3f &delta_pos);
|
||||
void operator+=(const matrix::Vector2f &delta_pos);
|
||||
matrix::Vector3f operator-(const LatLonAlt &lla) const;
|
||||
|
||||
void operator=(const LatLonAlt &lla)
|
||||
{
|
||||
_latitude_rad = lla.latitude_rad();
|
||||
_longitude_rad = lla.longitude_rad();
|
||||
_altitude = lla.altitude();
|
||||
}
|
||||
|
||||
/*
|
||||
* Compute the angular rate of the local navigation frame at the current latitude and height
|
||||
* with respect to an inertial frame and resolved in the navigation frame
|
||||
*/
|
||||
matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const;
|
||||
|
||||
private:
|
||||
// Convert between curvilinear and cartesian errors
|
||||
static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude);
|
||||
|
||||
static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
|
||||
double &transverse_radius_of_curvature);
|
||||
|
||||
struct Wgs84 {
|
||||
static constexpr double equatorial_radius = 6378137.0;
|
||||
static constexpr double eccentricity = 0.0818191908425;
|
||||
static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity);
|
||||
};
|
||||
|
||||
double _latitude_rad{0.0};
|
||||
double _longitude_rad{0.0};
|
||||
float _altitude{0.0};
|
||||
};
|
||||
@@ -0,0 +1,107 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
#include "lat_lon_alt.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
using math::radians;
|
||||
using math::degrees;
|
||||
|
||||
TEST(TestLatLonAlt, init)
|
||||
{
|
||||
LatLonAlt lla(5.7, -2.3, 420);
|
||||
ASSERT_FLOAT_EQ(lla.latitude_deg(), 5.7);
|
||||
ASSERT_FLOAT_EQ(lla.longitude_deg(), -2.3);
|
||||
ASSERT_EQ(lla.altitude(), 420);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, set)
|
||||
{
|
||||
LatLonAlt lla(0.0, 0.0, 0);
|
||||
ASSERT_EQ(lla.latitude_rad(), 0.0);
|
||||
ASSERT_EQ(lla.longitude_rad(), 0.0);
|
||||
ASSERT_EQ(lla.altitude(), 0);
|
||||
|
||||
lla.setLatLonRad(0.1, -0.5);
|
||||
lla.setAltitude(420);
|
||||
ASSERT_EQ(lla.latitude_rad(), 0.1);
|
||||
ASSERT_EQ(lla.longitude_rad(), -0.5);
|
||||
ASSERT_EQ(lla.altitude(), 420);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, copy)
|
||||
{
|
||||
LatLonAlt lla(-0.8, -0.1, 500);
|
||||
|
||||
LatLonAlt lla_copy = lla;
|
||||
ASSERT_EQ(lla_copy.latitude_deg(), -0.8);
|
||||
ASSERT_EQ(lla_copy.longitude_deg(), -0.1);
|
||||
ASSERT_EQ(lla_copy.altitude(), 500);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, addDeltaPos)
|
||||
{
|
||||
MapProjection pos_ref(60.0, 5.0);
|
||||
LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 400.f);
|
||||
|
||||
Vector3f delta_pos(5.f, -2.f, 3.f);
|
||||
lla += delta_pos;
|
||||
|
||||
double lat_new, lon_new;
|
||||
pos_ref.reproject(delta_pos(0), delta_pos(1), lat_new, lon_new);
|
||||
|
||||
EXPECT_NEAR(lla.latitude_deg(), lat_new, 1e-6);
|
||||
EXPECT_NEAR(lla.longitude_deg(), lon_new, 1e-6);
|
||||
EXPECT_EQ(lla.altitude(), 397.f);
|
||||
}
|
||||
|
||||
TEST(TestLatLonAlt, subLatLonAlt)
|
||||
{
|
||||
MapProjection pos_ref(60.0, 5.0);
|
||||
LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 0.f);
|
||||
|
||||
const Vector3f delta_pos_true(1.f, -2.f, 3.f);
|
||||
|
||||
double lat_new, lon_new;
|
||||
pos_ref.reproject(delta_pos_true(0), delta_pos_true(1), lat_new, lon_new);
|
||||
LatLonAlt lla_new(lat_new, lon_new, -3.f);
|
||||
Vector3f delta_pos = lla_new - lla;
|
||||
|
||||
EXPECT_NEAR(delta_pos(0), delta_pos_true(0), 1e-2);
|
||||
EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
|
||||
EXPECT_EQ(delta_pos(2), delta_pos_true(2));
|
||||
}
|
||||
@@ -36,4 +36,4 @@ add_library(output_predictor
|
||||
output_predictor.h
|
||||
)
|
||||
|
||||
add_dependencies(output_predictor prebuild_targets)
|
||||
add_dependencies(output_predictor prebuild_targets lat_lon_alt)
|
||||
|
||||
@@ -69,7 +69,7 @@ void OutputPredictor::print_status()
|
||||
_output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
|
||||
}
|
||||
|
||||
void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)
|
||||
void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state)
|
||||
{
|
||||
const outputSample &output_delayed = _output_buffer.get_oldest();
|
||||
|
||||
@@ -77,9 +77,12 @@ void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f
|
||||
Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()};
|
||||
q_delta.normalize();
|
||||
|
||||
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
|
||||
// calculate the velocity delta between the output and EKF at the EKF fusion time horizon
|
||||
const Vector3f vel_delta = vel_state - output_delayed.vel;
|
||||
const Vector3f pos_delta = pos_state - output_delayed.pos;
|
||||
|
||||
// zero the position error at delayed time and reset the global reference
|
||||
const Vector3f pos_delta = -output_delayed.pos;
|
||||
_global_ref = gpos_state;
|
||||
|
||||
// loop through the output filter state history and add the deltas
|
||||
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
||||
@@ -156,29 +159,15 @@ void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel)
|
||||
_output_vert_new.vert_vel += delta_vert_vel;
|
||||
}
|
||||
|
||||
void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos)
|
||||
void OutputPredictor::resetLatLonTo(const double &new_latitude, const double &new_longitude)
|
||||
{
|
||||
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
|
||||
_output_buffer[index].pos.xy() += delta_horz_pos;
|
||||
}
|
||||
|
||||
_output_new.pos.xy() += delta_horz_pos;
|
||||
_global_ref.setLatitudeDeg(new_latitude);
|
||||
_global_ref.setLongitudeDeg(new_longitude);
|
||||
}
|
||||
|
||||
void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change)
|
||||
void OutputPredictor::resetAltitudeTo(const float new_altitude, const float vert_pos_change)
|
||||
{
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
_output_new.pos(2) += vert_pos_change;
|
||||
|
||||
// add the reset amount to the output observer buffered data
|
||||
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
||||
_output_buffer[i].pos(2) += vert_pos_change;
|
||||
_output_vert_buffer[i].vert_vel_integ += vert_pos_change;
|
||||
}
|
||||
|
||||
// add the reset amount to the output observer vertical position state
|
||||
_output_vert_new.vert_vel_integ = new_vert_pos;
|
||||
_global_ref.setAltitude(new_altitude);
|
||||
}
|
||||
|
||||
void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle,
|
||||
@@ -261,7 +250,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
|
||||
}
|
||||
|
||||
void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
|
||||
const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias,
|
||||
const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias,
|
||||
const matrix::Vector3f &accel_bias)
|
||||
{
|
||||
// calculate an average filter update time
|
||||
@@ -317,6 +306,8 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
|
||||
const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f);
|
||||
const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f);
|
||||
|
||||
const Vector3f pos_state = gpos_state - _global_ref;
|
||||
|
||||
// calculate down velocity and position tracking errors
|
||||
const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel);
|
||||
const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ);
|
||||
@@ -325,7 +316,7 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
|
||||
// using a PD feedback tuned to a 5% overshoot
|
||||
const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f;
|
||||
|
||||
applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
|
||||
applyCorrectionToVerticalOutputBuffer(vert_vel_correction, pos_state(2));
|
||||
|
||||
// calculate velocity and position tracking errors
|
||||
const Vector3f vel_err(vel_state - output_delayed.vel);
|
||||
@@ -342,10 +333,14 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
|
||||
_pos_err_integ += pos_err;
|
||||
const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
|
||||
|
||||
applyCorrectionToOutputBuffer(vel_correction, pos_correction);
|
||||
// as the reference changes, adjust the position correction to keep a constant global position
|
||||
const Vector3f pos_correction_with_ref_change = pos_correction - pos_state;
|
||||
applyCorrectionToOutputBuffer(vel_correction, pos_correction_with_ref_change);
|
||||
|
||||
_global_ref = gpos_state;
|
||||
}
|
||||
|
||||
void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
|
||||
void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel_correction, const float pos_ref_change)
|
||||
{
|
||||
// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
|
||||
// vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel
|
||||
@@ -367,7 +362,7 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre
|
||||
|
||||
// position is propagated forward using the corrected velocity and a trapezoidal integrator
|
||||
next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f *
|
||||
next_state.dt;
|
||||
next_state.dt - pos_ref_change;
|
||||
|
||||
// advance the index
|
||||
index = (index + 1) % size;
|
||||
|
||||
@@ -37,6 +37,7 @@
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include "../RingBuffer.h"
|
||||
#include "../lat_lon_alt/lat_lon_alt.hpp"
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
@@ -52,7 +53,7 @@ public:
|
||||
|
||||
// modify output filter to match the the EKF state at the fusion time horizon
|
||||
void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state,
|
||||
const matrix::Vector3f &pos_state);
|
||||
const LatLonAlt &gpos_state);
|
||||
/*
|
||||
* Implement a strapdown INS algorithm using the latest IMU data at the current time horizon.
|
||||
* Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon.
|
||||
@@ -67,7 +68,7 @@ public:
|
||||
const matrix::Vector3f &delta_velocity, const float delta_velocity_dt);
|
||||
|
||||
void correctOutputStates(const uint64_t time_delayed_us,
|
||||
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state,
|
||||
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const LatLonAlt &gpos_state,
|
||||
const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
|
||||
|
||||
void resetQuaternion(const matrix::Quatf &quat_change);
|
||||
@@ -75,8 +76,8 @@ public:
|
||||
void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel);
|
||||
void resetVerticalVelocityTo(float delta_vert_vel);
|
||||
|
||||
void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos);
|
||||
void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change);
|
||||
void resetLatLonTo(const double &new_latitude, const double &new_longitude);
|
||||
void resetAltitudeTo(float new_altitude, float vert_pos_change);
|
||||
|
||||
void print_status();
|
||||
|
||||
@@ -106,13 +107,12 @@ public:
|
||||
// get the derivative of the vertical position of the body frame origin in local NED earth frame
|
||||
float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); }
|
||||
|
||||
// get the position of the body frame origin in local earth frame
|
||||
matrix::Vector3f getPosition() const
|
||||
LatLonAlt getLatLonAlt() const
|
||||
{
|
||||
// rotate the position of the IMU relative to the boy origin into earth frame
|
||||
const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body};
|
||||
// subtract from the EKF position (which is at the IMU) to get position at the body origin
|
||||
return _output_new.pos - pos_offset_earth;
|
||||
return _global_ref + (_output_new.pos - pos_offset_earth);
|
||||
}
|
||||
|
||||
// return an array containing the output predictor angular, velocity and position tracking
|
||||
@@ -133,7 +133,7 @@ private:
|
||||
* This provides an alternative vertical velocity output that is closer to the first derivative
|
||||
* of the position but does degrade tracking relative to the EKF state.
|
||||
*/
|
||||
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
|
||||
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction, const float pos_ref_change);
|
||||
|
||||
/*
|
||||
* Calculate corrections to be applied to vel and pos output state history.
|
||||
@@ -159,6 +159,8 @@ private:
|
||||
float dt{0.f}; ///< delta time (sec)
|
||||
};
|
||||
|
||||
LatLonAlt _global_ref{0.0, 0.0, 0.f};
|
||||
|
||||
RingBuffer<outputSample> _output_buffer{12};
|
||||
RingBuffer<outputVert> _output_vert_buffer{12};
|
||||
|
||||
|
||||
@@ -36,7 +36,7 @@
|
||||
void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us,
|
||||
const float observation, const float observation_variance, const float innovation_gate) const
|
||||
{
|
||||
float innovation = _state.pos(2) - observation;
|
||||
float innovation = -_gpos.altitude() - observation;
|
||||
float innovation_variance = getStateVariance<State::pos>()(2) + observation_variance;
|
||||
|
||||
updateAidSourceStatus(aid_src, time_us,
|
||||
@@ -93,10 +93,20 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
|
||||
return aid_src.fused;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
|
||||
void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude,
|
||||
const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
|
||||
_state.pos.xy() = new_horz_pos;
|
||||
const Vector2f delta_horz_pos = computeDeltaHorizontalPosition(new_latitude, new_longitude);
|
||||
|
||||
updateHorizontalPositionResetStatus(delta_horz_pos);
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
_gpos.setLatLonDeg(new_latitude, new_longitude);
|
||||
_output_predictor.resetLatLonTo(new_latitude, new_longitude);
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
|
||||
@@ -106,54 +116,89 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1)));
|
||||
}
|
||||
|
||||
_output_predictor.resetHorizontalPositionTo(delta_horz_pos);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
|
||||
_state_reset_status.posNE_change = delta_horz_pos;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posNE_change += delta_horz_pos;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
|
||||
Vector2f Ekf::computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const
|
||||
{
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
_state.pos(2) = new_vert_pos;
|
||||
Vector2f pos;
|
||||
Vector2f pos_new;
|
||||
|
||||
if (_local_origin_lat_lon.isInitialized()) {
|
||||
_local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1));
|
||||
_local_origin_lat_lon.project(new_latitude, new_longitude, pos_new(0), pos_new(1));
|
||||
|
||||
} else {
|
||||
MapProjection zero_ref;
|
||||
zero_ref.initReference(0.0, 0.0);
|
||||
zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1));
|
||||
zero_ref.project(new_latitude, new_longitude, pos_new(0), pos_new(1));
|
||||
}
|
||||
|
||||
return pos_new - pos;
|
||||
}
|
||||
|
||||
Vector2f Ekf::getLocalHorizontalPosition() const
|
||||
{
|
||||
if (_local_origin_lat_lon.isInitialized()) {
|
||||
return _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg());
|
||||
|
||||
} else {
|
||||
MapProjection zero_ref;
|
||||
zero_ref.initReference(0.0, 0.0);
|
||||
return zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg());
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateHorizontalPositionResetStatus(const Vector2f &delta)
|
||||
{
|
||||
if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) {
|
||||
_state_reset_status.posNE_change = delta;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posNE_change += delta;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posNE++;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos,
|
||||
const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
double new_latitude;
|
||||
double new_longitude;
|
||||
|
||||
if (_local_origin_lat_lon.isInitialized()) {
|
||||
_local_origin_lat_lon.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude);
|
||||
|
||||
} else {
|
||||
MapProjection zero_ref;
|
||||
zero_ref.initReference(0.0, 0.0);
|
||||
zero_ref.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude);
|
||||
}
|
||||
|
||||
resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var);
|
||||
}
|
||||
|
||||
void Ekf::resetAltitudeTo(const float new_altitude, float new_vert_pos_var)
|
||||
{
|
||||
const float old_altitude = _gpos.altitude();
|
||||
_gpos.setAltitude(new_altitude);
|
||||
|
||||
if (PX4_ISFINITE(new_vert_pos_var)) {
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var));
|
||||
}
|
||||
|
||||
const float delta_z = new_vert_pos - old_vert_pos;
|
||||
const float delta_z = -(new_altitude - old_altitude);
|
||||
|
||||
// apply the change in height / height rate to our newest height / height rate estimate
|
||||
// which have already been taken out from the output buffer
|
||||
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
|
||||
_output_predictor.resetAltitudeTo(new_altitude, delta_z);
|
||||
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
|
||||
_state_reset_status.posD_change = delta_z;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posD_change += delta_z;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posD++;
|
||||
updateVerticalPositionResetStatus(delta_z);
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
@@ -166,9 +211,29 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
updateTerrainResetStatus(delta_z);
|
||||
_state.terrain += delta_z;
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// record the state change
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::updateVerticalPositionResetStatus(const float delta_z)
|
||||
{
|
||||
if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) {
|
||||
_state_reset_status.posD_change = delta_z;
|
||||
|
||||
} else {
|
||||
// there's already a reset this update, accumulate total delta
|
||||
_state_reset_status.posD_change += delta_z;
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.posD++;
|
||||
}
|
||||
|
||||
void Ekf::updateTerrainResetStatus(const float delta_z)
|
||||
{
|
||||
if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
|
||||
_state_reset_status.hagl_change = delta_z;
|
||||
|
||||
@@ -178,17 +243,15 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
|
||||
}
|
||||
|
||||
_state_reset_status.reset_count.hagl++;
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_delayed_us;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToLastKnown()
|
||||
{
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_gpos.latitude_deg(),
|
||||
(double)_last_known_gpos.longitude_deg());
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
|
||||
resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(),
|
||||
sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
@@ -91,20 +91,21 @@ def generate_python_function(function_name, output_names):
|
||||
def build_state_struct(state, T="float"):
|
||||
out = "struct StateSample {\n"
|
||||
|
||||
def TypeFromLength(len):
|
||||
if len == 1:
|
||||
def get_px4_type(obj):
|
||||
if isinstance(obj, sf.M11):
|
||||
return f"{T}"
|
||||
elif len == 2:
|
||||
elif isinstance(obj, sf.M21):
|
||||
return f"matrix::Vector2<{T}>"
|
||||
elif len == 3:
|
||||
elif isinstance(obj, sf.M31):
|
||||
return f"matrix::Vector3<{T}>"
|
||||
elif len == 4:
|
||||
elif isinstance(obj, sf.Rot3):
|
||||
return f"matrix::Quaternion<{T}>"
|
||||
else:
|
||||
print(f"unknown type {type(obj)}")
|
||||
raise NotImplementedError
|
||||
|
||||
for key, val in state.items():
|
||||
out += f"\t{TypeFromLength(val.storage_dim())} {key}{{}};\n"
|
||||
out += f"\t{get_px4_type(val)} {key}{{}};\n"
|
||||
|
||||
state_size = state.storage_dim()
|
||||
out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \
|
||||
|
||||
@@ -43,7 +43,7 @@
|
||||
void Ekf::initTerrain()
|
||||
{
|
||||
// assume a ground clearance
|
||||
_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
|
||||
_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
|
||||
|
||||
// use the ground clearance value as our uncertainty
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
|
||||
@@ -53,7 +53,7 @@ void Ekf::controlTerrainFakeFusion()
|
||||
{
|
||||
// If we are on ground, store the local position and time to use as a reference
|
||||
if (!_control_status.flags.in_air) {
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
_last_on_ground_posD = -_gpos.altitude();
|
||||
_control_status.flags.rng_fault = false;
|
||||
|
||||
} else if (!_control_status_prev.flags.in_air) {
|
||||
|
||||
@@ -1166,17 +1166,17 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp)
|
||||
void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.global_origin_valid() && _ekf.control_status().flags.yaw_align) {
|
||||
const Vector3f position{_ekf.getPosition()};
|
||||
|
||||
// generate and publish global position data
|
||||
vehicle_global_position_s global_pos{};
|
||||
global_pos.timestamp_sample = timestamp;
|
||||
|
||||
// Position of local NED origin in GPS / WGS84 frame
|
||||
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
|
||||
// Position GPS / WGS84 frame
|
||||
const LatLonAlt lla = _ekf.getLatLonAlt();
|
||||
global_pos.lat = lla.latitude_deg();
|
||||
global_pos.lon = lla.longitude_deg();
|
||||
global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid();
|
||||
|
||||
global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
|
||||
global_pos.alt = lla.altitude();
|
||||
global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid();
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user