mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-19 08:40:34 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 Ramon Roche
|
f0244a2f2e |
@@ -0,0 +1,233 @@
|
||||
#!/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 .')
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,904 @@
|
||||
#!/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,6 +6,11 @@
|
||||
name: Build all targets
|
||||
|
||||
on:
|
||||
workflow_dispatch:
|
||||
inputs:
|
||||
tag:
|
||||
required: true
|
||||
description: release version
|
||||
push:
|
||||
tags:
|
||||
- 'v*'
|
||||
@@ -46,15 +51,26 @@ 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 "${{ steps.set-timestamp.outputs.timestamp }}"
|
||||
echo "${{ steps.set-branch.outputs.branchname }}"
|
||||
echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)"
|
||||
run: echo "$(./Tools/ci/generate_board_targets_json.py --group --verbose)"
|
||||
|
||||
setup:
|
||||
name: Build Group [${{ matrix.group }}]
|
||||
@@ -143,16 +159,20 @@ jobs:
|
||||
# runs-on: ubuntu-latest
|
||||
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')
|
||||
if: startsWith(github.ref, 'refs/tags/v') || github.event_name == 'workflow_dispatch'
|
||||
steps:
|
||||
- name: Download Artifacts
|
||||
uses: actions/download-artifact@v4
|
||||
with:
|
||||
path: artifacts/
|
||||
merge-multiple: true
|
||||
|
||||
- name: Arrange Binaries
|
||||
run: |
|
||||
mkdir artifacts
|
||||
cp **/**/*.px4 artifacts/
|
||||
|
||||
- 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
|
||||
|
||||
@@ -25,12 +25,6 @@ jobs:
|
||||
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:
|
||||
@@ -44,16 +38,6 @@ jobs:
|
||||
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
|
||||
@@ -97,6 +81,6 @@ jobs:
|
||||
platforms: |
|
||||
linux/amd64
|
||||
provenance: mode=max
|
||||
push: true
|
||||
push: ${{ github.event_name == 'push' }}
|
||||
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: Analyzing ${{ matrix.target }}
|
||||
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
name: FLASH usage analysis
|
||||
runs-on: ubuntu-latest
|
||||
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 Target
|
||||
- name: Build
|
||||
run: make ${{ matrix.target }}
|
||||
|
||||
- name: Store the ELF with the change
|
||||
@@ -73,46 +73,36 @@ jobs:
|
||||
echo "$EOF" >> $GITHUB_OUTPUT
|
||||
|
||||
post_pr_comment:
|
||||
name: Publish Results
|
||||
runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
|
||||
name: Post PR comment
|
||||
runs-on: ubuntu-latest
|
||||
needs: [analyze_flash]
|
||||
if: ${{ github.event.pull_request }}
|
||||
steps:
|
||||
- name: Find Comment
|
||||
uses: peter-evans/find-comment@v3
|
||||
id: fc
|
||||
- name: If it's a PR add a comment with the bloaty output
|
||||
if: ${{ github.event.pull_request }}
|
||||
uses: actions/github-script@v6
|
||||
with:
|
||||
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
|
||||
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')
|
||||
})
|
||||
|
||||
+5
-10
@@ -241,24 +241,19 @@ if(NOT CMAKE_BUILD_TYPE)
|
||||
endif()
|
||||
|
||||
if((CMAKE_BUILD_TYPE STREQUAL "Debug") OR (CMAKE_BUILD_TYPE STREQUAL "Coverage"))
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPEED -O0)
|
||||
set(MAX_CUSTOM_OPT_LEVEL -O0)
|
||||
elseif(CMAKE_BUILD_TYPE MATCHES "Sanitizer")
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPEED -O1)
|
||||
set(MAX_CUSTOM_OPT_LEVEL -O1)
|
||||
elseif(CMAKE_BUILD_TYPE MATCHES "Release")
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPEED -O3)
|
||||
set(MAX_CUSTOM_OPT_LEVEL -O3)
|
||||
else()
|
||||
if(px4_constrained_flash_build)
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPEED -Os)
|
||||
set(MAX_CUSTOM_OPT_LEVEL -Os)
|
||||
else()
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPEED -O2)
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPACE -Os)
|
||||
set(MAX_CUSTOM_OPT_LEVEL -O2)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
if(NOT MAX_CUSTOM_OPT_LEVEL_SPACE)
|
||||
set(MAX_CUSTOM_OPT_LEVEL_SPACE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
endif()
|
||||
|
||||
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage;AddressSanitizer;UndefinedBehaviorSanitizer")
|
||||
message(STATUS "cmake build type: ${CMAKE_BUILD_TYPE}")
|
||||
|
||||
|
||||
@@ -75,6 +75,7 @@ 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
|
||||
|
||||
@@ -67,6 +67,7 @@ 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,6 +68,7 @@ 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
|
||||
|
||||
@@ -64,7 +64,6 @@ 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 \
|
||||
|
||||
@@ -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: 85151aaf6b...ca16e99074
@@ -7,7 +7,7 @@ uint32 device_id
|
||||
|
||||
uint64 time_last_fuse
|
||||
|
||||
float64[2] observation
|
||||
float32[2] observation
|
||||
float32[2] observation_variance
|
||||
|
||||
float32[2] innovation
|
||||
|
||||
@@ -43,4 +43,4 @@ if(PX4_TESTING)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -87,12 +87,7 @@ 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) {
|
||||
|
||||
@@ -80,7 +80,7 @@ if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx")
|
||||
${SRCS_KERNEL}
|
||||
)
|
||||
target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm heatshrink)
|
||||
target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED} -D__KERNEL__)
|
||||
target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__)
|
||||
|
||||
# User side library in nuttx kernel/protected build
|
||||
px4_add_library(uORB
|
||||
@@ -106,7 +106,7 @@ else()
|
||||
endif()
|
||||
|
||||
target_link_libraries(uORB PRIVATE uorb_msgs heatshrink)
|
||||
target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
if(PX4_TESTING)
|
||||
add_subdirectory(uORB_tests)
|
||||
|
||||
@@ -34,4 +34,4 @@
|
||||
px4_add_library(arch_dshot
|
||||
dshot.c
|
||||
)
|
||||
target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -34,6 +34,6 @@
|
||||
px4_add_library(arch_spi
|
||||
spi.cpp
|
||||
)
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses
|
||||
|
||||
@@ -36,6 +36,6 @@ px4_add_library(arch_hrt
|
||||
)
|
||||
target_compile_options(arch_hrt
|
||||
PRIVATE
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPEED}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
)
|
||||
|
||||
@@ -39,6 +39,6 @@ px4_add_library(arch_io_pins
|
||||
rp2040_pinset.c
|
||||
)
|
||||
|
||||
target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
target_link_libraries(arch_io_pins PRIVATE drivers_board)
|
||||
|
||||
@@ -34,4 +34,4 @@
|
||||
px4_add_library(arch_spi
|
||||
spi.cpp
|
||||
)
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -34,4 +34,4 @@
|
||||
px4_add_library(arch_dshot
|
||||
dshot.c
|
||||
)
|
||||
target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -36,6 +36,6 @@ px4_add_library(arch_hrt
|
||||
)
|
||||
target_compile_options(arch_hrt
|
||||
PRIVATE
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPEED}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
)
|
||||
|
||||
@@ -37,6 +37,6 @@ px4_add_library(arch_io_pins
|
||||
pwm_servo.c
|
||||
pwm_trigger.c
|
||||
)
|
||||
target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
target_link_libraries(arch_io_pins PRIVATE drivers_board) # timer_io_channels
|
||||
|
||||
@@ -34,6 +34,6 @@
|
||||
px4_add_library(arch_spi
|
||||
spi.cpp
|
||||
)
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses
|
||||
|
||||
@@ -35,7 +35,7 @@ px4_add_module(
|
||||
MODULE drivers__imu__analog_devices__adis16448
|
||||
MAIN adis16448
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPACE}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
ADIS16448.cpp
|
||||
|
||||
@@ -34,7 +34,7 @@ px4_add_module(
|
||||
MODULE drivers__imu__invensense__icm42688p
|
||||
MAIN icm42688p
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPACE}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
icm42688p_main.cpp
|
||||
|
||||
@@ -34,7 +34,7 @@ px4_add_module(
|
||||
MODULE drivers__imu__invensense__icm45686
|
||||
MAIN icm45686
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPACE}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
icm45686_main.cpp
|
||||
|
||||
@@ -34,7 +34,7 @@ px4_add_module(
|
||||
MODULE drivers__imu__invensense__iim42652
|
||||
MAIN iim42652
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPACE}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
iim42652_main.cpp
|
||||
|
||||
@@ -34,7 +34,7 @@ px4_add_module(
|
||||
MODULE drivers__imu__invensense__iim42653
|
||||
MAIN iim42653
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPACE}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
iim42653_main.cpp
|
||||
|
||||
@@ -35,7 +35,7 @@ px4_add_module(
|
||||
MODULE modules__fake_imu
|
||||
MAIN fake_imu
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPACE}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
FakeImu.cpp
|
||||
FakeImu.hpp
|
||||
|
||||
@@ -49,7 +49,7 @@ px4_add_library(cdev
|
||||
CDev.hpp
|
||||
${SRCS_PLATFORM}
|
||||
)
|
||||
target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(cdev PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
if (${PX4_PLATFORM} STREQUAL "nuttx")
|
||||
target_link_libraries(cdev PRIVATE nuttx_fs) # register/unregister driver
|
||||
|
||||
@@ -41,19 +41,51 @@
|
||||
#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(BIN_SIZE >= 5, "BIN_SIZE must be at least 5");
|
||||
static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly");
|
||||
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");
|
||||
|
||||
// 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 = BIN_SIZE;
|
||||
_obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG;
|
||||
_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 < BIN_COUNT; i++) {
|
||||
for (uint32_t i = 0 ; i < internal_bins; i++) {
|
||||
_data_timestamps[i] = current_time;
|
||||
_data_maxranges[i] = 0;
|
||||
_data_fov[i] = 0;
|
||||
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
|
||||
}
|
||||
}
|
||||
@@ -80,234 +112,48 @@ bool CollisionPrevention::is_active()
|
||||
return activated;
|
||||
}
|
||||
|
||||
void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel)
|
||||
void
|
||||
CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
|
||||
{
|
||||
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);
|
||||
|
||||
int msg_index = 0;
|
||||
float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
|
||||
float increment_factor = 1.f / obstacle.increment;
|
||||
|
||||
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 < 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);
|
||||
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);
|
||||
|
||||
// if a bin stretches over the 0/360 degree line, adjust the angles
|
||||
if (bin_lower_angle > bin_upper_angle) {
|
||||
bin_lower_angle -= 360;
|
||||
//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 (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 < 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);
|
||||
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);
|
||||
|
||||
// if a bin stretches over the 0/360 degree line, adjust the angles
|
||||
if (bin_lower_angle > bin_upper_angle) {
|
||||
bin_lower_angle -= 360;
|
||||
//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 (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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -351,60 +197,80 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_
|
||||
return false;
|
||||
}
|
||||
|
||||
bool
|
||||
CollisionPrevention::_checkSetpointDirectionFeasability()
|
||||
void
|
||||
CollisionPrevention::_updateObstacleMap()
|
||||
{
|
||||
bool setpoint_feasible = true;
|
||||
_sub_vehicle_attitude.update();
|
||||
|
||||
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;
|
||||
// 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, Quatf(_sub_vehicle_attitude.get().q));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return setpoint_feasible;
|
||||
// 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);
|
||||
}
|
||||
|
||||
void
|
||||
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)
|
||||
CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
|
||||
{
|
||||
// clamp at maximum sensor range
|
||||
float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_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) {
|
||||
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)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);
|
||||
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);
|
||||
|
||||
// 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
|
||||
Quatf attitude_sensor_frame = vehicle_attitude;
|
||||
matrix::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()); // verify
|
||||
float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
|
||||
|
||||
if (distance_reading < distance_sensor.max_distance) {
|
||||
distance_reading = distance_reading * sensor_dist_scale;
|
||||
@@ -413,7 +279,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);
|
||||
@@ -428,7 +294,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
|
||||
void
|
||||
CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
|
||||
{
|
||||
const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE);
|
||||
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 sp_index_original = setpoint_index;
|
||||
float best_cost = 9999.f;
|
||||
int new_sp_index = setpoint_index;
|
||||
@@ -440,19 +307,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 += _param_cp_dist.get() * 100.f;
|
||||
mean_dist += col_prev_d * 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 = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
|
||||
const float deviation_cost = col_prev_d * 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) {
|
||||
@@ -463,7 +330,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 * BIN_SIZE + _obstacle_map_body_frame.angle_offset);
|
||||
float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _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;
|
||||
@@ -473,7 +340,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
|
||||
float
|
||||
CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
|
||||
{
|
||||
float offset = math::max(math::radians(angle_offset), 0.f);
|
||||
float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
|
||||
|
||||
switch (distance_sensor.orientation) {
|
||||
case distance_sensor_s::ROTATION_YAW_0:
|
||||
@@ -509,147 +376,180 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
|
||||
break;
|
||||
|
||||
case distance_sensor_s::ROTATION_CUSTOM:
|
||||
offset = Eulerf(Quatf(distance_sensor.q)).psi();
|
||||
offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
|
||||
break;
|
||||
}
|
||||
|
||||
return offset;
|
||||
}
|
||||
|
||||
float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
|
||||
void
|
||||
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel)
|
||||
{
|
||||
float obstacle_distance = 0.f;
|
||||
const float direction_norm = direction.norm();
|
||||
_updateObstacleMap();
|
||||
|
||||
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;
|
||||
}
|
||||
// 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();
|
||||
|
||||
return obstacle_distance;
|
||||
}
|
||||
const float setpoint_length = setpoint.norm();
|
||||
|
||||
Vector2f
|
||||
CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length)
|
||||
{
|
||||
Vector2f new_setpoint{};
|
||||
const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir));
|
||||
const Vector2f tangential_component = _setpoint_dir - normal_component;
|
||||
const hrt_abstime constrain_time = getTime();
|
||||
int num_fov_bins = 0;
|
||||
|
||||
const float normal_scale = _getScale(_closest_dist);
|
||||
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 closest_dist_tangential = _getObstacleDistance(tangential_component);
|
||||
const float tangential_scale = _getScale(closest_dist_tangential);
|
||||
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);
|
||||
|
||||
// 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;
|
||||
// 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 {
|
||||
new_setpoint = _setpoint_dir * setpoint_length;
|
||||
}
|
||||
//allow no movement
|
||||
float vel_max = 0.f;
|
||||
setpoint = setpoint * vel_max;
|
||||
|
||||
return new_setpoint;
|
||||
}
|
||||
// if distance data is stale, switch to Loiter
|
||||
if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) {
|
||||
|
||||
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);
|
||||
if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
|
||||
&& getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
|
||||
_publishVehicleCmdDoLoiter();
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
_last_timeout_warning = getTime();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
|
||||
const Vector2f &curr_vel)
|
||||
{
|
||||
//calculate movement constraints based on range data
|
||||
Vector2f new_setpoint = original_setpoint;
|
||||
_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
|
||||
|
||||
//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);
|
||||
|
||||
_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);
|
||||
|
||||
original_setpoint = new_setpoint;
|
||||
}
|
||||
|
||||
void CollisionPrevention::_publishVehicleCmdDoLoiter()
|
||||
{
|
||||
vehicle_command_s command{};
|
||||
command.timestamp = getTime();
|
||||
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_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.param1 = (float)1; // base mode
|
||||
command.param3 = (float)0; // sub mode
|
||||
command.target_system = 1;
|
||||
command.target_component = 1;
|
||||
command.source_system = 1;
|
||||
command.source_component = 1;
|
||||
command.confirmation = false;
|
||||
command.from_external = false;
|
||||
command.timestamp = getTime();
|
||||
command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
|
||||
|
||||
// publish the vehicle command
|
||||
_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,7 +34,6 @@
|
||||
/**
|
||||
* @file CollisionPrevention.hpp
|
||||
* @author Tanja Baumann <tanja@auterion.com>
|
||||
* @author Claudio Chies <claudio@chies.com>
|
||||
*
|
||||
* CollisionPrevention controller.
|
||||
*
|
||||
@@ -75,29 +74,21 @@ public:
|
||||
|
||||
/**
|
||||
* Computes collision free setpoints
|
||||
* @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
|
||||
* @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
|
||||
*/
|
||||
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
|
||||
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
|
||||
const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
|
||||
|
||||
protected:
|
||||
/** Aggregates the sensor data into an internal obstacle map in body frame */
|
||||
void _updateObstacleMap();
|
||||
|
||||
/** 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 */
|
||||
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 */
|
||||
|
||||
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
|
||||
|
||||
@@ -105,7 +96,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 float vehicle_yaw);
|
||||
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
|
||||
|
||||
/**
|
||||
* Computes an adaption to the setpoint direction to guide towards free space
|
||||
@@ -115,34 +106,6 @@ 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
|
||||
@@ -151,39 +114,25 @@ 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 _data_stale{true}; /**< states if the data is stale */
|
||||
|
||||
bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
|
||||
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_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
|
||||
uORB::Publication<obstacle_distance_s> _obstacle_distance_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};
|
||||
@@ -193,15 +142,13 @@ 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_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*/
|
||||
(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*/
|
||||
)
|
||||
|
||||
/**
|
||||
@@ -233,11 +180,14 @@ 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
@@ -36,6 +36,6 @@ px4_add_library(conversion
|
||||
rotation.h
|
||||
)
|
||||
|
||||
target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion)
|
||||
|
||||
@@ -36,4 +36,4 @@ add_library(crc STATIC EXCLUDE_FROM_ALL
|
||||
crc.h
|
||||
)
|
||||
add_dependencies(crc prebuild_targets)
|
||||
target_compile_options(crc PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(crc PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -35,5 +35,5 @@ px4_add_library(drivers_accelerometer
|
||||
PX4Accelerometer.cpp
|
||||
PX4Accelerometer.hpp
|
||||
)
|
||||
target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
|
||||
target_compile_options(drivers_accelerometer PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_link_libraries(drivers_accelerometer PRIVATE conversion)
|
||||
|
||||
@@ -35,5 +35,5 @@ px4_add_library(drivers_gyroscope
|
||||
PX4Gyroscope.cpp
|
||||
PX4Gyroscope.hpp
|
||||
)
|
||||
target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPACE})
|
||||
target_compile_options(drivers_gyroscope PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_link_libraries(drivers_gyroscope PRIVATE conversion)
|
||||
|
||||
@@ -36,6 +36,6 @@ add_library(geo
|
||||
geo.h
|
||||
)
|
||||
add_dependencies(geo prebuild_targets)
|
||||
target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo)
|
||||
|
||||
@@ -38,5 +38,5 @@ px4_add_library(heatshrink
|
||||
)
|
||||
|
||||
target_compile_options(heatshrink PRIVATE
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPEED}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
-DHEATSHRINK_DYNAMIC_ALLOC=0)
|
||||
|
||||
@@ -61,7 +61,7 @@ px4_add_library(mixer_module
|
||||
)
|
||||
|
||||
add_dependencies(mixer_module output_functions_header)
|
||||
target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module)
|
||||
|
||||
@@ -170,6 +170,8 @@ 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]; }
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_compile_options(${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
add_compile_options(${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
if (NOT PARAM_DEFAULT_OVERRIDES)
|
||||
set(PARAM_DEFAULT_OVERRIDES "{}")
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -33,4 +33,4 @@
|
||||
|
||||
add_library(perf perf_counter.cpp)
|
||||
add_dependencies(perf prebuild_targets)
|
||||
target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(perf PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2018 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,10 +31,4 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(PID
|
||||
PID.cpp
|
||||
PID.hpp
|
||||
)
|
||||
target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)
|
||||
px4_add_library(pid pid.cpp)
|
||||
|
||||
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022-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 "PID.hpp"
|
||||
#include "lib/mathlib/math/Functions.hpp"
|
||||
|
||||
void PID::setGains(const float P, const float I, const float D)
|
||||
{
|
||||
_gain_proportional = P;
|
||||
_gain_integral = I;
|
||||
_gain_derivative = D;
|
||||
}
|
||||
|
||||
float PID::update(const float feedback, const float dt, const bool update_integral)
|
||||
{
|
||||
const float error = _setpoint - feedback;
|
||||
const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
|
||||
|
||||
if (update_integral) {
|
||||
updateIntegral(error, dt);
|
||||
}
|
||||
|
||||
_last_feedback = feedback;
|
||||
return math::constrain(output, -_limit_output, _limit_output);
|
||||
}
|
||||
|
||||
void PID::updateIntegral(float error, const float dt)
|
||||
{
|
||||
const float integral_new = _integral + _gain_integral * error * dt;
|
||||
|
||||
if (std::isfinite(integral_new)) {
|
||||
_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
|
||||
}
|
||||
}
|
||||
|
||||
float PID::updateDerivative(float feedback, const float dt)
|
||||
{
|
||||
float feedback_change = 0.f;
|
||||
|
||||
if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
|
||||
feedback_change = (feedback - _last_feedback) / dt;
|
||||
}
|
||||
|
||||
return feedback_change;
|
||||
}
|
||||
@@ -1,65 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 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 <cmath>
|
||||
|
||||
class PID
|
||||
{
|
||||
public:
|
||||
PID() = default;
|
||||
virtual ~PID() = default;
|
||||
void setOutputLimit(const float limit) { _limit_output = limit; }
|
||||
void setIntegralLimit(const float limit) { _limit_integral = limit; }
|
||||
void setGains(const float P, const float I, const float D);
|
||||
void setSetpoint(const float setpoint) { _setpoint = setpoint; }
|
||||
float update(const float feedback, const float dt, const bool update_integral = true);
|
||||
float getIntegral() { return _integral; }
|
||||
void resetIntegral() { _integral = 0.f; };
|
||||
void resetDerivative() { _last_feedback = NAN; };
|
||||
private:
|
||||
void updateIntegral(float error, const float dt);
|
||||
float updateDerivative(float feedback, const float dt);
|
||||
|
||||
float _setpoint{0.f}; ///< current setpoint to track
|
||||
float _integral{0.f}; ///< integral state
|
||||
float _last_feedback{NAN};
|
||||
|
||||
// Gains, Limits
|
||||
float _gain_proportional{0.f};
|
||||
float _gain_integral{0.f};
|
||||
float _gain_derivative{0.f};
|
||||
float _limit_integral{0.f};
|
||||
float _limit_output{0.f};
|
||||
};
|
||||
@@ -1,130 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 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 <PID.hpp>
|
||||
|
||||
TEST(PIDTest, AllZeroCase)
|
||||
{
|
||||
PID pid;
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
|
||||
}
|
||||
|
||||
TEST(PIDTest, OutputLimit)
|
||||
{
|
||||
PID pid;
|
||||
pid.setOutputLimit(.01f);
|
||||
pid.setGains(.1f, 0.f, 0.f);
|
||||
pid.setSetpoint(1.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
|
||||
EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
|
||||
EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
|
||||
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
|
||||
EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
|
||||
EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
|
||||
EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
|
||||
EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
|
||||
}
|
||||
|
||||
TEST(PIDTest, ProportinalOnly)
|
||||
{
|
||||
PID pid;
|
||||
pid.setOutputLimit(1.f);
|
||||
pid.setGains(.1f, 0.f, 0.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
|
||||
pid.setSetpoint(1.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
|
||||
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
|
||||
|
||||
float plant = 0.f;
|
||||
float output = 10000.f;
|
||||
int i; // need function scope to check how many steps
|
||||
|
||||
for (i = 1000; i > 0; i--) {
|
||||
const float output_new = pid.update(plant, 0.f, false);
|
||||
plant += output_new;
|
||||
|
||||
// expect the output to get smaller with each iteration
|
||||
if (output_new >= output) {
|
||||
break;
|
||||
}
|
||||
|
||||
output = output_new;
|
||||
}
|
||||
|
||||
EXPECT_FLOAT_EQ(plant, 1.f);
|
||||
EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
|
||||
}
|
||||
|
||||
TEST(PIDTest, InteralOpenLoop)
|
||||
{
|
||||
PID pid;
|
||||
pid.setOutputLimit(1.f);
|
||||
pid.setGains(0.f, .1f, 0.f);
|
||||
pid.setIntegralLimit(.05f);
|
||||
pid.setSetpoint(1.f);
|
||||
|
||||
// Zero error
|
||||
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
|
||||
|
||||
// Open loop ramp up
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
|
||||
|
||||
// Open loop ramp down
|
||||
pid.setSetpoint(-1.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
|
||||
EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
|
||||
EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
|
||||
EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
|
||||
EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
|
||||
pid.resetIntegral();
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
|
||||
EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
|
||||
}
|
||||
@@ -0,0 +1,185 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file pid.cpp
|
||||
*
|
||||
* Implementation of generic PID controller.
|
||||
*
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include "pid.h"
|
||||
#include <math.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
#define SIGMA 0.000001f
|
||||
|
||||
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
|
||||
{
|
||||
pid->mode = mode;
|
||||
pid->dt_min = dt_min;
|
||||
pid->kp = 0.0f;
|
||||
pid->ki = 0.0f;
|
||||
pid->kd = 0.0f;
|
||||
pid->integral = 0.0f;
|
||||
pid->integral_limit = 0.0f;
|
||||
pid->output_limit = 0.0f;
|
||||
pid->error_previous = 0.0f;
|
||||
pid->last_output = 0.0f;
|
||||
}
|
||||
|
||||
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
if (PX4_ISFINITE(kp)) {
|
||||
pid->kp = kp;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(ki)) {
|
||||
pid->ki = ki;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(kd)) {
|
||||
pid->kd = kd;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(integral_limit)) {
|
||||
pid->integral_limit = integral_limit;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(output_limit)) {
|
||||
pid->output_limit = output_limit;
|
||||
|
||||
} else {
|
||||
ret = 1;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
|
||||
{
|
||||
if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
|
||||
return pid->last_output;
|
||||
}
|
||||
|
||||
float i, d;
|
||||
|
||||
/* current error value */
|
||||
float error = sp - val;
|
||||
|
||||
/* current error derivative */
|
||||
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
|
||||
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
|
||||
pid->error_previous = error;
|
||||
|
||||
} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
|
||||
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
|
||||
pid->error_previous = -val;
|
||||
|
||||
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
|
||||
d = -val_dot;
|
||||
|
||||
} else {
|
||||
d = 0.0f;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(d)) {
|
||||
d = 0.0f;
|
||||
}
|
||||
|
||||
/* calculate PD output */
|
||||
float output = (error * pid->kp) + (d * pid->kd);
|
||||
|
||||
if (pid->ki > SIGMA) {
|
||||
// Calculate the error integral and check for saturation
|
||||
i = pid->integral + (error * dt);
|
||||
|
||||
/* check for saturation */
|
||||
if (PX4_ISFINITE(i)) {
|
||||
if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
|
||||
fabsf(i) <= pid->integral_limit) {
|
||||
/* not saturated, use new integral value */
|
||||
pid->integral = i;
|
||||
}
|
||||
}
|
||||
|
||||
/* add I component to output */
|
||||
output += pid->integral * pid->ki;
|
||||
}
|
||||
|
||||
/* limit output */
|
||||
if (PX4_ISFINITE(output)) {
|
||||
if (pid->output_limit > SIGMA) {
|
||||
if (output > pid->output_limit) {
|
||||
output = pid->output_limit;
|
||||
|
||||
} else if (output < -pid->output_limit) {
|
||||
output = -pid->output_limit;
|
||||
}
|
||||
}
|
||||
|
||||
pid->last_output = output;
|
||||
}
|
||||
|
||||
return pid->last_output;
|
||||
}
|
||||
|
||||
|
||||
__EXPORT void pid_reset_integral(PID_t *pid)
|
||||
{
|
||||
pid->integral = 0.0f;
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file pid.h
|
||||
*
|
||||
* Definition of generic PID controller.
|
||||
*
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef PID_H_
|
||||
#define PID_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
typedef enum PID_MODE {
|
||||
/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
|
||||
PID_MODE_DERIVATIV_NONE = 0,
|
||||
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
|
||||
* val_dot in pid_calculate() will be ignored */
|
||||
PID_MODE_DERIVATIV_CALC,
|
||||
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
|
||||
* setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
|
||||
PID_MODE_DERIVATIV_CALC_NO_SP,
|
||||
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
|
||||
PID_MODE_DERIVATIV_SET
|
||||
} pid_mode_t;
|
||||
|
||||
typedef struct {
|
||||
pid_mode_t mode;
|
||||
float dt_min;
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
float integral;
|
||||
float integral_limit;
|
||||
float output_limit;
|
||||
float error_previous;
|
||||
float last_output;
|
||||
} PID_t;
|
||||
|
||||
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
|
||||
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
|
||||
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
|
||||
__EXPORT void pid_reset_integral(PID_t *pid);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
#endif /* PID_H_ */
|
||||
@@ -35,7 +35,7 @@ px4_add_library(RateControl
|
||||
rate_control.cpp
|
||||
rate_control.hpp
|
||||
)
|
||||
target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(RateControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(RateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(RateControl PRIVATE mathlib)
|
||||
|
||||
|
||||
@@ -47,4 +47,4 @@ if(${PX4_PLATFORM} STREQUAL "nuttx")
|
||||
endif()
|
||||
|
||||
px4_add_library(systemlib ${SRCS})
|
||||
target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
@@ -39,7 +39,7 @@ target_compile_definitions(tinybson PRIVATE -DMODULE_NAME="tinybson")
|
||||
target_compile_options(tinybson
|
||||
PRIVATE
|
||||
-Wno-sign-compare # TODO: fix this
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPEED}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
)
|
||||
|
||||
add_dependencies(tinybson prebuild_targets)
|
||||
|
||||
@@ -53,7 +53,7 @@ add_library(world_magnetic_model
|
||||
geo_magnetic_tables.hpp
|
||||
)
|
||||
add_dependencies(world_magnetic_model prebuild_targets)
|
||||
target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
px4_add_unit_gtest(SRC test_geo_lookup.cpp LINKLIBS world_magnetic_model)
|
||||
|
||||
@@ -716,8 +716,6 @@ Commander::Commander() :
|
||||
}
|
||||
|
||||
updateParameters();
|
||||
|
||||
_failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this);
|
||||
}
|
||||
|
||||
Commander::~Commander()
|
||||
@@ -2801,8 +2799,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, "Remote ID system regained\t");
|
||||
events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "Remote ID system regained");
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2863,11 +2861,11 @@ void Commander::dataLinkCheck()
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
// Remote ID system
|
||||
// OpenDroneID 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, "Remote ID system lost");
|
||||
events::send(events::ID("commander_remote_id_lost"), events::Log::Critical, "Remote 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");
|
||||
_vehicle_status.open_drone_id_system_present = false;
|
||||
_vehicle_status.open_drone_id_system_healthy = false;
|
||||
_open_drone_id_system_lost = true;
|
||||
@@ -3002,20 +3000,6 @@ 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,9 +200,6 @@ 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 force)
|
||||
bool Report::report(bool is_armed, bool force)
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
const bool has_difference = _had_unreported_difference || _results_changed;
|
||||
@@ -317,12 +317,3 @@ bool Report::report(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,12 +337,7 @@ private:
|
||||
*/
|
||||
bool finalize();
|
||||
|
||||
bool report(bool force);
|
||||
|
||||
/**
|
||||
* Send out any unreported changes if there are any
|
||||
*/
|
||||
bool reportIfUnreportedDifferences();
|
||||
bool report(bool is_armed, bool force);
|
||||
|
||||
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(force_reporting);
|
||||
const bool reported = _reporter.report(_context.isArmed(), force_reporting);
|
||||
|
||||
if (reported) {
|
||||
|
||||
@@ -89,7 +89,7 @@ bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request)
|
||||
}
|
||||
|
||||
_reporter.finalize();
|
||||
_reporter.report(false);
|
||||
_reporter.report(_context.isArmed(), false);
|
||||
_reporter._mavlink_log_pub = nullptr;
|
||||
// LEGACY end
|
||||
|
||||
@@ -120,8 +120,3 @@ void HealthAndArmingChecks::updateParams()
|
||||
_checks[i]->updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
bool HealthAndArmingChecks::reportIfUnreportedDifferences()
|
||||
{
|
||||
return _reporter.reportIfUnreportedDifferences();
|
||||
}
|
||||
|
||||
@@ -88,8 +88,6 @@ 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);
|
||||
reporter.report(false, 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);
|
||||
reporter.report(false, 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);
|
||||
reporter.report(false, 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);
|
||||
reporter.report(false, 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);
|
||||
reporter.report(false, 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);
|
||||
reporter.report(false, 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);
|
||||
reporter.report(false, false);
|
||||
ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL));
|
||||
|
||||
if (i == 0) {
|
||||
|
||||
@@ -170,10 +170,6 @@ 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,17 +165,6 @@ 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)
|
||||
@@ -289,9 +278,6 @@ 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
|
||||
);
|
||||
|
||||
@@ -64,7 +64,7 @@ px4_add_library(ActuatorEffectiveness
|
||||
ActuatorEffectivenessRoverAckermann.cpp
|
||||
)
|
||||
|
||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ActuatorEffectiveness
|
||||
PRIVATE
|
||||
|
||||
@@ -39,7 +39,7 @@ px4_add_module(
|
||||
MODULE modules__control_allocator
|
||||
MAIN control_allocator
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPEED}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
STACK_MAIN
|
||||
3000
|
||||
SRCS
|
||||
|
||||
@@ -39,7 +39,7 @@ px4_add_library(ControlAllocation
|
||||
ControlAllocationSequentialDesaturation.cpp
|
||||
ControlAllocationSequentialDesaturation.hpp
|
||||
)
|
||||
target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL_SPEED})
|
||||
target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ControlAllocation PRIVATE mathlib)
|
||||
|
||||
|
||||
@@ -238,7 +238,7 @@ px4_add_module(
|
||||
MODULE modules__ekf2
|
||||
MAIN ekf2
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL_SPEED}
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
-fno-associative-math
|
||||
#-DDEBUG_BUILD
|
||||
#-O0
|
||||
@@ -277,7 +277,6 @@ px4_add_module(
|
||||
world_magnetic_model
|
||||
|
||||
${EKF_LIBS}
|
||||
lat_lon_alt
|
||||
bias_estimator
|
||||
output_predictor
|
||||
UNITY_BUILD
|
||||
|
||||
@@ -33,7 +33,6 @@
|
||||
|
||||
add_subdirectory(bias_estimator)
|
||||
add_subdirectory(output_predictor)
|
||||
add_subdirectory(lat_lon_alt)
|
||||
|
||||
set(EKF_LIBS)
|
||||
set(EKF_SRCS)
|
||||
@@ -155,7 +154,6 @@ target_link_libraries(ecl_EKF
|
||||
PRIVATE
|
||||
bias_estimator
|
||||
geo
|
||||
lat_lon_alt
|
||||
output_predictor
|
||||
world_magnetic_model
|
||||
${EKF_LIBS}
|
||||
|
||||
@@ -74,21 +74,24 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
}
|
||||
|
||||
estimator_aid_source2d_s aid_src{};
|
||||
const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl);
|
||||
const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used
|
||||
Vector2f position;
|
||||
|
||||
// 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);
|
||||
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);
|
||||
|
||||
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
|
||||
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
|
||||
}
|
||||
|
||||
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
|
||||
&& ekf.control_status_flags().yaw_align;
|
||||
@@ -110,8 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
|
||||
|
||||
} else {
|
||||
// Try to initialize using measurement
|
||||
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
|
||||
sample.epv)) {
|
||||
if (ekf.setEkfGlobalOriginFromCurrentPos(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;
|
||||
@@ -128,7 +131,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(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
|
||||
ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), 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(-_gpos.altitude() + _baro_lpf.getState());
|
||||
bias_est.setBias(_state.pos(2) + _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 - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), 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;
|
||||
resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
|
||||
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
|
||||
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
|
||||
bias_est.setBias(_state.pos(2) + _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;
|
||||
initialiseAltitudeTo(measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
|
||||
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
|
||||
bias_est.setBias(_state.pos(2) + _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 + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
bias_est.fuseBias(measurement - _state.pos(2), 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;
|
||||
resetAltitudeTo(-measurement, measurement_var);
|
||||
resetVerticalPositionTo(measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
bias_est.setBias(_gpos.altitude() + measurement);
|
||||
bias_est.setBias(-_state.pos(2) + 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;
|
||||
resetAltitudeTo(-measurement - bias_est.getBias(), measurement_var);
|
||||
bias_est.setBias(_gpos.altitude() + measurement);
|
||||
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
|
||||
bias_est.setBias(-_state.pos(2) + 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;
|
||||
resetAltitudeTo(-measurement, measurement_var);
|
||||
resetVerticalPositionTo(measurement, measurement_var);
|
||||
|
||||
_height_sensor_ref = HeightSensor::EV;
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s fusion", AID_SRC_NAME);
|
||||
bias_est.setBias(_gpos.altitude() + measurement);
|
||||
bias_est.setBias(-_state.pos(2) + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@@ -137,8 +137,6 @@ 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{
|
||||
@@ -152,7 +150,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(-position_estimate + measurement);
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
|
||||
} else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) {
|
||||
// otherwise stop EV position, when quality is good again it will restart with reset bias
|
||||
@@ -167,7 +165,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
|
||||
ev_sample.time_us, // sample timestamp
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
position_estimate - position, // innovation
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
|
||||
|
||||
@@ -176,7 +174,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 - position_estimate,
|
||||
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
|
||||
measurement_var + Vector2f(getStateVariance<State::pos>()));
|
||||
}
|
||||
|
||||
@@ -215,7 +213,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(-getLocalHorizontalPosition() + measurement);
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
_ev_pos_b_est.setFusionActive();
|
||||
|
||||
} else {
|
||||
@@ -247,7 +245,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
|
||||
_ev_pos_b_est.reset();
|
||||
|
||||
} else {
|
||||
_ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement);
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
@@ -277,14 +275,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(getLocalHorizontalPosition()) + measurement);
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + 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(-getLocalHorizontalPosition() + measurement);
|
||||
_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + 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_gpos.altitude(), obs_var, innov_gate);
|
||||
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), 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_gpos.setAltitude(_gpos.altitude());
|
||||
_last_known_pos(2) = _state.pos(2);
|
||||
|
||||
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_gpos.altitude());
|
||||
resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
|
||||
ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2));
|
||||
resetVerticalPositionTo(_last_known_pos(2), 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 innovation = (_gpos - _last_known_gpos).xy();
|
||||
const Vector2f position(_last_known_pos);
|
||||
|
||||
const float innov_gate = 3.f;
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
_time_delayed_us,
|
||||
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
|
||||
position, // observation
|
||||
obs_var, // observation variance
|
||||
Vector2f(_state.pos) - position, // 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_gpos.setLatLon(_gpos);
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
|
||||
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.alt + pos_offset_earth(2);
|
||||
const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2);
|
||||
|
||||
const float measurement = gnss_alt;
|
||||
const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
|
||||
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 - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), 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
|
||||
&& _local_origin_lat_lon.isInitialized()
|
||||
&& _pos_ref.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;
|
||||
resetAltitudeTo(measurement, measurement_var);
|
||||
bias_est.setBias(-_gpos.altitude() + measurement);
|
||||
resetVerticalPositionTo(aid_src.observation, measurement_var);
|
||||
bias_est.setBias(_state.pos(2) + 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;
|
||||
|
||||
initialiseAltitudeTo(measurement, measurement_var);
|
||||
resetVerticalPositionTo(-measurement, measurement_var);
|
||||
_gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
bias_est.setBias(-_gpos.altitude() + measurement);
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _time_delayed_us;
|
||||
|
||||
@@ -55,6 +55,32 @@
|
||||
#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,13 +67,11 @@ 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;
|
||||
@@ -84,7 +82,10 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
}
|
||||
}
|
||||
|
||||
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
|
||||
if (_pos_ref.isInitialized()) {
|
||||
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
|
||||
}
|
||||
|
||||
updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
|
||||
|
||||
} else if (_control_status.flags.gps) {
|
||||
@@ -107,9 +108,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;
|
||||
&& _control_status.flags.yaw_align
|
||||
&& _pos_ref.isInitialized();
|
||||
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) {
|
||||
@@ -173,9 +174,6 @@ 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -223,10 +221,8 @@ 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 = 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();
|
||||
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();
|
||||
|
||||
// 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);
|
||||
@@ -241,13 +237,12 @@ 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
|
||||
observation, // observation
|
||||
position, // observation
|
||||
pos_obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(_state.pos) - position, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
@@ -327,9 +322,8 @@ 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;
|
||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||
aid_src.observation_variance[0] +
|
||||
aid_src.observation_variance[1]);
|
||||
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
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
}
|
||||
|
||||
@@ -90,7 +90,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
|
||||
if (global_origin_valid()
|
||||
&& (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
|
||||
) {
|
||||
if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) {
|
||||
// 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)) {
|
||||
wmm_updated = true;
|
||||
}
|
||||
|
||||
@@ -363,7 +368,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() + _gpos.altitude()) > mag_anomalies_max_hagl;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
return above_mag_anomalies;
|
||||
}
|
||||
|
||||
|
||||
@@ -230,6 +230,12 @@ 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;
|
||||
@@ -241,7 +247,7 @@ void Ekf::resetTerrainToFlow()
|
||||
ECL_INFO("reset hagl to flow");
|
||||
|
||||
// TODO: use the flow data
|
||||
const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
|
||||
const float new_terrain = fmaxf(0.0f, _state.pos(2));
|
||||
const float delta_terrain = new_terrain - _state.terrain;
|
||||
_state.terrain = new_terrain;
|
||||
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
|
||||
|
||||
@@ -67,8 +67,7 @@ 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) - static_cast<float>
|
||||
(_aid_src_optical_flow.observation[1]);
|
||||
_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _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;
|
||||
resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
|
||||
resetVerticalPositionTo(-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;
|
||||
resetAltitudeTo(aid_src.observation - _state.terrain);
|
||||
resetVerticalPositionTo(-(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 = -_gpos.altitude() + _params.rng_gnd_clearance;
|
||||
_state.terrain = _state.pos(2) + _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
|
||||
_local_origin_alt = NAN;
|
||||
_gps_alt_ref = NAN;
|
||||
|
||||
_output_predictor.reset();
|
||||
|
||||
@@ -113,7 +113,7 @@ void Ekf::reset()
|
||||
_time_last_heading_fuse = 0;
|
||||
_time_last_terrain_fuse = 0;
|
||||
|
||||
_last_known_gpos.setZero();
|
||||
_last_known_pos.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, _gpos,
|
||||
_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos,
|
||||
_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, _gpos);
|
||||
_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -230,11 +230,6 @@ 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;
|
||||
@@ -259,18 +254,15 @@ 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, 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;
|
||||
// compensate for acceleration due to gravity
|
||||
_state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt;
|
||||
|
||||
// predict position states via trapezoidal integration of velocity
|
||||
_gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
|
||||
_state.pos(2) = -_gpos.altitude();
|
||||
_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
|
||||
|
||||
// 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
|
||||
@@ -291,12 +283,14 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_local_origin_lat_lon.isInitialized()) {
|
||||
if (!resetLatLonTo(latitude, longitude, sq(eph))) {
|
||||
if (!_pos_ref.isInitialized()) {
|
||||
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
initialiseAltitudeTo(altitude, sq(epv));
|
||||
if (!PX4_ISFINITE(_gps_alt_ref)) {
|
||||
setAltOriginFromCurrentPos(altitude, epv);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -321,20 +315,12 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
pos_correction = _state.vel * dt_s;
|
||||
}
|
||||
|
||||
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 Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
|
||||
|
||||
const float obs_var = math::max(sq(eph), sq(0.01f));
|
||||
|
||||
const Vector2f innov = (_gpos - gpos_corrected).xy();
|
||||
const Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
|
||||
const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
|
||||
|
||||
const float sq_gate = sq(5.f); // magic hardcoded gate
|
||||
@@ -348,8 +334,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(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var);
|
||||
_last_known_gpos.setLatLon(gpos_corrected);
|
||||
resetHorizontalPositionTo(hpos, obs_var);
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
|
||||
} else {
|
||||
ECL_INFO("fuse external observation as position measurement");
|
||||
@@ -362,16 +348,24 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl
|
||||
_state_reset_status.posNE_change.zero();
|
||||
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
_last_known_gpos.setLatLon(gpos_corrected);
|
||||
_last_known_pos.xy() = _state.pos.xy();
|
||||
}
|
||||
}
|
||||
|
||||
if (alt_valid) {
|
||||
const float obs_var = math::max(sq(epv), sq(0.01f));
|
||||
if (checkAltitudeValidity(altitude)) {
|
||||
const float altitude_corrected = altitude - pos_correction(2);
|
||||
|
||||
ECL_INFO("reset height to external observation");
|
||||
initialiseAltitudeTo(gpos_corrected.altitude(), obs_var);
|
||||
_last_known_gpos.setAltitude(gpos_corrected.altitude());
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -431,10 +425,9 @@ 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)position(0), (double)position(1), (double) position(2),
|
||||
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
|
||||
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1),
|
||||
(double)getStateVariance<State::pos>()(2)
|
||||
);
|
||||
|
||||
+23
-33
@@ -66,8 +66,6 @@
|
||||
# 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;
|
||||
|
||||
@@ -104,8 +102,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 + getEkfGlobalOriginAltitude(); };
|
||||
float getHagl() const { return _state.terrain + _gpos.altitude(); }
|
||||
float getTerrainVertPos() const { return _state.terrain; };
|
||||
float getHagl() const { return _state.terrain - _state.pos(2); }
|
||||
|
||||
// get the terrain variance
|
||||
float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
|
||||
@@ -194,8 +192,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 resetGlobalPositionTo(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);
|
||||
|
||||
// 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;
|
||||
@@ -219,14 +217,17 @@ 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 _local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid();
|
||||
return _pos_ref.isInitialized() && isLocalHorizontalPositionValid();
|
||||
}
|
||||
|
||||
bool isGlobalVerticalPositionValid() const
|
||||
{
|
||||
return PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid();
|
||||
return _pos_ref.isInitialized() && isLocalVerticalPositionValid();
|
||||
}
|
||||
|
||||
bool isLocalHorizontalPositionValid() const
|
||||
@@ -374,6 +375,8 @@ 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; }
|
||||
|
||||
@@ -470,8 +473,6 @@ 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)
|
||||
@@ -485,10 +486,9 @@ private:
|
||||
uint64_t _time_last_heading_fuse{0};
|
||||
uint64_t _time_last_terrain_fuse{0};
|
||||
|
||||
LatLonAlt _last_known_gpos{};
|
||||
Vector3f _last_known_pos{}; ///< last known local position vector (m)
|
||||
|
||||
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)
|
||||
Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s
|
||||
|
||||
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 hpos_var = NAN);
|
||||
bool setAltOrigin(float altitude, float vpos_var = NAN);
|
||||
bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
|
||||
bool setAltOrigin(float altitude, float epv = NAN);
|
||||
|
||||
bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN);
|
||||
bool initialiseAltitudeTo(float altitude, float vpos_var = NAN);
|
||||
bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
|
||||
bool setAltOriginFromCurrentPos(float altitude, float epv = 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,22 +711,14 @@ private:
|
||||
|
||||
void resetHorizontalPositionToLastKnown();
|
||||
|
||||
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 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 resetWindTo(const Vector2f &wind, const Vector2f &wind_var);
|
||||
|
||||
bool isHeightResetRequired() const;
|
||||
|
||||
void resetAltitudeTo(float new_altitude, float new_vert_pos_var = NAN);
|
||||
void updateVerticalPositionResetStatus(const float delta_z);
|
||||
void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);
|
||||
|
||||
void resetVerticalVelocityToZero();
|
||||
|
||||
@@ -748,7 +740,6 @@ 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
|
||||
@@ -841,7 +832,6 @@ 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();
|
||||
@@ -1059,9 +1049,9 @@ private:
|
||||
}
|
||||
|
||||
// helper used for populating and filtering estimator aid source struct for logging
|
||||
template <typename T, typename S, typename D>
|
||||
template <typename T, typename S>
|
||||
void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample,
|
||||
const D &observation, const S &observation_variance,
|
||||
const S &observation, const S &observation_variance,
|
||||
const S &innovation, const S &innovation_variance,
|
||||
float innovation_gate = 1.f) const
|
||||
{
|
||||
@@ -1116,7 +1106,7 @@ private:
|
||||
|
||||
status.test_ratio[i] = test_ratio;
|
||||
|
||||
status.observation[i] = static_cast<double>(observation(i));
|
||||
status.observation[i] = 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 = _local_origin_lat_lon.getProjectionReferenceTimestamp();
|
||||
latitude = _local_origin_lat_lon.getProjectionReferenceLat();
|
||||
longitude = _local_origin_lat_lon.getProjectionReferenceLon();
|
||||
origin_time = _pos_ref.getProjectionReferenceTimestamp();
|
||||
latitude = _pos_ref.getProjectionReferenceLat();
|
||||
longitude = _pos_ref.getProjectionReferenceLon();
|
||||
origin_alt = getEkfGlobalOriginAltitude();
|
||||
}
|
||||
|
||||
@@ -85,170 +85,156 @@ 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 hpos_var,
|
||||
const float vpos_var)
|
||||
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
|
||||
const float epv)
|
||||
{
|
||||
if (!setLatLonOrigin(latitude, longitude, hpos_var)) {
|
||||
if (!setLatLonOrigin(latitude, longitude, eph)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// altitude is optional
|
||||
setAltOrigin(altitude, vpos_var);
|
||||
setAltOrigin(altitude, epv);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var)
|
||||
bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
|
||||
{
|
||||
if (!checkLatLonValidity(latitude, longitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
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);
|
||||
bool current_pos_available = false;
|
||||
double current_lat = static_cast<double>(NAN);
|
||||
double current_lon = static_cast<double>(NAN);
|
||||
|
||||
} 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);
|
||||
// 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);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::setAltOrigin(const float altitude, const float vpos_var)
|
||||
bool Ekf::setAltOrigin(const float altitude, const float epv)
|
||||
{
|
||||
if (!checkAltitudeValidity(altitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt,
|
||||
(double)altitude);
|
||||
const float gps_alt_ref_prev = _gps_alt_ref;
|
||||
_gps_alt_ref = altitude;
|
||||
|
||||
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(epv) && (epv >= 0.f)) {
|
||||
_gpos_origin_epv = epv;
|
||||
}
|
||||
|
||||
} 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
|
||||
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
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude,
|
||||
const float hpos_var, const float vpos_var)
|
||||
bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
|
||||
const float eph, const float epv)
|
||||
{
|
||||
if (!resetLatLonTo(latitude, longitude, hpos_var)) {
|
||||
if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// altitude is optional
|
||||
initialiseAltitudeTo(altitude, vpos_var);
|
||||
setAltOriginFromCurrentPos(altitude, epv);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var)
|
||||
bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
|
||||
{
|
||||
if (!checkLatLonValidity(latitude, longitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
Vector2f pos_prev;
|
||||
_pos_ref.initReference(latitude, longitude, _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 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);
|
||||
}
|
||||
|
||||
_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));
|
||||
if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
|
||||
_gpos_origin_eph = eph;
|
||||
}
|
||||
|
||||
// Reset the timout timer
|
||||
_time_last_hor_pos_fuse = _time_delayed_us;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var)
|
||||
bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
|
||||
{
|
||||
if (!checkAltitudeValidity(altitude)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_local_origin_alt)) {
|
||||
const float local_alt_prev = _gpos.altitude();
|
||||
_gps_alt_ref = altitude + _state.pos(2);
|
||||
|
||||
if (isLocalVerticalPositionValid()) {
|
||||
_local_origin_alt = altitude - local_alt_prev;
|
||||
|
||||
} else {
|
||||
_local_origin_alt = altitude;
|
||||
}
|
||||
|
||||
ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt);
|
||||
if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
|
||||
_gpos_origin_epv = epv;
|
||||
}
|
||||
|
||||
resetAltitudeTo(altitude, vpos_var);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
{
|
||||
if (global_origin_valid()) {
|
||||
get_ekf_lpos_accuracy(ekf_eph, ekf_epv);
|
||||
float eph = INFINITY;
|
||||
float epv = INFINITY;
|
||||
|
||||
} else {
|
||||
*ekf_eph = INFINITY;
|
||||
*ekf_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));
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
*ekf_eph = eph;
|
||||
*ekf_epv = epv;
|
||||
}
|
||||
|
||||
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
@@ -740,13 +726,7 @@ 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
|
||||
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();
|
||||
_state.pos = matrix::constrain(_state.pos - K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f);
|
||||
|
||||
// gyro_bias
|
||||
_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx,
|
||||
|
||||
@@ -568,26 +568,6 @@ 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,7 +42,6 @@
|
||||
#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
|
||||
@@ -242,8 +241,7 @@ 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;
|
||||
LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); }
|
||||
Vector3f getPosition() const { return _output_predictor.getPosition(); }
|
||||
const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
@@ -309,9 +307,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 _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; }
|
||||
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; }
|
||||
|
||||
OutputPredictor &output_predictor() { return _output_predictor; };
|
||||
|
||||
@@ -381,8 +379,10 @@ 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 _local_origin_lat_lon{};
|
||||
float _local_origin_alt{NAN};
|
||||
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
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
RingBuffer<gnssSample> *_gps_buffer {nullptr};
|
||||
|
||||
@@ -1,108 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -1,113 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#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};
|
||||
};
|
||||
@@ -1,107 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#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 lat_lon_alt)
|
||||
add_dependencies(output_predictor prebuild_targets)
|
||||
|
||||
@@ -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 LatLonAlt &gpos_state)
|
||||
void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)
|
||||
{
|
||||
const outputSample &output_delayed = _output_buffer.get_oldest();
|
||||
|
||||
@@ -77,12 +77,9 @@ 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 delta between the output and EKF at the EKF fusion time horizon
|
||||
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
|
||||
const Vector3f vel_delta = vel_state - output_delayed.vel;
|
||||
|
||||
// zero the position error at delayed time and reset the global reference
|
||||
const Vector3f pos_delta = -output_delayed.pos;
|
||||
_global_ref = gpos_state;
|
||||
const Vector3f pos_delta = pos_state - output_delayed.pos;
|
||||
|
||||
// loop through the output filter state history and add the deltas
|
||||
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
||||
@@ -159,15 +156,29 @@ void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel)
|
||||
_output_vert_new.vert_vel += delta_vert_vel;
|
||||
}
|
||||
|
||||
void OutputPredictor::resetLatLonTo(const double &new_latitude, const double &new_longitude)
|
||||
void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos)
|
||||
{
|
||||
_global_ref.setLatitudeDeg(new_latitude);
|
||||
_global_ref.setLongitudeDeg(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;
|
||||
}
|
||||
|
||||
void OutputPredictor::resetAltitudeTo(const float new_altitude, const float vert_pos_change)
|
||||
void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change)
|
||||
{
|
||||
_global_ref.setAltitude(new_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_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;
|
||||
}
|
||||
|
||||
void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle,
|
||||
@@ -250,7 +261,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 LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias,
|
||||
const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias,
|
||||
const matrix::Vector3f &accel_bias)
|
||||
{
|
||||
// calculate an average filter update time
|
||||
@@ -306,8 +317,6 @@ 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);
|
||||
@@ -316,7 +325,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, pos_state(2));
|
||||
applyCorrectionToVerticalOutputBuffer(vert_vel_correction);
|
||||
|
||||
// calculate velocity and position tracking errors
|
||||
const Vector3f vel_err(vel_state - output_delayed.vel);
|
||||
@@ -333,14 +342,10 @@ 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;
|
||||
|
||||
// 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;
|
||||
applyCorrectionToOutputBuffer(vel_correction, pos_correction);
|
||||
}
|
||||
|
||||
void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel_correction, const float pos_ref_change)
|
||||
void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
|
||||
{
|
||||
// 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
|
||||
@@ -362,7 +367,7 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel
|
||||
|
||||
// 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 - pos_ref_change;
|
||||
next_state.dt;
|
||||
|
||||
// advance the index
|
||||
index = (index + 1) % size;
|
||||
|
||||
@@ -37,7 +37,6 @@
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include "../RingBuffer.h"
|
||||
#include "../lat_lon_alt/lat_lon_alt.hpp"
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
|
||||
@@ -53,7 +52,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 LatLonAlt &gpos_state);
|
||||
const matrix::Vector3f &pos_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.
|
||||
@@ -68,7 +67,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 LatLonAlt &gpos_state,
|
||||
const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state,
|
||||
const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias);
|
||||
|
||||
void resetQuaternion(const matrix::Quatf &quat_change);
|
||||
@@ -76,8 +75,8 @@ public:
|
||||
void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel);
|
||||
void resetVerticalVelocityTo(float delta_vert_vel);
|
||||
|
||||
void resetLatLonTo(const double &new_latitude, const double &new_longitude);
|
||||
void resetAltitudeTo(float new_altitude, float vert_pos_change);
|
||||
void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos);
|
||||
void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change);
|
||||
|
||||
void print_status();
|
||||
|
||||
@@ -107,12 +106,13 @@ 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); }
|
||||
|
||||
LatLonAlt getLatLonAlt() const
|
||||
// get the position of the body frame origin in local earth frame
|
||||
matrix::Vector3f getPosition() 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 _global_ref + (_output_new.pos - pos_offset_earth);
|
||||
return _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, const float pos_ref_change);
|
||||
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
|
||||
|
||||
/*
|
||||
* Calculate corrections to be applied to vel and pos output state history.
|
||||
@@ -159,8 +159,6 @@ 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 = -_gpos.altitude() - observation;
|
||||
float innovation = _state.pos(2) - observation;
|
||||
float innovation_variance = getStateVariance<State::pos>()(2) + observation_variance;
|
||||
|
||||
updateAidSourceStatus(aid_src, time_us,
|
||||
@@ -93,20 +93,10 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
|
||||
return aid_src.fused;
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude,
|
||||
const Vector2f &new_horz_pos_var)
|
||||
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
|
||||
{
|
||||
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);
|
||||
const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}};
|
||||
_state.pos.xy() = new_horz_pos;
|
||||
|
||||
if (PX4_ISFINITE(new_horz_pos_var(0))) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0)));
|
||||
@@ -116,89 +106,54 @@ void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &ne
|
||||
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;
|
||||
}
|
||||
|
||||
Vector2f Ekf::computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const
|
||||
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
|
||||
{
|
||||
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);
|
||||
const float old_vert_pos = _state.pos(2);
|
||||
_state.pos(2) = new_vert_pos;
|
||||
|
||||
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_altitude - old_altitude);
|
||||
const float delta_z = new_vert_pos - old_vert_pos;
|
||||
|
||||
// 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.resetAltitudeTo(new_altitude, delta_z);
|
||||
_output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z);
|
||||
|
||||
updateVerticalPositionResetStatus(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++;
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
|
||||
@@ -211,29 +166,9 @@ void Ekf::resetAltitudeTo(const float new_altitude, float new_vert_pos_var)
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
updateTerrainResetStatus(delta_z);
|
||||
_state.terrain += delta_z;
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
||||
// 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)
|
||||
{
|
||||
// record the state change
|
||||
if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
|
||||
_state_reset_status.hagl_change = delta_z;
|
||||
|
||||
@@ -243,15 +178,17 @@ void Ekf::updateTerrainResetStatus(const float delta_z)
|
||||
}
|
||||
|
||||
_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_gpos.latitude_deg(),
|
||||
(double)_last_known_gpos.longitude_deg());
|
||||
ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1));
|
||||
_information_events.flags.reset_pos_to_last_known = true;
|
||||
|
||||
// Used when falling back to non-aiding mode of operation
|
||||
resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(),
|
||||
sq(_params.pos_noaid_noise));
|
||||
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
|
||||
}
|
||||
|
||||
@@ -337,7 +337,9 @@ def predict_sideslip(
|
||||
vel_rel = state["vel"] - wind
|
||||
relative_wind_body = state["quat_nominal"].inverse() * vel_rel
|
||||
|
||||
sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon)
|
||||
# Small angle approximation of side slip model
|
||||
# Protect division by zero using epsilon
|
||||
sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
|
||||
|
||||
return sideslip_pred
|
||||
|
||||
|
||||
@@ -30,65 +30,66 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 518
|
||||
// Total ops: 513
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (50)
|
||||
const Scalar _tmp0 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp1 = 2 * _tmp0;
|
||||
const Scalar _tmp2 = 2 * state(3, 0);
|
||||
const Scalar _tmp3 = _tmp2 * state(6, 0);
|
||||
const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp6 = _tmp2 * state(0, 0);
|
||||
const Scalar _tmp7 = 2 * state(1, 0);
|
||||
const Scalar _tmp8 = _tmp7 * state(2, 0);
|
||||
const Scalar _tmp9 = _tmp6 + _tmp8;
|
||||
const Scalar _tmp10 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp11 = 2 * state(2, 0);
|
||||
const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0);
|
||||
const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0);
|
||||
const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp16 = 2 * _tmp10;
|
||||
const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0);
|
||||
const Scalar _tmp18 = std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp20 = -_tmp6 + _tmp8;
|
||||
const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0);
|
||||
const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0);
|
||||
const Scalar _tmp23 = _tmp22 / _tmp18;
|
||||
const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) -
|
||||
_tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17);
|
||||
const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2)));
|
||||
const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25;
|
||||
const Scalar _tmp27 = _tmp26 * state(3, 0);
|
||||
const Scalar _tmp28 = _tmp7 * state(6, 0);
|
||||
const Scalar _tmp29 = _tmp11 * state(6, 0);
|
||||
// Intermediate terms (43)
|
||||
const Scalar _tmp0 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp1 = 2 * state(1, 0);
|
||||
const Scalar _tmp2 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp3 = 4 * _tmp2;
|
||||
const Scalar _tmp4 = 2 * state(6, 0);
|
||||
const Scalar _tmp5 = _tmp4 * state(0, 0);
|
||||
const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp8 = 2 * state(0, 0);
|
||||
const Scalar _tmp9 = _tmp8 * state(3, 0);
|
||||
const Scalar _tmp10 = 2 * state(2, 0);
|
||||
const Scalar _tmp11 = _tmp10 * state(1, 0);
|
||||
const Scalar _tmp12 = _tmp11 + _tmp9;
|
||||
const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
|
||||
const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
|
||||
const Scalar _tmp15 =
|
||||
_tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
|
||||
const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp17 = _tmp11 - _tmp9;
|
||||
const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
|
||||
const Scalar _tmp19 =
|
||||
(_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
|
||||
const Scalar _tmp20 = _tmp4 * state(3, 0);
|
||||
const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
|
||||
const Scalar _tmp22 =
|
||||
-Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
|
||||
const Scalar _tmp23 = 4 * _tmp0;
|
||||
const Scalar _tmp24 =
|
||||
-Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
|
||||
const Scalar _tmp25 = 2 * state(3, 0);
|
||||
const Scalar _tmp26 = _tmp4 * state(2, 0);
|
||||
const Scalar _tmp27 = _tmp4 * state(1, 0);
|
||||
const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
|
||||
const Scalar _tmp29 =
|
||||
-Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
|
||||
(Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
|
||||
const Scalar _tmp30 =
|
||||
_tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29);
|
||||
const Scalar _tmp31 = _tmp26 * state(1, 0);
|
||||
const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) -
|
||||
_tmp23 * (_tmp16 * state(2, 0) + _tmp3);
|
||||
const Scalar _tmp33 = _tmp26 * state(0, 0);
|
||||
const Scalar _tmp34 = 4 * state(3, 0);
|
||||
const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) -
|
||||
_tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28);
|
||||
const Scalar _tmp36 = _tmp26 * state(2, 0);
|
||||
const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36;
|
||||
const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35;
|
||||
const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35;
|
||||
const Scalar _tmp40 = _tmp23 * _tmp5;
|
||||
const Scalar _tmp41 = _tmp15 * _tmp20;
|
||||
const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41);
|
||||
const Scalar _tmp43 = _tmp15 * _tmp19;
|
||||
const Scalar _tmp44 = _tmp23 * _tmp9;
|
||||
const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44);
|
||||
const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21);
|
||||
const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41);
|
||||
const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44);
|
||||
const Scalar _tmp49 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
-_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
|
||||
const Scalar _tmp31 =
|
||||
_tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
|
||||
const Scalar _tmp32 =
|
||||
_tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
|
||||
const Scalar _tmp33 = _tmp19 * _tmp7;
|
||||
const Scalar _tmp34 = _tmp17 * _tmp21;
|
||||
const Scalar _tmp35 = -_tmp33 + _tmp34;
|
||||
const Scalar _tmp36 = _tmp12 * _tmp19;
|
||||
const Scalar _tmp37 = _tmp16 * _tmp21;
|
||||
const Scalar _tmp38 = -_tmp36 + _tmp37;
|
||||
const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
|
||||
const Scalar _tmp40 = _tmp33 - _tmp34;
|
||||
const Scalar _tmp41 = _tmp36 - _tmp37;
|
||||
const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
@@ -96,91 +97,91 @@ void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
|
||||
_h.setZero();
|
||||
|
||||
_h(0, 0) = _tmp37;
|
||||
_h(1, 0) = _tmp38;
|
||||
_h(2, 0) = _tmp39;
|
||||
_h(3, 0) = _tmp42;
|
||||
_h(4, 0) = _tmp45;
|
||||
_h(5, 0) = _tmp46;
|
||||
_h(21, 0) = _tmp47;
|
||||
_h(22, 0) = _tmp48;
|
||||
_h(0, 0) = _tmp30;
|
||||
_h(1, 0) = _tmp31;
|
||||
_h(2, 0) = _tmp32;
|
||||
_h(3, 0) = _tmp35;
|
||||
_h(4, 0) = _tmp38;
|
||||
_h(5, 0) = _tmp39;
|
||||
_h(21, 0) = _tmp40;
|
||||
_h(22, 0) = _tmp41;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _k = (*K);
|
||||
|
||||
_k(0, 0) =
|
||||
_tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 +
|
||||
P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46);
|
||||
_tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
|
||||
P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
|
||||
_k(1, 0) =
|
||||
_tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 +
|
||||
P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46);
|
||||
_tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
|
||||
P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
|
||||
_k(2, 0) =
|
||||
_tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 +
|
||||
P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46);
|
||||
_tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
|
||||
P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
|
||||
_k(3, 0) =
|
||||
_tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 +
|
||||
P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46);
|
||||
_tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
|
||||
P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
|
||||
_k(4, 0) =
|
||||
_tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 +
|
||||
P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46);
|
||||
_tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
|
||||
P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
|
||||
_k(5, 0) =
|
||||
_tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 +
|
||||
P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46);
|
||||
_tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
|
||||
P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
|
||||
_k(6, 0) =
|
||||
_tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 +
|
||||
P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46);
|
||||
_tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
|
||||
P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
|
||||
_k(7, 0) =
|
||||
_tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 +
|
||||
P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46);
|
||||
_tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
|
||||
P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
|
||||
_k(8, 0) =
|
||||
_tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 +
|
||||
P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46);
|
||||
_tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
|
||||
P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
|
||||
_k(9, 0) =
|
||||
_tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 +
|
||||
P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46);
|
||||
_tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
|
||||
P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
|
||||
_k(10, 0) =
|
||||
_tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 +
|
||||
P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46);
|
||||
_tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
|
||||
P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
|
||||
_k(11, 0) =
|
||||
_tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 +
|
||||
P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46);
|
||||
_tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
|
||||
P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
|
||||
_k(12, 0) =
|
||||
_tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 +
|
||||
P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46);
|
||||
_tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
|
||||
P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
|
||||
_k(13, 0) =
|
||||
_tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 +
|
||||
P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46);
|
||||
_tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
|
||||
P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
|
||||
_k(14, 0) =
|
||||
_tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 +
|
||||
P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46);
|
||||
_tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
|
||||
P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
|
||||
_k(15, 0) =
|
||||
_tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 +
|
||||
P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46);
|
||||
_tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
|
||||
P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
|
||||
_k(16, 0) =
|
||||
_tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 +
|
||||
P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46);
|
||||
_tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
|
||||
P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
|
||||
_k(17, 0) =
|
||||
_tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 +
|
||||
P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46);
|
||||
_tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
|
||||
P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
|
||||
_k(18, 0) =
|
||||
_tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 +
|
||||
P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46);
|
||||
_tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
|
||||
P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
|
||||
_k(19, 0) =
|
||||
_tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 +
|
||||
P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46);
|
||||
_tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
|
||||
P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
|
||||
_k(20, 0) =
|
||||
_tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 +
|
||||
P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46);
|
||||
_tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
|
||||
P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
|
||||
_k(21, 0) =
|
||||
_tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 +
|
||||
P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46);
|
||||
_tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
|
||||
P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
|
||||
_k(22, 0) =
|
||||
_tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 +
|
||||
P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46);
|
||||
_tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
|
||||
P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
|
||||
_k(23, 0) =
|
||||
_tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 +
|
||||
P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46);
|
||||
_tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
|
||||
P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
+67
-66
@@ -30,69 +30,70 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 266
|
||||
// Total ops: 265
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (49)
|
||||
// Intermediate terms (42)
|
||||
const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp3 = 2 * state(3, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(0, 0);
|
||||
const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp2 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp3 = 2 * state(0, 0);
|
||||
const Scalar _tmp4 = _tmp3 * state(3, 0);
|
||||
const Scalar _tmp5 = 2 * state(1, 0);
|
||||
const Scalar _tmp6 = _tmp5 * state(2, 0);
|
||||
const Scalar _tmp7 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp7 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp9 = 2 * state(2, 0);
|
||||
const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
|
||||
const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
|
||||
const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8;
|
||||
const Scalar _tmp12 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp13 = _tmp4 + _tmp6;
|
||||
const Scalar _tmp14 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0);
|
||||
const Scalar _tmp15 = _tmp12 * _tmp8 + _tmp13 * _tmp2 + _tmp14 * state(6, 0);
|
||||
const Scalar _tmp16 = _tmp15 + epsilon * ((((_tmp15) > 0) - ((_tmp15) < 0)) + Scalar(0.5));
|
||||
const Scalar _tmp17 = Scalar(1.0) / (_tmp16);
|
||||
const Scalar _tmp18 = _tmp1 * _tmp17;
|
||||
const Scalar _tmp19 = std::pow(_tmp16, Scalar(2));
|
||||
const Scalar _tmp20 = _tmp11 / _tmp19;
|
||||
const Scalar _tmp21 = _tmp13 * _tmp20;
|
||||
const Scalar _tmp22 = _tmp19 / (std::pow(_tmp11, Scalar(2)) + _tmp19);
|
||||
const Scalar _tmp23 = _tmp22 * (-_tmp18 + _tmp21);
|
||||
const Scalar _tmp24 = _tmp12 * _tmp20;
|
||||
const Scalar _tmp25 = _tmp17 * _tmp7;
|
||||
const Scalar _tmp26 = _tmp22 * (-_tmp24 + _tmp25);
|
||||
const Scalar _tmp27 = _tmp22 * (_tmp10 * _tmp17 - _tmp14 * _tmp20);
|
||||
const Scalar _tmp28 = _tmp3 * state(6, 0);
|
||||
const Scalar _tmp29 = 4 * _tmp8;
|
||||
const Scalar _tmp30 = 2 * state(0, 0);
|
||||
const Scalar _tmp31 = _tmp30 * state(6, 0);
|
||||
const Scalar _tmp32 =
|
||||
_tmp17 * (_tmp28 + _tmp5 * _tmp8) - _tmp20 * (_tmp2 * _tmp5 - _tmp29 * state(2, 0) - _tmp31);
|
||||
const Scalar _tmp33 = (Scalar(1) / Scalar(2)) * _tmp22;
|
||||
const Scalar _tmp34 = _tmp33 * state(1, 0);
|
||||
const Scalar _tmp35 = _tmp5 * state(6, 0);
|
||||
const Scalar _tmp36 = _tmp9 * state(6, 0);
|
||||
const Scalar _tmp37 = _tmp17 * (-_tmp3 * _tmp8 + _tmp35) - _tmp20 * (_tmp2 * _tmp3 - _tmp36);
|
||||
const Scalar _tmp38 = _tmp33 * state(3, 0);
|
||||
const Scalar _tmp39 = 4 * _tmp2;
|
||||
const Scalar _tmp40 =
|
||||
_tmp17 * (_tmp31 - _tmp39 * state(1, 0) + _tmp8 * _tmp9) - _tmp20 * (_tmp2 * _tmp9 + _tmp28);
|
||||
const Scalar _tmp41 = _tmp33 * state(2, 0);
|
||||
const Scalar _tmp42 = _tmp17 * (-_tmp30 * _tmp8 + _tmp36 - _tmp39 * state(3, 0)) -
|
||||
_tmp20 * (_tmp2 * _tmp30 - _tmp29 * state(3, 0) + _tmp35);
|
||||
const Scalar _tmp43 = _tmp33 * state(0, 0);
|
||||
const Scalar _tmp44 = _tmp32 * _tmp34 - _tmp37 * _tmp38 - _tmp40 * _tmp41 + _tmp42 * _tmp43;
|
||||
const Scalar _tmp45 = -_tmp32 * _tmp38 - _tmp34 * _tmp37 + _tmp40 * _tmp43 + _tmp41 * _tmp42;
|
||||
const Scalar _tmp46 = _tmp32 * _tmp43 - _tmp34 * _tmp42 - _tmp37 * _tmp41 + _tmp38 * _tmp40;
|
||||
const Scalar _tmp47 = _tmp22 * (_tmp24 - _tmp25);
|
||||
const Scalar _tmp48 = _tmp22 * (_tmp18 - _tmp21);
|
||||
const Scalar _tmp12 =
|
||||
_tmp11 + epsilon * (2 * math::min<Scalar>(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1);
|
||||
const Scalar _tmp13 = Scalar(1.0) / (_tmp12);
|
||||
const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp15 = -_tmp4 + _tmp6;
|
||||
const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0);
|
||||
const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0);
|
||||
const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2));
|
||||
const Scalar _tmp19 = _tmp18 * _tmp7;
|
||||
const Scalar _tmp20 = _tmp13 * _tmp14;
|
||||
const Scalar _tmp21 = _tmp19 - _tmp20;
|
||||
const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16;
|
||||
const Scalar _tmp23 = _tmp1 * _tmp18;
|
||||
const Scalar _tmp24 = _tmp13 * _tmp15;
|
||||
const Scalar _tmp25 = -_tmp23 + _tmp24;
|
||||
const Scalar _tmp26 = 2 * state(6, 0);
|
||||
const Scalar _tmp27 = _tmp26 * state(0, 0);
|
||||
const Scalar _tmp28 = _tmp26 * state(3, 0);
|
||||
const Scalar _tmp29 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8);
|
||||
const Scalar _tmp30 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9);
|
||||
const Scalar _tmp31 = 2 * state(3, 0);
|
||||
const Scalar _tmp32 = _tmp26 * state(2, 0);
|
||||
const Scalar _tmp33 = _tmp26 * state(1, 0);
|
||||
const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32);
|
||||
const Scalar _tmp35 = 4 * state(3, 0);
|
||||
const Scalar _tmp36 =
|
||||
(Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) -
|
||||
Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33);
|
||||
const Scalar _tmp37 =
|
||||
_tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0);
|
||||
const Scalar _tmp38 =
|
||||
-_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0);
|
||||
const Scalar _tmp39 =
|
||||
_tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0);
|
||||
const Scalar _tmp40 = _tmp23 - _tmp24;
|
||||
const Scalar _tmp41 = -_tmp19 + _tmp20;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = std::atan2(_tmp11, _tmp16);
|
||||
_innov = _tmp13 * _tmp17;
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
@@ -100,22 +101,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
|
||||
|
||||
_innov_var =
|
||||
R +
|
||||
_tmp23 * (P(0, 22) * _tmp45 + P(1, 22) * _tmp46 + P(2, 22) * _tmp44 + P(21, 22) * _tmp47 +
|
||||
P(22, 22) * _tmp23 + P(3, 22) * _tmp26 + P(4, 22) * _tmp48 + P(5, 22) * _tmp27) +
|
||||
_tmp26 * (P(0, 3) * _tmp45 + P(1, 3) * _tmp46 + P(2, 3) * _tmp44 + P(21, 3) * _tmp47 +
|
||||
P(22, 3) * _tmp23 + P(3, 3) * _tmp26 + P(4, 3) * _tmp48 + P(5, 3) * _tmp27) +
|
||||
_tmp27 * (P(0, 5) * _tmp45 + P(1, 5) * _tmp46 + P(2, 5) * _tmp44 + P(21, 5) * _tmp47 +
|
||||
P(22, 5) * _tmp23 + P(3, 5) * _tmp26 + P(4, 5) * _tmp48 + P(5, 5) * _tmp27) +
|
||||
_tmp44 * (P(0, 2) * _tmp45 + P(1, 2) * _tmp46 + P(2, 2) * _tmp44 + P(21, 2) * _tmp47 +
|
||||
P(22, 2) * _tmp23 + P(3, 2) * _tmp26 + P(4, 2) * _tmp48 + P(5, 2) * _tmp27) +
|
||||
_tmp45 * (P(0, 0) * _tmp45 + P(1, 0) * _tmp46 + P(2, 0) * _tmp44 + P(21, 0) * _tmp47 +
|
||||
P(22, 0) * _tmp23 + P(3, 0) * _tmp26 + P(4, 0) * _tmp48 + P(5, 0) * _tmp27) +
|
||||
_tmp46 * (P(0, 1) * _tmp45 + P(1, 1) * _tmp46 + P(2, 1) * _tmp44 + P(21, 1) * _tmp47 +
|
||||
P(22, 1) * _tmp23 + P(3, 1) * _tmp26 + P(4, 1) * _tmp48 + P(5, 1) * _tmp27) +
|
||||
_tmp47 * (P(0, 21) * _tmp45 + P(1, 21) * _tmp46 + P(2, 21) * _tmp44 + P(21, 21) * _tmp47 +
|
||||
P(22, 21) * _tmp23 + P(3, 21) * _tmp26 + P(4, 21) * _tmp48 + P(5, 21) * _tmp27) +
|
||||
_tmp48 * (P(0, 4) * _tmp45 + P(1, 4) * _tmp46 + P(2, 4) * _tmp44 + P(21, 4) * _tmp47 +
|
||||
P(22, 4) * _tmp23 + P(3, 4) * _tmp26 + P(4, 4) * _tmp48 + P(5, 4) * _tmp27);
|
||||
_tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 +
|
||||
P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) +
|
||||
_tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 +
|
||||
P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) +
|
||||
_tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 +
|
||||
P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) +
|
||||
_tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 +
|
||||
P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) +
|
||||
_tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 +
|
||||
P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) +
|
||||
_tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 +
|
||||
P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) +
|
||||
_tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 +
|
||||
P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) +
|
||||
_tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 +
|
||||
P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
|
||||
@@ -91,21 +91,20 @@ def generate_python_function(function_name, output_names):
|
||||
def build_state_struct(state, T="float"):
|
||||
out = "struct StateSample {\n"
|
||||
|
||||
def get_px4_type(obj):
|
||||
if isinstance(obj, sf.M11):
|
||||
def TypeFromLength(len):
|
||||
if len == 1:
|
||||
return f"{T}"
|
||||
elif isinstance(obj, sf.M21):
|
||||
elif len == 2:
|
||||
return f"matrix::Vector2<{T}>"
|
||||
elif isinstance(obj, sf.M31):
|
||||
elif len == 3:
|
||||
return f"matrix::Vector3<{T}>"
|
||||
elif isinstance(obj, sf.Rot3):
|
||||
elif len == 4:
|
||||
return f"matrix::Quaternion<{T}>"
|
||||
else:
|
||||
print(f"unknown type {type(obj)}")
|
||||
raise NotImplementedError
|
||||
|
||||
for key, val in state.items():
|
||||
out += f"\t{get_px4_type(val)} {key}{{}};\n"
|
||||
out += f"\t{TypeFromLength(val.storage_dim())} {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 = -_gpos.altitude() + _params.rng_gnd_clearance;
|
||||
_state.terrain = _state.pos(2) + _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 = -_gpos.altitude();
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
_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 GPS / WGS84 frame
|
||||
const LatLonAlt lla = _ekf.getLatLonAlt();
|
||||
global_pos.lat = lla.latitude_deg();
|
||||
global_pos.lon = lla.longitude_deg();
|
||||
// Position of local NED origin in GPS / WGS84 frame
|
||||
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
|
||||
global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid();
|
||||
|
||||
global_pos.alt = lla.altitude();
|
||||
global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
|
||||
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