diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile deleted file mode 100644 index e8a97b48e6..0000000000 --- a/.ci/Jenkinsfile-compile +++ /dev/null @@ -1,228 +0,0 @@ -#!/usr/bin/env groovy - -pipeline { - agent none - stages { - - stage('Build') { - steps { - script { - def build_nodes = [:] - def docker_images = [ - armhf: "px4io/px4-dev-armhf:2023-06-26", - arm64: "px4io/px4-dev-aarch64:2022-08-12", - base: "px4io/px4-dev-ros2-foxy:2022-08-12", - nuttx: "px4io/px4-dev-nuttx-focal:2022-08-12", - ] - - def armhf_builds = [ - target: ["beaglebone_blue_default", "emlid_navio2_default", "px4_raspberrypi_default", "scumaker_pilotpi_default"], - image: docker_images.armhf, - archive: false - ] - - def arm64_builds = [ - target: ["scumaker_pilotpi_arm64"], - image: docker_images.arm64, - archive: false - ] - - def base_builds = [ - target: ["px4_sitl_default"], - image: docker_images.base, - archive: false - ] - - def nuttx_builds_archive = [ - target: [ - "3dr_ctrl-zero-h7-oem-revg_default", - "airmind_mindpx-v2_default", - "ark_can-flow_canbootloader", - "ark_can-flow_default", - "ark_can-gps_canbootloader", - "ark_can-gps_default", - "ark_can-rtk-gps_canbootloader", - "ark_can-rtk-gps_default", - "ark_cannode_canbootloader", - "ark_cannode_default", - "ark_fmu-v6x_bootloader", - "ark_fmu-v6x_default", - "ark_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", - "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_ucans32k146_canbootloader", - "nxp_ucans32k146_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 .') - } - } - } - } - } - } -} diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware deleted file mode 100644 index 73483107a7..0000000000 --- a/.ci/Jenkinsfile-hardware +++ /dev/null @@ -1,904 +0,0 @@ -#!/usr/bin/env groovy - -pipeline { - agent none - stages { - stage('Hardware Test') { - - parallel { - - stage("cubepilot_cubeorange_test") { - stages { - stage("build cubepilot_cubeorange_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make cubepilot_cubeorange_bootloader' - sh 'make cubepilot_cubeorange_test' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*, build/cubepilot_cubeorange_test/etc/init.d/airframes/*', name: 'cubepilot_cubeorange_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'cubepilot_cubeorange' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'cubepilot_cubeorange_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_bootloader/cubepilot_cubeorange_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - // run tests - runTests() - - // load all airframes - // sh("./Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` `cd build/cubepilot_cubeorange_test/etc/init.d/airframes/; find . -regex '.*/[0-9].*' -exec basename {} \\; | cut -d '_' -f 1` || true") // test loading all airframes\ - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true' - } - } - } // stage test - } - } - - stage("cuav_x7pro_test") { - stages { - stage("build cuav_x7pro_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make cuav_x7pro_bootloader' - sh 'make cuav_x7pro_test' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'cuav_x7pro_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'cuav_x7pro' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'cuav_x7pro_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_bootloader/cuav_x7pro_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "2000"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v4_test") { - stages { - stage("build px4_fmu-v4_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v4_test' - sh 'make px4_fmu-v4_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v4' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v4_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v4pro_test") { - stages { - stage("build px4_fmu-v4pro_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v4pro_test' - sh 'make px4_fmu-v4pro_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v4pro_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v4pro' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v4pro_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "13000"' // generic vtol standard - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v5_debug") { - stages { - stage("build px4_fmu-v5_debug") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v5_debug' - sh 'make px4_fmu-v5_debug bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_debug' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_debug' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600 || true' - resetBoard() - } - } - stage("tests") { - steps { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"' - - // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - checkStatus() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v5_stackcheck") { - stages { - stage("build px4_fmu-v5_stackcheck") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v5_stackcheck' - sh 'make px4_fmu-v5_stackcheck bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_stackcheck' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_stackcheck' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' - - // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - checkStatus() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true' - } - } - } // stage test - } - } - - stage("px4_fmu-v5_test") { - stages { - stage("build px4_fmu-v5_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make px4_fmu-v5_test' - sh 'make px4_fmu-v5_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v5_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_test' - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true' - } - } - } // stage test - } - } - - stage("nxp_fmuk66-v3_test") { - stages { - stage("build nxp_fmuk66-v3_test") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2022-08-12' - args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkoutSCM() - sh 'make nxp_fmuk66-v3_test' - //sh 'make nxp_fmuk66-v3_test bootloader_elf' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'nxp_fmuk66-v3_test' - } - post { - always { - sh 'make distclean; git clean -ff -x -d .' - } - } - } // stage build - stage("hardware") { - agent { - label 'nxp_fmuk66-v3' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'nxp_fmuk66-v3_test' - //sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_bootloader.elf' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - resetBoard() - } - } - stage("tests") { - steps { - runTests() - } - } - stage("status") { - steps { - // configure - resetParameters() - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "400"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"' // generic quadcopter - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"' // update bootloader - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' // run logger - checkStatus() - quickCalibrate() - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' // stop logger - } - } - stage("print topics") { - steps { - printTopics() - } - } - } - post { - always { - sh 'cat /tmp/pyserial_spy_file.txt || true' - sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true' - } - } - } // stage test - } - } - - } // parallel - } // stage Hardware Test - } // stages - environment { - CCACHE_DIR = '/tmp/ccache' - CCACHE_NOHASHDIR = 1 - CI = true - } - options { - buildDiscarder(logRotator(numToKeepStr: '30', artifactDaysToKeepStr: '60')) - timeout(time: 180, unit: 'MINUTES') - skipDefaultCheckout() - } -} - -void checkoutSCM() { - retry(3) { - checkout scm - sh 'export' - sh 'make distclean; git clean -ff -x -d .' - sh 'git fetch --tags' - sh 'ccache -z' - } -} - -void quickCalibrate() { - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters before - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"' -} - -void checkStatus() { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - - // status commands - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/meminfo"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/uptime"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander check" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ekf2 status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload; top once; listener cpuload"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /bin"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /dev"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /etc"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status streams" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf latency"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ps"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm_out status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' -} - -void resetParameters() { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param reset_all"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "CBRK_BUZZER" --value "782097"' - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SDLOG_DIRS_MAX" --value "1"' -} - -void runTests() { - - // test loading a range of airframes - sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001' - - resetParameters() - - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_IMU_AUTOCAL" --value "0" || true' // disable during testing - sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SENS_MAG_AUTOCAL" --value "0" || true' // disable during testing - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"' - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ostest"' - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot after ostest - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during test - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander_tests" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "controllib_test"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "lightware_laser_test"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink_tests" || true' // TODO - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd readtest"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "bsondump /fs/mtd_params" || true' // expected to fail after erase - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -v"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u -v"' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"' - - // tests (stop modules first) - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander stop"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink stop-all"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "navigator stop"' - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during microbenchmarks - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "microbench all"' - - //sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "calib_udelay"' -} - -void printTopics() { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true' - - // these are for casually inspecting the system, output failure doesn't matter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_wind" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_optical_flow_vel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_states" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_wind" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener event" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener heater_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener input_rc" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener led_control" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener log_message" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener manual_control_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mavlink_log" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mission" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener multirotor_motor_limits" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener optical_flow" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_controller_landing_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_setpoint_triplet" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener radio_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command_ack" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_control_mode" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_global_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_land_detected" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true' -} - -void resetBoard() { - resetParameters() - - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "echo > /fs/microsd/.format" || true' - sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply -} diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml index 0aa6ce92e5..e6d0ece289 100644 --- a/.github/workflows/build_all_targets.yml +++ b/.github/workflows/build_all_targets.yml @@ -7,44 +7,63 @@ name: Build all targets on: push: + tags: + - 'v*' branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' + - 'main' + - 'stable' + - 'beta' + - 'release/**' pull_request: branches: - - '*' + - '*' jobs: group_targets: name: Scan for Board Targets # runs-on: ubuntu-latest - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] outputs: matrix: ${{ steps.set-matrix.outputs.matrix }} timestamp: ${{ steps.set-timestamp.outputs.timestamp }} + tagname: ${{ steps.set-tag.outputs.tagname }} + branchname: ${{ steps.set-branch.outputs.branchname }} steps: - uses: actions/checkout@v4 + - name: Update python packaging to avoid canonicalize_version() error + run: | + pip3 install -U packaging + - name: Install Python Dependencies uses: py-actions/py-dependency-install@v4 with: path: "./Tools/setup/requirements.txt" - id: set-matrix - run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py --group)" + run: echo "::set-output name=matrix::$(./Tools/ci/generate_board_targets_json.py --group)" - id: set-timestamp run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")" + - 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)" + setup: - name: ${{ matrix.group }} + name: Build Group [${{ matrix.group }}] # runs-on: ubuntu-latest - runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] needs: group_targets strategy: matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} + fail-fast: false container: image: ${{ matrix.container }} steps: @@ -52,17 +71,17 @@ jobs: with: fetch-depth: 0 - - name: ownership workaround + - name: Git ownership workaround run: git config --system --add safe.directory '*' - - name: ccache setup keys + - name: Setup ccache uses: actions/cache@v4 with: path: ~/.ccache key: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} restore-keys: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} - - name: setup ccache + - name: Configure ccache run: | mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf @@ -73,18 +92,67 @@ jobs: ccache -s ccache -z - - name: build target group + - name: Building [${{ matrix.group }}] run: | - ./Tools/ci_build_all_runner.sh ${{matrix.targets}} + ./Tools/ci/build_all_runner.sh ${{matrix.targets}} - - name: Upload px4 package + - name: Arrange Build Artifacts + run: | + ./Tools/ci/package_build_artifacts.sh + + - name: Upload Build Artifacts uses: actions/upload-artifact@v4 with: name: px4_${{matrix.group}}_build_artifacts - path: | - build/**/*.px4 - build/**/*.bin - compression-level: 0 + path: artifacts/ - - name: ccache post-run + - name: Cache Save run: ccache -s + + artifacts: + name: Upload Artifacts to S3 + # runs-on: ubuntu-latest + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] + needs: [setup, group_targets] + if: contains(fromJSON('["main", "stable", "beta"]'), needs.group_targets.outputs.branchname) + steps: + - name: Download Artifacts + uses: actions/download-artifact@v4 + with: + path: artifacts/ + merge-multiple: true + + - name: Branch Name + run: | + echo "${{ needs.group_targets.outputs.branchname }}" + + - name: Uploading Artifacts to S3 [${{ needs.group_targets.outputs.branchname == 'main' && 'master' || needs.group_targets.outputs.branchname }}] + uses: jakejarvis/s3-sync-action@master + with: + args: --acl public-read + env: + AWS_S3_BUCKET: 'px4-travis' + AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }} + AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }} + AWS_REGION: 'us-west-1' + SOURCE_DIR: artifacts/ + DEST_DIR: Firmware/${{ needs.group_targets.outputs.branchname == 'main' && 'master' || needs.group_targets.outputs.branchname }}/ + + release: + name: Create Release and Upload Artifacts + # runs-on: ubuntu-latest + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] + needs: [setup, group_targets] + if: startsWith(github.ref, 'refs/tags/v') + steps: + - name: Download Artifacts + uses: actions/download-artifact@v4 + with: + path: artifacts/ + merge-multiple: true + + - name: Upload Binaries to Release + uses: softprops/action-gh-release@v2 + with: + draft: true + files: artifacts/*.px4 diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index a2d7aa5f97..1e8d2238a1 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -25,28 +25,27 @@ jobs: "NO_NINJA_BUILD=1 px4_fmu-v5_default", "NO_NINJA_BUILD=1 px4_sitl_default", "px4_sitl_allyes", - "airframe_metadata", "module_documentation", - "parameters_metadata", ] - container: - image: px4io/px4-dev-nuttx-focal:2022-08-12 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} + - uses: actions/checkout@v4 + with: + fetch-depth: 0 - - name: check environment - run: | - export - ulimit -a - - name: ${{matrix.check}} - run: make ${{matrix.check}} - - name: upload coverage - if: contains(matrix.check, 'coverage') - uses: codecov/codecov-action@v1 - with: - token: ${{ secrets.CODECOV_TOKEN }} - flags: unittests - file: coverage/lcov.info + - name: Building [${{ matrix.check }}] + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-nuttx-focal:2022-08-12 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make ${{ matrix.check }} + + - name: Uploading Coverage to Codecov.io + if: contains(matrix.check, 'coverage') + uses: codecov/codecov-action@v1 + with: + token: ${{ secrets.CODECOV_TOKEN }} + flags: unittests + file: coverage/lcov.info diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index a8cba7ce6b..89c1ffe1ec 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -11,11 +11,17 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-clang:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{secrets.ACCESS_TOKEN}} + fetch-depth: 0 - - name: make clang-tidy-quiet - run: make clang-tidy-quiet + - name: Testing (clang-tidy-quiet) + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-clang:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make clang-tidy-quiet diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml index 5c45a77108..c9b83bdfec 100644 --- a/.github/workflows/compile_macos.yml +++ b/.github/workflows/compile_macos.yml @@ -19,13 +19,11 @@ jobs: ] steps: - name: install Python 3.10 - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: "3.10" - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} + - uses: actions/checkout@v4 - name: setup run: ./Tools/setup/macos.sh; ./Tools/setup/macos.sh @@ -37,7 +35,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v4 with: path: ~/.ccache key: macos_${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/compile_ubuntu.yml b/.github/workflows/compile_ubuntu.yml new file mode 100644 index 0000000000..31d1a1e537 --- /dev/null +++ b/.github/workflows/compile_ubuntu.yml @@ -0,0 +1,46 @@ +name: Ubuntu environment build + +on: + push: + branches: + - 'main' + - 'stable' + - 'beta' + - 'release/**' + pull_request: + branches: + - '*' + +jobs: + build_and_test: + name: Build and Test + strategy: + fail-fast: false + matrix: + version: ['ubuntu:22.04', 'ubuntu:24.04'] + runs-on: [runs-on,runner=8cpu-linux-x64,"image=ubuntu24-full-x64","run-id=${{ github.run_id }}",spot=false] + container: + image: ${{ matrix.version }} + volumes: + - /github/workspace:/github/workspace + steps: + + - name: Fix git in container + run: | + # we only need this because we are running the job in a container + # when checkout pulls git it does it in a shared volume + # and file ownership changes between steps + # first we install git since its missing from the base image + # then we mark the directory as safe for other instances + # of git to use. + apt update && apt install git -y + git config --global --add safe.directory $(realpath .) + + - uses: actions/checkout@v4 + + - name: Install Deps, Build, and Make Quick Check + run: | + # we need to install dependencies and build on the same step + # given the stateless nature of docker images + ./Tools/setup/ubuntu.sh + make quick_check diff --git a/.github/workflows/deploy_all.yml b/.github/workflows/deploy_all.yml deleted file mode 100644 index 3f1a4c0049..0000000000 --- a/.github/workflows/deploy_all.yml +++ /dev/null @@ -1,56 +0,0 @@ -name: Deploy metadata for all targets - -on: - push: - branches: - - 'main' - - 'release/*' - - 'pr-metadata-test' - -jobs: - enumerate_targets: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - outputs: - matrix: ${{ steps.set-matrix.outputs.matrix }} - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - id: set-matrix - run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py)" - build: - runs-on: ubuntu-latest - needs: enumerate_targets - strategy: - matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}} - container: ${{ matrix.container }} - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: ownership workaround - run: git config --system --add safe.directory '*' - - - name: make ${{matrix.target}} - run: make ${{matrix.target}} - - - name: parameter & events metadata - run: | - make ${{matrix.target}} ver_gen events_json actuators_json - ./src/lib/version/get_git_tag_or_branch_version.sh build/${{ matrix.target }} >> $GITHUB_ENV - cd build/${{ matrix.target }} - mkdir _metadata || true - cp parameters.* events/*.xz actuators.json* _metadata - - - uses: jakejarvis/s3-sync-action@master - with: - args: --acl public-read - env: - AWS_S3_BUCKET: 'px4-travis' - AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }} - AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }} - AWS_REGION: 'us-west-1' - SOURCE_DIR: 'build/${{ matrix.target }}/_metadata/' - DEST_DIR: 'Firmware/${{ env.version }}/${{ matrix.target }}/' diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml new file mode 100644 index 0000000000..b3f9542ead --- /dev/null +++ b/.github/workflows/dev_container.yml @@ -0,0 +1,101 @@ + +name: Container build + +on: + push: + branches: + - 'main' + - 'stable' + - 'beta' + - 'release/**' + tags: + - 'v*' + pull_request: + branches: + - '*' + +jobs: + build: + name: Build and Push Container + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] + steps: + - uses: actions/checkout@v4 + with: + fetch-tags: true + submodules: false + fetch-depth: 0 + + - name: Set PX4 Tag + id: px4-tag + run: | + echo "tag=$(git describe --tags --match 'v[0-9]*')" >> $GITHUB_OUTPUT + + - name: Login to Docker Hub + uses: docker/login-action@v3 + if: github.event_name != 'pull_request' + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract metadata (tags, labels) for Docker + id: meta + uses: docker/metadata-action@v5 + with: + images: | + ghcr.io/PX4/px4-dev + ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }} + tags: | + type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Build and load container image + uses: docker/build-push-action@v6 + id: docker + with: + context: Tools/setup + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + platforms: | + linux/amd64 + load: true + push: false + cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} + cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max + + - name: Get Tag Name + id: tag_name + run: | + echo "::set-output name=tag_name::$(echo '${{ fromJSON(steps.docker.outputs.metadata)['image.name'] }}' | sed 's/,.*$//')" + + - name: make quick_check + uses: addnab/docker-run-action@v3 + with: + image: ${{ steps.tag_name.outputs.tag_name }} + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make px4_sitl_default + make px4_fmu-v6x_default + + - name: Push container image + uses: docker/build-push-action@v6 + with: + context: Tools/setup + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + platforms: | + linux/amd64 + provenance: mode=max + push: ${{ github.event_name != 'pull_request' }} + 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 diff --git a/.github/workflows/ekf_functional_change_indicator.yml b/.github/workflows/ekf_functional_change_indicator.yml index a5fa9db599..7fa9a834aa 100644 --- a/.github/workflows/ekf_functional_change_indicator.yml +++ b/.github/workflows/ekf_functional_change_indicator.yml @@ -1,21 +1,28 @@ name: EKF Change Indicator -on: pull_request +on: + pull_request: + branches: + - '*' jobs: unit_tests: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v2.3.1 + - uses: actions/checkout@v4 with: fetch-depth: 0 - - name: checkout newest version of branch - run: | - git fetch origin pull/${{github.event.pull_request.number}}/head:${{github.head_ref}} - git checkout ${GITHUB_HEAD_REF} + - name: main test - run: make tests TESTFILTER=EKF + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-base-focal:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make tests TESTFILTER=EKF + - name: Check if there is a functional change run: git diff --exit-code working-directory: src/modules/ekf2/test/change_indication diff --git a/.github/workflows/ekf_update_change_indicator.yml b/.github/workflows/ekf_update_change_indicator.yml index 7de35207f0..ecf4f2a564 100644 --- a/.github/workflows/ekf_update_change_indicator.yml +++ b/.github/workflows/ekf_update_change_indicator.yml @@ -5,25 +5,40 @@ on: push jobs: unit_tests: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 env: GIT_COMMITTER_EMAIL: bot@px4.io GIT_COMMITTER_NAME: PX4BuildBot steps: - - uses: actions/checkout@v2.3.1 + - uses: actions/checkout@v4 with: fetch-depth: 0 - - name: main test updates change indication files - run: make tests TESTFILTER=EKF + + - name: main test + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-base-focal:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make tests TESTFILTER=EKF + - name: Check if there exists diff and save result in variable - run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_ENV + id: diff-check + run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_OUTPUT working-directory: src/modules/ekf2/test/change_indication + - name: auto-commit any changes to change indication uses: stefanzweifel/git-auto-commit-action@v4 with: - commit_message: '[AUTO COMMIT] update change indication' + file_pattern: 'src/modules/ekf2/test/change_indication/*.csv' commit_user_name: ${GIT_COMMITTER_NAME} commit_user_email: ${GIT_COMMITTER_EMAIL} - - if: ${{env.CHANGE_INDICATED}} - name: if there is a functional change, fail check + commit_message: | + '[AUTO COMMIT] update change indication' + + See .github/workflopws/ekf_update_change_indicator.yml for more details + + - name: if there is a functional change, fail check + if: ${{ steps.diff-check.outputs.CHANGE_INDICATED }} run: exit 1 diff --git a/.github/workflows/failsafe_sim.yml b/.github/workflows/failsafe_sim.yml index 4d2ed21728..6210b713dc 100644 --- a/.github/workflows/failsafe_sim.yml +++ b/.github/workflows/failsafe_sim.yml @@ -24,21 +24,23 @@ jobs: image: px4io/px4-dev-nuttx-focal:2022-08-12 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} + - name: Install Node v20.18.0 + uses: actions/setup-node@v4 + with: + node-version: 20.18.0 - - name: check environment - run: | - export - ulimit -a - - name: install emscripten - run: | - git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk - cd _emscripten_sdk - ./emsdk install latest - ./emsdk activate latest - - name: ${{matrix.check}} - run: | - . ./_emscripten_sdk/emsdk_env.sh - make ${{matrix.check}} + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Install empscripten + run: | + git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk + cd _emscripten_sdk + ./emsdk install latest + ./emsdk activate latest + + - name: Testing [${{ matrix.check }}] + run: | + . ./_emscripten_sdk/emsdk_env.sh + make ${{ matrix.check }} diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml new file mode 100644 index 0000000000..8ab97fdda8 --- /dev/null +++ b/.github/workflows/flash_analysis.yml @@ -0,0 +1,141 @@ +name: FLASH usage analysis + +on: + push: + branches: + - 'main' + pull_request: + branches: + - '*' + +env: + MIN_FLASH_POS_DIFF_FOR_COMMENT: 50 + MIN_FLASH_NEG_DIFF_FOR_COMMENT: -50 + +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] + container: + image: px4io/px4-dev-nuttx-focal + strategy: + matrix: + target: [px4_fmu-v5x, px4_fmu-v6x] + outputs: + px4_fmu-v5x-bloaty-output: ${{ steps.gen-output.outputs.px4_fmu-v5x-bloaty-output }} + px4_fmu-v5x-bloaty-summary-map: ${{ steps.gen-output.outputs.px4_fmu-v5x-bloaty-summary-map }} + px4_fmu-v6x-bloaty-output: ${{ steps.gen-output.outputs.px4_fmu-v6x-bloaty-output }} + px4_fmu-v6x-bloaty-summary-map: ${{ steps.gen-output.outputs.px4_fmu-v6x-bloaty-summary-map }} + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + submodules: recursive + + - name: Git ownership workaround + run: git config --system --add safe.directory '*' + + - name: Build Target + run: make ${{ matrix.target }}_flash-analysis + + - name: Store the ELF with the change + run: cp ./build/**/*.elf ./with-change.elf + + - name: Clean previous build + run: | + make clean + make distclean + + - name: If it's a PR checkout the base branch + if: ${{ github.event.pull_request }} + # As checkout creates a merge commit (merging the base branch into the PR branch), the base branch is the base for a diff of the PR changes. + run: git checkout ${{ github.event.pull_request.base.ref }} + + - name: If it's a push checkout the previous commit + if: github.event_name == 'push' + run: git checkout ${{ github.event.before }} + + - name: Update submodules + run: make submodulesupdate + + - name: Build + run: make ${{ matrix.target }}_flash-analysis + + - name: Store the ELF before the change + run: cp ./build/**/*.elf ./before-change.elf + + - name: bloaty-action + uses: PX4/bloaty-action@v1.0.0 + id: bloaty-step + with: + bloaty-file-args: ./with-change.elf -- ./before-change.elf + bloaty-additional-args: -d sections,compileunits -s vm -n 20 + output-to-summary: true + + - name: Generate output + id: gen-output + run: | + EOF=$(dd if=/dev/urandom bs=15 count=1 status=none | base64) + echo "${{ matrix.target }}-bloaty-output<<$EOF" >> $GITHUB_OUTPUT + echo "${{ steps.bloaty-step.outputs.bloaty-output-encoded }}" >> $GITHUB_OUTPUT + echo "$EOF" >> $GITHUB_OUTPUT + echo "${{ matrix.target }}-bloaty-summary-map<<$EOF" >> $GITHUB_OUTPUT + echo '${{ steps.bloaty-step.outputs.bloaty-summary-map }}' >> $GITHUB_OUTPUT + 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] + needs: [analyze_flash] + env: + V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }} + V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }} + V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }} + V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }} + if: ${{ github.event.pull_request }} + steps: + - name: Find Comment + uses: peter-evans/find-comment@v3 + id: fc + 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 + # This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env. + if: | + steps.fc.outputs.comment-id != '' || + env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) || + env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_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 +
+ px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)] + + ``` + ${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }} + ``` +
+ +
+ px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)] + + ``` + ${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }} + ``` +
+ + **Updated: _${{ steps.bt.outputs.timestamp }}_** + edit-mode: replace diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index f1b6737334..b2807f988f 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -11,131 +11,26 @@ on: jobs: build: runs-on: ubuntu-latest - env: - ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true strategy: fail-fast: false matrix: config: - - {vehicle: "iris", mission: "MC_mission_box", build_type: "RelWithDebInfo"} - - {vehicle: "rover", mission: "rover_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "plane", mission: "FW_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "plane_catapult",mission: "FW_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "standard_vtol", mission: "VTOL_mission_1", build_type: "Coverage"} - #- {vehicle: "standard_vtol", mission: "VTOL_mission_1", build_type: "AddressSanitizer"} - #- {vehicle: "tailsitter", mission: "VTOL_mission_1", build_type: "RelWithDebInfo"} - #- {vehicle: "tiltrotor", mission: "VTOL_mission_1", build_type: "RelWithDebInfo"} + - {vehicle: "iris", mission: "MC_mission_box"} + - {vehicle: "rover", mission: "rover_mission_1"} - container: - image: px4io/px4-dev-ros-melodic:2021-09-08 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{ secrets.ACCESS_TOKEN }} + fetch-depth: 0 - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 + - name: Build SITL and Run Tests + uses: addnab/docker-run-action@v3 with: - path: ~/.ccache - key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: check environment - run: | - export - ulimit -a - - name: Build PX4 and sitl_gazebo-classic - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - ccache -z - make px4_sitl_default - make px4_sitl_default sitl_gazebo-classic - ccache -s - - - name: Core dump settings - run: | - ulimit -c unlimited - echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern - - - name: Run SITL tests - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - export - ./test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}} - timeout-minutes: 45 - - - name: Look at core files - if: failure() - run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - - name: Upload px4 coredump - if: failure() - uses: actions/upload-artifact@v3 - with: - name: coredump - path: px4.core - - - name: ecl EKF analysis - if: always() - run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg || true - - - name: Upload logs to flight review - if: always() - run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg - - - name: Upload px4 binary - if: failure() - uses: actions/upload-artifact@v3 - with: - name: binary - path: build/px4_sitl_default/bin/px4 - - - name: Store PX4 log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: px4_log - path: ~/.ros/log/*/*.ulg - - - name: Store ROS log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: ros_log - path: ~/.ros/**/rostest-*.log - - # Report test coverage - - name: Upload coverage - if: contains(matrix.config.build_type, 'Coverage') - run: | - git config --global credential.helper "" # disable the keychain credential helper - git config --global --add credential.helper store # enable the local store credential helper - echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential - git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential - mkdir -p coverage - lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - - name: Upload coverage information to Codecov - if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 - with: - token: ${{ secrets.CODECOV_TOKEN }} - flags: mavros_mission - file: coverage/lcov.info + image: px4io/px4-dev-ros-melodic:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make px4_sitl_default + make px4_sitl_default sitl_gazebo-classic + ./test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}} diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index c45c45ac0b..a968af24ef 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -17,120 +17,21 @@ jobs: fail-fast: false matrix: config: - - {test_file: "mavros_posix_tests_offboard_posctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} - #- {test_file: "mavros_posix_tests_offboard_attctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} - #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} + - {test_file: "mavros_posix_tests_offboard_posctl.test", vehicle: "iris"} - container: - image: px4io/px4-dev-ros-melodic:2021-09-08 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{ secrets.ACCESS_TOKEN }} + fetch-depth: 0 - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 + - name: Build PX4 and Run Tests + uses: addnab/docker-run-action@v3 with: - path: ~/.ccache - key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: check environment - run: | - export - ulimit -a - - name: Build PX4 and sitl_gazebo-classic - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - ccache -z - make px4_sitl_default - make px4_sitl_default sitl_gazebo-classic - ccache -s - - - name: Core dump settings - run: | - ulimit -c unlimited - echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern - - - name: Run SITL tests - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - export - ./test/rostest_px4_run.sh ${{matrix.config.test_file}} vehicle:=${{matrix.config.vehicle}} - timeout-minutes: 45 - - - name: Look at core files - if: failure() - run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - - name: Upload px4 coredump - if: failure() - uses: actions/upload-artifact@v3 - with: - name: coredump - path: px4.core - - - name: ecl EKF analysis - if: always() - run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg || true - - - name: Upload logs to flight review - if: always() - run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg - - - name: Upload px4 binary - if: failure() - uses: actions/upload-artifact@v3 - with: - name: binary - path: build/px4_sitl_default/bin/px4 - - - name: Store PX4 log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: px4_log - path: ~/.ros/log/*/*.ulg - - - name: Store ROS log - if: failure() - uses: actions/upload-artifact@v3 - with: - name: ros_log - path: ~/.ros/**/rostest-*.log - - # Report test coverage - - name: Upload coverage - if: contains(matrix.config.build_type, 'Coverage') - run: | - git config --global credential.helper "" # disable the keychain credential helper - git config --global --add credential.helper store # enable the local store credential helper - echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential - git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential - mkdir -p coverage - lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - - name: Upload coverage information to Codecov - if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 - with: - token: ${{ secrets.CODECOV_TOKEN }} - flags: mavros_offboard - file: coverage/lcov.info + image: px4io/px4-dev-ros-melodic:2021-09-08 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + make px4_sitl_default + make px4_sitl_default sitl_gazebo-classic + ./test/rostest_px4_run.sh ${{matrix.config.test_file}} vehicle:=${{matrix.config.vehicle}} diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml deleted file mode 100644 index bdcf178590..0000000000 --- a/.github/workflows/metadata.yml +++ /dev/null @@ -1,133 +0,0 @@ -name: Metadata - -on: - push: - branches: - - 'main' - - 'release/*' - - 'pr-metadata-test' - -jobs: - - airframe: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: airframe metadata - run: | - make airframe_metadata - ./src/lib/version/get_git_tag_or_branch_version.sh build/px4_sitl_default >> $GITHUB_ENV - cd build/px4_sitl_default/docs - # TODO: deploy to userguide gitbook - - - uses: jakejarvis/s3-sync-action@master - with: - args: --acl public-read - env: - AWS_S3_BUCKET: 'px4-travis' - AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }} - AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }} - AWS_REGION: 'us-west-1' - SOURCE_DIR: 'build/px4_sitl_default/docs/' - DEST_DIR: 'Firmware/${{ env.version }}/_general/' - - module: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: module documentation - run: | - make module_documentation - cd build/px4_sitl_default/docs - ls -ls * - # TODO: deploy to userguide gitbook and s3 - - parameter: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: parameter metadata - run: | - make parameters_metadata - ./src/lib/version/get_git_tag_or_branch_version.sh build/px4_sitl_default >> $GITHUB_ENV - - - uses: jakejarvis/s3-sync-action@master - with: - args: --acl public-read - env: - AWS_S3_BUCKET: 'px4-travis' - AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }} - AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }} - AWS_REGION: 'us-west-1' - SOURCE_DIR: 'build/px4_sitl_default/docs/' - DEST_DIR: 'Firmware/${{ env.version }}/_general/' - - events: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: events metadata - run: | - make extract_events - ./src/lib/version/get_git_tag_or_branch_version.sh build/px4_sitl_default >> $GITHUB_ENV - cd build/px4_sitl_default - mkdir _events_full || true - cp events/all_events_full.json.xz _events_full/all_events.json.xz - - - uses: jakejarvis/s3-sync-action@master - with: - args: --acl public-read - env: - AWS_S3_BUCKET: 'px4-travis' - AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }} - AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }} - AWS_REGION: 'us-west-1' - SOURCE_DIR: 'build/px4_sitl_default/_events_full/' - DEST_DIR: 'Firmware/${{ env.version }}/_general/' - - uorb_graph: - runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2022-08-12 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: uORB graph - run: | - make uorb_graphs - cd Tools/uorb_graph - ls -ls * - # TODO: deploy graph_px4_sitl.json to S3 px4-travis:Firmware/master/ - - ROS2_msgs: - runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-09-08 - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - name: PX4 ROS2 msgs - run: | - git clone https://github.com/PX4/px4_msgs.git - rm px4_msgs/msg/*.msg - rm px4_msgs/srv/*.srv - cp msg/*.msg px4_msgs/msg/ - cp srv/*.srv px4_msgs/srv/ diff --git a/.github/workflows/nuttx_env_config.yml b/.github/workflows/nuttx_env_config.yml index d151444ea9..4a655ffc7a 100644 --- a/.github/workflows/nuttx_env_config.yml +++ b/.github/workflows/nuttx_env_config.yml @@ -11,22 +11,27 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2022-08-12 strategy: matrix: config: [ - px4_fmu-v5, + px4_fmu-v5_default, ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - name: make ${{matrix.config}} - env: - PX4_EXTRA_NUTTX_CONFIG: "CONFIG_NSH_LOGIN_PASSWORD=\"test\";CONFIG_NSH_CONSOLE_LOGIN=y" - run: | - echo "PX4_EXTRA_NUTTX_CONFIG: $PX4_EXTRA_NUTTX_CONFIG" - make ${{matrix.config}} nuttx_context - # Check that the config option is set - grep CONFIG_NSH_LOGIN_PASSWORD build/${{matrix.config}}_default/NuttX/nuttx/.config + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Build PX4 and Run Test [${{ matrix.config }}] + uses: addnab/docker-run-action@v3 + with: + image: px4io/px4-dev-nuttx-focal:2022-08-12 + options: -v ${{ github.workspace }}:/workspace + run: | + cd /workspace + git config --global --add safe.directory /workspace + export PX4_EXTRA_NUTTX_CONFIG="CONFIG_NSH_LOGIN_PASSWORD=\"test\";CONFIG_NSH_CONSOLE_LOGIN=y" + echo "PX4_EXTRA_NUTTX_CONFIG: $PX4_EXTRA_NUTTX_CONFIG" + make ${{ matrix.config }} nuttx_context + echo "Check that the config option is set" + grep CONFIG_NSH_LOGIN_PASSWORD build/${{ matrix.config }}/NuttX/nuttx/.config diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index 1d9cda083e..3c273ba124 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -10,16 +10,20 @@ on: jobs: build: - runs-on: ubuntu-latest + runs-on: ubuntu-24.04 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: - token: ${{ secrets.ACCESS_TOKEN }} + fetch-depth: 0 + - name: Install Python3 run: sudo apt-get install python3 python3-setuptools python3-pip -y + - name: Install tools - run: pip3 install --user mypy types-requests flake8 + run: python3 -m pip install mypy types-requests flake8 --break-system-packages + - name: Check MAVSDK test scripts with mypy run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py + - name: Check MAVSDK test scripts with flake8 run: $HOME/.local/bin/flake8 test/mavsdk_tests/*.py diff --git a/.github/workflows/ros_translation_node.yml b/.github/workflows/ros_translation_node.yml new file mode 100644 index 0000000000..5a65064edb --- /dev/null +++ b/.github/workflows/ros_translation_node.yml @@ -0,0 +1,52 @@ +name: ROS Translation Node Tests +on: + push: + branches: + - 'main' + pull_request: + branches: + - '*' +defaults: + run: + shell: bash +jobs: + build_and_test: + name: Build and test + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] + strategy: + fail-fast: false + matrix: + config: + - {ros_version: "humble", ubuntu: "jammy"} + - {ros_version: "jazzy", ubuntu: "noble"} + container: + image: rostooling/setup-ros-docker:ubuntu-${{ matrix.config.ubuntu }}-latest + steps: + - name: Setup ROS 2 (${{ matrix.config.ros_version }}) + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.config.ros_version }} + - name: Checkout repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + # Workaround for https://github.com/actions/runner/issues/2033 + - name: ownership workaround + run: git config --system --add safe.directory '*' + + - name: Check .msg file versioning + if: github.event_name == 'pull_request' + run: | + ./Tools/ci/check_msg_versioning.sh ${{ github.event.pull_request.base.sha }} ${{github.event.pull_request.head.sha}} + + - name: Build and test + run: | + ros_ws=/ros_ws + mkdir -p $ros_ws/src + ./Tools/copy_to_ros_ws.sh $ros_ws + cd $ros_ws + source /opt/ros/${{ matrix.config.ros_version }}/setup.sh + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --event-handlers=console_cohesion+ + source ./install/setup.sh + ./build/translation_node/translation_node_unit_tests diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index 0492137a49..3d326cda50 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -16,7 +16,7 @@ on: jobs: build: name: Testing PX4 ${{ matrix.config.model }} - runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] + runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] container: image: px4io/px4-dev-simulation-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined diff --git a/.gitmodules b/.gitmodules index 8950bcc750..1ebafcf5a4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,10 +2,6 @@ path = src/modules/mavlink/mavlink url = https://github.com/mavlink/mavlink.git branch = master -[submodule "src/drivers/uavcan/libuavcan"] - path = src/drivers/uavcan/libuavcan - url = https://github.com/dronecan/libuavcan.git - branch = main [submodule "Tools/simulation/jmavsim/jMAVSim"] path = Tools/simulation/jmavsim/jMAVSim url = https://github.com/PX4/jMAVSim.git @@ -87,3 +83,9 @@ path = src/drivers/actuators/vertiq_io/iq-module-communication-cpp url = https://github.com/PX4/iq-module-communication-cpp.git branch = master +[submodule "src/drivers/uavcan/libdronecan/dsdl"] + path = src/drivers/uavcan/libdronecan/dsdl + url = https://github.com/PX4/DSDL.git +[submodule "src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan"] + path = src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan + url = https://github.com/dronecan/pydronecan diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 02d4b3f36e..03968a6e06 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -131,6 +131,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_can-flow_canbootloader + ark_can-flow-mr_canbootloader: + short: ark_can-flow-mr_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_can-flow-mr_canbootloader ark_can-gps_default: short: ark_can-gps_default buildType: MinSizeRel @@ -166,6 +171,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_septentrio-gps_canbootloader + ark_teseo-gps_canbootloader: + short: ark_teseo-gps_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_teseo-gps_canbootloader ark_cannode_default: short: ark_cannode_default buildType: MinSizeRel @@ -186,6 +196,16 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_fmu-v6x_default + ark_fpv_bootloader: + short: ark_fpv_bootloader + buildType: MinSizeRel + settings: + CONFIG: ark_fpv_bootloader + ark_fpv_default: + short: ark_fpv_default + buildType: MinSizeRel + settings: + CONFIG: ark_fpv_default ark_pi6x_bootloader: short: ark_pi6x_bootloader buildType: MinSizeRel @@ -291,11 +311,36 @@ CONFIG: buildType: MiniSizeRel settings: CONFIG: matek_gnss-m9n-f4_default + micoair_h743_bootloader: + short: micoair_h743_bootloader + buildType: MinSizeRel + settings: + CONFIG: micoair_h743_bootloader micoair_h743_default: short: micoair_h743 buildType: MinSizeRel settings: CONFIG: micoair_h743_default + micoair_h743-aio_bootloader: + short: micoair_h743-aio_bootloader + buildType: MinSizeRel + settings: + CONFIG: micoair_h743-aio_bootloader + micoair_h743-aio_default: + short: micoair_h743-aio + buildType: MinSizeRel + settings: + CONFIG: micoair_h743-aio_default + micoair_h743-v2_bootloader: + short: micoair_h743-v2_bootloader + buildType: MinSizeRel + settings: + CONFIG: micoair_h743-v2_bootloader + micoair_h743-v2_default: + short: micoair_h743-v2 + buildType: MinSizeRel + settings: + CONFIG: micoair_h743-v2_default modalai_fc-v1_default: short: modalai_fc-v1 buildType: MinSizeRel @@ -346,6 +391,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: nxp_mr-canhubk3_fmu + nxp_tropic-community_default: + short: nxp_tropic-community_default + buildType: MinSizeRel + settings: + CONFIG: nxp_tropic-community_default raspberrypi_pico_default: short: raspberrypi_pico buildType: MinSizeRel diff --git a/.vscode/settings.json b/.vscode/settings.json index 0192c81e86..1e799721ce 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -129,5 +129,4 @@ "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" }, - "ros.distro": "humble" } diff --git a/CMakeLists.txt b/CMakeLists.txt index b63a44dfe9..55362b1425 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -408,8 +408,15 @@ include(px4_add_gtest) if(BUILD_TESTING) include(gtest) + # Ensure there's no -R without any filter expression since that trips newer ctest versions + if(TESTFILTER) + set(TESTFILTERARG "-R") + else() + set(TESTFILTERARG "") + endif() + add_custom_target(test_results - COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test -R ${TESTFILTER} USES_TERMINAL + COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test ${TESTFILTERARG} ${TESTFILTER} DEPENDS px4 examples__dyn_hello diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in index 0874d152cf..979366b838 100644 --- a/Documentation/Doxyfile.in +++ b/Documentation/Doxyfile.in @@ -828,7 +828,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \ +EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libdronecan \ @CMAKE_SOURCE_DIR@/src/examples \ @CMAKE_SOURCE_DIR@/src/templates diff --git a/Jenkinsfile b/Jenkinsfile index d7f166b5fe..9e9f5787c8 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -231,9 +231,13 @@ pipeline { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_msgs.git") // 'main' branch sh('rm -f px4_msgs/msg/*.msg') + sh('rm -f px4_msgs/msg/versioned/*.msg') sh('rm -f px4_msgs/srv/*.srv') + sh('rm -f px4_msgs/srv/versioned/*.srv') sh('cp msg/*.msg px4_msgs/msg/') + sh('mkdir -p px4_msgs/msg/versioned && cp msg/versioned/*.msg px4_msgs/msg/versioned/') sh('cp srv/*.srv px4_msgs/srv/') + sh('mkdir -p px4_msgs/srv/versioned && cp srv/versioned/*.srv px4_msgs/srv/versioned/') sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') sh('cd px4_msgs; git push origin main || true') sh('rm -rf px4_msgs') diff --git a/MAINTAINERS.md b/MAINTAINERS.md new file mode 100644 index 0000000000..426852e7b2 --- /dev/null +++ b/MAINTAINERS.md @@ -0,0 +1,42 @@ +Maintainers +=========== + +See [the documentation on Maintainers](https://docs.px4.io/main/en/contribute/maintainers.html) to learn about the role of the maintainers and the process to become one. + +**Active Maintainers** + +| Name | Sector | GitHub | Chat | email +|-------------------------|--------|--------|------|---------------- +| Lorenz Meier | Founder | [LorenzMeier][LorenzMeier] | | +| Daniel Agar | Architecture | [dagar][dagar] | daniel_agar | +| Beat Küng | Architecture | [bkueng][bkueng] | beatkueng | +| Ramón Roche | CI / Testing | [mrpollo][mrpollo] | rroche | +| Mathieu Bresciani | State Estimation | [bresch][bresch] | mbresch | +| Paul Riseborough | State Estimation | [priseborough][priseborough] | | +| David Sidrane | RTOS / NuttX | [davids5][davids5] | david_s5 | +| Jayoung Lim | Simulation | [Jaeyoung-Lim][Jaeyoung-Lim] | jaeyounglim. | +| Beniamino Pozzan | ROS 2 | [beniaminopozzan][beniaminopozzan] | beniaminopozzan | +| Matthias Grob | Multirotor | [MaEtUgR][MaEtUgR] | maetugr | +| Silvan Fuhrer | Fixed-Wing / VTOL | [sfuhrer][sfuhrer] | sfuhrer | +| Christian Friedrich | Rover | [chfriedrich98][chfriedrich98] | christian982564 | +| Pedro Roque | Spacecraft | [Pedro-Roque][Pedro-Roque] | .pedroroque | + + +**Documentation Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Hamish Willee | [hamishwillee][hamishwillee] | hamishwillee | + +**Release Managers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Ramón Roche | [mrpollo][mrpollo] | rroche | +| Daniel Agar | [dagar][dagar] | daniel_agar | + +**Retired Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| | | | diff --git a/Makefile b/Makefile index 5ef162fc3a..cfd87b6e23 100644 --- a/Makefile +++ b/Makefile @@ -326,6 +326,7 @@ px4io_update: bootloaders_update: \ 3dr_ctrl-zero-h7-oem-revg_bootloader \ ark_fmu-v6x_bootloader \ + ark_fpv_bootloader \ ark_pi6x_bootloader \ cuav_nora_bootloader \ cuav_x7pro_bootloader \ @@ -342,6 +343,8 @@ bootloaders_update: \ matek_h743-mini_bootloader \ matek_h743-slim_bootloader \ micoair_h743_bootloader \ + micoair_h743-aio_bootloader \ + micoair_h743-v2_bootloader \ modalai_fc-v2_bootloader \ mro_ctrl-zero-classic_bootloader \ mro_ctrl-zero-h7_bootloader \ @@ -401,7 +404,7 @@ check_newlines: # Testing # -------------------------------------------------------------------- -.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance +.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard .PHONY: rostest python_coverage tests: @@ -454,10 +457,6 @@ tests_offboard: rostest @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_rpyrt_ctl.test -tests_avoidance: rostest - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_safe_landing.test - python_coverage: @mkdir -p "$(SRC_DIR)"/build/python_coverage @cd "$(SRC_DIR)"/build/python_coverage && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=px4_sitl_default -DPYTHON_COVERAGE=ON diff --git a/README.md b/README.md index 1e54d7a907..dac25f0ea1 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ [![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot) -[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22) +[![Build Targets](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml/badge.svg?branch=main)](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22) [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode) @@ -44,30 +44,9 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con ## Maintenance Team -Note: This is the source of truth for the active maintainers of PX4 ecosystem. +See the latest list of maintainers on [MAINTAINERS](MAINTAINERS.md) file at the root of the project. -| Sector | Maintainer | -|---|---| -| Founder | [Lorenz Meier](https://github.com/LorenzMeier) | -| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)| -| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) | -| OS/NuttX | [David Sidrane](https://github.com/davids5) | -| Drivers | [Daniel Agar](https://github.com/dagar) | -| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) | -| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) | -| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) | -| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) | - -| Vehicle Type | Maintainer | -|---|---| -| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) | -| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) | -| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) | -| Rover | [Christian Friedrich](https://github.com/chfriedrich98) | -| Boat | x | - - -See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date. +For the latest stats on contributors please see the latest stats for the Dronecode ecosystem in our project dashboard under [LFX Insights](https://insights.lfx.linuxfoundation.org/foundation/dronecode). For information on how to update your profile and affiliations please see the following support link on how to [Complete Your LFX Profile](https://docs.linuxfoundation.org/lfx/my-profile/complete-your-lfx-profile). Dronecode publishes a yearly snapshot of contributions and achievements on its [website under the Reports section](https://dronecode.org). ## Supported Hardware @@ -104,7 +83,7 @@ These boards fully comply with Pixhawk Standard, and are maintained by the PX4-A These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers. -* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html) +* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/ark_v6x.html) * [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html) * [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html) * [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html) @@ -119,7 +98,7 @@ These boards don't fully comply industry standards, and thus is solely maintaine ### Experimental -These boards are nor maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases. +These boards are not maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases. * [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html) * [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx index 7c04d44650..623ee41f1f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx @@ -16,10 +16,6 @@ param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 -# disable some checks to allow to fly: -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 - # Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR0_PX 1 @@ -38,4 +34,6 @@ param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 +param set-default EKF2_GPS_DELAY 0 + param set SIH_VEHICLE_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index fa132e6d6d..4f1ddf2635 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -16,12 +16,6 @@ param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 -# disable some checks to allow to fly: -# - with usb -param set-default CBRK_USB_CHK 197848 -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 - param set-default SIH_T_MAX 6 param set-default SIH_MASS 0.3 param set-default SIH_IXX 0.00402 @@ -33,19 +27,22 @@ param set-default SIH_KDV 0.2 param set-default SIH_VEHICLE_TYPE 1 # sih as fixed wing param set-default RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched) - param set-default CA_AIRFRAME 1 - param set-default CA_ROTOR_COUNT 1 +param set-default CA_ROTOR0_PX 0.3 +# SIH for now hardcodes this configuration which we need to match in the airframe files. param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TRQ_R -0.5 -param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron param set-default CA_SV_CS1_TRQ_P 1 -param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS1_TYPE 3 # elevator param set-default CA_SV_CS2_TRQ_Y 1 -param set-default CA_SV_CS2_TYPE 4 -param set-default PWM_MAIN_FUNC3 201 -param set-default PWM_MAIN_FUNC4 202 -param set-default PWM_MAIN_FUNC5 203 -param set-default PWM_MAIN_FUNC6 101 +param set-default CA_SV_CS2_TYPE 4 # rudder + +param set-default PWM_MAIN_FUNC1 201 +param set-default PWM_MAIN_FUNC2 202 +param set-default PWM_MAIN_FUNC3 203 +param set-default PWM_MAIN_FUNC4 101 + +param set-default EKF2_GPS_DELAY 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index 4910dbc850..ee5ea9401c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -11,6 +11,7 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert} +param set-default EKF2_GPS_DELAY 0 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters param set-default SENS_EN_GPSSIM 1 @@ -27,10 +28,6 @@ param set-default MC_PITCH_P 5 param set-default MAV_TYPE 19 -# disable some checks to allow to fly: -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 - param set-default SIH_T_MAX 2 param set-default SIH_Q_MAX 0.0165 param set-default SIH_MASS 0.2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol new file mode 100644 index 0000000000..4abc9de264 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol @@ -0,0 +1,96 @@ +#!/bin/sh +# +# @name SIH Standard VTOL +# +# @type Simulation +# @class VTOL +# +# @output Motor1 MC motor front right +# @output Motor2 MC motor back left +# @output Motor3 MC motor front left +# @output Motor4 MC motor back right +# @output Motor5 Forward thrust motor +# @output Servo1 Ailerons (single channel) +# @output Servo2 Elevator +# @output Servo3 Rudder +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 + +param set-default EKF2_GPS_DELAY 0 + +param set-default VT_TYPE 2 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR0_PX 0.2 +param set-default CA_ROTOR0_PY 0.2 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_PX -0.2 +param set-default CA_ROTOR1_PY -0.2 +param set-default CA_ROTOR2_PX 0.2 +param set-default CA_ROTOR2_PY -0.2 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.2 +param set-default CA_ROTOR3_PY 0.2 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_PX -0.3 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 + +# SIH for now hardcodes this configuration which we need to match in the airframe files. +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 # elevator +param set-default CA_SV_CS2_TRQ_Y 1 +param set-default CA_SV_CS2_TYPE 4 # rudder + +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 +param set-default FW_AIRSPD_MAX 12 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 201 +param set-default PWM_MAIN_FUNC6 202 +param set-default PWM_MAIN_FUNC7 203 +param set-default PWM_MAIN_FUNC8 105 + +param set-default MAV_TYPE 22 + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +# param set-default SYS_HITL 2 + +param set-default SENS_DPRES_OFF 0.001 + +param set SIH_T_MAX 2.0 +param set SIH_Q_MAX 0.0165 +param set SIH_MASS 0.2 +# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg +param set SIH_IXX 0.00354 +param set SIH_IYY 0.000625 +param set SIH_IZZ 0.00300 +param set SIH_IXZ 0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.2 + +# sih as standard vtol +param set SIH_VEHICLE_TYPE 3 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid deleted file mode 100644 index 8221969873..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/sh -# -# @name 3DR Iris Quadrotor SITL (Obstacle Avoidance) -# -# @type Quadrotor Wide -# - -. ${R}etc/init.d/rc.mc_defaults - - -param set-default CA_AIRFRAME 0 - -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 -param set-default CA_ROTOR3_KM -0.05 - -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 - -param set-default COM_OBS_AVOID 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post deleted file mode 100644 index 64b8ce8b19..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post +++ /dev/null @@ -1,2 +0,0 @@ -# shellcheck disable=SC2154 -mavlink start -x -u 14558 -r 4000000 -m onboard -o 14541 -p # add mavlink stream for SDK diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post new file mode 100644 index 0000000000..647362a710 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam.post @@ -0,0 +1 @@ +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index d7ea1f3b37..04fc1acfd8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -24,33 +24,33 @@ param set-default FW_LND_ANG 8 param set-default NPFG_PERIOD 12 param set-default FW_PR_P 0.9 -param set-default FW_PR_FF 0.5 +param set-default FW_PR_FF 0.2 param set-default FW_PR_I 0.5 param set-default TRIM_PITCH -0.15 -param set-default FW_PSP_OFF 2 -param set-default FW_P_LIM_MIN -15 - param set-default FW_RR_FF 0.5 -param set-default FW_RR_P 0.3 -param set-default FW_RR_I 0.5 +param set-default FW_RR_P 0.4 +param set-default FW_RR_I 0.7 -param set-default FW_YR_FF 0.5 -param set-default FW_YR_P 0.6 -param set-default FW_YR_I 0.5 +param set-default FW_R_RMAX 56.15 + +param set-default FW_YR_FF 0.3 +param set-default FW_YR_P 1.3 +param set-default FW_YR_I 0.7 + +param set-default FW_PSP_OFF 0 +param set-default FW_P_LIM_MIN -15 param set-default FW_SPOILERS_LND 0.4 -param set-default FW_THR_MAX 0.6 -param set-default FW_THR_MIN 0.05 -param set-default FW_THR_TRIM 0.25 - -param set-default FW_T_CLMB_MAX 8 -param set-default FW_T_SINK_MAX 2.7 -param set-default FW_T_SINK_MIN 2.2 +param set-default FW_T_CLMB_MAX 5 +param set-default FW_T_SINK_MAX 3.5 +param set-default FW_T_SINK_MIN 3 param set-default FW_W_EN 1 +param set-default FD_ESCS_EN 0 + param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index c590938178..967051b042 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -19,7 +19,6 @@ param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters -param set-default COM_OBS_AVOID 0 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters @@ -75,7 +74,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index b41d61355c..f05bbdcf45 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -11,29 +11,39 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAN_YAW_SCALE 0.1 -param set-default RD_MAX_ACCEL 6 -param set-default RD_MAX_JERK 30 -param set-default RD_MAX_THR_YAW_R 5 -param set-default RD_YAW_RATE_P 0.1 -param set-default RD_YAW_RATE_I 0 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0 -param set-default RD_MAX_SPEED 5 -param set-default RD_MAX_THR_SPD 7 -param set-default RD_SPEED_P 1 -param set-default RD_SPEED_I 0 -param set-default RD_MAX_YAW_RATE 180 -param set-default RD_MISS_SPD_DEF 5 +param set-default RD_MAX_THR_YAW_R 1.5 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 -# Pure pursuit parameters -param set-default PP_LOOKAHD_MAX 30 -param set-default PP_LOOKAHD_MIN 2 +# Rover Control Parameters +param set-default RO_ACCEL_LIM 5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_ACCEL_LIM 120 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 # Simulated sensors param set-default SENS_EN_GPSSIM 1 @@ -43,13 +53,13 @@ param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel -param set-default SIM_GZ_WH_MIN1 0 -param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_MIN1 70 +param set-default SIM_GZ_WH_MAX1 130 param set-default SIM_GZ_WH_DIS1 100 param set-default SIM_GZ_WH_FUNC2 102 # left wheel -param set-default SIM_GZ_WH_MIN2 0 -param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_MIN2 70 +param set-default SIM_GZ_WH_MAX2 130 param set-default SIM_GZ_WH_DIS2 100 param set-default SIM_GZ_WH_REV 1 # reverse right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 8a05e2a15e..0fec76b5c4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -21,7 +21,6 @@ param set-default COM_ARM_WO_GPS 1 # Rover parameters param set-default RD_WHEEL_TRACK 0.9 -param set-default RD_MAN_YAW_SCALE 0.1 param set-default RD_YAW_RATE_I 0.1 param set-default RD_YAW_RATE_P 5 param set-default RD_MAX_ACCEL 1 @@ -30,7 +29,6 @@ param set-default RD_MAX_SPEED 8 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0.1 param set-default RD_MAX_YAW_RATE 30 -param set-default RD_MISS_SPD_DEF 8 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 @@ -62,19 +60,25 @@ param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): -param set-default SIM_GZ_SV_FUNC3 203 -param set-default SIM_GZ_SV_MIN3 0 -param set-default SIM_GZ_SV_MAX3 1000 -param set-default SIM_GZ_SV_DIS3 500 -param set-default SIM_GZ_SV_FAIL3 500 +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_MIN1 0 +param set-default SIM_GZ_SV_MAX1 1000 +param set-default SIM_GZ_SV_DIS1 500 +param set-default SIM_GZ_SV_FAIL1 500 +param set-default SIM_GZ_SV_MAXA1 90 +param set-default SIM_GZ_SV_MINA1 -90 # Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204): # - on minimum when disarmed or failed: -param set-default SIM_GZ_SV_FUNC4 204 -param set-default SIM_GZ_SV_MIN4 0 -param set-default SIM_GZ_SV_MAX4 1000 -param set-default SIM_GZ_SV_DIS4 500 -param set-default SIM_GZ_SV_FAIL4 500 +param set-default SIM_GZ_SV_FUNC2 202 +param set-default SIM_GZ_SV_MIN2 0 +param set-default SIM_GZ_SV_MAX2 1000 +param set-default SIM_GZ_SV_DIS2 500 +param set-default SIM_GZ_SV_FAIL2 500 +param set-default SIM_GZ_SV_MAXA2 90 +param set-default SIM_GZ_SV_MINA2 -90 + +param set-default CA_SV_CS_COUNT 2 # Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 6f81b7036a..cd870caabb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -11,21 +11,33 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_MAX_ACCEL 1.5 -param set-default RA_MAX_JERK 15 -param set-default RA_MAX_SPEED 3 param set-default RA_MAX_STR_ANG 0.5236 -param set-default RA_MAX_STR_RATE 360 -param set-default RA_MISS_VEL_DEF 3 -param set-default RA_MISS_VEL_GAIN 5 -param set-default RA_MISS_VEL_MIN 1 -param set-default RA_SPEED_I 0.01 -param set-default RA_SPEED_P 2 -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_STR_RATE_LIM 360 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 6 +param set-default RO_JERK_LIM 15 +param set-default RO_MAX_THR_SPEED 3.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.1 +param set-default RO_YAW_RATE_P 1 +param set-default RO_YAW_RATE_LIM 180 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 3 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 3 +param set-default RO_SPEED_I 0.1 +param set-default RO_SPEED_P 1 # Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 @@ -45,5 +57,8 @@ param set-default SIM_GZ_WH_MAX1 200 param set-default SIM_GZ_WH_DIS1 100 # Steering +param set-default SIM_GZ_SV_MAXA1 30 +param set-default SIM_GZ_SV_MINA1 -30 +param set-default CA_SV_CS_COUNT 1 param set-default SIM_GZ_SV_FUNC1 201 param set-default SIM_GZ_SV_REV 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar_2d similarity index 52% rename from ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar rename to ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar_2d index a316310abe..da908b6d55 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar_2d @@ -1,10 +1,10 @@ #!/bin/sh # -# @name Gazebo x500 lidar +# @name Gazebo x500 lidar 2d # # @type Quadrotor # -PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_2d} . ${R}etc/init.d-posix/airframes/4001_gz_x500 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index e0968a3db5..e252b3cfa0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -13,18 +13,18 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge # Rover parameters param set-default RM_WHEEL_TRACK 0.3 -param set-default RM_MAN_YAW_SCALE 0.1 -param set-default RM_YAW_RATE_I 0 -param set-default RM_YAW_RATE_P 0.01 +param set-default RM_YAW_RATE_I 0.1 +param set-default RM_YAW_RATE_P 0.1 param set-default RM_MAX_ACCEL 3 +param set-default RM_MAX_DECEL 5 param set-default RM_MAX_JERK 5 -param set-default RM_MAX_SPEED 4 -param set-default RM_MAX_THR_SPD 7 -param set-default RM_MAX_THR_YAW_R 7.5 +param set-default RM_MAX_SPEED 2 +param set-default RM_MAX_THR_SPD 2.2 +param set-default RM_MAX_THR_YAW_R 1.2 param set-default RM_YAW_P 5 param set-default RM_YAW_I 0.1 -param set-default RM_MAX_YAW_RATE 180 -param set-default RM_MISS_SPD_DEF 3 +param set-default RM_MAX_YAW_RATE 120 +param set-default RM_MAX_YAW_ACCEL 240 param set-default RM_MISS_VEL_GAIN 1 param set-default RM_SPEED_I 0.01 param set-default RM_SPEED_P 0.1 @@ -42,23 +42,23 @@ param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 102 # right wheel front -param set-default SIM_GZ_WH_MIN1 0 -param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_MIN1 70 +param set-default SIM_GZ_WH_MAX1 130 param set-default SIM_GZ_WH_DIS1 100 param set-default SIM_GZ_WH_FUNC2 101 # left wheel front -param set-default SIM_GZ_WH_MIN2 0 -param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_MIN2 70 +param set-default SIM_GZ_WH_MAX2 130 param set-default SIM_GZ_WH_DIS2 100 param set-default SIM_GZ_WH_FUNC3 104 # right wheel back -param set-default SIM_GZ_WH_MIN3 0 -param set-default SIM_GZ_WH_MAX3 200 +param set-default SIM_GZ_WH_MIN3 70 +param set-default SIM_GZ_WH_MAX3 130 param set-default SIM_GZ_WH_DIS3 100 param set-default SIM_GZ_WH_FUNC4 103 # left wheel back -param set-default SIM_GZ_WH_MIN4 0 -param set-default SIM_GZ_WH_MAX4 200 +param set-default SIM_GZ_WH_MIN4 70 +param set-default SIM_GZ_WH_MAX4 130 param set-default SIM_GZ_WH_DIS4 100 param set-default SIM_GZ_WH_REV 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down b/ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down new file mode 100644 index 0000000000..99e986231e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down @@ -0,0 +1,10 @@ +#!/bin/sh +# +# @name Gazebo x500 lidar down +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_down} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4017_gz_x500_lidar_front b/ROMFS/px4fmu_common/init.d-posix/airframes/4017_gz_x500_lidar_front new file mode 100644 index 0000000000..6d8773e091 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4017_gz_x500_lidar_front @@ -0,0 +1,10 @@ +#!/bin/sh +# +# @name Gazebo x500 lidar front +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_front} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter new file mode 100644 index 0000000000..9856b1bf27 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -0,0 +1,97 @@ +#!/bin/sh +# +# @name Quadrotor + Tailsitter +# +# @type VTOL Quad Tailsitter +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + + +param set-default MAV_TYPE 20 + +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.23 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.23 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.23 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.23 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_SV_CS_COUNT 0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 + +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MIN4 10 + +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default FD_FAIL_R 70 + +param set-default FW_P_TC 0.6 + +param set-default FW_PR_I 0.3 +param set-default FW_PR_P 0.5 +param set-default FW_PSP_OFF 2 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_I 0.1 +param set-default FW_RR_P 0.2 +param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P +param set-default FW_YR_I 0 +param set-default FW_THR_TRIM 0.35 +param set-default FW_THR_MAX 0.8 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 6 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 3 +param set-default FW_T_SINK_MIN 1.6 +param set-default FW_AIRSPD_STALL 10 +param set-default FW_AIRSPD_MIN 14 +param set-default FW_AIRSPD_TRIM 18 +param set-default FW_AIRSPD_MAX 22 + +param set-default MC_AIRMODE 2 +param set-default MAN_ARM_GESTURE 0 # required for yaw airmode +param set-default MC_ROLL_P 3 +param set-default MC_PITCH_P 3 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_PITCHRATE_P 0.3 + +param set-default VT_ARSP_TRANS 15 +param set-default VT_B_TRANS_DUR 5 +param set-default VT_FW_DIFTHR_EN 7 +param set-default VT_FW_DIFTHR_S_Y 1 +param set-default VT_F_TRANS_DUR 1.5 +param set-default VT_TYPE 0 + +param set-default WV_EN 0 + +param set-default EKF2_FUSE_BETA 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal new file mode 100644 index 0000000000..80719ae3d3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4019_gz_x500_gimbal @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name Gazebo x500 gimbal +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_gimbal} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 + +# Gimbal settings +param set-default MNT_MODE_IN 4 +param set-default MNT_MODE_OUT 2 +param set-default MNT_RC_IN_MODE 1 + +param set-default MNT_MAN_ROLL 1 +param set-default MNT_MAN_PITCH 2 +param set-default MNT_MAN_YAW 3 + +param set-default MNT_RANGE_ROLL 180 +param set-default MNT_RANGE_PITCH 180 +param set-default MNT_RANGE_YAW 720 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor new file mode 100644 index 0000000000..c798650b54 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -0,0 +1,112 @@ +#!/bin/sh +# +# @name VTOL Tiltrotor +# +# @type VTOL Tiltrotor +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + +param set-default MAV_TYPE 21 + +param set-default CA_AIRFRAME 3 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.1515 +param set-default CA_ROTOR0_PY 0.245 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.1515 +param set-default CA_ROTOR1_PY -0.1875 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.1515 +param set-default CA_ROTOR2_PY -0.245 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.1515 +param set-default CA_ROTOR3_PY 0.1875 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_ROTOR0_TILT 1 +param set-default CA_ROTOR2_TILT 2 +param set-default CA_SV_TL0_MAXA 90 +param set-default CA_SV_TL0_MINA 0 +param set-default CA_SV_TL0_TD 0 +param set-default CA_SV_TL0_CT 1 +param set-default CA_SV_TL1_MAXA 90 +param set-default CA_SV_TL1_MINA 0 +param set-default CA_SV_TL1_TD 0 +param set-default CA_SV_TL1_CT 1 + +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS2_TRQ_P 1 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_TL_COUNT 2 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 + +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MIN4 10 + +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_FUNC2 202 +param set-default SIM_GZ_SV_FUNC3 203 +param set-default SIM_GZ_SV_FUNC4 204 +param set-default SIM_GZ_SV_FUNC5 205 + +param set-default SIM_GZ_SV_MAXA4 90 +param set-default SIM_GZ_SV_MINA4 0 +param set-default SIM_GZ_SV_MAXA5 90 +param set-default SIM_GZ_SV_MINA5 0 + +param set-default NPFG_PERIOD 12 +param set-default FW_PR_FF 0.2 +param set-default FW_PR_P 0.9 +param set-default FW_PSP_OFF 2 +param set-default FW_P_LIM_MIN -15 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_P 0.3 +param set-default FW_THR_TRIM 0.38 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 + +param set-default MC_AIRMODE 1 +param set-default MC_YAWRATE_P 0.4 +param set-default MC_YAWRATE_I 0.1 + +param set-default MPC_XY_VEL_P_ACC 3 +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_D_ACC 0.1 + +param set-default MIS_TAKEOFF_ALT 10 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_FWD_THRUST_SC 0.6 +param set-default VT_TILT_TRANS 0.6 +param set-default VT_TYPE 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart new file mode 100644 index 0000000000..737ab67b85 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart @@ -0,0 +1,155 @@ +#!/bin/sh +# +# @name 6DoF Spacecraft Model +# +# @type Freeflyer with 8 thrusters +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 15 +param set-default MAV_TYPE 99 + +param set-default CA_THRUSTER_CNT 12 +param set-default CA_R_REV 0 + +# param set-default FW_ARSP_MODE 1 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# Set thrusters +param set-default CA_THRUSTER0_PX -0.50 +param set-default CA_THRUSTER0_PY 0.50 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 0.237 +param set-default CA_THRUSTER0_AX 0.0 +param set-default CA_THRUSTER0_AY -1.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.50 +param set-default CA_THRUSTER1_PY 0.50 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 0.237 +param set-default CA_THRUSTER1_AX 0.0 +param set-default CA_THRUSTER1_AY -1.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX 0.50 +param set-default CA_THRUSTER2_PY -0.50 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 0.237 +param set-default CA_THRUSTER2_AX 0.0 +param set-default CA_THRUSTER2_AY 1.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX -0.50 +param set-default CA_THRUSTER3_PY -0.50 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 0.237 +param set-default CA_THRUSTER3_AX 0.0 +param set-default CA_THRUSTER3_AY 1.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX -0.50 +param set-default CA_THRUSTER4_PY 0.0 +param set-default CA_THRUSTER4_PZ -0.50 +param set-default CA_THRUSTER4_CT 0.237 +param set-default CA_THRUSTER4_AX 1.0 +param set-default CA_THRUSTER4_AY 0.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.50 +param set-default CA_THRUSTER5_PY 0.0 +param set-default CA_THRUSTER5_PZ -0.50 +param set-default CA_THRUSTER5_CT 0.237 +param set-default CA_THRUSTER5_AX -1.0 +param set-default CA_THRUSTER5_AY 0.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX 0.50 +param set-default CA_THRUSTER6_PY 0.0 +param set-default CA_THRUSTER6_PZ 0.50 +param set-default CA_THRUSTER6_CT 0.237 +param set-default CA_THRUSTER6_AX -1.0 +param set-default CA_THRUSTER6_AY 0.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.50 +param set-default CA_THRUSTER7_PY 0.0 +param set-default CA_THRUSTER7_PZ 0.50 +param set-default CA_THRUSTER7_CT 0.237 +param set-default CA_THRUSTER7_AX 1.0 +param set-default CA_THRUSTER7_AY 0.0 +param set-default CA_THRUSTER7_AZ 0.0 + +param set-default CA_THRUSTER8_PX 0.0 +param set-default CA_THRUSTER8_PY -0.50 +param set-default CA_THRUSTER8_PZ -0.50 +param set-default CA_THRUSTER8_CT 0.237 +param set-default CA_THRUSTER8_AX 0.0 +param set-default CA_THRUSTER8_AY 0.0 +param set-default CA_THRUSTER8_AZ 1.0 + +param set-default CA_THRUSTER9_PX 0.0 +param set-default CA_THRUSTER9_PY 0.50 +param set-default CA_THRUSTER9_PZ -0.50 +param set-default CA_THRUSTER9_CT 0.237 +param set-default CA_THRUSTER9_AX 0.0 +param set-default CA_THRUSTER9_AY 0.0 +param set-default CA_THRUSTER9_AZ 1.0 + +param set-default CA_THRUSTER10_PX 0.0 +param set-default CA_THRUSTER10_PY 0.50 +param set-default CA_THRUSTER10_PZ 0.50 +param set-default CA_THRUSTER10_CT 0.237 +param set-default CA_THRUSTER10_AX 0.0 +param set-default CA_THRUSTER10_AY 0.0 +param set-default CA_THRUSTER10_AZ -1.0 + +param set-default CA_THRUSTER11_PX 0.0 +param set-default CA_THRUSTER11_PY -0.50 +param set-default CA_THRUSTER11_PZ 0.50 +param set-default CA_THRUSTER11_CT 0.237 +param set-default CA_THRUSTER11_AX 0.0 +param set-default CA_THRUSTER11_AY 0.0 +param set-default CA_THRUSTER11_AZ -1.0 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 105 +param set-default PWM_MAIN_FUNC6 106 +param set-default PWM_MAIN_FUNC7 107 +param set-default PWM_MAIN_FUNC8 108 +param set-default PWM_MAIN_FUNC9 109 +param set-default PWM_MAIN_FUNC10 110 +param set-default PWM_MAIN_FUNC11 111 +param set-default PWM_MAIN_FUNC12 112 + +# PWM Simulation +param set PWM_SIM_PWM_MAX 10000 +param set PWM_SIM_PWM_MIN 0 + +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d new file mode 100644 index 0000000000..480454d72b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -0,0 +1,149 @@ +#!/bin/sh +# +# @name 3DoF Spacecraft Model +# +# @type 2D Freeflyer with 8 thrusters - Planar motion +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs +param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 99 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 0 + +# param set-default FW_ARSP_MODE 1 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 # 0 is PseudoInverse, 3 is Metric + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.4 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.4 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.4 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.4 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.4 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.4 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.4 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.4 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + +param set-default SIM_GZ_TH_FUNC1 101 +param set-default SIM_GZ_TH_FUNC2 102 +param set-default SIM_GZ_TH_FUNC3 103 +param set-default SIM_GZ_TH_FUNC4 104 +param set-default SIM_GZ_TH_FUNC5 105 +param set-default SIM_GZ_TH_FUNC6 106 +param set-default SIM_GZ_TH_FUNC7 107 +param set-default SIM_GZ_TH_FUNC8 108 + +param set-default SIM_GZ_TH_MIN1 0 +param set-default SIM_GZ_TH_MIN2 0 +param set-default SIM_GZ_TH_MIN3 0 +param set-default SIM_GZ_TH_MIN4 0 +param set-default SIM_GZ_TH_MIN5 0 +param set-default SIM_GZ_TH_MIN6 0 +param set-default SIM_GZ_TH_MIN7 0 +param set-default SIM_GZ_TH_MIN8 0 + +param set-default SIM_GZ_TH_MAX1 10000 +param set-default SIM_GZ_TH_MAX2 10000 +param set-default SIM_GZ_TH_MAX3 10000 +param set-default SIM_GZ_TH_MAX4 10000 +param set-default SIM_GZ_TH_MAX5 10000 +param set-default SIM_GZ_TH_MAX6 10000 +param set-default SIM_GZ_TH_MAX7 10000 +param set-default SIM_GZ_TH_MAX8 10000 + +# Controller Tunings +param set SC_YAWRATE_P 3.335 +param set SC_YAWRATE_I 0.87 +param set SC_YAWRATE_D 0.15 +param set SC_YR_INT_LIM 0.2 +param set SC_YAW_P 3.0 + +param set SPC_POS_P 0.20 +param set SPC_VEL_P 6.55 +param set SPC_VEL_I 0.0 +param set SPC_VEL_D 0.0 +param set SPC_VEL_MAX 12.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 1ba611c275..4ae56f51e9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -39,8 +39,6 @@ px4_add_romfs_files( 1012_gazebo-classic_iris_rplidar 1013_gazebo-classic_iris_vision 1013_gazebo-classic_iris_vision.post - 1014_gazebo-classic_iris_obs_avoid - 1014_gazebo-classic_iris_obs_avoid.post 1015_gazebo-classic_iris_depth_camera 1016_gazebo-classic_iris_downward_depth_camera 1017_gazebo-classic_iris_opt_flow_mockup @@ -49,6 +47,7 @@ px4_add_romfs_files( 1022_gazebo-classic_uuv_bluerov2_heavy 1030_gazebo-classic_plane 1031_gazebo-classic_plane_cam + 1031_gazebo-classic_plane_cam.post 1032_gazebo-classic_plane_catapult 1033_jsbsim_rascal 1034_flightgear_rascal-electric @@ -84,9 +83,14 @@ px4_add_romfs_files( 4010_gz_x500_mono_cam 4011_gz_lawnmower 4012_gz_rover_ackermann - 4013_gz_x500_lidar + 4013_gz_x500_lidar_2d 4014_gz_x500_mono_cam_down 4015_gz_r1_rover_mecanum + 4016_gz_x500_lidar_down + 4017_gz_x500_lidar_front + 4018_gz_quadtailsitter + 4019_gz_x500_gimbal + 4020_gz_tiltrotor 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post @@ -103,9 +107,13 @@ px4_add_romfs_files( 10040_sihsim_quadx 10041_sihsim_airplane 10042_sihsim_xvert + 10043_sihsim_standard_vtol 17001_flightgear_tf-g1 17002_flightgear_tf-g2 + 71001_gazebo-classic_spacecraft_dart + 71002_gz_spacecraft_2d + # [22000, 22999] Reserve for custom models ) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink index 15c6c2a793..17ef44521b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink @@ -29,7 +29,7 @@ mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_of mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote # Onboard link to gimbal -mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote +mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -f -m gimbal -o $udp_onboard_gimbal_port_remote # To display for SIH sitl if [ "$PX4_SIMULATOR" = "sihsim" ]; then diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 76be887e84..fd0954744e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -4,6 +4,9 @@ # Simulator IMU data provided at 250 Hz param set-default IMU_INTEG_RATE 250 +# For simulation, allow registering modes while armed for developer convenience +param set-default COM_MODE_ARM_CHK 1 + if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then echo "INFO [init] SIH simulator" @@ -33,6 +36,10 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" then sensor_mag_sim start fi + if param compare -s SENS_EN_AGPSIM 1 + then + sensor_agp_sim start + fi else echo "ERROR [init] simulator_sih failed to start" @@ -153,6 +160,10 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then then sensor_airspeed_sim start fi + if param compare -s SENS_EN_AGPSIM 1 + then + sensor_agp_sim start + fi elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 8902b3664f..c53dbbd46a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -153,8 +153,6 @@ fi param set-default BAT1_N_CELLS 4 -param set-default CBRK_SUPPLY_CHK 894281 - # disable check, no CPU load reported on posix yet param set-default COM_CPU_MAX -1 param set-default COM_RAM_MAX -1 @@ -181,7 +179,7 @@ param set-default TRIG_INTERFACE 3 param set-default SYS_FAILURE_EN 1 # Enable low-battery actions by default for (automated) testing. Battery sim # does not go below 50% by default, but failure injection can trigger failsafes. -param set-default COM_LOW_BAT_ACT 2 +param set-default COM_LOW_BAT_ACT 3 # Adapt timeout parameters if simulation runs faster or slower than realtime. @@ -190,11 +188,11 @@ if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER" param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER - COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc) + COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc) echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER" param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER - COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc) + COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc) echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER" param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER @@ -247,6 +245,8 @@ then battery_simulator start fi +system_power_simulator start + tone_alarm start rc_update start manual_control start @@ -337,6 +337,11 @@ then payload_deliverer start fi +if param compare -s ICE_EN 1 +then + internal_combustion_engine_control start +fi + #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 488b41575a..7c6f8f4369 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN) ) endif() +if(CONFIG_MODULES_SPACECRAFT) + px4_add_romfs_files( + rc.sc_apps + rc.sc_defaults + ) +endif() + if(CONFIG_MODULES_ROVER_MECANUM) px4_add_romfs_files( rc.rover_mecanum_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index b5a633dc1b..4129d2569c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -5,6 +5,15 @@ # @type Standard VTOL # @class VTOL # +# @output Motor1 MC motor front right +# @output Motor2 MC motor back left +# @output Motor3 MC motor front left +# @output Motor4 MC motor back right +# @output Motor5 Forward thrust motor +# @output Servo1 Aileron +# @output Servo2 Elevator +# @output Servo3 Rudder +# # @maintainer Roman Bapst # # @board px4_fmu-v2 exclude diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index a404092039..2b54650a07 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -12,28 +12,25 @@ . ${R}etc/init.d/rc.fw_defaults - param set UAVCAN_ENABLE 0 param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 param set-default CA_ROTOR0_PX 0.3 -param set-default CA_SV_CS_COUNT 4 -param set-default CA_SV_CS0_TRQ_R 0.5 -param set-default CA_SV_CS0_TYPE 2 -param set-default CA_SV_CS1_TRQ_P 1 -param set-default CA_SV_CS1_TYPE 3 -param set-default CA_SV_CS2_TRQ_Y 1 -param set-default CA_SV_CS2_TYPE 4 -param set-default CA_SV_CS3_TYPE 10 -param set-default HIL_ACT_REV 2 -param set-default HIL_ACT_FUNC1 201 -param set-default HIL_ACT_FUNC2 202 -param set-default HIL_ACT_FUNC3 203 -param set-default HIL_ACT_FUNC4 101 -param set-default HIL_ACT_FUNC5 204 -param set-default HIL_ACT_FUNC6 400 +# SIH for now hardcodes this configuration which we need to match in the airframe files. +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 # elevator +param set-default CA_SV_CS2_TRQ_Y 1 +param set-default CA_SV_CS2_TYPE 4 # rudder + +param set HIL_ACT_FUNC1 201 +param set HIL_ACT_FUNC2 202 +param set HIL_ACT_FUNC3 203 +param set HIL_ACT_FUNC4 101 # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set-default SYS_HITL 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil new file mode 100644 index 0000000000..5ac9e6b1ab --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -0,0 +1,92 @@ +#!/bin/sh +# +# @name SIH Standard VTOL QuadPlane +# +# @type Simulation +# @class VTOL +# +# @output Motor1 MC motor front right +# @output Motor2 MC motor back left +# @output Motor3 MC motor front left +# @output Motor4 MC motor back right +# @output Motor5 Forward thrust motor +# @output Servo1 Ailerons (single channel) +# @output Servo2 Elevator +# @output Servo3 Rudder +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +param set UAVCAN_ENABLE 0 + +param set-default VT_TYPE 2 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR0_PX 0.2 +param set-default CA_ROTOR0_PY 0.2 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_PX -0.2 +param set-default CA_ROTOR1_PY -0.2 +param set-default CA_ROTOR2_PX 0.2 +param set-default CA_ROTOR2_PY -0.2 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.2 +param set-default CA_ROTOR3_PY 0.2 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_PX -0.3 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 + +# SIH for now hardcodes this configuration which we need to match in the airframe files. +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 # elevator +param set-default CA_SV_CS2_TRQ_Y 1 +param set-default CA_SV_CS2_TYPE 4 # rudder + +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 +param set-default FW_AIRSPD_MAX 12 + +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 +param set-default HIL_ACT_FUNC5 201 +param set-default HIL_ACT_FUNC6 202 +param set-default HIL_ACT_FUNC7 203 +param set-default HIL_ACT_FUNC8 105 + +param set-default MAV_TYPE 22 + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +param set-default SYS_HITL 2 + +# disable some checks to allow to fly: +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 + +param set-default SENS_DPRES_OFF 0.001 + +param set SIH_T_MAX 2.0 +param set SIH_Q_MAX 0.0165 +param set SIH_MASS 0.2 +# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg +param set SIH_IXX 0.00354 +param set SIH_IYY 0.000625 +param set SIH_IZZ 0.00300 +param set SIH_IXZ 0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.2 + +# sih as standard vtol +param set SIH_VEHICLE_TYPE 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox b/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox index 1dba92b89d..d72eaa2132 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/12001_octo_cox @@ -21,7 +21,6 @@ . ${R}etc/init.d/rc.mc_defaults - param set-default MAV_TYPE 14 param set-default CA_ROTOR_COUNT 8 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad deleted file mode 100644 index ace0e397c6..0000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ /dev/null @@ -1,131 +0,0 @@ -#!/bin/sh -# -# @name Vertical Technologies DeltaQuad -# -# @type Standard VTOL -# @class VTOL -# -# @maintainer Sander Smeets -# -# @output Motor1 motor 1 -# @output Motor2 motor 2 -# @output Motor3 motor 3 -# @output Motor4 motor 4 -# @output Servo1 Right elevon -# @output Servo2 Left elevon -# @output Servo3 Pusher motor -# @output Servo4 Pusher reverse channel -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.vtol_defaults - -param set-default MAV_TYPE 22 - -param set-default BAT1_CAPACITY 23000 -param set-default BAT1_N_CELLS 4 -param set-default BAT1_R_INTERNAL 0.0025 - -param set-default SYS_HAS_NUM_ASPD 0 - -param set-default EKF2_GPS_POS_X -0.12 -param set-default EKF2_IMU_POS_X -0.12 - -param set-default FW_USE_AIRSPD 0 -param set-default NPFG_PERIOD 25 -param set-default FW_PR_FF 0.7 -param set-default FW_PR_I 0.18 -param set-default FW_PR_P 0.15 -param set-default FW_P_TC 0.5 -param set-default FW_PSP_OFF 5 -param set-default FW_R_LIM 35 -param set-default FW_RR_FF 0.9 -param set-default FW_RR_I 0.08 -param set-default FW_RR_P 0.18 -param set-default FW_T_CLMB_MAX 3 -param set-default FW_T_SINK_MAX 3 -param set-default FW_T_SINK_MIN 1 -param set-default FW_THR_TRIM 0.70 -param set-default FW_THR_SLEW_MAX 1 -param set-default FW_P_LIM_MAX 15 -param set-default FW_P_LIM_MIN -25 -param set-default FW_P_RMAX_NEG 45 -param set-default FW_P_RMAX_POS 45 -param set-default FW_R_RMAX 50 -param set-default FW_THR_MIN 0.55 -param set-default FW_BAT_SCALE_EN 1 -param set-default FW_T_RLL2THR 20 - -param set-default MC_ROLLRATE_P 0.16 -param set-default MC_ROLLRATE_I 0.01 -param set-default MC_PITCHRATE_I 0.05 - -param set-default MC_YAWRATE_MAX 20 -param set-default MC_AIRMODE 1 - -param set-default MIS_TAKEOFF_ALT 15 - -param set-default MPC_XY_P 0.8 -param set-default MPC_XY_VEL_MAX 5 -param set-default MPC_LAND_SPEED 1.2 -param set-default MPC_TILTMAX_LND 35 -param set-default MPC_Z_VEL_MAX_UP 1.5 -param set-default MPC_TKO_RAMP_T 0.8 -param set-default MPC_TILTMAX_AIR 25 -param set-default MPC_TILTMAX_LND 25 -param set-default MPC_YAWRAUTO_MAX 20 - -param set-default NAV_LOITER_RAD 100 - -param set-default PWM_MAIN_DIS5 1500 -param set-default PWM_MAIN_DIS6 1500 -param set-default PWM_MAIN_DIS7 900 -param set-default PWM_MAIN_DIS8 900 - - -param set-default SENS_BOARD_ROT 18 - -# TELEM2 config -param set-default MAV_1_CONFIG 102 -param set-default MAV_1_RATE 5000 -param set-default MAV_1_FORWARD 1 -param set-default SER_TEL2_BAUD 57600 - -param set-default VT_TYPE 2 -param set-default VT_PITCH_MIN 8 -param set-default VT_FW_QC_P 55 -param set-default VT_FW_QC_R 55 -param set-default VT_TRANS_MIN_TM 15 -param set-default VT_FWD_THRUST_SC 4 -param set-default VT_TRANS_TIMEOUT 22 - -param set-default COM_RC_OVERRIDE 0 - -param set-default CA_AIRFRAME 2 - -param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 -param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1 -param set-default CA_ROTOR4_AZ 0 -param set-default CA_ROTOR4_PX 0.2 - -param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TYPE 1 -param set-default CA_SV_CS0_TRQ_R -0.5 -param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS1_TRQ_R 0.5 -param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark deleted file mode 100644 index ee98e2c889..0000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ /dev/null @@ -1,121 +0,0 @@ -#!/bin/sh -# -# @name BabyShark VTOL -# -# @type Standard VTOL -# @class VTOL -# -# @maintainer Silvan Fuhrer -# -# @output Motor1 motor 1 -# @output Motor2 motor 2 -# @output Motor3 motor 3 -# @output Motor4 motor 4 -# @output Motor5 Pusher motor -# @output Servo1 Ailerons -# @output Servo2 A-tail left -# @output Servo3 A-tail right -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# @board holybro_kakutef7 exclude -# - -. ${R}etc/init.d/rc.vtol_defaults - -param set-default MAV_TYPE 22 - -param set-default BAT1_N_CELLS 6 - -param set-default FW_AIRSPD_MAX 30 -param set-default FW_AIRSPD_MIN 19 -param set-default FW_AIRSPD_TRIM 23 -param set-default FW_PN_R_SLEW_MAX 40 -param set-default FW_PSP_OFF 3 -param set-default FW_P_LIM_MAX 18 -param set-default FW_P_LIM_MIN -25 -param set-default FW_RLL_TO_YAW_FF 0.1 -param set-default FW_RR_P 0.08 -param set-default FW_R_LIM 45 -param set-default FW_R_RMAX 50 -param set-default FW_THR_TRIM 0.65 -param set-default FW_THR_MIN 0.3 -param set-default FW_THR_SLEW_MAX 0.6 -param set-default FW_T_SINK_MAX 15 -param set-default FW_T_SINK_MIN 3 -param set-default FW_YR_P 0.15 - -param set-default IMU_DGYRO_CUTOFF 15 -param set-default MC_PITCHRATE_MAX 60 -param set-default MC_ROLLRATE_MAX 60 -param set-default MC_YAWRATE_I 0.15 -param set-default MC_YAWRATE_MAX 40 -param set-default MC_YAWRATE_P 0.3 - -param set-default MC_AIRMODE 1 -param set-default MPC_LAND_SPEED 1 -param set-default MPC_MAN_TILT_MAX 25 -param set-default MPC_MAN_Y_MAX 40 -param set-default MPC_THR_HOVER 0.45 -param set-default MPC_TILTMAX_AIR 25 -param set-default MPC_YAWRAUTO_MAX 40 -param set-default MPC_Z_VEL_MAX_UP 2 - -param set-default NAV_ACC_RAD 3 - -param set-default PWM_MAIN_DIS3 1000 -param set-default PWM_MAIN_MIN3 1120 - -param set-default SENS_BOARD_ROT 4 - -param set-default VT_ARSP_BLEND 10 -param set-default VT_ARSP_TRANS 21 -param set-default VT_B_DEC_MSS 1.5 -param set-default VT_ELEV_MC_LOCK 0 -param set-default VT_FWD_THRUST_SC 1.2 -param set-default VT_F_TR_OL_TM 8 -param set-default VT_TRANS_MIN_TM 4 -param set-default VT_TYPE 2 - - -param set-default CA_AIRFRAME 2 -param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR4_AX 1 -param set-default CA_ROTOR4_AZ 0 - -# Square quadrotor X PX4 numbering -param set-default CA_ROTOR0_PX 1 -param set-default CA_ROTOR0_PY 1 -param set-default CA_ROTOR1_PX -1 -param set-default CA_ROTOR1_PY -1 -param set-default CA_ROTOR2_PX 1 -param set-default CA_ROTOR2_PY -1 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -1 -param set-default CA_ROTOR3_PY 1 -param set-default CA_ROTOR3_KM -0.05 - -param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TYPE 15 -param set-default CA_SV_CS0_TRQ_R 1 -param set-default CA_SV_CS1_TRQ_P 0.5 -param set-default CA_SV_CS1_TRQ_R 0 -param set-default CA_SV_CS1_TRQ_Y -0.5 -param set-default CA_SV_CS1_TYPE 13 -param set-default CA_SV_CS2_TRQ_P 0.5 -param set-default CA_SV_CS2_TRQ_Y 0.5 -param set-default CA_SV_CS2_TYPE 14 - -param set-default PWM_MAIN_FUNC1 201 -param set-default PWM_MAIN_FUNC2 202 -param set-default PWM_MAIN_FUNC3 105 -param set-default PWM_MAIN_FUNC4 203 -param set-default PWM_MAIN_FUNC5 101 -param set-default PWM_MAIN_FUNC6 102 -param set-default PWM_MAIN_FUNC7 103 -param set-default PWM_MAIN_FUNC8 104 - -param set-default PWM_MAIN_TIM0 50 -param set-default PWM_MAIN_DIS1 1500 -param set-default PWM_MAIN_DIS2 1500 -param set-default PWM_MAIN_DIS4 1500 diff --git a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter index de21a88216..695604e0b1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter +++ b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter @@ -11,7 +11,6 @@ . ${R}etc/init.d/rc.heli_defaults - # Disable PID gains for initial setup. These should be enabled after setting the FF gain. # P is expected to be lower than FF. param set-default MC_ROLLRATE_P 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 index 0ed3b201c8..7f85a770e1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 +++ b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 @@ -22,7 +22,6 @@ . ${R}etc/init.d/rc.fw_defaults - param set-default BAT1_CAPACITY 2500 param set-default BAT1_N_CELLS 3 @@ -41,7 +40,6 @@ param set-default FW_P_LIM_MAX 25 param set-default FW_P_LIM_MIN -5 param set-default FW_P_RMAX_NEG 20 - param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 index 0fced30c8e..1907b33a32 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 +++ b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 @@ -20,7 +20,6 @@ . ${R}etc/init.d/rc.fw_defaults - param set-default BAT1_CAPACITY 3300 param set-default BAT1_N_CELLS 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox index f894d47d74..6f75f86198 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox @@ -19,7 +19,6 @@ param set-default NAV_ACC_RAD 2 param set-default RTL_DESCEND_ALT 10 param set-default RTL_RETURN_ALT 30 - param set-default CA_ROTOR_COUNT 12 # Bottom motors param set-default CA_ROTOR0_PX 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index 090415e5ec..9b86703c31 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters @@ -67,7 +66,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index a73c357437..e63a9eef01 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters @@ -68,7 +67,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 index d460138cc8..129eacbffd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 @@ -21,7 +21,6 @@ param set-default MC_PITCHRATE_P 0.08 param set-default MC_PITCHRATE_D 0.001 param set-default MC_YAW_P 4 - param set-default MC_ROLLRATE_MAX 1600 param set-default MC_PITCHRATE_MAX 1600 param set-default MC_YAWRATE_MAX 1000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index e5e5e08343..fe40b95aa0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -38,7 +38,6 @@ param set-default COM_FLTMODE5 -1 param set-default COM_FLTMODE6 1 param set-default COM_RC_LOSS_T 3 - # ekf2 param set-default EKF2_BARO_NOISE 2 @@ -79,19 +78,16 @@ param set-default EKF2_RNG_POS_Z 0.033 param set-default EKF2_TERR_NOISE 1 - # Maximum allowed angle velocity in the landed state param set-default LNDMC_ROT_MAX 40 # Maximum vertical velocity allowed in the landed state param set-default LNDMC_Z_VEL_MAX 0.7 - # filtering param set-default IMU_DGYRO_CUTOFF 50 param set-default IMU_GYRO_CUTOFF 65 - # Pitch angle & rate setting param set-default MC_PITCHRATE_P 0.075 param set-default MC_PITCHRATE_I 0.1 @@ -148,7 +144,6 @@ param set-default RC_MAP_RETURN_SW 7 param set-default RC1_TRIM 1000 - # optical flow param set-default SENS_FLOW_MAXR 7.4 param set-default SENS_FLOW_MINHGT 0.15 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index 8cfc14ae1e..141ca861c4 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -30,7 +30,6 @@ param set-default IMU_DGYRO_CUTOFF 90 param set-default IMU_GYRO_CUTOFF 100 # System - param set-default SENS_BOARD_ROT 10 # EKF2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 b/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 index d599af5b38..38ffa06896 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 @@ -42,12 +42,16 @@ param set-default CA_ROTOR3_PX -0.0762 param set-default CA_ROTOR3_PY 0.09525 param set-default CA_ROTOR3_KM -0.05 +param set-default PWM_MAIN_TIM0 -3 +param set-default DSHOT_TEL_CFG 104 + param set-default EKF2_MIN_RNG 0.01 param set-default EKF2_OF_POS_X 0.043 param set-default EKF2_OF_POS_Y 0.011 param set-default EKF2_OF_QMIN_GND 1 param set-default EKF2_RNG_POS_X 0.043 param set-default EKF2_RNG_POS_Y 0.0 +param set-default EKF2_RNG_A_HMAX 25.0 param set-default IMU_GYRO_DNF_EN 2 @@ -78,3 +82,9 @@ param set-default SENS_FLOW_MINHGT 0.0 param set-default SER_TEL2_BAUD 3000000 param set-default UXRCE_DDS_CFG 102 + +param set-default EKF2_HGT_REF 2 + +# I saw a very sketchy extreme yaw setpoint that I cant explain +# https://review.px4.io/plot_app?log=4b69399f-b001-4dc2-acd5-e779fe266799 +param set-default MC_YAWRATE_MAX 100 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index 4833d17af2..85b045e84a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -18,7 +18,6 @@ # . ${R}etc/init.d/rc.mc_defaults - param set-default SYS_HAS_MAG 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_GPS_CTRL 0 @@ -85,6 +84,5 @@ param set-default PWM_MAIN_MAX3 255 param set-default SENS_FLOW_MINRNG 0.05 - syslink start mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 901127f09b..45b35ffeaa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -21,26 +21,37 @@ param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 param set-default RBCLW_REV 1 # reverse right wheels -# Rover parameters -param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAN_YAW_SCALE 1 -param set-default RD_MAX_ACCEL 5 -param set-default RD_MAX_JERK 10 -param set-default RD_MAX_THR_YAW_R 4 -param set-default RD_YAW_RATE_P 0.1 -param set-default RD_YAW_RATE_I 0 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0 -param set-default RD_MAX_SPEED 1.8 -param set-default RD_MAX_THR_SPD 2 -param set-default RD_SPEED_P 0.5 -param set-default RD_SPEED_I 0.1 -param set-default RD_MAX_YAW_RATE 300 -param set-default RD_MISS_SPD_DEF 1.8 -param set-default RD_TRANS_DRV_TRN 0.349066 -param set-default RD_TRANS_TRN_DRV 0.174533 -# Pure pursuit parameters +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters +param set-default RD_WHEEL_TRACK 0.3 +param set-default RD_MAX_THR_YAW_R 0.7 +param set-default RD_TRANS_DRV_TRN 0.785398 +param set-default RD_TRANS_TRN_DRV 0.139626 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 4 +param set-default RO_JERK_LIM 5 +param set-default RO_MAX_THR_SPEED 1.9 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 250 +param set-default RO_YAW_ACCEL_LIM 600 +param set-default RO_YAW_DECEL_LIM 600 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 1.6 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho index 6f50c7f4a1..6bd7bc3e2b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -14,22 +14,33 @@ . ${R}etc/init.d/rc.rover_ackermann_defaults param set-default BAT1_N_CELLS 3 - -# Rover parameters param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_MAX_ACCEL 0.5 -param set-default RA_MAX_JERK 10 -param set-default RA_MAX_SPEED 2.7 param set-default RA_MAX_STR_ANG 0.5236 -param set-default RA_MAX_STR_RATE 270 -param set-default RA_MISS_VEL_DEF 2.7 -param set-default RA_MISS_VEL_GAIN 3.5 -param set-default RA_MISS_VEL_MIN 1 -param set-default RA_SPEED_I 0.1 -param set-default RA_SPEED_P 0.5 -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_STR_RATE_LIM 270 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 1.5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 20 +param set-default RO_MAX_THR_SPEED 2.8 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0 +param set-default RO_YAW_RATE_P 0 +param set-default RO_YAW_RATE_LIM 0 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 0 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2.5 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 # Pure pursuit parameters param set-default PP_LOOKAHD_GAIN 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle index a773c1158d..078ea7859d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle @@ -15,7 +15,6 @@ . ${R}etc/init.d/rc.rover_defaults - param set-default BAT1_N_CELLS 2 param set-default EKF2_ANGERR_INIT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy index 06a2d3fb2f..1449a6d3ad 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy @@ -22,7 +22,6 @@ . ${R}etc/init.d/rc.uuv_defaults - param set-default MAV_1_CONFIG 102 param set-default BAT1_A_PER_V 37.8798 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d new file mode 100644 index 0000000000..c0f55737f5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d @@ -0,0 +1,150 @@ +#!/bin/sh +# +# @name KTH Space Robot +# +# @type Space Robot +# @class 2D Space Robot +# +# @maintainer DISCOWER +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 99 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 0 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# Set Mocap Vision frame +param set EKF2_EV_CTRL 15 +param set EKF2_HGT_REF 3 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.4 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.4 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.4 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.4 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.4 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.4 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.4 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.4 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + + +param set-default PWM_AUX_TIM0 10 +param set-default PWM_AUX_TIM1 10 +param set-default PWM_AUX_TIM2 10 + +param set-default PWM_AUX_FUNC1 101 +param set-default PWM_AUX_FUNC2 102 +param set-default PWM_AUX_FUNC3 103 +param set-default PWM_AUX_FUNC4 104 +param set-default PWM_AUX_FUNC5 105 +param set-default PWM_AUX_FUNC6 106 +param set-default PWM_AUX_FUNC7 107 +param set-default PWM_AUX_FUNC8 108 + +param set-default PWM_AUX_DIS1 0 +param set-default PWM_AUX_DIS2 0 +param set-default PWM_AUX_DIS3 0 +param set-default PWM_AUX_DIS4 0 +param set-default PWM_AUX_DIS5 0 +param set-default PWM_AUX_DIS6 0 +param set-default PWM_AUX_DIS7 0 +param set-default PWM_AUX_DIS8 0 + +param set-default PWM_AUX_MIN1 0 +param set-default PWM_AUX_MIN2 0 +param set-default PWM_AUX_MIN3 0 +param set-default PWM_AUX_MIN4 0 +param set-default PWM_AUX_MIN5 0 +param set-default PWM_AUX_MIN6 0 +param set-default PWM_AUX_MIN7 0 +param set-default PWM_AUX_MIN8 0 + +# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec +param set-default PWM_AUX_MAX1 10000 +param set-default PWM_AUX_MAX2 10000 +param set-default PWM_AUX_MAX3 10000 +param set-default PWM_AUX_MAX4 10000 +param set-default PWM_AUX_MAX5 10000 +param set-default PWM_AUX_MAX6 10000 +param set-default PWM_AUX_MAX7 10000 +param set-default PWM_AUX_MAX8 10000 + +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f8823bf657..d9142ec1c5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) 1100_rc_quad_x_sih.hil 1101_rc_plane_sih.hil 1102_tailsitter_duo_sih.hil + 1103_standard_vtol_sih.hil ) endif() @@ -129,8 +130,6 @@ if(CONFIG_MODULES_VTOL_ATT_CONTROL) # [13000, 13999] VTOL 13000_generic_vtol_standard 13100_generic_vtol_tiltrotor - 13013_deltaquad - 13014_vtol_babyshark 13030_generic_vtol_quad_tiltrotor 13200_generic_vtol_tailsitter ) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 32991e59b9..e6271d8144 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -13,8 +13,6 @@ param set-default MAV_TYPE 1 # # Default parameters for fixed wing UAVs. # -param set-default COM_POS_FS_DELAY 5 - # there is a 2.5 factor applied on the _FS thresholds if for invalidation param set-default COM_POS_FS_EPH 50 param set-default COM_VEL_FS_EVH 3 @@ -26,7 +24,6 @@ param set-default COM_DISARM_PRFLT -1 param set-default EKF2_ARSP_THR 8 param set-default EKF2_FUSE_BETA 1 -param set-default EKF2_GPS_CHECK 21 param set-default EKF2_MAG_ACCLIM 0 param set-default EKF2_REQ_EPH 10 param set-default EKF2_REQ_EPV 10 diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca268..93e6f0d33f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -12,6 +12,7 @@ param set-default MAV_TYPE 4 param set-default COM_PREARM_MODE 2 param set-default COM_SPOOLUP_TIME 10 +param set-default COM_DISARM_PRFLT 60 # No need for minimum collective pitch (or airmode) to keep torque authority param set-default MPC_MANTHR_MIN 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 7b2b9b095d..db11cedbb3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -21,3 +21,6 @@ param set-default RTL_RETURN_ALT 30 param set-default RTL_DESCEND_ALT 10 param set-default GPS_UBX_DYNMODEL 6 + +# lower RNG_FOG since MC are expected to fly closer over obstacles +param set-default EKF2_RNG_FOG 1.0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps new file mode 100644 index 0000000000..2df68601ac --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -0,0 +1,36 @@ +#!/bin/sh +# +# Standard apps for sr. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +# Start Spacecraft App +spacecraft start + +# Estimator Group Selection +# ekf2 start & + +# Start MicroDDS Client +# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# uxrce_dds_client start -t udp -p 8888 + +# +# Start Control Allocator +# +# sc_control_allocator start + +# +# Start Spacecraft Rate Controller. +# +# sc_rate_control start + +# +# Start Spacecraft Attitude Controller. +# +# sc_att_control start + +# +# Start Spacecraft Position Controller. +# +# sc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults new file mode 100644 index 0000000000..8dc8867b22 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -0,0 +1,27 @@ +#!/bin/sh +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE sc + +# MAV_TYPE_QUADROTOR 2 +#param set-default MAV_TYPE 12 + +# Set micro-dds-client to use ethernet and IP-address 192.168.0.1 +param set-default UXRCE_DDS_AG_IP -1062731775 + +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 +param set-default CBRK_SUPPLY_CHK 894281 +param set-default COM_ARM_HFLT_CHK 0 + +#Missing params +param set-default CP_DIST -1.0 + +# Default to MoCap fusion +param set-default ATT_EXT_HDG_M 2 +param set-default EKF2_EV_CTRL 15 +param set-default EKF2_EV_DELAY 5 +param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 3 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index b85f36e09d..7f479a3b1f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -211,6 +211,13 @@ then spl06 -X -a 0x77 start fi +# SPA06 sensor external I2C +if param compare -s SENS_EN_SPA06 1 +then + spa06 -X start + spa06 -X -a 0x77 start +fi + # PCF8583 counter (RPM sensor) if param compare -s SENS_EN_PCF8583 1 then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b8a8828c94..b9da86b330 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -301,6 +301,75 @@ else . $FCONFIG fi + + # + # Sensors System (start before Commander so Preflight checks are properly run). + # + if param greater SYS_HITL 0 + then + sensors start -h + + # disable GPS + param set GPS_1_CONFIG 0 + + # start the simulator in hardware if needed + if param compare SYS_HITL 2 + then + simulator_sih start + sensor_baro_sim start + sensor_mag_sim start + sensor_gps_sim start + sensor_agp_sim start + fi + + else + # + # board sensors: rc.sensors + # + set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors + if [ -f $BOARD_RC_SENSORS ] + then + echo "Board sensors: ${BOARD_RC_SENSORS}" + . $BOARD_RC_SENSORS + fi + unset BOARD_RC_SENSORS + + . ${R}etc/init.d/rc.sensors + + if param compare -s BAT1_SOURCE 2 + then + esc_battery start + fi + + if ! param compare BAT1_SOURCE 1 + then + battery_status start + fi + + sensors start + fi + + # + # state estimator selection + # + if param compare -s EKF2_EN 1 + then + ekf2 start & + fi + + if param compare -s LPE_EN 1 + then + local_position_estimator start + fi + + if param compare -s ATT_EN 1 + then + attitude_estimator_q start + fi + + # + # px4io + # if px4io supported then # Check if PX4IO present and update firmware if needed. @@ -353,6 +422,11 @@ else then pps_capture start fi + # RPM capture driver + if param greater -s RPM_CAP_ENABLE 0 + then + rpm_capture start + fi # Camera capture driver if param greater -s CAM_CAP_FBACK 0 then @@ -363,78 +437,24 @@ else fi # - # Sensors System (start before Commander so Preflight checks are properly run). - # Commander needs to be this early for in-air-restarts. + # Commander # if param greater SYS_HITL 0 then + commander start -h + if ! pwm_out_sim start -m hil then tune_control play error fi - sensors start -h - commander start -h - # disable GPS - param set GPS_1_CONFIG 0 - - # start the simulator in hardware if needed - if param compare SYS_HITL 2 - then - simulator_sih start - sensor_baro_sim start - sensor_mag_sim start - sensor_gps_sim start - fi - else - # - # board sensors: rc.sensors - # - set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors - if [ -f $BOARD_RC_SENSORS ] - then - echo "Board sensors: ${BOARD_RC_SENSORS}" - . $BOARD_RC_SENSORS - fi - unset BOARD_RC_SENSORS - - . ${R}etc/init.d/rc.sensors - - if param compare -s BAT1_SOURCE 2 - then - esc_battery start - fi - - if ! param compare BAT1_SOURCE 1 - then - battery_status start - fi - - sensors start commander start dshot start pwm_out start fi - # - # state estimator selection - if param compare -s EKF2_EN 1 - then - ekf2 start & - fi - - if param compare -s LPE_EN 1 - then - local_position_estimator start - fi - - if param compare -s ATT_EN 1 - then - attitude_estimator_q start - fi - # # Configure vehicle type specific parameters. # Note: rc.vehicle_setup is the entry point for all vehicle type specific setup. @@ -531,6 +551,11 @@ else payload_deliverer start fi + if param compare -s ICE_EN 1 + then + internal_combustion_engine_control start + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py index 3e79c70227..0d9c5f5d1b 100755 --- a/Tools/HIL/run_tests.py +++ b/Tools/HIL/run_tests.py @@ -136,9 +136,6 @@ class TestHardwareMethods(unittest.TestCase): def test_atomic_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "atomic_bitset")) - def test_bezier(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bezier")) - def test_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bitset")) diff --git a/Tools/astyle/check_code_style_all.sh b/Tools/astyle/check_code_style_all.sh index 5ace9ea52b..c4f4669003 100755 --- a/Tools/astyle/check_code_style_all.sh +++ b/Tools/astyle/check_code_style_all.sh @@ -43,7 +43,7 @@ fi # install git pre-commit hook HOOK_FILE="$DIR/../../.git/hooks/pre-commit" -if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then +if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then echo "" echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m" echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]" diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index c94132867e..266760d7af 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -8,9 +8,11 @@ if [ $# -gt 0 ]; then fi exec find boards msg src platforms test \ + -path msg/translation_node -prune -o \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/ins/vectornav/libvnc -prune -o \ + -path src/drivers/uavcan/libdronecan -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ -path src/drivers/cyphal/libcanard -prune -o \ @@ -32,4 +34,5 @@ exec find boards msg src platforms test \ -path src/lib/cdrstream/rosidl -prune -o \ -path src/modules/zenoh/zenoh-pico -prune -o \ -path boards/modalai/voxl2/libfc-sensor-api -prune -o \ + -path src/drivers/actuators/vertiq_io/iq-module-communication-cpp -prune -o \ \( -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) -print \) | grep $PATTERN diff --git a/Tools/ci_build_all_runner.sh b/Tools/ci/build_all_runner.sh similarity index 100% rename from Tools/ci_build_all_runner.sh rename to Tools/ci/build_all_runner.sh diff --git a/Tools/ci/check_msg_versioning.sh b/Tools/ci/check_msg_versioning.sh new file mode 100755 index 0000000000..37c593d277 --- /dev/null +++ b/Tools/ci/check_msg_versioning.sh @@ -0,0 +1,76 @@ +#! /bin/bash +# Copy a git diff between two commits if msg versioning is added + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +PX4_SRC_DIR="$DIR/.." + +BASE_COMMIT="$1" +HEAD_COMMIT="$2" +if [ -z "${BASE_COMMIT}" ] || [ -z "${HEAD_COMMIT}" ] +then + echo "Usage: $0 " + exit 1 +fi + +failed=0 + +# Iterate git diff files between BASE_COMMIT and HEAD_COMMIT +modified_files="$(git --no-pager diff --no-color --name-only --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}")" +all_files="$( git --no-pager diff --no-color --name-only "${BASE_COMMIT}"..."${HEAD_COMMIT}")" +for file in ${modified_files} +do + if [[ "$file" == msg/versioned/*.msg ]]; then + echo "Modified msg file: ${file}" + # A modified versioned .msg file requires: + # - Incrementing the version + # - An old .msg version exists + # - A translation header exists and is included + + # Ignore changes to comments or constants + content_a=$(git show "${BASE_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + content_b=$(git show "${HEAD_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + if [ "${content_a}" == "${content_b}" ]; then + echo "No version update required for ${file}" + continue + fi + + diff=$(git --no-pager diff --no-color --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}" -- "${file}") + old_version=$(echo "${diff}" | sed -n 's/^-uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') + new_version=$(echo "${diff}" | sed -n 's/^+uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') + + # Check that the version is incremented + if [ -z "${new_version}" ] || [ -z "${old_version}" ]; then + echo "ERROR: Missing version update for ${file}" + failed=1 + else + if [ $((new_version-old_version)) -ne 1 ]; then + echo "ERROR: Version not incremented by +1 for ${file}" + failed=1 + fi + fi + + # Check that an old version exists + filename=$(basename -- "$file") + filename="${filename%.*}" + old_version_file="px4_msgs_old/msg/${filename}V${old_version}.msg" + if [[ "${all_files}" != *"${old_version_file}"* ]]; then + echo "ERROR: Old message version does not exist for ${file} (missing ${old_version_file})" + failed=1 + fi + + # Check that a translation header got added by checking for a modification to all_translations.h + # If it is changed, we assume a new header got added too, so we don't explicitly check for that + if [[ "${modified_files}" != *"translations/all_translations.h"* ]]; then + echo "ERROR: missing modification to translations/all_translations.h" + failed=1 + fi + fi +done + +if [ ${failed} -ne 0 ]; then + echo "" + echo "ERROR: missing message versioning due to changed .msg file(s) (see above)" + echo "Check the documentation under https://docs.px4.io/main/en/ros2/px4_ros2_msg_translation_node.html for how to add a new version" + exit 1 +fi diff --git a/Tools/generate_board_targets_json.py b/Tools/ci/generate_board_targets_json.py similarity index 63% rename from Tools/generate_board_targets_json.py rename to Tools/ci/generate_board_targets_json.py index 649b3fceef..218862cecd 100755 --- a/Tools/generate_board_targets_json.py +++ b/Tools/ci/generate_board_targets_json.py @@ -25,10 +25,16 @@ parser.add_argument('-p', '--pretty', dest='pretty', action='store_true', help='Pretty output instead of a single line') parser.add_argument('-g', '--groups', dest='group', action='store_true', help='Groups targets') +parser.add_argument('-f', '--filter', dest='filter', help='comma separated list of board names to use instead of all') args = parser.parse_args() verbose = args.verbose +board_filter = [] +if args.filter: + for board in args.filter.split(','): + board_filter.append(board) + build_configs = [] grouped_targets = {} excluded_boards = ['modalai_voxl2', 'px4_ros2'] # TODO: fix and enable @@ -67,7 +73,7 @@ def process_target(px4board_file, target_name): px4board_file.endswith("bootloader.px4board"): kconf.load_config(px4board_file, replace=True) else: # Merge config with default.px4board - default_kconfig = re.sub(r'[a-zA-Z\d_]+\.px4board', 'default.px4board', px4board_file) + default_kconfig = re.sub(r'[a-zA-Z\d_-]+\.px4board', 'default.px4board', px4board_file) kconf.load_config(default_kconfig, replace=True) kconf.load_config(px4board_file, replace=False) @@ -111,7 +117,19 @@ if(verbose): print("= scanning for boards =") print("=======================") -for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): +# We also need to build metadata +# includes: +# - Airframe +# - Parameters +# - Events +metadata_targets = ['airframe_metadata', 'parameters_metadata', 'extract_events'] +grouped_targets['base'] = {} +grouped_targets['base']['container'] = 'px4io/px4-dev-base-focal:2021-09-08' +grouped_targets['base']['manufacturers'] = {} +grouped_targets['base']['manufacturers']['px4'] = [] +grouped_targets['base']['manufacturers']['px4'] += metadata_targets + +for manufacturer in os.scandir(os.path.join(source_dir, '../boards')): if not manufacturer.is_dir(): continue if manufacturer.name in excluded_manufacturers: @@ -129,6 +147,10 @@ for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): label = files.name[:-9] target_name = manufacturer.name + '_' + board.name + '_' + label + if board_filter and not board_name in board_filter: + if verbose: print(f'excluding board {board_name} ({target_name})') + continue + if board_name in excluded_boards: if verbose: print(f'excluding board {board_name} ({target_name})') continue @@ -143,7 +165,6 @@ for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): grouped_targets[target['arch']]['container'] = target['container'] grouped_targets[target['arch']]['manufacturers'] = {} if(manufacturer.name not in grouped_targets[target['arch']]['manufacturers']): - grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = {} grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = [] grouped_targets[target['arch']]['manufacturers'][manufacturer.name].append(target_name) if target is not None: @@ -156,6 +177,11 @@ if(verbose): print("============================") pprint.pp(grouped_targets) +if(verbose): + print("===================") + print("= Generating JSON =") + print("===================") + if (args.group): # if we are using this script for grouping builds # we loop trough the manufacturers list and split their targets @@ -173,93 +199,88 @@ if (args.group): # nuttx-0 # nuttx-1 final_groups = [] - temp_group = [] - group_number = {} last_man = '' last_arch = '' SPLIT_LIMIT = 10 LOWER_LIMIT = 5 + if(verbose): + print(f'=:Architectures: [{grouped_targets.keys()}]') for arch in grouped_targets: - if(last_arch == ''): - last_arch = arch - if(arch not in group_number): - group_number[arch] = 0 - - if(last_arch != arch and len(temp_group) > 0): - group_name = last_arch + "-" + str(group_number[last_arch]) - group_number[last_arch] += 1 - targets = comma_targets(temp_group) - final_groups.append({ - "container": grouped_targets[last_arch]['container'], - "targets": targets, - "arch": last_arch, - "group": group_name, - "len": len(temp_group) - }) - last_arch = arch - temp_group = [] + if(verbose): + print(f'=:Processing: [{arch}]') + temp_group = [] for man in grouped_targets[arch]['manufacturers']: - for tar in grouped_targets[arch]['manufacturers'][man]: - if(last_man != man): - man_len = len(grouped_targets[arch]['manufacturers'][man]) - if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): - # Manufacturers can have their own group - group_name = arch + "-" + man - targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) - last_man = man - final_groups.append({ - "container": grouped_targets[arch]['container'], - "targets": targets, - "arch": arch, - "group": group_name, - "len": len(grouped_targets[arch]['manufacturers'][man]) - }) - elif(man_len >= (SPLIT_LIMIT + 1)): - # Split big man groups into subgroups - # example: Pixhawk - chunk_limit = SPLIT_LIMIT - chunk_counter = 0 - for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): - group_name = arch + "-" + man + "-" + str(chunk_counter) - targets = comma_targets(chunk) - last_man = man - final_groups.append({ - "container": grouped_targets[arch]['container'], - "targets": targets, - "arch": arch, - "group": group_name, - "len": len(chunk), - }) - chunk_counter += 1 - else: - temp_group.append(tar) - - if(last_arch != arch and len(temp_group) > 0): - group_name = last_arch + "-" + str(group_number[last_arch]) - group_number[last_arch] += 1 - targets = comma_targets(temp_group) - final_groups.append({ - "container": grouped_targets[last_arch]['container'], - "targets": targets, - "arch": last_arch, - "group": group_name, - "len": len(temp_group) - }) - last_arch = arch - temp_group = [] - if(len(temp_group) > (LOWER_LIMIT - 1)): - group_name = arch + "-" + str(group_number[arch]) - last_arch = arch - group_number[arch] += 1 - targets = comma_targets(temp_group) + if(verbose): + print(f'=:Processing: [{arch}][{man}]') + man_len = len(grouped_targets[arch]['manufacturers'][man]) + if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): + # Manufacturers can have their own group + if(verbose): + print(f'=:Processing: [{arch}][{man}][{man_len}]==Manufacturers can have their own group') + group_name = arch + "-" + man + targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) final_groups.append({ "container": grouped_targets[arch]['container'], "targets": targets, "arch": arch, "group": group_name, - "len": len(temp_group) + "len": len(grouped_targets[arch]['manufacturers'][man]) }) - temp_group = [] + elif(man_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk + if(verbose): + print(f'=:Processing: [{arch}][{man}][{man_len}]==Manufacturers has multiple own groups') + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): + group_name = arch + "-" + man + "-" + str(chunk_counter) + targets = comma_targets(chunk) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(chunk), + }) + chunk_counter += 1 + else: + if(verbose): + print(f'=:Processing: [{arch}][{man}][{man_len}]==Manufacturers too small group with others') + temp_group.extend(grouped_targets[arch]['manufacturers'][man]) + + temp_len = len(temp_group) + chunk_counter = 0 + if(temp_len > 0 and temp_len < (SPLIT_LIMIT + 1)): + if(verbose): + print(f'=:Processing: [{arch}][orphan][{temp_len}]==Leftover arch can have their own group') + group_name = arch + "-" + str(chunk_counter) + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": temp_len + }) + elif(temp_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk + if(verbose): + print(f'=:Processing: [{arch}][orphan][{temp_len}]==Leftover arch can has multpile group') + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(temp_group, chunk_limit): + group_name = arch + "-" + str(chunk_counter) + targets = comma_targets(chunk) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(chunk), + }) + chunk_counter += 1 if(verbose): import pprint print("================") diff --git a/Tools/ci/package_build_artifacts.sh b/Tools/ci/package_build_artifacts.sh new file mode 100755 index 0000000000..a665e295df --- /dev/null +++ b/Tools/ci/package_build_artifacts.sh @@ -0,0 +1,46 @@ +#!/bin/bash + +mkdir artifacts +cp **/**/*.px4 artifacts/ +cp **/**/*.elf artifacts/ +for build_dir_path in build/*/ ; do + build_dir=${build_dir_path#*/} + build_dir=${build_dir::${#build_dir}-1} + mkdir artifacts/$build_dir + find artifacts/ -maxdepth 1 -type f -name "*$build_dir*" + # Airframe + cp $build_dir_path/airframes.xml artifacts/$build_dir/ + # Parameters + cp $build_dir_path/parameters.xml artifacts/$build_dir/ + cp $build_dir_path/parameters.json artifacts/$build_dir/ + cp $build_dir_path/parameters.json.xz artifacts/$build_dir/ + # Actuators + cp $build_dir_path/actuators.json artifacts/$build_dir/ + cp $build_dir_path/actuators.json.xz artifacts/$build_dir/ + # Events + cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/ + # ROS 2 msgs + cp $build_dir_path/events/all_events.json.xz artifacts/$build_dir/ + # Module Docs + ls -la artifacts/$build_dir + echo "----------" +done + +# general metadata +mkdir artifacts/_general/ +cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/ +# Airframe +cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/ +# Parameters +cp artifacts/px4_sitl_default/parameters.xml artifacts/_general/ +cp artifacts/px4_sitl_default/parameters.json artifacts/_general/ +cp artifacts/px4_sitl_default/parameters.json.xz artifacts/_general/ +# Actuators +cp artifacts/px4_sitl_default/actuators.json artifacts/_general/ +cp artifacts/px4_sitl_default/actuators.json.xz artifacts/_general/ +# Events +cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/ +# ROS 2 msgs +cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/ +# Module Docs +ls -la artifacts/_general/ diff --git a/Tools/copy_to_ros_ws.sh b/Tools/copy_to_ros_ws.sh new file mode 100755 index 0000000000..ef663aaaa9 --- /dev/null +++ b/Tools/copy_to_ros_ws.sh @@ -0,0 +1,34 @@ +#! /bin/bash +# Copy msgs and the message translation node into a ROS workspace directory + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +PX4_SRC_DIR="$DIR/.." + +WS_DIR="$1" +if [ ! -e "${WS_DIR}" ] +then + echo "Usage: $0 " + exit 1 +fi +WS_DIR="$WS_DIR"/src +if [ ! -e "${WS_DIR}" ] +then + echo "'src' directory not found inside ROS workspace (${WS_DIR})" + exit 1 +fi + +cp -ar "${PX4_SRC_DIR}"/msg/translation_node "${WS_DIR}" +cp -ar "${PX4_SRC_DIR}"/msg/px4_msgs_old "${WS_DIR}" +PX4_MSGS_DIR="${WS_DIR}"/px4_msgs +if [ ! -e "${PX4_MSGS_DIR}" ] +then + git clone https://github.com/PX4/px4_msgs.git "${PX4_MSGS_DIR}" + rm -rf "${PX4_MSGS_DIR}"/msg/*.msg + rm -rf "${PX4_MSGS_DIR}"/msg/versioned/*.msg + rm -rf "${PX4_MSGS_DIR}"/srv/*.srv +fi +cp -ar "${PX4_SRC_DIR}"/msg/*.msg "${PX4_MSGS_DIR}"/msg +mkdir -p "${PX4_MSGS_DIR}"/msg/versioned +cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg/versioned +cp -ar "${PX4_SRC_DIR}"/srv/*.srv "${PX4_MSGS_DIR}"/srv diff --git a/Tools/decrypt_ulog.py b/Tools/decrypt_ulog.py index f6d888c56b..3015686b97 100755 --- a/Tools/decrypt_ulog.py +++ b/Tools/decrypt_ulog.py @@ -1,62 +1,91 @@ #!/usr/bin/env python3 +import sys + +try: + from Crypto.Cipher import ChaCha20 +except ImportError as e: + print("Failed to import crypto: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pycryptodome") + print("") + sys.exit(1) + from Crypto.PublicKey import RSA from Crypto.Cipher import PKCS1_OAEP -from Crypto.Cipher import ChaCha20 from Crypto.Hash import SHA256 -import binascii +from pathlib import Path import argparse -#from pathlib import Path -import sys + if __name__ == "__main__": parser = argparse.ArgumentParser(description="""CLI tool to decrypt an ulog file\n""") - parser.add_argument("ulog_file", help=".ulog file", nargs='?', default=None) - parser.add_argument("ulog_key", help=".ulogk, encrypted key", nargs='?', default=None) + parser.add_argument("ulog_file", help=".ulge/.ulgc, encrypted log file", nargs='?', default=None) + parser.add_argument("ulog_key", help=".ulgk, legacy encrypted key (give empty string '' to ignore for .ulge)", nargs='?', default=None) parser.add_argument("rsa_key", help=".pem format key for decrypting the ulog key", nargs='?', default=None) args = parser.parse_args() - # Only generate a key pair, don't sign - if not args.ulog_file or not args.ulog_key or not args.rsa_key: - print('Need all arguments, the encrypted ulog file, the key and the key decryption key') - sys.exit(1); + # Check all arguments are given + if not args.rsa_key: + print('Need all arguments, the encrypted ulog file, key file (or empty string if not needed) and the key decryption key (.pem)') + sys.exit(1) # Read the private RSA key to decrypt the cahcha key with open(args.rsa_key, 'rb') as f: r = RSA.importKey(f.read(), passphrase='') - # Read the encrypted xchacha key and the nonce - with open(args.ulog_key, 'rb') as f: + if args.ulog_key == "": + key_data_filename = args.ulog_file + magic = "ULogEnc" + else: + key_data_filename = args.ulog_key + magic = "ULogKey" + + with open(key_data_filename, 'rb') as f: + # Read the encrypted xchacha key and the nonce ulog_key_header = f.read(22) # Parse the header try: # magic - if not ulog_key_header.startswith(bytearray("ULogKey".encode())): + if not ulog_key_header.startswith(bytearray(magic.encode())): + print("Incorrect header magic") raise Exception() # version if ulog_key_header[7] != 1: + print("Unsupported header version") raise Exception() # expected key exchange algorithm (RSA_OAEP) if ulog_key_header[16] != 4: + print("Unsupported key algorithm") raise Exception() - key_size = ulog_key_header[19] << 8 | ulog_key_header[18]; - nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20]; + key_size = ulog_key_header[19] << 8 | ulog_key_header[18] + nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20] ulog_key_cipher = f.read(key_size) nonce = f.read(nonce_size) except: - print("Keyfile format error") - sys.exit(1); + print("Keydata format error") + sys.exit(1) + + if magic == "ULogEnc": + data_offset = 22 + key_size + nonce_size + else: + data_offset = 0 # Decrypt the xchacha key cipher_rsa = PKCS1_OAEP.new(r,SHA256) ulog_key = cipher_rsa.decrypt(ulog_key_cipher) #print(binascii.hexlify(ulog_key)) - # Read and decrypt the .ulgc + # Read and decrypt the ulog data cipher = ChaCha20.new(key=ulog_key, nonce=nonce) + + outfilename = Path(args.ulog_file).stem + ".ulog" with open(args.ulog_file, 'rb') as f: - with open(args.ulog_file.rstrip(args.ulog_file[-1]), 'wb') as out: + if data_offset > 0: + f.seek(data_offset) + with open(outfilename, 'wb') as out: out.write(cipher.decrypt(f.read())) diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index c78b9cc7e1..46888cd066 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -47,6 +47,7 @@ CCACHE_DIR=${HOME}/.ccache mkdir -p "${CCACHE_DIR}" docker run -it --rm -w "${SRC_DIR}" \ + --user="$(id -u):$(id -g)" \ --env=AWS_ACCESS_KEY_ID \ --env=AWS_SECRET_ACCESS_KEY \ --env=BRANCH_NAME \ @@ -54,7 +55,6 @@ docker run -it --rm -w "${SRC_DIR}" \ --env=CI \ --env=CODECOV_TOKEN \ --env=COVERALLS_REPO_TOKEN \ - --env=LOCAL_USER_ID="$(id -u)" \ --env=PX4_ASAN \ --env=PX4_MSAN \ --env=PX4_TSAN \ diff --git a/Tools/kconfig/allyesconfig.py b/Tools/kconfig/allyesconfig.py index 889b659195..2c0f58d75a 100644 --- a/Tools/kconfig/allyesconfig.py +++ b/Tools/kconfig/allyesconfig.py @@ -40,6 +40,7 @@ exception_list = [ 'DRIVERS_DISTANCE_SENSOR_SRF05', # Requires hardcoded GPIO_ULTRASOUND 'DRIVERS_PPS_CAPTURE', # Requires PPS GPIO config 'DRIVERS_PWM_INPUT', # Requires PWM config + 'DRIVERS_RPM_CAPTURE', # Requires PPS GPIO config 'DRIVERS_TEST_PPM', # PIN config not portable 'DRIVERS_TATTU_CAN', # Broken needs fixing 'MODULES_REPLAY', # Fails on NuttX targets maybe force POSIX dependency? @@ -52,6 +53,7 @@ exception_list = [ 'SYSTEMCMDS_I2C_LAUNCHER', # undefined reference to `system', 'MODULES_MUORB_APPS', # Weird QURT/Posix package doesn't work on x86 px4 sitl 'MODULES_SIMULATION_SIMULATOR_SIH', # Causes compile errors + 'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code) ] exception_list_sitl = [ @@ -72,6 +74,7 @@ exception_list_sitl = [ 'SYSTEMCMDS_I2CDETECT', # Not supported in SITL 'SYSTEMCMDS_DMESG', # Not supported in SITL 'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL + 'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code) ] def main(): diff --git a/Tools/msg/generate_msg_docs.py b/Tools/msg/generate_msg_docs.py index a44a317b52..193c56431f 100755 --- a/Tools/msg/generate_msg_docs.py +++ b/Tools/msg/generate_msg_docs.py @@ -11,9 +11,22 @@ import sys def get_msgs_list(msgdir): """ - Makes list of msg files in the given directory + Makes a list of relative paths of .msg files in the given directory + and its subdirectories. + + Parameters: + msgdir (str): The directory to search for .msg files. + + Returns: + list: A list of relative paths to .msg files. """ - return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] + msgs = [] + for root, _, files in os.walk(msgdir): + for fn in files: + if fn.endswith(".msg"): + relative_path = os.path.relpath(os.path.join(root, fn), msgdir) + msgs.append(relative_path) + return msgs if __name__ == "__main__": @@ -29,10 +42,11 @@ if __name__ == "__main__": msg_files = get_msgs_list(msg_path) msg_files.sort() - filelist_in_markdown='' + versioned_msgs_list = '' + unversioned_msgs_list = '' for msg_file in msg_files: - msg_name = os.path.splitext(msg_file)[0] + msg_name = os.path.splitext(os.path.basename(msg_file))[0] output_file = os.path.join(output_dir, msg_name+'.md') msg_filename = os.path.join(msg_path, msg_file) print("{:} -> {:}".format(msg_filename, output_file)) @@ -81,10 +95,17 @@ if __name__ == "__main__": with open(output_file, 'w') as content_file: content_file.write(markdown_output) - index_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name) - if summary_description: - index_markdown_file_link+=" — %s" % summary_description - filelist_in_markdown+=index_markdown_file_link+"\n" + # Categorize as versioned or unversioned + if "versioned" in msg_file: + versioned_msgs_list += '- [%s](%s.md)' % (msg_name, msg_name) + if summary_description: + versioned_msgs_list += " — %s" % summary_description + versioned_msgs_list += "\n" + else: + unversioned_msgs_list += '- [%s](%s.md)' % (msg_name, msg_name) + if summary_description: + unversioned_msgs_list += " — %s" % summary_description + unversioned_msgs_list += "\n" # Write out the index.md file index_text="""# uORB Message Reference @@ -94,10 +115,20 @@ This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Too ::: This topic lists the UORB messages available in PX4 (some of which may be may be shared by the [PX4-ROS 2 Bridge](../ros/ros2_comm.md)). + +[Versioned messages](../middleware/uorb.md#message-versioning) track changes to their definitions, with each modification resulting in a version increment. +These messages are most likely shared through the PX4-ROS 2 Bridge. + Graphs showing how these are used [can be found here](../middleware/uorb_graph.md). +## Versioned Messages + %s - """ % (filelist_in_markdown) + +## Unversioned Messages + +%s + """ % (versioned_msgs_list, unversioned_msgs_list) index_file = os.path.join(output_dir, 'index.md') with open(index_file, 'w') as content_file: content_file.write(index_text) diff --git a/Tools/px4airframes/markdownout.py b/Tools/px4airframes/markdownout.py index bf1a605723..866aa1abef 100644 --- a/Tools/px4airframes/markdownout.py +++ b/Tools/px4airframes/markdownout.py @@ -137,7 +137,7 @@ div.frame_variant td, div.frame_variant th { #print(output_name,value, attribstrs[0].strip(),attribstrs[1].strip()) outputs += '' if has_outputs: - outputs_entry = '

Specific Outputs:' + outputs + '

' + outputs_entry = '
Specific Outputs:' + outputs else: outputs_entry = '' diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index 3eacbf4325..4937f67b7e 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/Tools/px4moduledoc/srcparser.py @@ -442,7 +442,7 @@ class SourceParser(object): re.findall(r"\bstrcmp\b.*\bverb\b.*\"(.+)\"", contents) doc_commands = module_doc.all_commands() + \ - [x for value in module_doc.all_values() for x in value.split('|')] + [x for value in module_doc.all_values() for x in value.replace(' ', '|').split('|')] for command in commands: if len(command) == 2 and command[0] == '-': diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 0ab3a6ff25..6d8c7c69dc 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -189,7 +189,7 @@ class uploader: GET_CHIP = b'\x2c' # rev5+ , get chip version SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII - GET_VERSION = b'\x2f' # rev5+ , get chip description in ASCII + GET_VERSION = b'\x2f' # rev5+ , get bootloader version in ASCII CHIP_FULL_ERASE = b'\x40' # full erase of flash, rev6+ MAX_DES_LENGTH = 20 @@ -201,7 +201,6 @@ class uploader: INFO_BOARD_ID = b'\x02' # board type INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes - BL_VERSION = b'\x07' # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars) PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 READ_MULTI_MAX = 252 # protocol max is 255 @@ -428,7 +427,8 @@ class uploader: percent = (float(progress) / float(maxVal)) * 100.0 - sys.stdout.write("\r%s: [%-20s] %.1f%%" % (label, '='*int(percent/5.0), percent)) + redraw = "\r" if sys.stdout.isatty() else "\n" + sys.stdout.write("%s%s: [%-20s] %.1f%%" % (redraw, label, '='*int(percent/5.0), percent)) sys.stdout.flush() # send the CHIP_ERASE command and wait for the bootloader to become ready @@ -708,7 +708,7 @@ class uploader: # https://github.com/PX4/Firmware/blob/master/src/drivers/boards/common/stm32/board_mcu_version.c#L125-L144 if self.fw_maxsize > fw.property('image_maxsize') and not force: - raise RuntimeError(f"Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes). Please use the correct board configuration to avoid lacking critical functionality.") + print(f"WARNING: Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes)") else: # If we're still on bootloader v4 on a Pixhawk, we don't know if we # have the silicon errata and therefore need to flash px4_fmu-v2 diff --git a/Tools/setup/Dockerfile b/Tools/setup/Dockerfile new file mode 100644 index 0000000000..0cf8762b9d --- /dev/null +++ b/Tools/setup/Dockerfile @@ -0,0 +1,31 @@ +# PX4 base development environment +FROM ubuntu:24.04 +LABEL maintainer="Daniel Agar , Ramon Roche " + +ENV DEBIAN_FRONTEND=noninteractive +ENV LANG=C.UTF-8 +ENV LC_ALL=C.UTF-8 +ENV DISPLAY=:99 +ENV TERM=xterm +ENV TZ=UTC + +# SITL UDP PORTS +EXPOSE 14556/udp +EXPOSE 14557/udp + +COPY docker-entrypoint.sh /usr/local/bin/docker-entrypoint.sh + +# Install PX4 Requirements +COPY requirements.txt /tmp/requirements.txt +COPY ubuntu.sh /tmp/ubuntu.sh +RUN touch /.dockerenv +RUN bash /tmp/ubuntu.sh --no-sim-tools + +RUN git config --global --add safe.directory '*' + +# create user with id 1001 (jenkins docker workflow default) +RUN useradd --shell /bin/bash -u 1001 -c "" -m user && usermod -a -G dialout user + +ENTRYPOINT ["/usr/local/bin/docker-entrypoint.sh"] + +CMD ["/bin/bash"] diff --git a/Tools/setup/docker-entrypoint.sh b/Tools/setup/docker-entrypoint.sh new file mode 100755 index 0000000000..0412558902 --- /dev/null +++ b/Tools/setup/docker-entrypoint.sh @@ -0,0 +1,19 @@ +#!/bin/bash + +# Start virtual X server in the background +# - DISPLAY default is :99, set in dockerfile +# - Users can override with `-e DISPLAY=` in `docker run` command to avoid +# running Xvfb and attach their screen +if [[ -x "$(command -v Xvfb)" && "$DISPLAY" == ":99" ]]; then + echo "[docker-entrypoint.sh] Starting Xvfb" + Xvfb :99 -screen 0 1600x1200x24+32 & +fi + +# Check if the ROS_DISTRO is passed and use it +# to source the ROS environment +if [ -n "${ROS_DISTRO}" ]; then + echo "[docker-entrypoint.sh] ROS: ${ROS_DISTRO}" + source "/opt/ros/$ROS_DISTRO/setup.bash" +fi + +exec "$@" diff --git a/Tools/setup/requirements.txt b/Tools/setup/requirements.txt index 0202963fd7..51108b7c71 100644 --- a/Tools/setup/requirements.txt +++ b/Tools/setup/requirements.txt @@ -1,7 +1,7 @@ argcomplete cerberus coverage -empy==3.3.4 +empy>=3.3,<4 future jinja2>=2.8 jsonschema diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 48e23709fb..e641a82da3 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -2,7 +2,7 @@ set -e -## Bash script to setup PX4 development environment on Ubuntu LTS (22.04, 20.04, 18.04). +## Bash script to setup PX4 development environment on Ubuntu LTS (24.04, 22.04). ## Can also be used in docker. ## ## Installs: @@ -33,7 +33,9 @@ if [ -f /.dockerenv ]; then apt-get --quiet -y update && DEBIAN_FRONTEND=noninteractive apt-get --quiet -y install \ ca-certificates \ gnupg \ - lsb-core \ + gosu \ + lsb-release \ + software-properties-common \ sudo \ wget \ ; @@ -53,23 +55,7 @@ fi # check ubuntu version # otherwise warn and point to docker? UBUNTU_RELEASE="`lsb_release -rs`" - -if [[ "${UBUNTU_RELEASE}" == "14.04" ]]; then - echo "Ubuntu 14.04 is no longer supported" - exit 1 -elif [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then - echo "Ubuntu 16.04 is no longer supported" - exit 1 -elif [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then - echo "Ubuntu 18.04" -elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then - echo "Ubuntu 20.04" -elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then - echo "Ubuntu 22.04" -elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then - echo "Linux Mint 21.3" -fi - +echo "Ubuntu ${UBUNTU_RELEASE}" echo echo "Installing PX4 general dependencies" @@ -78,6 +64,7 @@ sudo apt-get update -y --quiet sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ astyle \ build-essential \ + ccache \ cmake \ cppcheck \ file \ @@ -86,7 +73,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i gdb \ git \ lcov \ - libfuse2 \ + libssl-dev \ libxml2-dev \ libxml2-utils \ make \ @@ -105,12 +92,17 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i # Python3 dependencies echo echo "Installing PX4 Python3 dependencies" -if [ -n "$VIRTUAL_ENV" ]; then - # virtual environments don't allow --user option - python -m pip install -r ${DIR}/requirements.txt +PYTHON_VERSION=$(python3 --version 2>&1 | awk '{print $2}') +REQUIRED_VERSION="3.11" +if [[ "$(printf '%s\n' "$REQUIRED_VERSION" "$PYTHON_VERSION" | sort -V | head -n1)" == "$REQUIRED_VERSION" ]]; then + python3 -m pip install --break-system-packages -r ${DIR}/requirements.txt else - # older versions of Ubuntu require --user option - python3 -m pip install --user -r ${DIR}/requirements.txt + if [ -n "$VIRTUAL_ENV" ]; then + # virtual environments don't allow --user option + python -m pip install -r ${DIR}/requirements.txt + else + python3 -m pip install --user -r ${DIR}/requirements.txt + fi fi # NuttX toolchain (arm-none-eabi-gcc) @@ -124,23 +116,29 @@ if [[ $INSTALL_NUTTX == "true" ]]; then binutils-dev \ bison \ build-essential \ + curl \ flex \ g++-multilib \ + gcc-arm-none-eabi \ gcc-multilib \ gdb-multiarch \ genromfs \ gettext \ gperf \ + kconfig-frontends \ libelf-dev \ libexpat-dev \ libgmp-dev \ libisl-dev \ libmpc-dev \ libmpfr-dev \ - libncurses5 \ - libncurses5-dev \ - libncursesw5-dev \ + libncurses-dev \ + libncurses6 \ + libncursesw6 \ + libnewlib-arm-none-eabi \ + libstdc++-arm-none-eabi-newlib \ libtool \ + libunwind-dev \ pkg-config \ screen \ texinfo \ @@ -148,46 +146,11 @@ if [[ $INSTALL_NUTTX == "true" ]]; then util-linux \ vim-common \ ; - if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" || "${UBUNTU_RELEASE}" == "21.3" ]]; then - sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ - kconfig-frontends \ - ; - fi - if [ -n "$USER" ]; then # add user to dialout group (serial port access) sudo usermod -aG dialout $USER fi - - # arm-none-eabi-gcc - NUTTX_GCC_VERSION="9-2020-q2-update" - NUTTX_GCC_VERSION_SHORT="9-2020q2" - - source $HOME/.profile # load changed path for the case the script is reran before relogin - if [ $(which arm-none-eabi-gcc) ]; then - GCC_VER_STR=$(arm-none-eabi-gcc --version) - GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}" || true) - fi - - if [[ "$GCC_FOUND_VER" == "1" ]]; then - echo "arm-none-eabi-gcc-${NUTTX_GCC_VERSION} found, skipping installation" - - else - echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}"; - wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \ - sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/; - - # add arm-none-eabi-gcc to user's PATH - exportline="export PATH=/opt/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}/bin:\$PATH" - - if grep -Fxq "$exportline" $HOME/.profile; then - echo "${NUTTX_GCC_VERSION} path already set."; - else - echo $exportline >> $HOME/.profile; - source $HOME/.profile; # Allows to directly build NuttX targets in the same terminal - fi - fi fi # Simulation tools @@ -201,51 +164,8 @@ if [[ $INSTALL_SIM == "true" ]]; then bc \ ; - if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then - java_version=11 - elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then - java_version=13 - elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then - java_version=11 - elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then - java_version=11 - else - java_version=14 - fi - # Java (jmavsim) - sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ - ant \ - openjdk-$java_version-jre \ - openjdk-$java_version-jdk \ - libvecmath-java \ - ; - - # Set Java 11 as default - sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version") - # Gazebo / Gazebo classic installation - if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then - echo "Gazebo (Harmonic) will be installed" - echo "Earlier versions will be removed" - # Add Gazebo binary repository - sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null - sudo apt-get update -y --quiet - - # Install Gazebo - gazebo_packages="gz-harmonic" - elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then - echo "Gazebo (Garden) will be installed" - echo "Earlier versions will be removed" - # Add Gazebo binary repository - sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null - - sudo apt-get update -y --quiet - - # Install Gazebo - gazebo_packages="gz-garden" - else + if [[ "${UBUNTU_RELEASE}" == "18.04" || "${UBUNTU_RELEASE}" == "20.04" ]]; then sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - # Update list, since new gazebo-stable.list has been added @@ -260,6 +180,21 @@ if [[ $INSTALL_SIM == "true" ]]; then gazebo_classic_version=11 gazebo_packages="gazebo$gazebo_classic_version libgazebo$gazebo_classic_version-dev" fi + else + # Expects Ubuntu 22.04 > by default + echo "Gazebo (Harmonic) will be installed" + echo "Earlier versions will be removed" + # Add Gazebo binary repository + sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + sudo apt-get update -y --quiet + + # Install Gazebo + gazebo_packages="gz-harmonic libunwind-dev" + + if [[ "${UBUNTU_RELEASE}" == "24.04" ]]; then + gazebo_packages="$gazebo_packages cppzmq-dev" + fi fi sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ @@ -285,8 +220,3 @@ if [[ $INSTALL_SIM == "true" ]]; then fi fi - -if [[ $INSTALL_NUTTX == "true" ]]; then - echo - echo "Relogin or reboot computer before attempting to build NuttX targets" -fi diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index f1d11a6126..fbd8e9e6bb 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit f1d11a6126990d487d4aa8ff68c23ff370516510 +Subproject commit fbd8e9e6bbd2188de81677494f15885dead99c48 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 536305adee..db4af69088 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 +Subproject commit db4af69088cccef4549cf3a5c195d5cd97d6b36a diff --git a/Tools/teensy_uploader.py b/Tools/teensy_uploader.py new file mode 100644 index 0000000000..aad97135c3 --- /dev/null +++ b/Tools/teensy_uploader.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python3 +############################################################################ +# +# 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. +# +############################################################################ + +# +# Serial firmware uploader for the Teensy + +import sys +import argparse +import time +import sys +import usb.core +import subprocess + +from sys import platform as _platform +from pymavlink import mavutil + +try: + import serial +except ImportError as e: + print("Failed to import serial: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyserial") + print("") + sys.exit(1) + +# Define time to use time.time() by default +def _time(): + return time.time() + +class uploader(object): + + def __init__(self, portname): + self.mavlink = mavutil.mavlink_connection(portname) + + def send_reboot(self): + try: + self.mavlink.wait_heartbeat() + self.mavlink.reboot_autopilot(True) + except: + pass + return True + +TEENSY_BL_VENDORID = 0x16c0 +TEENSY_BL_PRODUCTID = 0x0478 + +def main(): + # Parse commandline arguments + parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") + parser.add_argument('--port', action="store", required=True, help="Comma-separated list of serial port(s) to which the FMU may be attached") + parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading') + parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') + parser.add_argument('--vendor-id', type=lambda x: int(x,0), default=None, help='PX4 USB vendorid') + parser.add_argument('--product-id', type=lambda x: int(x,0), default=None, help='PX4 USB productid') + parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)") + args = parser.parse_args() + + found_bootloader = False + + # find USB devices + dev = usb.core.find(idVendor=TEENSY_BL_VENDORID, idProduct=TEENSY_BL_PRODUCTID) + # loop through devices, printing vendor and product ids in decimal and hex + if dev is not None: + print("Found teensy bootloader") + found_bootloader = True + else: + dev = usb.core.find(idVendor=args.vendor_id, idProduct=args.product_id) + if dev is None: + print("No PX4 Device found try to press the button program push button") + print("Attempting to reboot into Teensy bootloader...", end="", flush=True) + + try: + while True: + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform or "cygwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if "COM" not in port and "tty.usb" not in port: + up = uploader(port) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if "COM" not in port and "ACM" not in port: + up = uploader(port) + elif "cygwin" in _platform: + # Cygwin, don't open native Windows COM and Linux ports + if "COM" not in port and "ACM" not in port: + up = uploader(port) + elif "win" in _platform: + # Windows, don't open POSIX ports + if "/" not in port: + up = uploader(port) + except Exception: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + while True: + up.send_reboot() + + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.25) + + # wait for the close, without we might run into Serial I/O Error 6 + time.sleep(0.3) + + dev = usb.core.find(idVendor=TEENSY_BL_VENDORID, idProduct=TEENSY_BL_PRODUCTID) + # loop through devices, printing vendor and product ids in decimal and hex + if dev is not None: + print("") + print("Found teensy bootloader") + found_bootloader = True + break + + print('.', end="", flush=True) + + if not found_bootloader: + # Go to the next port + continue + + if(found_bootloader): + while True: + result = subprocess.Popen("teensy_loader_cli -v --mcu=TEENSY41 " + args.firmware[0], shell=True) + text = result.communicate()[0] + if(result.returncode == 0): + sys.exit(0) + + # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU + time.sleep(0.05) + + # CTRL+C aborts the upload/spin-lock by interrupt mechanics + except KeyboardInterrupt: + print("\n Upload aborted by user.") + sys.exit(0) + +if __name__ == '__main__': + main() diff --git a/Tools/upload.sh b/Tools/upload.sh deleted file mode 100755 index 2a6416ed37..0000000000 --- a/Tools/upload.sh +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env bash - -EXEDIR=`pwd` -BASEDIR=$(dirname $0) - -SYSTYPE=`uname -s` - -# -# Serial port defaults. -# -# XXX The uploader should be smarter than this. -# -if [ $SYSTYPE = "Darwin" ]; then -SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" -fi - -if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/usb-ARK*," -fi - -if [[ $SYSTYPE = *"CYGWIN"* ]]; then -SERIAL_PORTS="/dev/ttyS*" -fi - -if [ $SYSTYPE = "" ]; then -SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" -fi - -python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c index daaace48fb..0081dc62dd 100755 --- a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/ark/can-flow-mr/canbootloader.px4board b/boards/ark/can-flow-mr/canbootloader.px4board new file mode 100644 index 0000000000..46917280f6 --- /dev/null +++ b/boards/ark/can-flow-mr/canbootloader.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y diff --git a/boards/ark/can-flow-mr/firmware.prototype b/boards/ark/can-flow-mr/firmware.prototype new file mode 100644 index 0000000000..2f5c9cf0d9 --- /dev/null +++ b/boards/ark/can-flow-mr/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 88, + "magic": "PX4FWv1", + "description": "Firmware for the ARK Flow MR board", + "image": "", + "build_time": 0, + "summary": "ARKFLOWMR", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/can-flow-mr/init/rc.board_sensors b/boards/ark/can-flow-mr/init/rc.board_sensors new file mode 100644 index 0000000000..2d476fcefb --- /dev/null +++ b/boards/ark/can-flow-mr/init/rc.board_sensors @@ -0,0 +1,20 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ + +param set-default IMU_GYRO_RATEMAX 1000 +param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_CLPNOTI 0 + +param set-default SENS_AFBR_S_RATE 25 +param set-default SENS_AFBR_L_RATE 10 +param set-default SENS_AFBR_THRESH 8 +param set-default SENS_AFBR_HYSTER 2 + +# Internal SPI +paa3905 -s start -Y 180 + +iim42653 -R 0 -s start + +afbrs50 start diff --git a/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig b/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig new file mode 100644 index 0000000000..a4540cbe54 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig @@ -0,0 +1,57 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INIT_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/can-flow-mr/nuttx-config/include/board.h b/boards/ark/can-flow-mr/nuttx-config/include/board.h new file mode 100644 index 0000000000..70818cb251 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/include/board.h @@ -0,0 +1,136 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#include "board_dma_map.h" + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/* HSI - 8 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 8 MHz Crystal + * LSE - not installed + */ +#define STM32_BOARD_USEHSE 1 +#define STM32_BOARD_XTAL 8000000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 + +/* Main PLL Configuration */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8) +#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) + +#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/ + +#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB +#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ + +#define STM32_SYSCLK_FREQUENCY 96000000ul + +/* AHB clock (HCLK) is SYSCLK (96MHz) */ +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK (96MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* Timers driven from APB2 will be PCLK2 since no prescale division */ +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */ +#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* UARTs */ +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_1 +#define GPIO_CAN1_TX GPIO_CAN1_TX_1 + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_1 /* PB10 */ + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h b/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..0eb81fc589 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX // DMA1, Stream 3, Channel 0 +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA1, Stream 4, Channel 0 + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_2 // DMA2, Stream 2, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 // DMA2, Stream 5, Channel 3 diff --git a/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig b/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..ed36c45b2e --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig @@ -0,0 +1,149 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_CROMFS=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2624 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_VARS=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=254 +CONFIG_SCHED_HPWORKSTACKSIZE=3000 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_WAITPID=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=2048 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI2_DMA=y +CONFIG_STM32_SPI2_DMA_BUFFER=2048 +CONFIG_STM32_TIM8=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 0000000000..48a59fe92d --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,134 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld b/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..2f4769b8f5 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/can-flow-mr/src/CMakeLists.txt b/boards/ark/can-flow-mr/src/CMakeLists.txt new file mode 100644 index 0000000000..4fae41fc0e --- /dev/null +++ b/boards/ark/can-flow-mr/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + init.c + led.c + spi.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/ark/can-flow-mr/src/board_config.h b/boards/ark/can-flow-mr/src/board_config.h new file mode 100644 index 0000000000..ad71f05b63 --- /dev/null +++ b/boards/ark/can-flow-mr/src/board_config.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PB11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) + +/* CAN termination software control */ +#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI) + +/* LEDs are driven with open drain to support Anode to 5V or 3.3V */ +#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) + +#define BROADCOM_AFBR_S50_S2PI_SPI_BUS 2 +#define BROADCOM_AFBR_S50_S2PI_CS /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12) +#define BROADCOM_AFBR_S50_S2PI_IRQ /* PB4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN4|GPIO_EXTI) +#define BROADCOM_AFBR_S50_S2PI_CLK /* PB10 */ GPIO_SPI2_SCK_1 +#define BROADCOM_AFBR_S50_S2PI_MOSI /* PB15 */ GPIO_SPI2_MOSI_1 +#define BROADCOM_AFBR_S50_S2PI_MISO /* PB14 */ GPIO_SPI2_MISO_1 + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define PX4_GPIO_INIT_LIST { \ + GPIO_BOOT_CONFIG, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN1_TERMINATION, \ + } + +__BEGIN_DECLS + +#define BOARD_HAS_N_S_RGB_LED 1 +#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/can-flow-mr/src/boot.c b/boards/ark/can-flow-mr/src/boot.c new file mode 100644 index 0000000000..a26034e254 --- /dev/null +++ b/boards/ark/can-flow-mr/src/boot.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "boot_config.h" +#include "board.h" + +#include +#include +#include + +#include +#include "led.h" + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)} + +typedef begin_packed_struct struct led_t { + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t hz; +} end_packed_struct led_t; + +static const led_t i2l[] = { + + led(0, off, 0, 0, 0, 0), + led(1, reset, 128, 128, 128, 30), + led(2, autobaud_start, 0, 128, 0, 1), + led(3, autobaud_end, 0, 128, 0, 2), + led(4, allocation_start, 0, 0, 64, 2), + led(5, allocation_end, 0, 128, 64, 3), + led(6, fw_update_start, 32, 128, 64, 3), + led(7, fw_update_erase_fail, 32, 128, 32, 3), + led(8, fw_update_invalid_response, 64, 0, 0, 1), + led(9, fw_update_timeout, 64, 0, 0, 2), + led(a, fw_update_invalid_crc, 64, 0, 0, 4), + led(b, jump_to_app, 0, 128, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red, + i2l[indication].green, + i2l[indication].blue, + i2l[indication].hz); +} diff --git a/boards/ark/can-flow-mr/src/boot_config.h b/boards/ark/can-flow-mr/src/boot_config.h new file mode 100644 index 0000000000..76782f9a93 --- /dev/null +++ b/boards/ark/can-flow-mr/src/boot_config.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/* + * @file boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#define OPT_TBOOT_MS 3000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 50 +#define OPT_BL_NUMBER_TIMERS 7 + +/* + * This Option set is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + * + */ +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/can-flow-mr/src/can.c b/boards/ark/can-flow-mr/src/can.c new file mode 100644 index 0000000000..7737965dc6 --- /dev/null +++ b/boards/ark/can-flow-mr/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/ark/can-flow-mr/src/init.c b/boards/ark/can-flow-mr/src/init.c new file mode 100644 index 0000000000..a6290bdc7a --- /dev/null +++ b/boards/ark/can-flow-mr/src/init.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" +#include "led.h" +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + watchdog_init(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {2, 16 * 1024, 0x08008000}, + {3, 16 * 1024, 0x0800C000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + //px4_platform_configure(); + + return OK; +} diff --git a/boards/ark/can-flow-mr/src/led.c b/boards/ark/can-flow-mr/src/led.c new file mode 100644 index 0000000000..9a80cae089 --- /dev/null +++ b/boards/ark/can-flow-mr/src/led.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +#include "led.h" + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +void rgb_led(int r, int g, int b, int freqs) +{ + + long fosc = TMR_FREQUENCY; + long prescale = 2048; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = 0; + + if (!once) { + once = 1; + + /* Enabel Clock to Block */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + + /* Reload */ + + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); + val |= ATIM_EGR_UG; + putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); + + /* Set Prescaler STM32_TIM_SETCLOCK */ + + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); + + /* Enable STM32_TIM_SETMODE*/ + + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); + + + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | + (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); + putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P | + ATIM_CCER_CC2E | ATIM_CCER_CC2P | + ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET)); + + + stm32_configgpio(GPIO_TIM1_CH1); + stm32_configgpio(GPIO_TIM1_CH2); + stm32_configgpio(GPIO_TIM1_CH3); + + /* master output enable = on */ + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); + } + + long p = freqs == 0 ? p1s : p1s / freqs; + putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + + putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); + putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); + + val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); + + if (freqs == 0) { + val &= ~ATIM_CR1_CEN; + + } else { + val |= ATIM_CR1_CEN; + } + + putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + +} diff --git a/boards/ark/can-flow-mr/src/led.h b/boards/ark/can-flow-mr/src/led.h new file mode 100644 index 0000000000..b68e4aa70d --- /dev/null +++ b/boards/ark/can-flow-mr/src/led.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void rgb_led(int r, int g, int b, int freqs); +__END_DECLS diff --git a/boards/ark/can-flow-mr/src/spi.cpp b/boards/ark/can-flow-mr/src/spi.cpp new file mode 100644 index 0000000000..697ddbbf1a --- /dev/null +++ b/boards/ark/can-flow-mr/src/spi.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}), + initSPIDevice(DRV_FLOW_DEVTYPE_PAA3905, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin12}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ark/can-flow-mr/uavcan_board_identity b/boards/ark/can-flow-mr/uavcan_board_identity new file mode 100644 index 0000000000..b0ecb3df4f --- /dev/null +++ b/boards/ark/can-flow-mr/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 88) +set(uavcanblid_name "\"org.ark.can-flow-mr\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) diff --git a/boards/ark/can-flow/init/rc.board_sensors b/boards/ark/can-flow/init/rc.board_sensors index 841049dc86..56adfca281 100644 --- a/boards/ark/can-flow/init/rc.board_sensors +++ b/boards/ark/can-flow/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ param set-default IMU_GYRO_RATEMAX 1000 +param set-default SENS_FLOW_RATE 150 param set-default SENS_IMU_CLPNOTI 0 # Internal SPI diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults index 13e16a24f1..3d085fe527 100644 --- a/boards/ark/can-gps/init/rc.board_defaults +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 99b30c747f..85d6a92063 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -7,7 +7,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 param set-default GPS_1_GNSS 63 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/cannode/init/rc.board_defaults b/boards/ark/cannode/init/rc.board_defaults index 7d149ce5ad..9fe7db23e0 100644 --- a/boards/ark/cannode/init/rc.board_defaults +++ b/boards/ark/cannode/init/rc.board_defaults @@ -3,6 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 pwm_out start diff --git a/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin b/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin index ea510ab36e..98473b2319 100755 Binary files a/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin and b/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin differ diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 52d098101d..d0732e90ef 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -3,6 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default EKF2_MULTI_IMU 0 + # Mavlink ethernet (CFG 1000) param set-default MAV_2_CONFIG 1000 param set-default MAV_2_BROADCAST 1 @@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 param set-default SENS_EN_THERMAL 1 +param set-default SENS_IMU_MODE 1 param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_FF 0.0 #param set-default SENS_IMU_TEMP_I 0.025 diff --git a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig index f5e59d2c37..94fbcc11b5 100644 --- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig @@ -145,10 +145,11 @@ CONFIG_NETDB_DNSCLIENT=y CONFIG_NETDB_DNSCLIENT_ENTRIES=8 CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y -CONFIG_NETINIT_DHCPC=y +CONFIG_NETMAN_FALLBACK_IPADDR=0xC0A80004 CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0xA290AFE -CONFIG_NETINIT_DRIPADDR=0xA290AFE +CONFIG_NETINIT_IPADDR=0xC0A80004 +CONFIG_NETINIT_DNSIPADDR=0xC0A800FE +CONFIG_NETINIT_DRIPADDR=0xC0A80001 CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 diff --git a/boards/ark/fmu-v6x/src/init.c b/boards/ark/fmu-v6x/src/init.c index 56c94f805b..6c1c2257ed 100644 --- a/boards/ark/fmu-v6x/src/init.c +++ b/boards/ark/fmu-v6x/src/init.c @@ -150,8 +150,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/ark/fpv/bootloader.px4board b/boards/ark/fpv/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/ark/fpv/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board new file mode 100644 index 0000000000..a7c34922cf --- /dev/null +++ b/boards/ark/fpv/default.px4board @@ -0,0 +1,79 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y +CONFIG_DRIVERS_IMU_MURATA_SCH16T=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_COMMON_RC=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin new file mode 100755 index 0000000000..d4f13bd428 Binary files /dev/null and b/boards/ark/fpv/extras/ark_fpv_bootloader.bin differ diff --git a/boards/ark/fpv/firmware.prototype b/boards/ark/fpv/firmware.prototype new file mode 100644 index 0000000000..f75702a85e --- /dev/null +++ b/boards/ark/fpv/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 59, + "magic": "ARKFPVFWv1", + "description": "Firmware for the ARKFPV board", + "image": "", + "build_time": 0, + "summary": "ARKFPV", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/fpv/init/rc.board_defaults b/boards/ark/fpv/init/rc.board_defaults new file mode 100644 index 0000000000..5605f60d02 --- /dev/null +++ b/boards/ark/fpv/init/rc.board_defaults @@ -0,0 +1,37 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# transision from params file to flash-based params (2022-08) +if [ -f $PARAM_FILE ] +then + param load $PARAM_FILE + param save + # create a backup + mv $PARAM_FILE ${PARAM_FILE}.bak + reboot +fi + +# TODO: Tune the following parameters +param set-default SENS_EN_THERMAL 1 +param set-default SENS_IMU_TEMP 10.0 +#param set-default SENS_IMU_TEMP_FF 0.0 +#param set-default SENS_IMU_TEMP_I 0.025 +#param set-default SENS_IMU_TEMP_P 1.0 + +if ver hwtypecmp ARKFPV000 +then + param set-default SENS_TEMP_ID 3014666 +fi + +param set-default BAT1_V_DIV 21.0 + +param set-default RC_CRSF_PRT_CFG 300 +param set-default RC_SBUS_PRT_CFG 0 + +param set-default IMU_GYRO_DNF_EN 3 + +# Single IMU +param set-default EKF2_MULTI_IMU 0 +param set-default SENS_IMU_MODE 1 diff --git a/boards/ark/fpv/init/rc.board_sensors b/boards/ark/fpv/init/rc.board_sensors new file mode 100644 index 0000000000..896b9ffd7d --- /dev/null +++ b/boards/ark/fpv/init/rc.board_sensors @@ -0,0 +1,18 @@ +#!/bin/sh +# +# ARKFPV specific board sensors init +#------------------------------------------------------------------------------ + +board_adc start + +if ver hwtypecmp ARKFPV000 +then + # Internal SPI bus IIM42653 + iim42653 -R 14 -s -b 1 start +fi + +# Internal magnetometer on I2C +iis2mdc -R 0 -I -b 4 start + +# Internal Baro on I2C +bmp388 -I -b 2 start diff --git a/boards/ark/fpv/nuttx-config/Kconfig b/boards/ark/fpv/nuttx-config/Kconfig new file mode 100644 index 0000000000..bb33d3cfda --- /dev/null +++ b/boards/ark/fpv/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. diff --git a/boards/ark/fpv/nuttx-config/bootloader/defconfig b/boards/ark/fpv/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..70c6114fea --- /dev/null +++ b/boards/ark/fpv/nuttx-config/bootloader/defconfig @@ -0,0 +1,95 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/fpv/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="ark" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x003B +CONFIG_CDCACM_PRODUCTSTR="ARK BL FPV.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="ARK" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_UART7=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_TXDMA=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/fpv/nuttx-config/include/board.h b/boards/ark/fpv/nuttx-config/include/board.h new file mode 100644 index 0000000000..bfa2887b66 --- /dev/null +++ b/boards/ark/fpv/nuttx-config/include/board.h @@ -0,0 +1,507 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6x/include/board.h + * + * Copyright (C) 2016-2024 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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 NuttX 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_PX4_FMU_V6X_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_PX4_FMU_V6X_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The px4_fmu-v6X board provides the following clock sources: + * + * X1: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (16,000,000 / 1) * 40 = 640 MHz + * + * PLL1P = PLL1_VCO/2 = 640 MHz / 2 = 320 MHz + * PLL1Q = PLL1_VCO/4 = 640 MHz / 4 = 160 MHz + * PLL1R = PLL1_VCO/8 = 640 MHz / 8 = 80 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(40) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 40) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 160 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (80 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (80 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (80 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (80 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 2 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The ARKV6X board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and + * LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +/* LED definitions ******************************************************************/ +/* The px4_fmu-v6x board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and + * LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_6 /* PH14 */ +#define GPIO_UART4_TX GPIO_UART4_TX_6 /* PH13 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ +#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ +// GPIO_UART5_RTS No remap /* PC8 */ +//#define GPIO_UART5_CTS (GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9|GPIO_PULLDOWN) /* PC9 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_4 /* PF6 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_2 /* PF8 */ +#define GPIO_UART7_CTS (GPIO_UART7_CTS_1 | GPIO_PULLDOWN) /* PE10 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +/* SPI + * SPI1 is sensors1 + * SPI2 is sensors2 + * SPI3 is sensors3 + * SPI4 is Not Used + * SPI5 is Not Used + * SPI6 is EXTERNAL1 + * + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_3 /* PG9 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_2 /* PB5 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI6_MISO GPIO_SPI6_MISO_2 /* PA6 */ +#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */ +#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_3) /* PB3 */ + +/* I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_2 /* PF1 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_2 /* PF0 */ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN1) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN0) + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ + +#define GPIO_I2C4_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN14) +#define GPIO_I2C4_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN15) + +/* SDMMC2 + * + * VDD 3.3 + * GND + * SDMMC2_CK PD6 + * SDMMC2_CMD PD7 + * SDMMC2_D0 PB14 + * SDMMC2_D1 PB15 + * SDMMC2_D2 PG11 + * SDMMC2_D3 PB4 + */ + +#define GPIO_SDMMC2_CK GPIO_SDMMC2_CK_1 /* PD6 */ +#define GPIO_SDMMC2_CMD GPIO_SDMMC2_CMD_1 /* PD7 */ +// GPIO_SDMMC2_D0 No Remap /* PB14 */ +// GPIO_SDMMC2_D1 No Remap /* PB15 */ +#define GPIO_SDMMC2_D2 GPIO_SDMMC2_D2_1 /* PG11 */ +// GPIO_SDMMC2_D3 No Remap /* PB4 */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# include "stm32_gpio.h" +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) /* PI0 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN12) /* PH12 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN11) /* PH11 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN10) /* PH10 AUX4 */ +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) /* PD13 AUX5 */ +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX6 */ +# define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) /* PH6 AUX7 */ +# define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) /* PH9 AUX8 */ + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \ + if ((mask)& PROBE_N(9)) { stm32_configgpio(PROBE_9); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +#endif /*__NUTTX_CONFIG_PX4_FMU_V6X_INCLUDE_BOARD_H */ diff --git a/boards/ark/fpv/nuttx-config/include/board_dma_map.h b/boards/ark/fpv/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..adbaaffc20 --- /dev/null +++ b/boards/ark/fpv/nuttx-config/include/board_dma_map.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 Using at most 8 Channels on DMA1 -------- Assigned +// V + +// Timer 4 Channel 1 /* DMA1:29 TIM4CH1 */ + +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 IIM-42653 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 IIM-42653 */ + +//#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 ICM-42688-P */ +//#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 ICM-42688-P */ + +#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ +#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ + +//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ + +// Timer 8 Channel 1 /* DMA1:47 TIM8CH1 */ +// Timer 8 Channel 2 /* DMA1:48 TIM8CH2 */ +// Timer 8 Channel 3 /* DMA1:49 TIM8CH3 */ +// Timer 8 Channel 4 /* DMA1:50 TIM8CH4 */ + +// Timer 5 Channel 1 /* DMA1:55 TIM5CH1 */ +// Timer 5 Channel 2 /* DMA1:56 TIM5CH2 */ +// Timer 5 Channel 3 /* DMA1:57 TIM5CH3 */ +// Timer 5 Channel 4 /* DMA1:58 TIM5CH4 */ + +// #define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 UART4 */ +// #define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 UART4 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 RC */ +// #define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 RC */ + +// Assigned in timer_config.cpp + +// Timer 4 /* 7 DMA1:32 TIM4UP */ +// Timer 5 /* 8 DMA1:50 TIM5UP */ + +// DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned +// V + +// Timer 4 Channel 1 /* DMA2:29 TIM4CH1 */ + +#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* 3 DMA2:43 TELEM3 */ +#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_1 /* 4 DMA2:44 TELEM3 */ + +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* 3 DMA2:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* 4 DMA2:46 DEBUG */ + +// Timer 8 Channel 1 /* DMA2:47 TIM8CH1 */ +// Timer 8 Channel 2 /* DMA2:48 TIM8CH2 */ +// Timer 8 Channel 3 /* DMA2:49 TIM8CH3 */ +// Timer 8 Channel 4 /* DMA2:50 TIM8CH4 */ + +// Timer 5 Channel 1 /* DMA2:55 TIM5CH1 */ +// Timer 5 Channel 2 /* DMA2:56 TIM5CH2 */ +// Timer 5 Channel 3 /* DMA2:57 TIM5CH3 */ +// Timer 5 Channel 4 /* DMA2:58 TIM5CH4 */ + +//#define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_1 /* 1 DMA2:61 BMI088 */ +//#define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_1 /* 2 DMA2:62 BMI088 */ + +#define DMAMAP_UART5_RX DMAMAP_DMA12_UART5RX_1 /* 5 DMA2:65 TELEM2 */ +#define DMAMAP_UART5_TX DMAMAP_DMA12_UART5TX_1 /* 6 DMA2:66 TELEM2 */ + +#define DMAMAP_UART7_RX DMAMAP_DMA12_UART7RX_1 /* 7 DMA1:79 TELEM1 */ +#define DMAMAP_UART7_TX DMAMAP_DMA12_UART7TX_1 /* 8 DMA1:80 TELEM1 */ + +// DMAMUX2 Using at most 8 Channels on BDMA -------- Assigned +// V + +#define DMAMAP_SPI6_RX DMAMAP_BDMA_SPI6_RX /* 1 BDMA:11 SPI J11 */ +#define DMAMAP_SPI6_TX DMAMAP_BDMA_SPI6_TX /* 2 BDMA:12 SPI J11 */ diff --git a/boards/ark/fpv/nuttx-config/nsh/defconfig b/boards/ark/fpv/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..f43478025c --- /dev/null +++ b/boards/ark/fpv/nuttx-config/nsh/defconfig @@ -0,0 +1,277 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_IFUPDOWN is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TELNETD is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/fpv/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="ark" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x003B +CONFIG_CDCACM_PRODUCTSTR="ARK FPV.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="ARK" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC2_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC2=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI6=y +CONFIG_STM32H7_SPI6_DMA=y +CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=10000 +CONFIG_UART5_TXDMA=y +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART7_TXDMA=y +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART1_TXDMA=y +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART2_TXDMA=y +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART3_TXDMA=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/ark/fpv/nuttx-config/scripts/bootloader_script.ld b/boards/ark/fpv/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..b0515c91c7 --- /dev/null +++ b/boards/ark/fpv/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2024 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The ARKV6X uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The ARKV6X has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The bootloader uses the first sector of the flash, which is 128K in length. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/fpv/nuttx-config/scripts/script.ld b/boards/ark/fpv/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..8e6dca3e49 --- /dev/null +++ b/boards/ark/fpv/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2024 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The ARKV6X uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The ARKV6X has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K /* params in last sector */ + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) +EXTERN(board_get_manifest) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/fpv/src/CMakeLists.txt b/boards/ark/fpv/src/CMakeLists.txt new file mode 100644 index 0000000000..78b8222f19 --- /dev/null +++ b/boards/ark/fpv/src/CMakeLists.txt @@ -0,0 +1,77 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_compile_definitions(BOOTLOADER) + add_library(drivers_board + bootloader_main.c + init.c + usb.c + timer_config.cpp + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer #gpio + arch_io_pins # iotimer + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + mtd.cpp + sdio.c + spi.cpp + spix_sync.c + spix_sync.h + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h new file mode 100644 index 0000000000..7b3c54183e --- /dev/null +++ b/boards/ark/fpv/src/board_config.h @@ -0,0 +1,400 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * ARK FPV internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#undef TRACE_PINS + +/* Configuration ************************************************************************************/ + +# define BOARD_HAS_USB_VALID 1 +# define BOARD_HAS_NBAT_V 1 +# define BOARD_HAS_NBAT_I 1 + +/* PX4FMU GPIOs ***********************************************************************************/ + +/* Trace Clock and D0-D3 are available on the trace connector + * + * TRACECLK PE2 - Dedicated - Trace Connector Pin 1 + * TRACED0 PE3 - nLED_RED - Trace Connector Pin 3 + * TRACED1 PE4 - nLED_GREEN - Trace Connector Pin 5 + * TRACED2 PE5 - nLED_BLUE - Trace Connector Pin 7 + * TRACED3 PE6 - nARMED - Trace Connector Pin 8 + + */ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V or used as TRACE0-2 */ + +#if !defined(TRACE_PINS) +# define GPIO_nLED_RED /* PE3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +# define GPIO_nLED_GREEN /* PE4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +# define GPIO_nLED_BLUE /* PE5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + +#else + +# define GPIO_TRACECLK1 (GPIO_TRACECLK |GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN2) +# define GPIO_TRACED0 (GPIO_TRACED0_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN3) +# define GPIO_TRACED1 (GPIO_TRACED1_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN4) +# define GPIO_TRACED2 (GPIO_TRACED2_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN5) +# define GPIO_TRACED3 (GPIO_TRACED3_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN6) +//#define GPIO_TRACESWO //(GPIO_ALT|GPIO_AF0|GPIO_PORTB|GPIO_PIN3) + +# undef BOARD_HAS_CONTROL_STATUS_LEDS +# undef BOARD_OVERLOAD_LED +# undef BOARD_ARMED_STATE_LED + +# define GPIO_nLED_RED GPIO_TRACED0 +# define GPIO_nLED_GREEN GPIO_TRACED1 +# define GPIO_nLED_BLUE GPIO_TRACED2 +# define GPIO_nARMED GPIO_TRACED3 +# define GPIO_nARMED_INIT GPIO_TRACED3 +#endif + +/* SPI */ + +#define SPI6_nRESET_EXTERNAL1 /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN10) +#define SPI6_RESET(on_true) px4_arch_gpiowrite(SPI6_nRESET_EXTERNAL1, !(on_true)) + +/* I2C busses */ + +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_SE050 0x48 + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define ADC1_CH(n) (n) + +/* N.B. there is no offset mapping needed for ADC3 because */ +#define ADC3_CH(n) (n) + +/* We are only use ADC3 for REV/VER. + * ADC3_6V6 and ADC3_3V3 are mapped back to ADC1 + * To do this We are relying on PC2_C, PC3_C being connected to PC2, PC3 + * respectively by the SYSCFG_PMCR default of setting for PC3SO PC2SO PA1SO + * PA0SO of 0. + * + * 0 Analog switch closed (pads are connected through the analog switch) + * + * So ADC3_INP0 is GPIO_ADC123_INP12 + * ADC3_INP1 is GPIO_ADC12_INP13 + */ + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PA0 */ GPIO_ADC1_INP16, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PB1 */ GPIO_ADC12_INP5, \ + /* PC2 */ GPIO_ADC123_INP12, \ + /* PC3 */ GPIO_ADC12_INP13, \ + /* PF12 */ GPIO_ADC1_INP6, \ + /* PH3 */ GPIO_ADC3_INP14, \ + /* PH4 */ GPIO_ADC3_INP15 + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PB0 */ ADC1_CH(9) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC2 */ ADC3_CH(12) +#define ADC_SCALED_12V_CHANNEL /* PA4 */ ADC1_CH(18) +#define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* PA0 */ ADC1_CH(16) +#define ADC_SCALED_V5_CHANNEL /* PB1 */ ADC1_CH(5) +#define ADC_HW_VER_SENSE_CHANNEL /* PH3 */ ADC3_CH(14) +#define ADC_HW_REV_SENSE_CHANNEL /* PH4 */ ADC3_CH(15) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_SCALED_12V_CHANNEL)) + + +#define BOARD_BATTERY1_V_DIV (21.0f) // (20k + 1k) / 1k = 21 + +#define BOARD_BATTERY_ADC_VOLTAGE_FILTER_S 0.075f +#define BOARD_BATTERY_ADC_CURRENT_FILTER_S 0.125f + +#define ADC_SCALED_PAYLOAD_SENSE ADC_SCALED_12V_CHANNEL + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ + +#define HW_REV_VER_ADC_BASE STM32_ADC3_BASE + +#define SYSTEM_ADC_BASE STM32_ADC1_BASE + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ +#define BOARD_HAS_HW_SPLIT_VERSIONING + +#define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) +#define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 +#define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 +#define HW_INFO_INIT_PREFIX "ARKFPV" + +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 +// Base/FMUM +#define ARKFPV_0 HW_FMUM_ID(0x0) // ARKFPV, Sensor Set Rev 0 +#define ARKFPV_1 HW_FMUM_ID(0x1) // ARKFPV, Sensor Set Rev 1 + +#define UAVCAN_NUM_IFACES_RUNTIME 1 + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PB10 T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PE6 is nARMED + * The GPIO will be set as input while not armed HW will have external HW Pull UP. + * While armed it shall be configured at a GPIO OUT set LOW + */ +#if !defined(TRACE_PINS) +#define GPIO_nARMED_INIT /* PE6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN6) +#define GPIO_nARMED /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6) +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED) +#endif + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 9 + +#define GPIO_FMU_CH1 /* PI0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0) +#define GPIO_FMU_CH2 /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12) +#define GPIO_FMU_CH3 /* PH11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN11) +#define GPIO_FMU_CH4 /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10) +#define GPIO_FMU_CH5 /* PI5 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN5) +#define GPIO_FMU_CH6 /* PI6 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN6) +#define GPIO_FMU_CH7 /* PI7 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN7) +#define GPIO_FMU_CH8 /* PI2 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN2) +#define GPIO_FMU_CH9 /* PD12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN12) + +#define GPIO_SPIX_SYNC /* PE9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) + +/* Power supply control and monitoring GPIOs */ + +#define GPIO_VDD_5V_PGOOD /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) +#define GPIO_VDD_12V_PGOOD /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) +#define GPIO_5V_ON_BATTERY /* PG1 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTG|GPIO_PIN1) +#define GPIO_VDD_12V_EN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN4) + +#define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + + +#define BOARD_NUMBER_BRICKS 1 + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) +#define PAYLOAD_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_12V_EN, (on_true)) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS4" + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL /* T4C2 */ 2 +#define GPIO_PWM_IN /* PD13 */ GPIO_TIM4_CH2IN_2 + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) + +/* ARKFPV never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_5V_PGOOD)) +#define BOARD_GPIO_PAYOLOAD_V_VALID (px4_arch_gpioread(GPIO_VDD_12V_PGOOD)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +#if defined(TRACE_PINS) +#define GPIO_TRACE \ + GPIO_TRACECLK1, \ + GPIO_TRACED0, \ + GPIO_TRACED1, \ + GPIO_TRACED2, \ + GPIO_TRACED3 +#else +#define GPIO_TRACE (GPIO_OUTPUT|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#endif + +#define PX4_GPIO_INIT_LIST { \ + GPIO_TRACE, \ + PX4_ADC_GPIO, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_VDD_5V_PGOOD, \ + GPIO_VDD_12V_PGOOD, \ + GPIO_VDD_12V_EN, \ + GPIO_5V_ON_BATTERY, \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_nARMED_INIT, \ + SPI6_nRESET_EXTERNAL1, \ + GPIO_FMU_CH1, \ + GPIO_FMU_CH2, \ + GPIO_FMU_CH3, \ + GPIO_FMU_CH4, \ + GPIO_FMU_CH5, \ + GPIO_FMU_CH6, \ + GPIO_FMU_CH7, \ + GPIO_FMU_CH8, \ + GPIO_FMU_CH9, \ + GPIO_SPIX_SYNC \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 3 +#define BOARD_SPIX_SYNC_FREQ 32000 + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/fpv/src/bootloader_main.c b/boards/ark/fpv/src/bootloader_main.c new file mode 100644 index 0000000000..7a3ef5e019 --- /dev/null +++ b/boards/ark/fpv/src/bootloader_main.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure pins */ + const uint32_t list[] = PX4_GPIO_INIT_LIST; + + for (size_t gpio = 0; gpio < arraySize(list); gpio++) { + if (list[gpio] != 0) { + px4_arch_configgpio(list[gpio]); + } + } + + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/ark/fpv/src/can.c b/boards/ark/fpv/src/can.c new file mode 100644 index 0000000000..a586a64814 --- /dev/null +++ b/boards/ark/fpv/src/can.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +#if !defined(CONFIG_CAN) + +#include + +#include "board_config.h" + + +__EXPORT +uint16_t board_get_can_interfaces(void) +{ + uint16_t enabled_interfaces = 0x3; + + enabled_interfaces &= ~(1 << 1); + + return enabled_interfaces; +} + +#else + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} +#endif /* CONFIG_CAN */ diff --git a/boards/ark/fpv/src/hw_config.h b/boards/ark/fpv/src/hw_config.h new file mode 100644 index 0000000000..f677802289 --- /dev/null +++ b/boards/ark/fpv/src/hw_config.h @@ -0,0 +1,129 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,921600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 59 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_GREEN // GREEN +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/ark/fpv/src/i2c.cpp b/boards/ark/fpv/src/i2c.cpp new file mode 100644 index 0000000000..8add5e64f7 --- /dev/null +++ b/boards/ark/fpv/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(2), + initI2CBusInternal(4), +}; diff --git a/boards/ark/fpv/src/init.c b/boards/ark/fpv/src/init.c new file mode 100644 index 0000000000..cbd9382238 --- /dev/null +++ b/boards/ark/fpv/src/init.c @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * ARKFMU-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" +#include "spix_sync.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + PAYLOAD_POWER_EN(false); + board_control_spi_sensors_power(false, 0xffff); + SPI6_RESET(true); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + board_control_spi_sensors_power(true, 0xffff); + PAYLOAD_POWER_EN(true); + + /* Release SPI6 Reset */ + SPI6_RESET(false); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ + if (status >= 0) { + up_mdelay(100); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure USB interfaces */ + + stm32_usbinitialize(); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + PAYLOAD_POWER_EN(true); + + SPI6_RESET(false); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + // Use the default HW_VER_REV(0x0,0x0) for Ramtron + + stm32_spiinitialize(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_AMBER); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + /* Configure the Actual SPI interfaces (after we determined the HW version) */ + + stm32_spiinitialize(); + + board_spi_reset(10, 0xffff); + + /* Configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + + // Ensure Power is off for > 10 mS + usleep(15 * 1000); + VDD_3V3_SD_CARD_EN(true); + usleep(500 * 1000); + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + +#endif /* CONFIG_MMCSD */ + + /* Configure the SPIX_SYNC output */ + spix_sync_servo_init(BOARD_SPIX_SYNC_FREQ); + spix_sync_servo_set(0, 150); + + return OK; +} diff --git a/boards/ark/fpv/src/led.c b/boards/ark/fpv/src/led.c new file mode 100644 index 0000000000..b629ade32c --- /dev/null +++ b/boards/ark/fpv/src/led.c @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * ARKFMU LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +#ifdef CONFIG_ARCH_LEDS +static bool nuttx_owns_leds = true; +// B R S G +// 0 1 2 3 +static const uint8_t xlatpx4[] = {1, 2, 4, 0}; +# define xlat(p) xlatpx4[(p)] +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + 0, // Indexed by LED_SAFETY (defaulted to an input) + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +#endif + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} + +#ifdef CONFIG_ARCH_LEDS +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/ark/fpv/src/mtd.cpp b/boards/ark/fpv/src/mtd.cpp new file mode 100644 index 0000000000..bd74d551ee --- /dev/null +++ b/boards/ark/fpv/src/mtd.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include + +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = { + &mft_mft, + } +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/ark/fpv/src/sdio.c b/boards/ark/fpv/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/ark/fpv/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/ark/fpv/src/spi.cpp b/boards/ark/fpv/src/spi.cpp new file mode 100644 index 0000000000..fb922fc95d --- /dev/null +++ b/boards/ark/fpv/src/spi.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 +#include +#include + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIFmumID(ARKFPV_0, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) + }), + }), + initSPIFmumID(ARKFPV_1, { // Placeholder + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) + }), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/ark/fpv/src/spix_sync.c b/boards/ark/fpv/src/spix_sync.c new file mode 100644 index 0000000000..056e38e75f --- /dev/null +++ b/boards/ark/fpv/src/spix_sync.c @@ -0,0 +1,309 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 Airmind 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 spix_sync.c +* +* +*/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include + +#include "spix_sync.h" + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(spix_sync_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) + +#define BOARD_SPIX_SYNC_PWM_FREQ 1024000 + +unsigned +spix_sync_timer_get_period(unsigned timer) +{ + return (rARR(timer)); +} + +static void spix_sync_timer_init_timer(unsigned timer, unsigned rate) +{ + if (spix_sync_timers[timer].base) { + + irqstate_t flags = px4_enter_critical_section(); + + /* enable the timer clock before we try to talk to it */ + + modifyreg32(spix_sync_timers[timer].clock_register, 0, spix_sync_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCR1(timer) = 0; + rCCR2(timer) = 0; + rCCR3(timer) = 0; + rCCR4(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((spix_sync_timers[timer].base == STM32_TIM1_BASE) || (spix_sync_timers[timer].base == STM32_TIM8_BASE)) { + + /* master output enable = on */ + + rBDTR(timer) = ATIM_BDTR_MOE; + } + + /* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN + * then configure the timer to free-run at 1MHz. + * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. + */ + + rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1; + + /* configure the timer to update at the desired rate */ + + rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + + px4_leave_critical_section(flags); + } + +} + +void spix_sync_channel_init(unsigned channel) +{ + /* Only initialize used channels */ + + if (spix_sync_channels[channel].timer_channel) { + + unsigned timer = spix_sync_channels[channel].timer_index; + + /* configure the GPIO first */ + px4_arch_configgpio(spix_sync_channels[channel].gpio_out); + + uint16_t polarity = spix_sync_channels[channel].masks; + + /* configure the channel */ + switch (spix_sync_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; + rCCER(timer) |= polarity | GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + rCCER(timer) |= polarity | GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + rCCER(timer) |= polarity | GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + rCCER(timer) |= polarity | GTIM_CCER_CC4E; + break; + } + } +} + +int +spix_sync_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel >= arraySize(spix_sync_channels)) { + return -1; + } + + unsigned timer = spix_sync_channels[channel].timer_index; + + /* test timer for validity */ + if ((spix_sync_timers[timer].base == 0) || + (spix_sync_channels[channel].gpio_out == 0)) { + return -1; + } + + unsigned period = spix_sync_timer_get_period(timer); + + unsigned value = (unsigned)cvalue * period / 255; + + /* configure the channel */ + if (value > 0) { + value--; + } + + + switch (spix_sync_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; + } + + return 0; +} + +unsigned spix_sync_servo_get(unsigned channel) +{ + if (channel >= 3) { + return 0; + } + + unsigned timer = spix_sync_channels[channel].timer_index; + uint16_t value = 0; + + /* test timer for validity */ + if ((spix_sync_timers[timer].base == 0) || + (spix_sync_channels[channel].timer_channel == 0)) { + return 0; + } + + /* configure the channel */ + switch (spix_sync_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; + } + + unsigned period = spix_sync_timer_get_period(timer); + return ((value + 1) * 255 / period); +} + +int spix_sync_servo_init(unsigned rate) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) { + spix_sync_timer_init_timer(i, rate); + } + + /* now init channels */ + for (unsigned i = 0; i < arraySize(spix_sync_channels); i++) { + spix_sync_channel_init(i); + } + + spix_sync_servo_arm(true); + return OK; +} + +void +spix_sync_servo_deinit(void) +{ + /* disable the timers */ + spix_sync_servo_arm(false); +} +void +spix_sync_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) { + if (spix_sync_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + rCR1(i) = 0; + } + } + } +} diff --git a/boards/ark/fpv/src/spix_sync.h b/boards/ark/fpv/src/spix_sync.h new file mode 100644 index 0000000000..2e37c89086 --- /dev/null +++ b/boards/ark/fpv/src/spix_sync.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void spix_sync_channel_init(unsigned channel); +int spix_sync_servo_set(unsigned channel, uint8_t value); +unsigned spix_sync_servo_get(unsigned channel); +int spix_sync_servo_init(unsigned rate); +void spix_sync_servo_deinit(void); +void spix_sync_servo_arm(bool armed); +unsigned spix_sync_timer_get_period(unsigned timer); +__END_DECLS diff --git a/boards/ark/fpv/src/timer_config.cpp b/boards/ark/fpv/src/timer_config.cpp new file mode 100644 index 0000000000..623aad1f06 --- /dev/null +++ b/boards/ark/fpv/src/timer_config.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 + +/* Timer allocation + * + * TIM5_CH4 T FMU_CH1 + * TIM5_CH3 T FMU_CH2 + * TIM5_CH2 T FMU_CH3 + * TIM5_CH1 T FMU_CH4 + * + * TIM8_CH1 T FMU_CH5 + * TIM8_CH2 T FMU_CH6 + * TIM8_CH3 T FMU_CH7 + * TIM8_CH4 T FMU_CH8 + * + * TIM4_CH1 T FMU_CH9 + * + * TIM1_CH1 T SPIX_SYNC > Pulse or GPIO strobe + * + * TIM2_CH3 T HEATER > PWM OUT or GPIO + * + * TIM3_CH1 T HRT_TIMER + * + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortI, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortH, GPIO::Pin12}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortH, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortH, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel3}, {GPIO::PortI, GPIO::Pin7}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel4}, {GPIO::PortI, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); + + +constexpr io_timers_t spix_sync_timers[MAX_SPIX_SYNC_TIMERS] = { + initIOTimer(Timer::Timer1), +}; + +constexpr timer_io_channels_t spix_sync_channels[MAX_SPIX_SYNC_TIMERS] = { + initIOTimerChannel(spix_sync_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), +}; diff --git a/boards/ark/fpv/src/usb.c b/boards/ark/fpv/src/usb.c new file mode 100644 index 0000000000..1c64e94ba1 --- /dev/null +++ b/boards/ark/fpv/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "arm_internal.h" +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the ARKFMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults index 717632839b..1b92949d1d 100644 --- a/boards/ark/pi6x/init/rc.board_defaults +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -32,10 +32,11 @@ then param set-default SENS_TEMP_ID 2490378 fi -param set-default EKF2_MULTI_IMU 2 +param set-default EKF2_MULTI_IMU 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_OF_N_MIN 0.05 param set-default EKF2_RNG_A_HMAX 25 param set-default EKF2_RNG_QLTY_T 0.1 param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_MODE 1 diff --git a/boards/ark/pi6x/nuttx-config/bootloader/defconfig b/boards/ark/pi6x/nuttx-config/bootloader/defconfig index 745251793b..bba1ad3f39 100644 --- a/boards/ark/pi6x/nuttx-config/bootloader/defconfig +++ b/boards/ark/pi6x/nuttx-config/bootloader/defconfig @@ -32,7 +32,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK BL Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/ark/pi6x/nuttx-config/nsh/defconfig b/boards/ark/pi6x/nuttx-config/nsh/defconfig index 0e944e53c9..57ff449302 100644 --- a/boards/ark/pi6x/nuttx-config/nsh/defconfig +++ b/boards/ark/pi6x/nuttx-config/nsh/defconfig @@ -76,7 +76,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/ark/pi6x/src/init.c b/boards/ark/pi6x/src/init.c index 90bb4fc128..4dcef47cac 100644 --- a/boards/ark/pi6x/src/init.c +++ b/boards/ark/pi6x/src/init.c @@ -141,8 +141,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/ark/septentrio-gps/init/rc.board_defaults b/boards/ark/septentrio-gps/init/rc.board_defaults index 0db1234ce9..6ae5883882 100644 --- a/boards/ark/septentrio-gps/init/rc.board_defaults +++ b/boards/ark/septentrio-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/teseo-gps/canbootloader.px4board b/boards/ark/teseo-gps/canbootloader.px4board new file mode 100644 index 0000000000..46917280f6 --- /dev/null +++ b/boards/ark/teseo-gps/canbootloader.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y diff --git a/boards/ark/teseo-gps/firmware.prototype b/boards/ark/teseo-gps/firmware.prototype new file mode 100644 index 0000000000..d64a39eabf --- /dev/null +++ b/boards/ark/teseo-gps/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 86, + "magic": "PX4FWv1", + "description": "Firmware for the ARK Teseo GPS board", + "image": "", + "build_time": 0, + "summary": "ARKTESEOGPS", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/teseo-gps/init/rc.board_defaults b/boards/ark/teseo-gps/init/rc.board_defaults new file mode 100644 index 0000000000..c765117f40 --- /dev/null +++ b/boards/ark/teseo-gps/init/rc.board_defaults @@ -0,0 +1,14 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default CBRK_IO_SAFETY 0 +param set-default CANNODE_SUB_MBD 1 +param set-default CANNODE_SUB_RTCM 1 +param set-default MBE_ENABLE 0 +param set-default SENS_IMU_CLPNOTI 0 + +tone_alarm start + +ekf2 start diff --git a/boards/ark/teseo-gps/init/rc.board_sensors b/boards/ark/teseo-gps/init/rc.board_sensors new file mode 100644 index 0000000000..b685c0b209 --- /dev/null +++ b/boards/ark/teseo-gps/init/rc.board_sensors @@ -0,0 +1,15 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ +echo "Starting Teseo GPS" +teseo_gps start -d /dev/ttyS0 -b 460800 + +icm42688p -R 0 -s start + +bmp388 -I -b 1 start + +if ! iis2mdc -R 2 -I -b 1 start +then + bmm150 -I -b 1 start +fi diff --git a/boards/ark/teseo-gps/nuttx-config/canbootloader/defconfig b/boards/ark/teseo-gps/nuttx-config/canbootloader/defconfig new file mode 100644 index 0000000000..bd6ee6241e --- /dev/null +++ b/boards/ark/teseo-gps/nuttx-config/canbootloader/defconfig @@ -0,0 +1,57 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/teseo-gps/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INIT_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/teseo-gps/nuttx-config/include/board.h b/boards/ark/teseo-gps/nuttx-config/include/board.h new file mode 100644 index 0000000000..526392b92b --- /dev/null +++ b/boards/ark/teseo-gps/nuttx-config/include/board.h @@ -0,0 +1,152 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#include "board_dma_map.h" + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/* HSI - 8 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 8 MHz Crystal + * LSE - not installed + */ +#define STM32_BOARD_USEHSE 1 +#define STM32_BOARD_XTAL 8000000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 + +/* Main PLL Configuration */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8) +#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) + +#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/ + +#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB +#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ + +#define STM32_SYSCLK_FREQUENCY 96000000ul + +/* AHB clock (HCLK) is SYSCLK (96MHz) */ +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK (96MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* Timers driven from APB2 will be PCLK2 since no prescale division */ +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */ +#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* UARTs */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_3 + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_1 +#define GPIO_CAN1_TX GPIO_CAN1_TX_1 + +/* I2C */ + +#define GPIO_MCU_I2C1_SCL +#define GPIO_MCU_I2C1_SDA + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_4 + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/src/drivers/voxl2_io/voxl2_io_serial.hpp b/boards/ark/teseo-gps/nuttx-config/include/board_dma_map.h similarity index 70% rename from src/drivers/voxl2_io/voxl2_io_serial.hpp rename to boards/ark/teseo-gps/nuttx-config/include/board_dma_map.h index 638bdef288..efa3d824b2 100644 --- a/src/drivers/voxl2_io/voxl2_io_serial.hpp +++ b/boards/ark/teseo-gps/nuttx-config/include/board_dma_map.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2021 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 @@ -33,37 +33,14 @@ #pragma once -#include -#include -#include -#include +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- -#ifdef __PX4_QURT -#include -#define FAR -#endif -class Voxl2IoSerial -{ -public: - Voxl2IoSerial(); - virtual ~Voxl2IoSerial(); - - int uart_open(const char *dev, speed_t speed); - int uart_set_baud(speed_t speed); - int uart_close(); - int uart_write(FAR void *buf, size_t len); - int uart_read(FAR void *buf, size_t len); - bool is_open() { return _uart_fd >= 0; }; - int uart_get_baud() {return _speed; } - -private: - int _uart_fd = -1; - -#if ! defined(__PX4_QURT) - struct termios _orig_cfg; - struct termios _cfg; -#endif - - int _speed = -1; -}; +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 +#define DMACHAN_USART1_RX DMAMAP_USART1_RX_1 // DMA2, Stream 2, Channel 4 +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 +//#define DMACHAN_USART1_TX DMAMAP_USART1_TX // DMA2, Stream 7, Channel 4 diff --git a/boards/ark/teseo-gps/nuttx-config/nsh/defconfig b/boards/ark/teseo-gps/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..e8ff0a6bdb --- /dev/null +++ b/boards/ark/teseo-gps/nuttx-config/nsh/defconfig @@ -0,0 +1,155 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/teseo-gps/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_CROMFS=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2624 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_VARS=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_WAITPID=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=1024 +CONFIG_STM32_TIM8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=2000 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=2000 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/teseo-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/teseo-gps/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 0000000000..48a59fe92d --- /dev/null +++ b/boards/ark/teseo-gps/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,134 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/teseo-gps/nuttx-config/scripts/script.ld b/boards/ark/teseo-gps/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..2f4769b8f5 --- /dev/null +++ b/boards/ark/teseo-gps/nuttx-config/scripts/script.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/teseo-gps/src/CMakeLists.txt b/boards/ark/teseo-gps/src/CMakeLists.txt new file mode 100644 index 0000000000..06bd98156f --- /dev/null +++ b/boards/ark/teseo-gps/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + led.h + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + spi.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/ark/teseo-gps/src/board_config.h b/boards/ark/teseo-gps/src/board_config.h new file mode 100644 index 0000000000..e7a43e2c8e --- /dev/null +++ b/boards/ark/teseo-gps/src/board_config.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* GPS nRESET */ +#define GPIO_GPS_nRESET /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) + +/* GPS TIMEPULSE */ +#define GPIO_GPS_TIMEPULSE /* PB2 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN2|GPIO_EXTI) + +/* GPS EXTINT */ +#define GPIO_GPS_EXTINT /* PB4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) + +/* Tone alarm output. */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE /* PA0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TONE_ALARM /* PA0 */ (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) + +/* CAN termination software control */ +#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +/* ICM42688p FSYNC */ +#define GPIO_42688P_FSYNC /* PB8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI) + +/* LEDs are driven with open drain to support Anode to 5V or 3.3V */ +#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) + +#define GPIO_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6) +#define GPIO_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) + +#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL_INPUT /* PB10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_INPUT /* PB9 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN9) + +#define PX4_GPIO_I2C2_INIT_LIST { \ + GPIO_I2C2_SCL_RESET, \ + GPIO_I2C2_SDA_RESET, \ + } + +#define GPIO_USART1_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN3) +#define GPIO_USART1_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_USART2_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN3) +#define GPIO_USART2_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN2) + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define PX4_GPIO_INIT_LIST { \ + GPIO_GPS_nRESET, \ + GPIO_GPS_TIMEPULSE, \ + GPIO_GPS_EXTINT, \ + GPIO_I2C1_SCL_RESET, \ + GPIO_I2C1_SDA_RESET, \ + GPIO_I2C2_SCL_INPUT, \ + GPIO_I2C2_SDA_INPUT, \ + GPIO_42688P_FSYNC, \ + GPIO_BOOT_CONFIG, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN1_TERMINATION, \ + } + +__BEGIN_DECLS + +#define BOARD_HAS_N_S_RGB_LED 1 +#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/teseo-gps/src/boot.c b/boards/ark/teseo-gps/src/boot.c new file mode 100644 index 0000000000..a26034e254 --- /dev/null +++ b/boards/ark/teseo-gps/src/boot.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "boot_config.h" +#include "board.h" + +#include +#include +#include + +#include +#include "led.h" + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)} + +typedef begin_packed_struct struct led_t { + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t hz; +} end_packed_struct led_t; + +static const led_t i2l[] = { + + led(0, off, 0, 0, 0, 0), + led(1, reset, 128, 128, 128, 30), + led(2, autobaud_start, 0, 128, 0, 1), + led(3, autobaud_end, 0, 128, 0, 2), + led(4, allocation_start, 0, 0, 64, 2), + led(5, allocation_end, 0, 128, 64, 3), + led(6, fw_update_start, 32, 128, 64, 3), + led(7, fw_update_erase_fail, 32, 128, 32, 3), + led(8, fw_update_invalid_response, 64, 0, 0, 1), + led(9, fw_update_timeout, 64, 0, 0, 2), + led(a, fw_update_invalid_crc, 64, 0, 0, 4), + led(b, jump_to_app, 0, 128, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red, + i2l[indication].green, + i2l[indication].blue, + i2l[indication].hz); +} diff --git a/boards/ark/teseo-gps/src/boot_config.h b/boards/ark/teseo-gps/src/boot_config.h new file mode 100644 index 0000000000..76782f9a93 --- /dev/null +++ b/boards/ark/teseo-gps/src/boot_config.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/* + * @file boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#define OPT_TBOOT_MS 3000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 50 +#define OPT_BL_NUMBER_TIMERS 7 + +/* + * This Option set is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + * + */ +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/teseo-gps/src/can.c b/boards/ark/teseo-gps/src/can.c new file mode 100644 index 0000000000..7737965dc6 --- /dev/null +++ b/boards/ark/teseo-gps/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/ark/teseo-gps/src/i2c.cpp b/boards/ark/teseo-gps/src/i2c.cpp new file mode 100644 index 0000000000..6700d8c8f2 --- /dev/null +++ b/boards/ark/teseo-gps/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(1), + initI2CBusExternal(2), +}; diff --git a/boards/ark/teseo-gps/src/init.c b/boards/ark/teseo-gps/src/init.c new file mode 100644 index 0000000000..08e7d11763 --- /dev/null +++ b/boards/ark/teseo-gps/src/init.c @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" +#include "led.h" +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + watchdog_init(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); + + // Check if both I2C2 signals are grounded. If so go into gps passthrough mode + if (!stm32_gpioread(GPIO_I2C2_SCL_INPUT) && !stm32_gpioread(GPIO_I2C2_SDA_INPUT)) { + rgb_led(128, 128, 128, 10); + stm32_configgpio(GPIO_USART1_TX_GPIO); + stm32_configgpio(GPIO_USART1_RX_GPIO); + stm32_configgpio(GPIO_USART2_TX_GPIO); + stm32_configgpio(GPIO_USART2_RX_GPIO); + + while (1) { + watchdog_pet(); + stm32_gpiowrite(GPIO_USART2_TX_GPIO, stm32_gpioread(GPIO_USART1_RX_GPIO)); + stm32_gpiowrite(GPIO_USART1_TX_GPIO, stm32_gpioread(GPIO_USART2_RX_GPIO)); + } + + } else { + const uint32_t i2c2_gpio[] = PX4_GPIO_I2C2_INIT_LIST; + px4_gpio_init(i2c2_gpio, arraySize(i2c2_gpio)); + } +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {2, 16 * 1024, 0x08008000}, + {3, 16 * 1024, 0x0800C000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + //px4_platform_configure(); + + return OK; +} diff --git a/boards/ark/teseo-gps/src/led.c b/boards/ark/teseo-gps/src/led.c new file mode 100644 index 0000000000..9a80cae089 --- /dev/null +++ b/boards/ark/teseo-gps/src/led.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +#include "led.h" + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +void rgb_led(int r, int g, int b, int freqs) +{ + + long fosc = TMR_FREQUENCY; + long prescale = 2048; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = 0; + + if (!once) { + once = 1; + + /* Enabel Clock to Block */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + + /* Reload */ + + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); + val |= ATIM_EGR_UG; + putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); + + /* Set Prescaler STM32_TIM_SETCLOCK */ + + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); + + /* Enable STM32_TIM_SETMODE*/ + + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); + + + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | + (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); + putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P | + ATIM_CCER_CC2E | ATIM_CCER_CC2P | + ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET)); + + + stm32_configgpio(GPIO_TIM1_CH1); + stm32_configgpio(GPIO_TIM1_CH2); + stm32_configgpio(GPIO_TIM1_CH3); + + /* master output enable = on */ + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); + } + + long p = freqs == 0 ? p1s : p1s / freqs; + putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + + putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); + putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); + + val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); + + if (freqs == 0) { + val &= ~ATIM_CR1_CEN; + + } else { + val |= ATIM_CR1_CEN; + } + + putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + +} diff --git a/boards/ark/teseo-gps/src/led.h b/boards/ark/teseo-gps/src/led.h new file mode 100644 index 0000000000..b68e4aa70d --- /dev/null +++ b/boards/ark/teseo-gps/src/led.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void rgb_led(int r, int g, int b, int freqs); +__END_DECLS diff --git a/boards/ark/teseo-gps/src/spi.cpp b/boards/ark/teseo-gps/src/spi.cpp new file mode 100644 index 0000000000..baafb0354c --- /dev/null +++ b/boards/ark/teseo-gps/src/spi.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ark/teseo-gps/uavcan_board_identity b/boards/ark/teseo-gps/uavcan_board_identity new file mode 100644 index 0000000000..af0388b81c --- /dev/null +++ b/boards/ark/teseo-gps/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 86) +set(uavcanblid_name "\"org.ark.teseo-gps\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) diff --git a/boards/av/x-v1/src/init.c b/boards/av/x-v1/src/init.c index 8e12ee823e..0f455559f1 100644 --- a/boards/av/x-v1/src/init.c +++ b/boards/av/x-v1/src/init.c @@ -94,8 +94,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 9c90e55655..35abd8e338 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -31,6 +31,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 0c268b6983..0db161f13e 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/cuav/7-nano/src/board_config.h b/boards/cuav/7-nano/src/board_config.h index ada5c1ae07..07a02fa53c 100644 --- a/boards/cuav/7-nano/src/board_config.h +++ b/boards/cuav/7-nano/src/board_config.h @@ -350,6 +350,7 @@ GPIO_nSAFETY_SWITCH_LED_OUT, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_nARMED, \ + GPIO_PWM_LEVEL_CONTROL, \ } #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/cuav/7-nano/src/init.c b/boards/cuav/7-nano/src/init.c index 3bd2164622..6ee02bb529 100644 --- a/boards/cuav/7-nano/src/init.c +++ b/boards/cuav/7-nano/src/init.c @@ -142,8 +142,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cuav/nora/src/init.c b/boards/cuav/nora/src/init.c index d4c262b76a..a8f3bf4ad3 100644 --- a/boards/cuav/nora/src/init.c +++ b/boards/cuav/nora/src/init.c @@ -113,8 +113,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cuav/x7pro/src/init.c b/boards/cuav/x7pro/src/init.c index d4c262b76a..a8f3bf4ad3 100644 --- a/boards/cuav/x7pro/src/init.c +++ b/boards/cuav/x7pro/src/init.c @@ -113,8 +113,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board_dma_map.h b/boards/cubepilot/cubeorange/nuttx-config/include/board_dma_map.h index 050ae597d9..1ff9dee0bc 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board_dma_map.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board_dma_map.h @@ -37,8 +37,9 @@ #define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ #define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ -#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ -#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ - #define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_0 /* DMA1:83 */ #define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_0 /* DMA1:84 */ + +// DMAMUX2 +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA2:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA2:72 */ diff --git a/boards/cubepilot/cubeorange/src/init.c b/boards/cubepilot/cubeorange/src/init.c index f7e1b3ca77..9bf3d5f58e 100644 --- a/boards/cubepilot/cubeorange/src/init.c +++ b/boards/cubepilot/cubeorange/src/init.c @@ -104,8 +104,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cubepilot/cubeorangeplus/src/init.c b/boards/cubepilot/cubeorangeplus/src/init.c index f7e1b3ca77..9bf3d5f58e 100644 --- a/boards/cubepilot/cubeorangeplus/src/init.c +++ b/boards/cubepilot/cubeorangeplus/src/init.c @@ -104,8 +104,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cubepilot/cubeyellow/src/init.c b/boards/cubepilot/cubeyellow/src/init.c index 8ba8972402..0eb3701c04 100644 --- a/boards/cubepilot/cubeyellow/src/init.c +++ b/boards/cubepilot/cubeyellow/src/init.c @@ -102,8 +102,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index e329b23bb8..d6483c7faa 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -37,6 +37,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/emlid/navio2/arm64.px4board b/boards/emlid/navio2/arm64.px4board new file mode 100644 index 0000000000..35e1292202 --- /dev/null +++ b/boards/emlid/navio2/arm64.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" diff --git a/boards/flywoo/gn-f405/default.px4board b/boards/flywoo/gn-f405/default.px4board index b2e8d853b5..8bd64826d8 100644 --- a/boards/flywoo/gn-f405/default.px4board +++ b/boards/flywoo/gn-f405/default.px4board @@ -28,6 +28,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/hkust/nxt-dual/src/board_config.h b/boards/hkust/nxt-dual/src/board_config.h index c766ba5f39..eeb20ef8c9 100644 --- a/boards/hkust/nxt-dual/src/board_config.h +++ b/boards/hkust/nxt-dual/src/board_config.h @@ -95,7 +95,7 @@ /* Define GPIO pins used as ADC N.B. Channel numbers must match below */ /* Define Channel numbers must match above GPIO pin IN(n)*/ #define ADC_BATTERY_VOLTAGE_CHANNEL ADC12_CH(4) -#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(5) +#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(8) #define ADC_CHANNELS \ ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ diff --git a/boards/hkust/nxt-dual/src/init.c b/boards/hkust/nxt-dual/src/init.c index 657c0080c0..cd7d3d2162 100644 --- a/boards/hkust/nxt-dual/src/init.c +++ b/boards/hkust/nxt-dual/src/init.c @@ -100,8 +100,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/hkust/nxt-v1/src/init.c b/boards/hkust/nxt-v1/src/init.c index 657c0080c0..cd7d3d2162 100644 --- a/boards/hkust/nxt-v1/src/init.c +++ b/boards/hkust/nxt-v1/src/init.c @@ -100,8 +100,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/durandal-v1/src/init.c b/boards/holybro/durandal-v1/src/init.c index f8dc55d72f..283b44842e 100644 --- a/boards/holybro/durandal-v1/src/init.c +++ b/boards/holybro/durandal-v1/src/init.c @@ -144,8 +144,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/h-flow/canbootloader.px4board b/boards/holybro/h-flow/canbootloader.px4board new file mode 100644 index 0000000000..46917280f6 --- /dev/null +++ b/boards/holybro/h-flow/canbootloader.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y diff --git a/boards/holybro/h-flow/default.px4board b/boards/holybro/h-flow/default.px4board new file mode 100644 index 0000000000..8bc8f7f3ea --- /dev/null +++ b/boards/holybro/h-flow/default.px4board @@ -0,0 +1,31 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y +CONFIG_DRIVERS_OPTICAL_FLOW_PAA3905=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y +CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_ACCELERATION is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/holybro/h-flow/firmware.prototype b/boards/holybro/h-flow/firmware.prototype new file mode 100644 index 0000000000..eca0f111b6 --- /dev/null +++ b/boards/holybro/h-flow/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 87, + "magic": "PX4FWv1", + "description": "Firmware for the Holybro H-Flow board", + "image": "", + "build_time": 0, + "summary": "Holybro-H-Flow", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/holybro/h-flow/init/rc.board_sensors b/boards/holybro/h-flow/init/rc.board_sensors new file mode 100644 index 0000000000..0bfae26ccb --- /dev/null +++ b/boards/holybro/h-flow/init/rc.board_sensors @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ + +param set-default IMU_GYRO_RATEMAX 1000 +param set-default SENS_IMU_CLPNOTI 0 + +# SPI 1 +icm42688p -R 0 -s start + +# SPI 2 +paa3905 -s start -Y 180 + +# SPI 3 +afbrs50 start diff --git a/boards/holybro/h-flow/nuttx-config/canbootloader/defconfig b/boards/holybro/h-flow/nuttx-config/canbootloader/defconfig new file mode 100644 index 0000000000..0fce4427d2 --- /dev/null +++ b/boards/holybro/h-flow/nuttx-config/canbootloader/defconfig @@ -0,0 +1,56 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/h-flow/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INIT_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/holybro/h-flow/nuttx-config/include/board.h b/boards/holybro/h-flow/nuttx-config/include/board.h new file mode 100644 index 0000000000..0b3d23ef6e --- /dev/null +++ b/boards/holybro/h-flow/nuttx-config/include/board.h @@ -0,0 +1,146 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#include "board_dma_map.h" + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_BOARD_USEHSE 1 + +#define STM32_LSE_FREQUENCY 0 + +/* PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (16'000'000 / 16) * 384 + * = 384'000'000 + * SYSCLK = PLL_VCO / PLLP + * = 384'000'000 / 4 = 96,000,000 + */ + +/* Main PLL Configuration */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(16) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8) +#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) + +#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/ + +#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB +#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ + +#define STM32_SYSCLK_FREQUENCY 96000000ul + +/* AHB clock (HCLK) is SYSCLK (96MHz) */ +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK (96MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* Timers driven from APB2 will be PCLK2 since no prescale division */ +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */ +#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* UARTs */ +#define GPIO_USART1_RX GPIO_USART1_RX_1 +#define GPIO_USART1_TX GPIO_USART1_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_2 /* PB8 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_2 /* PB9 */ + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 /* PB13 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 /* PB5 */ +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_1 /* PB3 */ + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/holybro/h-flow/nuttx-config/include/board_dma_map.h b/boards/holybro/h-flow/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..433283656f --- /dev/null +++ b/boards/holybro/h-flow/nuttx-config/include/board_dma_map.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX // DMA1, Stream 3, Channel 0 +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA1, Stream 4, Channel 0 + +#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 // DMA1, Stream 0, Channel 0 +#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_1 // DMA1, Stream 5, Channel 0 + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_2 // DMA2, Stream 2, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 diff --git a/boards/holybro/h-flow/nuttx-config/nsh/defconfig b/boards/holybro/h-flow/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..b676f62764 --- /dev/null +++ b/boards/holybro/h-flow/nuttx-config/nsh/defconfig @@ -0,0 +1,149 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/h-flow/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_CROMFS=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2624 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_VARS=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=254 +CONFIG_SCHED_HPWORKSTACKSIZE=3000 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_WAITPID=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=2048 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI2_DMA=y +CONFIG_STM32_SPI2_DMA_BUFFER=2048 +CONFIG_STM32_SPI3=y +CONFIG_STM32_SPI3_DMA=y +CONFIG_STM32_SPI3_DMA_BUFFER=2048 +CONFIG_STM32_TIM8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/holybro/h-flow/nuttx-config/scripts/canbootloader_script.ld b/boards/holybro/h-flow/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 0000000000..6cc7c79e58 --- /dev/null +++ b/boards/holybro/h-flow/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,134 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader and params. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/holybro/h-flow/nuttx-config/scripts/script.ld b/boards/holybro/h-flow/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..3db1f5dc64 --- /dev/null +++ b/boards/holybro/h-flow/nuttx-config/scripts/script.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader and params. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 448K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/holybro/h-flow/src/CMakeLists.txt b/boards/holybro/h-flow/src/CMakeLists.txt new file mode 100644 index 0000000000..4fae41fc0e --- /dev/null +++ b/boards/holybro/h-flow/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + init.c + led.c + spi.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/holybro/h-flow/src/board_config.h b/boards/holybro/h-flow/src/board_config.h new file mode 100644 index 0000000000..2a8a0691e4 --- /dev/null +++ b/boards/holybro/h-flow/src/board_config.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PC14 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN14|GPIO_EXTI) + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB0 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_nLED_BLUE /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) + +#define BROADCOM_AFBR_S50_S2PI_SPI_BUS 3 +#define BROADCOM_AFBR_S50_S2PI_CS /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN15) +#define BROADCOM_AFBR_S50_S2PI_IRQ /* PB6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN6|GPIO_EXTI) +#define BROADCOM_AFBR_S50_S2PI_CLK /* PB3 */ GPIO_SPI3_SCK_1 +#define BROADCOM_AFBR_S50_S2PI_MOSI /* PB5 */ GPIO_SPI3_MOSI_1 +#define BROADCOM_AFBR_S50_S2PI_MISO /* PB4 */ GPIO_SPI3_MISO_1 + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +// TODO figure out +#define GPIO_GETNODEINFO_JUMPER 0 //(GPIO_BOOT_CONFIG & ~GPIO_EXTI) + +// CAN termination set by param, available from RC02 +#define GPIO_CAN1_TERMINATION /* PA12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +__BEGIN_DECLS + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/holybro/h-flow/src/boot.c b/boards/holybro/h-flow/src/boot.c new file mode 100644 index 0000000000..baad811bc0 --- /dev/null +++ b/boards/holybro/h-flow/src/boot.c @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "boot_config.h" +#include "board.h" + +#include +#include +#include + +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void bootloader_led_on(int led); +extern void bootloader_led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_SILENT_S0); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + + led_init(); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +void board_indicate(uiindication_t indication) +{ + if (indication == off) { + bootloader_led_off(GPIO_nLED_RED); + bootloader_led_off(GPIO_nLED_BLUE); + + } else if (indication == fw_update_start) { + bootloader_led_on(GPIO_nLED_RED); + bootloader_led_on(GPIO_nLED_BLUE); + + } else if ((indication == fw_update_erase_fail) || (indication == fw_update_invalid_response) + || (indication == fw_update_timeout) || (indication == fw_update_invalid_crc)) { + bootloader_led_on(GPIO_nLED_RED); + bootloader_led_off(GPIO_nLED_BLUE); + + } else if (indication == allocation_start) { + bootloader_led_on(GPIO_nLED_RED); + bootloader_led_off(GPIO_nLED_BLUE); + + } else { + bootloader_led_off(GPIO_nLED_RED); + bootloader_led_on(GPIO_nLED_BLUE); + } +} diff --git a/boards/holybro/h-flow/src/boot_config.h b/boards/holybro/h-flow/src/boot_config.h new file mode 100644 index 0000000000..e0f62e952f --- /dev/null +++ b/boards/holybro/h-flow/src/boot_config.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/* + * @file boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#define OPT_TBOOT_MS 3000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 50 +#define OPT_BL_NUMBER_TIMERS 7 + +// No GETNODEINFO +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* Bootloader Option***************************************************************** + * + */ +//#define GPIO_GETNODEINFO_JUMPER GPIO_NOPT_WAIT_FOR_GETNODEINFO diff --git a/boards/holybro/h-flow/src/can.c b/boards/holybro/h-flow/src/can.c new file mode 100644 index 0000000000..7737965dc6 --- /dev/null +++ b/boards/holybro/h-flow/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/holybro/h-flow/src/init.c b/boards/holybro/h-flow/src/init.c new file mode 100644 index 0000000000..644dfb63d7 --- /dev/null +++ b/boards/holybro/h-flow/src/init.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include +#include + +#include + +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + watchdog_init(); + + // Configure CAN interface + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + + stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); + + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + + // Configure LEDs. + board_autoled_initialize(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {2, 16 * 1024, 0x08008000}, + {3, 16 * 1024, 0x0800C000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + } + +#endif // FLASH_BASED_PARAMS + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_BLUE); + + /* Configure the HW based on the manifest */ + //px4_platform_configure(); + + return OK; +} diff --git a/boards/holybro/h-flow/src/led.c b/boards/holybro/h-flow/src/led.c new file mode 100644 index 0000000000..aa47edac93 --- /dev/null +++ b/boards/holybro/h-flow/src/led.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * board LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void bootloader_led_on(int led); +extern void bootloader_led_off(int led); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, + GPIO_nLED_RED, +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +__EXPORT void bootloader_led_on(int led) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(led, false); +} + +__EXPORT void bootloader_led_off(int led) +{ + /* Pull Up to switch on */ + stm32_gpiowrite(led, true); +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} diff --git a/boards/holybro/h-flow/src/spi.cpp b/boards/holybro/h-flow/src/spi.cpp new file mode 100644 index 0000000000..f4379998a8 --- /dev/null +++ b/boards/holybro/h-flow/src/spi.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortA, GPIO::Pin3}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_FLOW_DEVTYPE_PAA3905, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_DIST_DEVTYPE_AFBRS50, SPI::CS{GPIO::PortA, GPIO::Pin15}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/holybro/h-flow/uavcan_board_identity b/boards/holybro/h-flow/uavcan_board_identity new file mode 100644 index 0000000000..deba73df32 --- /dev/null +++ b/boards/holybro/h-flow/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 87) +set(uavcanblid_name "\"org.holybro.h-flow\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 8aaa037cc5..82a3261e87 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set @@ -40,6 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/holybro/kakutef7/src/init.c b/boards/holybro/kakutef7/src/init.c index 9b8a0140f4..e4b6bae87d 100644 --- a/boards/holybro/kakutef7/src/init.c +++ b/boards/holybro/kakutef7/src/init.c @@ -124,8 +124,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index dbf8aeb7f2..12259d0f34 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -34,10 +34,10 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set -# CONFIG_EKF2_EXTERNAL_VISION is not set # CONFIG_EKF2_GNSS_YAW is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y diff --git a/boards/holybro/kakuteh7/init/rc.board_sensors b/boards/holybro/kakuteh7/init/rc.board_sensors index eb0413343b..2d26d786e0 100644 --- a/boards/holybro/kakuteh7/init/rc.board_sensors +++ b/boards/holybro/kakuteh7/init/rc.board_sensors @@ -11,4 +11,7 @@ then icm42688p -R 6 -s start fi -bmp280 -X start +if ! bmp280 -X start +then + spa06 -X start +fi diff --git a/boards/holybro/kakuteh7/src/init.c b/boards/holybro/kakuteh7/src/init.c index cf73735aed..9c609d4486 100644 --- a/boards/holybro/kakuteh7/src/init.c +++ b/boards/holybro/kakuteh7/src/init.c @@ -129,8 +129,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index eb465e0027..b5b1ab5275 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -16,6 +16,7 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y @@ -33,10 +34,10 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set -# CONFIG_EKF2_EXTERNAL_VISION is not set # CONFIG_EKF2_GNSS_YAW is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y diff --git a/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin b/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin index 81108119f3..4efdeb5b21 100755 Binary files a/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin and b/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin differ diff --git a/boards/holybro/kakuteh7mini/firmware.prototype b/boards/holybro/kakuteh7mini/firmware.prototype index 369843379f..b0c8bf0adf 100644 --- a/boards/holybro/kakuteh7mini/firmware.prototype +++ b/boards/holybro/kakuteh7mini/firmware.prototype @@ -1,10 +1,10 @@ { - "board_id": 1054, + "board_id": 1058, "magic": "PX4FWv1", "description": "Firmware for the KakuteH7Mini board", "image": "", "build_time": 0, - "summary": "KAKUTEH7MINI-NAND", + "summary": "KAKUTEH7MINI", "version": "0.1", "image_size": 0, "image_maxsize": 1835008, diff --git a/boards/holybro/kakuteh7mini/init/rc.board_sensors b/boards/holybro/kakuteh7mini/init/rc.board_sensors index 0b58d912bb..58e58905af 100644 --- a/boards/holybro/kakuteh7mini/init/rc.board_sensors +++ b/boards/holybro/kakuteh7mini/init/rc.board_sensors @@ -2,11 +2,17 @@ # # Holybro KakuteH7Mini specific board sensors init #------------------------------------------------------------------------------ +# v1.1=mpu6000 v1.3=bmi270 v1.5=icm42688p board_adc start - -if ! bmi270 -s -q start +if ! mpu6000 -R 6 -s -q start then - icm42688p -R 0 -s start + if ! bmi270 -R 6 -s -q start + then + icm42688p -R 6 -s start + fi fi -bmp280 -X start +if ! bmp280 -X start +then + spa06 -X start +fi diff --git a/boards/holybro/kakuteh7mini/src/hw_config.h b/boards/holybro/kakuteh7mini/src/hw_config.h index 1ad290fd6c..bd7a8a6d3c 100644 --- a/boards/holybro/kakuteh7mini/src/hw_config.h +++ b/boards/holybro/kakuteh7mini/src/hw_config.h @@ -69,7 +69,7 @@ //#define USE_VBUS_PULL_DOWN #define BOOT_DELAY_ADDRESS 0x000001a0 -#define BOARD_TYPE 1054 +#define BOARD_TYPE 1058 #define BOARD_FLASH_SECTORS (14) #define BOARD_FLASH_SIZE (16 * 128 * 1024) #define APP_RESERVATION_SIZE (1 * 128 * 1024) diff --git a/boards/holybro/kakuteh7mini/src/init.c b/boards/holybro/kakuteh7mini/src/init.c index c8f19df734..4691703e8f 100644 --- a/boards/holybro/kakuteh7mini/src/init.c +++ b/boards/holybro/kakuteh7mini/src/init.c @@ -127,8 +127,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakuteh7mini/src/spi.cpp b/boards/holybro/kakuteh7mini/src/spi.cpp index 1f47a54022..462965df7e 100644 --- a/boards/holybro/kakuteh7mini/src/spi.cpp +++ b/boards/holybro/kakuteh7mini/src/spi.cpp @@ -43,9 +43,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), }), initSPIBus(SPI::Bus::SPI4, { - //= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/pix32v5/src/init.c b/boards/holybro/pix32v5/src/init.c index 2153838a60..fd2449ee7d 100644 --- a/boards/holybro/pix32v5/src/init.c +++ b/boards/holybro/pix32v5/src/init.c @@ -143,8 +143,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/matek/h743-mini/src/init.c b/boards/matek/h743-mini/src/init.c index bb1c6f975c..9ffa799caf 100644 --- a/boards/matek/h743-mini/src/init.c +++ b/boards/matek/h743-mini/src/init.c @@ -94,8 +94,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index ea9f4d4843..38a2557e2e 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_BOARD_PARAM_FILE="/fs/microsd/params" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y diff --git a/boards/matek/h743-slim/src/init.c b/boards/matek/h743-slim/src/init.c index f78e570c5a..899bb1ba4d 100644 --- a/boards/matek/h743-slim/src/init.c +++ b/boards/matek/h743-slim/src/init.c @@ -98,8 +98,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/matek/h743/src/init.c b/boards/matek/h743/src/init.c index a2a531882c..f24ad5eb7c 100644 --- a/boards/matek/h743/src/init.c +++ b/boards/matek/h743/src/init.c @@ -94,8 +94,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/micoair/h743-aio/bootloader.px4board b/boards/micoair/h743-aio/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/micoair/h743-aio/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board new file mode 100644 index 0000000000..2d92e89d96 --- /dev/null +++ b/boards/micoair/h743-aio/default.px4board @@ -0,0 +1,86 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_URT6="/dev/ttyS5" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_BOSCH_BMI270=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_PPS_CAPTURE=y +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_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_FAILURE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/micoair/h743-aio/extras/micoair_h743-aio_bootloader.bin b/boards/micoair/h743-aio/extras/micoair_h743-aio_bootloader.bin new file mode 100755 index 0000000000..362848ab80 Binary files /dev/null and b/boards/micoair/h743-aio/extras/micoair_h743-aio_bootloader.bin differ diff --git a/boards/micoair/h743-aio/firmware.prototype b/boards/micoair/h743-aio/firmware.prototype new file mode 100644 index 0000000000..c487b88912 --- /dev/null +++ b/boards/micoair/h743-aio/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1176, + "magic": "PX4FWv1", + "description": "Firmware for the MicoAir743AIO board", + "image": "", + "build_time": 0, + "summary": "MicoAir743AIO", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/micoair/h743-aio/init/rc.board_defaults b/boards/micoair/h743-aio/init/rc.board_defaults new file mode 100644 index 0000000000..5e3cd5bd60 --- /dev/null +++ b/boards/micoair/h743-aio/init/rc.board_defaults @@ -0,0 +1,24 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default BAT1_A_PER_V 14.14 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_DIV 21.12 +param set-default BAT1_V_EMPTY 3.2 + +param set-default SYS_HAS_MAG 0 +param set-default PWM_MAIN_TIM0 -4 +param set-default RC_INPUT_PROTO -1 + +param set-default IMU_GYRO_CUTOFF 100 +param set-default SYS_AUTOSTART 4001 +param set-default MC_PITCHRATE_K 0.4 +param set-default MC_ROLLRATE_K 0.35 +param set-default MC_YAWRATE_K 1.2 +param set-default MC_YAWRATE_MAX 360 +param set-default MAV_TYPE 2 +param set-default CA_AIRFRAME 0 +param set-default CA_ROTOR_COUNT 4 +param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/micoair/h743-aio/init/rc.board_extras b/boards/micoair/h743-aio/init/rc.board_extras new file mode 100644 index 0000000000..e7653077db --- /dev/null +++ b/boards/micoair/h743-aio/init/rc.board_extras @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# enable onboard OSD chip +# atxxxx start -s diff --git a/boards/micoair/h743-aio/init/rc.board_sensors b/boards/micoair/h743-aio/init/rc.board_sensors new file mode 100644 index 0000000000..baf97d5365 --- /dev/null +++ b/boards/micoair/h743-aio/init/rc.board_sensors @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# # Internal SPI bus BMI088 accel/gyro +bmi088 -s -b 2 -A -R 6 start +bmi088 -s -b 2 -G -R 6 start + +# # Internal SPI bus BMI270 accel/gyro +bmi270 -s -b 2 -R 0 start + +# Internal baro +dps310 -I start -a 118 diff --git a/boards/micoair/h743-aio/nuttx-config/bootloader/defconfig b/boards/micoair/h743-aio/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..7dc07fac8a --- /dev/null +++ b/boards/micoair/h743-aio/nuttx-config/bootloader/defconfig @@ -0,0 +1,85 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/micoair/h743-aio/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="MicoAir743AIO" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="MicoAir" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_TIM1=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/micoair/h743-aio/nuttx-config/include/board.h b/boards/micoair/h743-aio/nuttx-config/include/board.h new file mode 100644 index 0000000000..da0532da47 --- /dev/null +++ b/boards/micoair/h743-aio/nuttx-config/include/board.h @@ -0,0 +1,415 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6u/include/board.h + * + * Copyright (C) 2016-2019 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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 NuttX 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_MICOAIR743AIO_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MICOAIR743AIO_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The MicoAir743 board provides the following clock sources: + * + * X1: 8 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 64 MHz RC factory-trimmed + * HSE: 8 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 8,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (8,000,000 / 1) * 120 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(120) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 120) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(2) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 480kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 20 MHz Max for now - more reliable on some boards than 25 MHz + * 20 MHz = PLL1Q/(2*div), div = PLL1Q/(2*freq), div = 6 = 240 / 40 + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The board has two, LED_GREEN a Green LED and LED_BLUE a Blue LED, + * that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_1 /* PA2 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* SPI + * + + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + + + +/* I2C + * + + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 /* PB10 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 /* PB11 */ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11) + +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) + +#endif /*__NUTTX_CONFIG_MICOAIR743AIO_INCLUDE_BOARD_H */ diff --git a/boards/micoair/h743-aio/nuttx-config/include/board_dma_map.h b/boards/micoair/h743-aio/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..ba3f15eb97 --- /dev/null +++ b/boards/micoair/h743-aio/nuttx-config/include/board_dma_map.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */ diff --git a/boards/micoair/h743-aio/nuttx-config/nsh/defconfig b/boards/micoair/h743-aio/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..df1012959d --- /dev/null +++ b/boards/micoair/h743-aio/nuttx-config/nsh/defconfig @@ -0,0 +1,245 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PRINTF is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/micoair/h743-aio/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="MicoAir743AIO" +CONFIG_CDCACM_RXBUFSIZE=6000 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="MicoAir" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/micoair/h743-aio/nuttx-config/scripts/bootloader_script.ld b/boards/micoair/h743-aio/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..511ef26242 --- /dev/null +++ b/boards/micoair/h743-aio/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,213 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The Durandal-v1 uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 2048K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/micoair/h743-aio/nuttx-config/scripts/script.ld b/boards/micoair/h743-aio/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..1dc1a0ef97 --- /dev/null +++ b/boards/micoair/h743-aio/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/micoair/h743-aio/src/CMakeLists.txt b/boards/micoair/h743-aio/src/CMakeLists.txt new file mode 100644 index 0000000000..c47215375d --- /dev/null +++ b/boards/micoair/h743-aio/src/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2021 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/micoair/h743-aio/src/board_config.h b/boards/micoair/h743-aio/src/board_config.h new file mode 100644 index 0000000000..9db51a7db7 --- /dev/null +++ b/boards/micoair/h743-aio/src/board_config.h @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +// #define FLASH_BASED_PARAMS + + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +# define GPIO_nLED_RED /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +# define GPIO_nLED_GREEN /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) +# define GPIO_nLED_BLUE /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ +#define SYSTEM_ADC_BASE STM32_ADC1_BASE +#define ADC1_CH(n) (n) + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ +#define PX4_ADC_GPIO \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11 + + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC1 */ ADC1_CH(11) + + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) + + +/* Define Battery 1 Voltage Divider and A per V + */ + +// #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ +// #define BOARD_BATTERY1_A_PER_V (40.0f) +// #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 9 +#define DIRECT_INPUT_TIMER_CHANNELS 9 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +#define GPIO_TONE_ALARM_IDLE /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_TONE_ALARM_GPIO /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN15) + +/* USB OTG FS + * + * PA8 OTG_FS_VBUS VBUS sensing + */ + +#define GPIO_OTGFS_VBUS /* PA8 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN8) + + +/* High-resolution timer */ +#define HRT_TIMER 2 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 3 */ + + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS4" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* SD Card */ +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define FLASH_BASED_PARAMS + +#define BOARD_NUM_IO_TIMERS 4 + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/micoair/h743-aio/src/bootloader_main.c b/boards/micoair/h743-aio/src/bootloader_main.c new file mode 100644 index 0000000000..5670308a29 --- /dev/null +++ b/boards/micoair/h743-aio/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/micoair/h743-aio/src/hw_config.h b/boards/micoair/h743-aio/src/hw_config.h new file mode 100644 index 0000000000..b02994a1ea --- /dev/null +++ b/boards/micoair/h743-aio/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1176 +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (16 * 128 * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 8 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_GREEN // GREEN +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/micoair/h743-aio/src/i2c.cpp b/boards/micoair/h743-aio/src/i2c.cpp new file mode 100644 index 0000000000..1444ea1172 --- /dev/null +++ b/boards/micoair/h743-aio/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(2), +}; diff --git a/boards/micoair/h743-aio/src/init.c b/boards/micoair/h743-aio/src/init.c new file mode 100644 index 0000000000..c0e1f9776c --- /dev/null +++ b/boards/micoair/h743-aio/src/init.c @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * FMU-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + UNUSED(ms); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ + if (status >= 0) { + up_mdelay(100); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure LEDs */ + board_autoled_initialize(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure USB interfaces */ + stm32_usbinitialize(); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Need hrt running before using the ADC */ + px4_platform_init(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_BLUE); + } + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_BLUE); + return ret; + } + +#endif + +// TODO:internal flash store parameters +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_RED); + } + +#endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/micoair/h743-aio/src/led.c b/boards/micoair/h743-aio/src/led.c new file mode 100644 index 0000000000..d7794392db --- /dev/null +++ b/boards/micoair/h743-aio/src/led.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} diff --git a/boards/micoair/h743-aio/src/sdio.c b/boards/micoair/h743-aio/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/micoair/h743-aio/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/micoair/h743-aio/src/spi.cpp b/boards/micoair/h743-aio/src/spi.cpp new file mode 100644 index 0000000000..230b54bde7 --- /dev/null +++ b/boards/micoair/h743-aio/src/spi.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 +#include +#include + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin5}, SPI::DRDY{GPIO::PortD, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin2}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/micoair/h743-aio/src/timer_config.cpp b/boards/micoair/h743-aio/src/timer_config.cpp new file mode 100644 index 0000000000..b2bca6331f --- /dev/null +++ b/boards/micoair/h743-aio/src/timer_config.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/micoair/h743-aio/src/usb.c b/boards/micoair/h743-aio/src/usb.c new file mode 100644 index 0000000000..9591784866 --- /dev/null +++ b/boards/micoair/h743-aio/src/usb.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +#include "board_config.h" +#include +#include +#include +#include + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/micoair/h743-v2/bootloader.px4board b/boards/micoair/h743-v2/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/micoair/h743-v2/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board new file mode 100644 index 0000000000..74f1e2de29 --- /dev/null +++ b/boards/micoair/h743-v2/default.px4board @@ -0,0 +1,87 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_URT6="/dev/ttyS6" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS7" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_BOSCH_BMI270=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_PPS_CAPTURE=y +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_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_FAILURE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/micoair/h743-v2/extras/micoair_h743-v2_bootloader.bin b/boards/micoair/h743-v2/extras/micoair_h743-v2_bootloader.bin new file mode 100755 index 0000000000..678f208420 Binary files /dev/null and b/boards/micoair/h743-v2/extras/micoair_h743-v2_bootloader.bin differ diff --git a/boards/micoair/h743-v2/firmware.prototype b/boards/micoair/h743-v2/firmware.prototype new file mode 100644 index 0000000000..3f1a41def8 --- /dev/null +++ b/boards/micoair/h743-v2/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1179, + "magic": "PX4FWv1", + "description": "Firmware for the MicoAir743v2 board", + "image": "", + "build_time": 0, + "summary": "MicoAir743v2", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/micoair/h743-v2/init/rc.board_defaults b/boards/micoair/h743-v2/init/rc.board_defaults new file mode 100644 index 0000000000..2115bc8848 --- /dev/null +++ b/boards/micoair/h743-v2/init/rc.board_defaults @@ -0,0 +1,27 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default BAT1_A_PER_V 40 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_DIV 21.12 +param set-default BAT1_V_EMPTY 3.2 + +param set-default SYS_HAS_MAG 1 +param set-default PWM_MAIN_TIM0 -4 +param set-default RC_INPUT_PROTO -1 + +param set-default MAV_2_CONFIG 104 +param set-default SER_TEL4_BAUD 115200 + +param set-default IMU_GYRO_CUTOFF 80 +param set-default SYS_AUTOSTART 4001 +param set-default MC_PITCHRATE_K 0.4 +param set-default MC_ROLLRATE_K 0.35 +param set-default MC_YAWRATE_K 1.2 +param set-default MC_YAWRATE_MAX 360 +param set-default MAV_TYPE 2 +param set-default CA_AIRFRAME 0 +param set-default CA_ROTOR_COUNT 4 +param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/micoair/h743-v2/init/rc.board_extras b/boards/micoair/h743-v2/init/rc.board_extras new file mode 100644 index 0000000000..e7653077db --- /dev/null +++ b/boards/micoair/h743-v2/init/rc.board_extras @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# enable onboard OSD chip +# atxxxx start -s diff --git a/boards/micoair/h743-v2/init/rc.board_sensors b/boards/micoair/h743-v2/init/rc.board_sensors new file mode 100644 index 0000000000..011c223ef7 --- /dev/null +++ b/boards/micoair/h743-v2/init/rc.board_sensors @@ -0,0 +1,19 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# # Internal SPI bus BMI088 accel/gyro +bmi088 -s -b 2 -A -R 6 start +bmi088 -s -b 2 -G -R 6 start + +# # Internal SPI bus BMI270 accel/gyro +bmi270 -s -b 3 -R 0 start + +# Internal baro +spl06 -I -a 0x77 start + +# External mag +qmc5883l -I -a 0x0D -R 4 start diff --git a/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig b/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..10f43ff17d --- /dev/null +++ b/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig @@ -0,0 +1,85 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/micoair/h743-v2/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="MicoAir743v2" +CONFIG_CDCACM_RXBUFSIZE=6000 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="MicoAir" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_TIM1=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/micoair/h743-v2/nuttx-config/include/board.h b/boards/micoair/h743-v2/nuttx-config/include/board.h new file mode 100644 index 0000000000..433aa04a54 --- /dev/null +++ b/boards/micoair/h743-v2/nuttx-config/include/board.h @@ -0,0 +1,418 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6u/include/board.h + * + * Copyright (C) 2016-2019 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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 NuttX 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_MICOAIR743V2_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MICOAIR743V2_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The MicoAir743 board provides the following clock sources: + * + * X1: 8 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 64 MHz RC factory-trimmed + * HSE: 8 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 8,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (8,000,000 / 1) * 120 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(120) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 120) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(2) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 480kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 20 MHz Max for now - more reliable on some boards than 25 MHz + * 20 MHz = PLL1Q/(2*div), div = PLL1Q/(2*freq), div = 6 = 240 / 40 + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The board has two, LED_GREEN a Green LED and LED_BLUE a Blue LED, + * that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_1 /* PA2 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_2 /* PB5 */ +#define GPIO_UART5_TX GPIO_UART5_TX_2 /* PB6 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* SPI + * + + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 /* PD6 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_1) /* PB3 */ + +/* I2C + * + + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 /* PB10 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 /* PB11 */ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11) + +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) + +#endif /*__NUTTX_CONFIG_MICOAIR743V2_INCLUDE_BOARD_H */ diff --git a/boards/micoair/h743-v2/nuttx-config/include/board_dma_map.h b/boards/micoair/h743-v2/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..c56138f0dc --- /dev/null +++ b/boards/micoair/h743-v2/nuttx-config/include/board_dma_map.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 +#define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_0 +#define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_0 diff --git a/boards/micoair/h743-v2/nuttx-config/nsh/defconfig b/boards/micoair/h743-v2/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..dab70e35fa --- /dev/null +++ b/boards/micoair/h743-v2/nuttx-config/nsh/defconfig @@ -0,0 +1,246 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PRINTF is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/micoair/h743-v2/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="MicoAir743v2" +CONFIG_CDCACM_RXBUFSIZE=6000 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="MicoAir" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI3_DMA=y +CONFIG_STM32H7_SPI3_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=800 +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_TXBUFSIZE=800 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=800 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=800 +CONFIG_USART1_RXBUFSIZE=1200 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=800 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=800 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=800 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/micoair/h743-v2/nuttx-config/scripts/bootloader_script.ld b/boards/micoair/h743-v2/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..511ef26242 --- /dev/null +++ b/boards/micoair/h743-v2/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,213 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The Durandal-v1 uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 2048K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/micoair/h743-v2/nuttx-config/scripts/script.ld b/boards/micoair/h743-v2/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..1dc1a0ef97 --- /dev/null +++ b/boards/micoair/h743-v2/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/micoair/h743-v2/src/CMakeLists.txt b/boards/micoair/h743-v2/src/CMakeLists.txt new file mode 100644 index 0000000000..c47215375d --- /dev/null +++ b/boards/micoair/h743-v2/src/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2021 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/micoair/h743-v2/src/board_config.h b/boards/micoair/h743-v2/src/board_config.h new file mode 100644 index 0000000000..97f36585ad --- /dev/null +++ b/boards/micoair/h743-v2/src/board_config.h @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +// #define FLASH_BASED_PARAMS + + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +# define GPIO_nLED_RED /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +# define GPIO_nLED_GREEN /* PE2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +# define GPIO_nLED_BLUE /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ +#define SYSTEM_ADC_BASE STM32_ADC1_BASE +#define ADC1_CH(n) (n) + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ +#define PX4_ADC_GPIO \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11 + + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC1 */ ADC1_CH(11) + + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) + + +/* Define Battery 1 Voltage Divider and A per V + */ + +// #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ +// #define BOARD_BATTERY1_A_PER_V (40.0f) +// #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 10 +#define DIRECT_INPUT_TIMER_CHANNELS 10 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define GPIO_TONE_ALARM_IDLE /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_TONE_ALARM_GPIO /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN15) + +/* USB OTG FS + * + * PA8 OTG_FS_VBUS VBUS sensing + */ + +#define GPIO_OTGFS_VBUS /* PA8 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN8) + + +/* High-resolution timer */ +#define HRT_TIMER 2 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 3 */ + + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS5" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +#define GPIO_I2C2_DRDY1_SPL06 /* PD0 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) + + +/* SD Card */ +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define FLASH_BASED_PARAMS + +#define BOARD_NUM_IO_TIMERS 5 + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/micoair/h743-v2/src/bootloader_main.c b/boards/micoair/h743-v2/src/bootloader_main.c new file mode 100644 index 0000000000..5670308a29 --- /dev/null +++ b/boards/micoair/h743-v2/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/micoair/h743-v2/src/hw_config.h b/boards/micoair/h743-v2/src/hw_config.h new file mode 100644 index 0000000000..a74346eddb --- /dev/null +++ b/boards/micoair/h743-v2/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1179 +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (16 * 128 * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 8 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_GREEN // GREEN +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/micoair/h743-v2/src/i2c.cpp b/boards/micoair/h743-v2/src/i2c.cpp new file mode 100644 index 0000000000..1444ea1172 --- /dev/null +++ b/boards/micoair/h743-v2/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(2), +}; diff --git a/boards/micoair/h743-v2/src/init.c b/boards/micoair/h743-v2/src/init.c new file mode 100644 index 0000000000..c0e1f9776c --- /dev/null +++ b/boards/micoair/h743-v2/src/init.c @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * FMU-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + UNUSED(ms); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ + if (status >= 0) { + up_mdelay(100); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure LEDs */ + board_autoled_initialize(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure USB interfaces */ + stm32_usbinitialize(); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Need hrt running before using the ADC */ + px4_platform_init(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_BLUE); + } + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_BLUE); + return ret; + } + +#endif + +// TODO:internal flash store parameters +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_RED); + } + +#endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/micoair/h743-v2/src/led.c b/boards/micoair/h743-v2/src/led.c new file mode 100644 index 0000000000..d7794392db --- /dev/null +++ b/boards/micoair/h743-v2/src/led.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} diff --git a/boards/micoair/h743-v2/src/sdio.c b/boards/micoair/h743-v2/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/micoair/h743-v2/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/micoair/h743-v2/src/spi.cpp b/boards/micoair/h743-v2/src/spi.cpp new file mode 100644 index 0000000000..d8876bc186 --- /dev/null +++ b/boards/micoair/h743-v2/src/spi.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 +#include +#include + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin5}, SPI::DRDY{GPIO::PortC, GPIO::Pin15}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin4}, SPI::DRDY{GPIO::PortC, GPIO::Pin14}), + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin7}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/micoair/h743-v2/src/timer_config.cpp b/boards/micoair/h743-v2/src/timer_config.cpp new file mode 100644 index 0000000000..be33feed1c --- /dev/null +++ b/boards/micoair/h743-v2/src/timer_config.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer15), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/micoair/h743-v2/src/usb.c b/boards/micoair/h743-v2/src/usb.c new file mode 100644 index 0000000000..9591784866 --- /dev/null +++ b/boards/micoair/h743-v2/src/usb.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +#include "board_config.h" +#include +#include +#include +#include + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index 98d8f08aaf..149bcaff21 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -9,13 +9,17 @@ CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_DRIVERS_OSD_ATXXXX=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y @@ -24,7 +28,6 @@ CONFIG_DRIVERS_TAP_ESC=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_INTERFACES=1 -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y @@ -55,7 +58,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y @@ -73,8 +75,6 @@ CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SD_STRESS=y -CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y -CONFIG_SYSTEMCMDS_SERIAL_TEST=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y diff --git a/boards/micoair/h743/init/rc.board_defaults b/boards/micoair/h743/init/rc.board_defaults index c915812e00..55b69959dc 100644 --- a/boards/micoair/h743/init/rc.board_defaults +++ b/boards/micoair/h743/init/rc.board_defaults @@ -12,7 +12,7 @@ param set-default SYS_HAS_MAG 1 param set-default PWM_MAIN_TIM0 -4 param set-default RC_INPUT_PROTO -1 -param set-default IMU_GYRO_RATEMAX 2000 +param set-default IMU_GYRO_CUTOFF 80 param set-default SYS_AUTOSTART 4001 param set-default MC_PITCHRATE_K 0.4 param set-default MC_ROLLRATE_K 0.35 diff --git a/boards/micoair/h743/init/rc.board_extras b/boards/micoair/h743/init/rc.board_extras index 549676dd7d..e7653077db 100644 --- a/boards/micoair/h743/init/rc.board_extras +++ b/boards/micoair/h743/init/rc.board_extras @@ -4,4 +4,4 @@ #------------------------------------------------------------------------------ # enable onboard OSD chip -atxxxx start -s +# atxxxx start -s diff --git a/boards/micoair/h743/nuttx-config/include/board.h b/boards/micoair/h743/nuttx-config/include/board.h index 9c30129d38..363b3a5a58 100644 --- a/boards/micoair/h743/nuttx-config/include/board.h +++ b/boards/micoair/h743/nuttx-config/include/board.h @@ -391,9 +391,9 @@ #define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ -#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +//#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +//#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +//#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ #define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ diff --git a/boards/micoair/h743/nuttx-config/nsh/defconfig b/boards/micoair/h743/nuttx-config/nsh/defconfig index 9e4db72b90..be4f3e1e5f 100644 --- a/boards/micoair/h743/nuttx-config/nsh/defconfig +++ b/boards/micoair/h743/nuttx-config/nsh/defconfig @@ -74,9 +74,10 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0036 CONFIG_CDCACM_PRODUCTSTR="MicoAir743" -CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_RXBUFSIZE=6000 CONFIG_CDCACM_TXBUFSIZE=12000 CONFIG_CDCACM_VENDORID=0x1B8C CONFIG_CDCACM_VENDORSTR="MicoAir" @@ -194,7 +195,6 @@ CONFIG_STM32H7_SAVE_CRASHDUMP=y CONFIG_STM32H7_SDMMC1=y CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32H7_SPI1=y CONFIG_STM32H7_SPI2=y CONFIG_STM32H7_SPI2_DMA=y CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 diff --git a/boards/micoair/h743/src/init.c b/boards/micoair/h743/src/init.c index 43f86f902d..c0e1f9776c 100644 --- a/boards/micoair/h743/src/init.c +++ b/boards/micoair/h743/src/init.c @@ -98,8 +98,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/micoair/h743/src/spi.cpp b/boards/micoair/h743/src/spi.cpp index 81fdd03f28..2e0916973e 100644 --- a/boards/micoair/h743/src/spi.cpp +++ b/boards/micoair/h743/src/spi.cpp @@ -37,9 +37,6 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), - }), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin5}), initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin4}), diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index a04883ea7a..c5f22053ec 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/modalai/fc-v1/src/init.c b/boards/modalai/fc-v1/src/init.c index beee10b875..254d5b3397 100644 --- a/boards/modalai/fc-v1/src/init.c +++ b/boards/modalai/fc-v1/src/init.c @@ -141,8 +141,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/modalai/fc-v2/src/init.c b/boards/modalai/fc-v2/src/init.c index dbc6ec4637..651c301072 100644 --- a/boards/modalai/fc-v2/src/init.c +++ b/boards/modalai/fc-v2/src/init.c @@ -139,8 +139,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index db58d4e28a..1aec34124a 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -13,6 +13,7 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y CONFIG_DRIVERS_RC_CRSF_RC=y +CONFIG_DRIVERS_VOXL2_IO=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_EKF2=y diff --git a/boards/modalai/voxl2/libfc-sensor-api b/boards/modalai/voxl2/libfc-sensor-api index ca16e99074..85151aaf6b 160000 --- a/boards/modalai/voxl2/libfc-sensor-api +++ b/boards/modalai/voxl2/libfc-sensor-api @@ -1 +1 @@ -Subproject commit ca16e99074641a10d153961291243ede7720c2e2 +Subproject commit 85151aaf6ba8b24ce82b387e088452c63f7e2096 diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index bf1b62e981..514fc026bb 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -74,49 +74,49 @@ print_config_settings(){ while getopts "bcdhfmorwz" flag do case "${flag}" in - b) + b) echo "[INFO] Holybro GPS selected" GPS=HOLYBRO ;; - c) + c) echo "[INFO] Wiping old config file" if [ -f "$CONFIG_FILE" ]; then rm -rf ${CONFIG_FILE} fi exit 0 ;; - d) + d) echo "[INFO] Disabling daemon mode" DAEMON_MODE=DISABLE ;; - h) + h) print_usage ;; - f) + f) echo "[INFO] Setting RC to FAKE_RC_INPUT" RC=FAKE_RC_INPUT ;; - m) + m) echo "[INFO] Matek GPS selected" GPS=MATEK ;; - o) + o) echo "[INFO] OSD module selected" OSD=ENABLE ;; - r) + r) echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected" RC=CRSF_MAV ;; - w) + w) echo "[INFO] TBS Crossfire RC receiver, raw selected" RC=CRSF_RAW ;; - z) + z) echo "[INFO] Fake sensor calibration values selected" SENSOR_CAL=FAKE ;; - *) + *) print_usage ;; esac @@ -130,9 +130,9 @@ else fi if [ $SENSOR_CAL == "FAKE" ]; then - /bin/echo "[INFO] Setting up fake sensor calibration values" - px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config - /bin/sync + /bin/echo "[INFO] Setting up fake sensor calibration values" + px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config + /bin/sync fi print_config_settings diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start index 9dcc4bb4f0..3f8bcdd503 100755 --- a/boards/modalai/voxl2/target/voxl-px4-hitl-start +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start @@ -35,9 +35,9 @@ fi /bin/sleep 1 if [ ! -f /data/px4/param/hitl_parameters ]; then - echo "[INFO] Setting default parameters for PX4 on voxl" + echo "[INFO] Setting default parameters for PX4 on voxl" . /etc/modalai/voxl-px4-hitl-set-default-parameters.config - /bin/sync + /bin/sync else param select /data/px4/param/hitl_parameters param load diff --git a/boards/modalai/voxl2/target/voxl-px4-start b/boards/modalai/voxl2/target/voxl-px4-start index 014d046c8b..66dfa194e5 100755 --- a/boards/modalai/voxl2/target/voxl-px4-start +++ b/boards/modalai/voxl2/target/voxl-px4-start @@ -13,7 +13,7 @@ echo "OSD: $OSD" echo "EXTRA STEPS:" for i in "${EXTRA_STEPS[@]}" do - echo -e "\t$i" + echo -e "\t$i" done echo -e "*************************\n" @@ -83,17 +83,17 @@ qshell ist8310 start -R 10 -X -b 1 # GPS and magnetometer if [ "$GPS" != "NONE" ]; then - # On M0052 the GPS driver runs on the apps processor - if [ $PLATFORM = "M0052" ]; then - gps start -d /dev/ttyHS2 - # On M0054 and M0104 the GPS driver runs on SLPI DSP - else - qshell gps start -d 6 - fi + # On M0052 the GPS driver runs on the apps processor + if [ $PLATFORM = "M0052" ]; then + gps start -d /dev/ttyHS2 + # On M0054 and M0104 the GPS driver runs on SLPI DSP + else + qshell gps start -d 6 + fi fi # Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will -# fail but not cause any harm. +# fail but not cause any harm. /bin/echo "Looking for ncp5623c RGB LED" qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56 @@ -107,17 +107,17 @@ param touch SYS_AUTOSTART # ESC driver if [ "$ESC" == "VOXL_ESC" ]; then /bin/echo "Starting VOXL ESC driver" - qshell voxl_esc start + qshell voxl_esc start elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then - if [ "$RC" == "M0065_SBUS" ]; then - /bin/echo "Starting VOXL IO for PWM ESC with SBUS RC" - qshell voxl2_io start - else - /bin/echo "Starting VOXL IO for PWM ESC without SBUS RC" - qshell voxl2_io start -e - fi + if [ "$RC" == "M0065_SBUS" ]; then + /bin/echo "Starting VOXL IO for PWM ESC with SBUS RC" + qshell voxl2_io start + else + /bin/echo "Starting VOXL IO for PWM ESC without SBUS RC" + qshell voxl2_io start -e + fi else - /bin/echo "No ESC type specified, not starting an ESC driver" + /bin/echo "No ESC type specified, not starting an ESC driver" fi @@ -133,41 +133,41 @@ elif [ "$RC" == "CRSF_MAV" ]; then qshell mavlink_rc_in start -m -p 7 -b 115200 elif [ "$RC" == "SPEKTRUM" ]; then /bin/echo "Starting Spektrum RC" - # On M0052 the RC driver runs on the apps processor - if [ $PLATFORM = "M0052" ]; then + # On M0052 the RC driver runs on the apps processor + if [ $PLATFORM = "M0052" ]; then rc_input start -d /dev/ttyHS1 - # On M0054 and M0104 the RC driver runs on SLPI DSP - else + # On M0054 and M0104 the RC driver runs on SLPI DSP + else qshell spektrum_rc start - fi + fi elif [ "$RC" == "GHST" ]; then /bin/echo "Starting GHST RC driver" qshell ghst_rc start -d 7 elif [ "$RC" == "M0065_SBUS" ]; then - if [ $PLATFORM = "M0052" ]; then - apps_sbus start - elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then - /bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW" - qshell dsp_sbus start - retVal=$? - if [ $retVal -ne 0 ]; then - /bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed" - /bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW" - qshell voxl2_io start -d -p 7 - fi - else - /bin/echo "M0065 SBUS RC driver already started with PWM ESC start" - fi + if [ $PLATFORM = "M0052" ]; then + apps_sbus start + elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then + /bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW" + qshell dsp_sbus start + retVal=$? + if [ $retVal -ne 0 ]; then + /bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed" + /bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW" + qshell voxl2_io start -d -p 7 + fi + else + /bin/echo "M0065 SBUS RC driver already started with PWM ESC start" + fi fi if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then - # Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor - qshell lightware_laser_serial start -d 7 + # Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor + qshell lightware_laser_serial start -d 7 fi if [ "$POWER_MANAGER" == "VOXLPM" ]; then - # APM power monitor - qshell voxlpm start -X -b 2 + # APM power monitor + qshell voxlpm start -X -b 2 fi # Optional distance sensor on spare i2c @@ -191,7 +191,7 @@ qshell load_mon start # is publishing input_rc topics. Otherwise for external RC # over Mavlink this isn't needed. if [ "$RC" != "EXTERNAL" ]; then - qshell rc_update start + qshell rc_update start fi qshell commander start @@ -214,7 +214,7 @@ voxl_save_cal_params start # On M0052 there is only one IMU. So, PX4 needs to # publish IMU samples externally for VIO to use. if [ $PLATFORM = "M0052" ]; then - imu_server start + imu_server start fi # start the onboard fast link to connect to voxl-mavlink-server @@ -250,5 +250,5 @@ fi # Start optional EXTRA_STEPS for i in "${EXTRA_STEPS[@]}" do - $i + $i done diff --git a/boards/mro/ctrl-zero-classic/src/init.c b/boards/mro/ctrl-zero-classic/src/init.c index daaace48fb..0081dc62dd 100644 --- a/boards/mro/ctrl-zero-classic/src/init.c +++ b/boards/mro/ctrl-zero-classic/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-f7-oem/src/init.c b/boards/mro/ctrl-zero-f7-oem/src/init.c index 0b0b07ff12..af04388898 100644 --- a/boards/mro/ctrl-zero-f7-oem/src/init.c +++ b/boards/mro/ctrl-zero-f7-oem/src/init.c @@ -132,8 +132,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-f7/src/init.c b/boards/mro/ctrl-zero-f7/src/init.c index 0b0b07ff12..af04388898 100644 --- a/boards/mro/ctrl-zero-f7/src/init.c +++ b/boards/mro/ctrl-zero-f7/src/init.c @@ -132,8 +132,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-h7-oem/src/init.c b/boards/mro/ctrl-zero-h7-oem/src/init.c index daaace48fb..0081dc62dd 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/init.c +++ b/boards/mro/ctrl-zero-h7-oem/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-h7/src/init.c b/boards/mro/ctrl-zero-h7/src/init.c index daaace48fb..0081dc62dd 100644 --- a/boards/mro/ctrl-zero-h7/src/init.c +++ b/boards/mro/ctrl-zero-h7/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/pixracerpro/src/init.c b/boards/mro/pixracerpro/src/init.c index 481c35c336..6fe192857e 100644 --- a/boards/mro/pixracerpro/src/init.c +++ b/boards/mro/pixracerpro/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index 361aca4484..cec5dd074a 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -67,7 +67,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/boards/mro/x21-777/src/init.c b/boards/mro/x21-777/src/init.c index 3f5f00e377..dd6edda517 100644 --- a/boards/mro/x21-777/src/init.c +++ b/boards/mro/x21-777/src/init.c @@ -106,13 +106,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } - /** - * On resets invoked from system (not boot) insure we establish a low - * output state (discharge the pins) on PWM pins before they become inputs. + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. */ - if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/nxp/fmuk66-e/src/init.c b/boards/nxp/fmuk66-e/src/init.c index affa94492e..e8580466b2 100644 --- a/boards/nxp/fmuk66-e/src/init.c +++ b/boards/nxp/fmuk66-e/src/init.c @@ -120,8 +120,13 @@ void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/nxp/fmuk66-v3/src/init.c b/boards/nxp/fmuk66-v3/src/init.c index 57e8633459..d5bc68d52e 100644 --- a/boards/nxp/fmuk66-v3/src/init.c +++ b/boards/nxp/fmuk66-v3/src/init.c @@ -120,8 +120,13 @@ void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/nxp/tropic-community/cmake/upload.cmake b/boards/nxp/tropic-community/cmake/upload.cmake new file mode 100644 index 0000000000..79da5c60fd --- /dev/null +++ b/boards/nxp/tropic-community/cmake/upload.cmake @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.hex) + +add_custom_target(upload_teensy + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/teensy_uploader.py --port ${serial_ports} ${PX4_FW_NAME} --vendor-id 0x1FC9 --product-id 0x0024 + DEPENDS ${PX4_FW_NAME} + COMMENT "uploading px4" + VERBATIM + USES_TERMINAL + WORKING_DIRECTORY ${PX4_BINARY_DIR} +) diff --git a/boards/nxp/tropic-community/default.px4board b/boards/nxp/tropic-community/default.px4board new file mode 100644 index 0000000000..c7fcb205e0 --- /dev/null +++ b/boards/nxp/tropic-community/default.px4board @@ -0,0 +1,99 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_PARAM_FILE="/fs/nor/mtd_params" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_COMMON_UWB=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_I2C_LAUNCHER=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/nxp/tropic-community/firmware.prototype b/boards/nxp/tropic-community/firmware.prototype new file mode 100644 index 0000000000..3e156fbd48 --- /dev/null +++ b/boards/nxp/tropic-community/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 36, + "magic": "PX4FWv1", + "description": "Firmware for the TROPIC Community board", + "image": "", + "build_time": 0, + "summary": "TROPIC Community", + "version": "0.1", + "image_size": 0, + "image_maxsize": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/nxp/tropic-community/init/rc.board_defaults b/boards/nxp/tropic-community/init/rc.board_defaults new file mode 100644 index 0000000000..8a49eca594 --- /dev/null +++ b/boards/nxp/tropic-community/init/rc.board_defaults @@ -0,0 +1,34 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# Mavlink ethernet (CFG 1000) +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_BROADCAST 1 +param set-default MAV_2_MODE 0 +param set-default MAV_2_RADIO_CTL 0 +param set-default MAV_2_RATE 100000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP_PRT 14550 + +param set-default SENS_EN_INA238 0 +param set-default SENS_EN_INA228 0 +param set-default SENS_EN_INA226 0 + +param set-default BAT1_V_DIV 18.000000000 +param set-default BAT1_A_PER_V 38.462030303 + +if [ -f "/fs/microsd/ipcfg-eth0" ] +then +else + netman update -i eth0 +fi + +rgbled_pwm start +safety_button start + +if param greater -s UAVCAN_ENABLE 0 +then + ifup can0 +fi diff --git a/boards/nxp/tropic-community/init/rc.board_mavlink b/boards/nxp/tropic-community/init/rc.board_mavlink new file mode 100644 index 0000000000..d789d908e8 --- /dev/null +++ b/boards/nxp/tropic-community/init/rc.board_mavlink @@ -0,0 +1,4 @@ +#!/bin/sh +# +# Tropic Community specific board MAVLink startup script. +#------------------------------------------------------------------------------ diff --git a/boards/nxp/tropic-community/init/rc.board_sensors b/boards/nxp/tropic-community/init/rc.board_sensors new file mode 100644 index 0000000000..f6bcb1a8fa --- /dev/null +++ b/boards/nxp/tropic-community/init/rc.board_sensors @@ -0,0 +1,74 @@ +#!/bin/sh +# +# PX4 board sensors init +#------------------------------------------------------------------------------ +# +# UART mapping on Tropic Community: +# +# LPUART5 /dev/ttyS0 CONSOLE +# LPUART3 /dev/ttyS1 GPS +# LPUART2 /dev/ttyS2 TELEM1 +# LPUART4 /dev/ttyS3 TELEM2 +# LPUART8 /dev/ttyS4 RC +# +#------------------------------------------------------------------------------ + +set INA_CONFIGURED no + +if param compare -s ADC_ADS1115_EN 1 +then + ads1115 start -X +else + board_adc start +fi + + +if param compare SENS_EN_INA226 1 +then + # Start Digital power monitors + ina226 -X -b 1 -t 1 -k start + set INA_CONFIGURED yes +fi + +if param compare SENS_EN_INA228 1 +then + # Start Digital power monitors + ina228 -X -b 1 -t 1 -k start + set INA_CONFIGURED yes +fi + +if param compare SENS_EN_INA238 1 +then + # Start Digital power monitors + ina238 -X -b 1 -t 1 -k start + set INA_CONFIGURED yes +fi + +if [ $INA_CONFIGURED = no ] +then + # INA226, INA228, INA238 auto-start + i2c_launcher start -b 1 +fi + +# Internal SPI bus ICM42688p +icm42688p -R 2 -b 3 -s start + +# Internal on IMU SPI BMI088 +bmi088 -A -R 2 -b 4 -s start +bmi088 -G -R 2 -b 4 -s start + +# Internal magnetometer on I2c +bmm150 -I -b 4 -R 6 -a 18 start + +# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start + +# Possible internal Baro + +# Disable startup of internal baros if param is set to false +if param compare SENS_INT_BARO_EN 1 +then + bmp388 -I -b 4 start +fi + +unset INA_CONFIGURED diff --git a/boards/nxp/tropic-community/nuttx-config/Kconfig b/boards/nxp/tropic-community/nuttx-config/Kconfig new file mode 100644 index 0000000000..f72f3c094c --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/Kconfig @@ -0,0 +1,4 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# diff --git a/boards/nxp/tropic-community/nuttx-config/include/board.h b/boards/nxp/tropic-community/nuttx-config/include/board.h new file mode 100644 index 0000000000..21ded1ea38 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/include/board.h @@ -0,0 +1,391 @@ +/************************************************************************************ + * nuttx-configs/nxp/nxp_tropic-community/include/board.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __NUTTX_CONFIG_NXP_TROPIC_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_NXP_TROPIC_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* Set VDD_SOC to 1.25V */ + +#define IMXRT_VDD_SOC (0x12) + +/* Set Arm PLL (PLL1) to fOut = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR + * 576Mhz = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR + * ARM_PLL_DIV_SELECT = 96 + * ARM_PODF_DIVISOR = 2 + * 576Mhz = (24Mhz * 96/2) / 2 + * + * AHB_CLOCK_ROOT = PLL1fOut / IMXRT_AHB_PODF_DIVIDER + * 1Hz to 600 Mhz = 576Mhz / IMXRT_ARM_CLOCK_DIVIDER + * IMXRT_ARM_CLOCK_DIVIDER = 1 + * 576Mhz = 576Mhz / 1 + * + * PRE_PERIPH_CLK_SEL = PRE_PERIPH_CLK_SEL_PLL1 + * PERIPH_CLK_SEL = 1 (0 select PERIPH_CLK2_PODF, 1 select PRE_PERIPH_CLK_SEL_PLL1) + * PERIPH_CLK = 576Mhz + * + * IPG_CLOCK_ROOT = AHB_CLOCK_ROOT / IMXRT_IPG_PODF_DIVIDER + * IMXRT_IPG_PODF_DIVIDER = 4 + * 144Mhz = 576Mhz / 4 + * + * PRECLK_CLOCK_ROOT = IPG_CLOCK_ROOT / IMXRT_PERCLK_PODF_DIVIDER + * IMXRT_PERCLK_PODF_DIVIDER = 1 + * 16Mhz = 144Mhz / 9 + * + * SEMC_CLK_ROOT = 576Mhz / IMXRT_SEMC_PODF_DIVIDER (labeled AIX_PODF in 18.2) + * IMXRT_SEMC_PODF_DIVIDER = 8 + * 72Mhz = 576Mhz / 8 + * + * Set Sys PLL (PLL2) to fOut = (24Mhz * (20+(2*(DIV_SELECT))) + * 528Mhz = (24Mhz * (20+(2*(1))) + * + * Set USB1 PLL (PLL3) to fOut = (24Mhz * 20) + * 480Mhz = (24Mhz * 20) + * + * Set LPSPI PLL3 PFD0 to fOut = (480Mhz / 12 * 18) + * 720Mhz = (480Mhz / 12 * 18) + * 90Mhz = (720Mhz / LSPI_PODF_DIVIDER) + * + * Set LPI2C PLL3 / 8 to fOut = (480Mhz / 8) + * 60Mhz = (480Mhz / 8) + * 12Mhz = (60Mhz / LSPI_PODF_DIVIDER) + * + * Set USDHC1 PLL2 PFD2 to fOut = (528Mhz / 24 * 18) + * 396Mhz = (528Mhz / 24 * 18) + * 198Mhz = (396Mhz / IMXRT_USDHC1_PODF_DIVIDER) + */ + +#define BOARD_XTAL_FREQUENCY 24000000 +#define IMXRT_PRE_PERIPH_CLK_SEL CCM_CBCMR_PRE_PERIPH_CLK_SEL_PLL1 +#define IMXRT_PERIPH_CLK_SEL CCM_CBCDR_PERIPH_CLK_SEL_PRE_PERIPH +#define IMXRT_ARM_PLL_DIV_SELECT 96 +#define IMXRT_ARM_PODF_DIVIDER 2 +#define IMXRT_AHB_PODF_DIVIDER 1 +#define IMXRT_IPG_PODF_DIVIDER 4 +#define IMXRT_PERCLK_CLK_SEL CCM_CSCMR1_PERCLK_CLK_SEL_IPG_CLK_ROOT +#define IMXRT_PERCLK_PODF_DIVIDER 9 +#define IMXRT_SEMC_PODF_DIVIDER 8 +#define IMXRT_CAN_CLK_SELECT CCM_CSCMR2_CAN_CLK_SEL_PLL3_SW_80 +#define IMXRT_CAN_PODF_DIVIDER 1 + +#define IMXRT_LPSPI_CLK_SELECT CCM_CBCMR_LPSPI_CLK_SEL_PLL3_PFD0 +#define IMXRT_LSPI_PODF_DIVIDER 8 + +#define IMXRT_LPI2C_CLK_SELECT CCM_CSCDR2_LPI2C_CLK_SEL_PLL3_60M +#define IMXRT_LSI2C_PODF_DIVIDER 5 + +#define IMXRT_USDHC1_CLK_SELECT CCM_CSCMR1_USDHC1_CLK_SEL_PLL2_PFD0 +#define IMXRT_USDHC1_PODF_DIVIDER 2 + +#define IMXRT_USB1_PLL_DIV_SELECT CCM_ANALOG_PLL_USB1_DIV_SELECT_20 + +#define IMXRT_SYS_PLL_SELECT CCM_ANALOG_PLL_SYS_DIV_SELECT_22 + +#define IMXRT_USB1_PLL_DIV_SELECT CCM_ANALOG_PLL_USB1_DIV_SELECT_20 + +#define BOARD_CPU_FREQUENCY \ + (BOARD_XTAL_FREQUENCY * (IMXRT_ARM_PLL_DIV_SELECT / 2)) / IMXRT_ARM_PODF_DIVIDER + +#define BOARD_GPT_FREQUENCY \ + (BOARD_CPU_FREQUENCY / IMXRT_IPG_PODF_DIVIDER) / IMXRT_PERCLK_PODF_DIVIDER + + +/* 108Mhz clock for FlexIO using PLL3 PFD2 @ 520 */ +#define CONFIG_FLEXIO1_CLK 1 +#define CONFIG_FLEXIO1_PRED_DIVIDER 5 +#define CONFIG_FLEXIO1_PODF_DIVIDER 1 +#define CONFIG_PLL3_PFD2_FRAC 16 +#define BOARD_FLEXIO_PREQ 108000000 + +/* Define this to enable tracing */ +#if CONFIG_USE_TRACE +# define IMXRT_TRACE_PODF_DIVIDER 1 +# define IMXRT_TRACE_CLK_SELECT CCM_CBCMR_TRACE_CLK_SEL_PLL2_PFD0 +#endif + +/* SDIO *****************************************************************************/ + +/* Pin drive characteristics */ + +#define USDHC1_DATAX_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) +#define USDHC1_CMD_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) +#define USDHC1_CLK_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_SPEED_MAX) +#define USDHC1_CD_IOMUX (IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) + +#define PIN_USDHC1_D0 (GPIO_USDHC1_DATA0_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_02 */ +#define PIN_USDHC1_D1 (GPIO_USDHC1_DATA1_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_03 */ +#define PIN_USDHC1_D2 (GPIO_USDHC1_DATA2_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_04 */ +#define PIN_USDHC1_D3 (GPIO_USDHC1_DATA3_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_05 */ +#define PIN_USDHC1_DCLK (GPIO_USDHC1_CLK_1 | USDHC1_CLK_IOMUX) /* GPIO_SD_B0_01 */ +#define PIN_USDHC1_CMD (GPIO_USDHC1_CMD_1 | USDHC1_CMD_IOMUX) /* GPIO_SD_B0_00 */ +#define PIN_USDHC1_CD (PIN_USDHC1_D3) + +/* Ideal 400Khz for initial inquiry. + * Given input clock 198 Mhz. + * 386.71875 KHz = 198 Mhz / (256 * 2) + */ + +#define BOARD_USDHC_IDMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV256 +#define BOARD_USDHC_IDMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(2) + +/* Ideal 25 Mhz for other modes + * Given input clock 198 Mhz. + * 24.75 MHz = 198 Mhz / (8 * 1) + */ + +#define BOARD_USDHC_MMCMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_MMCMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD1MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD1MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD4MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD4MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +/* ETH Disambiguation *******************************************************/ + +/* Ethernet Interrupt: GPIO_B0_15 + * + * This pin has a week pull-up within the PHY, is open-drain, and requires + * an external 1k ohm pull-up resistor (present on the EVK). A falling + * edge then indicates a change in state of the PHY. + */ + +#define GPIO_ENET_INT (IOMUX_ENET_INT_DEFAULT | \ + GPIO_PORT2 | GPIO_PIN15) /* B0_15 */ +#define GPIO_ENET_IRQ IMXRT_IRQ_GPIO2_15 + +/* Ethernet Reset: GPIO_B0_14 + * + * The #RST uses inverted logic. The initial value of zero will put the + * PHY into the reset state. + */ + +#define GPIO_ENET_RST (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | \ + GPIO_PORT2 | GPIO_PIN14 | IOMUX_ENET_RST_DEFAULT) /* B0_14 */ + +#define GPIO_ENET_TX_DATA00 (GPIO_ENET_TX_DATA00_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_07 */ +#define GPIO_ENET_TX_DATA01 (GPIO_ENET_TX_DATA01_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_08 */ +#define GPIO_ENET_RX_DATA00 (GPIO_ENET_RX_DATA00_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_04 */ +#define GPIO_ENET_RX_DATA01 (GPIO_ENET_RX_DATA01_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_05 */ +#define GPIO_ENET_MDIO (GPIO_ENET_MDIO_1|IOMUX_ENET_MDIO_DEFAULT) /* GPIO_B1_15 */ +#define GPIO_ENET_MDC (GPIO_ENET_MDC_1|IOMUX_ENET_MDC_DEFAULT) /* GPIO_B1_14 */ +#define GPIO_ENET_RX_EN (GPIO_ENET_RX_EN_1|IOMUX_ENET_EN_DEFAULT) /* GPIO_B1_06 */ +#define GPIO_ENET_RX_ER (GPIO_ENET_RX_ER_1|IOMUX_ENET_RXERR_DEFAULT) /* GPIO_B1_11 */ +#define GPIO_ENET_TX_CLK (GPIO_ENET_REF_CLK_2|\ + IOMUX_ENET_TX_CLK_DEFAULT) /* GPIO_B1_10 */ +#define GPIO_ENET_TX_EN (GPIO_ENET_TX_EN_1|IOMUX_ENET_EN_DEFAULT) /* GPIO_B1_09 */ + +/* LED definitions ******************************************************************/ +/* The nxp fmutr1062 board has numerous LEDs but only three, LED_GREEN a Green LED, + * LED_BLUE a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* PIO Disambiguation ***************************************************************/ +/* LPUARTs + */ +#define LPUART_IOMUX (IOMUX_PULL_UP_22K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_SLOW | IOMUX_SPEED_LOW | IOMUX_SCHMITT_TRIGGER) +#define IOMUX_UART_CTS_DEFAULT (IOMUX_PULL_DOWN_100K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_SLOW) + +/* Debug */ + +#define GPIO_LPUART5_RX (GPIO_LPUART5_RX_1 | LPUART_IOMUX) /* GPIO_B1_13 */ +#define GPIO_LPUART5_TX (GPIO_LPUART5_TX_1 | LPUART_IOMUX) /* GPIO_B1_12 */ + +/* AUX */ + +#define GPIO_LPUART4_RX (GPIO_LPUART4_RX_3 | LPUART_IOMUX) /* GPIO_B1_01 */ +#define GPIO_LPUART4_TX (GPIO_LPUART4_TX_3 | LPUART_IOMUX) /* GPIO_B1_00 */ + +/* GPS 1 */ + +#define GPIO_LPUART2_RX (GPIO_LPUART2_RX_1 | LPUART_IOMUX) /* GPIO_AD_B1_03 */ +#define GPIO_LPUART2_TX (GPIO_LPUART2_TX_1 | LPUART_IOMUX) /* GPIO_AD_B1_02 */ + +/* Telem 1 */ + +#define HS_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_SLEW_SLOW | IOMUX_DRIVE_HIZ | IOMUX_SPEED_MEDIUM | IOMUX_PULL_UP_47K) +#define HS_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_PULL_KEEP) + +#define GPIO_LPUART3_RX (GPIO_LPUART3_RX_1 | LPUART_IOMUX) /* GPIO_AD_B1_07 */ +#define GPIO_LPUART3_TX (GPIO_LPUART3_TX_1 | LPUART_IOMUX) /* GPIO_AD_B1_06 */ +#define GPIO_LPUART3_CTS (GPIO_LPUART3_CTS_1 | IOMUX_UART_CTS_DEFAULT | PADMUX_SION) /* GPIO_AD_B1_04 GPIO1_IO20 (GPIO only, no HW Flow control) */ +#define GPIO_LPUART3_RTS (GPIO_PORT1 | GPIO_PIN21 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | HS_OUTPUT_IOMUX) /* GPIO_AD_B1_05 GPIO1_IO21 (GPIO only, no HW Flow control) */ +//TODO check RT7 partial HW handshake + +/* RC INPUT single wire mode on TX, RX is not used */ + +#define GPIO_LPUART8_RX (GPIO_LPUART8_RX_2 | LPUART_IOMUX) /* WRONG!! */ +#define GPIO_LPUART8_TX (GPIO_LPUART8_TX_1 | LPUART_IOMUX) /* GPIO_AD_B1_10 */ + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + * CAN3 is routed to transceiver. + */ +#define FLEXCAN_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MEDIUM) + +#define GPIO_FLEXCAN3_RX (GPIO_FLEXCAN3_RX_3 | FLEXCAN_IOMUX) /* GPIO_EMC_37 */ +#define GPIO_FLEXCAN3_TX (GPIO_FLEXCAN3_TX_3 | FLEXCAN_IOMUX) /* GPIO_EMC_36 */ + +/* LPSPI */ +#define LPSPI_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MAX) + + +#define GPIO_LPSPI3_SCK (GPIO_LPSPI3_SCK_1 | LPSPI_IOMUX) /* GPIO_AD_B1_15 */ +#define GPIO_LPSPI3_MISO (GPIO_LPSPI3_SDI_1 | LPSPI_IOMUX) /* GPIO_AD_B1_13 */ +#define GPIO_LPSPI3_MOSI (GPIO_LPSPI3_SDO_1 | LPSPI_IOMUX) /* GPIO_AD_B1_14 */ + +#define GPIO_LPSPI4_SCK (GPIO_LPSPI4_SCK_2 | LPSPI_IOMUX) /* GPIO_B0_03 */ +#define GPIO_LPSPI4_MISO (GPIO_LPSPI4_SDI_2 | LPSPI_IOMUX) /* GPIO_B0_01 */ +#define GPIO_LPSPI4_MOSI (GPIO_LPSPI4_SDO_2 | LPSPI_IOMUX) /* GPIO_B0_02 */ + +#define BOARD_SPI_BUS_MAX_BUS_ITEMS 2 + +/* LPI2Cs */ + +#define LPI2C_IOMUX (IOMUX_SPEED_MEDIUM | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | GPIO_SION_ENABLE) +#define LPI2C_IO_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | IOMUX_PULL_NONE) + +#define GPIO_LPI2C1_SDA (GPIO_LPI2C1_SDA_2 | LPI2C_IOMUX) /* EVK J24-9 R276 */ /* GPIO_AD_B1_01 */ +#define GPIO_LPI2C1_SCL (GPIO_LPI2C1_SCL_2 | LPI2C_IOMUX) /* EVK J24-10 R277 */ /* GPIO_AD_B1_00 */ + +#define GPIO_LPI2C1_SDA_RESET (GPIO_PORT1 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_01 GPIO1_IO17 */ +#define GPIO_LPI2C1_SCL_RESET (GPIO_PORT1 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_00 GPIO1_IO16 */ + +#define GPIO_LPI2C4_SDA (GPIO_LPI2C4_SDA_1 | LPI2C_IOMUX) /* GPIO_AD_B0_13 */ +#define GPIO_LPI2C4_SCL (GPIO_LPI2C4_SCL_1 | LPI2C_IOMUX) /* GPIO_AD_B0_12 */ + +#define GPIO_LPI2C4_SDA_RESET (GPIO_PORT1 | GPIO_PIN13 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B0_13 */ +#define GPIO_LPI2C4_SCL_RESET (GPIO_PORT1 | GPIO_PIN12 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B0_12 */ + +/* Board doesn't provide GPIO or other Hardware for signaling to timing analyzer */ + +#define PROBE_INIT(mask) +#define PROBE(n,s) +#define PROBE_MARK(n) + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __NUTTX_CONFIG_NXP_TROPIC_INCLUDE_BOARD_H */ diff --git a/boards/nxp/tropic-community/nuttx-config/nsh/defconfig b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..15122b439d --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig @@ -0,0 +1,239 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/tropic-community/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="imxrt" +CONFIG_ARCH_CHIP_IMXRT=y +CONFIG_ARCH_CHIP_MIMXRT1062DVL6A=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RAMVECTORS=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_ITCM=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU=y +CONFIG_ARM_MPU_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=114325 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_PRODUCTID=0x0024 +CONFIG_CDCACM_PRODUCTSTR="PX4 TROPIC Community" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1FC9 +CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_ETH0_PHY_DP83825I=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FSUTILS_IPCFG=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_LITTLEFS=y +CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=2 +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_IMXRT_EDMA=y +CONFIG_IMXRT_EDMA_EDBG=y +CONFIG_IMXRT_EDMA_ELINK=y +CONFIG_IMXRT_EDMA_NTCD=64 +CONFIG_IMXRT_ENET=y +CONFIG_IMXRT_ENET_NTXBUFFERS=8 +CONFIG_IMXRT_ENET_PHYINIT=y +CONFIG_IMXRT_FLEXCAN3=y +CONFIG_IMXRT_FLEXCAN_TXMB=1 +CONFIG_IMXRT_FLEXSPI1=y +CONFIG_IMXRT_FLEXSPI1_XIP=y +CONFIG_IMXRT_GPIO1_0_15_IRQ=y +CONFIG_IMXRT_GPIO1_16_31_IRQ=y +CONFIG_IMXRT_GPIO2_0_15_IRQ=y +CONFIG_IMXRT_GPIO2_16_31_IRQ=y +CONFIG_IMXRT_GPIO_IRQ=y +CONFIG_IMXRT_INIT_FLEXRAM=y +CONFIG_IMXRT_ITCM=384 +CONFIG_IMXRT_LPI2C1=y +CONFIG_IMXRT_LPI2C4=y +CONFIG_IMXRT_LPI2C_DMA=y +CONFIG_IMXRT_LPI2C_DMA_MAXMSG=16 +CONFIG_IMXRT_LPI2C_DYNTIMEO=y +CONFIG_IMXRT_LPI2C_DYNTIMEO_STARTSTOP=10 +CONFIG_IMXRT_LPSPI3=y +CONFIG_IMXRT_LPSPI3_DMA=y +CONFIG_IMXRT_LPSPI4=y +CONFIG_IMXRT_LPSPI4_DMA=y +CONFIG_IMXRT_LPSPI_DMA=y +CONFIG_IMXRT_LPUART2=y +CONFIG_IMXRT_LPUART3=y +CONFIG_IMXRT_LPUART4=y +CONFIG_IMXRT_LPUART5=y +CONFIG_IMXRT_LPUART8=y +CONFIG_IMXRT_LPUART_INVERT=y +CONFIG_IMXRT_LPUART_SINGLEWIRE=y +CONFIG_IMXRT_SNVS_LPSRTC=y +CONFIG_IMXRT_USBDEV=y +CONFIG_IMXRT_USDHC1=y +CONFIG_IMXRT_USDHC1_INVERT_CD=y +CONFIG_IMXRT_USDHC1_WIDTH_D1_D4=y +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_INTELHEX_BINARY=y +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_IPCFG_BINARY=y +CONFIG_IPCFG_PATH="/fs/nor" +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_LPI2C1_DMA=y +CONFIG_LPI2C4_DMA=y +CONFIG_LPUART2_RXDMA=y +CONFIG_LPUART2_TXDMA=y +CONFIG_LPUART3_BAUD=57600 +CONFIG_LPUART3_IFLOWCONTROL=y +CONFIG_LPUART3_OFLOWCONTROL=y +CONFIG_LPUART3_RXBUFSIZE=600 +CONFIG_LPUART3_RXDMA=y +CONFIG_LPUART3_TXBUFSIZE=3000 +CONFIG_LPUART3_TXDMA=y +CONFIG_LPUART5_BAUD=57600 +CONFIG_LPUART5_SERIAL_CONSOLE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_CAN_BITRATE_IOCTL=y +CONFIG_NETDEV_CAN_FILTER_IOCTL=y +CONFIG_NETDEV_LATEINIT=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DHCPC=y +CONFIG_NETINIT_DNS=y +CONFIG_NETINIT_DNSIPADDR=0XC0A800FE +CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_RETRY_MOUNTPATH=10 +CONFIG_NETINIT_THREAD=y +CONFIG_NETINIT_THREAD_PRIORITY=49 +CONFIG_NETUTILS_TELNETD=y +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_CAN=y +CONFIG_NET_CAN_EXTID=y +CONFIG_NET_CAN_NOTIFIER=y +CONFIG_NET_CAN_RAW_TX_DEADLINE=y +CONFIG_NET_CAN_SOCK_OPTS=y +CONFIG_NET_ETH_PKTSIZE=1518 +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOLINGER=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_TIMESTAMP=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_TELNET_LOGIN=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=1048576 +CONFIG_RAM_START=0x20200000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=2032 +CONFIG_SDIO_BLOCKSETUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CLE=y +CONFIG_SYSTEM_DHCPC_RENEW=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_SYSTEM_SYSTEM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld new file mode 100644 index 0000000000..60a76f97e1 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -0,0 +1,767 @@ +/* Auto-generated */ +*(.text._ZN4uORB7Manager27orb_add_internal_subscriberE6ORB_IDhPj) +*(.text._ZN13MavlinkStream6updateERKy) +*(.text._ZN7Mavlink16update_rate_multEv) +*(.text._ZN3sym17PredictCovarianceIfEEN6matrix6MatrixIT_Lj23ELj23EEERKNS2_IS3_Lj24ELj1EEERKS4_RKNS2_IS3_Lj3ELj1EEES3_SC_SC_S3_S3_) +*(.text._ZN13MavlinkStream12get_size_avgEv) +*(.text._ZN16ControlAllocator3RunEv) +*(.text._ZN22MulticopterRateControl3RunEv.part.0) +*(.text._ZN7Mavlink9task_mainEiPPc) +*(.text._ZN7sensors22VehicleAngularVelocity3RunEv) +*(.text._ZN4uORB12Subscription9subscribeEv.part.0) +*(.text._ZN4uORB7Manager13orb_data_copyEPvS1_Rjb) +*(.text._ZN4uORB10DeviceNode5writeEP4filePKcj) +*(.text._ZN4uORB10DeviceNode7publishEPK12orb_metadataPvPKv) +*(.text._ZN4uORB12DeviceMaster19getDeviceNodeLockedEPK12orb_metadatah) +*(.text._Z12get_orb_meta6ORB_ID) +*(.text._ZN9ICM42688P12ProcessAccelERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN3px49WorkQueue3RunEv) +*(.text._ZN9ICM42688P11ProcessGyroERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN4EKF23RunEv) +*(.text._ZN7sensors10VehicleIMU7PublishEv) +*(.text._ZN4math17WelfordMeanVectorIfLj3EE6updateERKN6matrix6VectorIfLj3EEE) +*(.text._ZN7sensors10VehicleIMU10UpdateGyroEv) +*(.text._ZN9ICM42688P8FIFOReadERKyh) +*(.text._ZN3Ekf20controlGravityFusionERKN9estimator9imuSampleE) +*(.text._ZN16PX4Accelerometer10updateFIFOER19sensor_accel_fifo_s) +*(.text._ZN7sensors22VehicleAngularVelocity19CalibrateAndPublishERKyRKN6matrix7Vector3IfEES7_) +*(.text._ZN4uORB12Subscription10advertisedEv) +*(.text._ZNK15AttitudeControl6updateERKN6matrix10QuaternionIfEE) +*(.text._ZN7sensors10VehicleIMU11UpdateAccelEv) +*(.text.perf_set_elapsed.part.0) +*(.text._ZN4uORB12Subscription6updateEPv) +*(.text._ZN12PX4Gyroscope10updateFIFOER18sensor_gyro_fifo_s) +*(.text._ZN7sensors10VehicleIMU3RunEv) +*(.text.__aeabi_l2f) +*(.text._ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4_) +*(.text.pthread_mutex_timedlock) +*(.text._ZN7sensors22VehicleAngularVelocity21FilterAngularVelocityEiPfi) +*(.text._ZN26MulticopterAttitudeControl3RunEv.part.0) +*(.text._ZN6device3SPI9_transferEPhS1_j) +*(.text._ZN15OutputPredictor21calculateOutputStatesEyRKN6matrix7Vector3IfEEfS4_f) +*(.text._ZN7sensors18VotedSensorsUpdate7imuPollER17sensor_combined_s) +*(.text._Z9rotate_3i8RotationRsS0_S0_) +*(.text.fs_getfilep) +*(.text.MEM_DataCopy0_1) +*(.text._ZN7sensors19VehicleAcceleration3RunEv) +*(.text.uart_ioctl) +*(.text._ZN26MulticopterPositionControl3RunEv.part.0) +*(.text.pthread_mutex_take) +*(.text._ZN14ImuDownSampler6updateERKN9estimator9imuSampleE) +*(.text._ZN39ControlAllocationSequentialDesaturation6mixYawEv) +*(.text._ZN16ControlAllocator25publish_actuator_controlsEv.part.0) +*(.text._ZN9ICM42688P7RunImplEv) +*(.text._ZN4uORB12Subscription9subscribeEv) +*(.text.param_get) +*(.text._ZN7sensors22VehicleAngularVelocity21SensorSelectionUpdateERKyb) +*(.text._ZN3px49WorkQueue3AddEPNS_8WorkItemE) +*(.text._ZN4EKF220PublishLocalPositionERKy) +*(.text._mav_finalize_message_chan_send) +*(.text._ZN3Ekf19fixCovarianceErrorsEb) +*(.text._ZN7sensors22VehicleAngularVelocity16ParametersUpdateEb) +*(.text._ZN6events12SendProtocol6updateERKy) +*(.text._ZN6device3SPI8transferEPhS1_j) +*(.text._ZN27MavlinkStreamDistanceSensor4sendEv) +*(.text.hrt_call_internal) +*(.text._ZN39ControlAllocationSequentialDesaturation18mixAirmodeDisabledEv) +*(.text._ZN7Mavlink15get_free_tx_bufEv) +*(.text.nx_poll) +*(.text._ZN15MavlinkReceiver3runEv) +*(.text._ZN9ICM42688P18ProcessTemperatureEPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN15OutputPredictor19correctOutputStatesEyRKN6matrix10QuaternionIfEERKNS0_7Vector3IfEES8_S8_S8_) +*(.text._ZN3Ekf12predictStateERKN9estimator9imuSampleE) +*(.text._ZN3px46logger6Logger3runEv) +*(.text._ZN4uORB20SubscriptionInterval7updatedEv) +*(.text._ZN24MavlinkStreamCommandLong4sendEv) +*(.text._ZN9Commander3runEv) +*(.text._ZN3Ekf17predictCovarianceERKN9estimator9imuSampleE) +*(.text.wd_cancel) +*(.text._ZN7Sensors3RunEv) +*(.text.perf_end) +*(.text._ZN4uORB12Subscription7updatedEv) +*(.text._ZN13land_detector12LandDetector3RunEv) +*(.text.sched_idletask) +*(.text.atanf) +*(.text.uart_write) +*(.text.pthread_mutex_unlock) +*(.text.__ieee754_asinf) +*(.text.MEM_DataCopy0_2) +*(.text._ZN20MavlinkCommandSender13check_timeoutE17mavlink_channel_t) +*(.text._ZN16ControlAllocator32publish_control_allocator_statusEi) +*(.text.__ieee754_atan2f) +*(.text._ZNK18DynamicSparseLayer3getEt) +*(.text.__udivmoddi4) +*(.text._ZN8Failsafe17checkStateAndModeERKyRKN12FailsafeBase5StateERK16failsafe_flags_s) +*(.text._ZN29MavlinkStreamHygrometerSensor4sendEv) +*(.text.pthread_mutex_give) +*(.text._ZN3Ekf18controlFusionModesERKN9estimator9imuSampleE) +*(.text._ZN4cdev4CDev11poll_notifyEm) +*(.text.file_vioctl) +*(.text._ZN7sensors18VotedSensorsUpdate11sensorsPollER17sensor_combined_s) +*(.text.nxsig_nanosleep) +*(.text.imxrt_lpspi1select) +*(.text.sem_wait) +*(.text.perf_count_interval.part.0) +*(.text._ZN16ControlAllocator37update_effectiveness_matrix_if_neededE25EffectivenessUpdateReason) +*(.text.MEM_LongCopyJump) +*(.text.px4_arch_adc_sample) +*(.text._ZN31MulticopterHoverThrustEstimator3RunEv) +*(.text._ZNK17ControlAllocation20clipActuatorSetpointERN6matrix6VectorIfLj16EEE) +*(.text._ZN4uORB7Manager17get_device_masterEv) +*(.text._ZN13DataValidator3putEyPKfmh) +*(.text.cdcuart_ioctl) +*(.text.cdcacm_sndpacket) +*(.text._ZN7sensors22VehicleAngularVelocity16SensorBiasUpdateEb) +*(.text._ZN13MavlinkStream11update_dataEv) +*(.text._ZN7sensors18VotedSensorsUpdate21calcGyroInconsistencyEv) +*(.text.param_set_used) +*(.text._ZN18EstimatorInterface10setIMUDataERKN9estimator9imuSampleE) +*(.text._ZN18DataValidatorGroup8get_bestEyPi) +*(.text._ZN4EKF218PublishInnovationsERKy) +*(.text._ZN21MavlinkMissionManager4sendEv) +*(.text._ZN22MulticopterRateControl28updateActuatorControlsStatusERK25vehicle_torque_setpoint_sf) +*(.text._ZN11RateControl6updateERKN6matrix7Vector3IfEES4_S4_fb) +*(.text._ZN39ControlAllocationSequentialDesaturation19desaturateActuatorsERN6matrix6VectorIfLj16EEERKS2_b) +*(.text._ZN22MavlinkStreamCollision4sendEv) +*(.text.imxrt_lpi2c_transfer) +*(.text.uart_putxmitchar) +*(.text.clock_nanosleep) +*(.text.up_release_pending) +*(.text.MEM_DataCopy0) +*(.text._ZN22MavlinkStreamGPSRawInt4sendEv) +*(.text.dq_rem) +*(.text._ZN15GyroCalibration3RunEv.part.0) +*(.text._ZN7sensors18VotedSensorsUpdate22calcAccelInconsistencyEv) +*(.text._ZN24MavlinkStreamADSBVehicle4sendEv) +*(.text.sinf) +*(.text.hrt_call_after) +*(.text._ZN39ControlAllocationSequentialDesaturation8allocateEv) +*(.text.up_invalidate_dcache) +*(.text._ZN15PositionControl16_velocityControlEf) +*(.text._ZN4EKF222PublishAidSourceStatusERKy) +*(.text._ZN4ListIP13MavlinkStreamE8IteratorppEv) +*(.text._ZN20MavlinkStreamESCInfo4sendEv) +*(.text.sem_post) +*(.text._ZN3px417ScheduledWorkItem15ScheduleDelayedEm) +*(.text._ZN10FlightTaskC1Ev) +*(.text.usleep) +*(.text._ZN14FlightTaskAutoC1Ev) +*(.text.sem_getvalue) +*(.text._ZN23MavlinkStreamHighresIMU4sendEv) +*(.text.imxrt_gpio_write) +*(.text._ZN3Ekf6updateEv) +*(.text.__ieee754_acosf) +*(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE) +*(.text._ZN9Commander13dataLinkCheckEv) +*(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex) +*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_) +*(.text._ZN12PX4Gyroscope9set_scaleEf) +*(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s) +*(.text._ZN18MavlinkStreamDebug4sendEv) +*(.text._ZN27MavlinkStreamServoOutputRawILi0EE4sendEv) +*(.text.asinf) +*(.text._ZN6matrix5EulerIfEC1ERKNS_3DcmIfEE) +*(.text._ZN4EKF227PublishInnovationTestRatiosERKy) +*(.text._ZN4EKF213PublishStatusERKy) +*(.text._ZN4EKF226PublishInnovationVariancesERKy) +*(.text._ZN13land_detector23MulticopterLandDetector25_get_ground_contact_stateEv) +*(.text.imxrt_dmach_start) +*(.text._ZN3ADC19update_system_powerEy) +*(.text._ZNK3Ekf19get_ekf_soln_statusEPt) +*(.text._ZN3px46logger15watchdog_updateERNS0_15watchdog_data_tEb) +*(.text.imxrt_gpio_read) +*(.text._ZN32MavlinkStreamNavControllerOutput4sendEv) +*(.text._ZN15ArchPX4IOSerial13_bus_exchangeEP8IOPacket) +*(.text._ZN39MavlinkStreamGimbalDeviceAttitudeStatus4sendEv) +*(.text._ZNK10ConstLayer3getEt) +*(.text.__aeabi_uldivmod) +*(.text.up_udelay) +*(.text.up_idle) +*(.text._ZN20MavlinkStreamGPS2Raw4sendEv) +*(.text._ZN4EKF217UpdateCalibrationERKyRNS_19InFlightCalibrationERKN6matrix7Vector3IfEES8_fbb) +*(.text._ZN28MavlinkStreamGpsGlobalOrigin4sendEv) +*(.text._ZN11ControlMath15bodyzToAttitudeEN6matrix7Vector3IfEEfR27vehicle_attitude_setpoint_s) +*(.text._ZN4EKF217UpdateRangeSampleER17ekf2_timestamps_s) +*(.text._ZN3Ekf24controlOpticalFlowFusionERKN9estimator9imuSampleE) +*(.text._ZN19MavlinkStreamRawRpm4sendEv) +*(.text._ZN13MavlinkStream10const_rateEv) +*(.text._ZN4EKF215PublishOdometryERKyRKN9estimator9imuSampleE) +*(.text._ZN15FailureDetector20updateAttitudeStatusERK16vehicle_status_s) +*(.text._ZN7Mavlink19configure_sik_radioEv) +*(.text._ZN13BatteryStatus8adc_pollEv) +*(.text.getpid) +*(.text._ZN13DataValidator10confidenceEy) +*(.text._ZN22MavlinkStreamGPSStatus4sendEv) +*(.text._ZN4EKF220UpdateAirspeedSampleER17ekf2_timestamps_s) +*(.text._ZN23MavlinkStreamStatustext4sendEv) +*(.text._ZN3Ekf15constrainStatesEv) +*(.text._ZN12PX4IO_serial4readEjPvj) +*(.text.uart_poll) +*(.text._ZN24MavlinkParametersManager4sendEv) +*(.text._ZN26MulticopterPositionControl18set_vehicle_statesERK24vehicle_local_position_s) +*(.text.file_poll) +*(.text.hrt_elapsed_time) +*(.text._ZN7Mavlink11send_finishEv) +*(.text._ZNK3Ekf36estimateInertialNavFallingLikelihoodEv) +*(.text._ZN15PositionControl16_positionControlEv) +*(.text._ZN28MavlinkStreamDebugFloatArray4sendEv) +*(.text._ZN11ControlMath9limitTiltERN6matrix7Vector3IfEERKS2_f) +*(.text.pthread_mutex_lock) +*(.text._ZN21MavlinkStreamAltitude8get_sizeEv) +*(.text._ZN7Mavlink29check_requested_subscriptionsEv) +*(.text.imxrt_lpspi_setmode) +*(.text._ZN3Ekf34controlZeroInnovationHeadingUpdateEv) +*(.text.perf_begin) +*(.text.imxrt_lpspi_setfrequency) +*(.text._ZN17FlightModeManager9_initTaskE15FlightTaskIndex) +*(.text._ZN22MulticopterRateControl3RunEv) +*(.text.cosf) +*(.text._ZN22MavlinkStreamESCStatus4sendEv) +*(.text._ZN26MavlinkStreamCameraTrigger4sendEv) +*(.text._ZN36MavlinkStreamPositionTargetGlobalInt4sendEv) +*(.text._ZN4uORB12Subscription4copyEPv) +*(.text._ZN7sensors19VehicleAcceleration21SensorSelectionUpdateEb) +*(.text.crc_accumulate) +*(.text._ZN3px46logger6Logger13update_paramsEv) +*(.text._ZN11calibration14DeviceExternalEm) +*(.text._ZN25MavlinkStreamHomePosition8get_sizeEv) +*(.text.imxrt_lpspi_modifyreg32) +*(.text._ZN7sensors19VehicleAcceleration16SensorBiasUpdateEb) +*(.text.modifyreg32) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EEmlEf) +*(.text._ZN6matrix5EulerIfEC1ERKNS_10QuaternionIfEE) +*(.text.imxrt_queuedtd) +*(.text._ZN27MavlinkStreamDistanceSensor8get_sizeEv) +*(.text._ZN3Ekf16fuseVelPosHeightEffi) +*(.text._ZN3Ekf23controlBaroHeightFusionEv) +*(.text._ZN16PX4Accelerometer9set_scaleEf) +*(.text._ZN11ControlMath11constrainXYERKN6matrix7Vector2IfEES4_RKf) +*(.text._ZN22MavlinkStreamEfiStatus4sendEv) +*(.text._ZN22MavlinkStreamDebugVect4sendEv) +*(.text._ZN4EKF217PublishSensorBiasERKy) +*(.text._ZN17FlightModeManager3RunEv) +*(.text._ZN15PositionControl11_inputValidEv) +*(.text._ZN7sensors14VehicleAirData3RunEv) +*(.text.perf_count) +*(.text._ZN3Ekf16controlMagFusionEv) +*(.text.pthread_sem_give) +*(.text._ZN7sensors10VehicleIMU16ParametersUpdateEb) +*(.text._ZN30MavlinkStreamUTMGlobalPosition4sendEv) +*(.text._ZN4uORB20SubscriptionInterval4copyEPv) +*(.text._ZN12I2CSPIDriverI9ICM42688PE3RunEv) +*(.text._ZN17ObstacleAvoidanceC1EP12ModuleParams) +*(.text.imxrt_epcomplete.constprop.0) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EEmiERKS1_) +*(.text._ZN9Commander30handleModeIntentionAndFailsafeEv) +*(.text.perf_event_count) +*(.text._ZN4EKF215PublishAttitudeERKy) +*(.text._ZN19MavlinkStreamRawRpm8get_sizeEv) +*(.text._ZNK3px46atomicIbE4loadEv) +*(.text._ZN29MavlinkStreamHygrometerSensor8get_sizeEv) +*(.text.pthread_mutex_add) +*(.text._ZN12HomePosition6updateEbb) +*(.text._ZN5PX4IO3RunEv) +*(.text.poll_fdsetup) +*(.text._ZN15PositionControl20_accelerationControlEv) +*(.text._ZN3Ekf19controlHeightFusionERKN9estimator9imuSampleE) +*(.text._ZN9Commander19control_status_ledsEbh) +*(.text._ZN6device3I2C8transferEPKhjPhj) +*(.text.orb_publish) +*(.text._ZN7sensors19VehicleAcceleration16ParametersUpdateEb) +*(.text._ZN22MavlinkStreamVibration8get_sizeEv) +*(.text._ZN15MavlinkReceiver15CheckHeartbeatsERKyb) +*(.text._ZNK6matrix7Vector3IfEmiES1_) +*(.text.__aeabi_f2ulz) +*(.text._ZN9ICM42688P26DataReadyInterruptCallbackEiPvS0_) +*(.text._ZN13land_detector23MulticopterLandDetector23_get_maybe_landed_stateEv) +*(.text.acosf) +*(.text._ZN14ImuDownSampler5resetEv) +*(.text._ZN3Ekf31checkVerticalAccelerationHealthERKN9estimator9imuSampleE) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1ERKS1_) +*(.text.udp_pollsetup) +*(.text._ZL14timer_callbackPv) +*(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj23EEEf) +*(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) +*(.text.nxsem_wait_irq) +*(.text._ZN20MavlinkCommandSender4lockEv) +*(.text.MEM_LongCopyEnd) +*(.text._ZThn24_N16ControlAllocator3RunEv) +*(.text._ZN15TimestampedListIN20MavlinkCommandSender14command_item_sELi3EE8get_nextEv) +*(.text._ZNK3Ekf21get_ekf_lpos_accuracyEPfS0_) +*(.text._ZN17FlightModeManager17start_flight_taskEv) +*(.text.MEM_DataCopyBytes) +*(.text._ZN29MavlinkStreamLocalPositionNED8get_sizeEv) +*(.text._ZN6SticksC1EP12ModuleParams) +*(.text._ZN27MavlinkStreamServoOutputRawILi1EE4sendEv) +*(.text._ZN3Ekf35updateHorizontalDeadReckoningstatusEv) +*(.text._ZN3Ekf20controlAirDataFusionERKN9estimator9imuSampleE) +*(.text._ZN24FlightTaskManualAltitudeC1Ev) +*(.text._ZN25MavlinkStreamHomePosition4sendEv) +*(.text._ZN24MavlinkParametersManager8send_oneEv) +*(.text._ZN15OutputPredictor29applyCorrectionToOutputBufferERKN6matrix7Vector3IfEES4_) +*(.text._ZN21HealthAndArmingChecks6updateEb) +*(.text._ZThn24_N22MulticopterRateControl3RunEv) +*(.text._ZN26MavlinkStreamManualControl4sendEv) +*(.text._ZN27MavlinkStreamOpticalFlowRad4sendEv) +*(.text._ZN18mag_bias_estimator16MagBiasEstimator3RunEv) +*(.text._ZN4uORB7Manager11orb_publishEPK12orb_metadataPvPKv) +*(.text._ZN24MavlinkParametersManager18send_untransmittedEv) +*(.text._ZN10MavlinkFTP4sendEv) +*(.text._ZN15ArchPX4IOSerial13_do_interruptEv) +*(.text._ZN3Ekf27controlExternalVisionFusionEv) +*(.text.clock_gettime) +*(.text._ZN3ADC17update_adc_reportEy) +*(.text._ZN3sym25ComputeYaw312InnovVarAndHIfEEvRKN6matrix6MatrixIT_Lj24ELj1EEERKNS2_IS3_Lj23ELj23EEES3_S3_PS3_PNS2_IS3_Lj23ELj1EEE) +*(.text._ZN3Ekf19runTerrainEstimatorERKN9estimator9imuSampleE) +*(.text._ZN32MavlinkStreamGimbalManagerStatus4sendEv) +*(.text._ZN9LockGuardD1Ev) +*(.text._ZN4EKF213PublishStatesERKy) +*(.text._ZN3ADC3RunEv) +*(.text._ZN6BMP38815compensate_dataEhPK16bmp3_uncomp_dataP9bmp3_data) +*(.text._ZN3Ekf20controlFakePosFusionEv) +*(.text._ZN11calibration9Gyroscope13set_device_idEm) +*(.text._ZN24MavlinkStreamOrbitStatus8get_sizeEv) +*(.text.imxrt_progressep) +*(.text.imxrt_gpio_configinput) +*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) +*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN21MavlinkStreamAttitude4sendEv) +*(.text._ZN4EKF220UpdateMagCalibrationERKy) +*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) +*(.text._ZN9ICM42688P9DataReadyEv) +*(.text._ZN21MavlinkMissionManager20check_active_missionEv) +*(.text._ZNK3Ekf20get_ekf_vel_accuracyEPfS0_) +*(.text._ZN4EKF216UpdateBaroSampleER17ekf2_timestamps_s) +*(.text._ZN4EKF223UpdateSystemFlagsSampleER17ekf2_timestamps_s) +*(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv) +*(.text._ZN29MavlinkStreamObstacleDistance4sendEv) +*(.text._ZN24MavlinkStreamOrbitStatus4sendEv) +*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf) +*(.text._ZN9Navigator3runEv) +*(.text._ZN24MavlinkParametersManager11send_paramsEv) +*(.text._ZN17MavlinkLogHandler4sendEv) +*(.text._ZN7control10SuperBlock5setDtEf) +*(.text._ZN29MavlinkStreamMountOrientation8get_sizeEv) +*(.text._ZN5PX4IO13io_get_statusEv) +*(.text._ZN26MulticopterAttitudeControl3RunEv) +*(.text._ZThn16_N31ActuatorEffectivenessMultirotor22getEffectivenessMatrixERN21ActuatorEffectiveness13ConfigurationE25EffectivenessUpdateReason) +*(.text._ZN4EKF218PublishStatusFlagsERKy) +*(.text._ZN11WeatherVaneC1EP12ModuleParams) +*(.text._ZN15FailureDetector6updateERK16vehicle_status_sRK22vehicle_control_mode_s) +*(.text._ZN7Mavlink10send_startEi) +*(.text.imxrt_lpspi_setbits) +*(.text._ZN15OutputPredictor37applyCorrectionToVerticalOutputBufferEf) +*(.text._ZN4EKF222UpdateAccelCalibrationERKy) +*(.text._ZN7sensors19VehicleMagnetometer3RunEv) +*(.text._ZN29MavlinkStreamMountOrientation4sendEv) +*(.text._ZN13land_detector12LandDetector19UpdateVehicleAtRestEv) +*(.text._ZN10FlightTask29_evaluateVehicleLocalPositionEv) +*(.text.board_autoled_off) +*(.text.__aeabi_f2lz) +*(.text._ZN32MavlinkStreamCameraImageCaptured4sendEv) +*(.text._ZN21MavlinkStreamOdometry8get_sizeEv) +*(.text._ZN28MavlinkStreamNamedValueFloat4sendEv) +*(.text.__aeabi_ul2f) +*(.text.poll) +*(.text._ZN14FlightTaskAutoD1Ev) +*(.text._ZN4uORB10DeviceNode22get_initial_generationEv) +*(.text._ZN3Ekf23controlGnssHeightFusionERKN9estimator9gpsSampleE) +*(.text._ZN3Ekf40updateOnGroundMotionForOpticalFlowChecksEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1Ev) +*(.text._ZN14ZeroGyroUpdate6updateER3EkfRKN9estimator9imuSampleE) +*(.text._ZN30MavlinkStreamOpenDroneIdSystem4sendEv) +*(.text._ZN22MavlinkStreamScaledIMU4sendEv) +*(.text._ZN46MavlinkStreamTrajectoryRepresentationWaypoints4sendEv) +*(.text.imxrt_ioctl) +*(.text._ZN3Ekf25checkMagBiasObservabilityEv) +*(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) +*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s) +*(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0) +*(.text._ZN19StickAccelerationXYC1EP12ModuleParams) +*(.text.imxrt_epsubmit) +*(.text._ZN15PositionControl6updateEf) +*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE) +*(.text._ZN23MavlinkStreamScaledIMU24sendEv) +*(.text._ZN5PX4IO10io_reg_getEhhPtj) +*(.text.imxrt_dma_send) +*(.text._ZN20MavlinkStreamWindCov4sendEv) +*(.text._ZN7sensors18VotedSensorsUpdate13checkFailoverERNS0_10SensorDataEPKcN6events3px45enums13sensor_type_tE) +*(.text._ZN21MavlinkStreamOdometry4sendEv) +*(.text.vsprintf_internal.constprop.0) +*(.text.udp_pollteardown) +*(.text._ZN12MixingOutput6updateEv) +*(.text.clock_abstime2ticks) +*(.text._ZN13BatteryStatus3RunEv) +*(.text._ZN32MavlinkStreamGimbalManagerStatus8get_sizeEv) +*(.text._ZN10FlightTask15_resetSetpointsEv) +*(.text._ZN9systemlib10Hysteresis20set_state_and_updateEbRKy) +*(.text.devif_callback_free.part.0) +*(.text._ZN6Sticks25checkAndUpdateStickInputsEv) +*(.text.atan2f) +*(.text._ZN23MavlinkStreamRCChannels4sendEv) +*(.text._ZN4EKF221UpdateExtVisionSampleER17ekf2_timestamps_s) +*(.text.imxrt_dmach_stop) +*(.text._ZN9Commander16handleAutoDisarmEv) +*(.text._ZN9Commander11updateTunesEv) +*(.text._ZN4EKF215UpdateMagSampleER17ekf2_timestamps_s) +*(.text._ZN18DataValidatorGroup3putEjyPKfmh) +*(.text._ZNK3Ekf19get_ekf_ctrl_limitsEPfS0_S0_S0_) +*(.text._ZN12FailsafeBase13checkFailsafeEibbRKNS_13ActionOptionsE) +*(.text._ZN17FlightTaskDescendD1Ev) +*(.text._ZN30MavlinkStreamOpenDroneIdSystem8get_sizeEv) +*(.text._ZNK3px46logger9LogWriter10is_startedENS0_7LogTypeE) +*(.text._ZN24FlightTaskManualAltitudeD1Ev) +*(.text._Z35px4_indicate_external_reset_lockout16LockoutComponentb) +*(.text.uart_pollnotify) +*(.text._ZN3Ekf11predictHaglERKN9estimator9imuSampleE) +*(.text._ZN4EKF215PublishBaroBiasERKy) +*(.text._ZN4EKF221UpdateGyroCalibrationERKy) +*(.text._ZN6matrix9constrainIfLj3ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) +*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh4EE16advertised_countEv) +*(.text._ZN23MavlinkStreamScaledIMU34sendEv) +*(.text.__aeabi_ldivmod) +*(.text._ZN15PositionControl16setInputSetpointERK21trajectory_setpoint_s) +*(.text.nxsig_pendingset) +*(.text.psock_poll) +*(.text._ZN15FailureInjector6updateEv) +*(.text.imxrt_writedtd) +*(.text.cdcacm_wrcomplete) +*(.text.cdcuart_txint) +*(.text._ZN3Ekf33updateVerticalDeadReckoningStatusEv) +*(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev) +*(.text.powf) +*(.text._ZN4EKF217PublishEventFlagsERKy) +*(.text._ZN17FlightTaskDescend6updateEv) +*(.text.imxrt_iomux_configure) +*(.text.hrt_store_absolute_time) +*(.text.nxsem_set_protocol) +*(.text.write) +*(.text._ZN22MavlinkStreamSysStatus4sendEv) +*(.text._ZN4EKF216UpdateFlowSampleER17ekf2_timestamps_s) +*(.text._ZN4cdevL10cdev_ioctlEP4fileim) +*(.text._ZN7Mavlink10send_bytesEPKhj) +*(.text._ZNK8Failsafe17checkModeFallbackERK16failsafe_flags_sh) +*(.text.clock_systime_timespec) +*(.text._ZN4uORB10DeviceNode26remove_internal_subscriberEv) +*(.text._ZThn16_N4EKF23RunEv) +*(.text._ZNK3Ekf22computeYawInnovVarAndHEfRfRN6matrix6VectorIfLj23EEE) +*(.text._ZN12ActuatorTest6updateEif) +*(.text._ZN17VelocitySmoothingC1Efff) +*(.text._ZN13AnalogBattery19get_voltage_channelEv) +*(.text._ZN10FlightTask37_evaluateVehicleLocalPositionSetpointEv) +*(.text._ZN4uORB12Subscription11unsubscribeEv) +*(.text.net_lock) +*(.text.clock_time2ticks) +*(.text._ZN12FailsafeBase16updateStartDelayERKyb) +*(.text._ZN23MavlinkStreamStatustext8get_sizeEv) +*(.text._ZN11calibration13Accelerometer13set_device_idEm) +*(.text._ZN3px46logger6Logger18start_stop_loggingEv) +*(.text._ZN14FlightTaskAuto17_evaluateTripletsEv) +*(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb) +*(.text._ZN25MavlinkStreamMagCalReport4sendEv) +*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf) +*(.text.imxrt_config_gpio) +*(.text.nxsig_timeout) +*(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_) +*(.text._ZN3Ekf17measurementUpdateERN6matrix6VectorIfLj23EEEff) +*(.text.dq_addlast) +*(.text._ZN19MavlinkStreamVFRHUD4sendEv) +*(.text.hrt_call_reschedule) +*(.text.nxsem_boost_priority) +*(.text._ZN4EKF215UpdateGpsSampleER17ekf2_timestamps_s) +*(.text._ZN8StickYawC1EP12ModuleParams) +*(.text._ZN7control12BlockLowPass6updateEf) +*(.text._ZN15FailureDetector26updateImbalancedPropStatusEv) +*(.text._ZN9systemlib10Hysteresis6updateERKy) +*(.text.nxsem_tickwait_uninterruptible) +*(.text._ZN12HomePosition31hasMovedFromCurrentHomeLocationEv) +*(.text.devif_callback_alloc) +*(.text._ZN28MavlinkStreamNamedValueFloat8get_sizeEv) +*(.text._ZN24MavlinkStreamADSBVehicle8get_sizeEv) +*(.text._ZN26MavlinkStreamBatteryStatus8get_sizeEv) +*(.text._ZN26MulticopterPositionControl17parameters_updateEb) +*(.text._ZN3px46logger6Logger29handle_vehicle_command_updateEv) +*(.text.imxrt_lpspi_send) +*(.text._ZN4uORB10DeviceNode23add_internal_subscriberEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEaSERKS1_) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE5emultERKS1_) +*(.text.mallinfo_handler) +*(.text._ZN13land_detector23MulticopterLandDetector14_update_topicsEv) +*(.text._ZN24ManualVelocitySmoothingZC1Ev) +*(.text._ZN3ADC6sampleEj) +*(.text._ZNK3Ekf22isTerrainEstimateValidEv) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK12sensor_gps_sR16failsafe_flags_sR6Report) +*(.text._ZN11ControlMath11addIfNotNanERff) +*(.text._ZN9Commander21checkForMissionUpdateEv) +*(.text._Z8set_tunei) +*(.text._ZN3Ekf13stopGpsFusionEv) +*(.text._ZNK6matrix7Vector3IfE5crossERKNS_6MatrixIfLj3ELj1EEE) +*(.text._ZN10FlightTask22_checkEkfResetCountersEv) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE11isAllFiniteEv) +*(.text._ZN14FlightTaskAuto24_evaluateGlobalReferenceEv) +*(.text._ZN6matrix9constrainIfLj2ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) +*(.text._ZN3px46logger6Logger23handle_file_write_errorEv) +*(.text._ZN10FlightTask16updateInitializeEv) +*(.text._ZN11calibration9Gyroscope10set_offsetERKN6matrix7Vector3IfEE) +*(.text._ZNK6matrix6VectorIfLj3EE4normEv) +*(.text._ZN14FlightTaskAuto16updateInitializeEv) +*(.text.fabsf) +*(.text._ZN27MavlinkStreamAttitudeTarget8get_sizeEv) +*(.text.nxsem_get_value) +*(.text._ZN13AnalogBattery8is_validEv) +*(.text._ZN4uORB16SubscriptionDataI15home_position_sEC1EPK12orb_metadatah) +*(.text._ZN22MavlinkStreamGPSStatus8get_sizeEv) +*(.text.nxsem_destroyholder) +*(.text.psock_udp_cansend) +*(.text.MEM_DataCopy2_2) +*(.text._ZN10FlightTask8activateERK21trajectory_setpoint_s) +*(.text.sock_file_poll) +*(.text._ZN3Ekf20controlHaglRngFusionEv) +*(.text._ZN10Ringbuffer9pop_frontEPhj) +*(.text.nx_write) +*(.text._ZN9Commander18manualControlCheckEv) +*(.text._ZN31MavlinkStreamAttitudeQuaternion4sendEv) +*(.text.nxsem_canceled) +*(.text._ZN10FlightTask21getTrajectorySetpointEv) +*(.text.imxrt_dmach_getcount) +*(.text.sem_clockwait) +*(.text.inet_poll) +*(.text._ZN6BMP3887collectEv) +*(.text._ZN15ArchPX4IOSerial19_do_rx_dma_callbackEbi) +*(.text._ZN15ArchPX4IOSerial10_abort_dmaEv) +*(.text._ZNK15PositionControl24getLocalPositionSetpointER33vehicle_local_position_setpoint_s) +*(.text._ZN3Ekf16controlGpsFusionERKN9estimator9imuSampleE) +*(.text._ZN23MavlinkStreamRCChannels8get_sizeEv) +*(.text._ZN20MavlinkStreamESCInfo8get_sizeEv) +*(.text._ZNK6matrix6VectorIfLj2EE4normEv) +*(.text._Z15arm_auth_updateyb) +*(.text._ZN3LED5ioctlEP4fileim) +*(.text._ZNK3px46logger9LogWriter20had_file_write_errorEv) +*(.text._ZN29MavlinkStreamLocalPositionNED4sendEv) +*(.text._ZN6matrix6MatrixIfLj2ELj1EEC1ILj3ELj1EEERKNS_5SliceIfLj2ELj1EXT_EXT0_EEE) +*(.text._ZNK6matrix6VectorIfLj3EE3dotERKNS_6MatrixIfLj3ELj1EEE) +*(.text.imxrt_lpi2c_setclock) +*(.text._ZN6matrix12typeFunction9constrainIfEET_S2_S2_S2_.part.0) +*(.text._ZN13MapProjection13initReferenceEddy) +*(.text._ZN11calibration13Accelerometer23SensorCorrectionsUpdateEb) +*(.text._ZN31MavlinkStreamAttitudeQuaternion8get_sizeEv) +*(.text._ZN30MavlinkStreamGlobalPositionInt4sendEv) +*(.text._ZN6SticksD1Ev) +*(.text._ZN13NavigatorMode3runEb) +*(.text._ZL19param_find_internalPKcb) +*(.text.uart_datasent) +*(.text._ZN4EKF221PublishOpticalFlowVelERKy) +*(.text.nxsem_destroy) +*(.text.file_write) +*(.text._ZN21MavlinkStreamAltitude4sendEv) +*(.text._ZN7sensors14VehicleAirData12UpdateStatusEv) +*(.text.imxrt_padmux_map) +*(.text._ZN6BMP38815get_sensor_dataEhP9bmp3_data) +*(.text._ZN18MavlinkRateLimiter5checkERKy) +*(.text._ZThn24_N26MulticopterAttitudeControl3RunEv) +*(.text._ZN15ArchPX4IOSerial10_interruptEiPvS0_) +*(.text.imxrt_periphclk_configure) +*(.text._ZN3Ekf8initHaglEv) +*(.text._ZN4EKF218UpdateAuxVelSampleER17ekf2_timestamps_s) +*(.text._ZN3RTL11on_inactiveEv) +*(.text._ZN12FailsafeBase10modeCanRunERK16failsafe_flags_sh) +*(.text._ZN4EKF216PublishEvPosBiasERKy) +*(.text._ZN21MavlinkStreamAttitude8get_sizeEv) +*(.text._ZThn16_N7sensors19VehicleAcceleration3RunEv) +*(.text._ZN3Ekf24controlRangeHeightFusionEv) +*(.text._ZN33MavlinkStreamTimeEstimateToTarget4sendEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EE6setAllEf) +*(.text._ZN12ModuleParamsD1Ev) +*(.text._ZN3Ekf20controlFakeHgtFusionEv) +*(.text.imxrt_reqcomplete) +*(.text._ZNK6matrix7Vector3IfEmlEf) +*(.text._ZN18ZeroVelocityUpdate6updateER3EkfRKN9estimator9imuSampleE) +*(.text._ZN11ControlMath19addIfNotNanVector3fERN6matrix7Vector3IfEERKS2_) +*(.text._ZN11ControlMath16thrustToAttitudeERKN6matrix7Vector3IfEEfR27vehicle_attitude_setpoint_s) +*(.text.cos) +*(.text.net_unlock) +*(.text._ZN7sensors18VotedSensorsUpdate21setRelativeTimestampsER17sensor_combined_s) +*(.text._ZN12FailsafeBase13ActionOptionsC1ENS_6ActionE) +*(.text._ZN24FlightTaskManualAltitude16updateInitializeEv) +*(.text._ZN26MulticopterPositionControl3RunEv) +*(.text._ZN8Failsafe21fromQuadchuteActParamEi) +*(.text._ZN24VariableLengthRingbuffer9pop_frontEPhj) +*(.text._ZN7control15BlockDerivative6updateEf) +*(.text._ZN5PX4IO10io_reg_getEhh) +*(.text._ZN9Commander18safetyButtonUpdateEv) +*(.text._ZN13BatteryChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN18DataValidatorGroup16get_sensor_stateEj) +*(.text.uart_xmitchars_done) +*(.text._ZN4EKF225PublishYawEstimatorStatusERKy) +*(.text.sin) +*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf) +*(.text._ZN6Safety19safetyButtonHandlerEv) +*(.text._ZN3Ekf19controlAuxVelFusionEv) +*(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_) +*(.text._ZThn24_N7Sensors3RunEv) +*(.text._ZN6matrix6MatrixIfLj2ELj1EEC1ERKS1_) +*(.text._ZN10FlightTask10reActivateEv) +*(.text._ZN5PX4IO17io_publish_raw_rcEv) +*(.text._ZNK15PositionControl19getAttitudeSetpointER27vehicle_attitude_setpoint_s) +*(.text._ZN4cdev4CDev4pollEP4fileP6pollfdb) +*(.text._ZN9Commander20offboardControlCheckEv) +*(.text._ZN4EKF216PublishGpsStatusERKy) +*(.text._ZN4uORB12SubscriptionaSEOS0_) +*(.text._ZN15TakeoffHandling18updateTakeoffStateEbbbfbRKy) +*(.text._ZN10ModeChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN14FlightTaskAuto24_updateInternalWaypointsEv) +*(.text._ZN8Failsafe17updateArmingStateERKybRK16failsafe_flags_s) +*(.text.imxrt_lpi2c_modifyreg) +*(.text.up_flush_dcache) +*(.text._ZN5PX4IO16io_handle_statusEt) +*(.text._ZN15GyroCalibration3RunEv) +*(.text.mavlink_start_uart_send) +*(.text.MEM_DataCopy2) +*(.text._ZNK9Commander14getPrearmStateEv) +*(.text._ZN15EstimatorChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN28FlightTaskManualAccelerationD1Ev) +*(.text._ZN11RateControl20getRateControlStatusER18rate_ctrl_status_s) +*(.text._ZN4uORB10DeviceNode15poll_notify_oneEP6pollfdm) +*(.text._ZN3GPS21handleInjectDataTopicEv.part.0) +*(.text._ZN9Commander17systemPowerUpdateEv) +*(.text._ZN4EKF221PublishGlobalPositionERKy) +*(.text._ZNK12FailsafeBase17getSelectedActionERKNS_5StateERK16failsafe_flags_sbbRNS_19SelectedActionStateE) +*(.text.imxrt_padctl_address) +*(.text._ZNK6matrix6VectorIfLj2EE4unitEv) +*(.text._ZN19RcCalibrationChecks14checkAndReportERK7ContextR6Report) +*(.text.devif_conn_callback_free) +*(.text._ZN13InnovationLpf6updateEfff.isra.0) +*(.text._ZN9Commander18landDetectorUpdateEv) +*(.text._ZN3Ekf18updateGroundEffectEv) +*(.text.nxsem_init) +*(.text._ZN9Commander16vtolStatusUpdateEv) +*(.text._ZN6matrix6MatrixIfLj4ELj1EEC1EPKf) +*(.text._ZN11ControlMath20setZeroIfNanVector3fERN6matrix7Vector3IfEE) +*(.text._ZThn8_N3ADC3RunEv) +*(.text._ZN11StickTiltXYC1EP12ModuleParams) +*(.text._ZN12SafetyButton3RunEv) +*(.text._ZN6BMP38811set_op_modeEh) +*(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) +*(.text._ZN13AnalogBattery19get_current_channelEv) +*(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN12FailsafeBase11updateDelayERKy) +*(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) +*(.text._ZN4EKF218PublishGnssHgtBiasERKy) +*(.text._ZN3Ekf21controlHaglFlowFusionEv) +*(.text._ZN6matrix6VectorIfLj3EE9normalizeEv) +*(.text._ZThn16_N7sensors10VehicleIMU3RunEv) +*(.text.__kernel_cos) +*(.text._ZN12SafetyButton19CheckPairingRequestEb) +*(.text.imxrt_dma_txavailable) +*(.text.perf_set_elapsed) +*(.text.pthread_sem_take) +*(.text._ZN8StickYawD1Ev) +*(.text._Z15blink_msg_statev) +*(.text._ZN19AccelerometerChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN8Failsafe14fromGfActParamEi) +*(.text._ZN3Ekf27checkAndFixCovarianceUpdateERKN6matrix12SquareMatrixIfLj23EEE) +*(.text._ZN3Ekf17controlBetaFusionERKN9estimator9imuSampleE) +*(.text._ZN36do_not_explicitly_use_this_namespace5ParamIfLN3px46paramsE919EEC1Ev) +*(.text._ZN22MavlinkStreamHeartbeat8get_sizeEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEdVEf) +*(.text._ZN17FlightTaskDescendC1Ev) +*(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv) +*(.text.iob_navail) +*(.text._ZN12FailsafeBase25removeNonActivatedActionsEv) +*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf) +*(.text._ZN15TakeoffHandling10updateRampEff) +*(.text._Z7led_offi) +*(.text._ZN16PreFlightChecker22selectHeadingTestLimitEv) +*(.text.led_off) +*(.text.udp_wrbuffer_test) +*(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s) +*(.text._ZNK4math17WelfordMeanVectorIfLj3EE8varianceEv) +*(.text._ZN27MavlinkStreamAttitudeTarget4sendEv) +*(.text._ZN12MixingOutput19updateSubscriptionsEb) +*(.text._ZN10FlightTaskD1Ev) +*(.text._ZThn24_N13land_detector12LandDetector3RunEv) +*(.text._ZN18MavlinkStreamDebug8get_sizeEv) +*(.text._ZN12GPSDriverUBX7receiveEj) +*(.text._ZN13BatteryStatus21parameter_update_pollEb) +*(.text._ZN3Ekf26checkYawAngleObservabilityEv) +*(.text._ZN3RTL18updateDatamanCacheEv) +*(.text.__ieee754_sqrtf) +*(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv) +*(.text.__kernel_sin) +*(.text._ZN11MissionBase17parameters_updateEv) +*(.text.nx_start) +*(.text._ZN3Ekf17controlDragFusionERKN9estimator9imuSampleE) +*(.text._ZNK8Failsafe22modifyUserIntendedModeEN12FailsafeBase6ActionES1_h) +*(.text._ZN3px417ScheduledWorkItem19schedule_trampolineEPv) +*(.text.uart_xmitchars_dma) +*(.text._ZN13land_detector23MulticopterLandDetector19_get_freefall_stateEv) +*(.text._ZThn24_N31MulticopterHoverThrustEstimator3RunEv) +*(.text._ZN11MissionBase11on_inactiveEv) +*(.text._ZN21FailureDetectorChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN12SystemChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1EPKf) +*(.text.imxrt_padmux_address) +*(.text._ZN3Ekf15setVelPosStatusEib) +*(.text._ZN19MavlinkStreamVFRHUD8get_sizeEv) +*(.text._ZN15EstimatorChecks15checkSensorBiasERK7ContextR6Report8NavModes) +*(.text._ZN20ImuConsistencyChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN17ObstacleAvoidanceD1Ev) +*(.text._ZN28MavlinkStreamGpsGlobalOrigin8get_sizeEv) +*(.text.MEM_DataCopy2_1) +*(.text._ZN6BMP3887measureEv) +*(.text._ZN4EKF217PublishRngHgtBiasERKy) +*(.text._ZN36MavlinkStreamPositionTargetGlobalInt8get_sizeEv) +*(.text._ZN28MavlinkStreamEstimatorStatus8get_sizeEv) +*(.text.up_clean_dcache) +*(.text._ZThn56_N26MulticopterPositionControl3RunEv) +*(.text._ZN16FlightTimeChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN13ManualControl12processInputEy) +*(.text._ZN17CpuResourceChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN10GyroChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN8Failsafe26fromImbalancedPropActParamEi) +*(.text._ZThn24_N13BatteryStatus3RunEv) +*(.text.mm_foreach) +*(.text._ZN35MavlinkStreamPositionTargetLocalNed8get_sizeEv) +*(.text._ZN32MavlinkStreamNavControllerOutput8get_sizeEv) +*(.text._ZN6matrix8wrap_2piIfEET_S1_) +*(.text._ZN4uORB7Manager30orb_remove_internal_subscriberEPv) +*(.text._ZN10BMP388_I2C7get_regEh) +*(.text._ZN4math17WelfordMeanVectorIfLj3EE5resetEv) +*(.text._ZN30MavlinkStreamUTMGlobalPosition10const_rateEv) +*(.text._ZN27MavlinkStreamScaledPressure8get_sizeEv) +*(.text._ZN3RTL17parameters_updateEv) +*(.text._ZN18EstimatorInterface11setBaroDataERKN9estimator10baroSampleE.part.0) +*(.text._ZN32MavlinkStreamOpenDroneIdLocation8get_sizeEv) +*(.text._ZN21MavlinkStreamTimesync4sendEv) +*(.text._ZN9Navigator23reset_position_setpointER19position_setpoint_s) +*(.text._ZN19RcAndDataLinkChecks14checkAndReportERK7ContextR6Report) +*(.text.imxrt_dma_txcallback) +*(.text._ZN24MavlinkParametersManager11send_uavcanEv) +*(.text._ZN4uORB10DeviceNode4readEP4filePcj) +*(.text._ZN4uORB10DeviceNode10poll_stateEP4file) +*(.text._ZN4uORB7Manager8orb_copyEPK12orb_metadataiPv) +*(.text._ZN27MavlinkStreamServoOutputRawILi0EE8get_sizeEv) +*(.text._ZN30MavlinkStreamUTMGlobalPosition8get_sizeEv) +*(.text._ZN8Geofence3runEv) +*(.text._ZN15EstimatorChecks25checkEstimatorStatusFlagsERK7ContextR6ReportRK18estimator_status_sRK24vehicle_local_position_s) +*(.text._ZN18MagnetometerChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN6events9SendEvent3RunEv) +*(.text._ZN30MavlinkStreamGlobalPositionInt8get_sizeEv) +*(.text._ZN22MavlinkStreamESCStatus8get_sizeEv) +*(.text._Z20px4_spi_bus_externalRK13px4_spi_bus_t) +*(.text.read) +*(.text._ZN4uORB15PublicationBaseD1Ev) +*(.text._ZN22MavlinkStreamDebugVect8get_sizeEv) +*(.text._ZN22MavlinkStreamCollision8get_sizeEv) +*(.text._ZN7Mission11on_inactiveEv) +*(.text._ZN7sensors19VehicleMagnetometer20UpdateMagCalibrationEv) +*(.text._ZN11calibration27FindCurrentCalibrationIndexEPKcm) +*(.text._ZN4cdevL9cdev_readEP4filePcj) +*(.text.sem_timedwait) +*(.text.snprintf) +*(.text._ZN27MavlinkStreamOpticalFlowRad8get_sizeEv) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE6copyToEPf) +*(.text._ZN6Report13healthFailureIJhEEEv8NavModes20HealthComponentIndexmRKN6events9LogLevelsEPKcDpT_.isra.0.constprop.0) +*(.text._ZN13BatteryChecks16rtlEstimateCheckERK7ContextR6Reportf) +*(.text.sigemptyset) +*(.text.nx_read) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld new file mode 100644 index 0000000000..2f6259209a --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld @@ -0,0 +1,135 @@ +/* Static */ +*(.text.arm_ack_irq) +*(.text.arm_doirq) +*(.text.arm_svcall) +*(.text.arm_switchcontext) +*(.text.board_autoled_on) +*(.text.clock_timer) +*(.text.exception_common) +*(.text.flexio_irq_handler) +*(.text.hrt_absolute_time) +*(.text.hrt_call_enter) +*(.text.hrt_tim_isr) +*(.text.imxrt_configwaitints) +*(.text.imxrt_dma_callback) +*(.text.imxrt_dmach_interrupt) +*(.text.imxrt_dmach_xfrsetup) +*(.text.imxrt_dmaterminate) +*(.text.imxrt_dispatch) +*(.text.imxrt_edma_interrupt) +*(.text.imxrt_endwait) +*(.text.imxrt_enet_interrupt) +*(.text.imxrt_enet_interrupt_work) +*(.text.imxrt_gpio3_16_31_interrupt) +*(.text.imxrt_interrupt) +*(.text.imxrt_lpi2c_isr) +*(.text.imxrt_lpspi_exchange) +*(.text.imxrt_recvdma) +*(.text.imxrt_tcd_free) +*(.text.imxrt_timerisr) +*(.text.imxrt_transmit) +*(.text.imxrt_txdone) +*(.text.imxrt_txtimeout_work) +*(.text.imxrt_txtimeout_expiry) +*(.text.imxrt_txpoll) +*(.text.imxrt_txringfull) +*(.text.imxrt_txavail_work) +*(.text.imxrt_txavail) +*(.text.imxrt_usbinterrupt) +*(.text.irq_dispatch) +*(.text.ioctl) +*(.text.memcpy) +*(.text.memset) +*(.text.nxsched_add_blocked) +*(.text.nxsched_add_prioritized) +*(.text.nxsched_add_readytorun) +*(.text.nxsched_get_files) +*(.text.nxsched_get_tcb) +*(.text.nxsched_merge_pending) +*(.text.nxsched_process_timer) +*(.text.nxsched_remove_blocked) +*(.text.nxsched_remove_readytorun) +*(.text.nxsched_resume_scheduler) +*(.text.nxsched_suspend_scheduler) +*(.text.nxsem_add_holder) +*(.text.nxsem_add_holder_tcb) +*(.text.nxsem_clockwait) +*(.text.nxsem_foreachholder) +*(.text.nxsem_freecount0holder) +*(.text.nxsem_freeholder) +*(.text.nxsem_post) +*(.text.nxsem_release_holder) +*(.text.nxsem_restore_baseprio) +*(.text.nxsem_tickwait) +*(.text.nxsem_timeout) +*(.text.nxsem_trywait) +*(.text.nxsem_wait) +*(.text.nxsem_wait_uninterruptible) +*(.text.nxsig_timedwait) +*(.text.sched_lock) +*(.text.sched_note_resume) +*(.text.sched_note_suspend) +*(.text.sched_unlock) +*(.text.strcmp) +*(.text.sq_addafter) +*(.text.sq_addlast) +*(.text.sq_rem) +*(.text.sq_remafter) +*(.text.sq_remfirst) +*(.text.uart_connected) +*(.text.up_block_task) +*(.text.up_unblock_task) +*(.text.wd_timer) +*(.text.wd_start) +*(.text.work_thread) +*(.text.work_queue) +*(.text._do_memcpy) + +/* Tropic Eth tune */ +*(.text.devif_poll) +*(.text.devif_poll_tcp_connections) +*(.text.tcp_poll) +*(.text.devif_poll_udp_connections) +*(.text.udp_nextconn) +*(.text.udp_poll) +*(.text.udp_ipv4_select) +*(.text.udp_callback) +*(.text.udp_datahandler) +*(.text.udp_send) +*(.text.udp_active) +*(.text.udp_ipv4_active) +*(.text.psock_udp_sendto) +*(.text.sendto_eventhandler) +*(.text.net_dataevent) +*(.text.devif_conn_event) +*(.text.devif_event_trigger) +*(.text.devif_poll_icmp) +*(.text.icmp_poll) +*(.text.devif_packet_conversion) +*(.text.arp_out) +*(.text.arp_find) +*(.text.arp_format) +*(.text.net_ipv4addr_hdrcmp) +*(.text.net_ipv4addr_copy) +*(.text.net_ipv4addr_broadcast) +*(.text.wd_start) +*(.text.arp_arpin) +*(.text.ipv4_input) +*(.text.work_thread) +*(.text.work_queue) + +/* Flash Storage */ +*(.text.imxrt_flexspi_transfer_blocking) +*(.text.imxrt_flexspi_transfer_blocking_private) +*(.text.imxrt_flexspi_write_blocking) +*(.text.imxrt_flexspi_read_blocking) +*(.text.imxrt_flexspi_check_and_clear_error) +*(.text.imxrt_flexspi_get_bus_idle_status) +*(.text.imxrt_flexspi_configure_prefetch) +*(.text.imxrt_flexspi_configure_prefetch_private) +*(.text.imxrt_flexspi_storage_write_enable) +*(.text.imxrt_flexspi_storage_wait_bus_busy) +*(.text.imxrt_flexspi_storage_read_status) +*(.text.imxrt_flexspi_storage_erase) +*(.text.imxrt_flexspi_storage_bwrite) +*(.text.imxrt_flexspi_storage_page_program) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/script.ld b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..01d2e8d951 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld @@ -0,0 +1,179 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/scripts/flash.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Specify the memory areas */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x60000000, LENGTH = 7936K - 128K + sram (rwx) : ORIGIN = 0x20200000, LENGTH = 512K + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 384K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flash_config) +EXTERN(g_image_vector_table) +EXTERN(g_boot_data) + +ENTRY(_stext) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } >flash + + .vectors : + { + KEEP(*(.vectors)) + *(.text .text.__start) + } >flash + + .itcmfunc : + { + . = ALIGN(8); + _sitcmfuncs = ABSOLUTE(.); + FILL(0xFF) + . = 0x40 ; + INCLUDE "itcm_static_functions.ld" + INCLUDE "itcm_functions_includes.ld" + . = ALIGN(8); + _eitcmfuncs = ABSOLUTE(.); + } > itcm AT > flash + + _fitcmfuncs = LOADADDR(.itcmfunc); + + /* The RAM vector table (if present) should lie at the beginning of SRAM */ + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > dtcm + + + /* Workaround for ethernet issue, by placing g_desc_pool into DTCM, + which effectively puts it into a no-cache region */ + .dtcm : { + *(.bss.g_desc_pool) + } > dtcm + + .text : + { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + . = ALIGN(4096); + _etext = ABSOLUTE(.); + _srodata = ABSOLUTE(.); + *(.rodata .rodata.*) + . = ALIGN(4096); + _erodata = ABSOLUTE(.); + } > flash + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(SORT_BY_INIT_PRIORITY(.init_array.*) SORT_BY_INIT_PRIORITY(.ctors.*))) + KEEP(*(.init_array .ctors)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } > flash + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + _sdtcm = ORIGIN(dtcm); + _edtcm = ORIGIN(dtcm) + LENGTH(dtcm); +} diff --git a/boards/nxp/tropic-community/src/CMakeLists.txt b/boards/nxp/tropic-community/src/CMakeLists.txt new file mode 100644 index 0000000000..6c37994652 --- /dev/null +++ b/boards/nxp/tropic-community/src/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(drivers_board + i2c.cpp + init.c + sdhc.c + spi.cpp + timer_config.cpp + tropic_led_pwm.cpp + usb.c + imxrt_flexspi_nor_boot.c + imxrt_flexspi_nor_flash.c + imxrt_flexspi_storage.c + imxrt_ocram_initialize.c +) + +# Force compiler not to use builtin functions (like memcpy) +# to optimize for loops in init.c (imxrt_ocram_initialize) +set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin) + +target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/nxp/tropic-community/src/board_config.h b/boards/nxp/tropic-community/src/board_config.h new file mode 100644 index 0000000000..83bbd101e2 --- /dev/null +++ b/boards/nxp/tropic-community/src/board_config.h @@ -0,0 +1,333 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Tropic Community internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include +#include + +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" +#include "hardware/imxrt_usb_analog.h" + +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Configuration ************************************************************************************/ + +/* FMURT1062 GPIOs ***********************************************************************************/ +/* LEDs */ +/* An RGB LED is connected through GPIO as shown below: + */ +#define LED_IOMUX (IOMUX_OPENDRAIN | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) +#define GPIO_nLED_RED /* GPIO_AD_B0_02 GPIO1_IO02 */ (GPIO_PORT1 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_GREEN /* GPIO_B1_03 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_BLUE /* GPIO_AD_B0_03 GPIO1_IO03 */ (GPIO_PORT1 | GPIO_PIN3 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT)) + +/* Define the Chip Selects, Data Ready and Control signals per SPI bus */ + +#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST) +#define OUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + + +/* SPI1 off */ + +#define _GPIO_LPSPI1_SCK /* GPIO_EMC_27 GPIO4_IO27 */ (GPIO_PORT4 | GPIO_PIN27 | CS_IOMUX) +#define _GPIO_LPSPI1_MISO /* GPIO_EMC_29 GPIO4_IO29 */ (GPIO_PORT4 | GPIO_PIN29 | CS_IOMUX) +#define _GPIO_LPSPI1_MOSI /* GPIO_EMC_28 GPIO4_IO28 */ (GPIO_PORT4 | GPIO_PIN28 | CS_IOMUX) + +#define GPIO_SPI1_SCK_OFF _PIN_OFF(_GPIO_LPSPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(_GPIO_LPSPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(_GPIO_LPSPI1_MOSI) + +#define _GPIO_LPSPI3_SCK /* GPIO_AD_B1_15 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN31 | CS_IOMUX) +#define _GPIO_LPSPI3_MISO /* GPIO_AD_B1_13 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN29 | CS_IOMUX) +#define _GPIO_LPSPI3_MOSI /* GPIO_AD_B1_14 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN30 | CS_IOMUX) + +#define GPIO_SPI3_SCK_OFF _PIN_OFF(_GPIO_LPSPI3_SCK) +#define GPIO_SPI3_MISO_OFF _PIN_OFF(_GPIO_LPSPI3_MISO) +#define GPIO_SPI3_MOSI_OFF _PIN_OFF(_GPIO_LPSPI3_MOSI) + +/* Define the SPI4 Data Ready and Control signals */ + +#define GPIO_SPI4_DRDY7_EXTERNAL1 /* GPIO_EMC_35 GPIO3_IO21*/ (GPIO_PORT3 | GPIO_PIN21 | GPIO_INPUT | DRDY_IOMUX) + +#define GPIO_DRDY_OFF_SPI4_DRDY7_EXTERNAL1 _PIN_OFF(GPIO_SPI4_DRDY7_EXTERNAL1) + + +#define ADC_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) + +#define ADC1_CH(n) (n) +#define ADC1_GPIO(n, p) (GPIO_PORT1 | GPIO_PIN##p | ADC_IOMUX) // + +/* Define GPIO pins used as ADC N.B. Channel numbers are for reference, */ + +#define PX4_ADC_GPIO \ + /* BATTERY1_VOLTAGE GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_GPIO(0, 27), \ + /* BATTERY1_CURRENT GPIO_AD_B1_11 GPIO1 Pin 24 */ ADC1_GPIO(0, 24) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY_VOLTAGE_CHANNEL /* GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_CH(13) +#define ADC_BATTERY_CURRENT_CHANNEL /* GPIO_AD_B1_11 GPIO1 Pin 12 */ ADC1_CH(0) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ + +//#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* Power supply control and monitoring GPIOs */ + +#define GENERAL_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ) +#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + + +/* PWM + */ + +#define DIRECT_PWM_OUTPUT_CHANNELS 6 +#define BOARD_NUM_IO_TIMERS 4 + +// Input Capture not supported on MVP + +#define BOARD_HAS_NO_CAPTURE + +//#define BOARD_HAS_UI_LED_PWM 1 Not ported yet (Still Kinetis driver) +#define BOARD_HAS_LED_PWM 1 +#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 +#define BOARD_HAS_CUSTOM_LED_PWM 1 + +#define PWM_LED_RED /* GPIO_AD_B0_02 */ (GPIO_XBAR1_INOUT16_1 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_GREEN /* GPIO_B1_03 */ (GPIO_FLEXPWM2_PWMB03_4 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_BLUE /* GPIO_AD_B0_03 */ (GPIO_XBAR1_INOUT17_1 | GENERAL_OUTPUT_IOMUX) + + + +/* Tone alarm output */ + +//FIXME FlexPWM TONE DRIVER +#define TONE_ALARM_FLEXPWM PWMA_VAL +#define TONE_ALARM_TIMER 3 /* FlexPWM 3 */ +#define TONE_ALARM_CHANNEL 1 /* GPIO_EMC_31 PWM3_PWMA01 */ + +#define GPIO_BUZZER_1 /* GPIO_EMC_31 GPIO4_IO31 */ (GPIO_PORT1 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM (GPIO_FLEXPWM3_PWMA01_1 | GENERAL_OUTPUT_IOMUX) + +/* USB OTG FS + * + * VBUS_VALID is detected in USB_ANALOG_USB1_VBUS_DETECT_STAT + */ + +/* High-resolution timer */ +#define HRT_TIMER 1 /* use GPT1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define RC_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_SINGLEWIRE 1 // Suport Single wire wiring +#define RC_SERIAL_SWAP_RXTX 1 // Set Swap (but not supported in HW) to use Single wire +#define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* Safety Switch is HW version dependent on having an PX4IO + * So we init to a benign state with the _INIT definition + * and provide the the non _INIT one for the driver to make a run time + * decision to use it. + */ +#define SAFETY_INIT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) +#define SAFETY_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) +#define SAFETY_SW_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_22K | IOMUX_DRIVE_HIZ) + +#define GPIO_SAFETY_SWITCH_IN /* GPIO_B0_12 GPIO2_IO12 */ (GPIO_PORT2 | GPIO_PIN12 | GPIO_INPUT | SAFETY_SW_IOMUX) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ + +#define BOARD_NUMBER_BRICKS 1 + +/* Use USB2 VBUS 4.7V comparator for valid check */ +static inline int board_read_usb2_vbus_state(void) +{ + return (getreg32(IMXRT_USB_ANALOG_USB2_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_VALID) ? 0 : 1; +} + +#define BOARD_ADC_BRICK_VALID (board_read_usb2_vbus_state() == 0) + + + + +#define BOARD_ADC_USB_CONNECTED (board_read_VBUS_state() == 0) + +/* FMUv5 never powers odd the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +// FIXME LETS NOT RESET FOR NOW +//#define BOARD_HAS_ON_RESET 1 +#define BOARD_HAS_TEENSY_BOOTLOADER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_FLEXCAN3_TX, \ + GPIO_FLEXCAN3_RX, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: fmurt1062_usdhc_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int fmurt1062_usdhc_initialize(void); + +/**************************************************************************************************** + * Name: imxrt_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void imxrt_spidev_initialize(void); + +/************************************************************************************ + * Name: imxrt_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +extern int imxrt1062_spi_bus_initialize(void); + +/************************************************************************************ + * Name: imxrt_usb_initialize + * + * Description: + * Called to configure USB. + * + ************************************************************************************/ + +extern int imxrt_usb_initialize(void); + +extern void imxrt_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +extern void fmurt1062_timer_initialize(void); + +#include + +int imxrt_flexspi_storage_initialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/nxp/tropic-community/src/i2c.cpp b/boards/nxp/tropic-community/src/i2c.cpp new file mode 100644 index 0000000000..6bab2dee4a --- /dev/null +++ b/boards/nxp/tropic-community/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(4), +}; diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.c b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.c new file mode 100644 index 0000000000..90237e7f23 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_boot.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "imxrt_flexspi_nor_boot.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +locate_data(".boot_hdr.ivt") +const struct ivt_s g_image_vector_table = { + IVT_HEADER, /* IVT Header */ + IMAGE_ENTRY_ADDRESS, /* Image Entry Function */ + IVT_RSVD, /* Reserved = 0 */ + (uint32_t) DCD_ADDRESS, /* Address where DCD information is + * stored */ + (uint32_t) BOOT_DATA_ADDRESS, /* Address where BOOT Data Structure + * is stored */ + (uint32_t) IMAG_VECTOR_TABLE, /* Pointer to IVT Self (absolute + * address */ + (uint32_t) CSF_ADDRESS, /* Address where CSF file is stored */ + IVT_RSVD /* Reserved = 0 */ +}; + +locate_data(".boot_hdr.boot_data") +const struct boot_data_s g_boot_data = { + IMAGE_DEST, /* boot start location */ + (IMAGE_DEST_END - IMAGE_DEST), /* size */ + PLUGIN_FLAG, /* Plugin flag */ + 0xffffffff /* empty - extra data word */ +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.h b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.h new file mode 100644 index 0000000000..75e6d9c463 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.h @@ -0,0 +1,155 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_boot.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_BOOT_H +#define __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_BOOT_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* IVT Data */ + +#define IVT_MAJOR_VERSION 0x4 +#define IVT_MAJOR_VERSION_SHIFT 0x4 +#define IVT_MAJOR_VERSION_MASK 0xf +#define IVT_MINOR_VERSION 0x1 +#define IVT_MINOR_VERSION_SHIFT 0x0 +#define IVT_MINOR_VERSION_MASK 0xf + +#define IVT_VERSION(major, minor) \ + ((((major) & IVT_MAJOR_VERSION_MASK) << IVT_MAJOR_VERSION_SHIFT) | \ + (((minor) & IVT_MINOR_VERSION_MASK) << IVT_MINOR_VERSION_SHIFT)) + +#define IVT_TAG_HEADER (0xd1) /* Image Vector Table */ +#define IVT_SIZE 0x2000 +#define IVT_PAR IVT_VERSION(IVT_MAJOR_VERSION, IVT_MINOR_VERSION) + +#define IVT_HEADER (IVT_TAG_HEADER | (IVT_SIZE << 8) | (IVT_PAR << 24)) +#define IVT_RSVD (uint32_t)(0x00000000) + +/* DCD Data */ + +#define DCD_TAG_HEADER (0xd2) +#define DCD_TAG_HEADER_SHIFT (24) +#define DCD_VERSION (0x41) +#define DCD_ARRAY_SIZE 1 + +#define FLASH_BASE 0x60000000 +#define FLASH_END 0x7f7fffff + +/* This needs to take into account the memory configuration at + * boot bootloader + */ + +#define ROM_BOOTLOADER_OCRAM_RES 0x8000 +#define OCRAM_BASE (0x20200000 + ROM_BOOTLOADER_OCRAM_RES) +#define OCRAM_END (OCRAM_BASE + (512 * 1024) + (256 * 1024) \ + - ROM_BOOTLOADER_OCRAM_RES) + +#define SCLK 1 +#if defined(CONFIG_BOOT_RUNFROMFLASH) +# define IMAGE_DEST FLASH_BASE +# define IMAGE_DEST_END FLASH_END +# define IMAGE_DEST_OFFSET 0 +#else +# define IMAGE_DEST OCRAM_BASE +# define IMAGE_DEST_END OCRAM_END +# define IMAGE_DEST_OFFSET IVT_SIZE +#endif + +#define LOCATE_IN_DEST(x) (((uint32_t)(x)) - FLASH_BASE + IMAGE_DEST) +#define LOCATE_IN_SRC(x) (((uint32_t)(x)) - IMAGE_DEST + FLASH_BASE) + +#define DCD_ADDRESS 0 +#define BOOT_DATA_ADDRESS LOCATE_IN_DEST(&g_boot_data) +#define CSF_ADDRESS 0 +#define PLUGIN_FLAG (uint32_t)0 + +/* Located in Destination Memory */ + +#define IMAGE_ENTRY_ADDRESS ((uint32_t)&_vectors) +#define IMAG_VECTOR_TABLE LOCATE_IN_DEST(&g_image_vector_table) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* IVT Data */ + +struct ivt_s { + /* Header with tag #HAB_TAG_IVT, length and HAB version fields */ + + uint32_t hdr; + + /* Absolute address of the first instr. to execute from the image */ + + uint32_t entry; + + /* Reserved in this version of HAB: should be NULL. */ + + uint32_t reserved1; + + /* Absolute address of the image DCD: may be NULL. */ + + uint32_t dcd; + + /* Absolute address of the Boot Data: may be NULL */ + + uint32_t boot_data; + + /* Absolute address of the IVT. */ + + uint32_t self; + + /* Absolute address of the image CSF. */ + + uint32_t csf; + + /* Reserved in this version of HAB: should be zero. */ + + uint32_t reserved2; +}; + +/* Boot Data */ + +struct boot_data_s { + uint32_t start; /* boot start location */ + uint32_t size; /* size */ + uint32_t plugin; /* plugin flag - 1 if downloaded application is + * plugin */ + uint32_t placeholder; /* placeholder to make even 0x10 size */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const struct boot_data_s g_boot_data; +extern const uint8_t g_dcd_data[]; +extern const uint32_t _vectors[]; + +#endif /* __BOARDS_ARM_IMXRT_TEENSY_4_SRC_IMXRT_FLEXSPI_NOR_BOOT_H */ diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.c b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.c new file mode 100644 index 0000000000..f7ffe4e46d --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_flash.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "imxrt_flexspi_nor_flash.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +locate_data(".boot_hdr.conf") +const struct flexspi_nor_config_s g_flash_config = { + .mem_config = + { + .tag = FLEXSPI_CFG_BLK_TAG, + .version = FLEXSPI_CFG_BLK_VERSION, + .read_sample_clksrc = FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD, + .cs_hold_time = 1u, + .cs_setup_time = 1u, + .column_address_width = 0u, + .device_type = FLEXSPI_DEVICE_TYPE_SERIAL_NOR, + .sflash_pad_type = SERIAL_FLASH_4PADS, + .serial_clk_freq = FLEXSPI_SERIAL_CLKFREQ_133MHz, + .sflash_a1size = 8u * 1024u * 1024u, + .data_valid_time = + { + 0u, 0u + }, + .lookup_table = + { + /* Fast Read Quad I/O */ + [0 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xeb, + RADDR_SDR, FLEXSPI_4PAD, 0x18), + [0 + 1] = FLEXSPI_LUT_SEQ(DUMMY_SDR, FLEXSPI_4PAD, 0x06, + READ_SDR, FLEXSPI_4PAD, 0x04), + + /* Read Status Register-1 */ + [4 * 1 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x05, + READ_SDR, FLEXSPI_1PAD, 0x04), + + /* Write Status Register-1 */ + [4 * 3 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x01, STOP, FLEXSPI_1PAD, 0x0), + + /* Sector Erase (4KB) */ + [4 * 5 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x20, + RADDR_SDR, FLEXSPI_1PAD, 0x18), + + /* Block Erase (64KB) */ + [4 * 8 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xd8, + RADDR_SDR, FLEXSPI_1PAD, 0x18), + + /* Page Program */ + [4 * 9 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x02, + RADDR_SDR, FLEXSPI_1PAD, 0x18), + [4 * 9 + 1] = FLEXSPI_LUT_SEQ(WRITE_SDR, FLEXSPI_1PAD, 0x04, + STOP, FLEXSPI_1PAD, 0x0), + + /* Chip Erase */ + [4 * 11 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x60, STOP, FLEXSPI_1PAD, 0x0), + }, + }, + + .page_size = 256u, + .sector_size = 4u * 1024u, + .blocksize = 64u * 1024u, + .is_uniform_blocksize = false, +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.h b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.h new file mode 100644 index 0000000000..11904fae65 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.h @@ -0,0 +1,354 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_flash.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ +#ifndef __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_FLASH_H +#define __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_FLASH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* FLEXSPI memory config block related definitions */ + +#define FLEXSPI_CFG_BLK_TAG (0x42464346ul) +#define FLEXSPI_CFG_BLK_VERSION (0x56010100ul) +#define FLEXSPI_CFG_BLK_SIZE (512) + +/* FLEXSPI Feature related definitions */ + +#define FLEXSPI_FEATURE_HAS_PARALLEL_MODE 1 + +/* Lookup table related definitions */ + +#define CMD_INDEX_READ 0 +#define CMD_INDEX_READSTATUS 1 +#define CMD_INDEX_WRITEENABLE 2 +#define CMD_INDEX_WRITE 4 + +#define CMD_LUT_SEQ_IDX_READ 0 +#define CMD_LUT_SEQ_IDX_READSTATUS 1 +#define CMD_LUT_SEQ_IDX_WRITEENABLE 3 +#define CMD_LUT_SEQ_IDX_WRITE 9 + +#define CMD_SDR 0x01 +#define CMD_DDR 0x21 +#define RADDR_SDR 0x02 +#define RADDR_DDR 0x22 +#define CADDR_SDR 0x03 +#define CADDR_DDR 0x23 +#define MODE1_SDR 0x04 +#define MODE1_DDR 0x24 +#define MODE2_SDR 0x05 +#define MODE2_DDR 0x25 +#define MODE4_SDR 0x06 +#define MODE4_DDR 0x26 +#define MODE8_SDR 0x07 +#define MODE8_DDR 0x27 +#define WRITE_SDR 0x08 +#define WRITE_DDR 0x28 +#define READ_SDR 0x09 +#define READ_DDR 0x29 +#define LEARN_SDR 0x0a +#define LEARN_DDR 0x2a +#define DATSZ_SDR 0x0b +#define DATSZ_DDR 0x2b +#define DUMMY_SDR 0x0c +#define DUMMY_DDR 0x2c +#define DUMMY_RWDS_SDR 0x0d +#define DUMMY_RWDS_DDR 0x2d +#define JMP_ON_CS 0x1f +#define STOP 0 + +#define FLEXSPI_1PAD 0 +#define FLEXSPI_2PAD 1 +#define FLEXSPI_4PAD 2 +#define FLEXSPI_8PAD 3 + +#define FLEXSPI_LUT_OPERAND0_MASK (0xffu) +#define FLEXSPI_LUT_OPERAND0_SHIFT (0U) +#define FLEXSPI_LUT_OPERAND0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPERAND0_SHIFT)) & \ + FLEXSPI_LUT_OPERAND0_MASK) +#define FLEXSPI_LUT_NUM_PADS0_MASK (0x300u) +#define FLEXSPI_LUT_NUM_PADS0_SHIFT (8u) +#define FLEXSPI_LUT_NUM_PADS0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS0_SHIFT)) & \ + FLEXSPI_LUT_NUM_PADS0_MASK) +#define FLEXSPI_LUT_OPCODE0_MASK (0xfc00u) +#define FLEXSPI_LUT_OPCODE0_SHIFT (10u) +#define FLEXSPI_LUT_OPCODE0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPCODE0_SHIFT)) & \ + FLEXSPI_LUT_OPCODE0_MASK) +#define FLEXSPI_LUT_OPERAND1_MASK (0xff0000u) +#define FLEXSPI_LUT_OPERAND1_SHIFT (16U) +#define FLEXSPI_LUT_OPERAND1(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPERAND1_SHIFT)) & \ + FLEXSPI_LUT_OPERAND1_MASK) +#define FLEXSPI_LUT_NUM_PADS1_MASK (0x3000000u) +#define FLEXSPI_LUT_NUM_PADS1_SHIFT (24u) +#define FLEXSPI_LUT_NUM_PADS1(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS1_SHIFT)) & \ + FLEXSPI_LUT_NUM_PADS1_MASK) +#define FLEXSPI_LUT_OPCODE1_MASK (0xfc000000u) +#define FLEXSPI_LUT_OPCODE1_SHIFT (26u) +#define FLEXSPI_LUT_OPCODE1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPCODE1_SHIFT)) & \ + FLEXSPI_LUT_OPCODE1_MASK) + +#define FLEXSPI_LUT_SEQ(cmd0, pad0, op0, cmd1, pad1, op1) \ + (FLEXSPI_LUT_OPERAND0(op0) | FLEXSPI_LUT_NUM_PADS0(pad0) | \ + FLEXSPI_LUT_OPCODE0(cmd0) | FLEXSPI_LUT_OPERAND1(op1) | \ + FLEXSPI_LUT_NUM_PADS1(pad1) | FLEXSPI_LUT_OPCODE1(cmd1)) + +/* */ + +#define NOR_CMD_INDEX_READ CMD_INDEX_READ +#define NOR_CMD_INDEX_READSTATUS CMD_INDEX_READSTATUS +#define NOR_CMD_INDEX_WRITEENABLE CMD_INDEX_WRITEENABLE +#define NOR_CMD_INDEX_ERASESECTOR 3 +#define NOR_CMD_INDEX_PAGEPROGRAM CMD_INDEX_WRITE +#define NOR_CMD_INDEX_CHIPERASE 5 +#define NOR_CMD_INDEX_DUMMY 6 +#define NOR_CMD_INDEX_ERASEBLOCK 7 + +/* READ LUT sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READ CMD_LUT_SEQ_IDX_READ + +/* Read Status LUT sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS CMD_LUT_SEQ_IDX_READSTATUS + +/* 2 Read status DPI/QPI/OPI sequence id in LUT stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS_XPI 2 + +/* 3 Write Enable sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE CMD_LUT_SEQ_IDX_WRITEENABLE + +/* 4 Write Enable DPI/QPI/OPI sequence id in LUT stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE_XPI 4 + +/* 5 Erase Sector sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_ERASESECTOR 5 + +/* 8 Erase Block sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_ERASEBLOCK 8 + +/* 9 Program sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_PAGEPROGRAM CMD_LUT_SEQ_IDX_WRITE + +/* 11 Chip Erase sequence in lookupTable id stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_CHIPERASE 11 + +/* 13 Read SFDP sequence in lookupTable id stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READ_SFDP 13 + +/* 14 Restore 0-4-4/0-8-8 mode sequence id in LUT stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_RESTORE_NOCMD 14 + +/* 15 Exit 0-4-4/0-8-8 mode sequence id in LUT stored in config blobk */ + +#define NOR_CMD_LUT_SEQ_IDX_EXIT_NOCMD 15 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* Definitions for FlexSPI Serial Clock Frequency */ + +enum flexspi_serial_clkfreq_e { + FLEXSPI_SERIAL_CLKFREQ_30MHz = 1, + FLEXSPI_SERIAL_CLKFREQ_50MHz = 2, + FLEXSPI_SERIAL_CLKFREQ_60MHz = 3, + FLEXSPI_SERIAL_CLKFREQ_75MHz = 4, + FLEXSPI_SERIAL_CLKFREQ_80MHz = 5, + FLEXSPI_SERIAL_CLKFREQ_100MHz = 6, + FLEXSPI_SERIAL_CLKFREQ_120MHz = 7, + FLEXSPI_SERIAL_CLKFREQ_133MHz = 8, + FLEXSPI_SERIAL_CLKFREQ_166MHz = 9, +}; + +/* FlexSPI clock configuration type */ + +enum flexspi_serial_clockmode_e { + FLEXSPI_CLKMODE_SDR, + FLEXSPI_CLKMODE_DDR, +}; + +/* FlexSPI Read Sample Clock Source definition */ + +enum flash_read_sample_clk_e { + FLASH_READ_SAMPLE_CLK_LOOPBACK_INTERNELLY = 0, + FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_DQSPAD = 1, + FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD = 2, + FLASH_READ_SAMPLE_CLK_EXTERNALINPUT_FROM_DQSPAD = 3, +}; + +/* Misc feature bit definitions */ + +enum flash_misc_feature_e { + FLEXSPIMISC_OFFSET_DIFFCLKEN = 0, /* Bit for Differential clock enable */ + FLEXSPIMISC_OFFSET_CK2EN = 1, /* Bit for CK2 enable */ + FLEXSPIMISC_OFFSET_PARALLELEN = 2, /* Bit for Parallel mode enable */ + + FLEXSPIMISC_OFFSET_WORD_ADDRESSABLE_EN = 3, /* Bit for Word Addressable + * enable */ + FLEXSPIMISC_OFFSET_SAFECONFIG_FREQ_EN = 4, /* Bit for Safe Configuration + * Frequency enable */ + + FLEXSPIMISC_OFFSET_PAD_SETTING_OVERRIDE_EN = 5, /* Bit for Pad setting + * override enable */ + + FLEXSPIMISC_OFFSET_DDR_MODE_EN = 6, /* Bit for DDR clock confiuration + * indication. */ +}; + +/* Flash Type Definition */ + +enum flash_flash_type_e { + FLEXSPI_DEVICE_TYPE_SERIAL_NOR = 1, /* Flash devices are Serial NOR */ + FLEXSPI_DEVICE_TYPE_SERIAL_NAND = 2, /* Flash devices are Serial + * NAND */ + FLEXSPI_DEVICE_TYPE_SERIAL_RAM = 3, /* Flash devices are Serial + * RAM/HyperFLASH */ + + FLEXSPI_DEVICE_TYPE_MCP_NOR_NAND = 0x12, /* Flash device is MCP device, + * A1 is Serial NOR, A2 is + * Serial NAND */ + FLEXSPI_DEVICE_TYPE_MCP_NOR_RAM = 0x13, /* Flash device is MCP device, + * A1 is Serial NOR, A2 is + * Serial RAMs */ +}; + +/* Flash Pad Definitions */ + +enum flash_flash_pad_e { + SERIAL_FLASH_1PAD = 1, + SERIAL_FLASH_2PADS = 2, + SERIAL_FLASH_4PADS = 4, + SERIAL_FLASH_8PADS = 8, +}; + +/* Flash Configuration Command Type */ + +enum flash_config_cmd_e { + DEVICE_CONFIG_CMD_TYPE_GENERIC, /* Generic command, for example: + * configure dummy cycles, drive + * strength, etc */ + DEVICE_CONFIG_CMD_TYPE_QUADENABLE, /* Quad Enable command */ + DEVICE_CONFIG_CMD_TYPE_SPI2XPI, /* Switch from SPI to DPI/QPI/OPI mode */ + DEVICE_CONFIG_CMD_TYPE_XPI2SPI, /* Switch from DPI/QPI/OPI to SPI mode */ + DEVICE_CONFIG_CMD_TYPE_SPI2NO_CMD, /* Switch to 0-4-4/0-8-8 mode */ + DEVICE_CONFIG_CMD_TYPE_RESET, /* Reset device command */ +}; + +/* FlexSPI LUT Sequence structure */ + +struct flexspi_lut_seq_s { + uint8_t seq_num; /* Sequence Number, valid number: 1-16 */ + uint8_t seq_id; /* Sequence Index, valid number: 0-15 */ + uint16_t reserved; +}; + +/* FlexSPI Memory Configuration Block */ + +struct flexspi_mem_config_s { + uint32_t tag; + uint32_t version; + uint32_t reserved0; + uint8_t read_sample_clksrc; + uint8_t cs_hold_time; + uint8_t cs_setup_time; + uint8_t column_address_width; /* [0x00f-0x00f] Column Address with, + * for HyperBus protocol, it is fixed + * to 3, For Serial NAND, need to refer + * to datasheet */ + uint8_t device_mode_cfg_enable; + uint8_t device_mode_type; + uint16_t wait_time_cfg_commands; + struct flexspi_lut_seq_s device_mode_seq; + uint32_t device_mode_arg; + uint8_t config_cmd_enable; + uint8_t config_mode_type[3]; + struct flexspi_lut_seq_s config_cmd_seqs[3]; + uint32_t reserved1; + uint32_t config_cmd_args[3]; + uint32_t reserved2; + uint32_t controller_misc_option; + uint8_t device_type; + uint8_t sflash_pad_type; + uint8_t serial_clk_freq; + uint8_t lut_custom_seq_enable; + uint32_t reserved3[2]; + uint32_t sflash_a1size; + uint32_t sflash_a2size; + uint32_t sflash_b1size; + uint32_t sflash_b2size; + uint32_t cspad_setting_override; + uint32_t sclkpad_setting_override; + uint32_t datapad_setting_override; + uint32_t dqspad_setting_override; + uint32_t timeout_in_ms; + uint32_t command_interval; + uint16_t data_valid_time[2]; + uint16_t busy_offset; + uint16_t busybit_polarity; + uint32_t lookup_table[64]; + struct flexspi_lut_seq_s lut_customseq[12]; + uint32_t reserved4[4]; +}; + +/* Serial NOR configuration block */ + +struct flexspi_nor_config_s { + struct flexspi_mem_config_s mem_config; /* Common memory configuration + * info via FlexSPI */ + + uint32_t page_size; /* Page size of Serial NOR */ + uint32_t sector_size; /* Sector size of Serial NOR */ + uint8_t ipcmd_serial_clkfreq; /* Clock frequency for IP command */ + uint8_t is_uniform_blocksize; /* Sector/Block size is the same */ + uint8_t reserved0[2]; /* Reserved for future use */ + uint8_t serial_nor_type; /* Serial NOR Flash type: 0/1/2/3 */ + uint8_t need_exit_nocmdmode; /* Need to exit NoCmd mode before other + * IP command */ + uint8_t halfclk_for_nonreadcmd; /* Half the Serial Clock for non-read + * command: true/false */ + uint8_t need_restore_nocmdmode; /* Need to Restore NoCmd mode after IP + * command execution */ + uint32_t blocksize; /* Block size */ + uint32_t reserve2[11]; /* Reserved for future use */ +}; + +#endif /* __BOARDS_ARM_IMXRT_TEENSY_4_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */ diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c b/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c new file mode 100644 index 0000000000..f3ac06804f --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c @@ -0,0 +1,614 @@ +/**************************************************************************** + * boards/px4/fmu-v6xrt/src/imxrt_flexspi_storage.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "px4_log.h" + +#include "imxrt_flexspi.h" +#include "board_config.h" +#include "hardware/imxrt_pinmux.h" + +#ifdef CONFIG_IMXRT_FLEXSPI + +/* Used sectors must be multiple of the flash block size + * i.e. W25Q32JV has a block size of 64KB +*/ + +#define NOR_USED_SECTORS (0x20U) /* 32 * 4KB = 128KB */ +#define NOR_TOTAL_SECTORS (0x0800U) +#define NOR_PAGE_SIZE (0x0100U) /* 256 bytes */ +#define NOR_SECTOR_SIZE (0x1000U) /* 4KB */ +#define NOR_START_SECTOR (NOR_TOTAL_SECTORS - NOR_USED_SECTORS) +#define NOR_START_PAGE ((NOR_START_SECTOR * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE) +#define NOR_STORAGE_ADDR (IMXRT_FLEXCIPHER_BASE + NOR_START_SECTOR * NOR_SECTOR_SIZE) + +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) + +enum { + /* SPI instructions */ + + READ_FAST = 0, + READ_STATUS_REG = 1, + WRITE_STATUS_REG = 3, + WRITE_ENABLE = 4, + SECTOR_ERASE_4K = 5, + READ_FAST_QUAD_OUTPUT = 6, + PAGE_PROGRAM_QUAD_INPUT = 7, + PAGE_PROGRAM = 9, + CHIP_ERASE = 11, +}; + +static const uint32_t g_flexspi_nor_lut[][4] = { + [READ_FAST] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0xeb, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_4PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x06, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_4PAD, 0x04), + }, + + [READ_STATUS_REG] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x05, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_1PAD, 0x04), + }, + + [WRITE_STATUS_REG] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x01, + FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04), + }, + + [WRITE_ENABLE] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x06, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + + [SECTOR_ERASE_4K] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x20, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + }, + + [CHIP_ERASE] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0xc7, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + + [PAGE_PROGRAM] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x02, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0x0), + }, + + [READ_FAST_QUAD_OUTPUT] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x6b, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x08, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_4PAD, 0x04), + }, + + [PAGE_PROGRAM_QUAD_INPUT] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x32, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_4PAD, 0x04, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + +}; + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* FlexSPI NOR device private data */ + +struct imxrt_flexspi_storage_dev_s { + struct mtd_dev_s mtd; + struct flexspi_dev_s *flexspi; /* Saved FlexSPI interface instance */ + uint8_t *ahb_base; + enum flexspi_port_e port; + struct flexspi_device_config_s *config; +}; + +/**************************************************************************** + * Private Functions Prototypes + ****************************************************************************/ + +/* MTD driver methods */ + +static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks); +static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev, + off_t offset, + size_t nbytes, + uint8_t *buffer); +static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + uint8_t *buffer); +static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + const uint8_t *buffer); +static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev, + int cmd, + unsigned long arg); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct flexspi_device_config_s g_flexspi_device_config = { + .flexspi_root_clk = 4000000, + .is_sck2_enabled = 0, + .flash_size = NOR_USED_SECTORS * NOR_SECTOR_SIZE / 4, + .cs_interval_unit = FLEXSPI_CS_INTERVAL_UNIT1_SCK_CYCLE, + .cs_interval = 0, + .cs_hold_time = 3, + .cs_setup_time = 3, + .data_valid_time = 0, + .columnspace = 0, + .enable_word_address = 0, + .awr_seq_index = 0, + .awr_seq_number = 0, + .ard_seq_index = READ_FAST, + .ard_seq_number = 1, + .ahb_write_wait_unit = FLEXSPI_AHB_WRITE_WAIT_UNIT2_AHB_CYCLE, + .ahb_write_wait_interval = 0, + .rx_sample_clock = FLEXSPI_READ_SAMPLE_CLK_LOOPBACK_FROM_DQS_PAD, +}; + +static struct imxrt_flexspi_storage_dev_s g_flexspi_nor = { + .mtd = + { + .erase = imxrt_flexspi_storage_erase, + .bread = imxrt_flexspi_storage_bread, + .bwrite = imxrt_flexspi_storage_bwrite, + .read = imxrt_flexspi_storage_read, + .ioctl = imxrt_flexspi_storage_ioctl, +#ifdef CONFIG_MTD_BYTE_WRITE + .write = NULL, +#endif + .name = "imxrt_flexspi_storage" + }, + .flexspi = (void *)0, + .ahb_base = (uint8_t *) NOR_STORAGE_ADDR, + .port = FLEXSPI_PORT_A1, + .config = &g_flexspi_device_config +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int imxrt_flexspi_storage_read_status( + const struct imxrt_flexspi_storage_dev_s *dev, + uint32_t *status) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_READ, + .seq_number = 1, + .seq_index = READ_STATUS_REG, + .data = status, + .data_size = 1, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_storage_write_enable( + const struct imxrt_flexspi_storage_dev_s *dev) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_COMMAND, + .seq_number = 1, + .seq_index = WRITE_ENABLE, + .data = NULL, + .data_size = 0, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_storage_erase_sector( + const struct imxrt_flexspi_storage_dev_s *dev, + off_t offset) +{ + int stat; + struct flexspi_transfer_s transfer = { + .device_address = offset, + .port = dev->port, + .cmd_type = FLEXSPI_COMMAND, + .seq_number = 1, + .seq_index = SECTOR_ERASE_4K, + .data = NULL, + .data_size = 0, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + + return 0; +} + +static int imxrt_flexspi_storage_erase_chip( + const struct imxrt_flexspi_storage_dev_s *dev) +{ + /* We can't erase the chip we're executing from */ + return -EINVAL; +} + +static int imxrt_flexspi_storage_page_program( + const struct imxrt_flexspi_storage_dev_s *dev, + off_t offset, + const void *buffer, + size_t len) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = offset, + .port = dev->port, + .cmd_type = FLEXSPI_WRITE, + .seq_number = 1, + .seq_index = PAGE_PROGRAM_QUAD_INPUT, + .data = (uint32_t *) buffer, + .data_size = len, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_storage_wait_bus_busy( + const struct imxrt_flexspi_storage_dev_s *dev) +{ + uint32_t status = 0; + int ret; + + do { + ret = imxrt_flexspi_storage_read_status(dev, &status); + + if (ret) { + return ret; + } + } while (status & 1); + + return 0; +} + +static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev, + off_t offset, + size_t nbytes, + uint8_t *buffer) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + uint8_t *src; + + finfo("offset: %08lx nbytes: %d\n", (long)offset, (int)nbytes); + + if (priv->port >= FLEXSPI_PORT_COUNT) { + return -EIO; + } + + src = priv->ahb_base + offset; + + memcpy(buffer, src, nbytes); + + finfo("return nbytes: %d\n", (int)nbytes); + return (ssize_t)nbytes; +} + +static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + uint8_t *buffer) +{ + ssize_t nbytes; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + /* On this device, we can handle the block read just like the byte-oriented + * read + */ + + nbytes = imxrt_flexspi_storage_read(dev, startblock * NOR_PAGE_SIZE, + nblocks * NOR_PAGE_SIZE, buffer); + + if (nbytes > 0) { + nbytes /= NOR_PAGE_SIZE; + } + + return nbytes; +} + +static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + const uint8_t *buffer) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + size_t len = nblocks * NOR_PAGE_SIZE; + off_t offset = (startblock + NOR_START_PAGE) * NOR_PAGE_SIZE; + uint8_t *src = (uint8_t *) buffer; +#ifdef CONFIG_ARMV7M_DCACHE + uint8_t *dst = priv->ahb_base + startblock * NOR_PAGE_SIZE; +#endif + int i; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false); + + irqstate_t flags = enter_critical_section(); + + while (len) { + i = MIN(NOR_PAGE_SIZE, len); + imxrt_flexspi_storage_write_enable(priv); + imxrt_flexspi_storage_page_program(priv, offset, src, i); + imxrt_flexspi_storage_wait_bus_busy(priv); + offset += i; + src += i; + len -= i; + } + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true); + + leave_critical_section(flags); + +#ifdef CONFIG_ARMV7M_DCACHE + up_invalidate_dcache((uintptr_t)dst, + (uintptr_t)dst + nblocks * NOR_PAGE_SIZE); +#endif + + return nblocks; +} + +static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + size_t blocksleft = nblocks; +#ifdef CONFIG_ARMV7M_DCACHE + uint8_t *dst = priv->ahb_base + startblock * NOR_SECTOR_SIZE; +#endif + + startblock += NOR_START_SECTOR; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false); + + irqstate_t flags = enter_critical_section(); + + while (blocksleft-- > 0) { + /* Erase each sector */ + + imxrt_flexspi_storage_write_enable(priv); + imxrt_flexspi_storage_erase_sector(priv, startblock * NOR_SECTOR_SIZE); + imxrt_flexspi_storage_wait_bus_busy(priv); + startblock++; + } + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true); + + leave_critical_section(flags); + +#ifdef CONFIG_ARMV7M_DCACHE + up_invalidate_dcache((uintptr_t)dst, + (uintptr_t)dst + nblocks * NOR_SECTOR_SIZE); +#endif + + return (int)nblocks; +} + +static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev, + int cmd, + unsigned long arg) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + finfo("cmd: %d\n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + struct mtd_geometry_s *geo = + (struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to + * know the capacity and how to access the device. + * + * NOTE: + * that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the + * client will expect the device logic to do whatever is + * necessary to make it appear so. + */ + + geo->blocksize = (NOR_PAGE_SIZE); + geo->erasesize = (NOR_SECTOR_SIZE); + geo->neraseblocks = (NOR_USED_SECTORS); + + ret = OK; + + finfo("blocksize: %lu erasesize: %lu neraseblocks: %lu\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case BIOC_PARTINFO: { + struct partition_info_s *info = + (struct partition_info_s *)arg; + + if (info != NULL) { + info->numsectors = (NOR_USED_SECTORS * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE; + info->sectorsize = NOR_PAGE_SIZE; + info->startsector = 0; + info->parent[0] = '\0'; + ret = OK; + } + } + break; + + case MTDIOC_BULKERASE: { + /* Erase the entire device */ + + imxrt_flexspi_storage_write_enable(priv); + imxrt_flexspi_storage_erase_chip(priv); + imxrt_flexspi_storage_wait_bus_busy(priv); + ret = OK; + } + break; + + default: + ret = -ENOTTY; /* Bad/unsupported command */ + break; + } + + finfo("return %d\n", ret); + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + + +/**************************************************************************** + * Name: imxrt_flexspi_storage_initialize + * + * Description: + * This function is called by board-bringup logic to configure the + * flash device. + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int imxrt_flexspi_storage_initialize(void) +{ + struct mtd_dev_s *mtd_dev = &g_flexspi_nor.mtd; + int ret = -ENODEV; + + /* Select FlexSPI1 */ + + g_flexspi_nor.flexspi = imxrt_flexspi_initialize(0); + + if (g_flexspi_nor.flexspi) { + ret = OK; + + + FLEXSPI_UPDATE_LUT(g_flexspi_nor.flexspi, + 0, + (const uint32_t *)g_flexspi_nor_lut, + sizeof(g_flexspi_nor_lut) / 4); + } + + /* Register the MTD driver so that it can be accessed from the + * VFS. + */ + + ret = register_mtddriver("/dev/nor", mtd_dev, 0755, NULL); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to register MTD driver: %d\n", + ret); + } + +#ifdef CONFIG_FS_LITTLEFS + + /* Mount the LittleFS file system */ + + ret = nx_mount("/dev/nor", "/fs/nor", "littlefs", 0, + "autoformat"); + + if (ret < 0) { + syslog(LOG_ERR, + "ERROR: Failed to mount LittleFS at /mnt/lfs: %d\n", + ret); + } + +#endif + + return ret; +} +#endif /* CONFIG_IMXRT_FLEXSPI */ diff --git a/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c b/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c new file mode 100644 index 0000000000..67c1a4ca36 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019, 2023 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. + * + ****************************************************************************/ + +/** + * @file imxrt_ocram_initialize.c + * + * PX4 fmu-v6xrt RAM startup early startup code. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "arm_internal.h" +#include "imxrt_iomuxc.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +__BEGIN_DECLS +extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ +extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ +extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ +extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ +extern uint64_t _edtcm; /* Copy destination end address in DTCM */ +__END_DECLS + +/**************************************************************************** + * Name: imxrt_ocram_initialize + * + * Description: + * Called off reset vector to reconfigure the flexRAM + * and finish the FLASH to RAM Copy. + * CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin + * + ****************************************************************************/ + +__EXPORT void imxrt_ocram_initialize(void) +{ + uint32_t regval; + register uint64_t *src; + register uint64_t *dest; + + /* FlexRAM Configuration + * F = 64K ITCM + * A = 64K DTCM + * 5 = 64K OCRAM + * So 0xFFFFFFAA is + * 384K FlexRAM ITCM + * 128K FlexRAM DTCM + * */ + + putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17); + regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); + putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16); + + /* Copy any necessary code sections from FLASH to ITCM. The process is the + * same as the code copying from FLASH to RAM above. */ + for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; + dest < (uint64_t *)&_eitcmfuncs;) { + *dest++ = *src++; + } + + /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be + * certain that there are no issues with the state of global variables. + */ + + for (dest = &_sdtcm; dest < &_edtcm;) { + *dest++ = 0; + } +} diff --git a/boards/nxp/tropic-community/src/init.c b/boards/nxp/tropic-community/src/init.c new file mode 100644 index 0000000000..a9c326f39c --- /dev/null +++ b/boards/nxp/tropic-community/src/init.c @@ -0,0 +1,381 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * NXP imxrt1062-v1 specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arm_internal.h" +#include "arm_internal.h" +#include "imxrt_flexspi_nor_boot.h" +#include "imxrt_iomuxc.h" +#include "imxrt_flexcan.h" +#include "imxrt_enet.h" +#include +#include "board_config.h" + +#include +#undef FLEXSPI_LUT_COUNT +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); + +extern uint32_t _srodata; /* Start of .rodata */ +extern uint32_t _erodata; /* End of .rodata */ +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + +} +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ + +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_gpio_output(i))); + } + + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ + if (status >= 0) { + up_mdelay(100); + } +} + + +locate_code(".ramfunc") +void imxrt_flash_setup_prefetch_partition(void) +{ +//Prefetch tuning to be determined +#if 0 + putreg32((uint32_t)&_srodata, IMXRT_FLEXSPI1_AHBBUFREGIONSTART0); + putreg32((uint32_t)&_erodata, IMXRT_FLEXSPI1_AHBBUFREGIONEND0); + /*putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART1); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND1); + putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART2); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND2); + putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART3); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND3);*/ +#endif +#if 0 + struct flexspi_type_s *g_flexspi = (struct flexspi_type_s *)IMXRT_FLEXSPIC_BASE; + /* RODATA */ + g_flexspi->AHBRXBUFCR0[0] = FLEXSPI_AHBRXBUFCR0_BUFSZ(48) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(1); + + + /* All Text */ + g_flexspi->AHBRXBUFCR0[1] = FLEXSPI_AHBRXBUFCR0_BUFSZ(80) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(1); + + /* Disable 2 */ + g_flexspi->AHBRXBUFCR0[2] = FLEXSPI_AHBRXBUFCR0_BUFSZ(0) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(0); + + /* Disable 3 */ + g_flexspi->AHBRXBUFCR0[3] = FLEXSPI_AHBRXBUFCR0_BUFSZ(0) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(0); +#endif + + ARM_DSB(); + ARM_ISB(); + ARM_DMB(); +} + +/**************************************************************************** + * Name: imxrt_boardinitialize + * + * Description: + * All i.MX RT architectures must provide the following entry point. This + * entry point is called early in the initialization -- after clocking and + * memory have been configured but before caches have been enabled and + * before any devices have been initialized. + * + ****************************************************************************/ + +__EXPORT void imxrt_boardinitialize(void) +{ + imxrt_flash_setup_prefetch_partition(); + + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + + imxrt_spidev_initialize(); + + imxrt_usb_initialize(); + + fmurt1062_timer_initialize(); +} + +/**************************************************************************** + * Function: imxrt_phy_boardinitialize + * + * Description: + * Some boards require specialized initialization of the PHY before it can + * be used. This may include such things as configuring GPIOs, resetting + * the PHY, etc. + * If CONFIG_IMXRT_ENET_PHYINIT is defined in the configuration then the + * board specific logic must provide imxrt_phyinitialize(); + * The i.MX RT Ethernet driver will call this function one time before it + * first uses the PHY. + * + * Input Parameters: + * intf - Always zero for now. + * + * Returned Value: + * OK on success; Negated errno on failure. + * + ****************************************************************************/ + +int imxrt_phy_boardinitialize(int intf) +{ +#ifdef CONFIG_IMXRT_GPIO1_0_15_IRQ + /* Configure the PHY interrupt pin */ + + printf("Configuring interrupt: %08x\n", GPIO_ENET_INT); + imxrt_config_gpio(GPIO_ENET_INT); +#endif + + /* Configure the PHY reset pin. + * + * The #RST uses inverted logic. The initial value of zero will put the + * PHY into the reset state. + */ + + printf("Configuring reset: %08x\n", GPIO_ENET_RST); + imxrt_config_gpio(GPIO_ENET_RST); + + /* Take the PHY out of reset. */ + + imxrt_gpio_write(GPIO_ENET_RST, true); + return OK; +} + +void imxrt_flexio_clocking(void) +{ + uint32_t reg; + + /* Init USB PLL3 PFD2 */ + + reg = getreg32(IMXRT_CCM_ANALOG_PFD_480); + + while ((getreg32(IMXRT_CCM_ANALOG_PLL_USB1) & + CCM_ANALOG_PLL_USB1_LOCK) == 0) { + } + + reg &= ~CCM_ANALOG_PFD_480_PFD2_FRAC_MASK; + + /* Set PLL3 PFD2 to 480 * 18 / CONFIG_PLL3_PFD2_FRAC */ + + reg |= ((uint32_t)(CONFIG_PLL3_PFD2_FRAC) << CCM_ANALOG_PFD_480_PFD3_FRAC_SHIFT); + + putreg32(reg, IMXRT_CCM_ANALOG_PFD_480); + + reg = getreg32(IMXRT_CCM_CDCDR); + reg &= ~(CCM_CDCDR_FLEXIO1_CLK_SEL_MASK | + CCM_CDCDR_FLEXIO1_CLK_PODF_MASK | + CCM_CDCDR_FLEXIO1_CLK_PRED_MASK); + reg |= CCM_CDCDR_FLEXIO1_CLK_SEL(CONFIG_FLEXIO1_CLK); + reg |= CCM_CDCDR_FLEXIO1_CLK_PODF + (CCM_PODF_FROM_DIVISOR(CONFIG_FLEXIO1_PODF_DIVIDER)); + reg |= CCM_CDCDR_FLEXIO1_CLK_PRED + (CCM_PRED_FROM_DIVISOR(CONFIG_FLEXIO1_PRED_DIVIDER)); + putreg32(reg, IMXRT_CCM_CDCDR); +} + + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + imxrt_flexio_clocking(); + + /* Power on Interfaces */ + + board_spi_reset(10, 0xffff); + + px4_platform_init(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)imxrt_serial_dma_poll, NULL); +#endif + + int ret = OK; +#if defined(CONFIG_IMXRT_USDHC) + ret = fmurt1062_usdhc_initialize(); +#endif + + /* Configure SPI-based devices */ + + ret = imxrt1062_spi_bus_initialize(); + +#ifdef CONFIG_IMXRT_ENET + imxrt_netinitialize(0); +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN3 + imxrt_caninitialize(3); +#endif + +#ifdef CONFIG_IMXRT_FLEXSPI + ret = imxrt_flexspi_storage_initialize(); + + if (ret < 0) { + syslog(LOG_ERR, + "ERROR: imxrt_flexspi_nor_initialize failed: %d\n", ret); + } + +#endif + + return ret; +} diff --git a/boards/nxp/tropic-community/src/sdhc.c b/boards/nxp/tropic-community/src/sdhc.c new file mode 100644 index 0000000000..df2f92cc1c --- /dev/null +++ b/boards/nxp/tropic-community/src/sdhc.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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 NuttX 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. + * + ****************************************************************************/ +/* A micro Secure Digital (SD) card slot is available on the board connected to + * the SD Host Controller (USDHC1) signals of the MCU. This slot will accept + * micro format SD memory cards. + * + * ------------ ------------- -------- + * SD Card Slot Board Signal IMXRT Pin + * ------------ ------------- -------- + * DAT0 USDHC1_DATA0 GPIO_SD_B0_02 + * DAT1 USDHC1_DATA1 GPIO_SD_B0_03 + * DAT2 USDHC1_DATA2 GPIO_SD_B0_04 + * CD/DAT3 USDHC1_DATA3 GPIO_SD_B0_05 + * CMD USDHC1_CMD GPIO_SD_B0_00 + * CLK USDHC1_CLK GPIO_SD_B0_01 + * CD USDHC1_CD GPIO_B1_12 + * ------------ ------------- -------- + * + * There are no Write Protect available to the IMXRT. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "imxrt_usdhc.h" + +#include "board_config.h" + +#ifdef CONFIG_IMXRT_USDHC +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ +/**************************************************************************** + * Private Data + ****************************************************************************/ +/**************************************************************************** + * Private Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: fmurt1062_usdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +int fmurt1062_usdhc_initialize(void) +{ + int ret; + + /* Mount the SDHC-based MMC/SD block driver */ + /* First, get an instance of the SDHC interface */ + + struct sdio_dev_s *sdhc = imxrt_usdhc_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdhc) { + PX4_ERR("ERROR: Failed to initialize SDHC slot %d\n", CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDHC interface to the MMC/SD driver */ + + ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdhc); + + if (ret != OK) { + PX4_ERR("ERROR: Failed to bind SDHC to the MMC/SD driver: %d\n", ret); + return ret; + } + + syslog(LOG_INFO, "Successfully bound SDHC to the MMC/SD driver\n"); + + return OK; +} +#endif /* CONFIG_IMXRT_USDHC */ diff --git a/boards/nxp/tropic-community/src/spi.cpp b/boards/nxp/tropic-community/src/spi.cpp new file mode 100644 index 0000000000..b267b18d06 --- /dev/null +++ b/boards/nxp/tropic-community/src/spi.cpp @@ -0,0 +1,315 @@ +/************************************************************************************ + * + * Copyright (C) 2016, 2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include "imxrt_lpspi.h" +#include "imxrt_gpio.h" +#include "board_config.h" +#include + +#if defined(CONFIG_IMXRT_LPSPI3) || defined(CONFIG_IMXRT_LPSPI4) + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::LPSPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port1, GPIO::Pin28}), /* GPIO_AD_B1_12 GPIO1_IO28 */ + }), + initSPIBus(SPI::Bus::LPSPI4, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}), /* GPIO_B1_02 GPIO2_IO18 */ + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin0}), /* GPIO_B0_00 GPIO2_IO00 */ + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT)) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +/************************************************************************************ + * Name: fmurt1062_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. + * + ************************************************************************************/ + +void imxrt_spidev_initialize(void) +{ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } +} + +/************************************************************************************ + * Name: imxrt_spi_bus_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. + * + ************************************************************************************/ + +static const px4_spi_bus_t *_spi_bus3; +static const px4_spi_bus_t *_spi_bus4; + +__EXPORT int imxrt1062_spi_bus_initialize(void) +{ + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 3: _spi_bus3 = &px4_spi_buses[i]; break; + + case 4: _spi_bus4 = &px4_spi_buses[i]; break; + } + } + + /* Configure SPI-based devices */ + + struct spi_dev_s *spi_icm = px4_spibus_initialize(3); + + if (!spi_icm) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 1); + return -ENODEV; + } + + /* Default bus 1 to 8MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_icm, 8 * 1000 * 1000); + SPI_SETBITS(spi_icm, 8); + SPI_SETMODE(spi_icm, SPIDEV_MODE3); + + /* Get the SPI port for the BMI088 */ + + struct spi_dev_s *spi_bmi = px4_spibus_initialize(4); + + if (!spi_bmi) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 4); + return -ENODEV; + } + + /* Default ext bus to 8MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_bmi, 8 * 1000 * 1000); + SPI_SETBITS(spi_bmi, 8); + SPI_SETMODE(spi_bmi, SPIDEV_MODE3); + + /* deselect all */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + SPI_SELECT(spi_bmi, px4_spi_buses[bus].devices[i].devid, false); + } + } + } + + return OK; + +} + +/**************************************************************************** + * Name: imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status + * + * Description: + * The external functions, imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status must be + * provided by board-specific logic. They are implementations of the select + * and status methods of the SPI interface defined by struct spi_ops_s (see + * include/nuttx/spi/spi.h). All other methods (including imxrt_lpspibus_initialize()) + * are provided by common STM32 logic. To use this common SPI logic on your + * board: + * + * 1. Provide logic in imxrt_boardinitialize() to configure SPI chip select + * pins. + * 2. Provide imxrt_lpspi1/2/3select() and imxrt_lpspi1/2/3status() functions in your + * board-specific logic. These functions will perform chip selection and + * status operations using GPIOs in the way your board is configured. + * 3. Add a calls to imxrt_lpspibus_initialize() in your low level application + * initialization logic + * 4. The handle returned by imxrt_lpspibus_initialize() may then be used to bind the + * SPI driver to higher level logic (e.g., calling + * mmcsd_spislotinitialize(), for example, will bind the SPI driver to + * the SPI MMC/SD driver). + * + ****************************************************************************/ + +static inline void imxrt_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } + + if (devid == bus->devices[i].devid) { + // SPI select is active low, so write !selected to select the device + imxrt_gpio_write(bus->devices[i].cs_gpio, !selected); + } + } +} + +#if defined(CONFIG_IMXRT_LPSPI3) +__EXPORT void imxrt_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_spixselect(_spi_bus3, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#if defined(CONFIG_IMXRT_LPSPI4) +__EXPORT void imxrt_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_spixselect(_spi_bus4, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +/************************************************************************************ + * Name: board_spi_reset + * + * Description: + * + * + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms, int bus_mask) +{ +#ifdef CONFIG_IMXRT_LPSPI1 + + /* Goal not to back feed the chips on the bus via IO lines */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio)); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio)); + } + } + } + } + + imxrt_config_gpio(GPIO_SPI1_SCK_OFF); + imxrt_config_gpio(GPIO_SPI1_MISO_OFF); + imxrt_config_gpio(GPIO_SPI1_MOSI_OFF); + + imxrt_config_gpio(GPIO_SPI3_SCK_OFF); + imxrt_config_gpio(GPIO_SPI3_MISO_OFF); + imxrt_config_gpio(GPIO_SPI3_MOSI_OFF); + + + imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SDA_RESET)); + imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SCL_RESET)); + + /* set the sensor rail off */ + imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + imxrt_config_gpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + imxrt_config_gpio(px4_spi_buses[bus].devices[i].drdy_gpio); + } + } + } + } + + imxrt_config_gpio(GPIO_LPSPI1_SCK); + imxrt_config_gpio(GPIO_LPSPI1_MISO); + imxrt_config_gpio(GPIO_LPSPI1_MOSI); + + imxrt_config_gpio(GPIO_LPSPI3_SCK); + imxrt_config_gpio(GPIO_LPSPI3_MISO); + imxrt_config_gpio(GPIO_LPSPI3_MOSI); + + imxrt_config_gpio(GPIO_LPI2C3_SDA); + imxrt_config_gpio(GPIO_LPI2C3_SCL); + +#endif /* CONFIG_IMXRT_LPSPI1 */ + +} + +#endif /* CONFIG_IMXRT_LPSPI3 || CONFIG_IMXRT_LPSPI4 */ diff --git a/boards/nxp/tropic-community/src/timer_config.cpp b/boards/nxp/tropic-community/src/timer_config.cpp new file mode 100644 index 0000000000..1955fcc26d --- /dev/null +++ b/boards/nxp/tropic-community/src/timer_config.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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 + +#include +#include "hardware/imxrt_tmr.h" +#include "hardware/imxrt_flexpwm.h" +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" +#include "imxrt_xbar.h" +#include "imxrt_periphclks.h" + +#include +#include + +#include "board_config.h" + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) + +/* QTimer3 register accessors */ + +#define REG(_reg) _REG(IMXRT_QTIMER3_BASE + IMXRT_TMR_OFFSET(IMXRT_TMR_CH0,(_reg))) + +#define rCOMP1 REG(IMXRT_TMR_COMP1_OFFSET) +#define rCOMP2 REG(IMXRT_TMR_COMP2_OFFSET) +#define rCAPT REG(IMXRT_TMR_CAPT_OFFSET) +#define rLOAD REG(IMXRT_TMR_LOAD_OFFSET) +#define rHOLD REG(IMXRT_TMR_HOLD_OFFSET) +#define rCNTR REG(IMXRT_TMR_CNTR_OFFSET) +#define rCTRL REG(IMXRT_TMR_CTRL_OFFSET) +#define rSCTRL REG(IMXRT_TMR_SCTRL_OFFSET) +#define rCMPLD1 REG(IMXRT_TMR_CMPLD1_OFFSET) +#define rCMPLD2 REG(IMXRT_TMR_CMPLD2_OFFSET) +#define rCSCTRL REG(IMXRT_TMR_CSCTRL_OFFSET) +#define rFILT REG(IMXRT_TMR_FILT_OFFSET) +#define rDMA REG(IMXRT_TMR_DMA_OFFSET) +#define rENBL REG(IMXRT_TMR_ENBL_OFFSET) + + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0), // PWM_1, PMW_5 + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1), // PWM_0 + initIOPWM(PWM::FlexPWM2, PWM::Submodule2), // PWM_4 + initIOPWMDshot(PWM::FlexPWM4, PWM::Submodule2), // PWM_2, PWM_3 +}; + +#define FXIO_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_06, GPIO_FLEXIO1_FLEXIO06_1 | FXIO_IOMUX, 6), /* RevA. PWM_1 RevB. PWM1 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_07, GPIO_FLEXIO1_FLEXIO07_1 | FXIO_IOMUX, 7), /* RevA. PWM_5 RevB. PWM2 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_08, GPIO_FLEXIO1_FLEXIO08_1 | FXIO_IOMUX, 8), /* RevA. PWM_0 RevB. PWM3 */ + initIOTimerChannel(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_B0_11), /* RevA. PWM_4 RevB. PWM4 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_04, GPIO_FLEXIO1_FLEXIO04_1 | FXIO_IOMUX, 4), /* RevA. PWM_3 RevB. PWM5 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_05, GPIO_FLEXIO1_FLEXIO05_1 | FXIO_IOMUX, 5), /* RevA. PWM_2 RevB. PWM6 */ +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { +}; + +constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +}; + +#include +void fmurt1062_timer_initialize(void) +{ + /* We must configure Qtimer 3 as the IPG divide by to yield 16 Mhz + * and deliver that clock to the eFlexPWM234 via XBAR + * + * IPG = 144 Mhz + * 16Mhz = 144 / 9 + * COMP 1 = 5, COMP2 = 4 + * + * */ + + /* Enable Block Clocks for Qtimer and XBAR1 */ + + imxrt_clockall_timer3(); + imxrt_clockall_xbar1(); + + /* Disable Timer */ + + rCTRL = 0; + rCOMP1 = 5 - 1; // N - 1 + rCOMP2 = 4 - 1; + + rCAPT = 0; + rLOAD = 0; + rCNTR = 0; + + rSCTRL = TMR_SCTRL_OEN; + + rCMPLD1 = 0; + rCMPLD2 = 0; + rCSCTRL = 0; + rFILT = 0; + rDMA = 0; + + /* Count rising edges of primary source, + * Prescaler is /1 + * Count UP until compare, then re-initialize. a successful compare occurs when the counter reaches a COMP1 value. + * Toggle OFLAG output using alternating compare registers + */ + rCTRL = (TMR_CTRL_CM_MODE1 | TMR_CTRL_PCS_DIV1 | TMR_CTRL_LENGTH | TMR_CTRL_OUTMODE_TOG_ALT); + + /* QTIMER3_TIMER0 -> Flexpwm234ExtClk */ + + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM234_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); +} diff --git a/boards/nxp/tropic-community/src/tropic_led_pwm.cpp b/boards/nxp/tropic-community/src/tropic_led_pwm.cpp new file mode 100644 index 0000000000..22ef3a3575 --- /dev/null +++ b/boards/nxp/tropic-community/src/tropic_led_pwm.cpp @@ -0,0 +1,218 @@ +/**************************************************************************** + * + * 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 Airmind 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 tropic_led_pwm.cpp +*/ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include "imxrt_xbar.h" +#include "imxrt_periphclks.h" +#include "hardware/imxrt_tmr.h" +#include "hardware/imxrt_flexpwm.h" + +#define LED_PWM_FREQ 1000 +#define FLEXPWM_FREQ 1000000 +#define QTMR_FREQ (144000000/128) + +#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET) + +#define FLEXPWM_TIMER_BASE IMXRT_FLEXPWM2_BASE + +/* Register accessors */ +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) +#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) +#define FLEXPWMREG(_tmr, _sm, _reg) _REG16(FLEXPWM_TIMER_BASE + ((_sm) * SM_SPACING), (_reg)) +#define QTMRREG(_reg, _chn) _REG16(IMXRT_QTIMER4_BASE + ((_chn) * IMXRT_TMR_CHANNEL_SPACING),(_reg)) + +/* FlexPWM Registers for LED_G */ +#define rINIT(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */ +#define rCTRL(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL_OFFSET) /* Control Register */ +#define rCTRL2(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL2_OFFSET) /* Control 2 Register */ +#define rFSTS0(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_FSTS0_OFFSET) /* Fault Status Register */ +#define rVAL0(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL0_OFFSET) /* Value Register 0 */ +#define rVAL1(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL1_OFFSET) /* Value Register 1 */ +#define rVAL2(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL2_OFFSET) /* Value Register 2 */ +#define rVAL3(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL3_OFFSET) /* Value Register 3 */ +#define rVAL4(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL4_OFFSET) /* Value Register 4 */ +#define rVAL5(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL5_OFFSET) /* Value Register 5 */ +#define rFFILT0(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_FFILT0_OFFSET) /* Fault Filter Register */ +#define rDISMAP0(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP0_OFFSET) /* Fault Disable Mapping Register 0 */ +#define rDISMAP1(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP1_OFFSET) /* Fault Disable Mapping Register 1 */ +#define rOUTEN(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_OUTEN_OFFSET) /* Output Enable Register */ +#define rDTSRCSEL(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */ +#define rMCTRL(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */ + +#define QTMR_rCOMP1(_chn) QTMRREG(IMXRT_TMR_COMP1_OFFSET, _chn) +#define QTMR_rCOMP2(_chn) QTMRREG(IMXRT_TMR_COMP2_OFFSET, _chn) +#define QTMR_rCAPT(_chn) QTMRREG(IMXRT_TMR_CAPT_OFFSET, _chn) +#define QTMR_rLOAD(_chn) QTMRREG(IMXRT_TMR_LOAD_OFFSET, _chn) +#define QTMR_rHOLD(_chn) QTMRREG(IMXRT_TMR_HOLD_OFFSET, _chn) +#define QTMR_rCNTR(_chn) QTMRREG(IMXRT_TMR_CNTR_OFFSET, _chn) +#define QTMR_rCTRL(_chn) QTMRREG(IMXRT_TMR_CTRL_OFFSET, _chn) +#define QTMR_rSCTRL(_chn) QTMRREG(IMXRT_TMR_SCTRL_OFFSET, _chn) +#define QTMR_rCMPLD1(_chn) QTMRREG(IMXRT_TMR_CMPLD1_OFFSET, _chn) +#define QTMR_rCMPLD2(_chn) QTMRREG(IMXRT_TMR_CMPLD2_OFFSET, _chn) +#define QTMR_rCSCTRL(_chn) QTMRREG(IMXRT_TMR_CSCTRL_OFFSET, _chn) +#define QTMR_rFILT(_chn) QTMRREG(IMXRT_TMR_FILT_OFFSET, _chn) +#define QTMR_rDMA(_chn) QTMRREG(IMXRT_TMR_DMA_OFFSET, _chn) +#define QTMR_rENBL(_chn) QTMRREG(IMXRT_TMR_ENBL_OFFSET, _chn) + +#define FLEXPWM_TIMER 2 +#define FLEXPWM_SM 3 + +#define FREQ + +static void flexpwm_led_green(uint16_t cvalue) +{ + if (cvalue == 0) { + rMCTRL(FLEXPWM_TIMER) &= ~MCTRL_RUN(1 << FLEXPWM_SM); + px4_arch_configgpio(GPIO_nLED_GREEN); + + } else { + rMCTRL(FLEXPWM_TIMER) |= (1 << (FLEXPWM_SM + MCTRL_CLDOK_SHIFT)); + rVAL1(FLEXPWM_TIMER, FLEXPWM_SM) = (FLEXPWM_FREQ / LED_PWM_FREQ) - 1; + rVAL5(FLEXPWM_TIMER, FLEXPWM_SM) = (FLEXPWM_FREQ / LED_PWM_FREQ) - (cvalue * 3); + rMCTRL(FLEXPWM_TIMER) |= MCTRL_LDOK(1 << FLEXPWM_SM) | MCTRL_RUN(1 << FLEXPWM_SM); + px4_arch_configgpio(PWM_LED_GREEN); + } +} + +static void init_qtimer(unsigned channel) +{ + QTMR_rCNTR(channel) = 0; /* Reset counter */ + QTMR_rSCTRL(channel) = (TMR_SCTRL_OEN | TMR_SCTRL_FORCE); /* Enable output */ + QTMR_rCSCTRL(channel) = (TMR_CSCTRL_CL1_COMP1); + QTMR_rCOMP1(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCMPLD1(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCOMP2(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCMPLD2(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCTRL(channel) = (TMR_CTRL_PCS_DIV32 | TMR_CTRL_OUTMODE_TOG_ALT | TMR_CTRL_DIR | + TMR_CTRL_CM_MODE1); /* Run counter */ +} + +int +led_pwm_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel == 2) { + + if (cvalue == 0) { + px4_arch_configgpio(GPIO_nLED_RED); + + } else { + px4_arch_configgpio(PWM_LED_RED); + QTMR_rCMPLD1(0) = (uint16_t)cvalue * 256; + } + + } else if (channel == 1) { + flexpwm_led_green(cvalue); + + } else if (channel == 0) { + + if (cvalue == 0) { + px4_arch_configgpio(GPIO_nLED_BLUE); + + } else { + px4_arch_configgpio(PWM_LED_BLUE); + QTMR_rCMPLD1(1) = (uint16_t)cvalue * 256; + } + } + + return 0; +} + +int led_pwm_servo_init() +{ + /* PWM_LED_GREEN - FLEXPWM2_PWMB03 */ + imxrt_clockall_pwm2(); + + + /* PWM_LED_RED PWM_LED_BLUE - QTIMER4 */ + imxrt_clockall_timer4(); + + /* Clear all Faults */ + rFSTS0(FLEXPWM_TIMER) = FSTS_FFLAG_MASK; + rMCTRL(FLEXPWM_TIMER) |= (1 << (FLEXPWM_SM + MCTRL_CLDOK_SHIFT)); + + rCTRL2(FLEXPWM_TIMER, FLEXPWM_SM) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(FLEXPWM_TIMER, FLEXPWM_SM) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rVAL0(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rVAL2(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rVAL4(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rFFILT0(FLEXPWM_TIMER) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(FLEXPWM_TIMER, FLEXPWM_SM) = 0xf000; + rDISMAP1(FLEXPWM_TIMER, FLEXPWM_SM) = 0xf000; + + rOUTEN(FLEXPWM_TIMER) |= OUTEN_PWMB_EN(1 << FLEXPWM_SM); + + rDTSRCSEL(FLEXPWM_TIMER) = 0; + rMCTRL(FLEXPWM_TIMER) |= MCTRL_LDOK(1 << FLEXPWM_SM); + + /* QTMR */ + init_qtimer(0); + init_qtimer(1); + + /* Red - QTIMER4_TMR0 */ + imxrt_xbar_connect(IMXRT_XBARA1_OUT_IOMUX_XBAR_IO16_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER4_TMR0_OUT); + + /* Blue - QTIMER4_TMR1 */ + imxrt_xbar_connect(IMXRT_XBARA1_OUT_IOMUX_XBAR_IO17_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER4_TMR1_OUT); + + /* Set XBAR 16 and 17 as an output */ + putreg32((1 << 28) | (1 << 29), IMXRT_IOMUXC_GPR_GPR6); + + return OK; +} diff --git a/boards/nxp/tropic-community/src/usb.c b/boards/nxp/tropic-community/src/usb.c new file mode 100644 index 0000000000..99e8859643 --- /dev/null +++ b/boards/nxp/tropic-community/src/usb.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" +#include "imxrt_periphclks.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int imxrt_usb_initialize(void) +{ + imxrt_clockall_usboh3(); + + // We abuse VBUS2 to check if system power checks VBUS gets below 4.7V + putreg32(USB_ANALOG_USB_VBUS_DETECT_VBUSVALID_THRESH_4V7 | USB_ANALOG_USB_VBUS_DETECT_VBUSVALID_PWRUP_CMPS, + IMXRT_USB_ANALOG_USB2_VBUS_DETECT); + return 0; +} +/************************************************************************************ + * Name: imxrt_usbpullup + * + * Description: + * If USB is supported and the board supports a pullup via GPIO (for USB software + * connect and disconnect), then the board software must provide imxrt_usbpullup. + * See include/nuttx/usb/usbdev.h for additional description of this method. + * Alternatively, if no pull-up GPIO the following EXTERN can be redefined to be + * NULL. + * + ************************************************************************************/ + +__EXPORT +int imxrt_usbpullup(FAR struct usbdev_s *dev, bool enable) +{ + usbtrace(TRACE_DEVPULLUP, (uint16_t)enable); + + return OK; +} + +/************************************************************************************ + * Name: imxrt_usbsuspend + * + * Description: + * Board logic must provide the imxrt_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT +void imxrt_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} + +/************************************************************************************ + * Name: board_read_VBUS_state + * + * Description: + * All boards must provide a way to read the state of VBUS, this my be simple + * digital input on a GPIO. Or something more complicated like a Analong input + * or reading a bit from a USB controller register. + * + * Returns - 0 if connected. + * + ************************************************************************************/ + +int board_read_VBUS_state(void) +{ + + return (getreg32(IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_VALID) ? 0 : 1; +} diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 5a84574edb..00e54c5044 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index b796d1bfbf..c5255743df 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set @@ -40,6 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index cc31ed3c2e..a74698d42d 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_BOARD_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y CONFIG_DRIVERS_SW_CRYPTO=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index b5f586406e..2c9e2d20d3 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -16,7 +16,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v5/src/init.c b/boards/px4/fmu-v5/src/init.c index cfec81bae6..660a338b20 100644 --- a/boards/px4/fmu-v5/src/init.c +++ b/boards/px4/fmu-v5/src/init.c @@ -143,8 +143,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index f598bbb5d9..e8bcadebd7 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -45,4 +45,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y diff --git a/boards/px4/fmu-v5x/flash-analysis.px4board b/boards/px4/fmu-v5x/flash-analysis.px4board new file mode 100644 index 0000000000..30717ad93c --- /dev/null +++ b/boards/px4/fmu-v5x/flash-analysis.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LINKER_PREFIX="flash-analysis" diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld new file mode 100644 index 0000000000..29c75f02ee --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/flash-analysis-script.ld @@ -0,0 +1,6 @@ +INCLUDE "script.ld" + +MEMORY +{ + FLASH_AXIM (rx) : ORIGIN = 0x08008000, LENGTH = 10080K +} diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index bc4c7d5d3f..42f8ec21ce 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp index c4debd31ae..b743bf1310 100644 --- a/boards/px4/fmu-v5x/src/init.cpp +++ b/boards/px4/fmu-v5x/src/init.cpp @@ -145,8 +145,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 31445bc090..dbb913d9e1 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -16,6 +16,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_BOSCH_BMI055=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6c/src/init.c b/boards/px4/fmu-v6c/src/init.c index 5a853b31b1..ff8d7a8b3c 100644 --- a/boards/px4/fmu-v6c/src/init.c +++ b/boards/px4/fmu-v6c/src/init.c @@ -137,8 +137,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index 1884020f0a..9d3d5e8e82 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -69,7 +69,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6u/src/init.c b/boards/px4/fmu-v6u/src/init.c index 1563331909..ce67db5066 100644 --- a/boards/px4/fmu-v6u/src/init.c +++ b/boards/px4/fmu-v6u/src/init.c @@ -144,8 +144,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 0df57e9595..2e9799b0c7 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -19,6 +19,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y diff --git a/boards/px4/fmu-v6x/flash-analysis.px4board b/boards/px4/fmu-v6x/flash-analysis.px4board new file mode 100644 index 0000000000..30717ad93c --- /dev/null +++ b/boards/px4/fmu-v6x/flash-analysis.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LINKER_PREFIX="flash-analysis" diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index f7b132af11..15168a6e8f 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -168,8 +168,8 @@ else bmp388 -X start fi -# Baro on I2C3 -ms5611 -X start +# Don't try to start external baro on I2C3 as it can conflict with the MS5525DSO airspeed sensor. +#ms5611 -X start unset INA_CONFIGURED unset HAVE_PM2 diff --git a/boards/px4/fmu-v6x/multicopter.px4board b/boards/px4/fmu-v6x/multicopter.px4board index 683220e0d7..9f540f567d 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -7,7 +7,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_COMMON_RC=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld b/boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld new file mode 100644 index 0000000000..5df2f5180a --- /dev/null +++ b/boards/px4/fmu-v6x/nuttx-config/scripts/flash-analysis-script.ld @@ -0,0 +1,6 @@ +INCLUDE "script.ld" + +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 10080K +} diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6x/src/init.cpp b/boards/px4/fmu-v6x/src/init.cpp index 4924120123..d914b75497 100644 --- a/boards/px4/fmu-v6x/src/init.cpp +++ b/boards/px4/fmu-v6x/src/init.cpp @@ -145,8 +145,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index 7f04202cc9..6031259eb2 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -18,6 +18,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 9fdee39f1e..f9fea7df79 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6xrt/src/CMakeLists.txt b/boards/px4/fmu-v6xrt/src/CMakeLists.txt index e34b9a134c..1ab333085b 100644 --- a/boards/px4/fmu-v6xrt/src/CMakeLists.txt +++ b/boards/px4/fmu-v6xrt/src/CMakeLists.txt @@ -75,9 +75,14 @@ else() imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c imxrt_clockconfig.c + imxrt_ocram_initialize.c ${SRCS} ) + # Force compiler not to use builtin functions (like memcpy) + # to optimize for loops in init.c (imxrt_ocram_initialize) + set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin) + target_link_libraries(drivers_board PRIVATE arch_board_hw_info diff --git a/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c b/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c new file mode 100644 index 0000000000..5a063bd46a --- /dev/null +++ b/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019, 2023 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. + * + ****************************************************************************/ + +/** + * @file imxrt_ocram_initialize.c + * + * PX4 fmu-v6xrt RAM startup early startup code. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "arm_internal.h" +#include "imxrt_iomuxc.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +__BEGIN_DECLS +extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ +extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ +extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ +extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ +extern uint64_t _edtcm; /* Copy destination end address in DTCM */ +__END_DECLS + +/**************************************************************************** + * Name: imxrt_ocram_initialize + * + * Description: + * Called off reset vector to reconfigure the flexRAM + * and finish the FLASH to RAM Copy. + * CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin + * + ****************************************************************************/ + +__EXPORT void imxrt_ocram_initialize(void) +{ + uint32_t regval; + register uint64_t *src; + register uint64_t *dest; + + /* Reallocate + * Final Configuration is + * No DTCM + * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) + * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) + * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) + * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) + * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) + * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) + * 256k System OCRAM M4 (2020:0000-2023:ffff) + */ + + putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17); + putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18); + regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); + putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16); + + /* Copy any necessary code sections from FLASH to ITCM. The process is the + * same as the code copying from FLASH to RAM above. */ + for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; + dest < (uint64_t *)&_eitcmfuncs;) { + *dest++ = *src++; + } + + /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be + * certain that there are no issues with the state of global variables. + */ + + for (dest = &_sdtcm; dest < &_edtcm;) { + *dest++ = 0; + } + +#if defined(CONFIG_BOOT_RUNFROMISRAM) + const uint32_t *src; + uint32_t *dest; + + for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size), + dest = (uint32_t *)(g_boot_data.start + g_boot_data.size); + dest < (uint32_t *) &_etext;) { + *dest++ = *src++; + } + +#endif +} diff --git a/boards/px4/fmu-v6xrt/src/init.c b/boards/px4/fmu-v6xrt/src/init.c index 4b2edbc658..25faa213f5 100644 --- a/boards/px4/fmu-v6xrt/src/init.c +++ b/boards/px4/fmu-v6xrt/src/init.c @@ -109,11 +109,6 @@ extern void led_off(int led); extern uint32_t _srodata; /* Start of .rodata */ extern uint32_t _erodata; /* End of .rodata */ -extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ -extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ -extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ -extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ -extern uint64_t _edtcm; /* Copy destination end address in DTCM */ __END_DECLS /************************************************************************************ @@ -158,8 +153,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_gpio_output(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } @@ -225,65 +225,7 @@ void imxrt_flash_setup_prefetch_partition(void) ARM_ISB(); ARM_DMB(); } -/**************************************************************************** - * Name: imxrt_ocram_initialize - * - * Description: - * Called off reset vector to reconfigure the flexRAM - * and finish the FLASH to RAM Copy. - * - ****************************************************************************/ -__EXPORT void imxrt_ocram_initialize(void) -{ - uint32_t regval; - register uint64_t *src; - register uint64_t *dest; - - /* Reallocate - * Final Configuration is - * No DTCM - * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) - * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) - * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) - * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) - * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) - * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) - * 256k System OCRAM M4 (2020:0000-2023:ffff) - */ - - putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17); - putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18); - regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); - putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16); - - /* Copy any necessary code sections from FLASH to ITCM. The process is the - * same as the code copying from FLASH to RAM above. */ - for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; - dest < (uint64_t *)&_eitcmfuncs;) { - *dest++ = *src++; - } - - /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be - * certain that there are no issues with the state of global variables. - */ - - for (dest = &_sdtcm; dest < &_edtcm;) { - *dest++ = 0; - } - -#if defined(CONFIG_BOOT_RUNFROMISRAM) - const uint32_t *src; - uint32_t *dest; - - for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size), - dest = (uint32_t *)(g_boot_data.start + g_boot_data.size); - dest < (uint32_t *) &_etext;) { - *dest++ = *src++; - } - -#endif -} /**************************************************************************** * Name: imxrt_boardinitialize diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index f157cf5ae8..b0b7a8a2b3 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y @@ -52,6 +51,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y @@ -80,3 +80,4 @@ CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y CONFIG_EXAMPLES_WORK_ITEM=y +CONFIG_MODULES_SPACECRAFT=n diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board new file mode 100644 index 0000000000..56c966598b --- /dev/null +++ b/boards/px4/sitl/spacecraft.px4board @@ -0,0 +1,19 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_ROVER_ACKERMANN=n +CONFIG_MODULES_ROVER_DIFFERENTIAL=n +CONFIG_MODULES_ROVER_MECANUM=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_CONTROL_ALLOCATOR=n +CONFIG_MODULES_SPACECRAFT=y diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board index d1ca456d01..f3e4d1364d 100644 --- a/boards/raspberrypi/pico/default.px4board +++ b/boards/raspberrypi/pico/default.px4board @@ -33,6 +33,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/siyi/n7/src/init.c b/boards/siyi/n7/src/init.c index 46cf4ae4aa..bc8147ece1 100644 --- a/boards/siyi/n7/src/init.c +++ b/boards/siyi/n7/src/init.c @@ -136,8 +136,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/sky-drones/smartap-airlink/src/init.cpp b/boards/sky-drones/smartap-airlink/src/init.cpp index c4b4c1fa7e..73a9582f9b 100644 --- a/boards/sky-drones/smartap-airlink/src/init.cpp +++ b/boards/sky-drones/smartap-airlink/src/init.cpp @@ -142,8 +142,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/spracing/h7extreme/src/init.c b/boards/spracing/h7extreme/src/init.c index b241fb6286..38e7d0cbe8 100644 --- a/boards/spracing/h7extreme/src/init.c +++ b/boards/spracing/h7extreme/src/init.c @@ -125,8 +125,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/x-mav/ap-h743v2/src/board_config.h b/boards/x-mav/ap-h743v2/src/board_config.h index 30802b50a0..ee5cb25a02 100644 --- a/boards/x-mav/ap-h743v2/src/board_config.h +++ b/boards/x-mav/ap-h743v2/src/board_config.h @@ -86,7 +86,7 @@ /* Define GPIO pins used as ADC N.B. Channel numbers must match below */ /* Define Channel numbers must match above GPIO pin IN(n)*/ #define ADC_BATTERY_VOLTAGE_CHANNEL ADC12_CH(4) -#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(5) +#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(8) #define ADC_CHANNELS \ ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ diff --git a/boards/x-mav/ap-h743v2/src/init.c b/boards/x-mav/ap-h743v2/src/init.c index 657c0080c0..cd7d3d2162 100644 --- a/boards/x-mav/ap-h743v2/src/init.c +++ b/boards/x-mav/ap-h743v2/src/init.c @@ -100,8 +100,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/zeroone/x6/init/rc.board_sensors b/boards/zeroone/x6/init/rc.board_sensors index 9658a6412c..19038ecd13 100644 --- a/boards/zeroone/x6/init/rc.board_sensors +++ b/boards/zeroone/x6/init/rc.board_sensors @@ -149,6 +149,9 @@ rm3100 -I -b 4 start # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) ist8310 -X -b 1 -R 10 start +# Internal compass +ist8310 start -I -a 0x0E -R 12 + # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then diff --git a/boards/zeroone/x6/src/init.c b/boards/zeroone/x6/src/init.c index cd6714af16..6320b5580e 100644 --- a/boards/zeroone/x6/src/init.c +++ b/boards/zeroone/x6/src/init.c @@ -144,8 +144,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/cmake/metadata.cmake b/cmake/metadata.cmake index ef946c7031..82b2042701 100644 --- a/cmake/metadata.cmake +++ b/cmake/metadata.cmake @@ -53,6 +53,7 @@ file(GLOB_RECURSE yaml_config_files ${PX4_SOURCE_DIR}/src/modules/*.yaml # avoid param duplicates list(FILTER yaml_config_files EXCLUDE REGEX ".*/pwm_out_sim/") list(FILTER yaml_config_files EXCLUDE REGEX ".*/linux_pwm_out/") +list(FILTER yaml_config_files EXCLUDE REGEX ".*/spacecraft/") add_custom_target(metadata_parameters COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs diff --git a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan deleted file mode 100644 index 1698c6b2bc..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan +++ /dev/null @@ -1,88 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5456085, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458765, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": -1, - "AltitudeMode": 0, - "autoContinue": true, - "command": 21, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458812, - -1 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.3977419, - 8.5458854, - 487.923 - ], - "vehicleType": 2, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/msg/Airspeed.msg b/msg/Airspeed.msg index aaed7b72ca..bc673cf0aa 100644 --- a/msg/Airspeed.msg +++ b/msg/Airspeed.msg @@ -5,6 +5,4 @@ float32 indicated_airspeed_m_s # indicated airspeed in m/s float32 true_airspeed_m_s # true filtered airspeed in m/s -float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown - float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 24f70e12a6..ac59212f40 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -40,19 +40,14 @@ set(msg_files ActionRequest.msg ActuatorArmed.msg ActuatorControlsStatus.msg - ActuatorMotors.msg ActuatorOutputs.msg - ActuatorServos.msg ActuatorServosTrim.msg ActuatorTest.msg AdcReport.msg Airspeed.msg AirspeedValidated.msg AirspeedWind.msg - ArmingCheckReply.msg - ArmingCheckRequest.msg AutotuneAttitudeControlStatus.msg - BatteryStatus.msg Buffer128.msg ButtonEvent.msg CameraCapture.msg @@ -61,8 +56,6 @@ set(msg_files CanInterfaceStatus.msg CellularStatus.msg CollisionConstraints.msg - CollisionReport.msg - ConfigOverrides.msg ControlAllocatorStatus.msg Cpuload.msg DatamanRequest.msg @@ -110,7 +103,6 @@ set(msg_files GimbalManagerSetAttitude.msg GimbalManagerSetManualControl.msg GimbalManagerStatus.msg - GotoSetpoint.msg GpioConfig.msg GpioIn.msg GpioOut.msg @@ -120,9 +112,9 @@ set(msg_files Gripper.msg HealthReport.msg HeaterStatus.msg - HomePosition.msg HoverThrustEstimate.msg InputRc.msg + InternalCombustionEngineControl.msg InternalCombustionEngineStatus.msg IridiumsbdStatus.msg IrlockReport.msg @@ -136,7 +128,6 @@ set(msg_files LogMessage.msg MagnetometerBiasEstimate.msg MagWorkerData.msg - ManualControlSetpoint.msg ManualControlSwitches.msg MavlinkLog.msg MavlinkTunnel.msg @@ -145,7 +136,6 @@ set(msg_files Mission.msg MissionResult.msg MountOrientation.msg - ModeCompleted.msg NavigatorMissionItem.msg NavigatorStatus.msg NormalizedUnsignedSetpoint.msg @@ -182,13 +172,13 @@ set(msg_files RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg - RegisterExtComponentReply.msg - RegisterExtComponentRequest.msg - RoverAckermannGuidanceStatus.msg - RoverAckermannStatus.msg - RoverDifferentialGuidanceStatus.msg - RoverDifferentialSetpoint.msg - RoverDifferentialStatus.msg + RoverAttitudeSetpoint.msg + RoverAttitudeStatus.msg + RoverVelocityStatus.msg + RoverRateSetpoint.msg + RoverRateStatus.msg + RoverSteeringSetpoint.msg + RoverThrottleSetpoint.msg RoverMecanumGuidanceStatus.msg RoverMecanumSetpoint.msg RoverMecanumStatus.msg @@ -222,48 +212,56 @@ set(msg_files TelemetryStatus.msg TiltrotorExtraControls.msg TimesyncStatus.msg - TrajectoryBezier.msg - TrajectorySetpoint.msg - TrajectoryWaypoint.msg TransponderReport.msg TuneControl.msg UavcanParameterRequest.msg UavcanParameterValue.msg UlogStream.msg UlogStreamAck.msg - UnregisterExtComponent.msg VehicleAcceleration.msg VehicleAirData.msg VehicleAngularAccelerationSetpoint.msg - VehicleAngularVelocity.msg - VehicleAttitude.msg - VehicleAttitudeSetpoint.msg - VehicleCommand.msg - VehicleCommandAck.msg VehicleConstraints.msg - VehicleControlMode.msg - VehicleGlobalPosition.msg VehicleImu.msg VehicleImuStatus.msg - VehicleLandDetected.msg - VehicleLocalPosition.msg VehicleLocalPositionSetpoint.msg VehicleMagnetometer.msg - VehicleOdometry.msg VehicleOpticalFlow.msg VehicleOpticalFlowVel.msg - VehicleRatesSetpoint.msg VehicleRoi.msg - VehicleStatus.msg VehicleThrustSetpoint.msg VehicleTorqueSetpoint.msg - VehicleTrajectoryBezier.msg - VehicleTrajectoryWaypoint.msg VelocityLimits.msg - VtolVehicleStatus.msg WheelEncoders.msg Wind.msg YawEstimatorStatus.msg + versioned/ActuatorMotors.msg + versioned/ActuatorServos.msg + versioned/ArmingCheckReply.msg + versioned/ArmingCheckRequest.msg + versioned/BatteryStatus.msg + versioned/ConfigOverrides.msg + versioned/GotoSetpoint.msg + versioned/HomePosition.msg + versioned/ManualControlSetpoint.msg + versioned/ModeCompleted.msg + versioned/RegisterExtComponentReply.msg + versioned/RegisterExtComponentRequest.msg + versioned/TrajectorySetpoint.msg + versioned/UnregisterExtComponent.msg + versioned/VehicleAngularVelocity.msg + versioned/VehicleAttitude.msg + versioned/VehicleAttitudeSetpoint.msg + versioned/VehicleCommandAck.msg + versioned/VehicleCommand.msg + versioned/VehicleControlMode.msg + versioned/VehicleGlobalPosition.msg + versioned/VehicleLandDetected.msg + versioned/VehicleLocalPosition.msg + versioned/VehicleOdometry.msg + versioned/VehicleRatesSetpoint.msg + versioned/VehicleStatus.msg + versioned/VtolVehicleStatus.msg ) list(SORT msg_files) @@ -315,7 +313,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${msg_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS @@ -336,7 +334,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --json -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${msg_source_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS @@ -374,7 +372,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --headers -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${ucdr_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/ucdr DEPENDS @@ -396,7 +394,7 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --sources -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${msg_source_out_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/uorb DEPENDS @@ -447,7 +445,7 @@ if(CONFIG_LIB_CDRSTREAM) # Copy .msg files foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) - configure_file(${PX4_SOURCE_DIR}/msg/${msg}.msg ${idl_out_path}/${msg}.msg COPYONLY) + configure_file(${msg_file} ${idl_out_path}/${msg}.msg COPYONLY) list(APPEND uorb_cdr_idl ${idl_out_path}/${msg}.idl) list(APPEND uorb_cdr_msg ${idl_out_path}/${msg}.msg) list(APPEND uorb_cdr_idl_uorb ${idl_uorb_path}/${msg}.h) @@ -492,7 +490,7 @@ if(CONFIG_LIB_CDRSTREAM) COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/msg/px_generate_uorb_topic_files.py --uorb-idl-header -f ${msg_files} - -i ${CMAKE_CURRENT_SOURCE_DIR} + -i ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/versioned -o ${idl_uorb_path} -e ${PX4_SOURCE_DIR}/Tools/msg/templates/cdrstream DEPENDS diff --git a/msg/CollisionReport.msg b/msg/CollisionReport.msg deleted file mode 100644 index 1ad7ce7265..0000000000 --- a/msg/CollisionReport.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint8 src -uint32 id -uint8 action -uint8 threat_level -float32 time_to_minimum_delta -float32 altitude_minimum_delta -float32 horizontal_minimum_delta diff --git a/msg/Ekf2Timestamps.msg b/msg/Ekf2Timestamps.msg index ae3ac06766..2487cf0200 100644 --- a/msg/Ekf2Timestamps.msg +++ b/msg/Ekf2Timestamps.msg @@ -14,6 +14,7 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times # difference of +-3.2s to the sensor_combined topic. int16 airspeed_timestamp_rel +int16 airspeed_validated_timestamp_rel int16 distance_sensor_timestamp_rel int16 optical_flow_timestamp_rel int16 vehicle_air_data_timestamp_rel diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 14e3ac3f84..8f3a9f0cda 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -7,7 +7,7 @@ uint32 device_id uint64 time_last_fuse -float32[2] observation +float64[2] observation float32[2] observation_variance float32[2] innovation diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index dd62bc4aca..3c7985e630 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -27,9 +27,9 @@ uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fus uint8 CS_MAG_DEC = 6 # 6 - true if synthetic magnetic declination measurements are being fused uint8 CS_IN_AIR = 7 # 7 - true when thought to be airborne uint8 CS_WIND = 8 # 8 - true when wind velocity is being estimated -uint8 CS_BARO_HGT = 9 # 9 - true when baro height is being fused as a primary height reference -uint8 CS_RNG_HGT = 10 # 10 - true when range finder height is being fused as a primary height reference -uint8 CS_GPS_HGT = 11 # 11 - true when GPS height is being fused as a primary height reference +uint8 CS_BARO_HGT = 9 # 9 - true when baro data is being fused +uint8 CS_RNG_HGT = 10 # 10 - true when range finder data is being fused for height aiding +uint8 CS_GPS_HGT = 11 # 11 - true when GPS altitude is being fused uint8 CS_EV_POS = 12 # 12 - true when local position data from external vision is being fused uint8 CS_EV_YAW = 13 # 13 - true when yaw data from external vision measurements is being fused uint8 CS_EV_HGT = 14 # 14 - true when height data from external vision measurements is being fused diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 6b17842c26..0e5525d545 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -13,9 +13,9 @@ bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fus bool cs_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended bool cs_in_air # 7 - true when the vehicle is airborne bool cs_wind # 8 - true when wind velocity is being estimated -bool cs_baro_hgt # 9 - true when baro height is being fused as a primary height reference -bool cs_rng_hgt # 10 - true when range finder height is being fused as a primary height reference -bool cs_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool cs_baro_hgt # 9 - true when baro data is being fused +bool cs_rng_hgt # 10 - true when range finder data is being fused for height aiding +bool cs_gps_hgt # 11 - true when GPS altitude is being fused bool cs_ev_pos # 12 - true when local position data fusion from external vision is intended bool cs_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended bool cs_ev_hgt # 14 - true when height data from external vision measurements is being fused @@ -47,6 +47,7 @@ bool cs_rng_terrain # 39 - true if we are fusing range finder data f bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused bool cs_constant_pos # 42 - true if the vehicle is at a constant position +bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/GimbalDeviceAttitudeStatus.msg b/msg/GimbalDeviceAttitudeStatus.msg index 0be66babe1..0007a1c03d 100644 --- a/msg/GimbalDeviceAttitudeStatus.msg +++ b/msg/GimbalDeviceAttitudeStatus.msg @@ -16,5 +16,8 @@ float32 angular_velocity_y float32 angular_velocity_z uint32 failure_flags +float32 delta_yaw +float32 delta_yaw_velocity +uint8 gimbal_device_id bool received_from_mavlink diff --git a/msg/GimbalDeviceInformation.msg b/msg/GimbalDeviceInformation.msg index 8f7a416439..0735167212 100644 --- a/msg/GimbalDeviceInformation.msg +++ b/msg/GimbalDeviceInformation.msg @@ -33,4 +33,4 @@ float32 pitch_max # [rad] float32 yaw_min # [rad] float32 yaw_max # [rad] -uint8 gimbal_device_compid +uint8 gimbal_device_id diff --git a/msg/GimbalManagerSetAttitude.msg b/msg/GimbalManagerSetAttitude.msg index d88acca8b6..e1a174fd0b 100644 --- a/msg/GimbalManagerSetAttitude.msg +++ b/msg/GimbalManagerSetAttitude.msg @@ -20,3 +20,5 @@ float32[4] q float32 angular_velocity_x float32 angular_velocity_y float32 angular_velocity_z + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/InternalCombustionEngineControl.msg b/msg/InternalCombustionEngineControl.msg new file mode 100644 index 0000000000..08b4198e01 --- /dev/null +++ b/msg/InternalCombustionEngineControl.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +bool ignition_on # activate/deactivate ignition (Spark Plug) +float32 throttle_control # [0,1] - Motor should idle with 0. Includes slew rate if enabled. +float32 choke_control # [0,1] - 1 fully closes the air inlet. +float32 starter_engine_control # [0,1] - control value for electric starter motor. + +uint8 user_request # user intent for the ICE being on/off diff --git a/msg/ManualControlSwitches.msg b/msg/ManualControlSwitches.msg index 4d1cbab232..39b6efe71f 100644 --- a/msg/ManualControlSwitches.msg +++ b/msg/ManualControlSwitches.msg @@ -29,6 +29,8 @@ uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGH uint8 photo_switch # Photo trigger switch uint8 video_switch # Photo trigger switch +uint8 payload_power_switch # Payload power switch + uint8 engage_main_motor_switch # Engage the main motor (for helicopters) uint32 switch_changes # number of switch changes diff --git a/msg/PositionControllerStatus.msg b/msg/PositionControllerStatus.msg index 7237351fd0..44ec5412ba 100644 --- a/msg/PositionControllerStatus.msg +++ b/msg/PositionControllerStatus.msg @@ -7,6 +7,4 @@ float32 target_bearing # Bearing angle from aircraft to current target [rad] float32 xtrack_error # Signed track error [m] float32 wp_dist # Distance to active (next) waypoint [m] float32 acceptance_radius # Current horizontal acceptance radius [m] -float32 yaw_acceptance # Yaw acceptance error[rad] -float32 altitude_acceptance # Current vertical acceptance error [m] uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/msg/PwmInput.msg b/msg/PwmInput.msg index fcc7dbe4ac..805d6fbd6b 100644 --- a/msg/PwmInput.msg +++ b/msg/PwmInput.msg @@ -1,4 +1,4 @@ -uint64 timestamp # Time since system start (microseconds) -uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) -uint32 pulse_width # Pulse width, timer counts -uint32 period # Period, timer counts +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts (microseconds) +uint32 period # Period, timer counts (microseconds) diff --git a/msg/RcChannels.msg b/msg/RcChannels.msg index 546755f66f..b932dfd33b 100644 --- a/msg/RcChannels.msg +++ b/msg/RcChannels.msg @@ -28,13 +28,14 @@ uint8 FUNCTION_FLTBTN_SLOT_4 = 24 uint8 FUNCTION_FLTBTN_SLOT_5 = 25 uint8 FUNCTION_FLTBTN_SLOT_6 = 26 uint8 FUNCTION_ENGAGE_MAIN_MOTOR = 27 +uint8 FUNCTION_PAYLOAD_POWER = 28 uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6 uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[28] function # Functions mapping +int8[29] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg deleted file mode 100644 index a06d1290bf..0000000000 --- a/msg/RoverAckermannGuidanceStatus.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 desired_speed # [m/s] Rover desired ground speed -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [deg] Heading error of the pure pursuit controller -float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions - -# TOPICS rover_ackermann_guidance_status diff --git a/msg/RoverAckermannStatus.msg b/msg/RoverAckermannStatus.msg deleted file mode 100644 index bf0556b945..0000000000 --- a/msg/RoverAckermannStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 throttle_setpoint # [-1, 1] Normalized throttle setpoint -float32 steering_setpoint # [-1, 1] Normalized steering setpoint -float32 actual_speed # [m/s] Rover ground speed - -# TOPICS rover_ackermann_status diff --git a/msg/RoverAttitudeSetpoint.msg b/msg/RoverAttitudeSetpoint.msg new file mode 100644 index 0000000000..0ea9a7ea17 --- /dev/null +++ b/msg/RoverAttitudeSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 yaw_setpoint # [rad] Expressed in NED frame + +# TOPICS rover_attitude_setpoint diff --git a/msg/RoverAttitudeStatus.msg b/msg/RoverAttitudeStatus.msg new file mode 100644 index 0000000000..197d31c2a7 --- /dev/null +++ b/msg/RoverAttitudeStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_yaw # [rad/s] Measured yaw rate +float32 adjusted_yaw_setpoint # [rad/s] Yaw setpoint that is being tracked (Applied slew rates) + +# TOPICS rover_attitude_status diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg deleted file mode 100644 index ce3d375111..0000000000 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] - -# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg deleted file mode 100644 index b16f712828..0000000000 --- a/msg/RoverDifferentialSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover -float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) -float32 yaw_rate_setpoint_normalized # [-1, 1] Normalized yaw rate for the rover -float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover - -# TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg deleted file mode 100644 index 808da3dc0d..0000000000 --- a/msg/RoverDifferentialStatus.msg +++ /dev/null @@ -1,13 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 actual_speed # [m/s] Actual forward speed of the rover -float32 actual_yaw # [rad] Actual yaw of the rover -float32 actual_yaw_rate # [rad/s] Actual yaw rate of the rover -float32 desired_yaw_rate # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller -float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint -float32 speed_diff_normalized # [-1, 1] Normalized speed difference setpoint between the left and right motor -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller - -# TOPICS rover_differential_status diff --git a/msg/RoverMecanumSetpoint.msg b/msg/RoverMecanumSetpoint.msg index 8718c039a7..0cc9415be1 100644 --- a/msg/RoverMecanumSetpoint.msg +++ b/msg/RoverMecanumSetpoint.msg @@ -5,7 +5,7 @@ float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward s float32 lateral_speed_setpoint # [m/s] Desired lateral speed float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed float32 yaw_rate_setpoint # [rad/s] Desired yaw rate -float32 yaw_rate_setpoint_normalized # [-1, 1] Desired normalized yaw rate +float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels float32 yaw_setpoint # [rad] Desired yaw (heading) # TOPICS rover_mecanum_setpoint diff --git a/msg/RoverMecanumStatus.msg b/msg/RoverMecanumStatus.msg index d9bc7b127e..7547fd5be6 100644 --- a/msg/RoverMecanumStatus.msg +++ b/msg/RoverMecanumStatus.msg @@ -1,13 +1,17 @@ uint64 timestamp # time since system start (microseconds) -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards -float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output of the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 measured_yaw # [rad] Measured yaw -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller -float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller +float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards +float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left +float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller +float32 measured_yaw # [rad] Measured yaw +float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller +float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller +float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller +float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller # TOPICS rover_mecanum_status diff --git a/msg/RoverRateSetpoint.msg b/msg/RoverRateSetpoint.msg new file mode 100644 index 0000000000..88054f9dd6 --- /dev/null +++ b/msg/RoverRateSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 yaw_rate_setpoint # [rad/s] Expressed in NED frame + +# TOPICS rover_rate_setpoint diff --git a/msg/RoverRateStatus.msg b/msg/RoverRateStatus.msg new file mode 100644 index 0000000000..4e82eb30fa --- /dev/null +++ b/msg/RoverRateStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint that is being tracked (Applied slew rates) +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller + +# TOPICS rover_rate_status diff --git a/msg/RoverSteeringSetpoint.msg b/msg/RoverSteeringSetpoint.msg new file mode 100644 index 0000000000..252215ee8a --- /dev/null +++ b/msg/RoverSteeringSetpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Only for Ackermann-steered rovers) +float32 normalized_speed_diff # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Only for Differential/Mecanum rovers) + +# TOPICS rover_steering_setpoint diff --git a/msg/RoverThrottleSetpoint.msg b/msg/RoverThrottleSetpoint.msg new file mode 100644 index 0000000000..3b6d7047b3 --- /dev/null +++ b/msg/RoverThrottleSetpoint.msg @@ -0,0 +1,7 @@ + +uint64 timestamp # time since system start (microseconds) + +float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] +float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] + +# TOPICS rover_throttle_setpoint diff --git a/msg/RoverVelocityStatus.msg b/msg/RoverVelocityStatus.msg new file mode 100644 index 0000000000..f16de0f6b7 --- /dev/null +++ b/msg/RoverVelocityStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_speed_body_x # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards +float32 speed_body_x_setpoint # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards +float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards +float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left +float32 speed_body_y_setpoint # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) +float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) +float32 pid_throttle_body_x_integral # Integral of the PID for the closed loop controller of the speed in body x direction +float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction + +# TOPICS rover_velocity_status diff --git a/msg/Rpm.msg b/msg/Rpm.msg index baab7c6a67..b4de2cf422 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,5 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -float32 indicated_frequency_rpm # indicated rotor Frequency in Revolution per minute -float32 estimated_accurancy_rpm # estimated accuracy in Revolution per minute +# rpm values of 0.0 mean within a timeout there is no movement measured +float32 rpm_estimate # filtered revolutions per minute +float32 rpm_raw diff --git a/msg/SystemPower.msg b/msg/SystemPower.msg index 21b35ba1dc..05754dab0e 100644 --- a/msg/SystemPower.msg +++ b/msg/SystemPower.msg @@ -1,5 +1,6 @@ uint64 timestamp # time since system start (microseconds) float32 voltage5v_v # peripheral 5V rail voltage +float32 voltage_payload_v # payload rail voltage float32[4] sensors3v3 # Sensors 3V3 rail voltage uint8 sensors3v3_valid # Sensors 3V3 rail voltage was read (bitfield). uint8 usb_connected # USB is connected when 1 @@ -10,6 +11,7 @@ uint8 periph_5v_oc # peripheral overcurrent when 1 uint8 hipower_5v_oc # high power peripheral overcurrent when 1 uint8 comp_5v_valid # 5V to companion valid uint8 can1_gps1_5v_valid # 5V for CAN1/GPS1 valid +uint8 payload_v_valid # payload rail voltage is valid uint8 BRICK1_VALID_SHIFTS=0 uint8 BRICK1_VALID_MASK=1 diff --git a/msg/TelemetryStatus.msg b/msg/TelemetryStatus.msg index 48d85f934d..b2b3920a36 100644 --- a/msg/TelemetryStatus.msg +++ b/msg/TelemetryStatus.msg @@ -51,13 +51,11 @@ bool heartbeat_type_open_drone_id # MAV_TYPE_ODID bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO bool heartbeat_component_log # MAV_COMP_ID_LOG bool heartbeat_component_osd # MAV_COMP_ID_OSD -bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE # Misc component health -bool avoidance_system_healthy bool open_drone_id_system_healthy bool parachute_system_healthy diff --git a/msg/TrajectoryBezier.msg b/msg/TrajectoryBezier.msg deleted file mode 100644 index e3d9d4e0ff..0000000000 --- a/msg/TrajectoryBezier.msg +++ /dev/null @@ -1,8 +0,0 @@ -# Bezier Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier - -uint64 timestamp # time since system start (microseconds) - -float32[3] position # local position x,y,z (metres) -float32 yaw # yaw angle (rad) -float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds) diff --git a/msg/TrajectoryWaypoint.msg b/msg/TrajectoryWaypoint.msg deleted file mode 100644 index 6ea9bae4d3..0000000000 --- a/msg/TrajectoryWaypoint.msg +++ /dev/null @@ -1,13 +0,0 @@ -# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint - -uint64 timestamp # time since system start (microseconds) - -float32[3] position -float32[3] velocity -float32[3] acceleration -float32 yaw -float32 yaw_speed - -bool point_valid -uint8 type diff --git a/msg/VehicleAirData.msg b/msg/VehicleAirData.msg index 59ca5e5c8d..41e5358ae8 100644 --- a/msg/VehicleAirData.msg +++ b/msg/VehicleAirData.msg @@ -6,10 +6,10 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 baro_device_id # unique device ID for the selected barometer float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. -float32 baro_temp_celcius # Temperature in degrees Celsius float32 baro_pressure_pa # Absolute pressure in Pascals +float32 ambient_temperature # Abient temperature in degrees Celsius +uint8 temperature_source # Source of temperature data: 0: Default Temperature (15°C), 1: External Baro, 2: Airspeed float32 rho # air density -float32 eas2tas # equivalent airspeed to true airspeed conversion factor uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/msg/VehicleImu.msg b/msg/VehicleImu.msg index a71bb7a01f..24912e06fb 100644 --- a/msg/VehicleImu.msg +++ b/msg/VehicleImu.msg @@ -9,8 +9,8 @@ uint32 gyro_device_id # Gyroscope unique device ID for the sensor that float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt) float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt) -uint16 delta_angle_dt # integration period in microseconds -uint16 delta_velocity_dt # integration period in microseconds +uint32 delta_angle_dt # integration period in microseconds +uint32 delta_velocity_dt # integration period in microseconds uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 diff --git a/msg/VehicleTrajectoryBezier.msg b/msg/VehicleTrajectoryBezier.msg deleted file mode 100644 index d4bf99b469..0000000000 --- a/msg/VehicleTrajectoryBezier.msg +++ /dev/null @@ -1,18 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the -# companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryBezier[5] control_points -uint8 bezier_order - -# TOPICS vehicle_trajectory_bezier diff --git a/msg/VehicleTrajectoryWaypoint.msg b/msg/VehicleTrajectoryWaypoint.msg deleted file mode 100644 index 6bff1cec84..0000000000 --- a/msg/VehicleTrajectoryWaypoint.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. -# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 - -uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum. - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryWaypoint[5] waypoints - -# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/msg/px4_msgs_old/CMakeLists.txt b/msg/px4_msgs_old/CMakeLists.txt new file mode 100644 index 0000000000..add08bc670 --- /dev/null +++ b/msg/px4_msgs_old/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.5) + +project(px4_msgs_old) + +list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# ############################################################################## +# Generate ROS messages, ROS2 interfaces and IDL files # +# ############################################################################## + +# get all msg files +set(MSGS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/msg") +file(GLOB PX4_MSGS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${MSGS_DIR}/*.msg") + +# get all srv files +set(SRVS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/srv") +file(GLOB PX4_SRVS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${SRVS_DIR}/*.srv") + + + +# For the versioned topics, replace the namespace (px4_msgs_old -> px4_msgs) and message type name (Vx -> ), +# so that DDS does not reject the subscription/publication due to mismatching type +# rosidl_typesupport_fastrtps_cpp +set(rosidl_typesupport_fastrtps_cpp_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp_wrapper.py) +file(TOUCH ${rosidl_typesupport_fastrtps_cpp_BIN}) + +# rosidl_typesupport_fastrtps_c +set(rosidl_typesupport_fastrtps_c_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_c_wrapper.py) +file(TOUCH ${rosidl_typesupport_fastrtps_c_BIN}) + +# rosidl_typesupport_introspection_cpp (for cyclonedds) +set(rosidl_typesupport_introspection_cpp_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_introspection_cpp_wrapper.py) +file(TOUCH ${rosidl_typesupport_introspection_cpp_BIN}) + +# Generate introspection typesupport for C and C++ and IDL files +if(PX4_MSGS) + rosidl_generate_interfaces(${PROJECT_NAME} + ${PX4_MSGS} + ${PX4_SRVS} + DEPENDENCIES builtin_interfaces + ADD_LINTER_TESTS + ) +endif() + +# rosidl_typesupport_fastrtps_cpp +set(rosidl_typesupport_fastrtps_cpp_orig ${rosidl_typesupport_fastrtps_cpp_DIR}) +string(REPLACE "share/rosidl_typesupport_fastrtps_cpp/cmake" "lib/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp" + rosidl_typesupport_fastrtps_cpp_orig ${rosidl_typesupport_fastrtps_cpp_DIR}) +set(original_script_path ${rosidl_typesupport_fastrtps_cpp_orig}) +configure_file(rename_msg_type.py.in ${rosidl_typesupport_fastrtps_cpp_BIN} @ONLY) + +# rosidl_typesupport_fastrtps_c +set(rosidl_typesupport_fastrtps_c_orig ${rosidl_typesupport_fastrtps_c_DIR}) +string(REPLACE "share/rosidl_typesupport_fastrtps_c/cmake" "lib/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c" + rosidl_typesupport_fastrtps_c_orig ${rosidl_typesupport_fastrtps_c_DIR}) +set(original_script_path ${rosidl_typesupport_fastrtps_c_orig}) +configure_file(rename_msg_type.py.in ${rosidl_typesupport_fastrtps_c_BIN} @ONLY) + +# rosidl_typesupport_introspection_cpp +set(rosidl_typesupport_introspection_cpp_orig ${rosidl_typesupport_introspection_cpp_DIR}) +string(REPLACE "share/rosidl_typesupport_introspection_cpp/cmake" "lib/rosidl_typesupport_introspection_cpp/rosidl_typesupport_introspection_cpp" + rosidl_typesupport_introspection_cpp_orig ${rosidl_typesupport_introspection_cpp_DIR}) +set(original_script_path ${rosidl_typesupport_introspection_cpp_orig}) +configure_file(rename_msg_type.py.in ${rosidl_typesupport_introspection_cpp_BIN} @ONLY) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/msg/px4_msgs_old/msg/VehicleStatusV0.msg b/msg/px4_msgs_old/msg/VehicleStatusV0.msg new file mode 100644 index 0000000000..a347dfce3d --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV0.msg @@ -0,0 +1,140 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_FREE4 = 8 +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_AIRSHIP = 4 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/px4_msgs_old/package.xml b/msg/px4_msgs_old/package.xml new file mode 100644 index 0000000000..42b83c6f9a --- /dev/null +++ b/msg/px4_msgs_old/package.xml @@ -0,0 +1,25 @@ + + + + px4_msgs_old + 2.0.1 + Package with the ROS-equivalent of PX4 uORB msgs (old message definitions) + PX4 + BSD 3-Clause + + ament_cmake + rosidl_default_generators + + builtin_interfaces + ros_environment + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/msg/px4_msgs_old/rename_msg_type.py.in b/msg/px4_msgs_old/rename_msg_type.py.in new file mode 100755 index 0000000000..31d8054e95 --- /dev/null +++ b/msg/px4_msgs_old/rename_msg_type.py.in @@ -0,0 +1,39 @@ +#! /bin/python +import sys +import subprocess +import json +import os +import re + +original_script = "@original_script_path@" +args = sys.argv[1:] + +json_file = [arg for arg in args if arg.endswith('.json')][0] + +proc = subprocess.run(['python3', original_script] + args) +proc.check_returncode() + +def replace_namespace_and_type(content: str): + # Replace namespace type + content = content.replace('"px4_msgs_old"', '"px4_msgs"') + content = content.replace('"px4_msgs_old::msg"', '"px4_msgs::msg"') + # Replace versioned type with non-versioned one + content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+"', '\\1"', content) + # Services + content = content.replace('"px4_msgs_old::srv"', '"px4_msgs::srv"') + content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Request"', '\\1_Request"', content) + content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Response"', '\\1_Response"', content) + return content + +with open(json_file, 'r') as f: + data = json.load(f) + output_dir = data['output_dir'] + + # Iterate files recursively + for root, dirs, files in os.walk(output_dir): + for file in files: + with open(os.path.join(root, file), 'r+') as f: + content = f.read() + f.seek(0) + f.write(replace_namespace_and_type(content)) + f.truncate() diff --git a/msg/translation_node/CMakeLists.txt b/msg/translation_node/CMakeLists.txt new file mode 100644 index 0000000000..4880709ce2 --- /dev/null +++ b/msg/translation_node/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.8) +project(translation_node) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Werror) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(px4_msgs_old REQUIRED) + +if(DEFINED ENV{ROS_DISTRO}) + set(ROS_DISTRO $ENV{ROS_DISTRO}) +else() + set(ROS_DISTRO "rolling") +endif() + + +add_library(${PROJECT_NAME}_lib + src/monitor.cpp + src/pub_sub_graph.cpp + src/service_graph.cpp + src/translations.cpp +) +ament_target_dependencies(${PROJECT_NAME}_lib rclcpp px4_msgs px4_msgs_old) +add_executable(${PROJECT_NAME}_bin + src/main.cpp +) +target_link_libraries(${PROJECT_NAME}_bin ${PROJECT_NAME}_lib) +target_include_directories(${PROJECT_NAME}_bin PUBLIC src) +ament_target_dependencies(${PROJECT_NAME}_bin rclcpp px4_msgs px4_msgs_old) +install(TARGETS + ${PROJECT_NAME}_bin + DESTINATION lib/${PROJECT_NAME}) + +option(DISABLE_SERVICES "Disable services" OFF) +if(${ROS_DISTRO} STREQUAL "humble") + message(WARNING "Disabling services for ROS humble (API is not supported)") + target_compile_definitions(${PROJECT_NAME}_lib PRIVATE DISABLE_SERVICES) + set(DISABLE_SERVICES ON) +endif() + +if(BUILD_TESTING) + find_package(std_msgs REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(rosidl_default_generators REQUIRED) + ament_lint_auto_find_test_dependencies() + + set(SRV_FILES + test/srv/TestV0.srv + test/srv/TestV1.srv + test/srv/TestV2.srv + ) + rosidl_generate_interfaces(${PROJECT_NAME} ${SRV_FILES}) + + # Unit tests + set(TEST_SRC + test/graph.cpp + test/main.cpp + test/pub_sub.cpp + ) + if (NOT DISABLE_SERVICES) + list(APPEND TEST_SRC test/services.cpp) + endif() + ament_add_gtest(${PROJECT_NAME}_unit_tests + ${TEST_SRC} + ) + target_include_directories(${PROJECT_NAME}_unit_tests PRIVATE ${CMAKE_CURRENT_LIST_DIR}) + target_compile_options(${PROJECT_NAME}_unit_tests PRIVATE -Wno-error=sign-compare) # There is a warning from gtest internal + target_link_libraries(${PROJECT_NAME}_unit_tests ${PROJECT_NAME}_lib) + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_unit_tests "${cpp_typesupport_target}") + ament_target_dependencies(${PROJECT_NAME}_unit_tests + std_msgs + rclcpp + ) +endif() + +ament_package() diff --git a/msg/translation_node/README.md b/msg/translation_node/README.md new file mode 100644 index 0000000000..69e0843bb5 --- /dev/null +++ b/msg/translation_node/README.md @@ -0,0 +1,6 @@ +# Message Translations + +This package contains a message translation node and a set of old message conversion methods. +It allows to run applications that are compiled with one set of message versions against a PX4 with another set of message versions, without having to change either the application or the PX4 side. + +For details, see https://docs.px4.io/main/en/ros2/px4_ros2_msg_translation_node.html. diff --git a/msg/translation_node/package.xml b/msg/translation_node/package.xml new file mode 100644 index 0000000000..7d6e03511b --- /dev/null +++ b/msg/translation_node/package.xml @@ -0,0 +1,27 @@ + + + + translation_node + 0.0.0 + Message version translation node + PX4 + BSD 3-Clause + + ament_cmake + rosidl_default_generators + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + std_msgs + + rclcpp + px4_msgs + px4_msgs_old + + + ament_cmake + + diff --git a/msg/translation_node/src/graph.h b/msg/translation_node/src/graph.h new file mode 100644 index 0000000000..0feac52fa9 --- /dev/null +++ b/msg/translation_node/src/graph.h @@ -0,0 +1,293 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include "util.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// This implements a directed graph with potential cycles used for translation. +// There are 2 types of nodes: messages (e.g. publication/subscription endpoints) and +// translations. Translation nodes are always in between message nodes, and can have N input messages +// and M output messages. + +struct MessageIdentifier { + std::string topic_name; + MessageVersionType version; + + bool operator==(const MessageIdentifier& other) const { + return topic_name == other.topic_name && version == other.version; + } + bool operator!=(const MessageIdentifier& other) const { + return !(*this == other); + } +}; + +template<> +struct std::hash +{ + std::size_t operator()(const MessageIdentifier& s) const noexcept + { + std::size_t h1 = std::hash{}(s.topic_name); + std::size_t h2 = std::hash{}(s.version); + return h1 ^ (h2 << 1); + } +}; + + +using MessageBuffer = std::shared_ptr; + +template +class MessageNode; +template +class Graph; + +template +using MessageNodePtrT = std::shared_ptr>; + +template +class TranslationNode { +public: + using TranslationCB = std::function&, std::vector&)>; + + TranslationNode(std::vector> inputs, + std::vector> outputs, + TranslationCB translation_db) + : _inputs(std::move(inputs)), _outputs(std::move(outputs)), _translation_cb(std::move(translation_db)) { + assert(_inputs.size() <= kMaxNumInputs); + + _input_buffers.resize(_inputs.size()); + for (unsigned i = 0; i < _inputs.size(); ++i) { + _input_buffers[i] = _inputs[i]->buffer(); + } + + _output_buffers.resize(_outputs.size()); + for (unsigned i = 0; i < _outputs.size(); ++i) { + _output_buffers[i] = _outputs[i]->buffer(); + } + } + + void setInputReady(unsigned index) { + _inputs_ready.set(index); + } + + bool translate() { + if (_inputs_ready.count() == _input_buffers.size()) { + _translation_cb(_input_buffers, _output_buffers); + _inputs_ready.reset(); + return true; + } + return false; + } + + const std::vector>& inputs() const { return _inputs; } + const std::vector>& outputs() const { return _outputs; } + +private: + static constexpr int kMaxNumInputs = 32; + + const std::vector> _inputs; + std::vector _input_buffers; ///< Cached buffers from _inputs.buffer() + const std::vector> _outputs; + std::vector _output_buffers; + const TranslationCB _translation_cb; + + std::bitset _inputs_ready; +}; + +template +using TranslationNodePtrT = std::shared_ptr>; + + +template +class MessageNode { +public: + + explicit MessageNode(NodeData node_data, size_t index, MessageBuffer message_buffer) + : _buffer(std::move(message_buffer)), _data(std::move(node_data)), _index(index) {} + + MessageBuffer& buffer() { return _buffer; } + + void addTranslationInput(TranslationNodePtrT node, unsigned input_index) { + _translations.push_back(Translation{std::move(node), input_index}); + } + + NodeData& data() { return _data; } + + void resetNodes() { + _translations.clear(); + } + +private: + struct Translation { + TranslationNodePtrT node; ///< Counterpart to the TranslationNode::_inputs + unsigned input_index; ///< Index into the TranslationNode::_inputs + }; + MessageBuffer _buffer; + std::vector _translations; + + NodeData _data; + + const size_t _index; + + friend class Graph; +}; + +template +class Graph { +public: + using MessageNodePtr = MessageNodePtrT; + + ~Graph() { + // Explicitly reset the nodes array to break up potential cycles and prevent memory leaks + for (auto& [id, node] : _nodes) { + node->resetNodes(); + } + } + + /** + * @brief Add a message node if it does not exist already + */ + bool addNodeIfNotExists(const IdType& id, NodeData node_data, const MessageBuffer& message_buffer) { + if (_nodes.find(id) != _nodes.end()) { + return false; + } + // Node that we cannot remove nodes due to using the index as an array index + const size_t index = _nodes.size(); + _nodes.insert({id, std::make_shared>(std::move(node_data), index, message_buffer)}); + return true; + } + + /** + * @brief Add a translation edge with N inputs and M output nodes. All nodes must already exist. + */ + void addTranslation(const typename TranslationNode::TranslationCB& translation_cb, + const std::vector& inputs, const std::vector& outputs) { + auto init = [this](const std::vector& from, std::vector>& to) { + for (unsigned i=0; i < from.size(); ++i) { + auto node_iter = _nodes.find(from[i]); + assert(node_iter != _nodes.end()); + to[i] = node_iter->second; + } + }; + std::vector> input_nodes(inputs.size()); + init(inputs, input_nodes); + std::vector> output_nodes(outputs.size()); + init(outputs, output_nodes); + + auto translation_node = std::make_shared>(std::move(input_nodes), std::move(output_nodes), translation_cb); + for (unsigned i=0; i < translation_node->inputs().size(); ++i) { + translation_node->inputs()[i]->addTranslationInput(translation_node, i); + } + } + + + /** + * @brief Translate a message node in the graph. + * + * @param node The message node to translate. + * @param on_translated A callback function that is called for translated nodes (with an updated message buffer). + * This will not be called for the provided node. + */ + void translate(const MessageNodePtr& node, + const std::function& on_translated) { + resetNodesVisited(); + + // Iterate all reachable nodes from a given node using the BFS (shortest path) algorithm, + // while using translation nodes as barriers (only continue when all inputs are ready) + + std::queue queue; + _node_visited[node->_index] = true; + queue.push(node); + + while (!queue.empty()) { + MessageNodePtr current = queue.front(); + queue.pop(); + for (auto& translation : current->_translations) { + const bool any_output_visited = + std::any_of(translation.node->outputs().begin(), translation.node->outputs().end(), [&](const MessageNodePtr& next_node) { + return _node_visited[next_node->_index]; + }); + // If any output node has already been visited, skip this translation node (prevents translating + // backwards, from where we came from already) + if (any_output_visited) { + continue; + } + translation.node->setInputReady(translation.input_index); + // Iterate the output nodes only if the translation node is ready + if (translation.node->translate()) { + + for (auto &next_node : translation.node->outputs()) { + if (_node_visited[next_node->_index]) { + continue; + } + _node_visited[next_node->_index] = true; + on_translated(next_node); + queue.push(next_node); + } + } + } + } + } + + std::optional findNode(const IdType& id) const { + auto iter = _nodes.find(id); + if (iter == _nodes.end()) { + return std::nullopt; + } + return iter->second; + } + + void iterateNodes(const std::function& cb) const { + for (const auto& [id, node] : _nodes) { + cb(id, node); + } + } + + /** + * Iterate all reachable nodes from a given node using the BFS (shortest path) algorithm + */ + void iterateBFS(const MessageNodePtr& node, const std::function& cb) { + resetNodesVisited(); + + std::queue queue; + _node_visited[node->_index] = true; + queue.push(node); + cb(node); + + while (!queue.empty()) { + MessageNodePtr current = queue.front(); + queue.pop(); + for (auto& translation : current->_translations) { + for (auto& next_node : translation.node->outputs()) { + if (_node_visited[next_node->_index]) { + continue; + } + _node_visited[next_node->_index] = true; + queue.push(next_node); + + cb(next_node); + } + } + } + } + + +private: + void resetNodesVisited() { + _node_visited.resize(_nodes.size()); + std::fill(_node_visited.begin(), _node_visited.end(), false); + } + + std::unordered_map _nodes; + std::vector _node_visited; ///< Cached, to avoid the need to re-allocate on each iteration +}; diff --git a/msg/translation_node/src/main.cpp b/msg/translation_node/src/main.cpp new file mode 100644 index 0000000000..218583d85d --- /dev/null +++ b/msg/translation_node/src/main.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include + +#include + +#include "../translations/all_translations.h" +#include "pub_sub_graph.h" +#include "service_graph.h" +#include "monitor.h" + +using namespace std::chrono_literals; + +class RosTranslationNode : public rclcpp::Node +{ +public: + RosTranslationNode() : Node("translation_node") + { + _pub_sub_graph = std::make_unique(*this, RegisteredTranslations::instance().topicTranslations()); + _service_graph = std::make_unique(*this, RegisteredTranslations::instance().serviceTranslations()); + _monitor = std::make_unique(*this, _pub_sub_graph.get(), _service_graph.get()); + } + +private: + std::unique_ptr _pub_sub_graph; + std::unique_ptr _service_graph; + rclcpp::TimerBase::SharedPtr _node_update_timer; + std::unique_ptr _monitor; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/msg/translation_node/src/monitor.cpp b/msg/translation_node/src/monitor.cpp new file mode 100644 index 0000000000..9746ec3f89 --- /dev/null +++ b/msg/translation_node/src/monitor.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include "monitor.h" +using namespace std::chrono_literals; + +Monitor::Monitor(rclcpp::Node &node, PubSubGraph* pub_sub_graph, ServiceGraph* service_graph) + : _node(node), _pub_sub_graph(pub_sub_graph), _service_graph(service_graph) { + + // Monitor subscriptions & publishers + // TODO: event-based + _node_update_timer = _node.create_wall_timer(1s, [this]() { + updateNow(); + }); +} + +void Monitor::updateNow() { + + // Topics + if (_pub_sub_graph != nullptr) { + std::vector topic_info; + const auto topics = _node.get_topic_names_and_types(); + for (const auto &[topic_name, topic_types]: topics) { + auto publishers = _node.get_publishers_info_by_topic(topic_name); + auto subscribers = _node.get_subscriptions_info_by_topic(topic_name); + // Filter out self + int num_publishers = 0; + for (const auto &publisher: publishers) { + num_publishers += publisher.node_name() != _node.get_name(); + } + int num_subscribers = 0; + for (const auto &subscriber: subscribers) { + num_subscribers += subscriber.node_name() != _node.get_name(); + } + + if (num_subscribers > 0 || num_publishers > 0) { + topic_info.emplace_back(PubSubGraph::TopicInfo{topic_name, num_subscribers, num_publishers}); + } + } + _pub_sub_graph->updateCurrentTopics(topic_info); + } + + // Services +#ifndef DISABLE_SERVICES // ROS Humble does not support the count_services() call + if (_service_graph != nullptr) { + std::vector service_info; + const auto services = _node.get_service_names_and_types(); + for (const auto& [service_name, service_types] : services) { + const int num_services = _node.get_node_graph_interface()->count_services(service_name); + const int num_clients = _node.get_node_graph_interface()->count_clients(service_name); + // We cannot filter out our own node, as we don't have that info. + // We could use `get_service_names_and_types_by_node`, but then we would not get + // services by non-ros nodes (e.g. microxrce dds bridge) + service_info.emplace_back(ServiceGraph::ServiceInfo{service_name, num_services, num_clients}); + } + _service_graph->updateCurrentServices(service_info); + } +#endif +} diff --git a/msg/translation_node/src/monitor.h b/msg/translation_node/src/monitor.h new file mode 100644 index 0000000000..cd335aa83b --- /dev/null +++ b/msg/translation_node/src/monitor.h @@ -0,0 +1,23 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include "pub_sub_graph.h" +#include "service_graph.h" +#include + +class Monitor { +public: + explicit Monitor(rclcpp::Node &node, PubSubGraph* pub_sub_graph, ServiceGraph* service_graph); + + void updateNow(); + +private: + rclcpp::Node &_node; + PubSubGraph* _pub_sub_graph{nullptr}; + ServiceGraph* _service_graph{nullptr}; + rclcpp::TimerBase::SharedPtr _node_update_timer; +}; diff --git a/msg/translation_node/src/pub_sub_graph.cpp b/msg/translation_node/src/pub_sub_graph.cpp new file mode 100644 index 0000000000..f75d20fe96 --- /dev/null +++ b/msg/translation_node/src/pub_sub_graph.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include "pub_sub_graph.h" +#include "util.h" + +PubSubGraph::PubSubGraph(rclcpp::Node &node, const TopicTranslations &translations) : _node(node) { + + std::unordered_map> known_versions; + + for (const auto& topic : translations.topics()) { + const std::string full_topic_name = getFullTopicName(_node.get_effective_namespace(), topic.id.topic_name); + _known_topics_warned.insert({full_topic_name, false}); + + const MessageIdentifier id{full_topic_name, topic.id.version}; + NodeDataPubSub node_data{topic.subscription_factory, topic.publication_factory, id, topic.max_serialized_message_size}; + _pub_sub_graph.addNodeIfNotExists(id, std::move(node_data), topic.message_buffer); + known_versions[full_topic_name].insert(id.version); + } + + auto get_full_topic_names = [this](std::vector ids) { + for (auto& id : ids) { + id.topic_name = getFullTopicName(_node.get_effective_namespace(), id.topic_name); + } + return ids; + }; + + for (const auto& translation : translations.translations()) { + const std::vector inputs = get_full_topic_names(translation.inputs); + const std::vector outputs = get_full_topic_names(translation.outputs); + _pub_sub_graph.addTranslation(translation.cb, inputs, outputs); + } + + printTopicInfo(known_versions); + handleLargestTopic(known_versions); +} + +void PubSubGraph::updateCurrentTopics(const std::vector &topics) { + + _pub_sub_graph.iterateNodes([](const MessageIdentifier& type, const Graph::MessageNodePtr& node) { + node->data().has_external_publisher = false; + node->data().has_external_subscriber = false; + node->data().visited = false; + }); + + for (const auto& info : topics) { + const auto [non_versioned_topic_name, version] = getNonVersionedTopicName(info.topic_name); + auto maybe_node = _pub_sub_graph.findNode({non_versioned_topic_name, version}); + if (!maybe_node) { + auto known_topic_iter = _known_topics_warned.find(non_versioned_topic_name); + if (known_topic_iter != _known_topics_warned.end() && !known_topic_iter->second) { + RCLCPP_WARN(_node.get_logger(), "No translation available for version %i of topic %s", version, non_versioned_topic_name.c_str()); + known_topic_iter->second = true; + } + continue; + } + const auto& node = maybe_node.value(); + + if (info.num_publishers > 0) { + node->data().has_external_publisher = true; + } + if (info.num_subscribers > 0) { + node->data().has_external_subscriber = true; + } + } + + // Iterate connected graph segments + _pub_sub_graph.iterateNodes([this](const MessageIdentifier& type, const Graph::MessageNodePtr& node) { + if (node->data().visited) { + return; + } + node->data().visited = true; + + // Count the number of external subscribers and publishers for each connected graph + int num_publishers = 0; + int num_subscribers = 0; + int num_subscribers_without_publisher = 0; + + _pub_sub_graph.iterateBFS(node, [&](const Graph::MessageNodePtr& node) { + if (node->data().has_external_publisher) { + ++num_publishers; + } + if (node->data().has_external_subscriber) { + ++num_subscribers; + if (!node->data().has_external_publisher) { + ++num_subscribers_without_publisher; + } + } + }); + + // We need to instantiate publishers and subscribers if: + // - there are multiple publishers and at least 1 subscriber + // - there is 1 publisher and at least 1 subscriber on another node + // Note that in case of splitting or merging topics, this might create more entities than actually needed + const bool require_translation = (num_publishers >= 2 && num_subscribers >= 1) + || (num_publishers == 1 && num_subscribers_without_publisher >= 1); + if (require_translation) { + _pub_sub_graph.iterateBFS(node, [&](const Graph::MessageNodePtr& node) { + node->data().visited = true; + // Has subscriber(s)? + if (node->data().has_external_subscriber && !node->data().publication) { + RCLCPP_INFO(_node.get_logger(), "Found subscriber for topic '%s', version: %i, adding publisher", node->data().topic_name.c_str(), node->data().version); + node->data().publication = node->data().publication_factory(_node); + } else if (!node->data().has_external_subscriber && node->data().publication) { + RCLCPP_INFO(_node.get_logger(), "No subscribers for topic '%s', version: %i, removing publisher", node->data().topic_name.c_str(), node->data().version); + node->data().publication.reset(); + } + // Has publisher(s)? + if (node->data().has_external_publisher && !node->data().subscription) { + RCLCPP_INFO(_node.get_logger(), "Found publisher for topic '%s', version: %i, adding subscriber", node->data().topic_name.c_str(), node->data().version); + node->data().subscription = node->data().subscription_factory(_node, [this, node_cpy=node]() { + onSubscriptionUpdate(node_cpy); + }); + } else if (!node->data().has_external_publisher && node->data().subscription) { + RCLCPP_INFO(_node.get_logger(), "No publishers for topic '%s', version: %i, removing subscriber", node->data().topic_name.c_str(), node->data().version); + node->data().subscription.reset(); + } + }); + + } else { + // Reset any publishers or subscribers + _pub_sub_graph.iterateBFS(node, [&](const Graph::MessageNodePtr& node) { + node->data().visited = true; + if (node->data().publication) { + RCLCPP_INFO(_node.get_logger(), "Removing publisher for topic '%s', version: %i", + node->data().topic_name.c_str(), node->data().version); + node->data().publication.reset(); + } + if (node->data().subscription) { + RCLCPP_INFO(_node.get_logger(), "Removing subscriber for topic '%s', version: %i", + node->data().topic_name.c_str(), node->data().version); + node->data().subscription.reset(); + } + }); + } + }); +} + +void PubSubGraph::onSubscriptionUpdate(const Graph::MessageNodePtr& node) { + _pub_sub_graph.translate( + node, + [this](const Graph::MessageNodePtr& node) { + if (node->data().publication != nullptr) { + const auto ret = rcl_publish(node->data().publication->get_publisher_handle().get(), + node->buffer().get(), nullptr); + if (ret != RCL_RET_OK) { + RCLCPP_WARN_ONCE(_node.get_logger(), "Failed to publish on topic '%s', version: %i", + node->data().topic_name.c_str(), node->data().version); + } + } + }); + +} + +void PubSubGraph::printTopicInfo(const std::unordered_map>& known_versions) const { + // Print info about known versions + RCLCPP_INFO(_node.get_logger(), "Registered pub/sub topics and versions:"); + for (const auto& [topic_name, version_set] : known_versions) { + if (version_set.empty()) { + continue; + } + const std::string versions = std::accumulate(std::next(version_set.begin()), version_set.end(), + std::to_string(*version_set.begin()), // start with first element + [](std::string a, auto&& b) { + return std::move(a) + ", " + std::to_string(b); + }); + RCLCPP_INFO(_node.get_logger(), "- %s: %s", topic_name.c_str(), versions.c_str()); + } +} + + +void PubSubGraph::handleLargestTopic(const std::unordered_map> &known_versions) { + // FastDDS caches some type information per DDS participant when first creating a publisher or subscriber for a given + // type. The information that is relevant for us is the maximum serialized message size. + // Since different versions can have different sizes, we need to ensure the first publication or subscription + // happens with the version of the largest size. Otherwise, an out-of-memory exception can be triggered. + // And the type must continue to be in use (so we cannot delete it) + for (const auto& [topic_name, versions] : known_versions) { + size_t max_serialized_message_size = 0; + const PublicationFactoryCB* publication_factory_for_max = nullptr; + for (auto version : versions) { + const auto& node = _pub_sub_graph.findNode(MessageIdentifier{topic_name, version}); + assert(node); + const auto& node_data = node.value()->data(); + if (node_data.max_serialized_message_size > max_serialized_message_size) { + max_serialized_message_size = node_data.max_serialized_message_size; + publication_factory_for_max = &node_data.publication_factory; + } + } + if (publication_factory_for_max) { + _largest_topic_publications.emplace_back((*publication_factory_for_max)(_node)); + } + } +} diff --git a/msg/translation_node/src/pub_sub_graph.h b/msg/translation_node/src/pub_sub_graph.h new file mode 100644 index 0000000000..3dcc96fa9e --- /dev/null +++ b/msg/translation_node/src/pub_sub_graph.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include "translations.h" +#include "translation_util.h" +#include "graph.h" + +class PubSubGraph { +public: + struct TopicInfo { + std::string topic_name; ///< fully qualified topic name (with namespace) + int num_subscribers; ///< does not include this node's subscribers + int num_publishers; ///< does not include this node's publishers + }; + + PubSubGraph(rclcpp::Node& node, const TopicTranslations& translations); + + void updateCurrentTopics(const std::vector& topics); + +private: + struct NodeDataPubSub { + explicit NodeDataPubSub(SubscriptionFactoryCB subscription_factory, PublicationFactoryCB publication_factory, + const MessageIdentifier& id, size_t max_serialized_message_size) + : subscription_factory(std::move(subscription_factory)), publication_factory(std::move(publication_factory)), + topic_name(id.topic_name), version(id.version), max_serialized_message_size(max_serialized_message_size) + { } + + const SubscriptionFactoryCB subscription_factory; + const PublicationFactoryCB publication_factory; + const std::string topic_name; + const MessageVersionType version; + const size_t max_serialized_message_size; + + // Keep track if there's currently a publisher/subscriber + bool has_external_publisher{false}; + bool has_external_subscriber{false}; + + rclcpp::SubscriptionBase::SharedPtr subscription; + rclcpp::PublisherBase::SharedPtr publication; + + bool visited{false}; + }; + + void onSubscriptionUpdate(const Graph::MessageNodePtr& node); + void printTopicInfo(const std::unordered_map>& known_versions) const; + void handleLargestTopic(const std::unordered_map>& known_versions); + + rclcpp::Node& _node; + Graph _pub_sub_graph; + std::unordered_map _known_topics_warned; + + std::vector _largest_topic_publications; +}; diff --git a/msg/translation_node/src/service_graph.cpp b/msg/translation_node/src/service_graph.cpp new file mode 100644 index 0000000000..66a389647c --- /dev/null +++ b/msg/translation_node/src/service_graph.cpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "service_graph.h" + +#include + +using namespace std::chrono_literals; + +ServiceGraph::ServiceGraph(rclcpp::Node &node, const ServiceTranslations& translations) + : _node(node) { + + std::unordered_map> known_versions; + + for (const auto& service : translations.nodes()) { + const std::string full_topic_name = getFullTopicName(_node.get_effective_namespace(), service.id.topic_name); + _known_services_warned.insert({full_topic_name, false}); + + const MessageIdentifier id{full_topic_name, service.id.version}; + auto node_data = std::make_shared(service, id); + _request_graph.addNodeIfNotExists(id, node_data, service.message_buffer_request); + _response_graph.addNodeIfNotExists(id, node_data, service.message_buffer_response); + known_versions[full_topic_name].insert(id.version); + } + + auto get_full_topic_names = [this](std::vector ids) { + for (auto& id : ids) { + id.topic_name = getFullTopicName(_node.get_effective_namespace(), id.topic_name); + } + return ids; + }; + + for (const auto& translation : translations.requestTranslations()) { + const std::vector inputs = get_full_topic_names(translation.inputs); + const std::vector outputs = get_full_topic_names(translation.outputs); + _request_graph.addTranslation(translation.cb, inputs, outputs); + } + for (const auto& translation : translations.responseTranslations()) { + const std::vector inputs = get_full_topic_names(translation.inputs); + const std::vector outputs = get_full_topic_names(translation.outputs); + _response_graph.addTranslation(translation.cb, inputs, outputs); + } + + printServiceInfo(known_versions); + handleLargestTopic(known_versions); + + _cleanup_timer = _node.create_wall_timer(10s, [this]() { + cleanupStaleRequests(); + }); +} + +void ServiceGraph::updateCurrentServices(const std::vector &services) { + _request_graph.iterateNodes([](const MessageIdentifier& type, const GraphForService::MessageNodePtr& node) { + node->data()->has_service = false; + node->data()->has_client = false; + node->data()->visited = false; + }); + + for (const auto& info : services) { + const auto [non_versioned_topic_name, version] = getNonVersionedTopicName(info.service_name); + auto maybe_node = _request_graph.findNode({non_versioned_topic_name, version}); + if (!maybe_node) { + auto known_topic_iter = _known_services_warned.find(non_versioned_topic_name); + if (known_topic_iter != _known_services_warned.end() && !known_topic_iter->second) { + RCLCPP_WARN(_node.get_logger(), "No translation available for version %i of service %s", version, non_versioned_topic_name.c_str()); + known_topic_iter->second = true; + } + continue; + } + const auto& node = maybe_node.value(); + + if (info.num_services > 0) { + node->data()->has_service = true; + } + if (info.num_clients > 0) { + node->data()->has_client = true; + } + } + + // Iterate connected graph segments + _request_graph.iterateNodes([this](const MessageIdentifier& type, const GraphForService::MessageNodePtr& node) { + if (node->data()->visited) { + return; + } + node->data()->visited = true; + + // Check if there's a reachable node with a service + int num_services = 0; + + _request_graph.iterateBFS(node, [&](const GraphForService::MessageNodePtr& node) { + if (node->data()->has_service && !node->data()->service) { + ++num_services; + } + }); + + // We need to instantiate a service and clients if there's exactly one external service. + if (num_services > 1 ) { + RCLCPP_ERROR_ONCE(_node.get_logger(), "Found %i services for service '%s', skipping this service", + num_services, node->data()->service_name.c_str()); + } else if (num_services == 1) { + _request_graph.iterateBFS(node, [&](const GraphForService::MessageNodePtr& node) { + node->data()->visited = true; + if (node->data()->has_service && !node->data()->client && !node->data()->service) { + RCLCPP_INFO(_node.get_logger(), "Found service for '%s', version: %i, adding client", node->data()->service_name.c_str(), node->data()->version); + auto tuple = node->data()->client_factory(_node, [this, tmp_node=node](rmw_request_id_t& request) { + onResponse(request, tmp_node); + }); + node->data()->client = std::get<0>(tuple); + node->data()->client_send_cb = std::get<1>(tuple); + + } else if (!node->data()->has_service && !node->data()->service && node->data()->has_client) { + RCLCPP_INFO(_node.get_logger(), "Found client for '%s', version: %i, adding service", node->data()->service_name.c_str(), node->data()->version); + node->data()->service = node->data()->service_factory(_node, [this, tmp_node=node](std::shared_ptr req_id) { + onNewRequest(std::move(req_id), tmp_node); + }); + } + }); + + } else { + // Reset any service or client + _request_graph.iterateBFS(node, [&](const GraphForService::MessageNodePtr& node) { + node->data()->visited = true; + if (node->data()->service) { + RCLCPP_INFO(_node.get_logger(), "Removing service for '%s', version: %i", + node->data()->service_name.c_str(), node->data()->version); + node->data()->service.reset(); + } + if (node->data()->client) { + RCLCPP_INFO(_node.get_logger(), "Removing client for '%s', version: %i", + node->data()->service_name.c_str(), node->data()->version); + node->data()->client.reset(); + } + }); + } + }); +} + +void ServiceGraph::printServiceInfo(const std::unordered_map>& known_versions) const { + // Print info about known versions + RCLCPP_INFO(_node.get_logger(), "Registered services and versions:"); + for (const auto& [topic_name, version_set] : known_versions) { + if (version_set.empty()) { + continue; + } + const std::string versions = std::accumulate(std::next(version_set.begin()), version_set.end(), + std::to_string(*version_set.begin()), // start with first element + [](std::string a, auto&& b) { + return std::move(a) + ", " + std::to_string(b); + }); + RCLCPP_INFO(_node.get_logger(), "- %s: %s", topic_name.c_str(), versions.c_str()); + } +} + +void ServiceGraph::handleLargestTopic(const std::unordered_map> &known_versions) { + // See PubSubGraph::handleLargestTopic for an explanation why this is needed + unsigned index = 0; + for (const auto& [topic_name, versions] : known_versions) { + std::array max_serialized_message_size{0, 0}; + std::array publication_factory_for_max{nullptr, nullptr}; + for (auto version : versions) { + const auto& node = _request_graph.findNode(MessageIdentifier{topic_name, version}); + assert(node); + const auto& node_data = node.value()->data(); + for (unsigned i = 0; i < max_serialized_message_size.size(); ++i) { + if (node_data->max_serialized_message_size[i] > max_serialized_message_size[i]) { + max_serialized_message_size[i] = node_data->max_serialized_message_size[i]; + publication_factory_for_max[i] = &node_data->publication_factory[i]; + } + } + } + for (unsigned i = 0; i < max_serialized_message_size.size(); ++i) { + if (publication_factory_for_max[i]) { + const std::string tmp_topic_name = "dummy_topic" + std::to_string(index++); + _largest_topic_publications.emplace_back((*publication_factory_for_max[i])(_node, tmp_topic_name)); + } + } + } +} + +void ServiceGraph::onNewRequest(std::shared_ptr req_id, GraphForService::MessageNodePtr node) { + bool service_called = false; + _request_graph.translate(node, [this, &service_called, &req_id, original_node=node](const GraphForService::MessageNodePtr& node) { + if (node->data()->client && node->data()->client_send_cb && !service_called) { + service_called = true; + const int64_t client_request_id = node->data()->client_send_cb(node->buffer()); + node->data()->ongoing_requests[client_request_id] = Request{req_id, original_node->data(), _node.now()}; + } + }); +} + +void ServiceGraph::onResponse(rmw_request_id_t &req_id, GraphForService::MessageNodePtr node) { + auto iter = node->data()->ongoing_requests.find(req_id.sequence_number); + if (iter == node->data()->ongoing_requests.end()) { + RCLCPP_ERROR(_node.get_logger(), "Got response with unknown request %li", req_id.sequence_number); + return; + } + bool service_called = false; + auto response_node = _response_graph.findNode({node->data()->service_name, node->data()->version}); + assert(response_node); + _response_graph.translate(response_node.value(), [this, &service_called, &iter](const GraphForService::MessageNodePtr &node) { + if (node->data()->service && !service_called && iter->second.original_node_data == node->data()) { + const rcl_ret_t ret = rcl_send_response(node->data()->service->get_service_handle().get(), + iter->second.original_request_id.get(), node->buffer().get()); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR(_node.get_logger(), "Failed to send response: %s", rcl_get_error_string().str); + } + service_called = true; + } + }); + + node->data()->ongoing_requests.erase(iter); +} + +void ServiceGraph::cleanupStaleRequests() { + static const auto kRequestTimeout = 20s; + _request_graph.iterateNodes([this](const MessageIdentifier& type, const GraphForService::MessageNodePtr& node) { + for (auto it = node->data()->ongoing_requests.begin(); it != node->data()->ongoing_requests.end();) { + const auto& request = it->second; + if (_node.now() - request.timestamp_received > kRequestTimeout) { + RCLCPP_INFO(_node.get_logger(), "Request timed out, dropping ongoing request for '%s', version: %i, request id: %li", + node->data()->service_name.c_str(), node->data()->version, request.original_request_id->sequence_number); + it = node->data()->ongoing_requests.erase(it); + } else { + ++it; + } + } + }); +} diff --git a/msg/translation_node/src/service_graph.h b/msg/translation_node/src/service_graph.h new file mode 100644 index 0000000000..f290bc1f35 --- /dev/null +++ b/msg/translation_node/src/service_graph.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include "translations.h" +#include "translation_util.h" +#include "graph.h" + +class ServiceGraph { +public: + struct ServiceInfo { + std::string service_name; ///< fully qualified service name (with namespace) + int num_services; ///< This can include a service created by the translation node + int num_clients; ///< This can include a client created by the translation node + }; + + ServiceGraph(rclcpp::Node &node, const ServiceTranslations& translations); + + void updateCurrentServices(const std::vector& services); + +private: + struct NodeDataService; + using GraphForService = Graph>; + + void printServiceInfo(const std::unordered_map> &known_versions) const; + void handleLargestTopic(const std::unordered_map>& known_versions); + + void onNewRequest(std::shared_ptr req_id, GraphForService::MessageNodePtr node); + void onResponse(rmw_request_id_t& req_id, GraphForService::MessageNodePtr node); + void cleanupStaleRequests(); + + struct Request { + std::shared_ptr original_request_id; + std::shared_ptr original_node_data{nullptr}; + rclcpp::Time timestamp_received; + }; + struct NodeDataService { + explicit NodeDataService(const Service& service, const MessageIdentifier& id) + : service_factory(service.service_factory), client_factory(service.client_factory), + service_name(id.topic_name), version(id.version), + publication_factory{service.publication_factory_request, service.publication_factory_response}, + max_serialized_message_size{service.max_serialized_message_size_request, service.max_serialized_message_size_response} + { } + + const ServiceFactoryCB service_factory; + const ClientFactoryCB client_factory; + const std::string service_name; + const MessageVersionType version; + const std::array publication_factory; // Request/Response + const std::array max_serialized_message_size; + + // Keep track if there's currently a client/service + bool has_service{false}; + bool has_client{false}; + + rclcpp::ClientBase::SharedPtr client; + ClientSendCB client_send_cb; + rclcpp::ServiceBase::SharedPtr service; + + std::unordered_map ongoing_requests; ///< Ongoing service calls for this node + + bool visited{false}; + }; + + rclcpp::Node& _node; + GraphForService _request_graph; + GraphForService _response_graph; + std::unordered_map _known_services_warned; + rclcpp::TimerBase::SharedPtr _cleanup_timer; + + std::vector _largest_topic_publications; +}; diff --git a/msg/translation_node/src/template_util.h b/msg/translation_node/src/template_util.h new file mode 100644 index 0000000000..8566f34d5e --- /dev/null +++ b/msg/translation_node/src/template_util.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include + +/** + * Helper struct to store template parameter packs + */ +template +struct Pack { +}; + +/** + * Struct for a template parameter pack with access to the individual types + */ +template +struct TypesArray { + template + struct TypeHelper { + using Type = T; + using Next = TypeHelper; + }; + + using Type1 = typename TypeHelper::Type; + using Type2 = typename TypeHelper::Next::Type; + using Type3 = typename TypeHelper::Next::Next::Type; + using Type4 = typename TypeHelper::Next::Next::Next::Type; + using Type5 = typename TypeHelper::Next::Next::Next::Next::Type; + using Type6 = typename TypeHelper::Next::Next::Next::Next::Next::Type; + + using args = Pack; +}; + + +/** + * Helper for call_translation_function() + */ +template +inline void call_translation_function_impl(F f, Pack, Pack, + const std::vector>& messages_in, + std::vector>& messages_out, + std::integer_sequence, std::integer_sequence) +{ + f(*static_cast(messages_in[Is].get())..., *static_cast(messages_out[Os].get())...); +} + +/** + * Call a translation function F which takes the arguments (const ArgsIn&..., ArgsOut&...), + * by passing messages_in and messages_out as arguments. + * Note that sizeof(ArgsIn) == messages_in.length() && sizeof(ArgsOut) == messages_out.length() must hold. + */ +template +inline void call_translation_function(F f, Pack pack_in, Pack pack_out, + const std::vector>& messages_in, + std::vector>& messages_out) { + call_translation_function_impl(f, pack_in, pack_out, messages_in, messages_out, + std::index_sequence_for{}, std::index_sequence_for{}); +} diff --git a/msg/translation_node/src/translation_util.h b/msg/translation_node/src/translation_util.h new file mode 100644 index 0000000000..c5ac4f8153 --- /dev/null +++ b/msg/translation_node/src/translation_util.h @@ -0,0 +1,386 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include "translations.h" +#include "util.h" +#include "template_util.h" + +#include +#include + +class RegisteredTranslations { +public: + + RegisteredTranslations(RegisteredTranslations const&) = delete; + void operator=(RegisteredTranslations const&) = delete; + + + static RegisteredTranslations& instance() { + static RegisteredTranslations instance; + return instance; + } + + /** + * @brief Register a translation class with 1 input and 1 output message. + * + * The translation class has the form: + * + * ``` + * class MyTranslation { + * public: + * using MessageOlder = px4_msgs_old::msg::VehicleAttitudeV2; + * + * using MessageNewer = px4_msgs::msg::VehicleAttitude; + * + * static constexpr const char* kTopic = "fmu/out/vehicle_attitude"; + * + * static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + * // set msg_older from msg_newer + * } + * }; + * ``` + */ + template + void registerDirectTranslation() { + const std::string topic_name = T::kTopic; + _topic_translations.addTopic(getTopicForMessageType(topic_name)); + _topic_translations.addTopic(getTopicForMessageType(topic_name)); + + // Translation callbacks + auto translation_cb_from_older = [](const std::vector& older_msg, std::vector& newer_msg) { + T::fromOlder(*(const typename T::MessageOlder*)older_msg[0].get(), *(typename T::MessageNewer*)newer_msg[0].get()); + }; + auto translation_cb_to_older = [](const std::vector& newer_msg, std::vector& older_msg) { + T::toOlder(*(const typename T::MessageNewer*)newer_msg[0].get(), *(typename T::MessageOlder*)older_msg[0].get()); + }; + _topic_translations.addTranslation({translation_cb_from_older, + {MessageIdentifier{topic_name, T::MessageOlder::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageNewer::MESSAGE_VERSION}}}); + _topic_translations.addTranslation({translation_cb_to_older, + {MessageIdentifier{topic_name, T::MessageNewer::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageOlder::MESSAGE_VERSION}}}); + } + + /** + * @brief Register a translation class for a service. + * + * The translation class has the form: + * + * ``` + * class MyServiceTranslation { + * public: + * using MessageOlder = px4_msgs_old::srv::VehicleCommandV0; + * using MessageNewer = px4_msgs::srv::VehicleCommand; + * + * static constexpr const char* kTopic = "fmu/vehicle_command"; + * + * static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + * // set msg_older from msg_newer + * } + * + * static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + * // set msg_older from msg_newer + * } + * }; + * ``` + */ + template + void registerServiceDirectTranslation() { + const std::string topic_name = T::kTopic; + _service_translations.addNode(getServiceForMessageType(topic_name)); + _service_translations.addNode(getServiceForMessageType(topic_name)); + // Add translations + { // Request + auto translation_cb_from_older = [](const std::vector &older_msg, + std::vector &newer_msg) { + T::fromOlder(*(const typename T::MessageOlder::Request *) older_msg[0].get(), + *(typename T::MessageNewer::Request *) newer_msg[0].get()); + }; + auto translation_cb_to_older = [](const std::vector &newer_msg, + std::vector &older_msg) { + T::toOlder(*(const typename T::MessageNewer::Request *) newer_msg[0].get(), + *(typename T::MessageOlder::Request *) older_msg[0].get()); + }; + _service_translations.addRequestTranslation({translation_cb_from_older, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}}); + _service_translations.addRequestTranslation({translation_cb_to_older, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}}); + } + { // Response + auto translation_cb_from_older = [](const std::vector &older_msg, + std::vector &newer_msg) { + T::fromOlder(*(const typename T::MessageOlder::Response *) older_msg[0].get(), + *(typename T::MessageNewer::Response *) newer_msg[0].get()); + }; + auto translation_cb_to_older = [](const std::vector &newer_msg, + std::vector &older_msg) { + T::toOlder(*(const typename T::MessageNewer::Response *) newer_msg[0].get(), + *(typename T::MessageOlder::Response *) older_msg[0].get()); + }; + _service_translations.addResponseTranslation({translation_cb_from_older, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}}); + _service_translations.addResponseTranslation({translation_cb_to_older, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}}); + } + + } + + /** + * @brief Register a translation class with N input and M output messages. + * + * The translation class has the form: + * ``` + * class MyTranslation { + * public: + * using MessagesOlder = TypesArray; + * static constexpr const char* kTopicsOlder[] = { + * "fmu/out/vehicle_global_position", + * "fmu/out/vehicle_local_position", + * ... + * }; + * + * using MessagesNewer = TypesArray; + * static constexpr const char* kTopicsNewer[] = { + * "fmu/out/vehicle_global_position", + * "fmu/out/vehicle_local_position", + * ... + * }; + * + * static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, ... + * MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2, ...) { + * // Set msg_newerX from msg_olderX + * } + * + * static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, ... + * MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2, ...) { + * // Set msg_olderX from msg_newerX + * } + * }; + * ``` + */ + template + void registerTranslation() { + const auto topics_older = getTopicsForMessageType(typename T::MessagesOlder::args(), T::kTopicsOlder); + std::vector topics_older_identifiers; + for (const auto& topic : topics_older) { + _topic_translations.addTopic(topic); + topics_older_identifiers.emplace_back(topic.id); + } + const auto topics_newer = getTopicsForMessageType(typename T::MessagesNewer::args(),T::kTopicsNewer); + std::vector topics_newer_identifiers; + for (const auto& topic : topics_newer) { + _topic_translations.addTopic(topic); + topics_newer_identifiers.emplace_back(topic.id); + } + + // Translation callbacks + const auto translation_cb_from_older = [](const std::vector& older_msgs, std::vector& newer_msgs) { + call_translation_function(&T::fromOlder, typename T::MessagesOlder::args(), typename T::MessagesNewer::args(), older_msgs, newer_msgs); + }; + const auto translation_cb_to_older = [](const std::vector& newer_msgs, std::vector& older_msgs) { + call_translation_function(&T::toOlder, typename T::MessagesNewer::args(), typename T::MessagesOlder::args(), newer_msgs, older_msgs); + }; + { + // Older -> Newer + Translation translation; + translation.cb = translation_cb_from_older; + translation.inputs = topics_older_identifiers; + translation.outputs = topics_newer_identifiers; + _topic_translations.addTranslation(std::move(translation)); + } + { + // Newer -> Older + Translation translation; + translation.cb = translation_cb_to_older; + translation.inputs = topics_newer_identifiers; + translation.outputs = topics_older_identifiers; + _topic_translations.addTranslation(std::move(translation)); + } + } + + const TopicTranslations& topicTranslations() const { return _topic_translations; } + const ServiceTranslations& serviceTranslations() const { return _service_translations; } + +protected: + RegisteredTranslations() = default; +private: + template + static size_t getMaxSerializedMessageSize() { + const auto type_handle = rclcpp::get_message_type_support_handle(); + const auto fastrtps_handle = rosidl_typesupport_cpp::get_message_typesupport_handle_function(&type_handle, "rosidl_typesupport_fastrtps_cpp"); + if (fastrtps_handle) { + const auto *callbacks = static_cast(fastrtps_handle->data); + char bound_info; + return callbacks->max_serialized_size(bound_info); + } + return 0; + } + + template + static Topic getTopicForMessageType(const std::string& topic_name) { + Topic ret{}; + ret.id.topic_name = topic_name; + ret.id.version = RosMessageType::MESSAGE_VERSION; + auto message_buffer = std::make_shared(); + ret.message_buffer = std::static_pointer_cast(message_buffer); + + // Subscription/Publication factory methods + const std::string topic_name_versioned = getVersionedTopicName(topic_name, ret.id.version); + ret.subscription_factory = [topic_name_versioned, message_buffer](rclcpp::Node& node, + const std::function& on_topic_cb) -> rclcpp::SubscriptionBase::SharedPtr { + return std::dynamic_pointer_cast( + // Note: template instantiation of subscriptions slows down compilation considerably, see + // https://github.com/ros2/rclcpp/issues/1949 + node.create_subscription(topic_name_versioned, rclcpp::QoS(1).best_effort(), + [on_topic_cb=on_topic_cb, message_buffer](typename RosMessageType::UniquePtr msg) -> void { + *message_buffer = *msg; + on_topic_cb(); + })); + }; + ret.publication_factory = [topic_name_versioned](rclcpp::Node& node) -> rclcpp::PublisherBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_publisher(topic_name_versioned, rclcpp::QoS(1).best_effort())); + }; + + ret.max_serialized_message_size = getMaxSerializedMessageSize(); + + return ret; + } + + template + static Service getServiceForMessageType(const std::string& topic_name) { + Service ret{}; + ret.id.topic_name = topic_name; + ret.id.version = RosMessageType::Request::MESSAGE_VERSION; + auto message_buffer_request = std::make_shared(); + ret.message_buffer_request = std::static_pointer_cast(message_buffer_request); + auto message_buffer_response = std::make_shared(); + ret.message_buffer_response = std::static_pointer_cast(message_buffer_response); + + // Service/client factory methods + const std::string topic_name_versioned = getVersionedTopicName(topic_name, ret.id.version); + ret.service_factory = [topic_name_versioned, message_buffer_request](rclcpp::Node& node, + const std::function req_id)>& on_request_cb) -> rclcpp::ServiceBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_service(topic_name_versioned, + [on_request_cb=on_request_cb, message_buffer_request]( + typename rclcpp::Service::SharedPtr service, + std::shared_ptr req_id, + const std::shared_ptr request + ) -> void { + *message_buffer_request = *request; + on_request_cb(std::move(req_id)); + })); + }; + ret.client_factory = [topic_name_versioned, message_buffer_response](rclcpp::Node& node, + const std::function& on_response_cb) { + auto client = node.create_client(topic_name_versioned); + client->set_on_new_response_callback([client, message_buffer_response, on_response_cb](size_t num) { + for (size_t i = 0; i < num; i++) { + rmw_request_id_t request_id{}; + if (client->take_response(*message_buffer_response, request_id)) { + on_response_cb(request_id); + } + } + }); + const auto send_request = [client](MessageBuffer request) { + auto result = client->async_send_request(std::static_pointer_cast(request)); + // We don't need the client to keep track of ongoing requests, so we remove it right away + // to prevent leaks + client->remove_pending_request(result.request_id); + return result.request_id; + }; + return std::make_tuple(std::dynamic_pointer_cast(client), send_request); + }; + + ret.publication_factory_request = [](rclcpp::Node& node, const std::string& topic_name) -> rclcpp::PublisherBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_publisher( + topic_name,rclcpp::QoS(1).best_effort().avoid_ros_namespace_conventions(true))); + }; + ret.publication_factory_response = [](rclcpp::Node& node, const std::string& topic_name) -> rclcpp::PublisherBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_publisher( + topic_name,rclcpp::QoS(1).best_effort().avoid_ros_namespace_conventions(true))); + }; + + ret.max_serialized_message_size_request = getMaxSerializedMessageSize(); + ret.max_serialized_message_size_response = getMaxSerializedMessageSize(); + + return ret; + } + + template + static std::vector getTopicsForMessageTypeImpl(const char* const topics[], std::integer_sequence) { + std::vector ret { + getTopicForMessageType(topics[Is])... + }; + return ret; + } + + template + static std::vector getTopicsForMessageType(Pack, const char* const (&topics)[N]) { + static_assert(N == sizeof...(RosMessageTypes), "Number of topics does not match number of message types"); + return getTopicsForMessageTypeImpl(topics, std::index_sequence_for{}); + } + + TopicTranslations _topic_translations; + ServiceTranslations _service_translations; +}; + +template +class RegistrationHelperDirect { +public: + explicit RegistrationHelperDirect(const char* dummy) { + // There's something strange: when there is no argument passed, the + // compiler removes the static object completely. I don't know + // why but this dummy variable prevents that. + (void)dummy; + RegisteredTranslations::instance().registerDirectTranslation(); + } + explicit RegistrationHelperDirect(const char* dummy, bool for_service) { + (void)dummy; + RegisteredTranslations::instance().registerServiceDirectTranslation(); + } + RegistrationHelperDirect(RegistrationHelperDirect const&) = delete; + void operator=(RegistrationHelperDirect const&) = delete; +}; + +#define REGISTER_TOPIC_TRANSLATION_DIRECT(class_name) \ + RegistrationHelperDirect class_name##_registration_direct("dummy"); + +#define REGISTER_SERVICE_TRANSLATION_DIRECT(class_name) \ + RegistrationHelperDirect class_name##_service_registration_direct("dummy", true); + +template +class TopicRegistrationHelperGeneric { +public: + explicit TopicRegistrationHelperGeneric(const char* dummy) { + (void)dummy; + RegisteredTranslations::instance().registerTranslation(); + } + TopicRegistrationHelperGeneric(TopicRegistrationHelperGeneric const&) = delete; + void operator=(TopicRegistrationHelperGeneric const&) = delete; +}; + +#define REGISTER_TOPIC_TRANSLATION(class_name) \ + TopicRegistrationHelperGeneric class_name##_registration_generic("dummy"); diff --git a/msg/translation_node/src/translations.cpp b/msg/translation_node/src/translations.cpp new file mode 100644 index 0000000000..0b7d8b0c93 --- /dev/null +++ b/msg/translation_node/src/translations.cpp @@ -0,0 +1,5 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include "translations.h" diff --git a/msg/translation_node/src/translations.h b/msg/translation_node/src/translations.h new file mode 100644 index 0000000000..3982d6a1c0 --- /dev/null +++ b/msg/translation_node/src/translations.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" +#include "graph.h" + +#include + + +using TranslationCB = std::function&, std::vector&)>; +using SubscriptionFactoryCB = std::function& on_topic_cb)>; +using PublicationFactoryCB = std::function; +using NamedPublicationFactoryCB = std::function; +using ServiceFactoryCB = std::function req_id)>& on_request_cb)>; +using ClientSendCB = std::function; +using ClientFactoryCB = std::function(rclcpp::Node&, const std::function& on_response_cb)>; + +struct Topic { + MessageIdentifier id; + + SubscriptionFactoryCB subscription_factory; + PublicationFactoryCB publication_factory; + + std::shared_ptr message_buffer; + size_t max_serialized_message_size{}; +}; + +struct Service { + MessageIdentifier id; + + ServiceFactoryCB service_factory; + ClientFactoryCB client_factory; + + NamedPublicationFactoryCB publication_factory_request; + NamedPublicationFactoryCB publication_factory_response; + + std::shared_ptr message_buffer_request; + size_t max_serialized_message_size_request{}; + + std::shared_ptr message_buffer_response; + size_t max_serialized_message_size_response{}; +}; + +struct Translation { + TranslationCB cb; + std::vector inputs; + std::vector outputs; +}; + +class TopicTranslations { +public: + TopicTranslations() = default; + + void addTopic(Topic topic) { _topics.push_back(std::move(topic)); } + void addTranslation(Translation translation) { _translations.push_back(std::move(translation)); } + + const std::vector& topics() const { return _topics; } + const std::vector& translations() const { return _translations; } +private: + std::vector _topics; + std::vector _translations; +}; + +class ServiceTranslations { +public: + ServiceTranslations() = default; + + void addNode(Service node) { _nodes.push_back(std::move(node)); } + void addRequestTranslation(Translation translation) { _request_translations.push_back(std::move(translation)); } + void addResponseTranslation(Translation translation) { _response_translations.push_back(std::move(translation)); } + + const std::vector& nodes() const { return _nodes; } + const std::vector& requestTranslations() const { return _request_translations; } + const std::vector& responseTranslations() const { return _response_translations; } +private: + std::vector _nodes; + std::vector _request_translations; + std::vector _response_translations; +}; diff --git a/msg/translation_node/src/util.h b/msg/translation_node/src/util.h new file mode 100644 index 0000000000..8d584598e9 --- /dev/null +++ b/msg/translation_node/src/util.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +using MessageVersionType = uint32_t; + +static inline std::string getVersionedTopicName(const std::string& topic_name, MessageVersionType version) { + // version == 0 can be used to transition from non-versioned topics to versioned ones + if (version == 0) { + return topic_name; + } + return topic_name + "_v" + std::to_string(version); +} + +static inline std::pair getNonVersionedTopicName(const std::string& topic_name) { + // topic name has the form _v, or just (with version=0) + auto pos = topic_name.find_last_of("_v"); + // Ensure there's at least one more char after the found string + if (pos == std::string::npos || pos + 2 > topic_name.length()) { + return std::make_pair(topic_name, 0); + } + std::string non_versioned_topic_name = topic_name.substr(0, pos - 1); + std::string version = topic_name.substr(pos + 1); + // Ensure only digits are in the version string + for (char c : version) { + if (!std::isdigit(c)) { + return std::make_pair(topic_name, 0); + } + } + return std::make_pair(non_versioned_topic_name, std::stol(version)); +} + +/** + * Get the full topic name, including namespace from a topic name. + * namespace_name should be set to Node::get_effective_namespace() + */ +static inline std::string getFullTopicName(const std::string& namespace_name, const std::string& topic_name) { + std::string full_topic_name = topic_name; + if (!full_topic_name.empty() && full_topic_name[0] != '/') { + if (namespace_name.empty() || namespace_name.back() != '/') { + full_topic_name = '/' + full_topic_name; + } + full_topic_name = namespace_name + full_topic_name; + } + return full_topic_name; +} diff --git a/msg/translation_node/test/graph.cpp b/msg/translation_node/test/graph.cpp new file mode 100644 index 0000000000..e0d1f360cb --- /dev/null +++ b/msg/translation_node/test/graph.cpp @@ -0,0 +1,623 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include + + +TEST(graph, basic) +{ + struct NodeData { + bool iterated{false}; + bool translated{false}; + }; + Graph graph; + + const int32_t message1_value = 3; + const int32_t offset = 4; + + // Add 2 nodes + const MessageIdentifier id1{"topic_name", 1}; + auto buffer1 = std::make_shared(); + *buffer1 = message1_value; + EXPECT_TRUE(graph.addNodeIfNotExists(id1, {}, buffer1)); + EXPECT_FALSE(graph.addNodeIfNotExists(id1, {}, std::make_shared())); + const MessageIdentifier id2{"topic_name", 4}; + auto buffer2 = std::make_shared(); + *buffer2 = 773; + EXPECT_TRUE(graph.addNodeIfNotExists(id2, {}, buffer2)); + + // Search nodes + EXPECT_TRUE(graph.findNode(id1).has_value()); + EXPECT_TRUE(graph.findNode(id2).has_value()); + + // Add 1 translation + auto translation_cb = [&offset](const std::vector& a, std::vector& b) { + auto a_value = static_cast(a[0].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value + offset; + }; + graph.addTranslation(translation_cb, {id1}, {id2}); + + // Iteration from id1 must reach id2 + auto node1 = graph.findNode(id1).value(); + auto node2 = graph.findNode(id2).value(); + auto iterate_cb = [](const Graph::MessageNodePtr& node) { + node->data().iterated = true; + }; + graph.iterateBFS(node1, iterate_cb); + EXPECT_TRUE(node1->data().iterated); + EXPECT_TRUE(node2->data().iterated); + node1->data().iterated = false; + node2->data().iterated = false; + + // Iteration from id2 must not reach id1 + graph.iterateBFS(node2, iterate_cb); + EXPECT_FALSE(node1->data().iterated); + EXPECT_TRUE(node2->data().iterated); + + // Test translation + graph.translate(node1, + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + EXPECT_FALSE(node1->data().translated); + EXPECT_TRUE(node2->data().translated); + EXPECT_EQ(*buffer1, message1_value); + EXPECT_EQ(*buffer2, message1_value + offset); +} + + +TEST(graph, multi_path) +{ + // Multiple paths with cycles + struct NodeData { + unsigned iterated_idx{0}; + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 6; + std::array ids{{ + {"topic_name", 1}, + {"topic_name", 2}, + {"topic_name", 3}, + {"topic_name", 4}, + {"topic_name", 5}, + {"topic_name", 6}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + // Translations + std::bitset<32> translated; + + auto get_translation_cb = [&translated](unsigned bit) { + auto translation_cb = [&translated, bit](const std::vector &a, std::vector &b) { + auto a_value = static_cast(a[0].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value | (1 << bit); + translated.set(bit); + }; + return translation_cb; + }; + + // Graph: + // ___ 2 -- 3 -- 4 + // | | + // 1 _______| + // | + // 5 + // | + // 6 + + unsigned next_bit = 0; + // Connect each node to the previous and next, except the last 3 + for (unsigned i=0; i < num_nodes - 3; ++i) { + graph.addTranslation(get_translation_cb(next_bit++), {ids[i]}, {ids[i+1]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[i+1]}, {ids[i]}); + } + + // Connect the first to the 3rd as well + graph.addTranslation(get_translation_cb(next_bit++), {ids[0]}, {ids[2]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[2]}, {ids[0]}); + + // Connect the second last to the first one + graph.addTranslation(get_translation_cb(next_bit++), {ids[0]}, {ids[num_nodes-2]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-2]}, {ids[0]}); + + // Connect the second last to the last one + graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-1]}, {ids[num_nodes-2]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-2]}, {ids[num_nodes-1]}); + + unsigned iteration_idx = 1; + graph.iterateBFS(graph.findNode(ids[0]).value(), [&iteration_idx](const Graph::MessageNodePtr& node) { + assert(node->data().iterated_idx == 0); + node->data().iterated_idx = iteration_idx++; + }); + + EXPECT_EQ(graph.findNode(ids[0]).value()->data().iterated_idx, 1); + // We're a bit stricter than we would have to be: ids[1,2,4] would be allowed to have any of the values (2,3,4) + EXPECT_EQ(graph.findNode(ids[1]).value()->data().iterated_idx, 2); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().iterated_idx, 3); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().iterated_idx, 4); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().iterated_idx, 5); + + + // Translation + graph.translate(graph.findNode(ids[0]).value(), + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + + // All nodes should be translated except the first + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + for (unsigned i = 1; i < num_nodes; ++i) { + EXPECT_EQ(graph.findNode(ids[i]).value()->data().translated, true) << "node[" << i << "]"; + } + + // Ensure the correct edges were used for translations + EXPECT_EQ("00000000000000000000100101010001", translated.to_string()); + + // Ensure correct translation path taken for each node (which is stored in the buffers), + // and translation callback got called + EXPECT_EQ(*buffers[0], 0); + EXPECT_EQ(*buffers[1], 0b1); + EXPECT_EQ(*buffers[2], 0b1000000); + EXPECT_EQ(*buffers[3], 0b1010000); + EXPECT_EQ(*buffers[4], 0b100000000); + EXPECT_EQ(*buffers[5], 0b100100000000); + + for (unsigned i=0; i < num_nodes; ++i) { + printf("node[%i]: translated: %i, buffer: %i\n", i, graph.findNode(ids[i]).value()->data().translated, + *buffers[i]); + } +} + +TEST(graph, multi_links) { + // Multiple topics (merging / splitting) + struct NodeData { + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 6; + std::array ids{{ + {"topic1", 1}, + {"topic2", 1}, + {"topic1", 2}, + {"topic3", 1}, + {"topic4", 1}, + {"topic1", 3}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + + // Graph + // ___ + // 1 - | | --- + // | | - 3 - | | - 6 + // 2 - | | --- + // | --- + // | ___ + // --- | | - 4 + // | | - 5 + // --- + + // Translations + auto translation_cb_merge = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 1); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value1 | *a_value2; + }; + auto translation_cb_split = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 2); + auto a_value = static_cast(a[0].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value & 0x0000ffffu; + *b_value2 = *a_value & 0xffff0000u; + }; + auto translation_cb_direct = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 1); + auto a_value = static_cast(a[0].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value; + }; + + auto addTranslation = [&](const std::vector& inputs, const std::vector& outputs) { + assert(inputs.size() <= 2); + assert(outputs.size() <= 2); + if (inputs.size() == 1) { + if (outputs.size() == 1) { + graph.addTranslation(translation_cb_direct, inputs, outputs); + graph.addTranslation(translation_cb_direct, outputs, inputs); + } else { + graph.addTranslation(translation_cb_split, inputs, outputs); + graph.addTranslation(translation_cb_merge, outputs, inputs); + } + } else { + assert(outputs.size() == 1); + graph.addTranslation(translation_cb_merge, inputs, outputs); + graph.addTranslation(translation_cb_split, outputs, inputs); + } + }; + addTranslation({ids[0], ids[1]}, {ids[2]}); + addTranslation({ids[1]}, {ids[3], ids[4]}); + addTranslation({ids[2]}, {ids[5]}); + + auto translate_node = [&](const MessageIdentifier& id) { + graph.translate(graph.findNode(id).value(), + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + + }; + auto reset_translated = [&]() { + for (const auto& id : ids) { + graph.findNode(id).value()->data().translated = false; + } + }; + + // Updating node 2 should trigger an output for nodes 4+5 (splitting) + *buffers[0] = 0xa00000b0; + *buffers[1] = 0x0f00000f; + translate_node(ids[1]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + EXPECT_EQ(*buffers[3], 0x0000000f); + EXPECT_EQ(*buffers[4], 0x0f000000); + + reset_translated(); + + // Now updating node 1 should update nodes 3+6 (merging, both inputs available now) + translate_node(ids[0]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true); + EXPECT_EQ(*buffers[2], 0xaf0000bf); + EXPECT_EQ(*buffers[5], 0xaf0000bf); + + reset_translated(); + + // Another update must not trigger any other updates + translate_node(ids[0]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + + reset_translated(); + + // Backwards: updating node 6 should trigger updates for 1+2, but also 4+5 + *buffers[5] = 0xc00000d0; + translate_node(ids[5]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + EXPECT_EQ(*buffers[0], 0x000000d0); + EXPECT_EQ(*buffers[1], 0xc0000000); + EXPECT_EQ(*buffers[2], 0xc00000d0); + EXPECT_EQ(*buffers[3], 0); + EXPECT_EQ(*buffers[4], 0xc0000000); + EXPECT_EQ(*buffers[5], 0xc00000d0); +} + +TEST(graph, multi_links2) { + // Multiple topics (merging / splitting) + struct NodeData { + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 8; + std::array ids{{ + {"topic1", 1}, + {"topic2", 1}, + {"topic3", 1}, + {"topic1", 2}, + {"topic2", 2}, + {"topic1", 3}, + {"topic2", 3}, + {"topic3", 3}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + + // Graph + // ___ ___ + // 1 - | | | | - 6 + // | | - 4 - | | + // 2 - | | | | - 7 + // | | - 5 - | | + // 3 - | | | | - 8 + // --- --- + + // Translations + auto translation_cb_32 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 3); + assert(b.size() == 2); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto a_value3 = static_cast(a[2].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value1 | *a_value2; + *b_value2 = *a_value3; + }; + auto translation_cb_23 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 3); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + auto b_value3 = static_cast(b[2].get()); + *b_value1 = *a_value1 & 0x0000ffffu; + *b_value2 = *a_value1 & 0xffff0000u; + *b_value3 = *a_value2; + }; + graph.addTranslation(translation_cb_32, {ids[0], ids[1], ids[2]}, {ids[3], ids[4]}); + graph.addTranslation(translation_cb_23, {ids[3], ids[4]}, {ids[0], ids[1], ids[2]}); + + graph.addTranslation(translation_cb_23, {ids[3], ids[4]}, {ids[5], ids[6], ids[7]}); + graph.addTranslation(translation_cb_32, {ids[5], ids[6], ids[7]}, {ids[3], ids[4]}); + + + auto translate_node = [&](const MessageIdentifier& id) { + graph.translate(graph.findNode(id).value(), + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + }; + auto reset_translated = [&]() { + for (const auto& id : ids) { + graph.findNode(id).value()->data().translated = false; + } + }; + + // Updating nodes 1+2+3 should update nodes 6+7+8 + *buffers[0] = 0xa00000b0; + *buffers[1] = 0x0f00000f; + *buffers[2] = 0x0c00000c; + translate_node(ids[1]); + translate_node(ids[0]); + translate_node(ids[2]); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[7]).value()->data().translated, true); + EXPECT_EQ(*buffers[3], 0xa00000b0 | 0x0f00000f); + EXPECT_EQ(*buffers[4], 0x0c00000c); + EXPECT_EQ(*buffers[5], (0xa00000b0 | 0x0f00000f) & 0x0000ffffu); + EXPECT_EQ(*buffers[6], (0xa00000b0 | 0x0f00000f) & 0xffff0000u); + EXPECT_EQ(*buffers[7], 0x0c00000c); + + reset_translated(); + + // Now updating nodes 6+7+8 should update nodes 1+2+3 + *buffers[5] = 0xa00000b0; + *buffers[6] = 0x0f00000f; + *buffers[7] = 0x0c00000c; + translate_node(ids[5]); + translate_node(ids[6]); + translate_node(ids[7]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(*buffers[3], 0xa00000b0 | 0x0f00000f); + EXPECT_EQ(*buffers[4], 0x0c00000c); + EXPECT_EQ(*buffers[0], (0xa00000b0 | 0x0f00000f) & 0x0000ffffu); + EXPECT_EQ(*buffers[1], (0xa00000b0 | 0x0f00000f) & 0xffff0000u); + EXPECT_EQ(*buffers[2], 0x0c00000c); +} + +TEST(graph, multi_links3) { + // Multiple topics (cannot use the shortest path) + struct NodeData { + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 7; + std::array ids{{ + {"topic1", 1}, + {"topic2", 1}, + {"topic1", 2}, + {"topic1", 3}, + {"topic1", 4}, + {"topic2", 4}, + {"topic1", 5}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + + // Graph + // ___ ___ ___ ___ + // 1 - | | - 3 - | | - 4 - | | - 5 - | | - 7 + // | | --- --- | | + // | | | | + // 2 - | | --------------------- 6 - | | + // --- --- + + // Translations + auto translation_cb_21 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 1); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value1 = static_cast(b[0].get()); + *b_value1 = *a_value1 | *a_value2; + }; + auto translation_cb_22 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 2); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value1; + *b_value2 = *a_value2; + }; + auto translation_cb_12 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 2); + auto a_value1 = static_cast(a[0].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value1 & 0x0000ffffu; + *b_value2 = *a_value1 & 0xffff0000u; + }; + auto translation_cb_11 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 1); + auto a_value1 = static_cast(a[0].get()); + auto b_value1 = static_cast(b[0].get()); + *b_value1 = *a_value1 + 1; + }; + graph.addTranslation(translation_cb_22, {ids[0], ids[1]}, {ids[2], ids[5]}); + graph.addTranslation(translation_cb_22, {ids[2], ids[5]}, {ids[0], ids[1]}); + graph.addTranslation(translation_cb_11, {ids[2]}, {ids[3]}); + graph.addTranslation(translation_cb_11, {ids[3]}, {ids[2]}); + graph.addTranslation(translation_cb_11, {ids[3]}, {ids[4]}); + graph.addTranslation(translation_cb_11, {ids[4]}, {ids[3]}); + graph.addTranslation(translation_cb_21, {ids[4], ids[5]}, {ids[6]}); + graph.addTranslation(translation_cb_12, {ids[6]}, {ids[4], ids[5]}); + + + auto translate_node = [&](const MessageIdentifier& id) { + graph.translate(graph.findNode(id).value(), + [](auto&& node) { + assert(!node->data().translated); + assert(!node->data().translated); + node->data().translated = true; + }); + }; + auto reset_translated = [&]() { + for (const auto& id : ids) { + graph.findNode(id).value()->data().translated = false; + } + }; + + // Updating nodes 1+2 should update node 7 + *buffers[0] = 0xa00000b0; + *buffers[1] = 0x0a00000b; + translate_node(ids[1]); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, false); + translate_node(ids[0]); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true); + EXPECT_EQ(*buffers[2], 0xa00000b0); + EXPECT_EQ(*buffers[3], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[4], 0xa00000b0 + 2); + EXPECT_EQ(*buffers[5], 0x0a00000b); + EXPECT_EQ(*buffers[6], ((0xa00000b0 + 2) | 0x0a00000b)); + + reset_translated(); + + // Now updating nodes 4+6 should update the rest + *buffers[3] = 0xa00000b0; + *buffers[5] = 0x0f00000f; + translate_node(ids[3]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, false); + translate_node(ids[5]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true); + EXPECT_EQ(*buffers[0], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[1], 0x0f00000f); + EXPECT_EQ(*buffers[2], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[4], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[6], (0xa00000b0 + 1) | 0x0f00000f); +} diff --git a/msg/translation_node/test/main.cpp b/msg/translation_node/test/main.cpp new file mode 100644 index 0000000000..9d7e484c70 --- /dev/null +++ b/msg/translation_node/test/main.cpp @@ -0,0 +1,16 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + const int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/msg/translation_node/test/pub_sub.cpp b/msg/translation_node/test/pub_sub.cpp new file mode 100644 index 0000000000..3ccd02a9b8 --- /dev/null +++ b/msg/translation_node/test/pub_sub.cpp @@ -0,0 +1,350 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include +#include +#include +using namespace std::chrono_literals; + +// Define a custom struct with MESSAGE_VERSION field that can be used in ROS pubs and subs +#define DEFINE_VERSIONED_ROS_MESSAGE_TYPE(CUSTOM_TYPE_NAME, ROS_TYPE_NAME, THIS_MESSAGE_VERSION) \ + struct CUSTOM_TYPE_NAME : public ROS_TYPE_NAME { \ + CUSTOM_TYPE_NAME() = default; \ + CUSTOM_TYPE_NAME(const ROS_TYPE_NAME& msg) : ROS_TYPE_NAME(msg) {} \ + static constexpr uint32_t MESSAGE_VERSION = THIS_MESSAGE_VERSION; \ + }; \ + template<> \ + struct rclcpp::TypeAdapter \ + { \ + using is_specialized = std::true_type; \ + using custom_type = CUSTOM_TYPE_NAME; \ + using ros_message_type = ROS_TYPE_NAME; \ + static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) \ + { \ + destination = source; \ + } \ + static void convert_to_custom(const ros_message_type & source, custom_type & destination) \ + { \ + destination = source; \ + } \ + }; \ + RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CUSTOM_TYPE_NAME, ROS_TYPE_NAME); + +class PubSubGraphTest : public testing::Test +{ +protected: + void SetUp() override + { + _test_node = std::make_shared("test_node"); + _app_node = std::make_shared("app_node"); + _executor.add_node(_test_node); + _executor.add_node(_app_node); + + for (auto& node : {_app_node, _test_node}) { + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + } + + bool spinWithTimeout(const std::function& predicate) { + const auto start = _app_node->now(); + while (_app_node->now() - start < 5s) { + _executor.spin_some(); + if (predicate()) { + return true; + } + } + return false; + } + + std::shared_ptr _test_node; + std::shared_ptr _app_node; + rclcpp::executors::SingleThreadedExecutor _executor; +}; + +class RegisteredTranslationsTest : public RegisteredTranslations { +public: + RegisteredTranslationsTest() = default; +}; + + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(Float32Versioned, std_msgs::msg::Float32, 1u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(ColorRGBAVersioned, std_msgs::msg::ColorRGBA, 2u); + +class DirectTranslationTest { +public: + using MessageOlder = Float32Versioned; + using MessageNewer = ColorRGBAVersioned; + + static constexpr const char* kTopic = "test/direct_translation"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.r = 1.f; + msg_newer.g = msg_older.data; + msg_newer.b = 2.f; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.data = msg_newer.r + msg_newer.g + msg_newer.b; + } +}; + + +TEST_F(PubSubGraphTest, DirectTranslation) +{ + RegisteredTranslationsTest registered_translations; + registered_translations.registerDirectTranslation(); + + PubSubGraph graph(*_test_node, registered_translations.topicTranslations()); + Monitor monitor(*_test_node, &graph, nullptr); + + const std::string topic_name = DirectTranslationTest::kTopic; + const std::string topic_name_older_version = getVersionedTopicName(topic_name, DirectTranslationTest::MessageOlder::MESSAGE_VERSION); + const std::string topic_name_newer_version = getVersionedTopicName(topic_name, DirectTranslationTest::MessageNewer::MESSAGE_VERSION); + + { + // Create publisher + subscriber + int num_topic_updates = 0; + DirectTranslationTest::MessageNewer latest_data{}; + auto publisher = _app_node->create_publisher(topic_name_older_version, + rclcpp::QoS(1).best_effort()); + auto subscriber = _app_node->create_subscription(topic_name_newer_version, + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data, this]( + DirectTranslationTest::MessageNewer::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated: %.3f", (double) msg->g); + latest_data = *msg; + ++num_topic_updates; + }); + + monitor.updateNow(); + + // Wait until there is a subscriber & publisher + ASSERT_TRUE(spinWithTimeout([&subscriber, &publisher]() { + return subscriber->get_publisher_count() > 0 && publisher->get_subscription_count() > 0; + })) << "Timeout, no publisher/subscriber found"; + + // Publish some data & wait for it to arrive + for (int i = 0; i < 10; ++i) { + DirectTranslationTest::MessageOlder msg_older; + msg_older.data = (float) i; + publisher->publish(msg_older); + + ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() { + return num_topic_updates == i + 1; + })) << "Timeout, topic update not received, i=" << i; + + // Check data + EXPECT_FLOAT_EQ(latest_data.r, 1.f); + EXPECT_FLOAT_EQ(latest_data.g, (float) i); + EXPECT_FLOAT_EQ(latest_data.b, 2.f); + } + } + + // Now check the translation into the other direction + { + int num_topic_updates = 0; + DirectTranslationTest::MessageOlder latest_data{}; + auto publisher = _app_node->create_publisher(topic_name_newer_version, + rclcpp::QoS(1).best_effort()); + auto subscriber = _app_node->create_subscription(topic_name_older_version, + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data, this]( + DirectTranslationTest::MessageOlder::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated: %.3f", (double) msg->data); + latest_data = *msg; + ++num_topic_updates; + }); + + monitor.updateNow(); + + // Wait until there is a subscriber & publisher + ASSERT_TRUE(spinWithTimeout([&subscriber, &publisher]() { + return subscriber->get_publisher_count() > 0 && publisher->get_subscription_count() > 0; + })) << "Timeout, no publisher/subscriber found"; + + // Publish some data & wait for it to arrive + for (int i = 0; i < 10; ++i) { + DirectTranslationTest::MessageNewer msg_newer; + msg_newer.r = (float)i; + msg_newer.g = (float)i * 10.f; + msg_newer.b = (float)i * 100.f; + publisher->publish(msg_newer); + + ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() { + return num_topic_updates == i + 1; + })) << "Timeout, topic update not received, i=" << i; + + // Check data + EXPECT_FLOAT_EQ(latest_data.data, 111.f * (float)i); + } + } +} + + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV1, std_msgs::msg::Float32, 1u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV1, std_msgs::msg::Float64, 1u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeCV1, std_msgs::msg::Int64, 1u); + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV2, std_msgs::msg::ColorRGBA, 2u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV2, std_msgs::msg::Int64, 2u); + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV3, std_msgs::msg::Float64, 3u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV3, std_msgs::msg::Int64, 3u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeCV3, std_msgs::msg::Float32, 3u); + +class TranslationMultiTestV2 { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + "test/multi_translation_topic_c", + }; + static_assert(MessageTypeAV1::MESSAGE_VERSION == 1); + static_assert(MessageTypeBV1::MESSAGE_VERSION == 1); + static_assert(MessageTypeCV1::MESSAGE_VERSION == 1); + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + }; + static_assert(MessageTypeAV2::MESSAGE_VERSION == 2); + static_assert(MessageTypeBV2::MESSAGE_VERSION == 2); + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + const MessagesOlder::Type3 &msg_older3, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2) { + msg_newer1.r = msg_older1.data; + msg_newer1.g = (float)msg_older2.data; + msg_newer1.b = (float)msg_older3.data; + msg_newer2.data = msg_older3.data * 10; + } + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2, MessagesOlder::Type3 &msg_older3) { + msg_older1.data = msg_newer1.r; + msg_older2.data = msg_newer1.g; + msg_older3.data = msg_newer2.data / 10; + } +}; + +class TranslationMultiTestV3 { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + }; + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + "test/multi_translation_topic_c", + }; + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2, MessagesNewer::Type3 &msg_newer3) { + msg_newer1.data = msg_older1.r; + msg_newer2.data = (int64_t)msg_older1.g; + msg_newer3.data = (float)msg_older2.data + 100; + } + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, const MessagesNewer::Type3 &msg_newer3, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2) { + msg_older1.r = (float)msg_newer1.data; + msg_older1.g = (float)msg_newer2.data; + msg_older2.data = (int64_t)msg_newer3.data - 100; + } +}; + +TEST_F(PubSubGraphTest, TranslationMulti) { + RegisteredTranslationsTest registered_translations; + // Register 3 different message versions, with 3 types -> 2 types -> 3 types + registered_translations.registerTranslation(); + registered_translations.registerTranslation(); + + PubSubGraph graph(*_test_node, registered_translations.topicTranslations()); + Monitor monitor(*_test_node, &graph, nullptr); + + const std::string topic_name_a = TranslationMultiTestV2::kTopicsOlder[0]; + const std::string topic_name_b = TranslationMultiTestV2::kTopicsOlder[1]; + const std::string topic_name_c = TranslationMultiTestV2::kTopicsOlder[2]; + + // Create publishers for version 1 + subscribers for version 3 + int num_topic_updates = 0; + MessageTypeAV3 latest_data_a{}; + MessageTypeBV3 latest_data_b{}; + MessageTypeCV3 latest_data_c{}; + auto publisher_a = _app_node->create_publisher(getVersionedTopicName(topic_name_a, MessageTypeAV1::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort()); + auto publisher_b = _app_node->create_publisher(getVersionedTopicName(topic_name_b, MessageTypeBV1::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort()); + auto publisher_c = _app_node->create_publisher(getVersionedTopicName(topic_name_c, MessageTypeCV1::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort()); + auto subscriber_a = _app_node->create_subscription(getVersionedTopicName(topic_name_a, MessageTypeAV3::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_a, this]( + MessageTypeAV3::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (A): %.3f", (double) msg->data); + latest_data_a = *msg; + ++num_topic_updates; + }); + auto subscriber_b = _app_node->create_subscription(getVersionedTopicName(topic_name_b, MessageTypeBV3::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_b, this]( + MessageTypeBV3::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (B): %.3f", (double) msg->data); + latest_data_b = *msg; + ++num_topic_updates; + }); + auto subscriber_c = _app_node->create_subscription(getVersionedTopicName(topic_name_c, MessageTypeCV3::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_c, this]( + MessageTypeCV3::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (C): %.3f", (double) msg->data); + latest_data_c = *msg; + ++num_topic_updates; + }); + + monitor.updateNow(); + + // Wait until there is a subscriber & publisher + ASSERT_TRUE(spinWithTimeout([&]() { + return subscriber_a->get_publisher_count() > 0 && subscriber_b->get_publisher_count() > 0 && subscriber_c->get_publisher_count() > 0 && + publisher_a->get_subscription_count() > 0 && publisher_b->get_subscription_count() > 0 && publisher_c->get_subscription_count() > 0; + })) << "Timeout, no publisher/subscriber found"; + + // Publish some data & wait for it to arrive + for (int i = 0; i < 10; ++i) { + MessageTypeAV1 msg_older_a; + msg_older_a.data = (float) i; + publisher_a->publish(msg_older_a); + + MessageTypeBV1 msg_older_b; + msg_older_b.data = (float) i * 10.f; + publisher_b->publish(msg_older_b); + + MessageTypeCV1 msg_older_c; + msg_older_c.data = i * 100; + publisher_c->publish(msg_older_c); + + ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() { + return num_topic_updates == (i + 1) * 3; + })) << "Timeout, topic update not received, i=" << i << ", num updates=" << num_topic_updates; + + // Check data + EXPECT_FLOAT_EQ(latest_data_a.data, (float)i); + EXPECT_FLOAT_EQ(latest_data_b.data, (float)i * 10.f); + EXPECT_FLOAT_EQ(latest_data_c.data, ((float)i * 100.f) * 10.f + 100.f); + } +} diff --git a/msg/translation_node/test/services.cpp b/msg/translation_node/test/services.cpp new file mode 100644 index 0000000000..f98569b55d --- /dev/null +++ b/msg/translation_node/test/services.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include +#include + +using namespace std::chrono_literals; + + +class ServiceTest : public testing::Test +{ +protected: + void SetUp() override + { + _test_node = std::make_shared("test_node"); + _app_node = std::make_shared("app_node"); + _executor.add_node(_test_node); + _executor.add_node(_app_node); + + for (auto& node : {_app_node, _test_node}) { + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + } + + bool spinWithTimeout(const std::function& predicate) { + const auto start = _app_node->now(); + while (_app_node->now() - start < 5s) { + _executor.spin_some(); + if (predicate()) { + return true; + } + } + return false; + } + + std::shared_ptr _test_node; + std::shared_ptr _app_node; + rclcpp::executors::SingleThreadedExecutor _executor; +}; + +class RegisteredTranslationsTest : public RegisteredTranslations { +public: + RegisteredTranslationsTest() = default; +}; + + +class ServiceTestV0V1 { +public: + using MessageOlder = translation_node::srv::TestV0; + using MessageNewer = translation_node::srv::TestV1; + + static constexpr const char* kTopic = "test/service"; + + static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + msg_newer.request_a = msg_older.request_a; + } + + static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + msg_older.request_a = msg_newer.request_a; + } + + static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + msg_newer.response_a = msg_older.response_a; + } + + static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + msg_older.response_a = msg_newer.response_a; + } +}; + +class ServiceTestV1V2 { +public: + using MessageOlder = translation_node::srv::TestV1; + using MessageNewer = translation_node::srv::TestV2; + + static constexpr const char* kTopic = "test/service"; + + static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + msg_newer.request_a = msg_older.request_a; + msg_newer.request_b = 1234; + } + + static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + msg_older.request_a = msg_newer.request_a + msg_newer.request_b; + } + + static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + msg_newer.response_a = msg_older.response_a; + msg_newer.response_b = 32; + } + + static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + msg_older.response_a = msg_newer.response_a + msg_newer.response_b; + } +}; + + +TEST_F(ServiceTest, Test) +{ + RegisteredTranslationsTest registered_translations; + registered_translations.registerServiceDirectTranslation(); + registered_translations.registerServiceDirectTranslation(); + + ServiceGraph graph(*_test_node, registered_translations.serviceTranslations()); + Monitor monitor(*_test_node, nullptr, &graph); + + const std::string topic_name = ServiceTestV1V2::kTopic; + const std::string topic_name_v0 = getVersionedTopicName(topic_name, ServiceTestV0V1::MessageOlder::Request::MESSAGE_VERSION); + const std::string topic_name_v1 = getVersionedTopicName(topic_name, ServiceTestV0V1::MessageNewer::Request::MESSAGE_VERSION); + const std::string topic_name_v2 = getVersionedTopicName(topic_name, ServiceTestV1V2::MessageNewer::Request::MESSAGE_VERSION); + + + // Create service + clients + int num_service_requests = 0; + auto service = _app_node->create_service(topic_name_v0, [&num_service_requests]( + const ServiceTestV0V1::MessageOlder::Request::SharedPtr request, ServiceTestV0V1::MessageOlder::Response::SharedPtr response) { + response->response_a = request->request_a + 1; + ++num_service_requests; + }); + auto client0 = _app_node->create_client(topic_name_v0); + auto client1 = _app_node->create_client(topic_name_v1); + auto client2 = _app_node->create_client(topic_name_v2); + + monitor.updateNow(); + + // Wait until there is a service for each client + ASSERT_TRUE(spinWithTimeout([&client0, &client1, &client2]() { + return client0->service_is_ready() && client1->service_is_ready() && client2->service_is_ready(); + })) << "Timeout, no service for clients found: " << client0->service_is_ready() << client1->service_is_ready() << client2->service_is_ready(); + + + + // Make some requests + int expected_num_service_requests = 1; + + // Client 1 + for (int i = 0; i < 10; ++i) { + auto request = std::make_shared(); + ServiceTestV0V1::MessageNewer::Response response; + request->request_a = i; + bool got_response = false; + client1->async_send_request(request, [&got_response, &response](rclcpp::Client::SharedFuture result) { + got_response = true; + response = *result.get(); + }); + + ASSERT_TRUE(spinWithTimeout([&got_response]() { + return got_response; + })) << "Timeout, reply not received, i=" << i; + + // Check data + EXPECT_EQ(response.response_a, i + 1); + EXPECT_EQ(num_service_requests, expected_num_service_requests); + ++expected_num_service_requests; + } + + // Client 0 + for (int i = 0; i < 10; ++i) { + auto request = std::make_shared(); + ServiceTestV0V1::MessageOlder::Response response; + request->request_a = i * 10; + bool got_response = false; + client0->async_send_request(request, [&got_response, &response](rclcpp::Client::SharedFuture result) { + got_response = true; + response = *result.get(); + }); + + ASSERT_TRUE(spinWithTimeout([&got_response]() { + return got_response; + })) << "Timeout, reply not received, i=" << i; + + // Check data + EXPECT_EQ(response.response_a, i * 10 + 1); + EXPECT_EQ(num_service_requests, expected_num_service_requests); + ++expected_num_service_requests; + } + + // Client 2 + for (int i = 0; i < 10; ++i) { + auto request = std::make_shared(); + ServiceTestV1V2::MessageNewer::Response response; + request->request_a = i * 10; + request->request_b = i; + bool got_response = false; + client2->async_send_request(request, [&got_response, &response](rclcpp::Client::SharedFuture result) { + got_response = true; + response = *result.get(); + }); + + ASSERT_TRUE(spinWithTimeout([&got_response]() { + return got_response; + })) << "Timeout, reply not received, i=" << i; + + // Check data + EXPECT_EQ(response.response_a, i + i * 10 + 1); + EXPECT_EQ(response.response_b, 32); + EXPECT_EQ(num_service_requests, expected_num_service_requests); + ++expected_num_service_requests; + } +} diff --git a/msg/translation_node/test/srv/TestV0.srv b/msg/translation_node/test/srv/TestV0.srv new file mode 100644 index 0000000000..4b06c4af35 --- /dev/null +++ b/msg/translation_node/test/srv/TestV0.srv @@ -0,0 +1,4 @@ +uint32 MESSAGE_VERSION = 0 +uint8 request_a +--- +uint64 response_a diff --git a/msg/translation_node/test/srv/TestV1.srv b/msg/translation_node/test/srv/TestV1.srv new file mode 100644 index 0000000000..d5efaac707 --- /dev/null +++ b/msg/translation_node/test/srv/TestV1.srv @@ -0,0 +1,4 @@ +uint32 MESSAGE_VERSION = 1 +uint64 request_a +--- +uint8 response_a diff --git a/msg/translation_node/test/srv/TestV2.srv b/msg/translation_node/test/srv/TestV2.srv new file mode 100644 index 0000000000..da8b79325f --- /dev/null +++ b/msg/translation_node/test/srv/TestV2.srv @@ -0,0 +1,6 @@ +uint32 MESSAGE_VERSION = 2 +uint8 request_a +uint64 request_b +--- +uint16 response_a +uint64 response_b diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h new file mode 100644 index 0000000000..4369a3c58a --- /dev/null +++ b/msg/translation_node/translations/all_translations.h @@ -0,0 +1,13 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include + +//#include "example_translation_direct_v1.h" +//#include "example_translation_multi_v2.h" +//#include "example_translation_service_v1.h" + +#include "translation_vehicle_status_v1.h" diff --git a/msg/translation_node/translations/example_translation_direct_v1.h b/msg/translation_node/translations/example_translation_direct_v1.h new file mode 100644 index 0000000000..b2c51aa628 --- /dev/null +++ b/msg/translation_node/translations/example_translation_direct_v1.h @@ -0,0 +1,30 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ExampleTopic v0 <--> v1 +#include +#include + +class ExampleTopicV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::ExampleTopicV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::ExampleTopic; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/example_topic"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(ExampleTopicV1Translation); diff --git a/msg/translation_node/translations/example_translation_multi_v2.h b/msg/translation_node/translations/example_translation_multi_v2.h new file mode 100644 index 0000000000..9c55deace6 --- /dev/null +++ b/msg/translation_node/translations/example_translation_multi_v2.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ExampleTopic and OtherTopic v1 <--> v2 +#include +#include +#include +#include + +class ExampleTopicOtherTopicV2Translation { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "fmu/out/example_topic", + "fmu/out/other_topic", + }; + static_assert(px4_msgs_old::msg::ExampleTopicV1::MESSAGE_VERSION == 1); + static_assert(px4_msgs_old::msg::OtherTopicV1::MESSAGE_VERSION == 1); + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "fmu/out/example_topic", + "fmu/out/other_topic", + }; + static_assert(px4_msgs::msg::ExampleTopic::MESSAGE_VERSION == 2); + static_assert(px4_msgs::msg::OtherTopic::MESSAGE_VERSION == 2); + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2) { + // Set msg_newer1, msg_newer2 from msg_older1, msg_older2 + } + + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2) { + // Set msg_older1, msg_older2 from msg_newer1, msg_newer2 + } +}; + +REGISTER_TOPIC_TRANSLATION(ExampleTopicOtherTopicV2Translation); diff --git a/msg/translation_node/translations/example_translation_service_v1.h b/msg/translation_node/translations/example_translation_service_v1.h new file mode 100644 index 0000000000..e98599ab01 --- /dev/null +++ b/msg/translation_node/translations/example_translation_service_v1.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ExampleService v0 <--> v1 +#include +#include + +class ExampleServiceV1Translation { +public: + using MessageOlder = px4_msgs_old::srv::ExampleServiceV0; + static_assert(MessageOlder::Request::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::srv::ExampleService; + static_assert(MessageNewer::Request::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/example_service"; + + static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + // Request: set msg_newer from msg_older + } + + static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + // Request: set msg_older from msg_newer + } + + static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + // Response: set msg_newer from msg_older + } + + static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + // Response: set msg_older from msg_newer + } +}; + +REGISTER_SERVICE_TRANSLATION_DIRECT(ExampleServiceV1Translation); diff --git a/msg/translation_node/translations/translation_vehicle_status_v1.h b/msg/translation_node/translations/translation_vehicle_status_v1.h new file mode 100644 index 0000000000..2041b013d8 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v1.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v0 <--> v1 +#include +#include + +class VehicleStatusV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.armed_time = msg_older.armed_time; + msg_newer.takeoff_time = msg_older.takeoff_time; + msg_newer.arming_state = msg_older.arming_state; + msg_newer.latest_arming_reason = msg_older.latest_arming_reason; + msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason; + msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp; + msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention; + msg_newer.nav_state = msg_older.nav_state; + msg_newer.executor_in_charge = msg_older.executor_in_charge; + msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask; + msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask; + msg_newer.failure_detector_status = msg_older.failure_detector_status; + msg_newer.hil_state = msg_older.hil_state; + msg_newer.vehicle_type = msg_older.vehicle_type; + msg_newer.failsafe = msg_older.failsafe; + msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over; + msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state; + msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost; + msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter; + msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost; + msg_newer.is_vtol = msg_older.is_vtol; + msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter; + msg_newer.in_transition_mode = msg_older.in_transition_mode; + msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw; + msg_newer.system_type = msg_older.system_type; + msg_newer.system_id = msg_older.system_id; + msg_newer.component_id = msg_older.component_id; + msg_newer.safety_button_available = msg_older.safety_button_available; + msg_newer.safety_off = msg_older.safety_off; + msg_newer.power_input_valid = msg_older.power_input_valid; + msg_newer.usb_connected = msg_older.usb_connected; + msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present; + msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; + msg_newer.parachute_system_present = msg_older.parachute_system_present; + msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; + msg_newer.calibration_enabled = msg_older.calibration_enabled; + msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.armed_time = msg_newer.armed_time; + msg_older.takeoff_time = msg_newer.takeoff_time; + msg_older.arming_state = msg_newer.arming_state; + msg_older.latest_arming_reason = msg_newer.latest_arming_reason; + msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason; + msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp; + msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; + msg_older.nav_state = msg_newer.nav_state; + msg_older.executor_in_charge = msg_newer.executor_in_charge; + msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; + msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; + msg_older.failure_detector_status = msg_newer.failure_detector_status; + msg_older.hil_state = msg_newer.hil_state; + msg_older.vehicle_type = msg_newer.vehicle_type; + msg_older.failsafe = msg_newer.failsafe; + msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over; + msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state; + msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost; + msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter; + msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost; + msg_older.is_vtol = msg_newer.is_vtol; + msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter; + msg_older.in_transition_mode = msg_newer.in_transition_mode; + msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw; + msg_older.system_type = msg_newer.system_type; + msg_older.system_id = msg_newer.system_id; + msg_older.component_id = msg_newer.component_id; + msg_older.safety_button_available = msg_newer.safety_button_available; + msg_older.safety_off = msg_newer.safety_off; + msg_older.power_input_valid = msg_newer.power_input_valid; + msg_older.usb_connected = msg_newer.usb_connected; + msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present; + msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; + msg_older.parachute_system_present = msg_newer.parachute_system_present; + msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + msg_older.avoidance_system_required = false; + msg_older.avoidance_system_valid = false; + msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; + msg_older.calibration_enabled = msg_newer.calibration_enabled; + msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV1Translation); diff --git a/msg/ActuatorMotors.msg b/msg/versioned/ActuatorMotors.msg similarity index 95% rename from msg/ActuatorMotors.msg rename to msg/versioned/ActuatorMotors.msg index e74566f1f7..da63a388bb 100644 --- a/msg/ActuatorMotors.msg +++ b/msg/versioned/ActuatorMotors.msg @@ -1,4 +1,7 @@ # Motor control message + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp the data this control response is based on was sampled diff --git a/msg/ActuatorServos.msg b/msg/versioned/ActuatorServos.msg similarity index 92% rename from msg/ActuatorServos.msg rename to msg/versioned/ActuatorServos.msg index 2c7900e811..e740f39c8c 100644 --- a/msg/ActuatorServos.msg +++ b/msg/versioned/ActuatorServos.msg @@ -1,4 +1,7 @@ # Servo control message + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp the data this control response is based on was sampled diff --git a/msg/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg similarity index 94% rename from msg/ArmingCheckReply.msg rename to msg/versioned/ArmingCheckReply.msg index 589ad1b1cd..4c1616d849 100644 --- a/msg/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -1,10 +1,11 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint8 request_id uint8 registration_id uint8 HEALTH_COMPONENT_INDEX_NONE = 0 -uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 uint8 health_component_index # HEALTH_COMPONENT_INDEX_* bool health_component_is_present diff --git a/msg/ArmingCheckRequest.msg b/msg/versioned/ArmingCheckRequest.msg similarity index 84% rename from msg/ArmingCheckRequest.msg rename to msg/versioned/ArmingCheckRequest.msg index 69e7e85f35..17c6e4bacd 100644 --- a/msg/ArmingCheckRequest.msg +++ b/msg/versioned/ArmingCheckRequest.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # broadcast message to request all registered arming checks to be reported diff --git a/msg/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg similarity index 99% rename from msg/BatteryStatus.msg rename to msg/versioned/BatteryStatus.msg index e865161615..9838786f3f 100644 --- a/msg/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool connected # Whether or not a battery is connected, based on a voltage threshold float32 voltage_v # Battery voltage in volts, 0 if unknown diff --git a/msg/ConfigOverrides.msg b/msg/versioned/ConfigOverrides.msg similarity index 95% rename from msg/ConfigOverrides.msg rename to msg/versioned/ConfigOverrides.msg index 09b87253a8..b274adfbec 100644 --- a/msg/ConfigOverrides.msg +++ b/msg/versioned/ConfigOverrides.msg @@ -1,4 +1,7 @@ # Configurable overrides by (external) modes or mode executors + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured) diff --git a/msg/GotoSetpoint.msg b/msg/versioned/GotoSetpoint.msg similarity index 97% rename from msg/GotoSetpoint.msg rename to msg/versioned/GotoSetpoint.msg index 5fe3ab8a79..4871a3cb7d 100644 --- a/msg/GotoSetpoint.msg +++ b/msg/versioned/GotoSetpoint.msg @@ -5,6 +5,8 @@ # Unset optional setpoints are not controlled # Unset optional constraints default to vehicle specifications +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # setpoints diff --git a/msg/HomePosition.msg b/msg/versioned/HomePosition.msg similarity index 96% rename from msg/HomePosition.msg rename to msg/versioned/HomePosition.msg index e6a517285f..a25bf24163 100644 --- a/msg/HomePosition.msg +++ b/msg/versioned/HomePosition.msg @@ -1,5 +1,7 @@ # GPS home position in WGS84 coordinates. +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) float64 lat # Latitude in degrees diff --git a/msg/ManualControlSetpoint.msg b/msg/versioned/ManualControlSetpoint.msg similarity index 98% rename from msg/ManualControlSetpoint.msg rename to msg/versioned/ManualControlSetpoint.msg index 95fa622283..ddf902f00f 100644 --- a/msg/ManualControlSetpoint.msg +++ b/msg/versioned/ManualControlSetpoint.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/msg/ModeCompleted.msg b/msg/versioned/ModeCompleted.msg similarity index 94% rename from msg/ModeCompleted.msg rename to msg/versioned/ModeCompleted.msg index 0a20b0294e..1022df36d3 100644 --- a/msg/ModeCompleted.msg +++ b/msg/versioned/ModeCompleted.msg @@ -2,6 +2,9 @@ # The possible values of nav_state are defined in the VehicleStatus msg. # Note that this is not always published (e.g. when a user switches modes or on # failsafe activation) + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) diff --git a/msg/RegisterExtComponentReply.msg b/msg/versioned/RegisterExtComponentReply.msg similarity index 93% rename from msg/RegisterExtComponentReply.msg rename to msg/versioned/RegisterExtComponentReply.msg index 7cd7eef07b..4495bcdb90 100644 --- a/msg/RegisterExtComponentReply.msg +++ b/msg/versioned/RegisterExtComponentReply.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 request_id # ID from the request diff --git a/msg/RegisterExtComponentRequest.msg b/msg/versioned/RegisterExtComponentRequest.msg similarity index 97% rename from msg/RegisterExtComponentRequest.msg rename to msg/versioned/RegisterExtComponentRequest.msg index 46ab0cb0a1..b0798705ca 100644 --- a/msg/RegisterExtComponentRequest.msg +++ b/msg/versioned/RegisterExtComponentRequest.msg @@ -1,4 +1,7 @@ # Request to register an external component + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 request_id # ID, set this to a random value diff --git a/msg/TrajectorySetpoint.msg b/msg/versioned/TrajectorySetpoint.msg similarity index 95% rename from msg/TrajectorySetpoint.msg rename to msg/versioned/TrajectorySetpoint.msg index 4a88c86769..150be404b9 100644 --- a/msg/TrajectorySetpoint.msg +++ b/msg/versioned/TrajectorySetpoint.msg @@ -3,6 +3,8 @@ # Needs to be kinematically consistent and feasible for smooth flight. # setting a value to NaN means the state should not be controlled +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # NED local world frame diff --git a/msg/UnregisterExtComponent.msg b/msg/versioned/UnregisterExtComponent.msg similarity index 92% rename from msg/UnregisterExtComponent.msg rename to msg/versioned/UnregisterExtComponent.msg index 2ad78d4b68..8f85aaebfe 100644 --- a/msg/UnregisterExtComponent.msg +++ b/msg/versioned/UnregisterExtComponent.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) char[25] name # either the mode name, or component name diff --git a/msg/VehicleAngularVelocity.msg b/msg/versioned/VehicleAngularVelocity.msg similarity index 94% rename from msg/VehicleAngularVelocity.msg rename to msg/versioned/VehicleAngularVelocity.msg index db3767c0aa..6c91f3c070 100644 --- a/msg/VehicleAngularVelocity.msg +++ b/msg/versioned/VehicleAngularVelocity.msg @@ -1,3 +1,4 @@ +uint32 MESSAGE_VERSION = 0 uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) diff --git a/msg/VehicleAttitude.msg b/msg/versioned/VehicleAttitude.msg similarity index 96% rename from msg/VehicleAttitude.msg rename to msg/versioned/VehicleAttitude.msg index 99e6f25c2e..fde3a85546 100644 --- a/msg/VehicleAttitude.msg +++ b/msg/versioned/VehicleAttitude.msg @@ -1,6 +1,8 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use # The quaternion uses the Hamilton convention, and the order is q(w, x, y, z) +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/versioned/VehicleAttitudeSetpoint.msg similarity index 96% rename from msg/VehicleAttitudeSetpoint.msg rename to msg/versioned/VehicleAttitudeSetpoint.msg index 74a753023d..28aa780699 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/versioned/VehicleAttitudeSetpoint.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) float32 yaw_sp_move_rate # rad/s (commanded by user) diff --git a/msg/VehicleCommand.msg b/msg/versioned/VehicleCommand.msg similarity index 99% rename from msg/VehicleCommand.msg rename to msg/versioned/VehicleCommand.msg index 2970410a88..96162e9f89 100644 --- a/msg/VehicleCommand.msg +++ b/msg/versioned/VehicleCommand.msg @@ -1,6 +1,8 @@ # Vehicle Command uORB message. Used for commanding a mission / action / etc. # Follows the MAVLink COMMAND_INT / COMMAND_LONG definition +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint16 VEHICLE_CMD_CUSTOM_0 = 0 # test command diff --git a/msg/VehicleCommandAck.msg b/msg/versioned/VehicleCommandAck.msg similarity index 98% rename from msg/VehicleCommandAck.msg rename to msg/versioned/VehicleCommandAck.msg index 6f54fa4631..c11a5f8c5d 100644 --- a/msg/VehicleCommandAck.msg +++ b/msg/versioned/VehicleCommandAck.msg @@ -2,6 +2,8 @@ # Used for acknowledging the vehicle command being received. # Follows the MAVLink COMMAND_ACK message definition +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # Result cases. This follows the MAVLink MAV_RESULT enum definition diff --git a/msg/VehicleControlMode.msg b/msg/versioned/VehicleControlMode.msg similarity index 97% rename from msg/VehicleControlMode.msg rename to msg/versioned/VehicleControlMode.msg index 9b33f9b8ca..0a1fdedfbf 100644 --- a/msg/VehicleControlMode.msg +++ b/msg/versioned/VehicleControlMode.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool flag_armed # synonym for actuator_armed.armed diff --git a/msg/VehicleGlobalPosition.msg b/msg/versioned/VehicleGlobalPosition.msg similarity index 95% rename from msg/VehicleGlobalPosition.msg rename to msg/versioned/VehicleGlobalPosition.msg index e37155fe25..387576d8c7 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/versioned/VehicleGlobalPosition.msg @@ -5,6 +5,8 @@ # e.g. control inputs of the vehicle in a Kalman-filter implementation. # +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) @@ -13,6 +15,9 @@ float64 lon # Longitude, (degrees) float32 alt # Altitude AMSL, (meters) float32 alt_ellipsoid # Altitude above ellipsoid, (meters) +bool lat_lon_valid +bool alt_valid + float32 delta_alt # Reset delta for altitude float32 delta_terrain # Reset delta for terrain uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates diff --git a/msg/VehicleLandDetected.msg b/msg/versioned/VehicleLandDetected.msg similarity index 96% rename from msg/VehicleLandDetected.msg rename to msg/versioned/VehicleLandDetected.msg index fc0ca4a6d8..1cbf545713 100644 --- a/msg/VehicleLandDetected.msg +++ b/msg/versioned/VehicleLandDetected.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) bool freefall # true if vehicle is currently in free-fall diff --git a/msg/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg similarity index 91% rename from msg/VehicleLocalPosition.msg rename to msg/versioned/VehicleLocalPosition.msg index 0e74ac0f4b..8c04b4153c 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -1,6 +1,8 @@ # Fused local position in NED. # The coordinate system origin is the vehicle position at the time when the EKF2-module was started. +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) @@ -75,10 +77,12 @@ float32 evv # Standard deviation of vertical velocity error, (metres/sec) bool dead_reckoning # True if this position is estimated through dead-reckoning # estimator specified vehicle limits -float32 vxy_max # maximum horizontal speed - set to 0 when limiting not required (meters/sec) -float32 vz_max # maximum vertical speed - set to 0 when limiting not required (meters/sec) -float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters) -float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) +# set to INFINITY when limiting not required +float32 vxy_max # maximum horizontal speed (meters/sec) +float32 vz_max # maximum vertical speed (meters/sec) +float32 hagl_min # minimum height above ground level (meters) +float32 hagl_max_z # maximum height above ground level for z-control (meters) +float32 hagl_max_xy # maximum height above ground level for xy-control (meters) # TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position # TOPICS estimator_local_position diff --git a/msg/VehicleOdometry.msg b/msg/versioned/VehicleOdometry.msg similarity index 97% rename from msg/VehicleOdometry.msg rename to msg/versioned/VehicleOdometry.msg index fbdd1920e1..f4d600d2b8 100644 --- a/msg/VehicleOdometry.msg +++ b/msg/versioned/VehicleOdometry.msg @@ -1,4 +1,7 @@ # Vehicle odometry data. Fits ROS REP 147 for aerial vehicles + +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample diff --git a/msg/VehicleRatesSetpoint.msg b/msg/versioned/VehicleRatesSetpoint.msg similarity index 95% rename from msg/VehicleRatesSetpoint.msg rename to msg/versioned/VehicleRatesSetpoint.msg index 35a06c35aa..a4c169e412 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/versioned/VehicleRatesSetpoint.msg @@ -1,3 +1,5 @@ +uint32 MESSAGE_VERSION = 0 + uint64 timestamp # time since system start (microseconds) # body angular rates in FRD frame diff --git a/msg/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg similarity index 96% rename from msg/VehicleStatus.msg rename to msg/versioned/VehicleStatus.msg index 6756da8129..b9a3edd91f 100644 --- a/msg/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,5 +1,7 @@ # Encodes the system state of the vehicle published by commander +uint32 MESSAGE_VERSION = 1 + uint64 timestamp # time since system start (microseconds) uint64 armed_time # Arming timestamp (microseconds) @@ -129,9 +131,6 @@ bool open_drone_id_system_healthy bool parachute_system_present bool parachute_system_healthy -bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter -bool avoidance_system_valid # Status of the obstacle avoidance system - bool rc_calibration_in_progress bool calibration_enabled diff --git a/msg/VtolVehicleStatus.msg b/msg/versioned/VtolVehicleStatus.msg similarity index 94% rename from msg/VtolVehicleStatus.msg rename to msg/versioned/VtolVehicleStatus.msg index 61a8246796..51beb30fa3 100644 --- a/msg/VtolVehicleStatus.msg +++ b/msg/versioned/VtolVehicleStatus.msg @@ -1,4 +1,7 @@ # VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE + +uint32 MESSAGE_VERSION = 0 + uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index ec3ffd489d..73697fe887 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -106,6 +106,13 @@ #define ADC_V5_SCALE (2.0f) // The scale factor defined by HW's resistive divider (Rt+Rb)/ Rb #endif +#if !defined(ADC_PAYLOAD_V_FULL_SCALE) +#define ADC_PAYLOAD_V_FULL_SCALE (25.3f) // Payload volt Rail full scale voltage +#endif +#if !defined(ADC_PAYLOAD_SCALE) +#define ADC_PAYLOAD_SCALE (7.667f) // The scale factor defined by HW's resistive divider (Rt+Rb)/ Rb +#endif + #if !defined(ADC_3V3_V_FULL_SCALE) #define ADC_3V3_V_FULL_SCALE (3.6f) // 3.3V volt Rail full scale voltage #endif diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c index 5750846672..13911c1440 100644 --- a/platforms/common/pab_manifest.c +++ b/platforms/common/pab_manifest.c @@ -356,6 +356,52 @@ static const px4_hw_mft_item_t base_configuration_17[] = { }, }; +// BASE ID 18 Auterion Skynode S +static const px4_hw_mft_item_t base_configuration_18[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + // BASE ID 0x100 Holybro Pixhawk Jetson Baseboard Alaised to ID 0 // BASE ID 0x150 ZeroOne Pixhawk Baseboard Alaised to ID 0 @@ -372,6 +418,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 {HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)}, // Auterion Skynode ver 17 + {HW_BASE_ID(18), base_configuration_18, arraySize(base_configuration_18)}, // Auterion Skynode S ver 18 {HW_BASE_ID(0x100), base_configuration_0, arraySize(base_configuration_0)}, // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0 {HW_BASE_ID(0x150), base_configuration_0, arraySize(base_configuration_0)}, // ZeroOne Pixhawk Baseboard ver 0x150 }; diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index 963372bdbe..bf553fbbf1 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -87,7 +87,12 @@ const px4_spi_bus_t *px4_spi_buses{nullptr}; int px4_find_spi_bus(uint32_t devid) { +// px4_spi_buses is only NULL on certain targets depending on defines +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Waddress" + for (int i = 0; ((px4_spi_bus_t *) px4_spi_buses) != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) { +#pragma GCC diagnostic pop const px4_spi_bus_t &bus_data = px4_spi_buses[i]; if (bus_data.bus == -1) { diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index b40b60ad47..035d2f9ac5 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -146,7 +146,14 @@ int uORBTest::UnitTest::pubsublatency_main() pubsubtest_passed = true; - if (mean > 150.0f) { +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + // relaxed on SITL (non-realtime) + const float kMaxMeanUs = 1000.f; // 1000 microseconds +#else + const float kMaxMeanUs = 150.f; // 150 microseconds +#endif + + if (mean > kMaxMeanUs) { pubsubtest_res = PX4_ERROR; } else { diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index cd8385827e..19fcae552e 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -297,6 +297,29 @@ else() endif() +# create .hex if configured in NuttX +if (CONFIG_INTELHEX_BINARY) + + string(REPLACE ".elf" ".hex" hex_package ${PX4_BINARY_DIR}/${FW_NAME}) + + add_custom_command( + OUTPUT ${hex_package} + COMMAND + ${CMAKE_OBJCOPY} -O ihex + ${PX4_BINARY_DIR}/${PX4_CONFIG}.elf + ${hex_package} + DEPENDS + ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + airframes_xml + parameters_xml + COMMENT "Creating ${hex_package}" + WORKING_DIRECTORY ${PX4_BINARY_DIR} + ) + + add_custom_target(px4_hex ALL DEPENDS ${hex_package}) + +endif() + # create .px4 with parameter and airframe metadata if (TARGET parameters_xml AND TARGET airframes_xml) @@ -383,7 +406,7 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/gdbinit.in ${PX4_BINARY_DIR}/.g if(NOT NUTTX_DIR MATCHES "external") if(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A) set(DEBUG_DEVICE "MIMXRT1062XXX6A") - set(DEBUG_SVD_FILE "MIMXRT1052.svd") + set(DEBUG_SVD_FILE "MIMXRT1062.xml") elseif(CONFIG_ARCH_CHIP_MIMXRT1176DVMAA) set(DEBUG_DEVICE "MIMXRT1176DVMAA") set(DEBUG_SVD_FILE "MIMXRT1176_cm7.xml") @@ -483,6 +506,11 @@ if(NOT NUTTX_DIR MATCHES "external") message(STATUS "Found SVD: ${DEBUG_SVD_FILE_PATH}") endif() + if(NOT CMAKE_GDB) + find_program(CMAKE_GDB gdb-multiarch) + message(STATUS "Found GDB: ${CMAKE_GDB}") + endif() + if(DEBUG_DEVICE) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/launch.json.in ${PX4_SOURCE_DIR}/.vscode/launch.json @ONLY) endif() diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 8ae3f06808..9a213327fb 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 8ae3f0680865b98d3b02961e14949850d080d22d +Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c diff --git a/platforms/nuttx/cmake/upload.cmake b/platforms/nuttx/cmake/upload.cmake index ce776b8a62..83f623c090 100644 --- a/platforms/nuttx/cmake/upload.cmake +++ b/platforms/nuttx/cmake/upload.cmake @@ -35,18 +35,21 @@ set(vendorstr_underscore) set(productstr_underscore) string(REPLACE " " "_" vendorstr_underscore ${CONFIG_CDCACM_VENDORSTR}) +string(REPLACE "," "_" vendorstr_underscore "${vendorstr_underscore}") string(REPLACE " " "_" productstr_underscore ${CONFIG_CDCACM_PRODUCTSTR}) set(serial_ports) if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux") set(px4_usb_path "${vendorstr_underscore}_${productstr_underscore}") + set(px4_bl_usb_path "${vendorstr_underscore}_BL") list(APPEND serial_ports # NuttX vendor + product string /dev/serial/by-id/*-${px4_usb_path}* # Bootloader + /dev/serial/by-id/*_${px4_bl_usb_path}* /dev/serial/by-id/*PX4_BL* # typical bootloader USB device string /dev/serial/by-id/*BL_FMU* diff --git a/platforms/nuttx/src/bootloader/common/bl.c b/platforms/nuttx/src/bootloader/common/bl.c index f67356be70..7690fe062a 100644 --- a/platforms/nuttx/src/bootloader/common/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -153,6 +153,7 @@ #define STATE_ALLOWS_REBOOT (STATE_ALLOWS_ERASE|STATE_PROTO_PROG_MULTI|STATE_PROTO_GET_CRC) # define SET_BL_STATE(s) bl_state |= (s) #endif +#define STATE_ALLOWS_BOOTLOADER (STATE_PROTO_GET_SYNC|STATE_PROTO_GET_DEVICE) static uint8_t bl_type; static uint8_t last_input; @@ -1107,9 +1108,10 @@ bootloader(unsigned timeout) continue; } - // we got a command worth syncing, so kill the timeout because - // we are probably talking to the uploader - timeout = 0; + // We got a sync command as well as a get_device command, we are very likely talking to the uploader. + if ((bl_state & STATE_ALLOWS_BOOTLOADER) == STATE_ALLOWS_BOOTLOADER) { + timeout = 0; + } // Set the bootloader port based on the port from which we received the first valid command if (bl_type == NONE) { diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c index cccfd41bea..9b3bab5022 100644 --- a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -95,7 +95,7 @@ struct boardinfo board_info = { .board_type = BOARD_TYPE, .board_rev = 0, .fw_size = 0, - .systick_mhz = 480, + .systick_mhz = STM32_CPUCLK_FREQUENCY / 1000000, }; static void board_init(void); diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt index cba22cb320..deff20f14c 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt @@ -35,7 +35,9 @@ px4_add_library(arch_board_reset board_reset.cpp ) +if (CONFIG_ARCH_FAMILY_IMXRT117x) target_link_libraries(arch_board_reset PRIVATE arch_board_romapi) +endif() # up_systemreset if (NOT DEFINED CONFIG_BUILD_FLAT) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp index 6f1e21700d..1d29a64dfb 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp @@ -42,11 +42,12 @@ #include #include #include + +#ifdef CONFIG_ARCH_FAMILY_IMXRT117x #include - - #include #include +#endif #define BOOT_RTC_SIGNATURE 0xb007b007 #define PX4_IMXRT_RTC_REBOOT_REG 3 @@ -58,9 +59,13 @@ static int board_reset_enter_bootloader() { +#ifdef CONFIG_ARCH_FAMILY_IMXRT117x uint32_t regvalue = BOOT_RTC_SIGNATURE; modifyreg32(IMXRT_SNVS_LPCR, 0, SNVS_LPCR_GPR_Z_DIS); putreg32(regvalue, PX4_IMXRT_RTC_REBOOT_REG_ADDRESS); +#elif defined(BOARD_HAS_TEENSY_BOOTLOADER) + asm("BKPT #251"); /* Enter Teensy MKL02 bootloader */ +#endif return OK; } @@ -68,13 +73,18 @@ int board_reset(int status) { if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); + } - } else if (status == REBOOT_TO_ISP) { +#if defined(BOARD_HAS_ISP_BOOTLOADER) + + else if (status == REBOOT_TO_ISP) { uint32_t arg = 0xeb100000; ROM_API_Init(); ROM_RunBootloader(&arg); } +#endif + #if defined(BOARD_HAS_ON_RESET) board_on_reset(status); #endif diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index 9ce7b37445..f9e341a43c 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -68,7 +68,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp index 25c47ecffa..883d13908e 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp @@ -61,6 +61,7 @@ #include #include "hardware/imxrt_tmr.h" +#ifndef BOARD_HAS_CUSTOM_LED_PWM int led_pwm_servo_set(unsigned channel, uint8_t cvalue) { return 0; @@ -68,8 +69,8 @@ int led_pwm_servo_set(unsigned channel, uint8_t cvalue) int led_pwm_servo_init(void) { return 0; - } +#endif #if 0 && defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp index 01389b1200..0c3a10d661 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp @@ -41,15 +41,155 @@ #include #include "chip.h" +#ifdef TONE_ALARM_FLEXPWM +#include "hardware/imxrt_flexpwm.h" +#else #include "hardware/imxrt_gpt.h" +#endif #include "imxrt_periphclks.h" +#define CAT4_(A, B, C, D) A##B##C##D +#define CAT4(A, B, C, D) CAT4_(A, B, C, D) + #define CAT3_(A, B, C) A##B##C #define CAT3(A, B, C) CAT3_(A, B, C) #define CAT2_(A, B) A##B #define CAT2(A, B) CAT2_(A, B) +#ifdef TONE_ALARM_FLEXPWM + + +/* +* Period of the free-running counter, in microseconds. +*/ +#define TONE_ALARM_COUNTER_PERIOD 4294967296 + +/* Tone Alarm configuration */ + +#define TONE_ALARM_TIMER_CLOCK BOARD_GPT_FREQUENCY /* The input clock frequency to the GPT block */ +#define TONE_ALARM_TIMER_BASE CAT3(IMXRT_FLEXPWM, TONE_ALARM_TIMER,_BASE) /* The Base address of the GPT */ +#define TONE_ALARM_TIMER_VECTOR CAT4(IMXRT_IRQ_FLEXPWM, TONE_ALARM_TIMER, _, TONE_ALARM_CHANNEL) /* The GPT Interrupt vector */ + +#if TONE_ALARM_TIMER == 1 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm1() /* The Clock Gating macro for this PWM */ +#elif TONE_ALARM_TIMER == 2 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm2() /* The Clock Gating macro for this PWM */ +#elif TONE_ALARM_TIMER == 3 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm3() /* The Clock Gating macro for this PWM */ +#elif TONE_ALARM_TIMER == 4 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm4() /* The Clock Gating macro for this PWM */ +#endif + +# define TONE_ALARM_TIMER_FREQ 1000000 + +#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET) + +/* Register accessors */ +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) +#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) +#define REG(_tmr, _sm, _reg) _REG16(TONE_ALARM_TIMER_BASE + ((_sm) * SM_SPACING), (_reg)) + + +#define rINIT(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */ +#define rCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL_OFFSET) /* Control Register */ +#define rCTRL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL2_OFFSET) /* Control 2 Register */ +#define rFSTS0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FSTS0_OFFSET) /* Fault Status Register */ +#define rVAL0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL0_OFFSET) /* Value Register 0 */ +#define rVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL1_OFFSET) /* Value Register 1 */ +#define rVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL2_OFFSET) /* Value Register 2 */ +#define rVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL3_OFFSET) /* Value Register 3 */ +#define rVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL4_OFFSET) /* Value Register 4 */ +#define rVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL5_OFFSET) /* Value Register 5 */ +#define rFFILT0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FFILT0_OFFSET) /* Fault Filter Register */ +#define rDISMAP0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP0_OFFSET) /* Fault Disable Mapping Register 0 */ +#define rDISMAP1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP1_OFFSET) /* Fault Disable Mapping Register 1 */ +#define rOUTEN(_tim) REG(_tim, 0, IMXRT_FLEXPWM_OUTEN_OFFSET) /* Output Enable Register */ +#define rDTSRCSEL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */ +#define rMCTRL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */ + +namespace ToneAlarmInterface +{ + +void init() +{ +#if defined(TONE_ALARM_TIMER) + /* configure the GPIO to the idle state */ + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); + + /* Enable the Module clock */ + + TONE_ALARM_CLOCK_ALL(); + + /* Clear all Faults */ + rFSTS0(TONE_ALARM_TIMER) = FSTS_FFLAG_MASK; + rMCTRL(TONE_ALARM_TIMER) |= (1 << (TONE_ALARM_CHANNEL + MCTRL_CLDOK_SHIFT)); + + rCTRL2(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rVAL0(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rVAL2(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rVAL4(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rFFILT0(TONE_ALARM_TIMER) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0xf000; + rDISMAP1(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0xf000; + + rOUTEN(TONE_ALARM_TIMER) |= TONE_ALARM_FLEXPWM == PWMA_VAL ? OUTEN_PWMA_EN(1 << TONE_ALARM_CHANNEL) + : OUTEN_PWMB_EN(1 << TONE_ALARM_CHANNEL); + + rDTSRCSEL(TONE_ALARM_TIMER) = 0; + rMCTRL(TONE_ALARM_TIMER) |= MCTRL_LDOK(1 << TONE_ALARM_CHANNEL); +#endif /* TONE_ALARM_TIMER */ +} + +hrt_abstime start_note(unsigned frequency) +{ + hrt_abstime time_started = 0; +#if defined(TONE_ALARM_TIMER) + + rMCTRL(TONE_ALARM_TIMER) |= (1 << (TONE_ALARM_CHANNEL + MCTRL_CLDOK_SHIFT)); + + rVAL1(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = (TONE_ALARM_TIMER_FREQ / frequency) - 1; + + if (TONE_ALARM_FLEXPWM == PWMA_VAL) { + rVAL3(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = ((TONE_ALARM_TIMER_FREQ / frequency) / 2); + + } else { + rVAL5(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = ((TONE_ALARM_TIMER_FREQ / frequency) / 2); + } + + rMCTRL(TONE_ALARM_TIMER) |= MCTRL_LDOK(1 << TONE_ALARM_CHANNEL) | MCTRL_RUN(1 << TONE_ALARM_CHANNEL); + + // configure the GPIO to enable timer output + irqstate_t flags = enter_critical_section(); + time_started = hrt_absolute_time(); + px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); +#endif /* TONE_ALARM_TIMER */ + + return time_started; +} + +void stop_note() +{ +#if defined(TONE_ALARM_TIMER) + /* stop the current note */ + + rMCTRL(TONE_ALARM_TIMER) &= ~MCTRL_RUN(1 << TONE_ALARM_CHANNEL); + + /* + * Make sure the GPIO is not driving the speaker. + */ + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +#endif /* TONE_ALARM_TIMER */ +} + +} /* namespace ToneAlarmInterface */ + +#else + /* Check that tone alarm and HRT timers are different */ #if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) # if TONE_ALARM_TIMER == HRT_TIMER @@ -134,8 +274,6 @@ #define CR_OM CAT3(GPT_CR_OM, TONE_ALARM_CHANNEL,_TOGGLE) /* Output Compare mode */ -#define CBRK_BUZZER_KEY 782097 - namespace ToneAlarmInterface { @@ -215,3 +353,5 @@ void stop_note() } } /* namespace ToneAlarmInterface */ + +#endif diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index 788da87485..836e214472 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -67,7 +67,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h index 9945e28de6..ca93f14a4f 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h @@ -68,7 +68,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h index 2454056a03..82369986b5 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -63,7 +63,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h index d093130b51..d6e0ad6eb5 100644 --- a/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/include/px4_arch/io_timer.h @@ -63,7 +63,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h index c9fbedd8f0..0d042d85ce 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h @@ -73,6 +73,10 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 31d6e9b1c7..8f64a825b6 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -79,7 +79,8 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Dshot = 6, IOTimerChanMode_LED = 7, IOTimerChanMode_PPS = 8, - IOTimerChanMode_Other = 9, + IOTimerChanMode_RPM = 9, + IOTimerChanMode_Other = 10, IOTimerChanModeSize } io_timer_channel_mode_t; diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index bf69b6a7ce..fdd8a5300e 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -5,7 +5,6 @@ # tests command arguments set(tests atomic_bitset - bezier bitset bson dataman diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index d12ab23442..3ae9b82d0a 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -15,7 +15,7 @@ pwm_out_sim start ver all -mavlink start -x -u 14556 -r 2000000 -p +mavlink start -x -u 14556 -r 2000000 mavlink boot_complete @cmd_name@ start diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index 174ab2cf08..57afc0ca5e 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -15,7 +15,7 @@ pwm_out_sim start ver all -mavlink start -x -u 14556 -r 2000000 -p +mavlink start -x -u 14556 -r 2000000 mavlink boot_complete mavlink_tests diff --git a/src/drivers/actuators/vertiq_io/module.yaml b/src/drivers/actuators/vertiq_io/module.yaml index cded74a8eb..cfcc8a0499 100644 --- a/src/drivers/actuators/vertiq_io/module.yaml +++ b/src/drivers/actuators/vertiq_io/module.yaml @@ -254,36 +254,36 @@ parameters: The module IDs [32, 62] that should be asked for telemetry. The data received from these IDs will be published via the esc_status topic. type: bitmask bit: - 32: Module ID 32 - 33: Module ID 33 - 34: Module ID 34 - 35: Module ID 35 - 36: Module ID 36 - 37: Module ID 37 - 38: Module ID 38 - 39: Module ID 39 - 40: Module ID 40 - 41: Module ID 41 - 42: Module ID 42 - 43: Module ID 43 - 44: Module ID 44 - 45: Module ID 45 - 46: Module ID 46 - 47: Module ID 47 - 48: Module ID 48 - 49: Module ID 49 - 50: Module ID 50 - 51: Module ID 51 - 52: Module ID 52 - 53: Module ID 53 - 54: Module ID 54 - 55: Module ID 55 - 56: Module ID 56 - 57: Module ID 57 - 58: Module ID 58 - 59: Module ID 59 - 60: Module ID 60 - 61: Module ID 61 - 62: Module ID 62 + 0: Module ID 32 + 1: Module ID 33 + 2: Module ID 34 + 3: Module ID 35 + 4: Module ID 36 + 5: Module ID 37 + 6: Module ID 38 + 7: Module ID 39 + 8: Module ID 40 + 9: Module ID 41 + 10: Module ID 42 + 11: Module ID 43 + 12: Module ID 44 + 13: Module ID 45 + 14: Module ID 46 + 15: Module ID 47 + 16: Module ID 48 + 17: Module ID 49 + 18: Module ID 50 + 19: Module ID 51 + 20: Module ID 52 + 21: Module ID 53 + 22: Module ID 54 + 23: Module ID 55 + 24: Module ID 56 + 25: Module ID 57 + 26: Module ID 58 + 27: Module ID 59 + 28: Module ID 60 + 29: Module ID 61 + 30: Module ID 62 reboot_required: true default: 0 diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index 906af69959..7bf553f9b4 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -204,6 +204,9 @@ void ADC::update_system_power(hrt_abstime now) # if defined(ADC_SCALED_V5_SENSE) && defined(ADC_SCALED_V3V3_SENSORS_SENSE) cnt += ADC_SCALED_V3V3_SENSORS_COUNT; # endif +# if defined(ADC_SCALED_PAYLOAD_SENSE) + cnt++; +# endif for (unsigned i = 0; i < _channel_count; i++) { # if defined(ADC_SCALED_V5_SENSE) @@ -234,6 +237,16 @@ void ADC::update_system_power(hrt_abstime now) } # endif +# if defined(ADC_SCALED_PAYLOAD_SENSE) + + if (_samples[i].am_channel == ADC_SCALED_PAYLOAD_SENSE) { + system_power.voltage_payload_v = _samples[i].am_data * ((ADC_PAYLOAD_V_FULL_SCALE / 3.3f) * + (px4_arch_adc_reference_v() / + px4_arch_adc_dn_fullcount())); + cnt--; + } + +# endif if (cnt == 0) { break; @@ -285,6 +298,9 @@ void ADC::update_system_power(hrt_abstime now) #ifdef BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID system_power.can1_gps1_5v_valid = read_gpio_value(_5v_can1_gps1_valid_fd); #endif +#ifdef BOARD_GPIO_PAYOLOAD_V_VALID + system_power.payload_v_valid = BOARD_GPIO_PAYOLOAD_V_VALID; +#endif system_power.timestamp = hrt_absolute_time(); _to_system_power.publish(system_power); diff --git a/src/drivers/barometer/Kconfig b/src/drivers/barometer/Kconfig index da9c2dd714..868b552621 100644 --- a/src/drivers/barometer/Kconfig +++ b/src/drivers/barometer/Kconfig @@ -11,6 +11,7 @@ menu "barometer" select DRIVERS_BAROMETER_MS5611 select DRIVERS_BAROMETER_MAIERTEK_MPC2520 select DRIVERS_BAROMETER_GOERTEK_SPL06 + select DRIVERS_BAROMETER_GOERTEK_SPA06 select DRIVERS_BAROMETER_INVENSENSE_ICP101XX select DRIVERS_BAROMETER_INVENSENSE_ICP201XX ---help--- diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp index ae5387ad7c..864d4d3f3b 100644 --- a/src/drivers/barometer/bmp280/BMP280.cpp +++ b/src/drivers/barometer/bmp280/BMP280.cpp @@ -67,7 +67,7 @@ BMP280::init() // set config, recommended settings _interface->set_reg(_curr_ctrl, BMP280_ADDR_CTRL); - _interface->set_reg(BMP280_CONFIG_F16, BMP280_ADDR_CONFIG); + _interface->set_reg(BMP280_CONFIG_F0, BMP280_ADDR_CONFIG); // get calibration and pre process them _cal = _interface->get_calibration(BMP280_ADDR_CAL); diff --git a/src/drivers/barometer/goertek/CMakeLists.txt b/src/drivers/barometer/goertek/CMakeLists.txt index 867eb64fca..b626766ddd 100644 --- a/src/drivers/barometer/goertek/CMakeLists.txt +++ b/src/drivers/barometer/goertek/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# 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 @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(spl06) +add_subdirectory(spa06) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt b/src/drivers/barometer/goertek/spa06/CMakeLists.txt similarity index 88% rename from src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt rename to src/drivers/barometer/goertek/spa06/CMakeLists.txt index 72928c7e25..572b5dab2a 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt +++ b/src/drivers/barometer/goertek/spa06/CMakeLists.txt @@ -31,9 +31,15 @@ # ############################################################################ -px4_add_library(RoverAckermannGuidance - RoverAckermannGuidance.cpp -) - -target_link_libraries(RoverAckermannGuidance PUBLIC pid) -target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +px4_add_module( + MODULE drivers__barometer__spa06 + MAIN spa06 + SRCS + SPA06.cpp + SPA06.hpp + SPA06_I2C.cpp + SPA06_SPI.cpp + spa06_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/goertek/spa06/Kconfig b/src/drivers/barometer/goertek/spa06/Kconfig new file mode 100644 index 0000000000..7190a27def --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_GOERTEK_SPA06 + bool "spa06" + default n + ---help--- + Enable support for spa06 diff --git a/src/drivers/barometer/goertek/spa06/SPA06.cpp b/src/drivers/barometer/goertek/spa06/SPA06.cpp new file mode 100644 index 0000000000..a1b1b4118f --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06.cpp @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * 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 "SPA06.hpp" + +SPA06::SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface) : + I2CSPIDriver(config), + _interface(interface), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) +{ +} + +SPA06::~SPA06() +{ + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + + delete _interface; +} + +/* +float +SPA06::scale_factor(int oversampling_rate) +{ + float k; + + switch (oversampling_rate) { + case 1: + k = 524288.0f; + break; + + case 2: + k = 1572864.0f; + break; + + case 4: + k = 3670016.0f; + break; + + case 8: + k = 7864320.0f; + break; + + case 16: + k = 253952.0f; + break; + + case 32: + k = 516096.0f; + break; + + case 64: + k = 1040384.0f; + break; + + case 128: + k = 2088960.0f; + break; + + default: + k = 0; + break; + } + + return k; +} +*/ + +int +SPA06::calibrate() +{ + uint8_t buf[21]; + + _interface->read(SPA06_ADDR_CAL, buf, sizeof(buf)); + + _cal.c0 = (uint16_t)(buf[0]) << 4 | (uint16_t)(buf[1]) >> 4; + // If value is negative, we need to fill the missing bits. + _cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0; + + _cal.c1 = (uint16_t)(buf[1] & 0x0F) << 8 | buf[2]; + _cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1; + + _cal.c00 = (uint32_t)(buf[3]) << 12 | (uint32_t)(buf[4]) << 4 | (buf[5]) >> 4; + _cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00; + + _cal.c10 = (uint32_t)(buf[5] & 0x0F) << 16 | (uint32_t)(buf[6]) << 8 | buf[7]; + _cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10; + + _cal.c01 = (uint16_t)(buf[8]) << 8 | buf[9]; + + _cal.c11 = (uint16_t)(buf[10]) << 8 | buf[11]; + + _cal.c20 = (uint16_t)(buf[12]) << 8 | buf[13]; + + _cal.c21 = (uint16_t)(buf[14]) << 8 | buf[15]; + + _cal.c30 = (uint16_t)(buf[16]) << 8 | buf[17]; + + _cal.c31 = (uint16_t)(buf[18]) << 4 | (uint16_t)(buf[19] & 0xF0) >> 4; + _cal.c31 = (_cal.c31 & 1 << 11) ? (0xf000 | _cal.c31) : _cal.c31; + + _cal.c40 = (uint16_t)(buf[19] & 0x0F) << 8 | buf[20]; + _cal.c40 = (_cal.c40 & 1 << 11) ? (0xf000 | _cal.c40) : _cal.c40; + + PX4_DEBUG("c0:%d\nc1:%d\nc00:%ld\nc10:%ld\nc01:%d\nc11:%d\nc20:%d\nc21:%d\nc30:%d\nc31:%d\nc40:%d\n", + _cal.c0, _cal.c1, + _cal.c00, _cal.c10, + _cal.c01, _cal.c11, _cal.c20, _cal.c21, _cal.c30, _cal.c31, _cal.c40); + + return OK; +} +int +SPA06::init() +{ + int8_t tries = 5; + // reset sensor + _interface->set_reg(SPA06_VALUE_RESET, SPA06_ADDR_RESET); + usleep(10000); + + // check id + if (_interface->get_reg(SPA06_ADDR_ID) != SPA06_VALUE_ID) { + PX4_DEBUG("id of your baro is not: 0x%02x", SPA06_VALUE_ID); + return -EIO; + } + + while (tries--) { + uint8_t meas_cfg = _interface->get_reg(SPA06_ADDR_MEAS_CFG); + + if (meas_cfg & (1 << 7) && meas_cfg & (1 << 6)) { + break; + } + + usleep(10000); + } + + if (tries < 0) { + PX4_DEBUG("spa06 sensor or coef not ready"); + return -EIO; + } + + // get calibration and pre process them + calibrate(); + + // set config, recommended settings + _interface->set_reg(_curr_prs_cfg, SPA06_ADDR_PRS_CFG); + kp = 253952.0f; // refer to scale_factor() + _interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG); + kt = 524288.0f; + + // Enable FIFO + _interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG); + // Continuous pressure and temperature mesasurement. + _interface->set_reg(7, SPA06_ADDR_MEAS_CFG); + + Start(); + + return OK; +} + +void +SPA06::Start() +{ + // schedule a cycle to start things + ScheduleNow(); +} + +void +SPA06::RunImpl() +{ + collect(); + + ScheduleDelayed(_measure_interval); +} +int +SPA06::collect() +{ + perf_begin(_sample_perf); + + // this should be fairly close to the end of the conversion, so the best approximation of the time + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (_interface->read(SPA06_ADDR_DATA, (uint8_t *)&_data, sizeof(_data)) != OK) { + perf_count(_comms_errors); + perf_cancel(_sample_perf); + return -EIO; + } + + int32_t temp_raw = (uint32_t)_data.t_msb << 16 | (uint32_t)_data.t_lsb << 8 | (uint32_t)_data.t_xlsb; + temp_raw = (temp_raw & 1 << 23) ? (0xff000000 | temp_raw) : temp_raw; + + int32_t press_raw = (uint32_t)_data.p_msb << 16 | (uint32_t) _data.p_lsb << 8 | (uint32_t) _data.p_xlsb; + press_raw = (press_raw & 1 << 23) ? (0xff000000 | press_raw) : press_raw; + + // calculate + float ftsc = (float)temp_raw / kt; + float fpsc = (float)press_raw / kp; + float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * ((float)_cal.c30) + fpsc * (float)_cal.c40); + float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * ((float)_cal.c21) + fpsc * (float)_cal.c31); + + float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3; + float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc; + + + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = fp; + sensor_baro.temperature = temperature; + sensor_baro.error_count = perf_event_count(_comms_errors); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + perf_end(_sample_perf); + + return OK; +} + +void +SPA06::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_measure_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/barometer/goertek/spa06/SPA06.hpp b/src/drivers/barometer/goertek/spa06/SPA06.hpp new file mode 100644 index 0000000000..255da70819 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 "spa06.h" + +#include +#include +#include +#include +#include +#include +#include + +class SPA06 : public I2CSPIDriver +{ +public: + SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface); + virtual ~SPA06(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + int init(); + void print_status(); + + void RunImpl(); +private: + void Start(); + // float scale_factor(int oversampling_rate); + + int collect(); //get results and publish + int calibrate(); + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + + spa06::ISPA06 *_interface; + spa06::data_s _data; + spa06::calibration_s _cal{}; + + // set config, recommended settings + // + // oversampling rate : single | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // scale factor(KP/KT): 524288 | 1572864 | 3670016 | 7864320 | 253952 | 516096 | 1040384 | 2088960 + + // configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC) + // + // PM_RATE[7:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200 + // note: applicable for measurements in background mode only + // + // PM_PRC[3:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // measurement time(ms): 3.6 | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8 + // precision(PaRMS) : 5.0 | | 2.5 | | 1.2 | 0.9 | 0.5 | + // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register + // + // -> 32 measurements per second, 16 oversampling + static constexpr uint8_t _curr_prs_cfg{5 << 4 | 4}; + + // configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC) + // + // temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element) + // PM_RATE[7:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200 + // note: applicable for measurements in background mode only + // + // TMP_PRC[2:0] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant + // note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register + + // -> 32 measurements per second, single oversampling + static constexpr uint8_t _curr_tmp_cfg{5 << 4 | 0}; + + bool _collect_phase{false}; + float kp; + float kt; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + + static constexpr uint32_t _sample_rate{32}; + static constexpr uint32_t _measure_interval{1000000 / _sample_rate}; +}; diff --git a/src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp b/src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp new file mode 100644 index 0000000000..e3e6044037 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06_I2C.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file SPA06_I2C.cpp + * + * SPI interface for Goertek SPA06 + */ + +#include "spa06.h" + +#include +#include + +#if defined(CONFIG_I2C) + +class SPA06_I2C: public device::I2C, public spa06::ISPA06 +{ +public: + SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency); + virtual ~SPA06_I2C() override = default; + + int init() override { return I2C::init(); } + + uint8_t get_reg(uint8_t addr) override; + int set_reg(uint8_t value, uint8_t addr) override; + + int read(uint8_t addr, uint8_t *buf, uint8_t len) override; + //spa06::data_s *get_data(uint8_t addr) override; + //spa06::calibration_s *get_calibration(uint8_t addr) override; + + uint32_t get_device_id() const override { return device::I2C::get_device_id(); } + + uint8_t get_device_address() const override { return device::I2C::get_device_address(); } +private: + spa06::calibration_s _cal{}; + spa06::data_s _data{}; +}; + +spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency) +{ + return new SPA06_I2C(busnum, device, bus_frequency); +} + +SPA06_I2C::SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency) : + I2C(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, bus_frequency) +{ +} + +uint8_t +SPA06_I2C::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), 0}; + transfer(&cmd[0], 1, &cmd[1], 1); + + return cmd[1]; +} + +int +SPA06_I2C::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), value}; + return transfer(cmd, sizeof(cmd), nullptr, 0); +} + +int +SPA06_I2C::read(uint8_t addr, uint8_t *buf, uint8_t len) +{ + return transfer(&addr, 1, buf, len); +} + +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp b/src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp new file mode 100644 index 0000000000..80c6112090 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/SPA06_SPI.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file SPA06_SPI.cpp + * + * SPI interface for Goertek SPA06 + */ + +#include "spa06.h" + +#include +#include + +#if defined(CONFIG_SPI) + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) //for set +#define DIR_WRITE ~(1<<7) //for clear + +class SPA06_SPI: public device::SPI, public spa06::ISPA06 +{ +public: + SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + virtual ~SPA06_SPI() override = default; + + int init() override { return SPI::init(); } + + uint8_t get_reg(uint8_t addr) override; + int set_reg(uint8_t value, uint8_t addr) override; + + int read(uint8_t addr, uint8_t *buf, uint8_t len) override; + + uint32_t get_device_id() const override { return device::SPI::get_device_id(); } + + uint8_t get_device_address() const override { return device::SPI::get_device_address(); } +}; + +spa06::ISPA06 * +spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode) +{ + return new SPA06_SPI(busnum, device, bus_frequency, spi_mode); +} + +SPA06_SPI::SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, spi_mode, bus_frequency) +{ +} + +uint8_t +SPA06_SPI::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; // set MSB bit + transfer(&cmd[0], &cmd[0], 2); + + return cmd[1]; +} + +int +SPA06_SPI::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; // clear MSB bit + return transfer(&cmd[0], nullptr, 2); +} + +int +SPA06_SPI::read(uint8_t addr, uint8_t *buf, uint8_t len) +{ + uint8_t tx_buf[len + 1] = {(uint8_t)(addr | DIR_READ)}; // GCC support VLA, let's use it + + return transfer(tx_buf, buf, len); +} + +#endif // CONFIG_SPI diff --git a/src/drivers/barometer/goertek/spa06/parameters.c b/src/drivers/barometer/goertek/spa06/parameters.c new file mode 100644 index 0000000000..a785329131 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Goertek SPA06 Barometer (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SPA06, 0); diff --git a/src/drivers/barometer/goertek/spa06/spa06.h b/src/drivers/barometer/goertek/spa06/spa06.h new file mode 100644 index 0000000000..4412974f35 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/spa06.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file spa06.h + * + * Shared defines for the SPA06 driver. + */ +#pragma once + +#include + +#define SPA06_ADDR_ID 0x0d +#define SPA06_ADDR_RESET 0x0c // set to reset +#define SPA06_ADDR_CAL 0x10 +#define SPA06_ADDR_PRS_CFG 0x06 +#define SPA06_ADDR_TMP_CFG 0x07 +#define SPA06_ADDR_MEAS_CFG 0x08 +#define SPA06_ADDR_CFG_REG 0x09 +#define SPA06_ADDR_DATA 0x00 + + +#define SPA06_VALUE_RESET 9 +#define SPA06_VALUE_ID 0x11 + +namespace spa06 +{ + +#pragma pack(push,1) +struct calibration_s { + int16_t c0, c1; + int32_t c00, c10; + int16_t c01, c11, c20, c21, c30, c31, c40; +}; + +struct data_s { + uint8_t p_msb; + uint8_t p_lsb; + uint8_t p_xlsb; + + uint8_t t_msb; + uint8_t t_lsb; + uint8_t t_xlsb; +}; +#pragma pack(pop) + +class ISPA06 +{ +public: + virtual ~ISPA06() = default; + + virtual int init() = 0; + + // read reg value + virtual uint8_t get_reg(uint8_t addr) = 0; + + // write reg value + virtual int set_reg(uint8_t value, uint8_t addr) = 0; + + // bulk read of data into buffer, return same pointer + virtual int read(uint8_t addr, uint8_t *buf, uint8_t len) = 0; + // bulk read of calibration data into buffer, return same pointer + + virtual uint32_t get_device_id() const = 0; + + virtual uint8_t get_device_address() const = 0; +}; + +} // namespace spa06 + + +#if defined(CONFIG_SPI) +extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) +extern spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency); +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/goertek/spa06/spa06_main.cpp b/src/drivers/barometer/goertek/spa06/spa06_main.cpp new file mode 100644 index 0000000000..848b1f19a7 --- /dev/null +++ b/src/drivers/barometer/goertek/spa06/spa06_main.cpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include "SPA06.hpp" + +#include + +extern "C" { __EXPORT int spa06_main(int argc, char *argv[]); } + +void +SPA06::print_usage() +{ + PRINT_MODULE_USAGE_NAME("spa06", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); +#if defined(CONFIG_I2C) + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); +#else + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); +#endif + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *SPA06::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + spa06::ISPA06 *interface = nullptr; + +#if defined(CONFIG_I2C) + + if (config.bus_type == BOARD_I2C_BUS) { + interface = spa06_i2c_interface(config.bus, config.i2c_address, config.bus_frequency); + + } + +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + + if (config.bus_type == BOARD_SPI_BUS) { + interface = spa06_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); + } + +#endif // CONFIG_SPI + + if (interface == nullptr) { + PX4_ERR("failed creating interface for bus %i", config.bus); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i", config.bus); + return nullptr; + } + + SPA06 *dev = new SPA06(config, interface); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +int +spa06_main(int argc, char *argv[]) +{ + using ThisDriver = SPA06; + BusCLIArguments cli{true, true}; +#if defined(CONFIG_I2C) + cli.i2c_address = 0x76; + cli.default_i2c_frequency = 100 * 1000; +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + cli.default_spi_frequency = 10 * 1000 * 1000; +#endif // CONFIG_SPI + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_SPA06); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 7bf3813542..8430209020 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -71,17 +71,14 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - // get the capture channel from function configuration params - _capture_channel = -1; - - for (unsigned i = 0; i < 16 && _capture_channel == -1; ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS && _capture_channel == -1; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2032) { // Camera_Capture + if (function == 2032) { // Camera_Capture see mixer_module/output_functions.yaml parameter metadata definition _capture_channel = i; } } diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 92dd2790d1..3074b5cc8d 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -96,7 +96,7 @@ public: static struct work_s _work_publisher; private: - int _capture_channel = 5; ///< by default, use FMU output 6 + int _capture_channel{-1}; // Publishers uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 3841e20e56..54e0fd3175 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -47,7 +47,7 @@ * * @value 1 GPIO * @value 2 Seagull MAP2 (over PWM) -* @value 3 MAVLink (forward via MAV_CMD_IMAGE_START_CAPTURE) +* @value 3 MAVLink (Camera Protocol v1) * @value 4 Generic PWM (IR trigger, servo) * * @reboot_required true diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index 02e9119cae..cf601ef414 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "camera_interface.h" +#include #include #include @@ -44,14 +45,14 @@ void CameraInterface::get_pins() unsigned pin_index = 0; - for (unsigned i = 0; i < 16 && pin_index < arraySize(_pins); ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS && pin_index < arraySize(_pins); ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2000) { // Camera_Trigger + if (function == 2000) { // Camera_Trigger see mixer_module/output_functions.yaml parameter metadata definition _pins[pin_index++] = i; } } diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index f45abe877c..25adedcd89 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -3,12 +3,9 @@ menu "Distance sensors" bool "Common distance sensor's" default n select DRIVERS_DISTANCE_SENSOR_CM8JL65 - select DRIVERS_DISTANCE_SENSOR_GY_US42 - select DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL select DRIVERS_DISTANCE_SENSOR_LL40LS - select DRIVERS_DISTANCE_SENSOR_SRF02 select DRIVERS_DISTANCE_SENSOR_TERARANGER select DRIVERS_DISTANCE_SENSOR_TF02PRO select DRIVERS_DISTANCE_SENSOR_TFMINI diff --git a/src/drivers/distance_sensor/cm8jl65/module.yaml b/src/drivers/distance_sensor/cm8jl65/module.yaml index 886dce7e3a..0c9e700836 100644 --- a/src/drivers/distance_sensor/cm8jl65/module.yaml +++ b/src/drivers/distance_sensor/cm8jl65/module.yaml @@ -18,7 +18,7 @@ parameters: values: 25: ROTATION_DOWNWARD_FACING 24: ROTATION_UPWARD_FACING - 12: ROTATION_BACKWARD_FACING + 4: ROTATION_BACKWARD_FACING 0: ROTATION_FORWARD_FACING 6: ROTATION_LEFT_FACING 2: ROTATION_RIGHT_FACING diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt index 561b7755c7..6d561f72db 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt +++ b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( DEPENDS drivers_rangefinder px4_work_queue + CollisionPrevention MODULE_CONFIG module.yaml ) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index 22ae7c8207..587348ca7d 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -32,24 +32,21 @@ ****************************************************************************/ #include "lightware_sf45_serial.hpp" -#include "sf45_commands.h" -#include #include -#include -#include - #include -#include +#include +#include + +#include +#include #include +#include -/* Configuration Constants */ -#define SF45_MAX_PAYLOAD 256 -#define SF45_SCALE_FACTOR 0.01f +using namespace time_literals; -SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : +SF45LaserSerial::SF45LaserSerial(const char *port) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _px4_rangefinder(0, rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { @@ -68,17 +65,17 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : device_id.devid_s.bus = bus_num; } - _num_retries = 2; - _px4_rangefinder.set_device_id(device_id.devid); - _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); - // populate obstacle map members - _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - _obstacle_map_msg.increment = 5; - _obstacle_map_msg.angle_offset = 2.5; - _obstacle_map_msg.min_distance = UINT16_MAX; - _obstacle_map_msg.max_distance = 5000; + _obstacle_distance.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + _obstacle_distance.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; + _obstacle_distance.increment = 5; + _obstacle_distance.min_distance = 20; + _obstacle_distance.max_distance = 5000; + _obstacle_distance.angle_offset = 0; + for (uint32_t i = 0 ; i < BIN_COUNT; i++) { + _obstacle_distance.distances[i] = UINT16_MAX; + } } SF45LaserSerial::~SF45LaserSerial() @@ -91,53 +88,45 @@ SF45LaserSerial::~SF45LaserSerial() int SF45LaserSerial::init() { - param_get(param_find("SF45_UPDATE_CFG"), &_update_rate); param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); - /* SF45/B (50M) */ - _px4_rangefinder.set_min_distance(0.2f); - _px4_rangefinder.set_max_distance(50.0f); - _interval = 10000; + // set the sensor orientation + const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast + (_yaw_cfg)); + _obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle)); start(); - return PX4_OK; } int SF45LaserSerial::measure() { - - int rate = (int)_update_rate; - _data_output = 0x101; // raw distance + yaw readings + int32_t rate = (int32_t)_update_rate; + _data_output = 0x101; // raw distance (first return) + yaw readings _stream_data = 5; // enable constant streaming - // send some packets so the sensor starts scanning + // send packets to the sensor depending on the state switch (_sensor_state) { - // sensor should now respond case STATE_UNINIT: - while (_num_retries--) { - sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); - _sensor_state = STATE_UNINIT; - } - - _sensor_state = STATE_SEND_PRODUCT_NAME; + // Used to probe if the sensor is alive + sf45_send(SF_PRODUCT_NAME, false, &_product_name[0], 0); break; - case STATE_SEND_PRODUCT_NAME: + case STATE_ACK_PRODUCT_NAME: // Update rate default to 50 readings/s sf45_send(SF_UPDATE_RATE, true, &rate, sizeof(uint8_t)); - _sensor_state = STATE_SEND_UPDATE_RATE; break; - case STATE_SEND_UPDATE_RATE: + case STATE_ACK_UPDATE_RATE: + // Configure the data that the sensor shall output sf45_send(SF_DISTANCE_OUTPUT, true, &_data_output, sizeof(_data_output)); - _sensor_state = STATE_SEND_DISTANCE_DATA; break; - case STATE_SEND_DISTANCE_DATA: + case STATE_ACK_DISTANCE_OUTPUT: + // Configure the sensor to automatically output data at the configured update rate sf45_send(SF_STREAM, true, &_stream_data, sizeof(_stream_data)); _sensor_state = STATE_SEND_STREAM; break; @@ -151,129 +140,87 @@ int SF45LaserSerial::measure() int SF45LaserSerial::collect() { - perf_begin(_sample_perf); + if (_sensor_state == STATE_UNINIT) { - /* clear buffer if last read was too long ago */ - int ret; - /* the buffer for read chars is buflen minus null termination */ - uint8_t readbuf[SF45_MAX_PAYLOAD]; + perf_begin(_sample_perf); + const int payload_length = 22; - float distance_m = -1.0f; + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_PRODUCT_NAME); - /* read from the sensor (uart buffer) */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - - - if (_sensor_state == STATE_SEND_PRODUCT_NAME) { - - ret = ::read(_fd, &readbuf[0], 22); - - if (ret < 0) { - PX4_ERR("ERROR (ack from sending product name cmd): %d", ret); - perf_count(_comms_errors); + if (_crc_valid) { + _sensor_state = STATE_ACK_PRODUCT_NAME; perf_end(_sample_perf); - return ret; + return PX4_OK; } - sf45_request_handle(ret, readbuf); - ScheduleDelayed(_interval * 3); + return -EAGAIN; - } else if (_sensor_state == STATE_SEND_UPDATE_RATE) { + } else if (_sensor_state == STATE_ACK_PRODUCT_NAME) { - ret = ::read(_fd, &readbuf[0], 7); + perf_begin(_sample_perf); + const int payload_length = 7; - if (ret < 0) { - PX4_ERR("ERROR (ack from sending update rate cmd): %d", ret); - perf_count(_comms_errors); + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_UPDATE_RATE); + + if (_crc_valid) { + _sensor_state = STATE_ACK_UPDATE_RATE; perf_end(_sample_perf); - return ret; + return PX4_OK; } - if (readbuf[3] == SF_UPDATE_RATE) { - sf45_request_handle(ret, readbuf); - ScheduleDelayed(_interval * 3); - } + return -EAGAIN; - } else if (_sensor_state == STATE_SEND_DISTANCE_DATA) { + } else if (_sensor_state == STATE_ACK_UPDATE_RATE) { - ret = ::read(_fd, &readbuf[0], 8); + perf_begin(_sample_perf); + const int payload_length = 10; - if (ret < 0) { - PX4_ERR("ERROR (ack from sending distance data cmd): %d", ret); - perf_count(_comms_errors); + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_DISTANCE_OUTPUT); + + if (_crc_valid) { + _sensor_state = STATE_ACK_DISTANCE_OUTPUT; perf_end(_sample_perf); - return ret; + return PX4_OK; } - if (readbuf[3] == SF_DISTANCE_OUTPUT) { - sf45_request_handle(ret, readbuf); - ScheduleDelayed(_interval * 3); - } - - // Stream data from sensor + return -EAGAIN; } else { - ret = ::read(_fd, &readbuf[0], 10); + // Stream data from sensor + perf_begin(_sample_perf); + const int payload_length = 10; - if (ret < 0) { - PX4_ERR("ERROR (ack from streaming distance data): %d", ret); - perf_count(_comms_errors); + _crc_valid = false; + sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM); + + if (_crc_valid) { + sf45_process_replies(); perf_end(_sample_perf); - return ret; + return PX4_OK; } - uint8_t flags_payload = (readbuf[1] >> 6) | (readbuf[2] << 2); - - // Process the incoming distance data - if (readbuf[3] == SF_DISTANCE_DATA_CM && flags_payload == 5) { - - for (uint8_t i = 0; i < ret; ++i) { - sf45_request_handle(ret, readbuf); - } - - if (_init_complete) { - sf45_process_replies(&distance_m); - } // end if - - } else { - - ret = ::read(_fd, &readbuf[0], 10); - - if (ret < 0) { - PX4_ERR("ERROR (unknown sensor data): %d", ret); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - } - } - - if (_consecutive_fail_count > 35 && !_sensor_ready) { - PX4_ERR("Restarting the state machine"); - return PX4_ERROR; - } - - _last_read = hrt_absolute_time(); - - if (!_crc_valid) { return -EAGAIN; } - - PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO")); - _px4_rangefinder.update(timestamp_sample, distance_m); - - perf_end(_sample_perf); - - return PX4_OK; } void SF45LaserSerial::start() { - /* reset the report ring and state machine */ + /* reset the sensor state */ + _sensor_state = STATE_UNINIT; + + /* reset the report ring */ _collect_phase = false; + /* reset the UART receive buffer size */ + _linebuf_size = 0; + + /* reset the fail counter */ + _last_received_time = hrt_absolute_time(); + /* schedule a cycle to start things */ ScheduleNow(); } @@ -288,8 +235,7 @@ void SF45LaserSerial::Run() /* fds initialized? */ if (_fd < 0) { /* open fd: non-blocking read mode*/ - - _fd = ::open(_port, O_RDWR | O_NOCTTY); + _fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); if (_fd < 0) { PX4_ERR("serial open failed (%i)", errno); @@ -303,34 +249,11 @@ void SF45LaserSerial::Run() /* fill the struct for the new configuration */ tcgetattr(_fd, &uart_config); - uart_config.c_cflag = (uart_config.c_cflag & ~CSIZE) | CS8; + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; - uart_config.c_cflag |= (CLOCAL | CREAD); - - // no parity, 1 stop bit, flow control disabled - uart_config.c_cflag &= ~(PARENB | PARODD); - - uart_config.c_cflag |= 0; - - uart_config.c_cflag &= ~CSTOPB; - - uart_config.c_cflag &= ~CRTSCTS; - - uart_config.c_iflag &= ~IGNBRK; - - uart_config.c_iflag &= ~ICRNL; - - uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); - - // echo and echo NL off, canonical mode off (raw mode) - // extended input processing off, signal chars off - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - uart_config.c_oflag = 0; - - uart_config.c_cc[VMIN] = 0; - - uart_config.c_cc[VTIME] = 1; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); unsigned speed = B921600; @@ -346,51 +269,37 @@ void SF45LaserSerial::Run() if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { PX4_ERR("baud %d ATTR", termios_state); } - } if (_collect_phase) { - - /* perform collection */ - int collect_ret = collect(); - - - if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ - ScheduleDelayed(1042 * 8); - - return; - } - - if (OK != collect_ret) { - // Too many packet errors in init, restart the consecutive fail count - _consecutive_fail_count = 0; - - /* restart the measurement state machine */ + if (hrt_absolute_time() - _last_received_time >= 1_s) { start(); return; - - } else { - /* apparently success */ - _consecutive_fail_count = 0; } - /* next phase is measurement */ - _collect_phase = false; + /* perform collection */ + if (collect() != PX4_OK && errno != EAGAIN) { + PX4_DEBUG("collect error"); + } + + if (_sensor_state != STATE_SEND_STREAM) { + /* next phase is measurement */ + _collect_phase = false; + } + + } else { + /* measurement phase */ + + if (measure() != PX4_OK) { + PX4_DEBUG("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; } - /* measurement phase */ - - if (OK != measure()) { - PX4_DEBUG("measure error"); - } - - /* next phase is collection */ - _collect_phase = true; - /* schedule a fresh cycle call when the measurement is done */ ScheduleDelayed(_interval); - } void SF45LaserSerial::print_info() @@ -399,9 +308,8 @@ void SF45LaserSerial::print_info() perf_print_counter(_comms_errors); } -void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf) +void SF45LaserSerial::sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id) { - // SF45 protocol // Start byte is 0xAA and is the start of packet // Payload length sanity check (0-1023) bytes @@ -410,172 +318,175 @@ void SF45LaserSerial::sf45_request_handle(int return_val, uint8_t *input_buf) // ID byte precedes the data in the payload // CRC comprised of 16-bit checksum (not included in checksum calc.) - uint16_t recv_crc = 0; - bool restart_flag = false; + int ret; + size_t max_read = sizeof(_linebuf) - _linebuf_size; + ret = ::read(_fd, &_linebuf[_linebuf_size], max_read); - while (restart_flag != true) { + if (ret < 0 && errno != EAGAIN) { + PX4_ERR("ERROR (ack from streaming distance data): %d", ret); + _linebuf_size = 0; + perf_count(_comms_errors); + perf_end(_sample_perf); + return; + } - switch (_parsed_state) { - case 0: { - if (input_buf[0] == 0xAA) { - // start of frame is valid, continue - _sop_valid = true; - _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); - _parsed_state = 1; - break; + if (ret > 0) { + _last_received_time = hrt_absolute_time(); + _linebuf_size += ret; + } - } else { - _sop_valid = false; - _crc_valid = false; - _parsed_state = 0; - restart_flag = true; - _calc_crc = 0; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("Start of packet not valid: %d", _sensor_state); - _consecutive_fail_count++; - break; - } // end else - } // end case 0 + // Not enough data to parse a complete packet. Gather more data in the next cycle. + if (_linebuf_size < payload_length) { + return; + } - case 1: { - rx_field.flags_lo = input_buf[1]; - _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); - _parsed_state = 2; - break; - } + int index = 0; - case 2: { - rx_field.flags_hi = input_buf[2]; - rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); - _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); + while (index <= _linebuf_size - payload_length && _crc_valid == false) { + bool restart_flag = false; - // Check payload length against known max value - if (rx_field.data_len > 17) { - _parsed_state = 0; - restart_flag = true; - _calc_crc = 0; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("Payload length error: %d", _sensor_state); - _consecutive_fail_count++; - break; - - } else { - _parsed_state = 3; - break; - } - } - - case 3: { - - rx_field.msg_id = input_buf[3]; - - if (rx_field.msg_id == SF_PRODUCT_NAME || rx_field.msg_id == SF_UPDATE_RATE || rx_field.msg_id == SF_DISTANCE_OUTPUT - || rx_field.msg_id == SF_STREAM || rx_field.msg_id == SF_DISTANCE_DATA_CM) { - - if (rx_field.msg_id == SF_DISTANCE_DATA_CM && rx_field.data_len > 1) { - _sensor_ready = true; + while (restart_flag != true) { + switch (_parsed_state) { + case SF45_PARSED_STATE::START: { + if (_linebuf[index] == 0xAA) { + // start of frame is valid, continue + _sop_valid = true; + _calc_crc = sf45_format_crc(_calc_crc, _start_of_frame); + _parsed_state = SF45_PARSED_STATE::FLG_LOW; + break; } else { - _sensor_ready = false; + _sop_valid = false; + _crc_valid = false; + _parsed_state = SF45_PARSED_STATE::START; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Start of packet not valid: %d", _sensor_state); + break; + } + } + + case SF45_PARSED_STATE::FLG_LOW: { + rx_field.flags_lo = _linebuf[index + 1]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_lo); + _parsed_state = SF45_PARSED_STATE::FLG_HIGH; + break; + } + + case SF45_PARSED_STATE::FLG_HIGH: { + rx_field.flags_hi = _linebuf[index + 2]; + rx_field.data_len = (rx_field.flags_hi << 2) | (rx_field.flags_lo >> 6); + _calc_crc = sf45_format_crc(_calc_crc, rx_field.flags_hi); + + // Check payload length against known max value + if (rx_field.data_len > 17) { + _parsed_state = SF45_PARSED_STATE::START; + restart_flag = true; + _calc_crc = 0; + PX4_DEBUG("Payload length error: %d", _sensor_state); + break; + + } else { + _parsed_state = SF45_PARSED_STATE::ID; + break; + } + } + + case SF45_PARSED_STATE::ID: { + rx_field.msg_id = _linebuf[index + 3]; + + if (rx_field.msg_id == msg_id) { + _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); + _parsed_state = SF45_PARSED_STATE::DATA; + break; } - _calc_crc = sf45_format_crc(_calc_crc, rx_field.msg_id); - - _parsed_state = 4; - break; + // Ignore message ID's that aren't searched + else { + _parsed_state = SF45_PARSED_STATE::START; + _calc_crc = 0; + restart_flag = true; + PX4_DEBUG("Non needed message ID: %d", _sensor_state); + break; + } } - // Ignore message ID's that aren't defined - else { - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - perf_count(_comms_errors); - perf_end(_sample_perf); - _consecutive_fail_count++; - PX4_DEBUG("Unknown message ID: %d", _sensor_state); - break; + case SF45_PARSED_STATE::DATA: { + // Process commands with & w/out data bytes + if (rx_field.data_len > 1) { + for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { - } - } + rx_field.data[_data_bytes_recv] = _linebuf[index + i]; + _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); + _data_bytes_recv = _data_bytes_recv + 1; - // Data - case 4: { - // Process commands with & w/out data bytes - if (rx_field.data_len > 1) { - for (uint8_t i = 4; i < 3 + rx_field.data_len; ++i) { + } + } - rx_field.data[_data_bytes_recv] = input_buf[i]; - _calc_crc = sf45_format_crc(_calc_crc, rx_field.data[_data_bytes_recv]); - _data_bytes_recv = _data_bytes_recv + 1; + else { - } // end for - } //end if + _parsed_state = SF45_PARSED_STATE::CRC_LOW; + _data_bytes_recv = 0; + _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); + } - else { - - _parsed_state = 5; + _parsed_state = SF45_PARSED_STATE::CRC_LOW; _data_bytes_recv = 0; - _calc_crc = sf45_format_crc(_calc_crc, _data_bytes_recv); - + break; } - _parsed_state = 5; - _data_bytes_recv = 0; - break; - } + case SF45_PARSED_STATE::CRC_LOW: { + rx_field.crc[0] = _linebuf[index + 3 + rx_field.data_len]; + _parsed_state = SF45_PARSED_STATE::CRC_HIGH; + break; + } - // CRC low byte - case 5: { - rx_field.crc[0] = input_buf[3 + rx_field.data_len]; - _parsed_state = 6; - break; - } + case SF45_PARSED_STATE::CRC_HIGH: { + rx_field.crc[1] = _linebuf[index + 4 + rx_field.data_len]; + uint16_t recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; - // CRC high byte - case 6: { - rx_field.crc[1] = input_buf[4 + rx_field.data_len]; - recv_crc = (rx_field.crc[1] << 8) | rx_field.crc[0]; - - // Check the received crc bytes from the sf45 against our own CRC calcuation - // If it matches, we can check if sensor ready - // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete - if (recv_crc == _calc_crc) { - _crc_valid = true; - - // Sensor is ready if we read msg ID 44: SF_DISTANCE_DATA_CM - if (_sensor_ready) { - _init_complete = true; + // Check the received crc bytes from the sf45 against our own CRC calcuation + // If it matches, we can check if sensor ready + // Only if crc match is valid and sensor ready (transmitting distance data) do we flag _init_complete + if (recv_crc == _calc_crc) { + _crc_valid = true; + _parsed_state = SF45_PARSED_STATE::START; + _calc_crc = 0; + restart_flag = true; + break; } else { - _init_complete = false; + + _crc_valid = false; + _parsed_state = SF45_PARSED_STATE::START; + _calc_crc = 0; + restart_flag = true; + perf_count(_comms_errors); + PX4_DEBUG("CRC mismatch: %d", _sensor_state); + break; } - - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - break; - - } else { - - _crc_valid = false; - _init_complete = false; - _parsed_state = 0; - _calc_crc = 0; - restart_flag = true; - perf_count(_comms_errors); - perf_end(_sample_perf); - PX4_DEBUG("CRC mismatch: %d", _sensor_state); - break; } } - } // end switch - } //end while + } + + index++; + } + + // If we parsed successfully, remove the parsed part from the buffer if it is still large enough + if (_crc_valid && index + payload_length < _linebuf_size) { + unsigned next_after_index = index + payload_length; + memmove(&_linebuf[0], &_linebuf[next_after_index], _linebuf_size - next_after_index); + _linebuf_size -= next_after_index; + } + + // The buffer is filled. Either we can't keep up with the stream and/or it contains only invalid data. Reset to try again. + if ((unsigned)_linebuf_size >= sizeof(_linebuf)) { + _linebuf_size = 0; + perf_count(_comms_errors); + } } -void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t data_len) +void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8_t data_len) { uint16_t crc_val = 0; uint8_t packet_buff[SF45_MAX_PAYLOAD]; @@ -612,7 +523,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d if (msg_id == SF_DISTANCE_OUTPUT) { uint8_t data_convert = data[0] & 0x00FF; // write data bytes to the output buffer - packet_buff[data_inc] = data_convert; + packet_buff[data_inc] = data_convert; // Add data bytes to crc add function crc_val = sf45_format_crc(crc_val, data_convert); data_inc = data_inc + 1; @@ -650,12 +561,11 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d // Add data bytes to crc add function crc_val = sf45_format_crc(crc_val, data[0]); data_inc = data_inc + 1; - } else { // Product Name - PX4_INFO("INFO: Product name"); + PX4_DEBUG("DEBUG: Product name"); } uint8_t crc_lo = crc_val & 0xFF; @@ -683,77 +593,71 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int *data, uint8_t d } } -void SF45LaserSerial::sf45_process_replies(float *distance_m) +void SF45LaserSerial::sf45_process_replies() { + const int16_t YAW_THRESHOLD = 32000; + const int16_t YAW_ADJUSTMENT = 65535; + switch (rx_field.msg_id) { case SF_DISTANCE_DATA_CM: { - - uint16_t obstacle_dist_cm = 0; const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); - int16_t scaled_yaw = 0; // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float - if (raw_yaw > 32000) { - raw_yaw = raw_yaw - 65535; - + if (raw_yaw > YAW_THRESHOLD) { + raw_yaw -= YAW_ADJUSTMENT; } - // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle - if (_orient_cfg == 1) { - raw_yaw = raw_yaw * -1; + // Adjust yaw for downward facing sensor + if (_orient_cfg == ROTATION_DOWNWARD_FACING) { + raw_yaw = -raw_yaw; } // SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}" - scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; + float scaled_yaw_sensor_frame = raw_yaw * SF45_SCALE_FACTOR; + float scaled_yaw_frd = ObstacleMath::wrap_360(scaled_yaw_sensor_frame + _obstacle_distance.angle_offset); + float distance_m = raw_distance * SF45_SCALE_FACTOR; - switch (_yaw_cfg) { - case 0: - break; + // Update the current bin distance + _current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist; - case 1: - if (scaled_yaw > 180) { - scaled_yaw = scaled_yaw - 180; - - } else { - scaled_yaw = scaled_yaw + 180; // rotation facing aft - } - - break; - - case 2: - scaled_yaw = scaled_yaw + 90; // rotation facing right - break; - - case 3: - scaled_yaw = scaled_yaw - 90; // rotation facing left - break; - - default: - break; - } - - // Convert to meters for rangefinder update - *distance_m = raw_distance * SF45_SCALE_FACTOR; - obstacle_dist_cm = (uint16_t)raw_distance; - - uint8_t current_bin = sf45_convert_angle(scaled_yaw); - - // If we have moved to a new bin + // Find bin index for the current sensor yaw angle (in sensor frame) + const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame); if (current_bin != _previous_bin) { + PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw_frd, current_bin, + (double)distance_m); - // update the current bin to the distance sensor reading - // readings in cm - _obstacle_map_msg.distances[current_bin] = obstacle_dist_cm; - _obstacle_map_msg.timestamp = hrt_absolute_time(); + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = matrix::Quatf(vehicle_attitude.q); + } + } + + // Scale distance with vehicle rotation + float current_bin_dist = static_cast(_current_bin_dist); + float scaled_yaw_frd_rad = math::radians(static_cast(scaled_yaw_frd)); + ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_frd_rad, _vehicle_attitude); + _current_bin_dist = static_cast(current_bin_dist); + + if (_current_bin_dist > _obstacle_distance.max_distance) { + _current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition + } + + hrt_abstime now = hrt_absolute_time(); + + _obstacle_distance.distances[current_bin] = _current_bin_dist; + _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); + + _publish_obstacle_msg(now); + + // reset the values for the next measurement + _current_bin_dist = UINT16_MAX; + _previous_bin = current_bin; } - _previous_bin = current_bin; - - _obstacle_distance_pub.publish(_obstacle_map_msg); - break; } @@ -762,20 +666,43 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) break; } } - -uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) +void SF45LaserSerial::_publish_obstacle_msg(hrt_abstime now) { + // This whole logic is in place because CollisionPrevention, just reads in the last published message on the topic, and not every message, + // This can result in messages being misssed if the sensor is publishing at a faster rate than the CollisionPrevention module is reading. + for (uint8_t i = 0; i < BIN_COUNT; i++) { + if (now - _data_timestamps[i] > SF45_MEAS_TIMEOUT) { + // If the data is older than SF45_MEAS_TIMEOUT, we discard the value (UINT16_MAX means no valid data) + _obstacle_distance.distances[i] = UINT16_MAX; + } + } - uint8_t mapped_sector = 0; - float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset); - mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment); - - return mapped_sector; + _obstacle_distance.timestamp = now; + _obstacle_distance_pub.publish(_obstacle_distance); } -float SF45LaserSerial::sf45_wrap_360(float f) +void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, + hrt_abstime now) { - return matrix::wrap(f, 0.f, 360.f); + // if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement. + // in this case we assume the measurement to be valid for all bins between the previous and the current bin. + + // Shift bin indices such that we can never have the wrap-around case. + const float fov_offset_angle = 360.0f - SF45_FIELDOF_VIEW / 2.f; + const uint16_t current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, _obstacle_distance.increment, + fov_offset_angle); + const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment, + fov_offset_angle); + + const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1; + const uint16_t end = math::max(current_bin_offset, previous_bin_offset); + + // populate the missed bins with the measurement + for (uint16_t i = start; i < end; i++) { + uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle); + _obstacle_distance.distances[bin_index] = measurement; + _data_timestamps[bin_index] = now; + } } uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index 89e7367192..313b9e60f7 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -49,83 +49,101 @@ #include #include +#include #include -#include +#include #include "sf45_commands.h" enum SF_SERIAL_STATE { STATE_UNINIT = 0, - STATE_SEND_PRODUCT_NAME = 1, - STATE_SEND_UPDATE_RATE = 2, - STATE_SEND_DISTANCE_DATA = 3, + STATE_ACK_PRODUCT_NAME = 1, + STATE_ACK_UPDATE_RATE = 2, + STATE_ACK_DISTANCE_OUTPUT = 3, STATE_SEND_STREAM = 4, }; +enum SF45_PARSED_STATE { + START = 0, + FLG_LOW, + FLG_HIGH, + ID, + DATA, + CRC_LOW, + CRC_HIGH +}; +enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90 + ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270 +}; + +using namespace time_literals; class SF45LaserSerial : public px4::ScheduledWorkItem { public: - SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + SF45LaserSerial(const char *port); ~SF45LaserSerial() override; - int init(); + int init(); void print_info(); - void sf45_request_handle(int val, uint8_t *value); - void sf45_send(uint8_t msg_id, bool r_w, int *data, uint8_t data_len); - uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); - void sf45_process_replies(float *data); - uint8_t sf45_convert_angle(const int16_t yaw); - float sf45_wrap_360(float f); -protected: - obstacle_distance_s _obstacle_map_msg{}; - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ + void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id); + void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); + uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); + void sf45_process_replies(); private: + obstacle_distance_s _obstacle_distance{}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; /**< obstacle_distance publication */ + static constexpr uint8_t BIN_COUNT = sizeof(obstacle_distance_s::distances) / sizeof( + obstacle_distance_s::distances[0]); + static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms}; + static constexpr float SF45_SCALE_FACTOR = 0.01f; + static constexpr float SF45_FIELDOF_VIEW = 320.f; // degrees void start(); void stop(); void Run() override; int measure(); int collect(); - bool _crc_valid{false}; - PX4Rangefinder _px4_rangefinder; + bool _crc_valid{false}; + + void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now); + void _publish_obstacle_msg(hrt_abstime now); + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uint64_t _data_timestamps[BIN_COUNT]; + char _port[20] {}; - int _interval{10000}; + int _interval{2000}; bool _collect_phase{false}; int _fd{-1}; - int _linebuf[256] {}; - unsigned _linebuf_index{0}; - hrt_abstime _last_read{0}; + uint8_t _linebuf[SF45_MAX_PAYLOAD] {}; + int _linebuf_size{0}; // SF45/B uses a binary protocol to include header,flags // message ID, payload, and checksum - bool _is_sf45{false}; - bool _init_complete{false}; - bool _sensor_ready{false}; - uint8_t _sensor_state{0}; - int _baud_rate{0}; - int _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int _stream_data{0}; - int32_t _update_rate{1}; - int _data_output{0}; - const uint8_t _start_of_frame{0xAA}; - uint16_t _data_bytes_recv{0}; - uint8_t _parsed_state{0}; - bool _sop_valid{false}; - uint16_t _calc_crc{0}; - uint8_t _num_retries{0}; - int32_t _yaw_cfg{0}; - int32_t _orient_cfg{0}; - int32_t _collision_constraint{0}; - uint16_t _previous_bin{0}; + bool _is_sf45{false}; + SF_SERIAL_STATE _sensor_state{STATE_UNINIT}; + int _baud_rate{0}; + int32_t _product_name[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + int32_t _stream_data{0}; + int32_t _update_rate{0}; + int32_t _data_output{0}; + const uint8_t _start_of_frame{0xAA}; + uint16_t _data_bytes_recv{0}; + uint8_t _parsed_state{0}; + bool _sop_valid{false}; + uint16_t _calc_crc{0}; + int32_t _yaw_cfg{0}; + int32_t _orient_cfg{0}; + uint8_t _previous_bin{0}; + uint16_t _current_bin_dist{UINT16_MAX}; + matrix::Quatf _vehicle_attitude{}; // end of SF45/B data members - unsigned _consecutive_fail_count; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - + hrt_abstime _last_received_time{0}; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; }; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp index f05417e053..4f6c0a814b 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp @@ -41,7 +41,7 @@ namespace lightware_sf45 SF45LaserSerial *g_dev{nullptr}; -static int start(const char *port, uint8_t rotation) +static int start(const char *port) { if (g_dev != nullptr) { PX4_WARN("already started"); @@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation) } /* create the driver */ - g_dev = new SF45LaserSerial(port, rotation); + g_dev = new SF45LaserSerial(port); if (g_dev == nullptr) { return -1; @@ -102,7 +102,6 @@ static int usage() Serial bus driver for the Lightware SF45/b Laser rangefinder. -Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html ### Examples @@ -116,7 +115,6 @@ $ lightware_sf45_serial stop PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false); PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); return PX4_OK; } @@ -125,18 +123,13 @@ $ lightware_sf45_serial stop extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) { - uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING; const char *device_path = nullptr; int ch; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - break; - case 'd': device_path = myoptarg; break; @@ -153,7 +146,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[]) } if (!strcmp(argv[myoptind], "start")) { - return lightware_sf45::start(device_path, rotation); + return lightware_sf45::start(device_path); } else if (!strcmp(argv[myoptind], "stop")) { return lightware_sf45::stop(); diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml index 6356411fbe..36eddddfa0 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_sf45_serial/module.yaml @@ -1,6 +1,6 @@ module_name: Lightware SF45 Rangefinder (serial) serial_config: - - command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV} + - command: lightware_sf45_serial start -d ${SERIAL_DEV} port_config_param: name: SENS_EN_SF45_CFG group: Sensors @@ -32,7 +32,7 @@ parameters: 12: 5000hz reboot_required: true num_instances: 1 - default: 1 + default: 5 SF45_ORIENT_CFG: description: @@ -41,11 +41,11 @@ parameters: The SF45 mounted facing upward or downward on the frame type: enum values: - 0: Rotation upward - 1: Rotation downward + 24: Rotation upward + 25: Rotation downward reboot_required: true num_instances: 1 - default: 0 + default: 24 SF45_YAW_CFG: description: @@ -55,9 +55,9 @@ parameters: type: enum values: 0: Rotation forward - 1: Rotation backward 2: Rotation right - 3: Rotation left + 4: Rotation backward + 6: Rotation left reboot_required: true num_instances: 1 default: 0 diff --git a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index 1ef65a5ca3..8e94a41710 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp @@ -109,7 +109,7 @@ LidarLitePWM::measure() return PX4_ERROR; } - const float current_distance = float(_pwm.pulse_width) * 1e-3f; /* 10 usec = 1 cm distance for LIDAR-Lite */ + const float current_distance = static_cast(_pwm.pulse_width) * 1e-3f; // 1us = 1mm distance for LIDAR-Lite /* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */ if (current_distance <= 0.0f) { diff --git a/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp b/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp index 25219f6a7d..d8898fbb68 100644 --- a/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp @@ -102,7 +102,7 @@ static int usage() Serial bus driver for the Aerotenna uLanding radar. -Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/ulanding_radar.html +Setup/usage information: https://docs.px4.io/main/en/sensor/ulanding_radar.html ### Examples diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 9c36dfd52d..431a56787d 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -88,7 +88,7 @@ typedef enum { * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. * This allows some of the channels to remain configured * as GPIOs or as another function. Already used channels/timers will not be configured as DShot - * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600 * @return <0 on error, the initialized channels mask. */ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot); diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 904d63aeba..8b33acb0f1 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -247,6 +247,7 @@ #define DRV_DIFF_PRESS_DEVTYPE_AUAV 0xE6 #define DRV_BARO_DEVTYPE_AUAV 0xE7 +#define DRV_BARO_DEVTYPE_SPA06 0xE8 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 940019d9bf..4bd9bdef19 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -125,9 +125,6 @@ void DShot::enable_dshot_outputs(const bool enabled) } else if (tim_config == -3) { dshot_frequency_request = DSHOT600; - } else if (tim_config == -2) { - dshot_frequency_request = DSHOT1200; - } else { _output_mask &= ~channels; // don't use for dshot } @@ -824,7 +821,7 @@ On startup, the module tries to occupy all available pins for DShot output. It skips all pins already in use (e.g. by a camera trigger module). It supports: -- DShot150, DShot300, DShot600, DShot1200 +- DShot150, DShot300, DShot600 - telemetry via separate UART and publishing as esc_status message - sending DShot commands via CLI diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index e60b33b862..4fc312c3e9 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -52,7 +52,6 @@ using namespace time_literals; static constexpr unsigned int DSHOT150 = 150000u; static constexpr unsigned int DSHOT300 = 300000u; static constexpr unsigned int DSHOT600 = 600000u; -static constexpr unsigned int DSHOT1200 = 1200000u; static constexpr int DSHOT_DISARM_VALUE = 0; static constexpr int DSHOT_MIN_THROTTLE = 1; @@ -107,7 +106,6 @@ private: DShot150 = 150, DShot300 = 300, DShot600 = 600, - DShot1200 = 1200, }; struct Command { diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 5c53db6098..9bd5a8bb05 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -1154,34 +1154,36 @@ int SeptentrioDriver::process_message() _message_gps_state.time_utc_usec = 0; #ifndef __PX4_QURT // NOTE: Functionality isn't available on Snapdragon yet. - struct tm timeinfo; - time_t epoch; + if (_time_synced) { + struct tm timeinfo; + time_t epoch; - // Convert to unix timestamp - memset(&timeinfo, 0, sizeof(timeinfo)); + // Convert to unix timestamp + memset(&timeinfo, 0, sizeof(timeinfo)); - timeinfo.tm_year = 1980 - 1900; - timeinfo.tm_mon = 0; - timeinfo.tm_mday = 6 + header.wnc * 7; - timeinfo.tm_hour = 0; - timeinfo.tm_min = 0; - timeinfo.tm_sec = header.tow / 1000; + timeinfo.tm_year = 1980 - 1900; + timeinfo.tm_mon = 0; + timeinfo.tm_mday = 6 + header.wnc * 7; + timeinfo.tm_hour = 0; + timeinfo.tm_min = 0; + timeinfo.tm_sec = header.tow / 1000; - epoch = mktime(&timeinfo); + epoch = mktime(&timeinfo); - if (epoch > k_gps_epoch_secs) { - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + if (epoch > k_gps_epoch_secs) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. - timespec ts; - memset(&ts, 0, sizeof(ts)); - ts.tv_sec = epoch; - ts.tv_nsec = (header.tow % 1000) * 1000 * 1000; - set_clock(ts); + timespec ts; + memset(&ts, 0, sizeof(ts)); + ts.tv_sec = epoch; + ts.tv_nsec = (header.tow % 1000) * 1000 * 1000; + set_clock(ts); - _message_gps_state.time_utc_usec = static_cast(epoch) * 1000000ULL; - _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + _message_gps_state.time_utc_usec = static_cast(epoch) * 1000000ULL; + _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + } } #endif @@ -1199,6 +1201,7 @@ int SeptentrioDriver::process_message() if (_sbf_decoder.parse(&receiver_status) == PX4_OK) { _message_gps_state.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED; + _time_synced = receiver_status.rx_state_wn_set && receiver_status.rx_state_tow_set; } break; diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index ec203cd861..d4b0971bbe 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -712,6 +712,7 @@ private: uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections uint8_t _spoofing_state {0}; ///< Receiver spoofing state uint8_t _jamming_state {0}; ///< Receiver jamming state + bool _time_synced {false}; ///< Receiver time in sync with GPS time const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls uint32_t _chosen_baud_rate {0}; ///< The baud rate requested by the user static px4::atomic _secondary_instance; diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index fb151bcaa2..bbdd5767a9 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit fb151bcaa2690b56d4b16aa8de9fe2448f637c3b +Subproject commit bbdd5767a961cc17bdcc577c79b1c708d2a7999a diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 004be4cc8b..ba6f24070f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -476,6 +476,10 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) if (_interface == GPSHelper::Interface::UART) { ret = _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted); + if (ret > 0) { + _num_bytes_read += ret; + } + // SPI is only supported on LInux #if defined(__PX4_LINUX) @@ -543,8 +547,13 @@ void GPS::handleInjectDataTopic() for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { const bool exists = _orb_inject_data_sub[instance].advertised(); - if (exists) { - if (_orb_inject_data_sub[instance].copy(&msg)) { + if (exists && _orb_inject_data_sub[instance].copy(&msg)) { + /* Don't select the own RTCM instance. In case it has a lower + * instance number, it will be selected and will be rejected + * later in the code, resulting in no RTCM injection at all. + */ + if (msg.device_id != get_device_id()) { + // Only use the message if it is up to date if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp index ccf5f40d3a..4d0ef6a8c1 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp @@ -124,8 +124,13 @@ private: { Register::FIFO_WTM_1, 0, 0 }, { Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 }, { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 }, +# if defined(CONFIG_BMI088_ACCELEROMETER_INT1) { Register::INT1_IO_CONF, INT1_IO_CONF_BIT::int1_out, 0 }, { Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0}, +# elif defined(CONFIG_BMI088_ACCELEROMETER_INT2) + { Register::INT2_IO_CONF, INT2_IO_CONF_BIT::int2_out, 0 }, + { Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int2_fwm, 0}, +# endif }; }; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp index 59d847bf02..8ccec5c00f 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp @@ -113,11 +113,16 @@ private: { Register::GYRO_RANGE, GYRO_RANGE_BIT::gyro_range_2000_dps, 0 }, { Register::GYRO_BANDWIDTH, 0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz }, { Register::GYRO_INT_CTRL, GYRO_INT_CTRL_BIT::fifo_en, 0 }, - { Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl }, - { Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 }, { Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 }, { Register::FIFO_CONFIG_0, 0, 0 }, // fifo_water_mark_level_trigger_retain<6:0> { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::FIFO_MODE, 0 }, +#if defined(CONFIG_BMI088_GYROSCOPE_INT3) + { Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl }, + { Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 }, +# elif defined(CONFIG_BMI088_GYROSCOPE_INT4) + { Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int4_od | INT3_INT4_IO_CONF_BIT::Int4_lvl }, + { Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int4_fifo, 0 }, +# endif }; }; diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp index 00b8ffe6d6..4543ccb091 100644 --- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp +++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp @@ -72,6 +72,7 @@ enum class Register : uint8_t { FIFO_CONFIG_1 = 0x49, INT1_IO_CONF = 0x53, + INT2_IO_CONF = 0x54, INT1_INT2_MAP_DATA = 0x58, @@ -113,10 +114,18 @@ enum FIFO_CONFIG_1_BIT : uint8_t { enum INT1_IO_CONF_BIT : uint8_t { int1_in = Bit4, int1_out = Bit3, - + int1_od = Bit2, int1_lvl = Bit1, }; +// INT2_IO_CONF +enum INT2_IO_CONF_BIT : uint8_t { + int2_io = Bit4, + int2_out = Bit3, + int2_od = Bit2, + int2_lvl = Bit1, +}; + // INT1_INT2_MAP_DATA enum INT1_INT2_MAP_DATA_BIT : uint8_t { int2_drdy = Bit6, diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp index 0f1974f2cb..d17a249390 100644 --- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp +++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp @@ -98,6 +98,8 @@ enum GYRO_INT_CTRL_BIT : uint8_t { // INT3_INT4_IO_CONF enum INT3_INT4_IO_CONF_BIT : uint8_t { + Int4_od = Bit3, // '0' Push-pull + Int4_lvl = Bit2, // '0' Active low Int3_od = Bit1, // ‘0’ Push-pull Int3_lvl = Bit0, // ‘0’ Active low }; diff --git a/src/drivers/imu/bosch/bmi088/Kconfig b/src/drivers/imu/bosch/bmi088/Kconfig index 6985ccb7eb..b273a44d84 100644 --- a/src/drivers/imu/bosch/bmi088/Kconfig +++ b/src/drivers/imu/bosch/bmi088/Kconfig @@ -3,3 +3,47 @@ menuconfig DRIVERS_IMU_BOSCH_BMI088 default n ---help--- Enable support for bosch bmi088 + +if DRIVERS_IMU_BOSCH_BMI088 + +choice + prompt "BMI088 accelerometer interrupt pin" + default BMI088_ACCELEROMETER_INT1 + +config BMI088_ACCELEROMETER_INT_NONE + bool "No interrupts" + ---help--- + No pin on the BMI088 will output accelerometer interrupts + +config BMI088_ACCELEROMETER_INT1 + bool "INT1" + ---help--- + Use INT1 pin of the BMI088 to output accelerometer interrupts + +config BMI088_ACCELEROMETER_INT2 + bool "INT2" + ---help--- + Use INT2 pin of the BMI088 to output accelerometer interrupts +endchoice + +choice + prompt "BMI088 gyroscope interrupt pin" + default BMI088_GYROSCOPE_INT3 + +config BMI088_GYROSCOPE_INT_NONE + bool "No interrupts" + ---help--- + No pin on the BMI088 will output gyroscope interrupts + +config BMI088_GYROSCOPE_INT3 + bool "INT3" + ---help--- + Use INT3 pin of the BMI088 to output gyroscope interrupts + +config BMI088_GYROSCOPE_INT4 + bool "INT4" + ---help--- + Use INT4 pin of the BMI088 to output gyroscope interrupts +endchoice + +endif diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 85dd4c1846..e4e9821d47 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -328,7 +328,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet) local_position.vxy_max = INFINITY; local_position.vz_max = INFINITY; local_position.hagl_min = INFINITY; - local_position.hagl_max = INFINITY; + local_position.hagl_max_z = INFINITY; + local_position.hagl_max_xy = INFINITY; local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); diff --git a/src/drivers/lights/rgbled_pwm/CMakeLists.txt b/src/drivers/lights/rgbled_pwm/CMakeLists.txt index 2001cdc492..6fc41d0301 100644 --- a/src/drivers/lights/rgbled_pwm/CMakeLists.txt +++ b/src/drivers/lights/rgbled_pwm/CMakeLists.txt @@ -41,3 +41,8 @@ px4_add_module( arch_io_pins arch_led_pwm ) + +target_link_libraries(drivers__rgbled_pwm + PRIVATE + drivers_board # Allows board to override PWM functions +) diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index 14573ccaa0..1817cbe651 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -78,7 +78,7 @@ bool AK09916::Reset() void AK09916::print_status() { I2CSPIDriverBase::print_status(); - PX4_INFO("Variant: %s", _is_ak09918 ? "AK09918" : "AK09916"); + PX4_INFO("Variant: %s", device_name()); perf_print_counter(_reset_perf); perf_print_counter(_bad_register_perf); @@ -94,23 +94,28 @@ int AK09916::probe() const uint8_t WIA1 = RegisterRead(Register::WIA1); const uint8_t WIA2 = RegisterRead(Register::WIA2); - if ((WIA1 == Company_ID) && (WIA2 == Device_ID)) { - _is_ak09918 = false; - return PX4_OK; - } - - if ((WIA1 == Company_ID) && (WIA2 == Device_ID_AK09918)) { - _is_ak09918 = true; - return PX4_OK; - } - - if (WIA1 != Company_ID) { + if ((WIA1 != Company_ID)) { PX4_DEBUG("unexpected WIA1 0x%02x", WIA1); + return PX4_ERROR; } - if (WIA2 != Device_ID && WIA2 != Device_ID_AK09918) { + switch (static_cast(WIA2)) { + case AKTYPE::AK09916: + _device = AKTYPE::AK09916; + return PX4_OK; + + case AKTYPE::AK09915: + _device = AKTYPE::AK09915; + return PX4_OK; + + case AKTYPE::AK09918: + _device = AKTYPE::AK09918; + return PX4_OK; + + default: PX4_DEBUG("unexpected WIA2 0x%02x", WIA2); - } + + }; } return PX4_ERROR; @@ -132,9 +137,7 @@ void AK09916::RunImpl() break; case STATE::WAIT_FOR_RESET: { - uint8_t device_id = _is_ak09918 ? Device_ID_AK09918 : Device_ID; - - if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == device_id)) { + if ((RegisterRead(Register::WIA1) == Company_ID) && (static_cast(RegisterRead(Register::WIA2)) == _device)) { // if reset succeeded then configure _state = STATE::CONFIGURE; ScheduleDelayed(100_ms); diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp index 49c92f445f..6fa731a404 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp @@ -64,6 +64,30 @@ public: void print_status() override; private: + enum class AKTYPE : uint8_t { + AK09916 = 0X09, + AK09915 = 0X10, + AK09918 = 0x0c, + }; + + constexpr char const *device_name() + { + switch (_device) { + case AKTYPE::AK09916: + return "AK09916"; + + case AKTYPE::AK09915: + return "AK09915"; + + case AKTYPE::AK09918: + return "AK09918"; + + default: + return "Unknown"; + + }; + } + struct register_config_t { Register reg; uint8_t set_bits{0}; @@ -107,5 +131,5 @@ private: { Register::CNTL2, CNTL2_BIT::MODE3_SET, CNTL2_BIT::MODE3_CLEAR }, }; - bool _is_ak09918 {false}; + AKTYPE _device; }; diff --git a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp index 0096e65a09..c408463db4 100644 --- a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp @@ -59,8 +59,6 @@ static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interfac static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100; static constexpr uint8_t Company_ID = 0x48; -static constexpr uint8_t Device_ID = 0x09; -static constexpr uint8_t Device_ID_AK09918 = 0x0C; enum class Register : uint8_t { WIA1 = 0x00, // Company ID of AKM diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt b/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt index 3bba9619d7..085faacfeb 100644 --- a/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt +++ b/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_module( COMPILE_FLAGS SRCS mmc5983ma_i2c.cpp + mmc5983ma_spi.cpp mmc5983ma_main.cpp mmc5983ma.cpp DEPENDS diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h index afa526dd1a..c29131f125 100644 --- a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h @@ -54,6 +54,7 @@ #define MMC5983MA_CTRL_REG1_SW_RESET (1 << 7) extern device::Device *MMC5983MA_I2C_interface(const I2CSPIDriverConfig &config); +extern device::Device *MMC5983MA_SPI_interface(const I2CSPIDriverConfig &config); class MMC5983MA : public I2CSPIDriver { diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp index b8d6e81e34..02efb0c039 100644 --- a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp @@ -36,7 +36,14 @@ I2CSPIDriverBase *MMC5983MA::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { - device::Device *interface = MMC5983MA_I2C_interface(config); + device::Device *interface = nullptr; + + if (config.bus_type == BOARD_I2C_BUS) { + interface = MMC5983MA_I2C_interface(config); + + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = MMC5983MA_SPI_interface(config); + } if (interface == nullptr) { PX4_ERR("alloc failed"); @@ -69,8 +76,9 @@ void MMC5983MA::print_usage() PRINT_MODULE_USAGE_NAME("mmc5983ma", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_COMMAND("reset"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -79,9 +87,10 @@ extern "C" int mmc5983ma_main(int argc, char *argv[]) { using ThisDriver = MMC5983MA; int ch; - BusCLIArguments cli{true, false}; + BusCLIArguments cli{true, true}; cli.i2c_address = 0x30; cli.default_i2c_frequency = 400000; + cli.default_spi_frequency = 10 * 1000 * 1000; while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_spi.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_spi.cpp new file mode 100644 index 0000000000..f3587193f5 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_spi.cpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 "mmc5983ma.h" +#include + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + + +class MMC5983MA_SPI : public device::SPI +{ +public: + MMC5983MA_SPI(const I2CSPIDriverConfig &config); + virtual ~MMC5983MA_SPI() = default; + + virtual int read(unsigned address, void *data, unsigned count) override; + virtual int write(unsigned address, void *data, unsigned count) override; + +protected: + virtual int probe(); +}; + +MMC5983MA_SPI::MMC5983MA_SPI(const I2CSPIDriverConfig &config) : + SPI(config) +{ +} + +int MMC5983MA_SPI::probe() +{ + uint8_t data = 0; + + if (read(MMC5983MA_ADDR_PRODUCT_ID, &data, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + if (data != MMC5983MA_PRODUCT_ID) { + DEVICE_DEBUG("MMC5983MA bad ID: %02x", data); + return -EIO; + } + + return OK; +} + +int MMC5983MA_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +int MMC5983MA_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = (uint8_t)(address | DIR_WRITE); + memcpy(&buf[1], data, count); + + return transfer(&buf[0], nullptr, count + 1); +} + +device::Device *MMC5983MA_SPI_interface(const I2CSPIDriverConfig &config) +{ + return new MMC5983MA_SPI(config); +} diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 7be449157d..36adc16e8e 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -37,10 +37,11 @@ */ // includes for mathematical manipulation -#include -#include +#include #include +#include #include +#include // clock access #include diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp index 3667ee56d4..305d1a61a2 100644 --- a/src/drivers/power_monitor/ina220/ina220.cpp +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -56,7 +56,7 @@ INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) : { float fvalue = MAX_CURRENT; _max_current = fvalue; - param_t ph = (_ch_type == PM_CH_TYPE_VBATT) ? param_find("INA220_CURRENT_BAT") : param_find("INA220_CURRENT_REG"); + param_t ph = (_ch_type == PM_CH_TYPE_VBATT) ? param_find("INA220_CUR_BAT") : param_find("INA220_CUR_REG"); if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 42f5e12858..b46b277a84 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -160,6 +160,8 @@ int INA238::Reset() int ret = PX4_ERROR; + _retries = 3; + if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { return ret; } @@ -231,6 +233,16 @@ int INA238::collect() success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK); + if (success) { + _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); + _battery.updateCurrent(static_cast(current * _current_lsb)); + _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); + + _battery.setConnected(success); + + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + } + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { @@ -242,26 +254,21 @@ int INA238::collect() PX4_DEBUG("register check failed"); perf_count(_bad_register_perf); success = false; + + _battery.setConnected(success); + + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } } - if (!success) { - PX4_DEBUG("error reading from sensor"); - bus_voltage = current = 0; - } - - _battery.setConnected(success); - _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); - _battery.updateCurrent(static_cast(current * _current_lsb)); - _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - perf_end(_sample_perf); if (success) { return PX4_OK; } else { + PX4_DEBUG("error reading from sensor"); + return PX4_ERROR; } } diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp index 399f034fbf..9bb8d47fba 100644 --- a/src/drivers/pps_capture/PPSCapture.cpp +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -63,14 +63,14 @@ bool PPSCapture::init() { bool success = false; - for (unsigned i = 0; i < 16; ++i) { + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; ++i) { char param_name[17]; snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); param_t function_handle = param_find(param_name); int32_t function; if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { - if (function == 2064) { // PPS_Input + if (function == 2064) { // PPS_Input see mixer_module/output_functions.yaml parameter metadata definition _channel = i; } } diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index c63b4195d2..8c2049cceb 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -23,7 +23,6 @@ actuator_output: -5: DShot150 -4: DShot300 -3: DShot600 - -2: DShot1200 -1: OneShot 50: PWM 50 Hz 100: PWM 100 Hz diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index fcc9828dad..f72c200efb 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/src/drivers/rpm/pcf8583/PCF8583.cpp @@ -195,13 +195,11 @@ void PCF8583::RunImpl() } // Calculate RPM and accuracy estimation - float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f; - float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f; + float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f; // publish data to uorb rpm_s msg{}; - msg.indicated_frequency_rpm = indicated_rpm; - msg.estimated_accurancy_rpm = estimated_accurancy; + msg.rpm_estimate = indicated_rpm; msg.timestamp = hrt_absolute_time(); _rpm_pub.publish(msg); diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 5e1e8b016c..4926d1ca9f 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp @@ -66,8 +66,7 @@ int rpm_simulator_main(int argc, char *argv[]) // prpepare RPM data message rpm.timestamp = timestamp_us; - rpm.indicated_frequency_rpm = frequency; - rpm.estimated_accurancy_rpm = frequency / 100.0f; + rpm.rpm_estimate = frequency; // Publish data and let the user know what was published rpm_pub.publish(rpm); diff --git a/src/drivers/rpm_capture/CMakeLists.txt b/src/drivers/rpm_capture/CMakeLists.txt new file mode 100644 index 0000000000..b2639cba1d --- /dev/null +++ b/src/drivers/rpm_capture/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + +px4_add_module( + MODULE drivers__rpm_capture + MAIN rpm_capture + COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" + SRCS + RPMCapture.cpp + ) diff --git a/src/drivers/rpm_capture/Kconfig b/src/drivers/rpm_capture/Kconfig new file mode 100644 index 0000000000..6f9f074fea --- /dev/null +++ b/src/drivers/rpm_capture/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RPM_CAPTURE + bool "rpm_capture" + default n + ---help--- + Enable support for rpm_capture diff --git a/src/drivers/rpm_capture/RPMCapture.cpp b/src/drivers/rpm_capture/RPMCapture.cpp new file mode 100644 index 0000000000..1e05471903 --- /dev/null +++ b/src/drivers/rpm_capture/RPMCapture.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * 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 "RPMCapture.hpp" +#include +#include +#include +#include +#include + +RPMCapture::RPMCapture() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + ModuleParams(nullptr) +{ + _pwm_input_pub.advertise(); + ScheduleNow(); +} + +RPMCapture::~RPMCapture() +{ + if (_channel >= 0) { + io_timer_unallocate_channel(_channel); + px4_arch_gpiosetevent(_rpm_capture_gpio, false, false, false, nullptr, nullptr); + } +} + +bool RPMCapture::init() +{ + bool success = false; + + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; ++i) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1); + param_t function_handle = param_find(param_name); + int32_t function; + + if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { + if (function == 2070) { // RPM_Input see mixer_module/output_functions.yaml parameter metadata definition + _channel = i; + } + } + } + + if (_channel == -1) { + PX4_WARN("No RPM channel configured"); + return false; + } + + int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_RPM); + + if (ret != PX4_OK) { + PX4_ERR("gpio alloc failed (%i) for RPM at channel (%d)", ret, _channel); + return false; + } + + _rpm_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel)); + int ret_val = px4_arch_gpiosetevent(_rpm_capture_gpio, true, false, true, &RPMCapture::gpio_interrupt_callback, this); + + if (ret_val == PX4_OK) { + success = true; + } + + success = success && _rpm_pub.advertise(); + return success; +} + +void RPMCapture::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + hrt_abstime now = hrt_absolute_time(); + + if (_interrupt_happened.load()) { + // There was an interrupt + _period = _hrt_timestamp - _hrt_timestamp_prev; + _hrt_timestamp_prev = _hrt_timestamp; + _interrupt_happened.store(false); + + pwm_input_s pwm_input{}; + pwm_input.timestamp = now; + pwm_input.period = _period; + pwm_input.error_count = _error_count; + _pwm_input_pub.publish(pwm_input); + + ScheduleClear(); // Do not run on previously scheduled timeout + + } else { + // Timeout for no interrupts + _period = UINT32_MAX; + } + + ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout + float rpm_raw{0.f}; + + if (_period < RPM_PULSE_TIMEOUT) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + rpm_raw = 60.f * 1e6f / static_cast(_param_rpm_puls_per_rev.get() * _period); + + } else { + _rpm_filter.reset(rpm_raw); + } + + if (rpm_raw < RPM_MAX_VALUE) { + // Don't update RPM filter with outliers + const float dt = math::min((now - _timestamp_last_update) * 1e-6f, 1.f); + _timestamp_last_update = now; + _rpm_filter.setParameters(dt, RPM_FILTER_TIME_CONSTANT); + _rpm_filter.update(_rpm_median_filter.apply(rpm_raw)); + } + + rpm_s rpm{}; + rpm.timestamp = now; + rpm.rpm_raw = rpm_raw; + rpm.rpm_estimate = _rpm_filter.getState(); + _rpm_pub.publish(rpm); +} + +int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg) +{ + RPMCapture *instance = static_cast(arg); + + if (instance->_interrupt_happened.load()) { + ++instance->_error_count; + } + + instance->_hrt_timestamp = hrt_absolute_time(); + instance->_interrupt_happened.store(true); + instance->ScheduleNow(); + + return PX4_OK; +} + +int RPMCapture::task_spawn(int argc, char *argv[]) +{ + RPMCapture *instance = new RPMCapture(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int RPMCapture::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RPMCapture::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_USAGE_NAME("rpm_capture", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +void RPMCapture::stop() +{ + exit_and_cleanup(); +} + +extern "C" __EXPORT int rpm_capture_main(int argc, char *argv[]) +{ + if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running()) { + RPMCapture::stop(); + } + + return RPMCapture::main(argc, argv); +} diff --git a/src/drivers/rpm_capture/RPMCapture.hpp b/src/drivers/rpm_capture/RPMCapture.hpp new file mode 100644 index 0000000000..63d7426848 --- /dev/null +++ b/src/drivers/rpm_capture/RPMCapture.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class RPMCapture : public ModuleBase, public px4::ScheduledWorkItem, public ModuleParams +{ +public: + RPMCapture(); + virtual ~RPMCapture(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + static int gpio_interrupt_callback(int irq, void *context, void *arg); + + /** RPMCapture is an interrupt-driven task and needs to be manually stopped */ + static void stop(); + +private: + static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s; + static constexpr float RPM_MAX_VALUE = 50e3f; + static constexpr float RPM_FILTER_TIME_CONSTANT = .5f; + + void Run() override; + + int _channel{-1}; + uint32_t _rpm_capture_gpio{0}; + uORB::Publication _pwm_input_pub{ORB_ID(pwm_input)}; + uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; + + hrt_abstime _hrt_timestamp{0}; + hrt_abstime _hrt_timestamp_prev{0}; + uint32_t _period{UINT32_MAX}; + uint32_t _error_count{0}; + px4::atomic _interrupt_happened{false}; + + hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt + AlphaFilter _rpm_filter; + MedianFilter _rpm_median_filter; + + DEFINE_PARAMETERS( + (ParamInt) _param_rpm_puls_per_rev + ) +}; diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c new file mode 100644 index 0000000000..13bd3fb7a7 --- /dev/null +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * RPM Capture Enable + * + * Enables the RPM capture module on FMU channel 5. + * + * @boolean + * @group System + * @reboot_required true + */ +PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0); + +/** + * Voltage pulses per revolution + * + * Number of voltage pulses per one rotor revolution on the capturing pin. + * + * @group System + * @min 1 + * @max 50 + * @reboot_required true + */ +PARAM_DEFINE_INT32(RPM_PULS_PER_REV, 1); diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 3be05d87a3..364d5005a7 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -150,7 +150,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(airdata.ambient_temperature + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index f8bc1eea8a..560c8b4d56 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -31,12 +31,14 @@ # ############################################################################ -set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) -set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) +set(LIBDRONECAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan) +set(LIBDRONECAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) -px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) +set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan/dsdl") + +px4_add_git_submodule(TARGET git_uavcan_dsdl PATH ${DSDLC_DIR}) +px4_add_git_submodule(TARGET git_uavcan_pydronecan PATH ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/pydronecan) -set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") if(CONFIG_ARCH_CHIP) @@ -90,25 +92,24 @@ add_compile_options( -Wno-address-of-packed-member ) set(CMAKE_WARN_DEPRECATED OFF CACHE BOOL "" FORCE) # silence libuavcan deprecation warning for now (TODO: fix and remove) -add_subdirectory(${LIBUAVCAN_DIR} libuavcan EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR} libdronecan EXCLUDE_FROM_ALL) add_dependencies(uavcan prebuild_targets) # driver -add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated ) # generated DSDL -set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl") set(DSDLC_INPUTS - "${LIBUAVCAN_DIR}/dsdl/ardupilot" - "${LIBUAVCAN_DIR}/dsdl/com" - "${LIBUAVCAN_DIR}/dsdl/cuav" - "${LIBUAVCAN_DIR}/dsdl/dronecan" - "${LIBUAVCAN_DIR}/dsdl/uavcan" + "${DSDLC_DIR}/ardupilot" + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/cuav" + "${DSDLC_DIR}/dronecan" + "${DSDLC_DIR}/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") @@ -119,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS}) endforeach(DSDLC_INPUT) add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp COMMAND - ${PYTHON_EXECUTABLE} ${LIBUAVCAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc + ${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc --outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS} #--verbose COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp @@ -138,10 +139,10 @@ px4_add_module( #-DDEBUG_BUILD INCLUDES ${DSDLC_OUTPUT} - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated - ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include - ${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR_DRIVERS}/posix/include + ${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include SRCS arming_status.cpp arming_status.hpp @@ -191,7 +192,8 @@ px4_add_module( mixer_module version - git_uavcan + git_uavcan_dsdl + git_uavcan_pydronecan uavcan_${UAVCAN_DRIVER}_driver drivers_rangefinder # Fix undefined reference when no distance sensors are selected diff --git a/src/drivers/uavcan/libdronecan/.gitignore b/src/drivers/uavcan/libdronecan/.gitignore new file mode 100644 index 0000000000..a901d64075 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/.gitignore @@ -0,0 +1,30 @@ +# Build outputs +*.o +*.d +lib*.so +lib*.so.* +*.a +build*/ +.dep +__pycache__ +*.pyc + +# Eclipse +.metadata +.settings +.project +.cproject +.pydevproject +.gdbinit + +# vsstudio code +.vscode + +# vagrant +.vagrant + +# libuavcan DSDL compiler default output directory +dsdlc_generated + +# Log files +*.log diff --git a/src/drivers/uavcan/libdronecan/CMakeLists.txt b/src/drivers/uavcan/libdronecan/CMakeLists.txt new file mode 100644 index 0000000000..5cb858dc14 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/CMakeLists.txt @@ -0,0 +1,113 @@ +# +# Copyright (C) 2014 Pavel Kirienko +# + +cmake_minimum_required(VERSION 3.9) + +project(uavcan C CXX) + +# +# Build options +# +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + set(DEFAULT_UAVCAN_PLATFORM "linux") +endif() + +# options are listed in a table format below +set(opts + # name: type: default value: string options list : description + "CMAKE_BUILD_TYPE:STRING:RelWithDebInfo:Debug Release RelWithDebInfo MinSizeRel:Build type." + "CMAKE_CXX_FLAGS:STRING:::C++ flags." + "CMAKE_C_FLAGS:STRING:::C flags." + "UAVCAN_PLATFORM:STRING:generic:generic kinetis linux stm32:Platform." + "CONTINUOUS_INTEGRATION_BUILD:BOOL:OFF::Disable error redirection and timing tests" + "UAVCAN_CMAKE_VERBOSE:BOOL:OFF::Verbose CMake configure output" + ) +foreach(_opt ${opts}) + # arguments are : delimited + string(REPLACE ":" ";" _opt ${_opt}) + list(GET _opt 0 _name) + list(GET _opt 1 _type) + list(GET _opt 2 _default) + list(GET _opt 3 _options) + list(GET _opt 4 _descr) + # options are space delimited + string(REPLACE " " ";" _options "${_options}") + # if a default has not already been defined, use default from table + if(NOT DEFINED DEFAULT_${_name}) + set(DEFAULT_${_name} ${_default}) + endif() + # option has not been set already or it is empty, set it with the default + if(NOT DEFINED ${_name} OR ${_name} STREQUAL "") + set(${_name} ${DEFAULT_${_name}}) + endif() + # create a cache from the variable and force it to set +if(UAVCAN_CMAKE_VERBOSE) + message(STATUS "${_name}\t: ${${_name}} : ${_descr}") +endif() + set("${_name}" "${${_name}}" CACHE "${_type}" "${_descr}" FORCE) + # if an options list is provided for the cache, set it + if("${_type}" STREQUAL "STRING" AND NOT "${_options}" STREQUAL "") + set_property(CACHE ${_name} PROPERTY STRINGS ${_options}) + endif() +endforeach() + +# +# Set flags +# +include_directories( + ./libuavcan/include/ + ./libuavcan/include/dsdlc_generated + ) + +# +# Install +# +# DSDL definitions +install(DIRECTORY dsdl DESTINATION share/uavcan) + +# +# Googletest +# +if( CMAKE_BUILD_TYPE STREQUAL "Debug" ) + # (Taken from googletest/README.md documentation) + # GTest executables + # Download and unpack googletest at configure time + configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt) + execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download ) + if(result) + message(WARNING "CMake step for googletest failed: ${result}") + else() + execute_process(COMMAND ${CMAKE_COMMAND} --build . + RESULT_VARIABLE result + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/googletest-download ) + if(result) + message(WARNING "Build step for googletest failed: ${result}") + else() + + # Prevent overriding the parent project's compiler/linker + # settings on Windows + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + + # Add googletest directly to our build. This defines + # the gtest and gtest_main targets. + add_subdirectory(${CMAKE_BINARY_DIR}/googletest-src + ${CMAKE_BINARY_DIR}/googletest-build + EXCLUDE_FROM_ALL) + + set(GTEST_FOUND ON) + set(BUILD_TESTING ON) + enable_testing() + endif() + endif() +endif() + +# +# Subdirectories +# +# library +add_subdirectory(libuavcan) + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : diff --git a/src/drivers/uavcan/libdronecan/CMakeLists.txt.in b/src/drivers/uavcan/libdronecan/CMakeLists.txt.in new file mode 100644 index 0000000000..6eeed756a4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/CMakeLists.txt.in @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.9) + +project(googletest-download NONE) + +include(ExternalProject) +ExternalProject_Add(googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG ba96d0b1161f540656efdaed035b3c062b60e006 + SOURCE_DIR "${CMAKE_BINARY_DIR}/googletest-src" + BINARY_DIR "${CMAKE_BINARY_DIR}/googletest-build" + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND "" + TEST_COMMAND "" +) diff --git a/src/drivers/uavcan/libdronecan/LICENSE b/src/drivers/uavcan/libdronecan/LICENSE new file mode 100644 index 0000000000..41d1234c01 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/LICENSE @@ -0,0 +1,20 @@ +The MIT License (MIT) + +Copyright (c) 2014 Pavel Kirienko + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of +the Software, and to permit persons to whom the Software is furnished to do so, +subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR +COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER +IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/src/drivers/uavcan/libdronecan/README.md b/src/drivers/uavcan/libdronecan/README.md new file mode 100644 index 0000000000..60f803ca9e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/README.md @@ -0,0 +1,120 @@ +DroneCAN stack in C++ +===================== + +Portable reference implementation of the [DroneCAN protocol stack](http://dronecan.org) in C++ for embedded systems +and Linux. + +DroneCAN is a lightweight protocol designed for reliable communication in aerospace and robotic applications via CAN bus. + +## Documentation + +* [DroneCAN website](http://dronecan.org) +* [DroneCAN forum](https://dronecan.org/discord) + +## Library usage + +### Cloning the repository + +```bash +git clone https://github.com/DroneCAN/libuavcan +cd libuavcan +git submodule update --init +``` + +If this repository is used as a git submodule in your project, make sure to use `--recursive` when updating it. + +### Using in a Linux application + +Libuavcan can be built as a static library and installed on the system globally as shown below. + +```bash +mkdir build +cd build +cmake .. # Default build type is RelWithDebInfo, which can be overriden if needed. +make -j8 +sudo make install +``` + +The following components will be installed: + +* Libuavcan headers and the static library +* Generated DSDL headers +* Libuavcan DSDL compiler (a Python script named `libuavcan_dsdlc`) +* Libuavcan DSDL compiler's support library (a Python package named `libuavcan_dsdl_compiler`) + +Note that Pyuavcan (an implementation of DroneCAN in Python) will not be installed. +You will need to install it separately if you intend to use the Libuavcan's DSDL compiler in your applications. + +It is also possible to use the library as a submodule rather than installing it system-wide. +Please refer to the example applications supplied with the Linux platform driver for more information. + +### Using with an embedded system + +For ARM targets, it is recommended to use [GCC ARM Embedded](https://launchpad.net/gcc-arm-embedded); +however, any other standard-compliant C++ compiler should also work. + +## Library development + +Despite the fact that the library itself can be used on virtually any platform that has a standard-compliant +C++11 compiler, the library development process assumes that the host OS is Linux. + +Prerequisites: + +* Google test library for C++ - gtest (dowloaded as part of the build from [github](https://github.com/google/googletest)) +* C++11 capable compiler with GCC-like interface (e.g. GCC, Clang) +* CMake 2.8+ +* Optional: static analysis tool for C++ - cppcheck (on Debian/Ubuntu use package `cppcheck`) + +Building the debug version and running the unit tests: +```bash +mkdir build +cd build +cmake .. -DCMAKE_BUILD_TYPE=Debug +make -j8 +make ARGS=-VV test +``` + +Test outputs can be found in the build directory under `libuavcan`. + +> Note that unit tests suffixed with "_RealTime" must be executed in real time, otherwise they may produce false warnings; +this implies that they will likely fail if ran on a virtual machine or on a highly loaded system. + +### Vagrant +Vagrant can be used to setup a compatible Ubuntu virtual image. Follow the instructions on [Vagrantup](https://www.vagrantup.com/) to install virtualbox and vagrant then do: + +```bash +vagrant up +vagrant ssh +mkdir build +cd build +mkdir build && cd build && cmake .. -DCMAKE_BUILD_TYPE=Debug -DCONTINUOUS_INTEGRATION_BUILD=1 +``` + +> Note that -DCONTINUOUS_INTEGRATION_BUILD=1 is required for this build as the realtime unit tests will not work on a virt. + +You can build using commands like: + +```bash +vagrant ssh -c "cd /vagrant/build && make -j4 && make test" +``` + +or to run a single test: + +```bash +vagrant ssh -c "cd /vagrant/build && make libuavcan_test && ./libuavcan/libuavcan_test --gtest_filter=Node.Basic" +``` + +### Developing with Eclipse + +An Eclipse project can be generated like that: + +```bash +cmake ../../libuavcan -G"Eclipse CDT4 - Unix Makefiles" \ + -DCMAKE_ECLIPSE_VERSION=4.3 \ + -DCMAKE_BUILD_TYPE=Debug \ + -DCMAKE_CXX_COMPILER_ARG1=-std=c++11 +``` + +Path `../../libuavcan` in the command above points at the directory where the top-level `CMakeLists.txt` is located; +you may need to adjust this per your environment. +Note that the directory where Eclipse project is generated must not be a descendant of the source directory. diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl new file mode 160000 index 0000000000..993be80a62 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -0,0 +1 @@ +Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt b/src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt new file mode 100644 index 0000000000..8e095162e2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/CMakeLists.txt @@ -0,0 +1,146 @@ +# +# Copyright (C) 2014 Pavel Kirienko +# + +cmake_minimum_required(VERSION 3.9) + +if(DEFINED CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "Debug Release RelWithDebInfo MinSizeRel") +else() + set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Debug Release RelWithDebInfo MinSizeRel") +endif() + +# Detecting whether we need to add debug targets +string(TOLOWER "${CMAKE_BUILD_TYPE}" build_type_lower) +if (build_type_lower STREQUAL "debug") + set(DEBUG_BUILD 1) + message(STATUS "Debug build") +else () + set(DEBUG_BUILD 0) +endif () + +project(libuavcan) + +find_package(PythonInterp) + +if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") + set(COMPILER_IS_GCC_COMPATIBLE 1) +else () + set(COMPILER_IS_GCC_COMPATIBLE 0) +endif () + +# +# DSDL compiler invocation +# Probably output files should be saved into CMake output dir? +# +set(DSDLC_INPUTS "test/dsdl_test/root_ns_a" "test/dsdl_test/root_ns_b" "${CMAKE_CURRENT_SOURCE_DIR}/../dsdl/uavcan") +set(DSDLC_OUTPUT "include/dsdlc_generated") + +set(DSDLC_INPUT_FILES "") +foreach(DSDLC_INPUT ${DSDLC_INPUTS}) + file(GLOB_RECURSE DSDLC_NEW_INPUT_FILES ${CMAKE_CURRENT_SOURCE_DIR} "${DSDLC_INPUT}/*.uavcan") + set(DSDLC_INPUT_FILES ${DSDLC_INPUT_FILES} ${DSDLC_NEW_INPUT_FILES}) +endforeach(DSDLC_INPUT) +add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT} + COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp + DEPENDS ${DSDLC_INPUT_FILES} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Running dsdl compiler") +add_custom_target(libuavcan_dsdlc DEPENDS ${CMAKE_BINARY_DIR}/libuavcan_dsdlc_run.stamp) +include_directories(${DSDLC_OUTPUT}) + +# +# Compiler flags +# +if (COMPILER_IS_GCC_COMPATIBLE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wundef") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +endif () + +if (DEBUG_BUILD) + add_definitions(-DUAVCAN_DEBUG=1) +endif () + +include_directories(include) + +# +# libuavcan +# +file(GLOB_RECURSE LIBUAVCAN_CXX_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "src/*.cpp") +add_library(uavcan STATIC ${LIBUAVCAN_CXX_FILES}) +add_dependencies(uavcan libuavcan_dsdlc) + +install(TARGETS uavcan DESTINATION lib) +install(DIRECTORY include/uavcan DESTINATION include) +install(DIRECTORY include/dsdlc_generated/uavcan DESTINATION include) # Generated and lib's .hpp + +# +# Tests and static analysis - only for debug builds +# +function(add_libuavcan_test name library flags) # Adds GTest executable and creates target to execute it every build + find_package(Threads REQUIRED) + + file(GLOB_RECURSE TEST_CXX_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} "test/*.cpp") + add_executable(${name} ${TEST_CXX_FILES}) + add_dependencies(${name} ${library}) + + if (flags) + set_target_properties(${name} PROPERTIES COMPILE_FLAGS ${flags}) + endif () + + target_link_libraries(${name} gmock_main) + target_link_libraries(${name} ${library}) + if (${UAVCAN_PLATFORM} STREQUAL "linux") + target_link_libraries(${name} rt) + endif() + + # Tests run automatically upon successful build + # If failing tests need to be investigated with debugger, use 'make --ignore-errors' + if (CONTINUOUS_INTEGRATION_BUILD) + # Don't redirect test output, and don't run tests suffixed with "RealTime" + add_test(NAME ${name} + COMMAND ${name} --gtest_filter=-*RealTime + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + else () + add_test(NAME ${name} + COMMAND ${name} 1>"${name}.log" 2>&1 + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + endif() +endfunction() + +if (DEBUG_BUILD) + message(STATUS "Debug build (note: requires gtest)") + + if (COMPILER_IS_GCC_COMPATIBLE) + # No such thing as too many warnings + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -Wfloat-equal -Wconversion") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations -Wlogical-op") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wtype-limits -Wno-error=array-bounds") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error=deprecated-declarations") + set(optim_flags "-O3 -DNDEBUG -g0") + else () + message(STATUS "Compiler ID: ${CMAKE_CXX_COMPILER_ID}") + message(FATAL_ERROR "This compiler cannot be used to build tests; use release build instead.") + endif () + + # Additional flavours of the library + + add_library(uavcan_optim STATIC ${LIBUAVCAN_CXX_FILES}) + set_target_properties(uavcan_optim PROPERTIES COMPILE_FLAGS ${optim_flags}) + add_dependencies(uavcan_optim libuavcan_dsdlc) + + if (GTEST_FOUND) + message(STATUS "GTest found, tests will be built and run.") + add_libuavcan_test(libuavcan_test uavcan "") # Default + add_libuavcan_test(libuavcan_test_optim uavcan_optim "${optim_flags}") # Max optimization + else (GTEST_FOUND) + message(STATUS "GTest was not found, tests will not be built") + endif (GTEST_FOUND) +else () + message(STATUS "Release build type: " ${CMAKE_BUILD_TYPE}) +endif () + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 : diff --git a/src/drivers/uavcan/libdronecan/libuavcan/cppcheck.sh b/src/drivers/uavcan/libdronecan/libuavcan/cppcheck.sh new file mode 100755 index 0000000000..416cc506c5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/cppcheck.sh @@ -0,0 +1,18 @@ +#!/bin/sh +# +# cppcheck static analysis +# For Debian based: apt-get install cppcheck +# + +num_cores=$(grep -c ^processor /proc/cpuinfo) +if [ -z "$num_cores" ]; then + echo "Hey, it looks like we're not on Linux. Please fix this script to add support for this OS." + num_cores=4 +fi +echo "Number of threads for cppcheck: $num_cores" + +# TODO: with future versions of cppcheck, add --library=glibc +cppcheck . --error-exitcode=1 --quiet --enable=all --platform=unix64 --std=c99 --std=c++11 \ + --inline-suppr --force --template=gcc -j$num_cores \ + -U__BIGGEST_ALIGNMENT__ -UUAVCAN_MEM_POOL_BLOCK_SIZE -UBIG_ENDIAN -UBYTE_ORDER \ + -Iinclude $@ diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py new file mode 100644 index 0000000000..cb70711462 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py @@ -0,0 +1,313 @@ +#!/usr/bin/env python +# +# UAVCAN DSDL compiler for libuavcan +# +# Copyright (C) 2014 Pavel Kirienko +# + +''' +This module implements the core functionality of the UAVCAN DSDL compiler for libuavcan. +Supported Python versions: 3.2+, 2.7. +It accepts a list of root namespaces and produces the set of C++ header files for libuavcan. +It is based on the DSDL parsing package from pyuavcan. +''' + +from __future__ import division, absolute_import, print_function, unicode_literals +import sys, os, logging, errno, re +from .pyratemp import Template +from dronecan import dsdl + +# Python 2.7 compatibility +try: + str = unicode +except NameError: + pass + +OUTPUT_FILE_EXTENSION = 'hpp' +OUTPUT_FILE_PERMISSIONS = 0o444 # Read only for all +TEMPLATE_FILENAME = os.path.join(os.path.dirname(__file__), 'data_type_template.tmpl') + +__all__ = ['run', 'logger', 'DsdlCompilerException'] + +class DsdlCompilerException(Exception): + pass + +logger = logging.getLogger(__name__) + +def run(source_dirs, include_dirs, output_dir): + ''' + This function takes a list of root namespace directories (containing DSDL definition files to parse), a + possibly empty list of search directories (containing DSDL definition files that can be referenced from the types + that are going to be parsed), and the output directory path (possibly nonexistent) where the generated C++ + header files will be stored. + + Note that this module features lazy write, i.e. if an output file does already exist and its content is not going + to change, it will not be overwritten. This feature allows to avoid unnecessary recompilation of dependent object + files. + + Args: + source_dirs List of root namespace directories to parse. + include_dirs List of root namespace directories with referenced types (possibly empty). This list is + automaitcally extended with source_dirs. + output_dir Output directory path. Will be created if doesn't exist. + ''' + assert isinstance(source_dirs, list) + assert isinstance(include_dirs, list) + output_dir = str(output_dir) + + types = run_parser(source_dirs, include_dirs + source_dirs) + if not types: + die('No type definitions were found') + + logger.info('%d types total', len(types)) + run_generator(types, output_dir) + +# ----------------- + +def pretty_filename(filename): + try: + a = os.path.abspath(filename) + r = os.path.relpath(filename) + return a if '..' in r else r + except ValueError: + return filename + +def type_output_filename(t): + assert t.category == t.CATEGORY_COMPOUND + return t.full_name.replace('.', os.path.sep) + '.' + OUTPUT_FILE_EXTENSION + +def makedirs(path): + try: + try: + os.makedirs(path, exist_ok=True) # May throw "File exists" when executed as root, which is wrong + except TypeError: + os.makedirs(path) # Python 2.7 compatibility + except OSError as ex: + if ex.errno != errno.EEXIST: # http://stackoverflow.com/questions/12468022 + raise + +def die(text): + raise DsdlCompilerException(str(text)) + +def run_parser(source_dirs, search_dirs): + try: + types = dsdl.parse_namespaces(source_dirs, search_dirs) + except dsdl.DsdlException as ex: + logger.info('Parser failure', exc_info=True) + die(ex) + return types + +def run_generator(types, dest_dir): + try: + template_expander = make_template_expander(TEMPLATE_FILENAME) + dest_dir = os.path.abspath(dest_dir) # Removing '..' + makedirs(dest_dir) + for t in types: + logger.info('Generating type %s', t.full_name) + filename = os.path.join(dest_dir, type_output_filename(t)) + text = generate_one_type(template_expander, t) + write_generated_data(filename, text) + except Exception as ex: + logger.info('Generator failure', exc_info=True) + die(ex) + +def write_generated_data(filename, data): + dirname = os.path.dirname(filename) + makedirs(dirname) + + # Lazy update - file will not be rewritten if its content is not going to change + if os.path.exists(filename): + with open(filename) as f: + existing_data = f.read() + if data == existing_data: + logger.info('Up to date [%s]', pretty_filename(filename)) + return + logger.info('Rewriting [%s]', pretty_filename(filename)) + os.remove(filename) + else: + logger.info('Creating [%s]', pretty_filename(filename)) + + # Full rewrite + with open(filename, 'w') as f: + f.write(data) + try: + os.chmod(filename, OUTPUT_FILE_PERMISSIONS) + except (OSError, IOError) as ex: + logger.warning('Failed to set permissions for %s: %s', pretty_filename(filename), ex) + +def type_to_cpp_type(t): + if t.category == t.CATEGORY_PRIMITIVE: + cast_mode = { + t.CAST_MODE_SATURATED: '::uavcan::CastModeSaturate', + t.CAST_MODE_TRUNCATED: '::uavcan::CastModeTruncate', + }[t.cast_mode] + if t.kind == t.KIND_FLOAT: + return '::uavcan::FloatSpec< %d, %s >' % (t.bitlen, cast_mode) + else: + signedness = { + t.KIND_BOOLEAN: '::uavcan::SignednessUnsigned', + t.KIND_UNSIGNED_INT: '::uavcan::SignednessUnsigned', + t.KIND_SIGNED_INT: '::uavcan::SignednessSigned', + }[t.kind] + return '::uavcan::IntegerSpec< %d, %s, %s >' % (t.bitlen, signedness, cast_mode) + elif t.category == t.CATEGORY_ARRAY: + value_type = type_to_cpp_type(t.value_type) + mode = { + t.MODE_STATIC: '::uavcan::ArrayModeStatic', + t.MODE_DYNAMIC: '::uavcan::ArrayModeDynamic', + }[t.mode] + return '::uavcan::Array< %s, %s, %d >' % (value_type, mode, t.max_size) + elif t.category == t.CATEGORY_COMPOUND: + return '::' + t.full_name.replace('.', '::') + elif t.category == t.CATEGORY_VOID: + return '::uavcan::IntegerSpec< %d, ::uavcan::SignednessUnsigned, ::uavcan::CastModeSaturate >' % t.bitlen + else: + raise DsdlCompilerException('Unknown type category: %s' % t.category) + +def generate_one_type(template_expander, t): + t.short_name = t.full_name.split('.')[-1] + t.cpp_type_name = t.short_name + '_' + t.cpp_full_type_name = '::' + t.full_name.replace('.', '::') + t.include_guard = t.full_name.replace('.', '_').upper() + '_HPP_INCLUDED' + + # Dependencies (no duplicates) + def fields_includes(fields): + def detect_include(t): + if t.category == t.CATEGORY_COMPOUND: + return type_output_filename(t) + if t.category == t.CATEGORY_ARRAY: + return detect_include(t.value_type) + return list(sorted(set(filter(None, [detect_include(x.type) for x in fields])))) + + if t.kind == t.KIND_MESSAGE: + t.cpp_includes = fields_includes(t.fields) + else: + t.cpp_includes = fields_includes(t.request_fields + t.response_fields) + + t.cpp_namespace_components = t.full_name.split('.')[:-1] + t.has_default_dtid = t.default_dtid is not None + + # Attribute types + def inject_cpp_types(attributes): + void_index = 0 + for a in attributes: + a.cpp_type = type_to_cpp_type(a.type) + a.void = a.type.category == a.type.CATEGORY_VOID + if a.void: + assert not a.name + a.name = '_void_%d' % void_index + void_index += 1 + + if t.kind == t.KIND_MESSAGE: + inject_cpp_types(t.fields) + inject_cpp_types(t.constants) + t.all_attributes = t.fields + t.constants + t.union = t.union and len(t.fields) + else: + inject_cpp_types(t.request_fields) + inject_cpp_types(t.request_constants) + inject_cpp_types(t.response_fields) + inject_cpp_types(t.response_constants) + t.all_attributes = t.request_fields + t.request_constants + t.response_fields + t.response_constants + t.request_union = t.request_union and len(t.request_fields) + t.response_union = t.response_union and len(t.response_fields) + + # Constant properties + def inject_constant_info(constants): + for c in constants: + if c.type.kind == c.type.KIND_FLOAT: + float(c.string_value) # Making sure that this is a valid float literal + c.cpp_value = c.string_value + else: + int(c.string_value) # Making sure that this is a valid integer literal + c.cpp_value = c.string_value + if c.type.kind == c.type.KIND_UNSIGNED_INT: + c.cpp_value += 'U' + + if t.kind == t.KIND_MESSAGE: + inject_constant_info(t.constants) + else: + inject_constant_info(t.request_constants) + inject_constant_info(t.response_constants) + + # Data type kind + t.cpp_kind = { + t.KIND_MESSAGE: '::uavcan::DataTypeKindMessage', + t.KIND_SERVICE: '::uavcan::DataTypeKindService', + }[t.kind] + + # Generation + text = template_expander(t=t) # t for Type + text = '\n'.join(x.rstrip() for x in text.splitlines()) + text = text.replace('\n\n\n\n\n', '\n\n').replace('\n\n\n\n', '\n\n').replace('\n\n\n', '\n\n') + text = text.replace('{\n\n ', '{\n ') + return text + +def make_template_expander(filename): + ''' + Templating is based on pyratemp (http://www.simple-is-better.org/template/pyratemp.html). + The pyratemp's syntax is rather verbose and not so human friendly, so we define some + custom extensions to make it easier to read and write. + The resulting syntax somewhat resembles Mako (which was used earlier instead of pyratemp): + Substitution: + ${expression} + Line joining through backslash (replaced with a single space): + ${foo(bar(very_long_arument=42, \ + second_line=72))} + Blocks: + % for a in range(10): + % if a == 5: + ${foo()} + % endif + % endfor + The extended syntax is converted into pyratemp's through regexp substitution. + ''' + with open(filename) as f: + template_text = f.read() + + # Backslash-newline elimination + template_text = re.sub(r'\\\r{0,1}\n\ *', r' ', template_text) + + # Substitution syntax transformation: ${foo} ==> $!foo!$ + template_text = re.sub(r'([^\$]{0,1})\$\{([^\}]+)\}', r'\1$!\2!$', template_text) + + # Flow control expression transformation: % foo: ==> + template_text = re.sub(r'(?m)^(\ *)\%\ *(.+?):{0,1}$', r'\1', template_text) + + # Block termination transformation: ==> + template_text = re.sub(r'\<\!--\(end[a-z]+\)--\>', r'', template_text) + + # Pyratemp workaround. + # The problem is that if there's no empty line after a macro declaration, first line will be doubly indented. + # Workaround: + # 1. Remove trailing comments + # 2. Add a newline after each macro declaration + template_text = re.sub(r'\ *\#\!.*', '', template_text) + template_text = re.sub(r'(\<\!--\(macro\ [a-zA-Z0-9_]+\)--\>.*?)', r'\1\n', template_text) + + # Preprocessed text output for debugging +# with open(filename + '.d', 'w') as f: +# f.write(template_text) + + template = Template(template_text) + + def expand(**args): + # This function adds one indentation level (4 spaces); it will be used from the template + args['indent'] = lambda text, idnt = ' ': idnt + text.replace('\n', '\n' + idnt) + # This function works like enumerate(), telling you whether the current item is the last one + def enum_last_value(iterable, start=0): + it = iter(iterable) + count = start + try: + last = next(it) + except StopIteration: + return + for val in it: + yield count, False, last + last = val + count += 1 + yield count, True, last + args['enum_last_value'] = enum_last_value + return template(**args) + + return expand diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl new file mode 100644 index 0000000000..3107426bf0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl @@ -0,0 +1,558 @@ +/* + * UAVCAN data structure definition for libuavcan. + * + * Autogenerated, do not edit. + * + * Source file: ${t.source_file} + */ + +#ifndef ${t.include_guard} +#define ${t.include_guard} + +#include +#include +#include + +% for inc in t.cpp_includes: +#include <${inc}> +% endfor + +/******************************* Source text ********************************** +% for line in t.source_text.strip().splitlines(): +${line} +% endfor +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +% for line in t.get_dsdl_signature_source_definition().splitlines(): +${line} +% endfor +******************************************************************************/ + +% for a in t.all_attributes: +#undef ${a.name} +% endfor + +% for nsc in t.cpp_namespace_components: +namespace ${nsc} +{ +% endfor + +% if t.kind != t.KIND_SERVICE: +template +% endif +struct UAVCAN_EXPORT ${t.cpp_type_name} +{ + #! type_name, max_bitlen, fields, constants, union + typedef const ${type_name}<_tmpl>& ParameterType; + typedef ${type_name}<_tmpl>& ReferenceType; + + #! group_name, attrs + struct ${group_name} + { + % for a in attrs: + typedef ${a.cpp_type} ${a.name}; + % endfor + }; + + ${expand_attr_types(group_name='ConstantTypes', attrs=constants)} + ${expand_attr_types(group_name='FieldTypes', attrs=fields)} + + % if union: + + struct Tag + { + enum Type + { + % for idx,last,a in enum_last_value(fields): + ${a.name}${',' if not last else ''} + % endfor + }; + }; + + typedef ::uavcan::IntegerSpec< ::uavcan::IntegerBitLen< ${len(fields)}U - 1U >::Result, + ::uavcan::SignednessUnsigned, ::uavcan::CastModeTruncate > TagType; + + #! enum_name, enum_comparator + enum + { + ${enum_name} = TagType::BitLen + + % for idx,last,a in enum_last_value(fields): + % if not last: + ::uavcan::${enum_comparator}::Result' * (len(fields) - 1)} + % endif + % endfor + }; + + + ${expand_enum_per_field(enum_name='MinBitLen', enum_comparator='EnumMin')} + ${expand_enum_per_field(enum_name='MaxBitLen', enum_comparator='EnumMax')} + + % else: + + #! enum_name + enum + { + ${enum_name} + % for idx,a in enumerate(fields): + ${'=' if idx == 0 else '+'} FieldTypes::${a.name}::${enum_name} + % endfor + }; + + + ${expand_enum_per_field(enum_name='MinBitLen')} + ${expand_enum_per_field(enum_name='MaxBitLen')} + + % endif + + // Constants + % for a in constants: + static const typename ::uavcan::StorageType< typename ConstantTypes::${a.name} >::Type ${a.name}; // ${a.init_expression} + % endfor + + // Fields + % for a in [x for x in fields if not x.void]: + typename ::uavcan::StorageType< typename FieldTypes::${a.name} >::Type ${a.name}; + % endfor + + % if union: +private: + typename ::uavcan::StorageType< TagType >::Type _tag_; // The name is mangled to avoid clashing with fields + + template + struct TagToType; + +public: + % endif + + ${type_name}() + % for idx,a in enumerate([x for x in fields if not x.void]): + ${':' if idx == 0 else ','} ${a.name}() + % endfor + % if union: + , _tag_() + % endif + { + ::uavcan::StaticAssert<_tmpl == 0>::check(); // Usage check + +#if UAVCAN_DEBUG + /* + * Cross-checking MaxBitLen provided by the DSDL compiler. + * This check shall never be performed in user code because MaxBitLen value + * actually depends on the nested types, thus it is not invariant. + */ + ::uavcan::StaticAssert<${max_bitlen} == MaxBitLen>::check(); +#endif + } + + bool operator==(ParameterType rhs) const; + bool operator!=(ParameterType rhs) const { return !operator==(rhs); } + + /** + * This comparison is based on @ref uavcan::areClose(), which ensures proper comparison of + * floating point fields at any depth. + */ + bool isClose(ParameterType rhs) const; + + static int encode(ParameterType self, ::uavcan::ScalarCodec& codec, + ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled); + + static int decode(ReferenceType self, ::uavcan::ScalarCodec& codec, + ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled); + + % if union: + /** + * Explicit access to the tag. + * It is safer to use is()/as()/to() instead. + */ + typename Tag::Type getTag() const { return typename Tag::Type(_tag_); } + void setTag(typename Tag::Type x) { _tag_ = typename ::uavcan::StorageType< TagType >::Type(x); } + + /** + * Whether the union is set to the given type. + * Access by tag; this will work even if there are non-unique types within the union. + */ + bool is(typename Tag::Type x) const { return typename Tag::Type(_tag_) == x; } + + /** + * If the union is currently set to the type T, returns pointer to the appropriate field. + * If the union is set to another type, returns null pointer. + */ + template + inline const typename TagToType::StorageType* as() const; + + /** + * Switches the union to the given type and returns a mutable reference to the appropriate field. + * If the previous type was different, a default constructor will be called first. + */ + template + inline typename TagToType::StorageType& to(); + % endif + + +% if t.kind == t.KIND_SERVICE: + template + struct Request_ + { + ${indent(generate_primary_body(type_name='Request_', max_bitlen=t.get_max_bitlen_request(), \ + fields=t.request_fields, constants=t.request_constants, \ + union=t.request_union))} + }; + + template + struct Response_ + { + ${indent(generate_primary_body(type_name='Response_', max_bitlen=t.get_max_bitlen_response(), \ + fields=t.response_fields, constants=t.response_constants, \ + union=t.response_union))} + }; + + typedef Request_<0> Request; + typedef Response_<0> Response; +% else: + ${generate_primary_body(type_name=t.cpp_type_name, max_bitlen=t.get_max_bitlen(), \ + fields=t.fields, constants=t.constants, union=t.union)} +% endif + + /* + * Static type info + */ + enum { DataTypeKind = ${t.cpp_kind} }; +% if t.has_default_dtid: + enum { DefaultDataTypeID = ${t.default_dtid} }; +% else: + // This type has no default data type ID +% endif + + static const char* getDataTypeFullName() + { + return "${t.full_name}"; + } + + static void extendDataTypeSignature(::uavcan::DataTypeSignature& signature) + { + signature.extend(getDataTypeSignature()); + } + + static ::uavcan::DataTypeSignature getDataTypeSignature(); + +% if t.kind == t.KIND_SERVICE: +private: + ${t.cpp_type_name}(); // Don't create objects of this type. Use Request/Response instead. +% endif +}; + +/* + * Out of line struct method definitions + */ + #! scope_prefix, fields, union + +template +bool ${scope_prefix}<_tmpl>::operator==(ParameterType rhs) const +{ + % if union: + if (_tag_ != rhs._tag_) + { + return false; + } + % for idx,a in enumerate(fields): + if (_tag_ == ${idx}) + { + return ${a.name} == rhs.${a.name}; + } + % endfor + UAVCAN_ASSERT(0); // Invalid tag + return false; + % else: + % if fields: + return + % for idx,last,a in enum_last_value([x for x in fields if not x.void]): + ${a.name} == rhs.${a.name}${' &&' if not last else ';'} + % endfor + % else: + (void)rhs; + return true; + % endif + % endif +} + +template +bool ${scope_prefix}<_tmpl>::isClose(ParameterType rhs) const +{ + % if union: + if (_tag_ != rhs._tag_) + { + return false; + } + % for idx,a in enumerate(fields): + if (_tag_ == ${idx}) + { + return ::uavcan::areClose(${a.name}, rhs.${a.name}); + } + % endfor + UAVCAN_ASSERT(0); // Invalid tag + return false; + % else: + % if fields: + return + % for idx,last,a in enum_last_value([x for x in fields if not x.void]): + ::uavcan::areClose(${a.name}, rhs.${a.name})${' &&' if not last else ';'} + % endfor + % else: + (void)rhs; + return true; + % endif + % endif +} + + #! call_name, self_parameter_type +template +int ${scope_prefix}<_tmpl>::${call_name}(${self_parameter_type} self, ::uavcan::ScalarCodec& codec, + ::uavcan::TailArrayOptimizationMode tao_mode) +{ + (void)self; + (void)codec; + (void)tao_mode; + % if union: + const int res = TagType::${call_name}(self._tag_, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + % for idx,a in enumerate(fields): + if (self._tag_ == ${idx}) + { + return FieldTypes::${a.name}::${call_name}(self.${a.name}, codec, tao_mode); + } + % endfor + return -1; // Invalid tag value + % else: + % for a in [x for x in fields if x.void]: + typename ::uavcan::StorageType< typename FieldTypes::${a.name} >::Type ${a.name} = 0; + % endfor + int res = 1; + % for idx,last,a in enum_last_value(fields): + res = FieldTypes::${a.name}::${call_name}(${'self.' * (not a.void)}${a.name}, codec, \ +${'::uavcan::TailArrayOptDisabled' if not last else 'tao_mode'}); + % if not last: + if (res <= 0) + { + return res; + } + % endif + % endfor + return res; + % endif +} + +${generate_codec_calls_per_field(call_name='encode', self_parameter_type='ParameterType')} +${generate_codec_calls_per_field(call_name='decode', self_parameter_type='ReferenceType')} + + % if union: + % for idx,a in enumerate(fields): +template <> +template <> +struct ${scope_prefix}<0>::TagToType<${scope_prefix}<0>::Tag::${a.name}> +{ + typedef typename ${scope_prefix}<0>::FieldTypes::${a.name} Type; + typedef typename ::uavcan::StorageType::Type StorageType; +}; + +template <> +template <> +inline const typename ${scope_prefix}<0>::TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType* +${scope_prefix}<0>::as< ${scope_prefix}<0>::Tag::${a.name} >() const +{ + return is(${scope_prefix}<0>::Tag::${a.name}) ? &${a.name} : UAVCAN_NULLPTR; +} + +template <> +template <> +inline typename ${scope_prefix}<0>::TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType& +${scope_prefix}<0>::to< ${scope_prefix}<0>::Tag::${a.name} >() +{ + if (_tag_ != ${idx}) + { + _tag_ = ${idx}; + ${a.name} = typename TagToType< ${scope_prefix}<0>::Tag::${a.name} >::StorageType(); + } + return ${a.name}; +} + + % endfor + % endif + + +% if t.kind == t.KIND_SERVICE: +${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name + '::Request_', fields=t.request_fields, \ + union=t.request_union)} +${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name + '::Response_', fields=t.response_fields, \ + union=t.response_union)} +% else: +${define_out_of_line_struct_methods(scope_prefix=t.cpp_type_name, fields=t.fields, union=t.union)} +% endif + +/* + * Out of line type method definitions + */ +% if t.kind == t.KIND_SERVICE: +inline ::uavcan::DataTypeSignature ${t.cpp_type_name}::getDataTypeSignature() +% else: +template +::uavcan::DataTypeSignature ${t.cpp_type_name}<_tmpl>::getDataTypeSignature() +% endif +{ + ::uavcan::DataTypeSignature signature(${'0x%08X' % t.get_dsdl_signature()}ULL); + #! scope_prefix, fields + % for a in fields: + ${scope_prefix}FieldTypes::${a.name}::extendDataTypeSignature(signature); + % endfor + +% if t.kind == t.KIND_SERVICE: +${extend_signature_per_field(scope_prefix='Request::', fields=t.request_fields)} +${extend_signature_per_field(scope_prefix='Response::', fields=t.response_fields)} +% else: +${extend_signature_per_field(scope_prefix='', fields=t.fields)} +% endif + return signature; +} + +/* + * Out of line constant definitions + */ + #! scope_prefix, constants + % for a in constants: +template +const typename ::uavcan::StorageType< typename ${scope_prefix}<_tmpl>::ConstantTypes::${a.name} >::Type + ${scope_prefix}<_tmpl>::${a.name} = ${a.cpp_value}; // ${a.init_expression} + + % endfor + +% if t.kind == t.KIND_SERVICE: +${define_out_of_line_constants(scope_prefix=t.cpp_type_name + '::Request_', constants=t.request_constants)} +${define_out_of_line_constants(scope_prefix=t.cpp_type_name + '::Response_', constants=t.response_constants)} +% else: +${define_out_of_line_constants(scope_prefix=t.cpp_type_name, constants=t.constants)} +% endif + +/* + * Final typedef + */ +% if t.kind == t.KIND_SERVICE: +typedef ${t.cpp_type_name} ${t.short_name}; +% else: +typedef ${t.cpp_type_name}<0> ${t.short_name}; +% endif + +% if t.has_default_dtid: +namespace +{ + +const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_full_type_name} > _uavcan_gdtr_registrator_${t.short_name}; + +} +% else: +// No default registration +% endif + +% for nsc in t.cpp_namespace_components[::-1]: +} // Namespace ${nsc} +% endfor + +/* + * YAML streamer specialization + */ +namespace uavcan +{ + + #! type_name, fields, union +template <> +class UAVCAN_EXPORT YamlStreamer< ${type_name} > +{ +public: + template + static void stream(Stream& s, ${type_name}::ParameterType obj, const int level); +}; + +template +void YamlStreamer< ${type_name} >::stream(Stream& s, ${type_name}::ParameterType obj, const int level) +{ + (void)s; + (void)obj; + (void)level; + % if union: + if (level > 0) + { + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + } + % for idx,a in enumerate(fields): + if (static_cast(obj.getTag()) == ${idx}) + { + s << "${a.name}: "; + YamlStreamer< ${type_name}::FieldTypes::${a.name} >::stream(s, obj.${a.name}, level + 1); + } + % endfor + % else: + % for idx,a in enumerate([x for x in fields if not x.void]): + % if idx == 0: + if (level > 0) + { + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + } + % else: + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + % endif + s << "${a.name}: "; + YamlStreamer< ${type_name}::FieldTypes::${a.name} >::stream(s, obj.${a.name}, level + 1); + % endfor + % endif +} + +% if t.kind == t.KIND_SERVICE: +${define_yaml_streamer(type_name=t.cpp_full_type_name + '::Request', fields=t.request_fields, union=t.request_union)} +${define_yaml_streamer(type_name=t.cpp_full_type_name + '::Response', fields=t.response_fields, union=t.response_union)} +% else: +${define_yaml_streamer(type_name=t.cpp_full_type_name, fields=t.fields, union=t.union)} +% endif + +} + +% for nsc in t.cpp_namespace_components: +namespace ${nsc} +{ +% endfor + + #! type_name +template +inline Stream& operator<<(Stream& s, ${type_name}::ParameterType obj) +{ + ::uavcan::YamlStreamer< ${type_name} >::stream(s, obj, 0); + return s; +} + +% if t.kind == t.KIND_SERVICE: +${define_streaming_operator(type_name=t.cpp_full_type_name + '::Request')} +${define_streaming_operator(type_name=t.cpp_full_type_name + '::Response')} +% else: +${define_streaming_operator(type_name=t.cpp_full_type_name)} +% endif + +% for nsc in t.cpp_namespace_components[::-1]: +} // Namespace ${nsc} +% endfor + +#endif // ${t.include_guard} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py new file mode 100755 index 0000000000..ed117b7c01 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/pyratemp.py @@ -0,0 +1,1225 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" +Small, simple and powerful template-engine for Python. + +A template-engine for Python, which is very simple, easy to use, small, +fast, powerful, modular, extensible, well documented and pythonic. + +See documentation for a list of features, template-syntax etc. + +:Version: 0.3.0 +:Requires: Python >=2.6 / 3.x + +:Usage: + see class ``Template`` and examples below. + +:Example: + + Note that the examples are in Python 2; they also work in + Python 3 if you replace u"..." by "...", unicode() by str() + and partly "..." by b"...". + + quickstart:: + >>> t = Template("hello @!name!@") + >>> print(t(name="marvin")) + hello marvin + + quickstart with a template-file:: + # >>> t = Template(filename="mytemplate.tmpl") + # >>> print(t(name="marvin")) + # hello marvin + + generic usage:: + >>> t = Template(u"output is in Unicode \\xe4\\xf6\\xfc\\u20ac") + >>> t #doctest: +ELLIPSIS + <...Template instance at 0x...> + >>> t() + u'output is in Unicode \\xe4\\xf6\\xfc\\u20ac' + >>> unicode(t) + u'output is in Unicode \\xe4\\xf6\\xfc\\u20ac' + + with data:: + >>> t = Template("hello @!name!@", data={"name":"world"}) + >>> t() + u'hello world' + >>> t(name="worlds") + u'hello worlds' + + # >>> t(note="data must be Unicode or ASCII", name=u"\\xe4") + # u'hello \\xe4' + + escaping:: + >>> t = Template("hello escaped: @!name!@, unescaped: $!name!$") + >>> t(name='''<>&'"''') + u'hello escaped: <>&'", unescaped: <>&\\'"' + + result-encoding:: + # encode the unicode-object to your encoding with encode() + >>> t = Template(u"hello \\xe4\\xf6\\xfc\\u20ac") + >>> result = t() + >>> result + u'hello \\xe4\\xf6\\xfc\\u20ac' + >>> result.encode("utf-8") + 'hello \\xc3\\xa4\\xc3\\xb6\\xc3\\xbc\\xe2\\x82\\xac' + >>> result.encode("ascii") + Traceback (most recent call last): + ... + UnicodeEncodeError: 'ascii' codec can't encode characters in position 6-9: ordinal not in range(128) + >>> result.encode("ascii", 'xmlcharrefreplace') + 'hello äöü€' + + Python-expressions:: + >>> Template('formatted: @! "%8.5f" % value !@')(value=3.141592653) + u'formatted: 3.14159' + >>> Template("hello --@!name.upper().center(20)!@--")(name="world") + u'hello -- WORLD --' + >>> Template("calculate @!var*5+7!@")(var=7) + u'calculate 42' + + blocks (if/for/macros/...):: + >>> t = Template("barbazunknown(@!foo!@)") + >>> t(foo=2) + u'baz' + >>> t(foo=5) + u'unknown(5)' + + >>> t = Template("@!i!@ (empty)") + >>> t(mylist=[]) + u'(empty)' + >>> t(mylist=[1,2,3]) + u'1 2 3 ' + + >>> t = Template(" - @!i!@: @!elem!@") + >>> t(mylist=["a","b","c"]) + u' - 0: a - 1: b - 2: c' + + >>> t = Template('hello @!name!@ @!greetings(name=user)!@') + >>> t(user="monty") + u' hello monty' + + exists:: + >>> t = Template('YESNO') + >>> t() + u'NO' + >>> t(foo=1) + u'YES' + >>> t(foo=None) # note this difference to 'default()' + u'YES' + + default-values:: + # non-existing variables raise an error + >>> Template('hi @!optional!@')() + Traceback (most recent call last): + ... + TemplateRenderError: Cannot eval expression 'optional'. (NameError: name 'optional' is not defined) + + >>> t = Template('hi @!default("optional","anyone")!@') + >>> t() + u'hi anyone' + >>> t(optional=None) + u'hi anyone' + >>> t(optional="there") + u'hi there' + + # the 1st parameter can be any eval-expression + >>> t = Template('@!default("5*var1+var2","missing variable")!@') + >>> t(var1=10) + u'missing variable' + >>> t(var1=10, var2=2) + u'52' + + # also in blocks + >>> t = Template('yesno') + >>> t() + u'no' + >>> t(opt1=23, opt2=42) + u'yes' + + >>> t = Template('@!i!@') + >>> t() + u'' + >>> t(optional_list=[1,2,3]) + u'123' + + + # but make sure to put the expression in quotation marks, otherwise: + >>> Template('@!default(optional,"fallback")!@')() + Traceback (most recent call last): + ... + TemplateRenderError: Cannot eval expression 'default(optional,"fallback")'. (NameError: name 'optional' is not defined) + + setvar:: + >>> t = Template('$!setvar("i", "i+1")!$@!i!@') + >>> t(i=6) + u'7' + + >>> t = Template('''$!setvar("s", '"\\\\\\\\n".join(s)')!$@!s!@''') + >>> t(isinstance=isinstance, s="123") + u'123' + >>> t(isinstance=isinstance, s=["123", "456"]) + u'123\\n456' + +:Author: Roland Koebler (rk at simple-is-better dot org) +:Copyright: Roland Koebler +:License: MIT/X11-like, see __license__ + +:RCS: $Id: pyratemp.py,v 1.12 2013/04/02 20:26:06 rk Exp $ +""" +from __future__ import unicode_literals + +__version__ = "0.3.0" +__author__ = "Roland Koebler " +__license__ = """Copyright (c) Roland Koebler, 2007-2013 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +IN THE SOFTWARE.""" + +#========================================= + +import os, re, sys +if sys.version_info[0] >= 3: + import builtins + unicode = str + long = int +else: + import __builtin__ as builtins + from codecs import open + +#========================================= +# some useful functions + +#---------------------- +# string-position: i <-> row,col + +def srow(string, i): + """Get line numer of ``string[i]`` in `string`. + + :Returns: row, starting at 1 + :Note: This works for text-strings with ``\\n`` or ``\\r\\n``. + """ + return string.count('\n', 0, max(0, i)) + 1 + +def scol(string, i): + """Get column number of ``string[i]`` in `string`. + + :Returns: column, starting at 1 (but may be <1 if i<0) + :Note: This works for text-strings with ``\\n`` or ``\\r\\n``. + """ + return i - string.rfind('\n', 0, max(0, i)) + +def sindex(string, row, col): + """Get index of the character at `row`/`col` in `string`. + + :Parameters: + - `row`: row number, starting at 1. + - `col`: column number, starting at 1. + :Returns: ``i``, starting at 0 (but may be <1 if row/col<0) + :Note: This works for text-strings with '\\n' or '\\r\\n'. + """ + n = 0 + for _ in range(row-1): + n = string.find('\n', n) + 1 + return n+col-1 + +#---------------------- + +def dictkeyclean(d): + """Convert all keys of the dict `d` to strings. + """ + new_d = {} + for k, v in d.items(): + new_d[str(k)] = v + return new_d + +#---------------------- + +def dummy(*_, **__): + """Dummy function, doing nothing. + """ + pass + +def dummy_raise(exception, value): + """Create an exception-raising dummy function. + + :Returns: dummy function, raising ``exception(value)`` + """ + def mydummy(*_, **__): + raise exception(value) + return mydummy + +#========================================= +# escaping + +(NONE, HTML, LATEX, MAIL_HEADER) = range(0, 4) +ESCAPE_SUPPORTED = {"NONE":None, "HTML":HTML, "LATEX":LATEX, "MAIL_HEADER":MAIL_HEADER} + +def escape(s, format=HTML): + """Replace special characters by their escape sequence. + + :Parameters: + - `s`: unicode-string to escape + - `format`: + + - `NONE`: nothing is replaced + - `HTML`: replace &<>'" by &...; + - `LATEX`: replace \#$%&_{}~^ + - `MAIL_HEADER`: escape non-ASCII mail-header-contents + :Returns: + the escaped string in unicode + :Exceptions: + - `ValueError`: if `format` is invalid. + + :Uses: + MAIL_HEADER uses module email + """ + #Note: If you have to make sure that every character gets replaced + # only once (and if you cannot achieve this with the following code), + # use something like "".join([replacedict.get(c,c) for c in s]) + # which is about 2-3 times slower (but maybe needs less memory). + #Note: This is one of the most time-consuming parts of the template. + if format is None or format == NONE: + pass + elif format == HTML: + s = s.replace("&", "&") # must be done first! + s = s.replace("<", "<") + s = s.replace(">", ">") + s = s.replace('"', """) + s = s.replace("'", "'") + elif format == LATEX: + s = s.replace("\\", "\\x") #must be done first! + s = s.replace("#", "\\#") + s = s.replace("$", "\\$") + s = s.replace("%", "\\%") + s = s.replace("&", "\\&") + s = s.replace("_", "\\_") + s = s.replace("{", "\\{") + s = s.replace("}", "\\}") + s = s.replace("\\x","\\textbackslash{}") + s = s.replace("~", "\\textasciitilde{}") + s = s.replace("^", "\\textasciicircum{}") + elif format == MAIL_HEADER: + import email.header + try: + s.encode("ascii") + return s + except UnicodeEncodeError: + return email.header.make_header([(s, "utf-8")]).encode() + else: + raise ValueError('Invalid format (only None, HTML, LATEX and MAIL_HEADER are supported).') + return s + +#========================================= + +#----------------------------------------- +# Exceptions + +class TemplateException(Exception): + """Base class for template-exceptions.""" + pass + +class TemplateParseError(TemplateException): + """Template parsing failed.""" + def __init__(self, err, errpos): + """ + :Parameters: + - `err`: error-message or exception to wrap + - `errpos`: ``(filename,row,col)`` where the error occured. + """ + self.err = err + self.filename, self.row, self.col = errpos + TemplateException.__init__(self) + def __str__(self): + if not self.filename: + return "line %d, col %d: %s" % (self.row, self.col, str(self.err)) + else: + return "file %s, line %d, col %d: %s" % (self.filename, self.row, self.col, str(self.err)) + +class TemplateSyntaxError(TemplateParseError, SyntaxError): + """Template syntax-error.""" + pass + +class TemplateIncludeError(TemplateParseError): + """Template 'include' failed.""" + pass + +class TemplateRenderError(TemplateException): + """Template rendering failed.""" + pass + +#----------------------------------------- +# Loader + +class LoaderString: + """Load template from a string/unicode. + + Note that 'include' is not possible in such templates. + """ + def __init__(self, encoding='utf-8'): + self.encoding = encoding + + def load(self, s): + """Return template-string as unicode. + """ + if isinstance(s, unicode): + u = s + else: + u = s.decode(self.encoding) + return u + +class LoaderFile: + """Load template from a file. + + When loading a template from a file, it's possible to including other + templates (by using 'include' in the template). But for simplicity + and security, all included templates have to be in the same directory! + (see ``allowed_path``) + """ + def __init__(self, allowed_path=None, encoding='utf-8'): + """Init the loader. + + :Parameters: + - `allowed_path`: path of the template-files + - `encoding`: encoding of the template-files + :Exceptions: + - `ValueError`: if `allowed_path` is not a directory + """ + if allowed_path and not os.path.isdir(allowed_path): + raise ValueError("'allowed_path' has to be a directory.") + self.path = allowed_path + self.encoding = encoding + + def load(self, filename): + """Load a template from a file. + + Check if filename is allowed and return its contens in unicode. + + :Parameters: + - `filename`: filename of the template without path + :Returns: + the contents of the template-file in unicode + :Exceptions: + - `ValueError`: if `filename` contains a path + """ + if filename != os.path.basename(filename): + raise ValueError("No path allowed in filename. (%s)" %(filename)) + filename = os.path.join(self.path, filename) + + f = open(filename, 'r', encoding=self.encoding) + u = f.read() + f.close() + + return u + +#----------------------------------------- +# Parser + +class Parser(object): + """Parse a template into a parse-tree. + + Includes a syntax-check, an optional expression-check and verbose + error-messages. + + See documentation for a description of the parse-tree. + """ + # template-syntax + _comment_start = "#!" + _comment_end = "!#" + _sub_start = "$!" + _sub_end = "!$" + _subesc_start = "@!" + _subesc_end = "!@" + _block_start = "" + + # build regexps + # comment + # single-line, until end-tag or end-of-line. + _strComment = r"""%s(?P.*?)(?P%s|\n|$)""" \ + % (re.escape(_comment_start), re.escape(_comment_end)) + _reComment = re.compile(_strComment, re.M) + + # escaped or unescaped substitution + # single-line ("|$" is needed to be able to generate good error-messges) + _strSubstitution = r""" + ( + %s\s*(?P.*?)\s*(?P%s|$) #substitution + | + %s\s*(?P.*?)\s*(?P%s|$) #escaped substitution + ) + """ % (re.escape(_sub_start), re.escape(_sub_end), + re.escape(_subesc_start), re.escape(_subesc_end)) + _reSubstitution = re.compile(_strSubstitution, re.X|re.M) + + # block + # - single-line, no nesting. + # or + # - multi-line, nested by whitespace indentation: + # * start- and end-tag of a block must have exactly the same indentation. + # * start- and end-tags of *nested* blocks should have a greater indentation. + # NOTE: A single-line block must not start at beginning of the line with + # the same indentation as the enclosing multi-line blocks! + # Note that " " and "\t" are different, although they may + # look the same in an editor! + _s = re.escape(_block_start) + _e = re.escape(_block_end) + _strBlock = r""" + ^(?P[ \t]*)%send%s(?P.*)\r?\n? # multi-line end (^ IGNORED_TEXT\n) + | + (?P)%send%s # single-line end () + | + (?P[ \t]*) # single-line tag (no nesting) + %s(?P\w+)[ \t]*(?P.*?)%s + (?P.*?) + (?=(?:%s.*?%s.*?)??%send%s) # (match until end or i.e. ) + | + # multi-line tag, nested by whitespace indentation + ^(?P[ \t]*) # save indentation of start tag + %s(?P\w+)\s*(?P.*?)%s(?P.*)\r?\n + (?P(?:.*\n)*?) + (?=(?P=indent)%s(?:.|\s)*?%s) # match indentation + """ % (_s, _e, + _s, _e, + _s, _e, _s, _e, _s, _e, + _s, _e, _s, _e) + _reBlock = re.compile(_strBlock, re.X|re.M) + + # "for"-block parameters: "var(,var)* in ..." + _strForParam = r"""^(?P\w+(?:\s*,\s*\w+)*)\s+in\s+(?P.+)$""" + _reForParam = re.compile(_strForParam) + + # allowed macro-names + _reMacroParam = re.compile(r"""^\w+$""") + + + def __init__(self, loadfunc=None, testexpr=None, escape=HTML): + """Init the parser. + + :Parameters: + - `loadfunc`: function to load included templates + (i.e. ``LoaderFile(...).load``) + - `testexpr`: function to test if a template-expressions is valid + (i.e. ``EvalPseudoSandbox().compile``) + - `escape`: default-escaping (may be modified by the template) + :Exceptions: + - `ValueError`: if `testexpr` or `escape` is invalid. + """ + if loadfunc is None: + self._load = dummy_raise(NotImplementedError, "'include' not supported, since no 'loadfunc' was given.") + else: + self._load = loadfunc + + if testexpr is None: + self._testexprfunc = dummy + else: + try: # test if testexpr() works + testexpr("i==1") + except Exception as err: + raise ValueError("Invalid 'testexpr'. (%s)" %(err)) + self._testexprfunc = testexpr + + if escape not in ESCAPE_SUPPORTED.values(): + raise ValueError("Unsupported 'escape'. (%s)" %(escape)) + self.escape = escape + self._includestack = [] + + def parse(self, template): + """Parse a template. + + :Parameters: + - `template`: template-unicode-string + :Returns: the resulting parse-tree + :Exceptions: + - `TemplateSyntaxError`: for template-syntax-errors + - `TemplateIncludeError`: if template-inclusion failed + - `TemplateException` + """ + self._includestack = [(None, template)] # for error-messages (_errpos) + return self._parse(template) + + def _errpos(self, fpos): + """Convert `fpos` to ``(filename,row,column)`` for error-messages.""" + filename, string = self._includestack[-1] + return filename, srow(string, fpos), scol(string, fpos) + + def _testexpr(self, expr, fpos=0): + """Test a template-expression to detect errors.""" + try: + self._testexprfunc(expr) + except SyntaxError as err: + raise TemplateSyntaxError(err, self._errpos(fpos)) + + def _parse_sub(self, parsetree, text, fpos=0): + """Parse substitutions, and append them to the parse-tree. + + Additionally, remove comments. + """ + curr = 0 + for match in self._reSubstitution.finditer(text): + start = match.start() + if start > curr: + parsetree.append(("str", self._reComment.sub('', text[curr:start]))) + + if match.group("sub") is not None: + if not match.group("end"): + raise TemplateSyntaxError("Missing closing tag '%s' for '%s'." + % (self._sub_end, match.group()), self._errpos(fpos+start)) + if len(match.group("sub")) > 0: + self._testexpr(match.group("sub"), fpos+start) + parsetree.append(("sub", match.group("sub"))) + else: + assert(match.group("escsub") is not None) + if not match.group("escend"): + raise TemplateSyntaxError("Missing closing tag '%s' for '%s'." + % (self._subesc_end, match.group()), self._errpos(fpos+start)) + if len(match.group("escsub")) > 0: + self._testexpr(match.group("escsub"), fpos+start) + parsetree.append(("esc", self.escape, match.group("escsub"))) + + curr = match.end() + + if len(text) > curr: + parsetree.append(("str", self._reComment.sub('', text[curr:]))) + + def _parse(self, template, fpos=0): + """Recursive part of `parse()`. + + :Parameters: + - template + - fpos: position of ``template`` in the complete template (for error-messages) + """ + # blank out comments + # (So that its content does not collide with other syntax, and + # because removing them completely would falsify the character- + # position ("match.start()") of error-messages) + template = self._reComment.sub(lambda match: self._comment_start+" "*len(match.group(1))+match.group(2), template) + + # init parser + parsetree = [] + curr = 0 # current position (= end of previous block) + block_type = None # block type: if,for,macro,raw,... + block_indent = None # None: single-line, >=0: multi-line + + # find blocks + for match in self._reBlock.finditer(template): + start = match.start() + # process template-part before this block + if start > curr: + self._parse_sub(parsetree, template[curr:start], fpos) + + # analyze block syntax (incl. error-checking and -messages) + keyword = None + block = match.groupdict() + pos__ = fpos + start # shortcut + if block["sKeyw"] is not None: # single-line block tag + block_indent = None + keyword = block["sKeyw"] + param = block["sParam"] + content = block["sContent"] + if block["sSpace"]: # restore spaces before start-tag + if len(parsetree) > 0 and parsetree[-1][0] == "str": + parsetree[-1] = ("str", parsetree[-1][1] + block["sSpace"]) + else: + parsetree.append(("str", block["sSpace"])) + pos_p = fpos + match.start("sParam") # shortcuts + pos_c = fpos + match.start("sContent") + elif block["mKeyw"] is not None: # multi-line block tag + block_indent = len(block["indent"]) + keyword = block["mKeyw"] + param = block["mParam"] + content = block["mContent"] + pos_p = fpos + match.start("mParam") + pos_c = fpos + match.start("mContent") + ignored = block["mIgnored"].strip() + if ignored and ignored != self._comment_start: + raise TemplateSyntaxError("No code allowed after block-tag.", self._errpos(fpos+match.start("mIgnored"))) + elif block["mEnd"] is not None: # multi-line block end + if block_type is None: + raise TemplateSyntaxError("No block to end here/invalid indent.", self._errpos(pos__) ) + if block_indent != len(block["mEnd"]): + raise TemplateSyntaxError("Invalid indent for end-tag.", self._errpos(pos__) ) + ignored = block["meIgnored"].strip() + if ignored and ignored != self._comment_start: + raise TemplateSyntaxError("No code allowed after end-tag.", self._errpos(fpos+match.start("meIgnored"))) + block_type = None + elif block["sEnd"] is not None: # single-line block end + if block_type is None: + raise TemplateSyntaxError("No block to end here/invalid indent.", self._errpos(pos__)) + if block_indent is not None: + raise TemplateSyntaxError("Invalid indent for end-tag.", self._errpos(pos__)) + block_type = None + else: + raise TemplateException("FATAL: Block regexp error. Please contact the author. (%s)" % match.group()) + + # analyze block content (mainly error-checking and -messages) + if keyword: + keyword = keyword.lower() + if 'for' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'for' + cond = self._reForParam.match(param) + if cond is None: + raise TemplateSyntaxError("Invalid 'for ...' at '%s'." %(param), self._errpos(pos_p)) + names = tuple(n.strip() for n in cond.group("names").split(",")) + self._testexpr(cond.group("iter"), pos_p+cond.start("iter")) + parsetree.append(("for", names, cond.group("iter"), self._parse(content, pos_c))) + elif 'if' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block at '%s'." %(match.group()), self._errpos(pos__)) + if not param: + raise TemplateSyntaxError("Missing condition for 'if' at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'if' + self._testexpr(param, pos_p) + parsetree.append(("if", param, self._parse(content, pos_c))) + elif 'elif' == keyword: + if block_type != 'if': + raise TemplateSyntaxError("'elif' may only appear after 'if' at '%s'." %(match.group()), self._errpos(pos__)) + if not param: + raise TemplateSyntaxError("Missing condition for 'elif' at '%s'." %(match.group()), self._errpos(pos__)) + self._testexpr(param, pos_p) + parsetree.append(("elif", param, self._parse(content, pos_c))) + elif 'else' == keyword: + if block_type not in ('if', 'for'): + raise TemplateSyntaxError("'else' may only appear after 'if' or 'for' at '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'else' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + parsetree.append(("else", self._parse(content, pos_c))) + elif 'macro' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'macro' + # make sure param is "\w+" (instead of ".+") + if not param: + raise TemplateSyntaxError("Missing name for 'macro' at '%s'." %(match.group()), self._errpos(pos__)) + if not self._reMacroParam.match(param): + raise TemplateSyntaxError("Invalid name for 'macro' at '%s'." %(match.group()), self._errpos(pos__)) + #remove last newline + if len(content) > 0 and content[-1] == '\n': + content = content[:-1] + if len(content) > 0 and content[-1] == '\r': + content = content[:-1] + parsetree.append(("macro", param, self._parse(content, pos_c))) + + # parser-commands + elif 'raw' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'raw' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'raw' + parsetree.append(("str", content)) + elif 'include' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'include' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'include' + try: + u = self._load(content.strip()) + except Exception as err: + raise TemplateIncludeError(err, self._errpos(pos__)) + self._includestack.append((content.strip(), u)) # current filename/template for error-msg. + p = self._parse(u) + self._includestack.pop() + parsetree.extend(p) + elif 'set_escape' == keyword: + if block_type is not None: + raise TemplateSyntaxError("Missing block-end-tag before new block '%s'." %(match.group()), self._errpos(pos__)) + if param: + raise TemplateSyntaxError("'set_escape' may not have parameters at '%s'." %(match.group()), self._errpos(pos__)) + block_type = 'set_escape' + esc = content.strip().upper() + if esc not in ESCAPE_SUPPORTED: + raise TemplateSyntaxError("Unsupported escape '%s'." %(esc), self._errpos(pos__)) + self.escape = ESCAPE_SUPPORTED[esc] + else: + raise TemplateSyntaxError("Invalid keyword '%s'." %(keyword), self._errpos(pos__)) + curr = match.end() + + if block_type is not None: + raise TemplateSyntaxError("Missing end-tag.", self._errpos(pos__)) + + if len(template) > curr: # process template-part after last block + self._parse_sub(parsetree, template[curr:], fpos+curr) + + return parsetree + +#----------------------------------------- +# Evaluation + +# some checks +assert len(eval("dir()", {'__builtins__':{'dir':dir}})) == 1, \ + "FATAL: 'eval' does not work as expected (%s)." +assert compile("0 .__class__", "", "eval").co_names == ('__class__',), \ + "FATAL: 'compile' does not work as expected." + +class EvalPseudoSandbox: + """An eval-pseudo-sandbox. + + The pseudo-sandbox restricts the available functions/objects, so the + code can only access: + + - some of the builtin Python-functions, which are considered "safe" + (see safe_builtins) + - some additional functions (exists(), default(), setvar(), escape()) + - the passed objects incl. their methods. + + Additionally, names beginning with "_" are forbidden. + This is to prevent things like '0 .__class__', with which you could + easily break out of a "sandbox". + + Be careful to only pass "safe" objects/functions to the template, + because any unsafe function/method could break the sandbox! + For maximum security, restrict the access to as few objects/functions + as possible! + + :Warning: + Note that this is no real sandbox! (And although I don't know any + way to break out of the sandbox without passing-in an unsafe object, + I cannot guarantee that there is no such way. So use with care.) + + Take care if you want to use it for untrusted code!! + """ + + safe_builtins = { + "True" : True, + "False" : False, + "None" : None, + + "abs" : builtins.abs, + "chr" : builtins.chr, + "divmod" : builtins.divmod, + "hash" : builtins.hash, + "hex" : builtins.hex, + "len" : builtins.len, + "max" : builtins.max, + "min" : builtins.min, + "oct" : builtins.oct, + "ord" : builtins.ord, + "pow" : builtins.pow, + "range" : builtins.range, + "round" : builtins.round, + "sorted" : builtins.sorted, + "sum" : builtins.sum, + "unichr" : builtins.chr, + "zip" : builtins.zip, + + "bool" : builtins.bool, + "bytes" : builtins.bytes, + "complex" : builtins.complex, + "dict" : builtins.dict, + "enumerate" : builtins.enumerate, + "float" : builtins.float, + "int" : builtins.int, + "list" : builtins.list, + "long" : long, + "reversed" : builtins.reversed, + "str" : builtins.str, + "tuple" : builtins.tuple, + "unicode" : unicode, + } + if sys.version_info[0] < 3: + safe_builtins["unichr"] = builtins.unichr + + def __init__(self): + self._compile_cache = {} + self.locals_ptr = None + self.eval_allowed_globals = self.safe_builtins.copy() + self.register("__import__", self.f_import) + self.register("exists", self.f_exists) + self.register("default", self.f_default) + self.register("setvar", self.f_setvar) + self.register("escape", self.f_escape) + + def register(self, name, obj): + """Add an object to the "allowed eval-globals". + + Mainly useful to add user-defined functions to the pseudo-sandbox. + """ + self.eval_allowed_globals[name] = obj + + def compile(self, expr): + """Compile a Python-eval-expression. + + - Use a compile-cache. + - Raise a `NameError` if `expr` contains a name beginning with ``_``. + + :Returns: the compiled `expr` + :Exceptions: + - `SyntaxError`: for compile-errors + - `NameError`: if expr contains a name beginning with ``_`` + """ + if expr not in self._compile_cache: + c = compile(expr, "", "eval") + for i in c.co_names: #prevent breakout via new-style-classes + if i[0] == '_': + raise NameError("Name '%s' is not allowed." % i) + self._compile_cache[expr] = c + return self._compile_cache[expr] + + def eval(self, expr, locals): + """Eval a Python-eval-expression. + + Sets ``self.locals_ptr`` to ``locales`` and compiles the code + before evaluating. + """ + sav = self.locals_ptr + self.locals_ptr = locals + x = eval(self.compile(expr), {"__builtins__":self.eval_allowed_globals}, locals) + self.locals_ptr = sav + return x + + def f_import(self, name, *_, **__): + """``import``/``__import__()`` for the sandboxed code. + + Since "import" is insecure, the PseudoSandbox does not allow to + import other modules. But since some functions need to import + other modules (e.g. "datetime.datetime.strftime" imports "time"), + this function replaces the builtin "import" and allows to use + modules which are already accessible by the sandboxed code. + + :Note: + - This probably only works for rather simple imports. + - For security, it may be better to avoid such (complex) modules + which import other modules. (e.g. use time.localtime and + time.strftime instead of datetime.datetime.strftime, + or write a small wrapper.) + + :Example: + + >>> from datetime import datetime + >>> import pyratemp + >>> t = pyratemp.Template('@!mytime.strftime("%H:%M:%S")!@') + + # >>> print(t(mytime=datetime.now())) + # Traceback (most recent call last): + # ... + # ImportError: import not allowed in pseudo-sandbox; try to import 'time' yourself and pass it to the sandbox/template + + >>> import time + >>> print(t(mytime=datetime.strptime("13:40:54", "%H:%M:%S"), time=time)) + 13:40:54 + + # >>> print(t(mytime=datetime.now(), time=time)) + # 13:40:54 + """ + import types + if self.locals_ptr is not None and name in self.locals_ptr and isinstance(self.locals_ptr[name], types.ModuleType): + return self.locals_ptr[name] + else: + raise ImportError("import not allowed in pseudo-sandbox; try to import '%s' yourself (and maybe pass it to the sandbox/template)" % name) + + def f_exists(self, varname): + """``exists()`` for the sandboxed code. + + Test if the variable `varname` exists in the current locals-namespace. + + This only works for single variable names. If you want to test + complicated expressions, use i.e. `default`. + (i.e. `default("expr",False)`) + + :Note: the variable-name has to be quoted! (like in eval) + :Example: see module-docstring + """ + return (varname in self.locals_ptr) + + def f_default(self, expr, default=None): + """``default()`` for the sandboxed code. + + Try to evaluate an expression and return the result or a + fallback-/default-value; the `default`-value is used + if `expr` does not exist/is invalid/results in None. + + This is very useful for optional data. + + :Parameter: + - expr: eval-expression + - default: fallback-falue if eval(expr) fails or is None. + :Returns: + the eval-result or the "fallback"-value. + + :Note: the eval-expression has to be quoted! (like in eval) + :Example: see module-docstring + """ + try: + r = self.eval(expr, self.locals_ptr) + if r is None: + return default + return r + #TODO: which exceptions should be catched here? + except (NameError, LookupError, TypeError): + return default + + def f_setvar(self, name, expr): + """``setvar()`` for the sandboxed code. + + Set a variable. + + :Example: see module-docstring + """ + self.locals_ptr[name] = self.eval(expr, self.locals_ptr) + return "" + + def f_escape(self, s, format="HTML"): + """``escape()`` for the sandboxed code. + """ + if isinstance(format, (str, unicode)): + format = ESCAPE_SUPPORTED[format.upper()] + return escape(unicode(s), format) + +#----------------------------------------- +# basic template / subtemplate + +class TemplateBase: + """Basic template-class. + + Used both for the template itself and for 'macro's ("subtemplates") in + the template. + """ + + def __init__(self, parsetree, renderfunc, data=None): + """Create the Template/Subtemplate/Macro. + + :Parameters: + - `parsetree`: parse-tree of the template/subtemplate/macro + - `renderfunc`: render-function + - `data`: data to fill into the template by default (dictionary). + This data may later be overridden when rendering the template. + :Exceptions: + - `TypeError`: if `data` is not a dictionary + """ + #TODO: parameter-checking? + self.parsetree = parsetree + if isinstance(data, dict): + self.data = data + elif data is None: + self.data = {} + else: + raise TypeError('"data" must be a dict (or None).') + self.current_data = data + self._render = renderfunc + + def __call__(self, **override): + """Fill out/render the template. + + :Parameters: + - `override`: objects to add to the data-namespace, overriding + the "default"-data. + :Returns: the filled template (in unicode) + :Note: This is also called when invoking macros + (i.e. ``$!mymacro()!$``). + """ + self.current_data = self.data.copy() + self.current_data.update(override) + u = "".join(self._render(self.parsetree, self.current_data)) + self.current_data = self.data # restore current_data + return _dontescape(u) # (see class _dontescape) + + def __unicode__(self): + """Alias for __call__().""" + return self.__call__() + def __str__(self): + """Alias for __call__().""" + return self.__call__() + +#----------------------------------------- +# Renderer + +class _dontescape(unicode): + """Unicode-string which should not be escaped. + + If ``isinstance(object,_dontescape)``, then don't escape the object in + ``@!...!@``. It's useful for not double-escaping macros, and it's + automatically used for macros/subtemplates. + + :Note: This only works if the object is used on its own in ``@!...!@``. + It i.e. does not work in ``@!object*2!@`` or ``@!object + "hi"!@``. + """ + __slots__ = [] + + +class Renderer(object): + """Render a template-parse-tree. + + :Uses: `TemplateBase` for macros + """ + + def __init__(self, evalfunc, escapefunc): + """Init the renderer. + + :Parameters: + - `evalfunc`: function for template-expression-evaluation + (i.e. ``EvalPseudoSandbox().eval``) + - `escapefunc`: function for escaping special characters + (i.e. `escape`) + """ + #TODO: test evalfunc + self.evalfunc = evalfunc + self.escapefunc = escapefunc + + def _eval(self, expr, data): + """evalfunc with error-messages""" + try: + return self.evalfunc(expr, data) + #TODO: any other errors to catch here? + except (TypeError,NameError,LookupError,AttributeError, SyntaxError) as err: + raise TemplateRenderError("Cannot eval expression '%s'. (%s: %s)" %(expr, err.__class__.__name__, err)) + + def render(self, parsetree, data): + """Render a parse-tree of a template. + + :Parameters: + - `parsetree`: the parse-tree + - `data`: the data to fill into the template (dictionary) + :Returns: the rendered output-unicode-string + :Exceptions: + - `TemplateRenderError` + """ + _eval = self._eval # shortcut + output = [] + do_else = False # use else/elif-branch? + + if parsetree is None: + return "" + for elem in parsetree: + if "str" == elem[0]: + output.append(elem[1]) + elif "sub" == elem[0]: + output.append(unicode(_eval(elem[1], data))) + elif "esc" == elem[0]: + obj = _eval(elem[2], data) + #prevent double-escape + if isinstance(obj, _dontescape) or isinstance(obj, TemplateBase): + output.append(unicode(obj)) + else: + output.append(self.escapefunc(unicode(obj), elem[1])) + elif "for" == elem[0]: + do_else = True + (names, iterable) = elem[1:3] + try: + loop_iter = iter(_eval(iterable, data)) + except TypeError: + raise TemplateRenderError("Cannot loop over '%s'." % iterable) + for i in loop_iter: + do_else = False + if len(names) == 1: + data[names[0]] = i + else: + data.update(zip(names, i)) #"for a,b,.. in list" + output.extend(self.render(elem[3], data)) + elif "if" == elem[0]: + do_else = True + if _eval(elem[1], data): + do_else = False + output.extend(self.render(elem[2], data)) + elif "elif" == elem[0]: + if do_else and _eval(elem[1], data): + do_else = False + output.extend(self.render(elem[2], data)) + elif "else" == elem[0]: + if do_else: + do_else = False + output.extend(self.render(elem[1], data)) + elif "macro" == elem[0]: + data[elem[1]] = TemplateBase(elem[2], self.render, data) + else: + raise TemplateRenderError("Invalid parse-tree (%s)." %(elem)) + + return output + +#----------------------------------------- +# template user-interface (putting it all together) + +class Template(TemplateBase): + """Template-User-Interface. + + :Usage: + :: + t = Template(...) (<- see __init__) + output = t(...) (<- see TemplateBase.__call__) + + :Example: + see module-docstring + """ + + def __init__(self, string=None,filename=None,parsetree=None, encoding='utf-8', data=None, escape=HTML, + loader_class=LoaderFile, + parser_class=Parser, + renderer_class=Renderer, + eval_class=EvalPseudoSandbox, + escape_func=escape): + """Load (+parse) a template. + + :Parameters: + - `string,filename,parsetree`: a template-string, + filename of a template to load, + or a template-parsetree. + (only one of these 3 is allowed) + - `encoding`: encoding of the template-files (only used for "filename") + - `data`: data to fill into the template by default (dictionary). + This data may later be overridden when rendering the template. + - `escape`: default-escaping for the template, may be overwritten by the template! + - `loader_class` + - `parser_class` + - `renderer_class` + - `eval_class` + - `escapefunc` + """ + if [string, filename, parsetree].count(None) != 2: + raise ValueError('Exactly 1 of string,filename,parsetree is necessary.') + + tmpl = None + # load template + if filename is not None: + incl_load = loader_class(os.path.dirname(filename), encoding).load + tmpl = incl_load(os.path.basename(filename)) + if string is not None: + incl_load = dummy_raise(NotImplementedError, "'include' not supported for template-strings.") + tmpl = LoaderString(encoding).load(string) + + # eval (incl. compile-cache) + templateeval = eval_class() + + # parse + if tmpl is not None: + p = parser_class(loadfunc=incl_load, testexpr=templateeval.compile, escape=escape) + parsetree = p.parse(tmpl) + del p + + # renderer + renderfunc = renderer_class(templateeval.eval, escape_func).render + + #create template + TemplateBase.__init__(self, parsetree, renderfunc, data) + + +#========================================= +#doctest + +def _doctest(): + """doctest this module.""" + import doctest + doctest.testmod() + +#---------------------- +if __name__ == '__main__': + if sys.version_info[0] <= 2: + _doctest() + +#========================================= diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdlc b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdlc new file mode 100755 index 0000000000..d1718173c0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/libuavcan_dsdlc @@ -0,0 +1,64 @@ +#!/usr/bin/env python +# +# UAVCAN DSDL compiler for libuavcan +# Supported Python versions: 3.2+, 2.7. +# +# Copyright (C) 2014 Pavel Kirienko +# + +from __future__ import division, absolute_import, print_function, unicode_literals +import os, sys, logging, argparse + +# This trickery allows to use the compiler even if pyuavcan is not installed in the system. +# This is extremely important, as it makes the compiler (and therefore libuavcan in general) +# totally dependency-free, except for the Python interpreter itself. +SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__)) +LOCAL_PYUAVCAN_DIR = os.path.join(SCRIPT_DIR, 'pydronecan') +RUNNING_FROM_SRC_DIR = os.path.isdir(LOCAL_PYUAVCAN_DIR) +if RUNNING_FROM_SRC_DIR: + #print('Running from the source directory') + sys.path.insert(0, SCRIPT_DIR) + sys.path.insert(0, LOCAL_PYUAVCAN_DIR) + +def configure_logging(verbosity): + fmt = '%(message)s' + level = { 0: logging.WARNING, 1: logging.INFO, 2: logging.DEBUG }.get(verbosity or 0, logging.DEBUG) + logging.basicConfig(stream=sys.stderr, level=level, format=fmt) + +def die(text): + print(text, file=sys.stderr) + exit(1) + +DEFAULT_OUTDIR = 'dsdlc_generated' + +DESCRIPTION = '''UAVCAN DSDL compiler for libuavcan. +Takes an input directory that contains an hierarchy of DSDL +definitions and converts it into compatible hierarchy of C++ types for libuavcan. +This script can be used directly from the source directory, no installation required! +Supported Python versions: 3.2+, 2.7. +''' + +argparser = argparse.ArgumentParser(description=DESCRIPTION) +argparser.add_argument('source_dir', nargs='+', help='source directory with DSDL definitions') +argparser.add_argument('--verbose', '-v', action='count', help='verbosity level (-v, -vv)') +argparser.add_argument('--outdir', '-O', default=DEFAULT_OUTDIR, help='output directory, default %s' % DEFAULT_OUTDIR) +argparser.add_argument('--incdir', '-I', default=[], action='append', help= +'''nested type namespaces, one path per argument. Can be also specified through the environment variable +UAVCAN_DSDL_INCLUDE_PATH, where the path entries are separated by colons ":"''') +args = argparser.parse_args() + +configure_logging(args.verbose) + +try: + extra_incdir = os.environ['UAVCAN_DSDL_INCLUDE_PATH'].split(':') + logging.info('Additional include directories: %s', extra_incdir) + args.incdir += extra_incdir +except KeyError: + pass + +from libuavcan_dsdl_compiler import run as dsdlc_run +try: + dsdlc_run(args.source_dir, args.incdir, args.outdir) +except Exception as ex: + logging.error('Compiler failure', exc_info=True) + die(str(ex)) diff --git a/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan new file mode 160000 index 0000000000..19fdf2e5b3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/dsdl_compiler/pydronecan @@ -0,0 +1 @@ +Subproject commit 19fdf2e5b383243ccdb1094edae0603cf11469e8 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/build_config.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/build_config.hpp new file mode 100644 index 0000000000..8a886c2975 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/build_config.hpp @@ -0,0 +1,276 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_BUILD_CONFIG_HPP_INCLUDED +#define UAVCAN_BUILD_CONFIG_HPP_INCLUDED + +/** + * UAVCAN version definition + */ +#define UAVCAN_VERSION_MAJOR 1 +#define UAVCAN_VERSION_MINOR 0 + +/** + * UAVCAN_CPP_VERSION - version of the C++ standard used during compilation. + * This definition contains the integer year number after which the standard was named: + * - 2003 for C++03 + * - 2011 for C++11 + * + * This config automatically sets according to the actual C++ standard used by the compiler. + * + * In C++03 mode the library has almost zero dependency on the C++ standard library, which allows + * to use it on platforms with a very limited C++ support. On the other hand, C++11 mode requires + * many parts of the standard library (e.g. ), thus the user might want to force older + * standard than used by the compiler, in which case this symbol can be overridden manually via + * compiler flags. + */ +#define UAVCAN_CPP11 2011 +#define UAVCAN_CPP03 2003 + +#ifndef UAVCAN_CPP_VERSION +# if __cplusplus > 201200 +# error Unsupported C++ standard. You can explicitly set UAVCAN_CPP_VERSION=UAVCAN_CPP11 to silence this error. +# elif (__cplusplus > 201100) || defined(__GXX_EXPERIMENTAL_CXX0X__) +# define UAVCAN_CPP_VERSION UAVCAN_CPP11 +# else +# define UAVCAN_CPP_VERSION UAVCAN_CPP03 +# endif +#endif + +/** + * The library uses UAVCAN_NULLPTR instead of UAVCAN_NULLPTR and nullptr in order to allow the use of + * -Wzero-as-null-pointer-constant. + */ +#ifndef UAVCAN_NULLPTR +# if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# define UAVCAN_NULLPTR nullptr +# else +# define UAVCAN_NULLPTR NULL +# endif +#endif + +/** + * By default, libuavcan enables all features if it detects that it is being built for a general-purpose + * target like Linux. Value of this macro influences other configuration options located below in this file. + * This macro can be overriden if needed. + */ +#ifndef UAVCAN_GENERAL_PURPOSE_PLATFORM +# if (defined(__linux__) || defined(__linux) || defined(__APPLE__) ||\ + defined(_WIN64) || defined(_WIN32) || defined(__ANDROID__) ||\ + defined(_SYSTYPE_BSD) || defined(__FreeBSD__)) +# define UAVCAN_GENERAL_PURPOSE_PLATFORM 1 +# else +# define UAVCAN_GENERAL_PURPOSE_PLATFORM 0 +# endif +#endif + +/** + * This macro enables built-in runtime checks and debug output via printf(). + * Should be used only for library development. + */ +#ifndef UAVCAN_DEBUG +# define UAVCAN_DEBUG 0 +#endif + +/** + * This option allows to select whether libuavcan should throw exceptions on fatal errors, or try to handle + * errors differently. By default, exceptions will be enabled only if the library is built for a general-purpose + * OS like Linux. Set UAVCAN_EXCEPTIONS explicitly to override. + */ +#ifndef UAVCAN_EXCEPTIONS +# define UAVCAN_EXCEPTIONS UAVCAN_GENERAL_PURPOSE_PLATFORM +#endif + +/** + * This specification is used by some error reporting functions like in the Logger class. + * The default can be overriden by defining the macro UAVCAN_NOEXCEPT explicitly, e.g. via compiler options. + */ +#ifndef UAVCAN_NOEXCEPT +# if UAVCAN_EXCEPTIONS +# if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# define UAVCAN_NOEXCEPT noexcept +# else +# define UAVCAN_NOEXCEPT throw() +# endif +# else +# define UAVCAN_NOEXCEPT +# endif +#endif + +/** + * Declaration visibility + * http://gcc.gnu.org/wiki/Visibility + */ +#ifndef UAVCAN_EXPORT +# define UAVCAN_EXPORT +#endif + +/** + * Trade-off between ROM/RAM usage and functionality/determinism. + * Note that this feature is not well tested and should be avoided. + * Use code search for UAVCAN_TINY to find what functionality will be disabled. + * This is particularly useful for embedded systems with less than 40kB of ROM. + */ +#ifndef UAVCAN_TINY +# define UAVCAN_TINY 0 +#endif + +/** + * Disable the global data type registry, which can save some space on embedded systems. + */ +#ifndef UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY +# define UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY 0 +#endif + +/** + * toString() methods will be disabled by default, unless the library is built for a general-purpose target like Linux. + * It is not recommended to enable toString() on embedded targets as code size will explode. + */ +#ifndef UAVCAN_TOSTRING +# if UAVCAN_EXCEPTIONS +# define UAVCAN_TOSTRING UAVCAN_GENERAL_PURPOSE_PLATFORM +# else +# define UAVCAN_TOSTRING 0 +# endif +#endif + +#if UAVCAN_TOSTRING +# if !UAVCAN_EXCEPTIONS +# error UAVCAN_TOSTRING requires UAVCAN_EXCEPTIONS +# endif +# include +#endif + +/** + * Some C++ implementations are half-broken because they don't implement the placement new operator. + * If UAVCAN_IMPLEMENT_PLACEMENT_NEW is defined, libuavcan will implement its own operator new (std::size_t, void*) + * and its delete() counterpart, instead of relying on the standard header . + */ +#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW +# define UAVCAN_IMPLEMENT_PLACEMENT_NEW 0 +#endif + +/** + * Allows the user's application to provide custom implementation of uavcan::snprintf(), + * which is often useful on deeply embedded systems. + */ +#ifndef UAVCAN_USE_EXTERNAL_SNPRINTF +# define UAVCAN_USE_EXTERNAL_SNPRINTF 0 +#endif + +/** + * Allows the user's application to provide a custom implementation of IEEE754Converter::nativeIeeeToHalf and + * IEEE754Converter::halfToNativeIeee. + */ +#ifndef UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION +# define UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 0 +#endif + +/** + * Run time checks. + * Resolves to the standard assert() by default. + * Disabled completely if UAVCAN_NO_ASSERTIONS is defined. + */ +#ifndef UAVCAN_ASSERT +# ifndef UAVCAN_NO_ASSERTIONS +# define UAVCAN_NO_ASSERTIONS 0 +# endif +# if UAVCAN_NO_ASSERTIONS +# define UAVCAN_ASSERT(x) +# else +# define UAVCAN_ASSERT(x) assert(x) +# endif +#endif + +#ifndef UAVCAN_LIKELY +# if __GNUC__ +# define UAVCAN_LIKELY(x) __builtin_expect(!!(x), true) +# else +# define UAVCAN_LIKELY(x) (x) +# endif +#endif + +#ifndef UAVCAN_UNLIKELY +# if __GNUC__ +# define UAVCAN_UNLIKELY(x) __builtin_expect(!!(x), false) +# else +# define UAVCAN_UNLIKELY(x) (x) +# endif +#endif + +namespace uavcan +{ +/** + * Memory pool block size. + * + * The default of 64 bytes should be OK for any target arch up to AMD64 and any compiler. + * + * The library leverages compile-time checks to ensure that all types that are subject to dynamic allocation + * fit this size, otherwise compilation fails. + * + * For platforms featuring small pointer width (16..32 bits), UAVCAN_MEM_POOL_BLOCK_SIZE can often be safely + * reduced to 56 or even 48 bytes, which leads to lower memory footprint. + * + * Note that the pool block size shall be aligned at biggest alignment of the target platform (detected and + * checked automatically at compile time). + */ +#ifdef UAVCAN_MEM_POOL_BLOCK_SIZE +/// Explicitly specified by the user. +static const unsigned MemPoolBlockSize = UAVCAN_MEM_POOL_BLOCK_SIZE; +#elif defined(__BIGGEST_ALIGNMENT__) && (__BIGGEST_ALIGNMENT__ <= 8) +/// Convenient default for GCC-like compilers - if alignment allows, pool block size can be safely reduced. +static const unsigned MemPoolBlockSize = 56; +#else +/// Safe default that should be OK for any platform. +static const unsigned MemPoolBlockSize = 64; +#endif + +#ifdef __BIGGEST_ALIGNMENT__ +static const unsigned MemPoolAlignment = __BIGGEST_ALIGNMENT__; +#else +static const unsigned MemPoolAlignment = 16; +#endif + +typedef char _alignment_check_for_MEM_POOL_BLOCK_SIZE[((MemPoolBlockSize & (MemPoolAlignment - 1)) == 0) ? 1 : -1]; + +/** + * This class that allows to check at compile time whether type T can be allocated using the memory pool. + * If the check fails, compilation fails. + */ +template +struct UAVCAN_EXPORT IsDynamicallyAllocatable +{ + static void check() + { + char dummy[(sizeof(T) <= MemPoolBlockSize) ? 1 : -1] = { '0' }; + (void)dummy; + } +}; + +/** + * Float comparison precision. + * For details refer to: + * http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * https://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h + */ +#ifdef UAVCAN_FLOAT_COMPARISON_EPSILON_MULT +static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSILON_MULT; +#else +static const unsigned FloatComparisonEpsilonMult = 10; +#endif + +/** + * Maximum number of CAN acceptance filters available on the platform + */ +#ifdef UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS +/// Explicitly specified by the user. +static const unsigned MaxCanAcceptanceFilters = UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS; +#else +/// Default that should be OK for any platform. +static const unsigned MaxCanAcceptanceFilters = 32; +#endif + +} + +#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/data_type.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/data_type.hpp new file mode 100644 index 0000000000..96a8a7f71b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/data_type.hpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DATA_TYPE_HPP_INCLUDED +#define UAVCAN_DATA_TYPE_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT TransferCRC; + +enum DataTypeKind +{ + DataTypeKindService, + DataTypeKindMessage +}; + +static const uint8_t NumDataTypeKinds = 2; + + +static inline DataTypeKind getDataTypeKindForTransferType(const TransferType tt) +{ + if (tt == TransferTypeServiceResponse || + tt == TransferTypeServiceRequest) + { + return DataTypeKindService; + } + else if (tt == TransferTypeMessageBroadcast) + { + return DataTypeKindMessage; + } + else + { + UAVCAN_ASSERT(0); + return DataTypeKind(0); + } +} + + +class UAVCAN_EXPORT DataTypeID +{ + uint32_t value_; + +public: + static const uint16_t MaxServiceDataTypeIDValue = 255; + static const uint16_t MaxMessageDataTypeIDValue = 65535; + static const uint16_t MaxPossibleDataTypeIDValue = MaxMessageDataTypeIDValue; + + DataTypeID() : value_(0xFFFFFFFFUL) { } + + DataTypeID(uint16_t id) // Implicit + : value_(id) + { } + + static DataTypeID getMaxValueForDataTypeKind(const DataTypeKind dtkind); + + bool isValidForDataTypeKind(DataTypeKind dtkind) const + { + return value_ <= getMaxValueForDataTypeKind(dtkind).get(); + } + + uint16_t get() const { return static_cast(value_); } + + bool operator==(DataTypeID rhs) const { return value_ == rhs.value_; } + bool operator!=(DataTypeID rhs) const { return value_ != rhs.value_; } + + bool operator<(DataTypeID rhs) const { return value_ < rhs.value_; } + bool operator>(DataTypeID rhs) const { return value_ > rhs.value_; } + bool operator<=(DataTypeID rhs) const { return value_ <= rhs.value_; } + bool operator>=(DataTypeID rhs) const { return value_ >= rhs.value_; } +}; + + +/** + * CRC-64-WE + * Description: http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 + * Initial value: 0xFFFFFFFFFFFFFFFF + * Poly: 0x42F0E1EBA9EA3693 + * Reverse: no + * Output xor: 0xFFFFFFFFFFFFFFFF + * Check: 0x62EC59E3F1A4F00A + */ +class UAVCAN_EXPORT DataTypeSignatureCRC +{ + uint64_t crc_; + +public: + static DataTypeSignatureCRC extend(uint64_t crc); + + DataTypeSignatureCRC() : crc_(0xFFFFFFFFFFFFFFFFULL) { } + + void add(uint8_t byte); + + void add(const uint8_t* bytes, unsigned len); + + uint64_t get() const { return crc_ ^ 0xFFFFFFFFFFFFFFFFULL; } +}; + + +class UAVCAN_EXPORT DataTypeSignature +{ + uint64_t value_; + + void mixin64(uint64_t x); + +public: + DataTypeSignature() : value_(0) { } + explicit DataTypeSignature(uint64_t value) : value_(value) { } + + void extend(DataTypeSignature dts); + + TransferCRC toTransferCRC() const; + + uint64_t get() const { return value_; } + + bool operator==(DataTypeSignature rhs) const { return value_ == rhs.value_; } + bool operator!=(DataTypeSignature rhs) const { return !operator==(rhs); } +}; + +/** + * This class contains complete description of a data type. + */ +class UAVCAN_EXPORT DataTypeDescriptor +{ + DataTypeSignature signature_; + const char* full_name_; + DataTypeKind kind_; + DataTypeID id_; + +public: + static const unsigned MaxFullNameLen = 80; + + DataTypeDescriptor() : + full_name_(""), + kind_(DataTypeKind(0)) + { } + + DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) : + signature_(signature), + full_name_(name), + kind_(kind), + id_(id) + { + UAVCAN_ASSERT((kind == DataTypeKindMessage) || (kind == DataTypeKindService)); + UAVCAN_ASSERT(name); + UAVCAN_ASSERT(std::strlen(name) <= MaxFullNameLen); + } + + bool isValid() const; + + DataTypeKind getKind() const { return kind_; } + DataTypeID getID() const { return id_; } + const DataTypeSignature& getSignature() const { return signature_; } + const char* getFullName() const { return full_name_; } + + bool match(DataTypeKind kind, const char* name) const; + bool match(DataTypeKind kind, DataTypeID id) const; + + bool operator!=(const DataTypeDescriptor& rhs) const { return !operator==(rhs); } + bool operator==(const DataTypeDescriptor& rhs) const; + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +} + +#endif // UAVCAN_DATA_TYPE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/debug.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/debug.hpp new file mode 100644 index 0000000000..8b2ca17247 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/debug.hpp @@ -0,0 +1,35 @@ +/* + * Debug stuff, should only be used for library development. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DEBUG_HPP_INCLUDED +#define UAVCAN_DEBUG_HPP_INCLUDED + +#include + +#if UAVCAN_DEBUG + +# include +# include + +# if __GNUC__ +__attribute__ ((format(printf, 2, 3))) +# endif +static void UAVCAN_TRACE(const char* src, const char* fmt, ...) +{ + va_list args; + (void)std::printf("UAVCAN: %s: ", src); + va_start(args, fmt); + (void)std::vprintf(fmt, args); + va_end(args); + (void)std::puts(""); +} + +#else + +# define UAVCAN_TRACE(...) ((void)0) + +#endif + +#endif // UAVCAN_DEBUG_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/can.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/can.hpp new file mode 100644 index 0000000000..d20feca154 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/can.hpp @@ -0,0 +1,249 @@ +/* + * CAN bus driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DRIVER_CAN_HPP_INCLUDED +#define UAVCAN_DRIVER_CAN_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This limit is defined by the specification. + */ +enum { MaxCanIfaces = 3 }; + +/** + * Raw CAN frame, as passed to/from the CAN driver. + */ +struct UAVCAN_EXPORT CanFrame +{ + static const uint32_t MaskStdID = 0x000007FFU; + static const uint32_t MaskExtID = 0x1FFFFFFFU; + static const uint32_t FlagEFF = 1U << 31; ///< Extended frame format + static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request + static const uint32_t FlagERR = 1U << 29; ///< Error frame + + static const uint8_t MaxDataLen = 8; + + uint32_t id; ///< CAN ID with flags (above) + uint8_t data[MaxDataLen]; + uint8_t dlc; ///< Data Length Code + + CanFrame() : + id(0), + dlc(0) + { + fill(data, data + MaxDataLen, uint8_t(0)); + } + + CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) : + id(can_id), + dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) + { + UAVCAN_ASSERT(can_data != UAVCAN_NULLPTR); + UAVCAN_ASSERT(data_len == dlc); + (void)copy(can_data, can_data + dlc, this->data); + } + + bool operator!=(const CanFrame& rhs) const { return !operator==(rhs); } + bool operator==(const CanFrame& rhs) const + { + return (id == rhs.id) && (dlc == rhs.dlc) && equal(data, data + dlc, rhs.data); + } + + bool isExtended() const { return id & FlagEFF; } + bool isRemoteTransmissionRequest() const { return id & FlagRTR; } + bool isErrorFrame() const { return id & FlagERR; } + +#if UAVCAN_TOSTRING + enum StringRepresentation + { + StrTight, ///< Minimum string length (default) + StrAligned ///< Fixed formatting for any frame + }; + + std::string toString(StringRepresentation mode = StrTight) const; + +#endif + + /** + * CAN frame arbitration rules, particularly STD vs EXT: + * Marco Di Natale - "Understanding and using the Controller Area Network" + * http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf + */ + bool priorityHigherThan(const CanFrame& rhs) const; + bool priorityLowerThan(const CanFrame& rhs) const { return rhs.priorityHigherThan(*this); } +}; + +/** + * CAN hardware filter config struct. + * Flags from @ref CanFrame can be applied to define frame type (EFF, EXT, etc.). + * @ref ICanIface::configureFilters(). + */ +struct UAVCAN_EXPORT CanFilterConfig +{ + uint32_t id; + uint32_t mask; + + bool operator==(const CanFilterConfig& rhs) const + { + return rhs.id == id && rhs.mask == mask; + } + + CanFilterConfig() : + id(0), + mask(0) + { } +}; + +/** + * Events to look for during @ref ICanDriver::select() call. + * Bit position defines iface index, e.g. read = 1 << 2 to read from the third iface. + */ +struct UAVCAN_EXPORT CanSelectMasks +{ + uint8_t read; + uint8_t write; + + CanSelectMasks() : + read(0), + write(0) + { } +}; + +/** + * Special IO flags. + * + * @ref CanIOFlagLoopback - Send the frame back to RX with true TX timestamps. + * + * @ref CanIOFlagAbortOnError - Abort transmission on first bus error instead of retransmitting. This does not + * affect the case of arbitration loss, in which case the retransmission will work + * as usual. This flag is used together with anonymous messages which allows to + * implement CSMA bus access. Read the spec for details. + */ +typedef uint16_t CanIOFlags; +static const CanIOFlags CanIOFlagLoopback = 1; +static const CanIOFlags CanIOFlagAbortOnError = 2; + +/** + * Single non-blocking CAN interface. + */ +class UAVCAN_EXPORT ICanIface +{ +public: + virtual ~ICanIface() { } + + /** + * Non-blocking transmission. + * + * If the frame wasn't transmitted upon TX deadline, the driver should discard it. + * + * Note that it is LIKELY that the library will want to send the frames that were passed into the select() + * method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new + * frames between the calls. + * + * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error. + */ + virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) = 0; + + /** + * Non-blocking reception. + * + * Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller. + * + * Monotonic timestamp is required and can be not precise since it is needed only for + * protocol timing validation (transfer timeouts and inter-transfer intervals). + * + * UTC timestamp is optional, if available it will be used for precise time synchronization; + * must be set to zero if not available. + * + * Refer to @ref ISystemClock to learn more about timestamps. + * + * @param [out] out_ts_monotonic Monotonic timestamp, mandatory. + * @param [out] out_ts_utc UTC timestamp, optional, zero if unknown. + * @return 1 = one frame received, 0 = RX buffer empty, negative for error. + */ + virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc, + CanIOFlags& out_flags) = 0; + + /** + * Configure the hardware CAN filters. @ref CanFilterConfig. + * + * @return 0 = success, negative for error. + */ + virtual int16_t configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs) = 0; + + /** + * Number of available hardware filters. + */ + virtual uint16_t getNumFilters() const = 0; + + /** + * Continuously incrementing counter of hardware errors. + * Arbitration lost should not be treated as a hardware error. + */ + virtual uint64_t getErrorCount() const = 0; +}; + +/** + * Generic CAN driver. + */ +class UAVCAN_EXPORT ICanDriver +{ +public: + virtual ~ICanDriver() { } + + /** + * Returns an interface by index, or null pointer if the index is out of range. + */ + virtual ICanIface* getIface(uint8_t iface_index) = 0; + + /** + * Default implementation of this method calls the non-const overload of getIface(). + * Can be overriden by the application if necessary. + */ + virtual const ICanIface* getIface(uint8_t iface_index) const + { + return const_cast(this)->getIface(iface_index); + } + + /** + * Total number of available CAN interfaces. + * This value shall not change after initialization. + */ + virtual uint8_t getNumIfaces() const = 0; + + /** + * Block until the deadline, or one of the specified interfaces becomes available for read or write. + * + * Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO. + * + * Bit position in the masks defines interface index. + * + * Note that it is allowed to return from this method even if no requested events actually happened, or if + * there are events that were not requested by the library. + * + * The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit + * next, per interface. This is intended to allow the driver to properly prioritize transmissions; many + * drivers will not need to use it. If a write flag for the given interface is set to one in the select mask + * structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR). + * + * @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO. + * @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next. + * @param [in] blocking_deadline Zero means non-blocking operation. + * @return Positive number of ready interfaces or negative error code. + */ + virtual int16_t select(CanSelectMasks& inout_masks, + const CanFrame* (& pending_tx)[MaxCanIfaces], + MonotonicTime blocking_deadline) = 0; +}; + +} + +#endif // UAVCAN_DRIVER_CAN_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/system_clock.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/system_clock.hpp new file mode 100644 index 0000000000..6926e9af61 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/driver/system_clock.hpp @@ -0,0 +1,60 @@ +/* + * System clock driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED +#define UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +/** + * System clock interface - monotonic and UTC. + */ +class UAVCAN_EXPORT ISystemClock +{ +public: + virtual ~ISystemClock() { } + + /** + * Monototic system clock. + * + * This clock shall never jump or change rate; the base time is irrelevant. + * This clock is mandatory and must remain functional at all times. + * + * On POSIX systems use clock_gettime() with CLOCK_MONOTONIC. + */ + virtual MonotonicTime getMonotonic() const = 0; + + /** + * Global network clock. + * It doesn't have to be UTC, the name is a bit misleading - actual time base doesn't matter. + * + * This clock can be synchronized with other nodes on the bus, hence it can jump and/or change + * rate occasionally. + * This clock is optional; if it is not supported, return zero. Also return zero if the UTC time + * is not available yet (e.g. the device has just started up with no battery clock). + * + * For POSIX refer to clock_gettime(), gettimeofday(). + */ + virtual UtcTime getUtc() const = 0; + + /** + * Adjust the network-synchronized clock. + * Refer to @ref getUtc() for details. + * + * For POSIX refer to adjtime(), settimeofday(). + * + * @param [in] adjustment Amount of time to add to the clock value. + */ + virtual void adjustUtc(UtcDuration adjustment) = 0; +}; + +} + +#endif // UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp new file mode 100644 index 0000000000..da977d8c68 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED +#define UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This interface is used by other library components that need dynamic memory. + */ +class UAVCAN_EXPORT IPoolAllocator +{ +public: + virtual ~IPoolAllocator() { } + + virtual void* allocate(std::size_t size) = 0; + virtual void deallocate(const void* ptr) = 0; + + /** + * Returns the maximum number of blocks this allocator can allocate. + */ + virtual uint16_t getBlockCapacity() const = 0; +}; + +/** + * Classic implementation of a pool allocator (Meyers). + * + * The allocator can be made thread-safe (optional) by means of providing a RAII-lock type via the second template + * argument. The allocator uses the lock only to access the shared state, therefore critical sections are only a few + * cycles long, which implies that it should be acceptable to use hardware IRQ disabling instead of a mutex for + * performance reasons. For example, an IRQ-based RAII-lock type can be implemented as follows: + * struct RaiiSynchronizer + * { + * RaiiSynchronizer() { __disable_irq(); } + * ~RaiiSynchronizer() { __enable_irq(); } + * }; + */ +template +class UAVCAN_EXPORT PoolAllocator : public IPoolAllocator, + Noncopyable +{ + union Node + { + uint8_t data[BlockSize]; + Node* next; + }; + + Node* free_list_; + union + { + uint8_t bytes[PoolSize]; + long double _aligner1; + long long _aligner2; + Node _aligner3; + } pool_; + + uint16_t used_; + uint16_t max_used_; + +public: + static const uint16_t NumBlocks = PoolSize / BlockSize; + + PoolAllocator(); + + virtual void* allocate(std::size_t size) override; + virtual void deallocate(const void* ptr) override; + + virtual uint16_t getBlockCapacity() const override { return NumBlocks; } + + /** + * Return the number of blocks that are currently allocated/unallocated. + */ + uint16_t getNumUsedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return used_; + } + uint16_t getNumFreeBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return static_cast(NumBlocks - used_); + } + + /** + * Returns the maximum number of blocks that were ever allocated at the same time. + */ + uint16_t getPeakNumUsedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return max_used_; + } +}; + +/** + * Limits the maximum number of blocks that can be allocated in a given allocator. + */ +class LimitedPoolAllocator : public IPoolAllocator +{ + IPoolAllocator& allocator_; + const uint16_t max_blocks_; + uint16_t used_blocks_; + +public: + LimitedPoolAllocator(IPoolAllocator& allocator, std::size_t max_blocks) + : allocator_(allocator) + , max_blocks_(static_cast(min(max_blocks, 0xFFFFU))) + , used_blocks_(0) + { + UAVCAN_ASSERT(max_blocks_ > 0); + } + + virtual void* allocate(std::size_t size) override; + virtual void deallocate(const void* ptr) override; + + virtual uint16_t getBlockCapacity() const override; +}; + +// ---------------------------------------------------------------------------- + +/* + * PoolAllocator<> + */ +template +const uint16_t PoolAllocator::NumBlocks; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" + +template +PoolAllocator::PoolAllocator() : + free_list_(reinterpret_cast(pool_.bytes)), + used_(0), + max_used_(0) +{ + // The limit is imposed by the width of the pool usage tracking variables. + StaticAssert<((PoolSize / BlockSize) <= 0xFFFFU)>::check(); + + (void)std::memset(pool_.bytes, 0, PoolSize); + for (unsigned i = 0; (i + 1) < (NumBlocks - 1 + 1); i++) // -Werror=type-limits + { + // coverity[dead_error_line : FALSE] + free_list_[i].next = free_list_ + i + 1; + } + free_list_[NumBlocks - 1].next = UAVCAN_NULLPTR; +} +#pragma GCC diagnostic pop + +template +void* PoolAllocator::allocate(std::size_t size) +{ + if (free_list_ == UAVCAN_NULLPTR || size > BlockSize) + { + return UAVCAN_NULLPTR; + } + + RaiiSynchronizer lock; + (void)lock; + + void* pmem = free_list_; + free_list_ = free_list_->next; + + // Statistics + UAVCAN_ASSERT(used_ < NumBlocks); + used_++; + if (used_ > max_used_) + { + max_used_ = used_; + } + + return pmem; +} + +template +void PoolAllocator::deallocate(const void* ptr) +{ + if (ptr == UAVCAN_NULLPTR) + { + return; + } + + RaiiSynchronizer lock; + (void)lock; + + Node* p = static_cast(const_cast(ptr)); + p->next = free_list_; + free_list_ = p; + + // Statistics + UAVCAN_ASSERT(used_ > 0); + used_--; +} + +} + +#endif // UAVCAN_DYNAMIC_MEMORY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/error.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/error.hpp new file mode 100644 index 0000000000..4f67e9a6ec --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/error.hpp @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_ERROR_HPP_INCLUDED +#define UAVCAN_ERROR_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace +{ +/** + * Common error codes. + * + * Functions that return signed integers may also return inverted error codes, + * i.e. returned value should be inverted back to get the actual error code. + * + * Return code 0 (zero) means no error. + * + * @{ + */ +const int16_t ErrFailure = 1; ///< General failure +const int16_t ErrInvalidParam = 2; +const int16_t ErrMemory = 3; +const int16_t ErrDriver = 4; ///< Platform driver error +const int16_t ErrUnknownDataType = 5; +const int16_t ErrInvalidMarshalData = 6; +const int16_t ErrInvalidTransferListener = 7; +const int16_t ErrNotInited = 8; +const int16_t ErrRecursiveCall = 9; +const int16_t ErrLogic = 10; +const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode +const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type +const int16_t ErrInvalidConfiguration = 13; +/** + * @} + */ + +} + +/** + * Fatal error handler. + * Behavior: + * - If exceptions are enabled, throws std::runtime_error() with the supplied message text; + * - If assertions are enabled (see UAVCAN_ASSERT()), aborts execution using zero assertion. + * - Otherwise aborts execution via std::abort(). + */ +#if __GNUC__ +__attribute__ ((noreturn)) +#endif +UAVCAN_EXPORT +// coverity[+kill] +void handleFatalError(const char* msg); + +} + +#endif // UAVCAN_ERROR_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp new file mode 100644 index 0000000000..42d4a35950 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/heap_based_pool_allocator.hpp @@ -0,0 +1,220 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED +#define UAVCAN_HELPERS_HEAP_BASED_POOL_ALLOCATOR_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * A special-purpose implementation of a pool allocator that keeps the pool in the heap using malloc()/free(). + * The pool grows dynamically, ad-hoc, thus using as little memory as possible. + * + * Allocated blocks will not be freed back automatically, but there are two ways to force their deallocation: + * - Call @ref shrink() - this method frees all blocks that are unused at the moment. + * - Destroy the object - the desctructor calls @ref shrink(). + * + * The pool can be limited in growth with hard and soft limits. + * The soft limit defines the value that will be reported via @ref IPoolAllocator::getBlockCapacity(). + * The hard limit defines the maximum number of blocks that can be allocated from heap. + * Typically, the hard limit should be equal or greater than the soft limit. + * + * The allocator can be made thread-safe (optional) by means of providing a RAII-lock type via the second template + * argument. The allocator uses the lock only to access the shared state, therefore critical sections are only a few + * cycles long, which implies that it should be acceptable to use hardware IRQ disabling instead of a mutex for + * performance reasons. For example, an IRQ-based RAII-lock type can be implemented as follows: + * struct RaiiSynchronizer + * { + * RaiiSynchronizer() { __disable_irq(); } + * ~RaiiSynchronizer() { __enable_irq(); } + * }; + */ +template +class UAVCAN_EXPORT HeapBasedPoolAllocator : public IPoolAllocator, + Noncopyable +{ + union Node + { + Node* next; + private: + uint8_t data[BlockSize]; + long double _aligner1; + long long _aligner2; + }; + + const uint16_t capacity_soft_limit_; + const uint16_t capacity_hard_limit_; + + uint16_t num_reserved_blocks_; + uint16_t num_allocated_blocks_; + + Node* reserve_; + +public: + /** + * The allocator initializes with empty reserve, so first allocations will be served from heap. + * + * @param block_capacity_soft_limit Block capacity that will be reported via @ref getBlockCapacity(). + * + * @param block_capacity_hard_limit Real block capacity limit; the number of allocated blocks will never + * exceed this value. Hard limit should be higher than soft limit. + * Default value is two times the soft limit. + */ + HeapBasedPoolAllocator(uint16_t block_capacity_soft_limit, + uint16_t block_capacity_hard_limit = 0) : + capacity_soft_limit_(block_capacity_soft_limit), + capacity_hard_limit_((block_capacity_hard_limit > 0) ? block_capacity_hard_limit : + static_cast(min(static_cast(block_capacity_soft_limit) * 2U, + static_cast(NumericTraits::max())))), + num_reserved_blocks_(0), + num_allocated_blocks_(0), + reserve_(UAVCAN_NULLPTR) + { } + + /** + * The destructor de-allocates all blocks that are currently in the reserve. + * BLOCKS THAT ARE CURRENTLY HELD BY THE APPLICATION WILL LEAK. + */ + ~HeapBasedPoolAllocator() + { + shrink(); +#if UAVCAN_DEBUG + if (num_allocated_blocks_ > 0) + { + UAVCAN_TRACE("HeapBasedPoolAllocator", "%u BLOCKS LEAKED", num_allocated_blocks_); + } +#endif + } + + /** + * Takes a block from the reserve, unless it's empty. + * In the latter case, allocates a new block in the heap. + */ + virtual void* allocate(std::size_t size) override + { + if (size > BlockSize) + { + return UAVCAN_NULLPTR; + } + + { + RaiiSynchronizer lock; + (void)lock; + + Node* const p = reserve_; + + if (UAVCAN_LIKELY(p != UAVCAN_NULLPTR)) + { + reserve_ = reserve_->next; + num_allocated_blocks_++; + return p; + } + + if (num_reserved_blocks_ >= capacity_hard_limit_) // Hard limit reached, no further allocations + { + return UAVCAN_NULLPTR; + } + } + + // Unlikely branch + void* const m = std::malloc(sizeof(Node)); + if (m != UAVCAN_NULLPTR) + { + RaiiSynchronizer lock; + (void)lock; + + num_reserved_blocks_++; + num_allocated_blocks_++; + } + return m; + } + + /** + * Puts the block back to reserve. + * The block will not be free()d automatically; see @ref shrink(). + */ + virtual void deallocate(const void* ptr) override + { + if (ptr != UAVCAN_NULLPTR) + { + RaiiSynchronizer lock; + (void)lock; + + Node* const node = static_cast(const_cast(ptr)); + node->next = reserve_; + reserve_ = node; + + num_allocated_blocks_--; + } + } + + /** + * The soft limit. + */ + virtual uint16_t getBlockCapacity() const override { return capacity_soft_limit_; } + + /** + * The hard limit. + */ + uint16_t getBlockCapacityHardLimit() const { return capacity_hard_limit_; } + + /** + * Frees all blocks that are not in use at the moment. + * Post-condition is getNumAllocatedBlocks() == getNumReservedBlocks(). + */ + void shrink() + { + Node* p = UAVCAN_NULLPTR; + for (;;) + { + { + RaiiSynchronizer lock; + (void)lock; + // Removing from reserve and updating the counter. + p = reserve_; + if (p != UAVCAN_NULLPTR) + { + reserve_ = reserve_->next; + num_reserved_blocks_--; + } + else + { + break; + } + } + // Then freeing, having left the critical section. + std::free(p); + } + } + + /** + * Number of blocks that are currently in use by the application. + */ + uint16_t getNumAllocatedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return num_allocated_blocks_; + } + + /** + * Number of blocks that are acquired from the heap. + */ + uint16_t getNumReservedBlocks() const + { + RaiiSynchronizer lock; + (void)lock; + return num_reserved_blocks_; + } +}; + +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/ostream.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/ostream.hpp new file mode 100644 index 0000000000..56fd9753a9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/helpers/ostream.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED +#define UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +/** + * Compact replacement for std::ostream for use on embedded systems. + * Can be used for printing UAVCAN messages to stdout. + * + * Relevant discussion: https://groups.google.com/forum/#!topic/px4users/6c1CLNutN90 + * + * Usage: + * OStream::instance() << "Hello world!" << OStream::endl; + */ +class UAVCAN_EXPORT OStream : uavcan::Noncopyable +{ + OStream() { } + +public: + static OStream& instance() + { + static OStream s; + return s; + } + + static OStream& endl(OStream& stream) + { + std::printf("\n"); + return stream; + } +}; + +inline OStream& operator<<(OStream& s, long long x) { std::printf("%lld", x); return s; } +inline OStream& operator<<(OStream& s, unsigned long long x) { std::printf("%llu", x); return s; } + +inline OStream& operator<<(OStream& s, long x) { std::printf("%ld", x); return s; } +inline OStream& operator<<(OStream& s, unsigned long x) { std::printf("%lu", x); return s; } + +inline OStream& operator<<(OStream& s, int x) { std::printf("%d", x); return s; } +inline OStream& operator<<(OStream& s, unsigned int x) { std::printf("%u", x); return s; } + +inline OStream& operator<<(OStream& s, short x) { return operator<<(s, static_cast(x)); } +inline OStream& operator<<(OStream& s, unsigned short x) { return operator<<(s, static_cast(x)); } + +inline OStream& operator<<(OStream& s, long double x) { std::printf("%Lg", x); return s; } +inline OStream& operator<<(OStream& s, double x) { std::printf("%g", x); return s; } +inline OStream& operator<<(OStream& s, float x) { return operator<<(s, static_cast(x)); } + +inline OStream& operator<<(OStream& s, char x) { std::printf("%c", x); return s; } +inline OStream& operator<<(OStream& s, const char* x) { std::printf("%s", x); return s; } + +inline OStream& operator<<(OStream& s, OStream&(*manip)(OStream&)) { return manip(s); } + +} + +#endif // UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/array.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/array.hpp new file mode 100644 index 0000000000..b409d5d0c1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/array.hpp @@ -0,0 +1,1270 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED +#define UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +#ifndef UAVCAN_EXCEPTIONS +# error UAVCAN_EXCEPTIONS +#endif + +#if UAVCAN_EXCEPTIONS +# include +#endif + +namespace uavcan +{ + +enum ArrayMode { ArrayModeStatic, ArrayModeDynamic }; + +/** + * Properties of a square matrix; assuming row-major representation. + */ +template +struct SquareMatrixTraits +{ + enum { NumElements = NumElements_ }; + + enum { NumRowsCols = CompileTimeIntSqrt::Result }; + + enum { NumElementsInTriangle = ((1 + NumRowsCols) * NumRowsCols) / 2 }; + + static inline bool isIndexOnDiagonal(unsigned index) { return (index / NumRowsCols) == (index % NumRowsCols); } + + static inline int computeElementIndexAtRowCol(int row, int col) { return row * NumRowsCols + col; } +}; + +/** + * This class can be used to detect properties of square matrices. + * Element iterator is a random access forward constant iterator. + */ +template +class SquareMatrixAnalyzer : public SquareMatrixTraits +{ + typedef SquareMatrixTraits Traits; + + const ElementIterator first_; + +public: + enum PackingMode + { + PackingModeEmpty, + PackingModeScalar, + PackingModeDiagonal, + PackingModeSymmetric, + PackingModeFull + }; + + SquareMatrixAnalyzer(ElementIterator first_element_iterator) + : first_(first_element_iterator) + { + StaticAssert<(NumElements > 0)>::check(); + } + + ElementIterator accessElementAtRowCol(int row, int col) const + { + return first_ + Traits::computeElementIndexAtRowCol(row, col); + } + + bool areAllElementsNan() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!isNaN(*it)) + { + return false; + } + } + return true; + } + + bool isScalar() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it)) + { + return false; + } + if (Traits::isIndexOnDiagonal(index) && !areClose(*it, *first_)) + { + return false; + } + } + return true; + } + + bool isDiagonal() const + { + unsigned index = 0; + for (ElementIterator it = first_; index < NumElements; ++it, ++index) + { + if (!Traits::isIndexOnDiagonal(index) && !isCloseToZero(*it)) + { + return false; + } + } + return true; + } + + bool isSymmetric() const + { + for (int i = 0; i < Traits::NumRowsCols; ++i) + { + for (int k = 0; k < Traits::NumRowsCols; ++k) + { + // On diagonal comparison is pointless + if ((i != k) && + !areClose(*accessElementAtRowCol(i, k), + *accessElementAtRowCol(k, i))) + { + return false; + } + } + } + return true; + } + + PackingMode detectOptimalPackingMode() const + { + if (areAllElementsNan()) + { + return PackingModeEmpty; + } + if (isScalar()) + { + return PackingModeScalar; + } + if (isDiagonal()) + { + return PackingModeDiagonal; + } + if (isSymmetric()) + { + return PackingModeSymmetric; + } + return PackingModeFull; + } +}; + + +template +class UAVCAN_EXPORT StaticArrayBase +{ +protected: + typedef IntegerSpec::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType; + +public: + enum { SizeBitLen = 0 }; + + typedef typename StorageType::Result>::Result, + SignednessUnsigned, CastModeSaturate> >::Type SizeType; + + SizeType size() const { return SizeType(Size); } + SizeType capacity() const { return SizeType(Size); } + +protected: + StaticArrayBase() { } + ~StaticArrayBase() { } + + SizeType validateRange(SizeType pos) const + { + if (pos < SizeType(Size)) + { + return pos; + } +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array"); +#else + UAVCAN_ASSERT(0); + return SizeType(Size - 1U); // Ha ha +#endif + } +}; + + +template +class UAVCAN_EXPORT DynamicArrayBase +{ +protected: + typedef IntegerSpec::Result, SignednessUnsigned, CastModeSaturate> RawEncodedSizeType; +public: + typedef typename StorageType::Result>::Result, + SignednessUnsigned, CastModeSaturate> >::Type SizeType; + +private: + SizeType size_; + +protected: + DynamicArrayBase() : size_(0) { } + ~DynamicArrayBase() { } + + SizeType validateRange(SizeType pos) const + { + if (pos < size_) + { + return pos; + } +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array"); +#else + UAVCAN_ASSERT(0); + return SizeType((size_ == 0U) ? 0U : (size_ - 1U)); +#endif + } + + void grow() + { + if (size_ >= MaxSize) + { + (void)validateRange(MaxSize); // Will throw, UAVCAN_ASSERT() or do nothing + } + else + { + size_++; + } + } + + void shrink() + { + if (size_ > 0) + { + size_--; + } + } + +public: + enum { SizeBitLen = RawEncodedSizeType::BitLen }; + + SizeType size() const + { + UAVCAN_ASSERT(size_ ? ((size_ - 1u) <= (MaxSize - 1u)) : 1); // -Werror=type-limits + return size_; + } + + SizeType capacity() const { return MaxSize; } + + void clear() { size_ = 0; } +}; + +/** + * Common functionality for both static and dynamic arrays. + * Static arrays are of fixed size; methods that can alter the size (e.g. push_back() and such) will fail to compile. + * Dynamic arrays contain a fixed-size buffer (it's size is enough to fit maximum number of elements) plus the + * currently allocated number of elements. + */ +template +class UAVCAN_EXPORT ArrayImpl : public Select, StaticArrayBase >::Result +{ + typedef ArrayImpl SelfType; + typedef typename Select, StaticArrayBase >::Result Base; + +public: + enum + { + /// True if the array contents can be interpreted as a 8-bit string (ASCII or UTF8). + IsStringLike = IsIntegerSpec::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) && + (ArrayMode == ArrayModeDynamic) + }; + +private: + typedef typename StorageType::Type BufferType[MaxSize + (IsStringLike ? 1 : 0)]; + BufferType data_; + + template + typename EnableIf= U())>::Type initialize(int) + { + if (ArrayMode != ArrayModeDynamic) + { + ::uavcan::fill(data_, data_ + MaxSize, U()); + } + } + template void initialize(...) { } + +protected: + ~ArrayImpl() { } + +public: + typedef typename StorageType::Type ValueType; + typedef typename Base::SizeType SizeType; + + using Base::size; + using Base::capacity; + + ArrayImpl() { initialize(0); } + + /** + * Returns zero-terminated string, same as std::string::c_str(). + * This method will compile only if the array can be interpreted as 8-bit string (ASCII of UTF8). + */ + const char* c_str() const + { + StaticAssert::check(); + UAVCAN_ASSERT(size() < (MaxSize + 1)); + const_cast(data_)[size()] = 0; // Ad-hoc string termination + return reinterpret_cast(data_); + } + + /** + * Range-checking subscript. + * If the index is out of range: + * - if exceptions are enabled, std::out_of_range will be thrown. + * - if exceptions are disabled and UAVCAN_ASSERT() is enabled, execution will be aborted. + * - if exceptions are disabled and UAVCAN_ASSERT() is disabled, index will be constrained to + * the closest valid value. + */ + ValueType& at(SizeType pos) { return data_[Base::validateRange(pos)]; } + const ValueType& at(SizeType pos) const { return data_[Base::validateRange(pos)]; } + + /** + * Range-checking subscript. @ref at() + */ + ValueType& operator[](SizeType pos) { return at(pos); } + const ValueType& operator[](SizeType pos) const { return at(pos); } + + /** + * Standard container methods. Applicable to both dynamic and static arrays. + */ + ValueType* begin() { return data_; } + const ValueType* begin() const { return data_; } + ValueType* end() { return data_ + Base::size(); } + const ValueType* end() const { return data_ + Base::size(); } + ValueType& front() { return at(0U); } + const ValueType& front() const { return at(0U); } + ValueType& back() { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); } + const ValueType& back() const { return at((Base::size() == 0U) ? 0U : SizeType(Base::size() - 1U)); } + + /** + * Performs standard lexicographical compare of the elements. + */ + template + bool operator<(const R& rhs) const + { + return ::uavcan::lexicographical_compare(begin(), end(), rhs.begin(), rhs.end()); + } + + /** + * Aliases for compatibility with standard containers. + */ + typedef ValueType* iterator; + typedef const ValueType* const_iterator; +}; + +/** + * Memory-efficient specialization for bit arrays (each element maps to a single bit rather than single byte). + * This should be compatible with std::bitset. + */ +template +class UAVCAN_EXPORT ArrayImpl, ArrayMode, MaxSize> + : public BitSet + , public Select, StaticArrayBase >::Result +{ + typedef typename Select, StaticArrayBase >::Result ArrayBase; + +public: + enum { IsStringLike = 0 }; + + typedef typename BitSet::Reference Reference; + typedef typename ArrayBase::SizeType SizeType; + + using ArrayBase::size; + using ArrayBase::capacity; + + /** + * Range-checking subscript. Throws if enabled; UAVCAN_ASSERT() if enabled; else constraints the position. + */ + Reference at(SizeType pos) { return BitSet::operator[](ArrayBase::validateRange(pos)); } + bool at(SizeType pos) const { return BitSet::operator[](ArrayBase::validateRange(pos)); } + + /** + * @ref at() + */ + Reference operator[](SizeType pos) { return at(pos); } + bool operator[](SizeType pos) const { return at(pos); } +}; + +/** + * Zero length arrays are not allowed + */ +template class ArrayImpl; + +/** + * Generic array implementation. + * This class is compatible with most standard library functions operating on containers (e.g. std::sort(), + * std::lexicographical_compare(), etc.). + * No dynamic memory is used. + * All functions that can modify the array or access elements are range checking. If the range error occurs: + * - if exceptions are enabled, std::out_of_range will be thrown; + * - if UAVCAN_ASSERT() is enabled, program will be terminated on UAVCAN_ASSERT(0); + * - otherwise the index value will be constrained to the closest valid value. + */ +template +class UAVCAN_EXPORT Array : public ArrayImpl +{ + typedef ArrayImpl Base; + typedef Array SelfType; + + static bool isOptimizedTailArray(TailArrayOptimizationMode tao_mode) + { + return (T::MinBitLen >= 8) && (tao_mode == TailArrayOptEnabled); + } + + int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) const /// Static + { + UAVCAN_ASSERT(size() > 0); + for (SizeType i = 0; i < size(); i++) + { + const bool last_item = i == (size() - 1); + const int res = RawValueType::encode(Base::at(i), codec, last_item ? tao_mode : TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + } + return 1; + } + + int encodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) const /// Dynamic + { + StaticAssert::check(); + const bool self_tao_enabled = isOptimizedTailArray(tao_mode); + if (!self_tao_enabled) + { + const int res_sz = + Base::RawEncodedSizeType::encode(typename StorageType::Type(size()), + codec, TailArrayOptDisabled); + if (res_sz <= 0) + { + return res_sz; + } + } + if (size() == 0) + { + return 1; + } + return encodeImpl(codec, self_tao_enabled ? TailArrayOptDisabled : tao_mode, FalseType()); + } + + int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, FalseType) /// Static + { + UAVCAN_ASSERT(size() > 0); + for (SizeType i = 0; i < size(); i++) + { + const bool last_item = i == (size() - 1); + ValueType value = ValueType(); // TODO: avoid extra copy + const int res = RawValueType::decode(value, codec, last_item ? tao_mode : TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + Base::at(i) = value; + } + return 1; + } + +#if __GNUC__ +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wtype-limits" +#endif + int decodeImpl(ScalarCodec& codec, const TailArrayOptimizationMode tao_mode, TrueType) /// Dynamic + { + StaticAssert::check(); + Base::clear(); + if (isOptimizedTailArray(tao_mode)) + { + while (true) + { + ValueType value = ValueType(); + const int res = RawValueType::decode(value, codec, TailArrayOptDisabled); + if (res < 0) + { + return res; + } + if (res == 0) // Success: End of stream reached (even if zero items were read) + { + return 1; + } + if (size() == MaxSize_) // Error: Max array length reached, but the end of stream is not + { + return -ErrInvalidMarshalData; + } + push_back(value); + } + } + else + { + typename StorageType::Type sz = 0; + const int res_sz = Base::RawEncodedSizeType::decode(sz, codec, TailArrayOptDisabled); + if (res_sz <= 0) + { + return res_sz; + } + // coverity[result_independent_of_operands] + if (static_cast(sz) > MaxSize_) // False 'type-limits' warning occurs here + { + return -ErrInvalidMarshalData; + } + resize(sz); + if (sz == 0) + { + return 1; + } + return decodeImpl(codec, tao_mode, FalseType()); + } + UAVCAN_ASSERT(0); // Unreachable + return -ErrLogic; + } +#if __GNUC__ +# pragma GCC diagnostic pop +#endif + + template + void packSquareMatrixImpl(const InputIter src_row_major) + { + StaticAssert::check(); + + this->clear(); + + typedef SquareMatrixAnalyzer Analyzer; + const Analyzer analyzer(src_row_major); + + switch (analyzer.detectOptimalPackingMode()) + { + case Analyzer::PackingModeEmpty: + { + break; // Nothing to insert + } + case Analyzer::PackingModeScalar: + { + this->push_back(ValueType(*src_row_major)); + break; + } + case Analyzer::PackingModeDiagonal: + { + for (int i = 0; i < Analyzer::NumRowsCols; i++) + { + this->push_back(ValueType(*analyzer.accessElementAtRowCol(i, i))); + } + break; + } + case Analyzer::PackingModeSymmetric: + { + for (int row = 0; row < Analyzer::NumRowsCols; row++) + { + for (int col = row; col < Analyzer::NumRowsCols; col++) + { + this->push_back(ValueType(*analyzer.accessElementAtRowCol(row, col))); + } + } + UAVCAN_ASSERT(this->size() == Analyzer::NumElementsInTriangle); + break; + } + case Analyzer::PackingModeFull: + { + InputIter it = src_row_major; + for (unsigned index = 0; index < MaxSize; index++, it++) + { + this->push_back(ValueType(*it)); + } + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + } + + template + void unpackSquareMatrixImpl(const OutputIter dst_row_major) const + { + StaticAssert::check(); + typedef SquareMatrixTraits Traits; + + if (this->size() == Traits::NumRowsCols || this->size() == 1) // Scalar or diagonal + { + OutputIter it = dst_row_major; + for (unsigned index = 0; index < MaxSize; index++) + { + if (Traits::isIndexOnDiagonal(index)) + { + const SizeType source_index = SizeType((this->size() == 1) ? 0 : (index / Traits::NumRowsCols)); + *it++ = ScalarType(this->at(source_index)); + } + else + { + *it++ = ScalarType(0); + } + } + } + else if (this->size() == Traits::NumElementsInTriangle) // Symmetric + { + OutputIter it = dst_row_major; + SizeType source_index = 0; + for (int row = 0; row < Traits::NumRowsCols; row++) + { + for (int col = 0; col < Traits::NumRowsCols; col++) + { + if (col >= row) // Diagonal or upper-right triangle + { + *it++ = ScalarType(this->at(source_index)); + source_index++; + } + else // Lower-left triangle + { + // Transposing one element, argument swapping is intentional + // coverity[swapped_arguments] + *it++ = *(dst_row_major + Traits::computeElementIndexAtRowCol(col, row)); + } + } + } + UAVCAN_ASSERT(source_index == Traits::NumElementsInTriangle); + } + else if (this->size() == MaxSize) // Full - no packing whatsoever + { + OutputIter it = dst_row_major; + for (SizeType index = 0; index < MaxSize; index++) + { + *it++ = ScalarType(this->at(index)); + } + } + else // Everything else + { + // coverity[suspicious_sizeof : FALSE] + ::uavcan::fill_n(dst_row_major, MaxSize, ScalarType(0)); + } + } + +public: + typedef T RawValueType; ///< This may be not the same as the element type. + typedef typename StorageType::Type ValueType; ///< This is the actual stored element type. + typedef typename Base::SizeType SizeType; ///< Minimal width size type. + + using Base::size; + using Base::capacity; + + enum { IsDynamic = ArrayMode == ArrayModeDynamic }; + enum { MaxSize = MaxSize_ }; + enum + { + MinBitLen = (IsDynamic == 0) + ? (static_cast(RawValueType::MinBitLen) * static_cast(MaxSize)) + : 0 + }; + enum + { + MaxBitLen = static_cast(Base::SizeBitLen) + + static_cast(RawValueType::MaxBitLen) * static_cast(MaxSize) + }; + + /** + * Default constructor zero-initializes the storage even if it consists of primitive types. + */ + Array() { } + + /** + * String constructor - only for string-like arrays. + * Refer to @ref operator+=(const char*) for details. + */ + Array(const char* str) // Implicit + { + operator+=(str); + } + + static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode) + { + return array.encodeImpl(codec, tao_mode, BooleanType()); + } + + static int decode(SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode) + { + return array.decodeImpl(codec, tao_mode, BooleanType()); + } + + static void extendDataTypeSignature(DataTypeSignature& signature) + { + RawValueType::extendDataTypeSignature(signature); + } + + bool empty() const { return size() == 0; } + + /** + * Only for dynamic arrays. Range checking. + */ + void pop_back() { Base::shrink(); } + void push_back(const ValueType& value) + { + Base::grow(); + Base::at(SizeType(size() - 1)) = value; + } + + /** + * Only for dynamic arrays. Range checking. + */ + void resize(SizeType new_size, const ValueType& filler) + { + if (new_size > size()) + { + SizeType cnt = SizeType(new_size - size()); + while (cnt-- > 0) + { + push_back(filler); + } + } + else if (new_size < size()) + { + SizeType cnt = SizeType(size() - new_size); + while (cnt-- > 0) + { + pop_back(); + } + } + else + { + ; // Exact size + } + } + + /** + * Only for dynamic arrays. Range checking. + */ + void resize(SizeType new_size) + { + resize(new_size, ValueType()); + } + + /** + * This operator accepts any container with size() and []. + * Members must be comparable via operator ==. + */ + template + typename EnableIf(0))->size()) && + sizeof((*(reinterpret_cast(0)))[0]), bool>::Type + operator==(const R& rhs) const + { + if (size() != rhs.size()) + { + return false; + } + for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators + { + if (!(Base::at(i) == rhs[i])) + { + return false; + } + } + return true; + } + + /** + * This method compares two arrays using @ref areClose(), which ensures proper comparison of + * floating point values, or DSDL data structures which contain floating point fields at any depth. + * Please refer to the documentation of @ref areClose() to learn more about how it works and how to + * define custom fuzzy comparison behavior. + * Any container with size() and [] is acceptable. + */ + template + typename EnableIf(0))->size()) && + sizeof((*(reinterpret_cast(0)))[0]), bool>::Type + isClose(const R& rhs) const + { + if (size() != rhs.size()) + { + return false; + } + for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators + { + if (!areClose(Base::at(i), rhs[i])) + { + return false; + } + } + return true; + } + + /** + * This operator can only be used with string-like arrays; otherwise it will fail to compile. + * @ref c_str() + */ + bool operator==(const char* chr) const + { + if (chr == UAVCAN_NULLPTR) + { + return false; + } + return std::strncmp(Base::c_str(), chr, MaxSize) == 0; + } + + /** + * @ref operator==() + */ + template bool operator!=(const R& rhs) const { return !operator==(rhs); } + + /** + * This operator can only be used with string-like arrays; otherwise it will fail to compile. + * @ref c_str() + */ + SelfType& operator=(const char* chr) + { + StaticAssert::check(); + StaticAssert::check(); + Base::clear(); + if (chr == UAVCAN_NULLPTR) + { + handleFatalError("Array::operator=(const char*)"); + } + while (*chr) + { + push_back(ValueType(*chr++)); // Value type is likely to be unsigned char, so conversion may be required. + } + return *this; + } + + /** + * This operator can only be used with string-like arrays; otherwise it will fail to compile. + * @ref c_str() + */ + SelfType& operator+=(const char* chr) + { + StaticAssert::check(); + StaticAssert::check(); + if (chr == UAVCAN_NULLPTR) + { + handleFatalError("Array::operator+=(const char*)"); + } + while (*chr) + { + push_back(ValueType(*chr++)); + } + return *this; + } + + /** + * Appends another Array<> with the same element type. Mode and max size can be different. + */ + template + SelfType& operator+=(const Array& rhs) + { + typedef Array Rhs; + StaticAssert::check(); + for (typename Rhs::SizeType i = 0; i < rhs.size(); i++) + { + push_back(rhs[i]); + } + return *this; + } + + /** + * Formatting appender. + * This method doesn't raise an overflow error; instead it silently truncates the data to fit the array capacity. + * Works only with string-like arrays, otherwise fails to compile. + * @param format Format string for std::snprintf(), e.g. "%08x", "%f" + * @param value Arbitrary value of a primitive type (should fail to compile if there's a non-primitive type) + */ + template + void appendFormatted(const char* const format, const A value) + { + StaticAssert::check(); + StaticAssert::check(); + + StaticAssert= A(0))>::check(); // This check allows to weed out most compound types + StaticAssert<(sizeof(A) <= sizeof(long double)) || + (sizeof(A) <= sizeof(long long))>::check(); // Another stupid check to catch non-primitive types + + if (!format) + { + UAVCAN_ASSERT(0); + return; + } + // Add some hardcore runtime checks for the format string correctness? + + ValueType* const ptr = Base::end(); + UAVCAN_ASSERT(capacity() >= size()); + const SizeType max_size = SizeType(capacity() - size()); + + // We have one extra byte for the null terminator, hence +1 + const int ret = snprintf(reinterpret_cast(ptr), SizeType(max_size + 1U), format, value); + + for (int i = 0; i < min(ret, int(max_size)); i++) + { + Base::grow(); + } + if (ret < 0) + { + UAVCAN_ASSERT(0); // Likely an invalid format string + (*this) += format; // So we print it as is in release builds + } + } + + /** + * Converts the string to upper/lower case in place, assuming that encoding is ASCII. + * These methods can only be used with string-like arrays; otherwise compilation will fail. + */ + void convertToUpperCaseASCII() + { + StaticAssert::check(); + + for (SizeType i = 0; i < size(); i++) + { + const int x = Base::at(i); + if ((x <= 'z') && (x >= 'a')) + { + Base::at(i) = static_cast(x + ('Z' - 'z')); + } + } + } + + void convertToLowerCaseASCII() + { + StaticAssert::check(); + + for (SizeType i = 0; i < size(); i++) + { + const int x = Base::at(i); + if ((x <= 'Z') && (x >= 'A')) + { + Base::at(i) = static_cast(x - ('Z' - 'z')); + } + } + } + + /** + * Fills this array as a packed square matrix from a static array. + * Please refer to the specification to learn more about matrix packing. + * Note that matrix packing code uses @ref areClose() for comparison. + */ + template + void packSquareMatrix(const ScalarType (&src_row_major)[MaxSize]) + { + packSquareMatrixImpl(src_row_major); + } + + /** + * Fills this array as a packed square matrix in place. + * Please refer to the specification to learn more about matrix packing. + * Note that matrix packing code uses @ref areClose() for comparison. + */ + void packSquareMatrix() + { + if (this->size() == MaxSize) + { + ValueType matrix[MaxSize]; + for (SizeType i = 0; i < MaxSize; i++) + { + matrix[i] = this->at(i); + } + packSquareMatrix(matrix); + } + else if (this->size() == 0) + { + ; // Nothing to do - leave the matrix empty + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::packSquareMatrix()"); +#else + UAVCAN_ASSERT(0); + this->clear(); +#endif + } + + } + + /** + * Fills this array as a packed square matrix from any container that has the following public entities: + * - method begin() + * - method size() + * - only for C++03: type value_type + * Please refer to the specification to learn more about matrix packing. + * Note that matrix packing code uses @ref areClose() for comparison. + */ + template + typename EnableIf(0))->begin()) && + sizeof((reinterpret_cast(0))->size())>::Type + packSquareMatrix(const R& src_row_major) + { + if (src_row_major.size() == MaxSize) + { + packSquareMatrixImpl(src_row_major.begin()); + } + else if (src_row_major.size() == 0) + { + this->clear(); + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::packSquareMatrix()"); +#else + UAVCAN_ASSERT(0); + this->clear(); +#endif + } + } + + /** + * Reconstructs full matrix, result will be saved into a static array. + * Please refer to the specification to learn more about matrix packing. + */ + template + void unpackSquareMatrix(ScalarType (&dst_row_major)[MaxSize]) const + { + unpackSquareMatrixImpl(dst_row_major); + } + + /** + * Reconstructs full matrix in place. + * Please refer to the specification to learn more about matrix packing. + */ + void unpackSquareMatrix() + { + ValueType matrix[MaxSize]; + unpackSquareMatrix(matrix); + + this->clear(); + for (unsigned i = 0; i < MaxSize; i++) + { + this->push_back(matrix[i]); + } + } + + /** + * Reconstructs full matrix, result will be saved into container that has the following public entities: + * - method begin() + * - method size() + * - only for C++03: type value_type + * Please refer to the specification to learn more about matrix packing. + */ + template + typename EnableIf(0))->begin()) && + sizeof((reinterpret_cast(0))->size())>::Type + unpackSquareMatrix(R& dst_row_major) const + { + if (dst_row_major.size() == MaxSize) + { +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 + typedef typename RemoveReference::Type RhsValueType; + unpackSquareMatrixImpl(dst_row_major.begin()); +#else + unpackSquareMatrixImpl(dst_row_major.begin()); +#endif + } + else + { +#if UAVCAN_EXCEPTIONS + throw std::out_of_range("uavcan::Array::unpackSquareMatrix()"); +#else + UAVCAN_ASSERT(0); +#endif + } + } + + /** + * Aliases for compatibility with standard containers. + */ + typedef ValueType value_type; + typedef SizeType size_type; +}; + +/** + * These operators will only be enabled if rhs and lhs are different types. This precondition allows to work-around + * the ambiguity arising from the scope containing two definitions: one here and the others in Array<>. + * Refer to https://github.com/UAVCAN/libuavcan/issues/55 for more info. + */ +template +UAVCAN_EXPORT +inline typename EnableIf >::Result, bool>::Type +operator==(const R& rhs, const Array& lhs) +{ + return lhs.operator==(rhs); +} + +template +UAVCAN_EXPORT +inline typename EnableIf >::Result, bool>::Type +operator!=(const R& rhs, const Array& lhs) +{ + return lhs.operator!=(rhs); +} + +/** + * Shortcut for string-like array type instantiation. + * The proper way of doing this is actually "template<> using ... = ...", but this feature is not available in + * older C++ revisions which the library has to support. + */ +template +class MakeString +{ + MakeString(); // This class is not instantiatable. +public: + typedef Array, ArrayModeDynamic, MaxSize> Type; +}; + +/** + * YAML streamer specification for any Array<> + */ +template +class UAVCAN_EXPORT YamlStreamer > +{ + typedef Array ArrayType; + + static bool isNiceCharacter(int c) + { + if (c >= 32 && c <= 126) + { + return true; + } + static const char Good[] = {'\n', '\r', '\t'}; + for (unsigned i = 0; i < sizeof(Good) / sizeof(Good[0]); i++) + { + if (Good[i] == c) + { + return true; + } + } + return false; + } + + template + static void streamPrimitives(Stream& s, const ArrayType& array) + { + s << '['; + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + YamlStreamer::stream(s, array.at(i), 0); + if ((i + 1) < array.size()) + { + s << ", "; + } + } + s << ']'; + } + + template + static void streamCharacters(Stream& s, const ArrayType& array) + { + s << '"'; + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + const int c = array.at(i); + if (c < 32 || c > 126) + { + char nibbles[2] = {char((c >> 4) & 0xF), char(c & 0xF)}; + for (int k = 0; k < 2; k++) + { + nibbles[k] = char(nibbles[k] + '0'); + if (nibbles[k] > '9') + { + nibbles[k] = char(nibbles[k] + 'A' - '9' - 1); + } + } + s << "\\x" << nibbles[0] << nibbles[1]; + } + else + { + if (c == '"' || c == '\\') // YAML requires to escape these two + { + s << '\\'; + } + s << char(c); + } + } + s << '"'; + } + + struct SelectorStringLike { }; + struct SelectorPrimitives { }; + struct SelectorObjects { }; + + template + static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorStringLike) + { + bool printable_only = true; + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + if (!isNiceCharacter(array[i])) + { + printable_only = false; + break; + } + } + if (printable_only) + { + streamCharacters(s, array); + } + else + { + streamPrimitives(s, array); + s << " # "; + streamCharacters(s, array); + } + } + + template + static void genericStreamImpl(Stream& s, const ArrayType& array, int, SelectorPrimitives) + { + streamPrimitives(s, array); + } + + template + static void genericStreamImpl(Stream& s, const ArrayType& array, int level, SelectorObjects) + { + if (array.empty()) + { + s << "[]"; + return; + } + for (typename ArrayType::SizeType i = 0; i < array.size(); i++) + { + s << '\n'; + for (int pos = 0; pos < level; pos++) + { + s << " "; + } + s << "- "; + YamlStreamer::stream(s, array.at(i), level + 1); + } + } + +public: + /** + * Prints Array<> into the stream in YAML format. + */ + template + static void stream(Stream& s, const ArrayType& array, int level) + { + typedef typename Select::Result, + SelectorPrimitives, + SelectorObjects>::Result >::Result Type; + genericStreamImpl(s, array, level, Type()); + } +}; + +} + +#endif // UAVCAN_MARSHAL_ARRAY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/bit_stream.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/bit_stream.hpp new file mode 100644 index 0000000000..edbe1a522d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/bit_stream.hpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED +#define UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This function implements fast copy of unaligned bit arrays. It isn't part of the library API, so it is not exported. + * @param src_org Source array + * @param src_offset Bit offset of the first source byte + * @param src_len Number of bits to copy + * @param dst_org Destination array + * @param dst_offset Bit offset of the first destination byte + */ +void bitarrayCopy(const unsigned char* src_org, std::size_t src_offset, std::size_t src_len, + unsigned char* dst_org, std::size_t dst_offset); + +/** + * This class treats a chunk of memory as an array of bits. + * It is used by the bit codec for serialization/deserialization. + */ +class UAVCAN_EXPORT BitStream +{ + static const unsigned MaxBytesPerRW = 16; + + ITransferBuffer& buf_; + unsigned bit_offset_; + uint8_t byte_cache_; + + static inline unsigned bitlenToBytelen(unsigned bits) { return (bits + 7) / 8; } + + static inline void copyBitArrayAlignedToUnaligned(const uint8_t* src_org, unsigned src_len, + uint8_t* dst_org, unsigned dst_offset) + { + bitarrayCopy(reinterpret_cast(src_org), 0, src_len, + reinterpret_cast(dst_org), dst_offset); + } + + static inline void copyBitArrayUnalignedToAligned(const uint8_t* src_org, unsigned src_offset, unsigned src_len, + uint8_t* dst_org) + { + bitarrayCopy(reinterpret_cast(src_org), src_offset, src_len, + reinterpret_cast(dst_org), 0); + } + +public: + static const unsigned MaxBitsPerRW = MaxBytesPerRW * 8; + + enum + { + ResultOutOfBuffer = 0, + ResultOk = 1 + }; + + explicit BitStream(ITransferBuffer& buf) + : buf_(buf) + , bit_offset_(0) + , byte_cache_(0) + { + StaticAssert::check(); + } + + /** + * Write/read calls interpret bytes as bit arrays, 8 bits per byte, where the most + * significant bits have lower index, i.e.: + * Hex: 55 2d + * Bits: 01010101 00101101 + * Indices: 0 .. 7 8 .. 15 + * Return values: + * Negative - Error + * Zero - Out of buffer space + * Positive - OK + */ + int write(const uint8_t* bytes, const unsigned bitlen); + int read(uint8_t* bytes, const unsigned bitlen); + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +} + +#endif // UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/char_array_formatter.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/char_array_formatter.hpp new file mode 100644 index 0000000000..244c3b1204 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/char_array_formatter.hpp @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED +#define UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED + +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +class UAVCAN_EXPORT CharArrayFormatter +{ + ArrayType_& array_; + + template + typename std::enable_if::value>::type + writeValue(T value) + { + array_.template appendFormatted("%g", double(value)); + } + + template + typename std::enable_if::value>::type + writeValue(T value) + { + if (std::is_same()) + { + if (array_.size() != array_.capacity()) + { + array_.push_back(typename ArrayType_::ValueType(value)); + } + } + else if (std::is_signed()) + { + array_.template appendFormatted("%lli", static_cast(value)); + } + else + { + array_.template appendFormatted("%llu", static_cast(value)); + } + } + + template + typename std::enable_if::value && !std::is_same::value>::type + writeValue(T value) + { + array_.template appendFormatted("%p", static_cast(value)); + } + + void writeValue(const char* value) + { + array_.template appendFormatted("%s", value); + } + +public: + typedef ArrayType_ ArrayType; + + explicit CharArrayFormatter(ArrayType& array) + : array_(array) + { } + + ArrayType& getArray() { return array_; } + const ArrayType& getArray() const { return array_; } + + void write(const char* text) + { + writeValue(text); + } + + template + void write(const char* s, T value, Args... args) + { + while (s && *s) + { + if (*s == '%') + { + s += 1; + if (*s != '%') + { + writeValue(value); + write(++s, args...); + break; + } + } + writeValue(*s++); + } + } +}; + +#else + +template +class UAVCAN_EXPORT CharArrayFormatter +{ + ArrayType_& array_; + +public: + typedef ArrayType_ ArrayType; + + CharArrayFormatter(ArrayType& array) + : array_(array) + { } + + ArrayType& getArray() { return array_; } + const ArrayType& getArray() const { return array_; } + + void write(const char* const text) + { + array_.template appendFormatted("%s", text); + } + + /** + * This version does not support more than one formatted argument, though it can be improved. + * And it is unsafe. + * There is typesafe version for C++11 above. + */ + template + void write(const char* const format, const A value) + { + array_.template appendFormatted(format, value); + } +}; + +#endif + +} + +#endif // UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/float_spec.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/float_spec.hpp new file mode 100644 index 0000000000..ef3e93b884 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/float_spec.hpp @@ -0,0 +1,238 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED +#define UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include // Assuming that in C++11 mode all standard headers are available +#endif + +namespace uavcan +{ + +template +struct NativeFloatSelector +{ + struct ErrorNoSuchFloat; + typedef typename Select<(sizeof(float) * 8 >= BitLen), float, + typename Select<(sizeof(double) * 8 >= BitLen), double, + typename Select<(sizeof(long double) * 8 >= BitLen), long double, + ErrorNoSuchFloat>::Result>::Result>::Result Type; +}; + + +class UAVCAN_EXPORT IEEE754Converter +{ + // TODO: Non-IEEE float support + + static uint16_t nativeIeeeToHalf(float value); + static float halfToNativeIeee(uint16_t value); + + IEEE754Converter(); + + template + static void enforceIeee() + { + /* + * Some compilers may have is_iec559 to be defined false despite the fact that IEEE754 is supported. + * An acceptable workaround would be to put an #ifdef here. + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + StaticAssert::Type>::is_iec559>::check(); +#endif + } + +public: +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + /// UAVCAN requires rounding to nearest for all float conversions + static std::float_round_style roundstyle() { return std::round_to_nearest; } +#endif + + template + static typename IntegerSpec::StorageType + toIeee(typename NativeFloatSelector::Type value) + { + enforceIeee(); + union + { + typename IntegerSpec::StorageType i; + typename NativeFloatSelector::Type f; + } u; + StaticAssert::check(); + u.f = value; + return u.i; + } + + template + static typename NativeFloatSelector::Type + toNative(typename IntegerSpec::StorageType value) + { + enforceIeee(); + union + { + typename IntegerSpec::StorageType i; + typename NativeFloatSelector::Type f; + } u; + StaticAssert::check(); + u.i = value; + return u.f; + } +}; +template <> +inline typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType +IEEE754Converter::toIeee<16>(typename NativeFloatSelector<16>::Type value) +{ + return nativeIeeeToHalf(value); +} +template <> +inline typename NativeFloatSelector<16>::Type +IEEE754Converter::toNative<16>(typename IntegerSpec<16, SignednessUnsigned, CastModeTruncate>::StorageType value) +{ + return halfToNativeIeee(value); +} + + +template struct IEEE754Limits; +template <> struct IEEE754Limits<16> +{ + typedef typename NativeFloatSelector<16>::Type NativeType; + static NativeType max() { return static_cast(65504.0); } + static NativeType epsilon() { return static_cast(9.77e-04); } +}; +template <> struct IEEE754Limits<32> +{ + typedef typename NativeFloatSelector<32>::Type NativeType; + static NativeType max() { return static_cast(3.40282346638528859812e+38); } + static NativeType epsilon() { return static_cast(1.19209289550781250000e-7); } +}; +template <> struct IEEE754Limits<64> +{ + typedef typename NativeFloatSelector<64>::Type NativeType; + static NativeType max() { return static_cast(1.79769313486231570815e+308L); } + static NativeType epsilon() { return static_cast(2.22044604925031308085e-16L); } +}; + + +template +class UAVCAN_EXPORT FloatSpec : public IEEE754Limits +{ + FloatSpec(); + +public: + enum { BitLen = BitLen_ }; + enum { MinBitLen = BitLen }; + enum { MaxBitLen = BitLen }; + enum { IsPrimitive = 1 }; + + typedef typename NativeFloatSelector::Type StorageType; + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 + enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) }; +#else + enum { IsExactRepresentation = (sizeof(StorageType) * 8 == BitLen) && std::numeric_limits::is_iec559 }; +#endif + + using IEEE754Limits::max; + using IEEE754Limits::epsilon; +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + static std::float_round_style roundstyle() { return IEEE754Converter::roundstyle(); } +#endif + + static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode) + { + // cppcheck-suppress duplicateExpression + if (CastMode == CastModeSaturate) + { + saturate(value); + } + else + { + truncate(value); + } + return codec.encode(IEEE754Converter::toIeee(value)); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode) + { + typename IntegerSpec::StorageType ieee = 0; + const int res = codec.decode(ieee); + if (res <= 0) + { + return res; + } + out_value = IEEE754Converter::toNative(ieee); + return res; + } + + static void extendDataTypeSignature(DataTypeSignature&) { } + +private: + static inline void saturate(StorageType& value) + { + if ((IsExactRepresentation == 0) && isFinite(value)) + { + if (value > max()) + { + value = max(); + } + else if (value < -max()) + { + value = -max(); + } + else + { + ; // Valid range + } + } + } + + static inline void truncate(StorageType& value) + { + if ((IsExactRepresentation == 0) && isFinite(value)) + { + if (value > max()) + { + value = NumericTraits::infinity(); + } + else if (value < -max()) + { + value = -NumericTraits::infinity(); + } + else + { + ; // Valid range + } + } + } +}; + + +template +class UAVCAN_EXPORT YamlStreamer > +{ + typedef typename FloatSpec::StorageType StorageType; + +public: + template // cppcheck-suppress passedByValue + static void stream(Stream& s, const StorageType value, int) + { + s << value; + } +}; + +} + +#endif // UAVCAN_MARSHAL_FLOAT_SPEC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/integer_spec.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/integer_spec.hpp new file mode 100644 index 0000000000..e5a87d7cb6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/integer_spec.hpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED +#define UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +enum Signedness { SignednessUnsigned, SignednessSigned }; + +/** + * This template will be used for signed and unsigned integers more than 1 bit long. + * There are explicit specializations for booleans below. + */ +template +class UAVCAN_EXPORT IntegerSpec +{ + struct ErrorNoSuchInteger; + +public: + enum { IsSigned = Signedness == SignednessSigned }; + enum { BitLen = BitLen_ }; + enum { MinBitLen = BitLen }; + enum { MaxBitLen = BitLen }; + enum { IsPrimitive = 1 }; + + typedef typename Select<(BitLen <= 8), typename Select::Result, + typename Select<(BitLen <= 16), typename Select::Result, + typename Select<(BitLen <= 32), typename Select::Result, + typename Select<(BitLen <= 64), typename Select::Result, + ErrorNoSuchInteger>::Result>::Result>::Result>::Result StorageType; + + typedef typename IntegerSpec::StorageType UnsignedStorageType; + +private: + IntegerSpec(); + + struct LimitsImplGeneric + { + static StorageType max() + { + StaticAssert<(sizeof(uintmax_t) >= 8)>::check(); + if (IsSigned == 0) + { + return StorageType((uintmax_t(1) << static_cast(BitLen)) - 1U); + } + else + { + return StorageType((uintmax_t(1) << (static_cast(BitLen) - 1U)) - 1); + } + } + static UnsignedStorageType mask() + { + StaticAssert<(sizeof(uintmax_t) >= 8U)>::check(); + return UnsignedStorageType((uintmax_t(1) << static_cast(BitLen)) - 1U); + } + }; + + struct LimitsImpl64 + { + static StorageType max() + { + return StorageType((IsSigned == 0) ? 0xFFFFFFFFFFFFFFFFULL : 0x7FFFFFFFFFFFFFFFLL); + } + static UnsignedStorageType mask() { return 0xFFFFFFFFFFFFFFFFULL; } + }; + + typedef typename Select<(BitLen == 64), LimitsImpl64, LimitsImplGeneric>::Result Limits; + + static void saturate(StorageType& value) + { + if (value > max()) + { + value = max(); + } + else if (value <= min()) // 'Less or Equal' allows to suppress compiler warning on unsigned types + { + value = min(); + } + else + { + ; // Valid range + } + } + + static void truncate(StorageType& value) { value = value & StorageType(mask()); } + + static void validate() + { + StaticAssert<(BitLen <= (sizeof(StorageType) * 8))>::check(); + // coverity[result_independent_of_operands : FALSE] + UAVCAN_ASSERT(max() <= NumericTraits::max()); + // coverity[result_independent_of_operands : FALSE] + UAVCAN_ASSERT(min() >= NumericTraits::min()); + } + +public: + static StorageType max() { return Limits::max(); } + static StorageType min() { return IsSigned ? StorageType(-max() - 1) : 0; } + static UnsignedStorageType mask() { return Limits::mask(); } + + static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode) + { + validate(); + // cppcheck-suppress duplicateExpression + if (CastMode == CastModeSaturate) + { + saturate(value); + } + else + { + truncate(value); + } + return codec.encode(value); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode) + { + validate(); + return codec.decode(out_value); + } + + static void extendDataTypeSignature(DataTypeSignature&) { } +}; + +/** + * Boolean specialization + */ +template +class UAVCAN_EXPORT IntegerSpec<1, SignednessUnsigned, CastMode> +{ +public: + enum { IsSigned = 0 }; + enum { BitLen = 1 }; + enum { MinBitLen = 1 }; + enum { MaxBitLen = 1 }; + enum { IsPrimitive = 1 }; + + typedef bool StorageType; + typedef bool UnsignedStorageType; + +private: + IntegerSpec(); + +public: + static StorageType max() { return true; } + static StorageType min() { return false; } + static UnsignedStorageType mask() { return true; } + + static int encode(StorageType value, ScalarCodec& codec, TailArrayOptimizationMode) + { + return codec.encode(value); + } + + static int decode(StorageType& out_value, ScalarCodec& codec, TailArrayOptimizationMode) + { + return codec.decode(out_value); + } + + static void extendDataTypeSignature(DataTypeSignature&) { } +}; + +template +class IntegerSpec<1, SignednessSigned, CastMode>; // Invalid instantiation + +template +class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation + + +template +struct IsIntegerSpec +{ + enum { Result = 0 }; +}; + +template +struct IsIntegerSpec > +{ + enum { Result = 1 }; +}; + + +template +class UAVCAN_EXPORT YamlStreamer > +{ + typedef IntegerSpec RawType; + typedef typename RawType::StorageType StorageType; + +public: + template // cppcheck-suppress passedByValue + static void stream(Stream& s, const StorageType value, int) + { + // Get rid of character types - we want its integer representation, not ASCII code + typedef typename Select<(sizeof(StorageType) >= sizeof(int)), StorageType, + typename Select::Result >::Result TempType; + s << TempType(value); + } +}; + +} + +#endif // UAVCAN_MARSHAL_INTEGER_SPEC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/scalar_codec.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/scalar_codec.hpp new file mode 100644 index 0000000000..2690f6e1d6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/scalar_codec.hpp @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED +#define UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements fast encoding/decoding of primitive type scalars into/from bit arrays. + * It uses the compile-time type information to eliminate run-time operations where possible. + */ +class UAVCAN_EXPORT ScalarCodec +{ + BitStream& stream_; + + static void swapByteOrder(uint8_t* bytes, unsigned len); + + template + static typename EnableIf<(BitLen > 8)>::Type + convertByteOrder(uint8_t (&bytes)[Size]) + { +#if defined(BYTE_ORDER) && defined(BIG_ENDIAN) + static const bool big_endian = BYTE_ORDER == BIG_ENDIAN; +#else + union { long int l; char c[sizeof(long int)]; } u; + u.l = 1; + const bool big_endian = u.c[sizeof(long int) - 1] == 1; +#endif + /* + * I didn't have any big endian machine nearby, so big endian support wasn't tested yet. + * It is likely to be OK anyway, so feel free to remove this UAVCAN_ASSERT() as needed. + */ + UAVCAN_ASSERT(big_endian == false); + if (big_endian) + { + swapByteOrder(bytes, Size); + } + } + + template + static typename EnableIf<(BitLen <= 8)>::Type + convertByteOrder(uint8_t (&)[Size]) { } + + template + static typename EnableIf(NumericTraits::IsSigned) && ((sizeof(T) * 8) > BitLen)>::Type + fixTwosComplement(T& value) + { + StaticAssert::IsInteger>::check(); // Not applicable to floating point types + if (value & (T(1) << (BitLen - 1))) // The most significant bit is set --> negative + { + value |= T(T(0xFFFFFFFFFFFFFFFFULL) & ~((T(1) << BitLen) - 1)); + } + } + + template + static typename EnableIf(NumericTraits::IsSigned) || ((sizeof(T) * 8) == BitLen)>::Type + fixTwosComplement(T&) { } + + template + static typename EnableIf<((sizeof(T) * 8) > BitLen)>::Type + clearExtraBits(T& value) + { + value &= (T(1) << BitLen) - 1; // Signedness doesn't matter + } + + template + static typename EnableIf<((sizeof(T) * 8) == BitLen)>::Type + clearExtraBits(T&) { } + + template + void validate() + { + StaticAssert<((sizeof(T) * 8) >= BitLen)>::check(); + StaticAssert<(BitLen <= BitStream::MaxBitsPerRW)>::check(); + StaticAssert(NumericTraits::IsSigned) ? (BitLen > 1) : true>::check(); + } + + int encodeBytesImpl(uint8_t* bytes, unsigned bitlen); + int decodeBytesImpl(uint8_t* bytes, unsigned bitlen); + +public: + explicit ScalarCodec(BitStream& stream) + : stream_(stream) + { } + + template + int encode(const T value); + + template + int decode(T& value); +}; + +// ---------------------------------------------------------------------------- + +template +int ScalarCodec::encode(const T value) +{ + validate(); + union ByteUnion + { + T value; + uint8_t bytes[sizeof(T)]; + } byte_union; + byte_union.value = value; + clearExtraBits(byte_union.value); + convertByteOrder(byte_union.bytes); + return encodeBytesImpl(byte_union.bytes, BitLen); +} + +template +int ScalarCodec::decode(T& value) +{ + validate(); + union ByteUnion + { + T value; + uint8_t bytes[sizeof(T)]; + } byte_union; + byte_union.value = T(); + const int read_res = decodeBytesImpl(byte_union.bytes, BitLen); + if (read_res > 0) + { + convertByteOrder(byte_union.bytes); + fixTwosComplement(byte_union.value); + value = byte_union.value; + } + return read_res; +} + +} + +#endif // UAVCAN_MARSHAL_SCALAR_CODEC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/type_util.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/type_util.hpp new file mode 100644 index 0000000000..5be02921e8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/type_util.hpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED +#define UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ +/** + * Read the specs to learn more about cast modes. + */ +enum CastMode { CastModeSaturate, CastModeTruncate }; + +/** + * Read the specs to learn more about tail array optimizations. + */ +enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled }; + +/** + * Compile-time: Returns the number of bits needed to represent an integer value. + */ +template +struct IntegerBitLen +{ + enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; +}; +template <> +struct IntegerBitLen<0> +{ + enum { Result = 0 }; +}; + +/** + * Compile-time: Returns the number of bytes needed to contain the given number of bits. Assumes 1 byte == 8 bit. + */ +template +struct BitLenToByteLen +{ + enum { Result = (BitLen + 7) / 8 }; +}; + +/** + * Compile-time: Helper for integer and float specialization classes. Returns the platform-specific storage type. + */ +template +struct StorageType +{ + typedef T Type; +}; +template +struct StorageType::Type> +{ + typedef typename T::StorageType Type; +}; + +/** + * Compile-time: Whether T is a primitive type on this platform. + */ +template +class IsPrimitiveType +{ + typedef char Yes; + struct No { Yes dummy[8]; }; + + template + static typename EnableIf::Type test(int); + + template + static No test(...); + +public: + enum { Result = sizeof(test(0)) == sizeof(Yes) }; +}; + +/** + * Streams a given value into YAML string. Please see the specializations. + */ +template +class UAVCAN_EXPORT YamlStreamer; + +} + +#endif // UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/types.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/types.hpp new file mode 100644 index 0000000000..4fbbe98371 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/marshal/types.hpp @@ -0,0 +1,13 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_MARSHAL_TYPES_HPP_INCLUDED +#define UAVCAN_MARSHAL_TYPES_HPP_INCLUDED + +#include +#include +#include +#include + +#endif // UAVCAN_MARSHAL_TYPES_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/abstract_node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/abstract_node.hpp new file mode 100644 index 0000000000..2976c32e75 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/abstract_node.hpp @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED +#define UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ +/** + * Abstract node class. If you're going to implement your own node class for your application, + * please inherit this class so it can be used with default publisher, subscriber, server, etc. classes. + * Normally you don't need to use it directly though - please refer to the class Node<> instead. + */ +class UAVCAN_EXPORT INode +{ +public: + virtual ~INode() { } + virtual IPoolAllocator& getAllocator() = 0; + virtual Scheduler& getScheduler() = 0; + virtual const Scheduler& getScheduler() const = 0; + virtual void registerInternalFailure(const char* msg) = 0; + + Dispatcher& getDispatcher() { return getScheduler().getDispatcher(); } + const Dispatcher& getDispatcher() const { return getScheduler().getDispatcher(); } + + ISystemClock& getSystemClock() { return getScheduler().getSystemClock(); } + MonotonicTime getMonotonicTime() const { return getScheduler().getMonotonicTime(); } + UtcTime getUtcTime() const { return getScheduler().getUtcTime(); } + + /** + * Returns the Node ID of this node. + * If Node ID was not set yet, an invalid value will be returned. + */ + NodeID getNodeID() const { return getScheduler().getDispatcher().getNodeID(); } + + /** + * Sets the Node ID of this node. + * Node ID can be assigned only once. This method returns true if the Node ID was successfully assigned, otherwise + * it returns false. + * As long as a valid Node ID is not set, the node will remain in passive mode. + * Using a non-unicast Node ID puts the node into passive mode (as default). + */ + bool setNodeID(NodeID nid) + { + return getScheduler().getDispatcher().setNodeID(nid); + } + + /** + * Whether the node is in passive mode, i.e. can't transmit anything to the bus. + * Please read the specs to learn more. + */ + bool isPassiveMode() const { return getScheduler().getDispatcher().isPassiveMode(); } + + /** + * Same as @ref spin(MonotonicDuration), but the deadline is specified as an absolute time value + * rather than duration. + */ + int spin(MonotonicTime deadline) + { + return getScheduler().spin(deadline); + } + + /** + * Runs the node. + * Normally your application should not block anywhere else. + * Block inside this method forever or call it periodically. + * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp). + */ + int spin(MonotonicDuration duration) + { + return getScheduler().spin(getMonotonicTime() + duration); + } + + /** + * This method is designed for non-blocking applications. + * Instead of blocking, it returns immediately once all available CAN frames and timer events are processed. + * Note that this is unlike plain @ref spin(), which will strictly return when the deadline is reached, + * even if there still are unprocessed events. + * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp). + */ + int spinOnce() + { + return getScheduler().spinOnce(); + } + + /** + * This method allows to directly transmit a raw CAN frame circumventing the whole UAVCAN stack. + * Mandatory parameters: + * + * @param frame CAN frame to be transmitted. + * + * @param tx_deadline The frame will be discarded if it could not be transmitted by this time. + * + * @param iface_mask This bitmask allows to select what CAN interfaces this frame should go into. + * Example: + * - 1 - the frame will be sent only to iface 0. + * - 4 - the frame will be sent only to iface 2. + * - 3 - the frame will be sent to ifaces 0 and 1. + * + * Optional parameters: + * + * @param qos Quality of service. Please refer to the CAN IO manager for details. + * + * @param flags CAN IO flags. Please refer to the CAN driver API for details. + */ + int injectTxFrame(const CanFrame& frame, MonotonicTime tx_deadline, uint8_t iface_mask, + CanTxQueue::Qos qos = CanTxQueue::Volatile, + CanIOFlags flags = 0) + { + return getDispatcher().getCanIOManager().send(frame, tx_deadline, MonotonicTime(), iface_mask, qos, flags); + } + +#if !UAVCAN_TINY + /** + * The @ref IRxFrameListener interface allows one to monitor all incoming CAN frames. + * This feature can be used to implement multithreaded nodes, or to add secondary protocol support. + */ + void removeRxFrameListener() { getDispatcher().removeRxFrameListener(); } + void installRxFrameListener(IRxFrameListener* lst) { getDispatcher().installRxFrameListener(lst); } +#endif +}; + +} + +#endif // UAVCAN_NODE_ABSTRACT_NODE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_publisher.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_publisher.hpp new file mode 100644 index 0000000000..a8197b6cf9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED +#define UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class GenericPublisherBase : Noncopyable +{ + TransferSender sender_; + MonotonicDuration tx_timeout_; + INode& node_; + +protected: + GenericPublisherBase(INode& node, MonotonicDuration tx_timeout, + MonotonicDuration max_transfer_interval) + : sender_(node.getDispatcher(), max_transfer_interval) + , tx_timeout_(tx_timeout) + , node_(node) + { + setTxTimeout(tx_timeout); +#if UAVCAN_DEBUG + UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK +#endif + } + + ~GenericPublisherBase() { } + + bool isInited() const; + + int doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos); + + MonotonicTime getTxDeadline() const; + + int genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); + + TransferSender& getTransferSender() { return sender_; } + const TransferSender& getTransferSender() const { return sender_; } + +public: + static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); } + static MonotonicDuration getMaxTxTimeout() { return MonotonicDuration::fromMSec(60000); } + + MonotonicDuration getTxTimeout() const { return tx_timeout_; } + void setTxTimeout(MonotonicDuration tx_timeout); + + /** + * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive. + * This option allows to enable sending anonymous transfers from passive mode. + */ + void allowAnonymousTransfers() + { + sender_.allowAnonymousTransfers(); + } + + /** + * Priority of outgoing transfers. + */ + TransferPriority getPriority() const { return sender_.getPriority(); } + void setPriority(const TransferPriority prio) { sender_.setPriority(prio); } + + INode& getNode() const { return node_; } +}; + +/** + * Generic publisher, suitable for messages and services. + * DataSpec - data type specification class + * DataStruct - instantiable class + */ +template +class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase +{ + struct ZeroTransferBuffer : public StaticTransferBufferImpl + { + ZeroTransferBuffer() : StaticTransferBufferImpl(UAVCAN_NULLPTR, 0) { } + }; + + typedef typename Select::Result> >::Result Buffer; + + enum + { + Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? + CanTxQueue::Volatile : CanTxQueue::Persistent + }; + + int checkInit(); + + int doEncode(const DataStruct& message, ITransferBuffer& buffer) const; + + int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, + TransferID* tid, MonotonicTime blocking_deadline); + +public: + /** + * @param max_transfer_interval Maximum expected time interval between subsequent publications. Leave default. + */ + GenericPublisher(INode& node, MonotonicDuration tx_timeout, + MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) + : GenericPublisherBase(node, tx_timeout, max_transfer_interval) + { } + + ~GenericPublisher() { } + + /** + * Init method can be called prior first publication, but it's not necessary + * because the publisher can be automatically initialized ad-hoc. + */ + int init() + { + return checkInit(); + } + + /** + * This overload allows to set the priority; otherwise it's the same. + */ + int init(TransferPriority priority) + { + setPriority(priority); + return checkInit(); + } + + int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, + MonotonicTime blocking_deadline = MonotonicTime()) + { + return genericPublish(message, transfer_type, dst_node_id, UAVCAN_NULLPTR, blocking_deadline); + } + + int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID tid, + MonotonicTime blocking_deadline = MonotonicTime()) + { + return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline); + } +}; + +// ---------------------------------------------------------------------------- + +template +int GenericPublisher::checkInit() +{ + if (isInited()) + { + return 0; + } + return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos)); +} + +template +int GenericPublisher::doEncode(const DataStruct& message, ITransferBuffer& buffer) const +{ + BitStream bitstream(buffer); + ScalarCodec codec(bitstream); + const int encode_res = DataStruct::encode(message, codec); + if (encode_res <= 0) + { + UAVCAN_ASSERT(0); // Impossible, internal error + return -ErrInvalidMarshalData; + } + return encode_res; +} + +template +int GenericPublisher::genericPublish(const DataStruct& message, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, + MonotonicTime blocking_deadline) +{ + const int res = checkInit(); + if (res < 0) + { + return res; + } + + Buffer buffer; + + const int encode_res = doEncode(message, buffer); + if (encode_res < 0) + { + return encode_res; + } + + return GenericPublisherBase::genericPublish(buffer, transfer_type, dst_node_id, tid, blocking_deadline); +} + +} + +#endif // UAVCAN_NODE_GENERIC_PUBLISHER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_subscriber.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_subscriber.hpp new file mode 100644 index 0000000000..ef0cb877bf --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -0,0 +1,299 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED +#define UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class extends the data structure with extra information obtained from the transport layer, + * such as Source Node ID, timestamps, Transfer ID, index of the interface this transfer was picked up from, etc. + * + * PLEASE NOTE that since this class inherits the data structure type, subscription callbacks can accept either + * object of this class or the data structure type directly if the extra information is not needed. + * + * For example, both of these callbacks can be used with the same data structure 'Foo': + * void first(const ReceivedDataStructure& msg); + * void second(const Foo& msg); + * In the latter case, an implicit cast will happen before the callback is invoked. + * + * This class is not copyable because it holds a reference to a stack-allocated transfer descriptor object. + * You can slice cast it to the underlying data type though, which would be copyable: + * DataType dt = rds; // where rds is of type ReceivedDataStructure + * // dt is now copyable + */ +template +class UAVCAN_EXPORT ReceivedDataStructure : public DataType_, Noncopyable +{ + const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields + + template + Ret safeget() const + { + if (_transfer_ == UAVCAN_NULLPTR) + { + return Ret(); + } + return (_transfer_->*Fun)(); + } + +protected: + ReceivedDataStructure() + : _transfer_(UAVCAN_NULLPTR) + { } + + ReceivedDataStructure(const IncomingTransfer* arg_transfer) + : _transfer_(arg_transfer) + { + UAVCAN_ASSERT(arg_transfer != UAVCAN_NULLPTR); + } + +public: + typedef DataType_ DataType; + + MonotonicTime getMonotonicTimestamp() const + { + return safeget(); + } + UtcTime getUtcTimestamp() const { return safeget(); } + TransferPriority getPriority() const { return safeget(); } + TransferType getTransferType() const { return safeget(); } + TransferID getTransferID() const { return safeget(); } + NodeID getSrcNodeID() const { return safeget(); } + uint8_t getIfaceIndex() const { return safeget(); } + bool isAnonymousTransfer() const { return safeget(); } +}; + +/** + * This operator neatly prints the data structure prepended with extra data from the transport layer. + * The extra data will be represented as YAML comment. + */ +template +static Stream& operator<<(Stream& s, const ReceivedDataStructure& rds) +{ + s << "# Received struct ts_m=" << rds.getMonotonicTimestamp() + << " ts_utc=" << rds.getUtcTimestamp() + << " snid=" << int(rds.getSrcNodeID().get()) << "\n"; + s << static_cast(rds); + return s; +} + + +class GenericSubscriberBase : Noncopyable +{ +protected: + INode& node_; + uint32_t failure_count_; + + explicit GenericSubscriberBase(INode& node) + : node_(node) + , failure_count_(0) + { } + + ~GenericSubscriberBase() { } + + int genericStart(TransferListener* listener, bool (Dispatcher::*registration_method)(TransferListener*)); + + void stop(TransferListener* listener); + +public: + /** + * Returns the number of failed attempts to decode received message. Generally, a failed attempt means either: + * - Transient failure in the transport layer. + * - Incompatible data types. + */ + uint32_t getFailureCount() const { return failure_count_; } + + INode& getNode() const { return node_; } +}; + +/** + * Please note that the reference passed to the RX callback points to a stack-allocated object, which means + * that it gets invalidated shortly after the callback returns. + */ +template +class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase +{ + typedef GenericSubscriber SelfType; + + // We need to break the inheritance chain here to implement lazy initialization + class TransferForwarder : public TransferListenerType + { + SelfType& obj_; + + void handleIncomingTransfer(IncomingTransfer& transfer) override + { + obj_.handleIncomingTransfer(transfer); + } + + public: + TransferForwarder(SelfType& obj, + const DataTypeDescriptor& data_type, + uint16_t max_buffer_size, + IPoolAllocator& allocator) : + TransferListenerType(obj.node_.getDispatcher().getTransferPerfCounter(), + data_type, + max_buffer_size, + allocator), + obj_(obj) + { } + }; + + LazyConstructor forwarder_; + + int checkInit(); + + void handleIncomingTransfer(IncomingTransfer& transfer); + + int genericStart(bool (Dispatcher::*registration_method)(TransferListener*)); + +protected: + struct ReceivedDataStructureSpec : public ReceivedDataStructure + { + ReceivedDataStructureSpec() { } + + ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) : + ReceivedDataStructure(arg_transfer) + { } + }; + + explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) + { } + + virtual ~GenericSubscriber() { stop(); } + + virtual void handleReceivedDataStruct(ReceivedDataStructure&) = 0; + + int startAsMessageListener() + { + UAVCAN_TRACE("GenericSubscriber", "Start as message listener; dtname=%s", DataSpec::getDataTypeFullName()); + return genericStart(&Dispatcher::registerMessageListener); + } + + int startAsServiceRequestListener() + { + UAVCAN_TRACE("GenericSubscriber", "Start as service request listener; dtname=%s", + DataSpec::getDataTypeFullName()); + return genericStart(&Dispatcher::registerServiceRequestListener); + } + + int startAsServiceResponseListener() + { + UAVCAN_TRACE("GenericSubscriber", "Start as service response listener; dtname=%s", + DataSpec::getDataTypeFullName()); + return genericStart(&Dispatcher::registerServiceResponseListener); + } + + /** + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. + */ + void allowAnonymousTransfers() + { + forwarder_->allowAnonymousTransfers(); + } + + /** + * Terminate the subscription. + * Dispatcher core will remove this instance from the subscribers list. + */ + void stop() + { + UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName()); + GenericSubscriberBase::stop(forwarder_); + } + + TransferListenerType* getTransferListener() { return forwarder_; } +}; + +// ---------------------------------------------------------------------------- + +/* + * GenericSubscriber + */ +template +int GenericSubscriber::checkInit() +{ + if (forwarder_) + { + return 0; + } + + GlobalDataTypeRegistry::instance().freeze(); + const DataTypeDescriptor* const descr = + GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName()); + if (descr == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName()); + return -ErrUnknownDataType; + } + + static const uint16_t MaxBufferSize = BitLenToByteLen::Result; + + forwarder_.template construct + (*this, *descr, MaxBufferSize, node_.getAllocator()); + + return 0; +} + +template +void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) +{ + ReceivedDataStructureSpec rx_struct(&transfer); + + /* + * Decoding into the temporary storage + */ + BitStream bitstream(transfer); + ScalarCodec codec(bitstream); + + const int decode_res = DataStruct::decode(rx_struct, codec); + + // We don't need the data anymore, the memory can be reused from the callback: + transfer.release(); + + if (decode_res <= 0) + { + UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]", + decode_res, DataSpec::getDataTypeFullName()); + failure_count_++; + node_.getDispatcher().getTransferPerfCounter().addError(); + return; + } + + /* + * Invoking the callback + */ + handleReceivedDataStruct(rx_struct); +} + +template +int GenericSubscriber:: +genericStart(bool (Dispatcher::*registration_method)(TransferListener*)) +{ + const int res = checkInit(); + if (res < 0) + { + UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName()); + return res; + } + return GenericSubscriberBase::genericStart(forwarder_, registration_method); +} + + +} + +#endif // UAVCAN_NODE_GENERIC_SUBSCRIBER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/global_data_type_registry.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/global_data_type_registry.hpp new file mode 100644 index 0000000000..0148ce0f50 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/global_data_type_registry.hpp @@ -0,0 +1,241 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED +#define UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#if UAVCAN_DEBUG +# include +#endif + +namespace uavcan +{ +/** + * This singleton is shared among all existing node instances. It is instantiated automatically + * when the C++ runtime executes contstructors before main(). + * + * Its purpose is to keep the list of all UAVCAN data types known and used by this application. + * + * Also, the mapping between Data Type name and its Data Type ID is also stored in this singleton. + * UAVCAN data types with default Data Type ID that are autogenerated by the libuavcan DSDL compiler + * are registered automatically before main() (refer to the generated headers to see how exactly). + * Data types that don't have a default Data Type ID must be registered manually using the methods + * of this class (read the method documentation). + * + * Attempt to use a data type that was not registered with this singleton (e.g. publish, subscribe, + * perform a service call etc.) will fail with an error code @ref ErrUnknownDataType. + */ +class UAVCAN_EXPORT GlobalDataTypeRegistry : Noncopyable +{ + struct Entry : public LinkedListNode + { + DataTypeDescriptor descriptor; + + Entry() { } + + Entry(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) + : descriptor(kind, id, signature, name) + { } + }; + + struct EntryInsertionComparator + { + const DataTypeID id; + explicit EntryInsertionComparator(const Entry* dtd) + : id((dtd == UAVCAN_NULLPTR) ? DataTypeID() : dtd->descriptor.getID()) + { + UAVCAN_ASSERT(dtd != UAVCAN_NULLPTR); + } + bool operator()(const Entry* entry) const + { + UAVCAN_ASSERT(entry != UAVCAN_NULLPTR); + return entry->descriptor.getID() > id; + } + }; + +public: + /** + * Result of data type registration + */ + enum RegistrationResult + { + RegistrationResultOk, ///< Success, data type is now registered and can be used. + RegistrationResultCollision, ///< Data type name or ID is not unique. + RegistrationResultInvalidParams, ///< Invalid input parameters. + RegistrationResultFrozen ///< Data Type Registery has been frozen and can't be modified anymore. + }; + +private: + typedef LinkedListRoot List; + mutable List msgs_; + mutable List srvs_; + bool frozen_; + + GlobalDataTypeRegistry() : frozen_(false) { } + + List* selectList(DataTypeKind kind) const; + + RegistrationResult remove(Entry* dtd); + RegistrationResult registImpl(Entry* dtd); + +public: + /** + * Returns the reference to the singleton. + */ + static GlobalDataTypeRegistry& instance(); + + /** + * Register a data type 'Type' with ID 'id'. + * If this data type was registered earlier, its old registration will be overridden. + * This method will fail if the data type registry is frozen. + * + * @tparam Type Autogenerated UAVCAN data type to register. Data types are generated by the + * libuavcan DSDL compiler from DSDL definitions. + * + * @param id Data Type ID for this data type. + */ + template + RegistrationResult registerDataType(DataTypeID id); + + /** + * Data Type registry needs to be frozen before a node instance can use it in + * order to prevent accidental change in data type configuration on a running + * node. + * + * This method will be called automatically by the node during start up, so + * the user does not need to call it from the application manually. Subsequent + * calls will not have any effect. + * + * Once frozen, data type registry can't be unfrozen. + */ + void freeze(); + bool isFrozen() const { return frozen_; } + + /** + * Finds data type descriptor by full data type name, e.g. "uavcan.protocol.NodeStatus". + * Messages are searched first, then services. + * Returns null pointer if the data type with this name is not registered. + * @param name Full data type name + * @return Descriptor for this data type or null pointer if not found + */ + const DataTypeDescriptor* find(const char* name) const; + + /** + * Finds data type descriptor by full data type name, e.g. "uavcan.protocol.NodeStatus", and data type kind. + * Returns null pointer if the data type with this name is not registered. + * @param kind Data Type Kind - message or service + * @param name Full data type name + * @return Descriptor for this data type or null pointer if not found + */ + const DataTypeDescriptor* find(DataTypeKind kind, const char* name) const; + + /** + * Finds data type descriptor by data type ID. + * Returns null pointer if the data type with this ID is not registered. + * @param kind Data Type Kind - message or service + * @param dtid Data Type ID + * @return Descriptor for this data type or null pointer if not found + */ + const DataTypeDescriptor* find(DataTypeKind kind, DataTypeID dtid) const; + + /** + * Returns the number of registered message types. + */ + unsigned getNumMessageTypes() const { return msgs_.getLength(); } + + /** + * Returns the number of registered service types. + */ + unsigned getNumServiceTypes() const { return srvs_.getLength(); } + +#if UAVCAN_DEBUG + /// Required for unit testing + void reset() + { + UAVCAN_TRACE("GlobalDataTypeRegistry", "Reset; was frozen: %i, num msgs: %u, num srvs: %u", + int(frozen_), getNumMessageTypes(), getNumServiceTypes()); + frozen_ = false; + while (msgs_.get()) + { + msgs_.remove(msgs_.get()); + } + while (srvs_.get()) + { + srvs_.remove(srvs_.get()); + } + } +#endif +}; + +/** + * This class is used by autogenerated data types to register with the + * data type registry automatically before main() is called. Note that + * if a generated data type header file is not included by any translation + * unit of the application, the data type will not be registered. + * + * Data type needs to have a default ID to be registrable by this class. + */ +template +struct UAVCAN_EXPORT DefaultDataTypeRegistrator +{ + DefaultDataTypeRegistrator() + { +#if !UAVCAN_NO_GLOBAL_DATA_TYPE_REGISTRY + const GlobalDataTypeRegistry::RegistrationResult res = + GlobalDataTypeRegistry::instance().registerDataType(Type::DefaultDataTypeID); + + if (res != GlobalDataTypeRegistry::RegistrationResultOk) + { + handleFatalError("Type reg failed"); + } +#endif + } +}; + +// ---------------------------------------------------------------------------- + +/* + * GlobalDataTypeRegistry + */ +template +GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::registerDataType(DataTypeID id) +{ + if (isFrozen()) + { + return RegistrationResultFrozen; + } + + static Entry entry; + + { + const RegistrationResult remove_res = remove(&entry); + if (remove_res != RegistrationResultOk) + { + return remove_res; + } + } + + // We can't just overwrite the entry itself because it's noncopyable + entry.descriptor = DataTypeDescriptor(DataTypeKind(Type::DataTypeKind), id, + Type::getDataTypeSignature(), Type::getDataTypeFullName()); + + { + const RegistrationResult remove_res = remove(&entry); + if (remove_res != RegistrationResultOk) + { + return remove_res; + } + } + return registImpl(&entry); +} + +} + +#endif // UAVCAN_NODE_GLOBAL_DATA_TYPE_REGISTRY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/node.hpp new file mode 100644 index 0000000000..b19b246824 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/node.hpp @@ -0,0 +1,319 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_NODE_HPP_INCLUDED +#define UAVCAN_NODE_NODE_HPP_INCLUDED + +#include +#include +#include +#include + +// High-level functionality available by default +#include + +#if !UAVCAN_TINY +# include +# include +# include +# include +#endif + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * This is the top-level node API. + * A custom node class can be implemented if needed, in which case it shall inherit INode. + * + * @tparam MemPoolSize Size of memory pool for this node, in bytes. + * Please refer to the documentation for details. + * If this value is zero, the constructor will accept a reference to user-provided allocator. + */ +template +class UAVCAN_EXPORT Node : public INode +{ + typedef typename + Select<(MemPoolSize > 0), + PoolAllocator, // If pool size is specified, use default allocator + IPoolAllocator& // Otherwise use reference to user-provided allocator + >::Result Allocator; + + Allocator pool_allocator_; + Scheduler scheduler_; + + NodeStatusProvider proto_nsp_; +#if !UAVCAN_TINY + DataTypeInfoProvider proto_dtp_; + Logger proto_logger_; + RestartRequestServer proto_rrs_; + TransportStatsProvider proto_tsp_; +#endif + + uint64_t internal_failure_cnt_; + bool started_; + + void commonInit() + { + internal_failure_cnt_ = 0; + started_ = false; + } + +protected: + virtual void registerInternalFailure(const char* msg) override + { + internal_failure_cnt_++; + UAVCAN_TRACE("Node", "Internal failure: %s", msg); +#if UAVCAN_TINY + (void)msg; +#else + (void)getLogger().log(protocol::debug::LogLevel::ERROR, "UAVCAN", msg); +#endif + } + +public: + /** + * This overload is only valid if MemPoolSize > 0. + */ + Node(ICanDriver& can_driver, + ISystemClock& system_clock) : + scheduler_(can_driver, pool_allocator_, system_clock), + proto_nsp_(*this) +#if !UAVCAN_TINY + , proto_dtp_(*this) + , proto_logger_(*this) + , proto_rrs_(*this) + , proto_tsp_(*this) +#endif + { + commonInit(); + } + + /** + * This overload is only valid if MemPoolSize == 0. + */ + Node(ICanDriver& can_driver, + ISystemClock& system_clock, + IPoolAllocator& allocator) : + pool_allocator_(allocator), + scheduler_(can_driver, pool_allocator_, system_clock), + proto_nsp_(*this) +#if !UAVCAN_TINY + , proto_dtp_(*this) + , proto_logger_(*this) + , proto_rrs_(*this) + , proto_tsp_(*this) +#endif + { + commonInit(); + } + + virtual typename RemoveReference::Type& getAllocator() override { return pool_allocator_; } + + virtual Scheduler& getScheduler() override { return scheduler_; } + virtual const Scheduler& getScheduler() const override { return scheduler_; } + + int spin(MonotonicTime deadline) + { + if (started_) + { + return INode::spin(deadline); + } + return -ErrNotInited; + } + + int spin(MonotonicDuration duration) + { + if (started_) + { + return INode::spin(duration); + } + return -ErrNotInited; + } + + int spinOnce() + { + if (started_) + { + return INode::spinOnce(); + } + return -ErrNotInited; + } + + bool isStarted() const { return started_; } + + uint64_t getInternalFailureCount() const { return internal_failure_cnt_; } + + /** + * Starts the node and publishes uavcan.protocol.NodeStatus immediately. + * Does not so anything if the node is already started. + * Once started, the node can't stop. + * If the node failed to start up, it's recommended to destroy the current node instance and start over. + * Returns negative error code. + * @param node_status_transfer_priority Transfer priority that will be used for outgoing NodeStatus messages. + * Normal priority is used by default. + */ + int start(const TransferPriority node_status_transfer_priority = TransferPriority::Default); + + /** + * Gets/sets the node name, e.g. "com.example.product_name". The node name can be set only once. + * The name must be set before the node is started, otherwise the node will refuse to start up. + */ + const NodeStatusProvider::NodeName& getName() const { return proto_nsp_.getName(); } + void setName(const NodeStatusProvider::NodeName& name) { proto_nsp_.setName(name); } + + /** + * Node health code helpers. + */ + void setHealthOk() { proto_nsp_.setHealthOk(); } + void setHealthWarning() { proto_nsp_.setHealthWarning(); } + void setHealthError() { proto_nsp_.setHealthError(); } + void setHealthCritical() { proto_nsp_.setHealthCritical(); } + + /** + * Node mode code helpers. + * Note that INITIALIZATION is the default mode; the application has to manually set it to OPERATIONAL. + */ + void setModeOperational() { proto_nsp_.setModeOperational(); } + void setModeInitialization() { proto_nsp_.setModeInitialization(); } + void setModeMaintenance() { proto_nsp_.setModeMaintenance(); } + void setModeSoftwareUpdate() { proto_nsp_.setModeSoftwareUpdate(); } + + void setModeOfflineAndPublish() + { + proto_nsp_.setModeOffline(); + (void)proto_nsp_.forcePublish(); + } + + /** + * Updates the vendor-specific status code. + */ + void setVendorSpecificStatusCode(NodeStatusProvider::VendorSpecificStatusCode code) + { + proto_nsp_.setVendorSpecificStatusCode(code); + } + + /** + * Gets/sets the node version information. + */ + void setSoftwareVersion(const protocol::SoftwareVersion& version) { proto_nsp_.setSoftwareVersion(version); } + void setHardwareVersion(const protocol::HardwareVersion& version) { proto_nsp_.setHardwareVersion(version); } + + const protocol::SoftwareVersion& getSoftwareVersion() const { return proto_nsp_.getSoftwareVersion(); } + const protocol::HardwareVersion& getHardwareVersion() const { return proto_nsp_.getHardwareVersion(); } + + NodeStatusProvider& getNodeStatusProvider() { return proto_nsp_; } + +#if !UAVCAN_TINY + /** + * Restart handler can be installed to handle external node restart requests (highly recommended). + */ + void setRestartRequestHandler(IRestartRequestHandler* handler) { proto_rrs_.setHandler(handler); } + + RestartRequestServer& getRestartRequestServer() { return proto_rrs_; } + + /** + * Node logging. + * Logging calls are passed directly into the @ref Logger instance. + * Type safe log formatting is supported only in C++11 mode. + * @{ + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + + template + inline void logDebug(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logDebug(source, format, args...); + } + + template + inline void logInfo(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logInfo(source, format, args...); + } + + template + inline void logWarning(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logWarning(source, format, args...); + } + + template + inline void logError(const char* source, const char* format, Args... args) + { + (void)proto_logger_.logError(source, format, args...); + } + +#else + + void logDebug(const char* source, const char* text) { (void)proto_logger_.logDebug(source, text); } + void logInfo(const char* source, const char* text) { (void)proto_logger_.logInfo(source, text); } + void logWarning(const char* source, const char* text) { (void)proto_logger_.logWarning(source, text); } + void logError(const char* source, const char* text) { (void)proto_logger_.logError(source, text); } + +#endif + /** + * @} + */ + + /** + * Use this method to configure logging. + */ + Logger& getLogger() { return proto_logger_; } + +#endif // UAVCAN_TINY +}; + +// ---------------------------------------------------------------------------- + +template +int Node::start(const TransferPriority priority) +{ + if (started_) + { + return 0; + } + GlobalDataTypeRegistry::instance().freeze(); + + int res = 0; + res = proto_nsp_.startAndPublish(priority); + if (res < 0) + { + goto fail; + } +#if !UAVCAN_TINY + res = proto_dtp_.start(); + if (res < 0) + { + goto fail; + } + res = proto_logger_.init(); + if (res < 0) + { + goto fail; + } + res = proto_rrs_.start(); + if (res < 0) + { + goto fail; + } + res = proto_tsp_.start(); + if (res < 0) + { + goto fail; + } +#endif + started_ = res >= 0; + return res; +fail: + UAVCAN_ASSERT(res < 0); + return res; +} + +} + +#endif // UAVCAN_NODE_NODE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/publisher.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/publisher.hpp new file mode 100644 index 0000000000..16ca3537a1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/publisher.hpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_PUBLISHER_HPP_INCLUDED +#define UAVCAN_NODE_PUBLISHER_HPP_INCLUDED + +#include + +namespace uavcan +{ +/** + * Use this class to publish messages to the bus (broadcast, unicast, or both). + * + * @tparam DataType_ Message data type + */ +template +class UAVCAN_EXPORT Publisher : protected GenericPublisher +{ + typedef GenericPublisher BaseType; + +public: + typedef DataType_ DataType; ///< Message data type + + /** + * @param node Node instance this publisher will be registered with. + * + * @param tx_timeout If CAN frames of this message are not delivered to the bus + * in this amount of time, they will be discarded. Default value + * is good enough for rather high-frequency, high-priority messages. + * + * @param max_transfer_interval Maximum expected transfer interval. It's absolutely safe + * to leave default value here. It just defines how soon the + * Transfer ID tracking objects associated with this message type + * will be garbage collected by the library if it's no longer + * being published. + */ + explicit Publisher(INode& node, MonotonicDuration tx_timeout = getDefaultTxTimeout(), + MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) + : BaseType(node, tx_timeout, max_transfer_interval) + { +#if UAVCAN_DEBUG + UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK +#endif + StaticAssert::check(); + } + + /** + * Broadcast the message. + * Returns negative error code. + */ + int broadcast(const DataType& message) + { + return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast); + } + + /** + * Warning: You probably don't want to use this method; it's for advanced use cases like + * e.g. network time synchronization. Use the overloaded method with fewer arguments instead. + * This overload allows to explicitly specify Transfer ID. + * Returns negative error code. + */ + int broadcast(const DataType& message, TransferID tid) + { + return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast, tid); + } + + static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(100); } + + /** + * Init method can be called prior first publication, but it's not necessary + * because the publisher can be automatically initialized ad-hoc. + */ + using BaseType::init; + + using BaseType::allowAnonymousTransfers; + using BaseType::getTransferSender; + using BaseType::getMinTxTimeout; + using BaseType::getMaxTxTimeout; + using BaseType::getTxTimeout; + using BaseType::setTxTimeout; + using BaseType::getPriority; + using BaseType::setPriority; + using BaseType::getNode; +}; + +} + +#endif // UAVCAN_NODE_PUBLISHER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/scheduler.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/scheduler.hpp new file mode 100644 index 0000000000..7973337983 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/scheduler.hpp @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SCHEDULER_HPP_INCLUDED +#define UAVCAN_NODE_SCHEDULER_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT Scheduler; + +class UAVCAN_EXPORT DeadlineHandler : public LinkedListNode +{ + MonotonicTime deadline_; + +protected: + Scheduler& scheduler_; + + explicit DeadlineHandler(Scheduler& scheduler) + : scheduler_(scheduler) + { } + + virtual ~DeadlineHandler() { stop(); } + +public: + virtual void handleDeadline(MonotonicTime current) = 0; + + void startWithDeadline(MonotonicTime deadline); + void startWithDelay(MonotonicDuration delay); + void generateDeadlineImmediately() { startWithDeadline(MonotonicTime::fromUSec(1)); } + + void stop(); + + bool isRunning() const; + + MonotonicTime getDeadline() const { return deadline_; } + Scheduler& getScheduler() const { return scheduler_; } +}; + + +class UAVCAN_EXPORT DeadlineScheduler : Noncopyable +{ + LinkedListRoot handlers_; // Ordered by deadline, lowest first + +public: + void add(DeadlineHandler* mdh); + void remove(DeadlineHandler* mdh); + bool doesExist(const DeadlineHandler* mdh) const; + unsigned getNumHandlers() const { return handlers_.getLength(); } + + MonotonicTime pollAndGetMonotonicTime(ISystemClock& sysclock); + MonotonicTime getEarliestDeadline() const; +}; + +/** + * This class distributes processing time between library components (IO handling, deadline callbacks, ...). + */ +class UAVCAN_EXPORT Scheduler : Noncopyable +{ + enum { DefaultDeadlineResolutionMs = 5 }; + enum { MinDeadlineResolutionMs = 1 }; + enum { MaxDeadlineResolutionMs = 100 }; + + enum { DefaultCleanupPeriodMs = 1000 }; + enum { MinCleanupPeriodMs = 10 }; + enum { MaxCleanupPeriodMs = 10000 }; + + DeadlineScheduler deadline_scheduler_; + Dispatcher dispatcher_; + MonotonicTime prev_cleanup_ts_; + MonotonicDuration deadline_resolution_; + MonotonicDuration cleanup_period_; + bool inside_spin_; + + struct InsideSpinSetter + { + Scheduler& owner; + InsideSpinSetter(Scheduler& o) + : owner(o) + { + owner.inside_spin_ = true; + } + ~InsideSpinSetter() { owner.inside_spin_ = false; } + }; + + MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const; + void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin); + +public: + Scheduler(ICanDriver& can_driver, IPoolAllocator& allocator, ISystemClock& sysclock) + : dispatcher_(can_driver, allocator, sysclock) + , prev_cleanup_ts_(sysclock.getMonotonic()) + , deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs)) + , cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs)) + , inside_spin_(false) + { } + + /** + * Spin until the deadline, or until some error occurs. + * This function will return strictly when the deadline is reached, even if there are unprocessed frames. + * Returns negative error code. + */ + int spin(MonotonicTime deadline); + + /** + * Non-blocking version of @ref spin() - spins until all pending frames and events are processed, + * or until some error occurs. If there's nothing to do, returns immediately. + * Returns negative error code. + */ + int spinOnce(); + + DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; } + + Dispatcher& getDispatcher() { return dispatcher_; } + const Dispatcher& getDispatcher() const { return dispatcher_; } + + ISystemClock& getSystemClock() { return dispatcher_.getSystemClock(); } + MonotonicTime getMonotonicTime() const { return dispatcher_.getSystemClock().getMonotonic(); } + UtcTime getUtcTime() const { return dispatcher_.getSystemClock().getUtc(); } + + /** + * Worst case deadline callback resolution. + * Higher resolution increases CPU usage. + */ + MonotonicDuration getDeadlineResolution() const { return deadline_resolution_; } + void setDeadlineResolution(MonotonicDuration res) + { + res = min(res, MonotonicDuration::fromMSec(MaxDeadlineResolutionMs)); + res = max(res, MonotonicDuration::fromMSec(MinDeadlineResolutionMs)); + deadline_resolution_ = res; + } + + /** + * How often the scheduler will run cleanup (listeners, outgoing transfer registry, ...). + * Cleanup execution time grows linearly with number of listeners and number of items + * in the Outgoing Transfer ID registry. + * Lower period increases CPU usage. + */ + MonotonicDuration getCleanupPeriod() const { return cleanup_period_; } + void setCleanupPeriod(MonotonicDuration period) + { + period = min(period, MonotonicDuration::fromMSec(MaxCleanupPeriodMs)); + period = max(period, MonotonicDuration::fromMSec(MinCleanupPeriodMs)); + cleanup_period_ = period; + } +}; + +} + +#endif // UAVCAN_NODE_SCHEDULER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_client.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_client.hpp new file mode 100644 index 0000000000..d5e3c47d49 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_client.hpp @@ -0,0 +1,585 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED +#define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED + +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * This struct describes a pending service call. + * Refer to @ref ServiceClient to learn more about service calls. + */ +struct ServiceCallID +{ + NodeID server_node_id; + TransferID transfer_id; + + ServiceCallID() { } + + ServiceCallID(NodeID arg_server_node_id, TransferID arg_transfer_id) + : server_node_id(arg_server_node_id) + , transfer_id(arg_transfer_id) + { } + + bool operator==(const ServiceCallID rhs) const + { + return (rhs.server_node_id == server_node_id) && + (rhs.transfer_id == transfer_id); + } + + bool isValid() const { return server_node_id.isUnicast(); } +}; + +/** + * Object of this type will be returned to the application as a result of service call. + * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. + * The class is made noncopyable because it keeps a reference to a stack-allocated object. + */ +template +class UAVCAN_EXPORT ServiceCallResult : Noncopyable +{ +public: + typedef ReceivedDataStructure ResponseFieldType; + + enum Status { Success, ErrorTimeout }; + +private: + const Status status_; ///< Whether successful or not. Failure to decode the response causes timeout. + ServiceCallID call_id_; ///< Identifies the call + ResponseFieldType& response_; ///< Returned data structure. Value undefined if the service call has failed. + +public: + ServiceCallResult(Status arg_status, ServiceCallID arg_call_id, ResponseFieldType& arg_response) + : status_(arg_status) + , call_id_(arg_call_id) + , response_(arg_response) + { + UAVCAN_ASSERT(call_id_.isValid()); + UAVCAN_ASSERT((status_ == Success) || (status_ == ErrorTimeout)); + } + + /** + * Shortcut to quickly check whether the call was successful. + */ + bool isSuccessful() const { return status_ == Success; } + + Status getStatus() const { return status_; } + + ServiceCallID getCallID() const { return call_id_; } + + /** + * Returned reference points to a stack-allocated object. + */ + const ResponseFieldType& getResponse() const { return response_; } + ResponseFieldType& getResponse() { return response_; } +}; + +/** + * This operator neatly prints the service call result prepended with extra data like Server Node ID. + * The extra data will be represented as YAML comment. + */ +template +static Stream& operator<<(Stream& s, const ServiceCallResult& scr) +{ + s << "# Service call result [" << DataType::getDataTypeFullName() << "] " + << (scr.isSuccessful() ? "OK" : "FAILURE") + << " server_node_id=" << int(scr.getCallID().server_node_id.get()) + << " tid=" << int(scr.getCallID().transfer_id.get()) << "\n"; + if (scr.isSuccessful()) + { + s << scr.getResponse(); + } + else + { + s << "# (no data)"; + } + return s; +} + +/** + * Do not use directly. + */ +class ServiceClientBase : protected ITransferAcceptanceFilter + , protected DeadlineHandler +{ + const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call + +protected: + class CallState : DeadlineHandler + { + ServiceClientBase& owner_; + const ServiceCallID id_; + bool timed_out_; + + virtual void handleDeadline(MonotonicTime) override; + + public: + CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id) + : DeadlineHandler(node.getScheduler()) + , owner_(owner) + , id_(call_id) + , timed_out_(false) + { + UAVCAN_ASSERT(id_.isValid()); + DeadlineHandler::startWithDelay(owner_.request_timeout_); + } + + ServiceCallID getCallID() const { return id_; } + + bool hasTimedOut() const { return timed_out_; } + + static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); } + + bool operator==(const CallState& rhs) const + { + return (&owner_ == &rhs.owner_) && (id_ == rhs.id_); + } + }; + + struct CallStateMatchingPredicate + { + const ServiceCallID id; + CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } + bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); } + }; + + struct ServerSearchPredicate + { + const NodeID server_node_id; + ServerSearchPredicate(NodeID nid) : server_node_id(nid) { } + bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; } + }; + + MonotonicDuration request_timeout_; + + ServiceClientBase(INode& node) + : DeadlineHandler(node.getScheduler()) + , data_type_descriptor_(UAVCAN_NULLPTR) + , request_timeout_(getDefaultRequestTimeout()) + { } + + virtual ~ServiceClientBase() { } + + int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id); + +public: + /** + * It's not recommended to override default timeouts. + * Change of this value will not affect pending calls. + */ + static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); } + static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); } +}; + +/** + * Use this class to invoke services on remote nodes. + * + * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent + * calls is limited only by amount of available pool memory. + * + * Note that the reference passed to the callback points to a stack-allocated object, which means that the + * reference invalidates once the callback returns. If you want to use this object after the callback execution, + * you need to copy it somewhere. + * + * Note that by default, service client objects use lower priority than message publishers. Use @ref setPriority() + * to override the default if necessary. + * + * @tparam DataType_ Service data type. + * + * @tparam Callback_ Service response will be delivered through the callback of this type. + * In C++11 mode this type defaults to std::function<>. + * In C++03 mode this type defaults to a plain function pointer; use binder to + * call member functions as callbacks. + */ +template = UAVCAN_CPP11 + typename Callback_ = std::function&)> +#else + typename Callback_ = void (*)(const ServiceCallResult&) +#endif + > +class UAVCAN_EXPORT ServiceClient + : public GenericSubscriber + , public ServiceClientBase +{ +public: + typedef DataType_ DataType; + typedef typename DataType::Request RequestType; + typedef typename DataType::Response ResponseType; + typedef ServiceCallResult ServiceCallResultType; + typedef Callback_ Callback; + +private: + typedef ServiceClient SelfType; + typedef GenericPublisher PublisherType; + typedef GenericSubscriber SubscriberType; + + typedef Multiset CallRegistry; + + struct TimeoutCallbackCaller + { + ServiceClient& owner; + + TimeoutCallbackCaller(ServiceClient& arg_owner) : owner(arg_owner) { } + + void operator()(const CallState& state) + { + if (state.hasTimedOut()) + { + UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s", + int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()), + DataType::getDataTypeFullName()); + + typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized + + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), + rx_struct); // Mutable! + + owner.invokeCallback(result); + } + } + }; + + CallRegistry call_registry_; + + PublisherType publisher_; + Callback callback_; + + virtual bool shouldAcceptFrame(const RxFrame& frame) const override; // Called from the transfer listener + + void invokeCallback(ServiceCallResultType& result); + + virtual void handleReceivedDataStruct(ReceivedDataStructure& response) override; + + virtual void handleDeadline(MonotonicTime) override; + + int addCallState(ServiceCallID call_id); + +public: + /** + * @param node Node instance this client will be registered with. + * @param callback Callback instance. Optional, can be assigned later. + */ + explicit ServiceClient(INode& node, const Callback& callback = Callback()) + : SubscriberType(node) + , ServiceClientBase(node) + , call_registry_(node.getAllocator()) + , publisher_(node, getDefaultRequestTimeout()) + , callback_(callback) + { + setPriority(TransferPriority::MiddleLower); + setRequestTimeout(getDefaultRequestTimeout()); +#if UAVCAN_DEBUG + UAVCAN_ASSERT(getRequestTimeout() == getDefaultRequestTimeout()); // Making sure default values are OK +#endif + } + + virtual ~ServiceClient() { cancelAllCalls(); } + + /** + * Shall be called before first use. + * Returns negative error code. + */ + int init() + { + return publisher_.init(); + } + + /** + * Shall be called before first use. + * This overload allows to set the priority, otherwise it's the same. + * Returns negative error code. + */ + int init(TransferPriority priority) + { + return publisher_.init(priority); + } + + /** + * Performs non-blocking service call. + * This method transmits the service request and returns immediately. + * + * Service response will be delivered into the application via the callback. + * Note that the callback will ALWAYS be called even if the service call times out; the + * actual result of the call (success/failure) will be passed to the callback as well. + * + * Returns negative error code. + */ + int call(NodeID server_node_id, const RequestType& request); + + /** + * Same as plain @ref call() above, but this overload also returns the call ID of the new call. + * The call ID structure can be used to cancel this request later if needed. + */ + int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); + + /** + * Cancels certain call referred via call ID structure. + */ + void cancelCall(ServiceCallID call_id); + + /** + * Cancels all pending calls. + */ + void cancelAllCalls(); + + /** + * Checks whether there's currently a pending call addressed to the specified node ID. + */ + bool hasPendingCallToServer(NodeID server_node_id) const; + + /** + * This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned. + * Warning: average complexity is O(index); worst case complexity is O(size). + */ + ServiceCallID getCallIDByIndex(unsigned index) const; + + /** + * Service response callback must be set prior service call. + */ + const Callback& getCallback() const { return callback_; } + void setCallback(const Callback& cb) { callback_ = cb; } + + /** + * Complexity is O(N) of number of pending calls. + * Note that the number of pending calls will not be updated until the callback is executed. + */ + unsigned getNumPendingCalls() const { return call_registry_.getSize(); } + + /** + * Complexity is O(1). + * Note that the number of pending calls will not be updated until the callback is executed. + */ + bool hasPendingCalls() const { return !call_registry_.isEmpty(); } + + /** + * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: + * - Transient failure in the transport layer. + * - Incompatible data types. + */ + uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); } + + /** + * Request timeouts. Note that changing the request timeout will not affect calls that are already pending. + * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts. + * Not recommended to change. + */ + MonotonicDuration getRequestTimeout() const { return request_timeout_; } + void setRequestTimeout(MonotonicDuration timeout) + { + timeout = max(timeout, getMinRequestTimeout()); + timeout = min(timeout, getMaxRequestTimeout()); + + publisher_.setTxTimeout(timeout); + request_timeout_ = max(timeout, publisher_.getTxTimeout()); // No less than TX timeout + } + + /** + * Priority of outgoing request transfers. + * The remote server is supposed to use the same priority for the response, but it's not guaranteed by + * the specification. + */ + TransferPriority getPriority() const { return publisher_.getPriority(); } + void setPriority(const TransferPriority prio) { publisher_.setPriority(prio); } +}; + +// ---------------------------------------------------------------------------- + +template +void ServiceClient::invokeCallback(ServiceCallResultType& result) +{ + if (coerceOrFallback(callback_, true)) + { + callback_(result); + } + else + { + handleFatalError("Srv client clbk"); + } +} + +template +bool ServiceClient::shouldAcceptFrame(const RxFrame& frame) const +{ + UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher + + return UAVCAN_NULLPTR != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))); + +} + +template +void ServiceClient::handleReceivedDataStruct(ReceivedDataStructure& response) +{ + UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); + + ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); + cancelCall(call_id); + ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! + invokeCallback(result); +} + + +template +void ServiceClient::handleDeadline(MonotonicTime) +{ + UAVCAN_TRACE("ServiceClient", "Shared deadline event received"); + /* + * Invoking callbacks for timed out call state objects. + */ + TimeoutCallbackCaller callback_caller(*this); + call_registry_.template forEach(callback_caller); + /* + * Removing timed out objects. + * This operation cannot be merged with the previous one because that will not work with recursive calls. + */ + call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate); + /* + * Subscriber does not need to be registered if we don't have any pending calls. + * Removing it makes processing of incoming frames a bit faster. + */ + if (call_registry_.isEmpty()) + { + SubscriberType::stop(); + } +} + +template +int ServiceClient::addCallState(ServiceCallID call_id) +{ + if (call_registry_.isEmpty()) + { + const int subscriber_res = SubscriberType::startAsServiceResponseListener(); + if (subscriber_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res); + return subscriber_res; + } + } + + if (UAVCAN_NULLPTR == call_registry_.template emplace(SubscriberType::getNode(), *this, call_id)) + { + SubscriberType::stop(); + return -ErrMemory; + } + + return 0; +} + +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request) +{ + ServiceCallID dummy; + return call(server_node_id, request, dummy); +} + +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request, + ServiceCallID& out_call_id) +{ + if (!coerceOrFallback(callback_, true)) + { + UAVCAN_TRACE("ServiceClient", "Invalid callback"); + return -ErrInvalidConfiguration; + } + + /* + * Common procedures that don't depend on the struct data type + */ + const int prep_res = + prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id); + if (prep_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res); + return prep_res; + } + + /* + * Initializing the call state - this will start the subscriber ad-hoc + */ + const int call_state_res = addCallState(out_call_id); + if (call_state_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res); + return call_state_res; + } + + /* + * Configuring the listener so it will accept only the matching responses + * TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder) + */ + TransferListenerWithFilter* const tl = SubscriberType::getTransferListener(); + if (tl == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // Must have been created + cancelCall(out_call_id); + return -ErrLogic; + } + tl->installAcceptanceFilter(this); + + /* + * Publishing the request + */ + const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, + out_call_id.transfer_id); + if (publisher_res < 0) + { + cancelCall(out_call_id); + return publisher_res; + } + + UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id); + return publisher_res; +} + +template +void ServiceClient::cancelCall(ServiceCallID call_id) +{ + call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); + if (call_registry_.isEmpty()) + { + SubscriberType::stop(); + } +} + +template +void ServiceClient::cancelAllCalls() +{ + call_registry_.clear(); + SubscriberType::stop(); +} + +template +bool ServiceClient::hasPendingCallToServer(NodeID server_node_id) const +{ + return UAVCAN_NULLPTR != call_registry_.find(ServerSearchPredicate(server_node_id)); +} + +template +ServiceCallID ServiceClient::getCallIDByIndex(unsigned index) const +{ + const CallState* const id = call_registry_.getByIndex(index); + return (id == UAVCAN_NULLPTR) ? ServiceCallID() : id->getCallID(); +} + +} + +#endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_server.hpp new file mode 100644 index 0000000000..574aaabf19 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/service_server.hpp @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED +#define UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED + +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * This type can be used in place of the response type in a service server callback to get more advanced control + * of service request processing. + * + * PLEASE NOTE that since this class inherits the response type, service server callbacks can accept either + * object of this class or the response type directly if the extra options are not needed. + * + * For example, both of these callbacks can be used with the same service type 'Foo': + * + * void first(const ReceivedDataStructure& request, + * ServiceResponseDataStructure& response); + * + * void second(const Foo::Request& request, + * Foo::Response& response); + * + * In the latter case, an implicit cast will happen before the callback is invoked. + */ +template +class ServiceResponseDataStructure : public ResponseDataType_ +{ + // Fields are weirdly named to avoid name clashing with the inherited data type + bool _enabled_; + +public: + typedef ResponseDataType_ ResponseDataType; + + ServiceResponseDataStructure() + : _enabled_(true) + { } + + /** + * When disabled, the server will not transmit the response transfer. + * By default it is enabled, i.e. response will be sent. + */ + void setResponseEnabled(bool x) { _enabled_ = x; } + + /** + * Whether the response will be sent. By default it will. + */ + bool isResponseEnabled() const { return _enabled_; } +}; + +/** + * Use this class to implement UAVCAN service servers. + * + * Note that the references passed to the callback may point to stack-allocated objects, which means that the + * references get invalidated once the callback returns. + * + * @tparam DataType_ Service data type. + * + * @tparam Callback_ Service calls will be delivered through the callback of this type, and service + * response will be returned via the output parameter of the callback. Note that + * the reference to service response data struct passed to the callback always points + * to a default initialized response object. + * Please also refer to @ref ReceivedDataStructure<> and @ref ServiceResponseDataStructure<>. + * In C++11 mode this type defaults to std::function<>. + * In C++03 mode this type defaults to a plain function pointer; use binder to + * call member functions as callbacks. + */ +template = UAVCAN_CPP11 + typename Callback_ = std::function&, + ServiceResponseDataStructure&)> +#else + typename Callback_ = void (*)(const ReceivedDataStructure&, + ServiceResponseDataStructure&) +#endif + > +class UAVCAN_EXPORT ServiceServer + : public GenericSubscriber +{ +public: + typedef DataType_ DataType; + typedef typename DataType::Request RequestType; + typedef typename DataType::Response ResponseType; + typedef Callback_ Callback; + +private: + typedef GenericSubscriber SubscriberType; + typedef GenericPublisher PublisherType; + + PublisherType publisher_; + Callback callback_; + uint32_t response_failure_count_; + + virtual void handleReceivedDataStruct(ReceivedDataStructure& request) override + { + UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); + + ServiceResponseDataStructure response; + + if (coerceOrFallback(callback_, true)) + { + UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default + callback_(request, response); + } + else + { + handleFatalError("Srv serv clbk"); + } + + if (response.isResponseEnabled()) + { + publisher_.setPriority(request.getPriority()); // Responding at the same priority. + + const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), + request.getTransferID()); + if (res < 0) + { + UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res); + publisher_.getNode().getDispatcher().getTransferPerfCounter().addError(); + response_failure_count_++; + } + } + else + { + UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application"); + } + } + +public: + explicit ServiceServer(INode& node) + : SubscriberType(node) + , publisher_(node, getDefaultTxTimeout()) + , callback_() + , response_failure_count_(0) + { + UAVCAN_ASSERT(getTxTimeout() == getDefaultTxTimeout()); // Making sure it is valid + + StaticAssert::check(); + } + + /** + * Starts the server. + * Incoming service requests will be passed to the application via the callback. + */ + int start(const Callback& callback) + { + stop(); + + if (!coerceOrFallback(callback, true)) + { + UAVCAN_TRACE("ServiceServer", "Invalid callback"); + return -ErrInvalidParam; + } + callback_ = callback; + + const int publisher_res = publisher_.init(); + if (publisher_res < 0) + { + UAVCAN_TRACE("ServiceServer", "Publisher initialization failure: %i", publisher_res); + return publisher_res; + } + return SubscriberType::startAsServiceRequestListener(); + } + + /** + * Stops the server. + */ + using SubscriberType::stop; + + static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getMinTxTimeout() { return PublisherType::getMinTxTimeout(); } + static MonotonicDuration getMaxTxTimeout() { return PublisherType::getMaxTxTimeout(); } + + MonotonicDuration getTxTimeout() const { return publisher_.getTxTimeout(); } + void setTxTimeout(MonotonicDuration tx_timeout) { publisher_.setTxTimeout(tx_timeout); } + + /** + * Returns the number of failed attempts to decode data structs. Generally, a failed attempt means either: + * - Transient failure in the transport layer. + * - Incompatible data types. + */ + uint32_t getRequestFailureCount() const { return SubscriberType::getFailureCount(); } + uint32_t getResponseFailureCount() const { return response_failure_count_; } +}; + +} + +#endif // UAVCAN_NODE_SERVICE_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/sub_node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/sub_node.hpp new file mode 100644 index 0000000000..35b80492e5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/sub_node.hpp @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_SUB_NODE_NODE_HPP_INCLUDED +#define UAVCAN_SUB_NODE_NODE_HPP_INCLUDED + +#include +#include +#include + +#if UAVCAN_TINY +# error "This functionality is not available in tiny mode" +#endif + +namespace uavcan +{ +/** + * This node object can be used in multiprocess UAVCAN nodes. + * Please refer to the @ref Node<> for documentation concerning the template arguments; refer to the tutorials + * to lean how to use libuavcan in multiprocess applications. + */ +template +class UAVCAN_EXPORT SubNode : public INode +{ + typedef typename + Select<(MemPoolSize > 0), + PoolAllocator, // If pool size is specified, use default allocator + IPoolAllocator& // Otherwise use reference to user-provided allocator + >::Result Allocator; + + Allocator pool_allocator_; + Scheduler scheduler_; + + uint64_t internal_failure_cnt_; + +protected: + virtual void registerInternalFailure(const char* msg) + { + internal_failure_cnt_++; + UAVCAN_TRACE("Node", "Internal failure: %s", msg); + (void)msg; + } + +public: + /** + * This overload is only valid if MemPoolSize > 0. + */ + SubNode(ICanDriver& can_driver, + ISystemClock& system_clock) : + scheduler_(can_driver, pool_allocator_, system_clock), + internal_failure_cnt_(0) + { } + + /** + * This overload is only valid if MemPoolSize == 0. + */ + SubNode(ICanDriver& can_driver, + ISystemClock& system_clock, + IPoolAllocator& allocator) : + pool_allocator_(allocator), + scheduler_(can_driver, pool_allocator_, system_clock), + internal_failure_cnt_(0) + { } + + virtual typename RemoveReference::Type& getAllocator() { return pool_allocator_; } + + virtual Scheduler& getScheduler() { return scheduler_; } + virtual const Scheduler& getScheduler() const { return scheduler_; } + + uint64_t getInternalFailureCount() const { return internal_failure_cnt_; } +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/subscriber.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/subscriber.hpp new file mode 100644 index 0000000000..a28a9f037e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/subscriber.hpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED +#define UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED + +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * Use this class to subscribe to a message. + * + * @tparam DataType_ Message data type. + * + * @tparam Callback_ Type of the callback that will be used to deliver received messages + * into the application. Type of the argument of the callback can be either: + * - DataType_& + * - const DataType_& + * - ReceivedDataStructure& + * - const ReceivedDataStructure& + * For the first two options, @ref ReceivedDataStructure<> will be casted implicitly. + * In C++11 mode this type defaults to std::function<>. + * In C++03 mode this type defaults to a plain function pointer; use binder to + * call member functions as callbacks. + */ +template = UAVCAN_CPP11 + typename Callback_ = std::function&)> +#else + typename Callback_ = void (*)(const ReceivedDataStructure&) +#endif + > +class UAVCAN_EXPORT Subscriber + : public GenericSubscriber +{ +public: + typedef Callback_ Callback; + +private: + typedef GenericSubscriber BaseType; + + Callback callback_; + + virtual void handleReceivedDataStruct(ReceivedDataStructure& msg) override + { + if (coerceOrFallback(callback_, true)) + { + callback_(msg); + } + else + { + handleFatalError("Sub clbk"); + } + } + +public: + typedef DataType_ DataType; + + explicit Subscriber(INode& node) + : BaseType(node) + , callback_() + { + StaticAssert::check(); + } + + /** + * Begin receiving messages. + * Each message will be passed to the application via the callback. + * Returns negative error code. + */ + int start(const Callback& callback) + { + stop(); + + if (!coerceOrFallback(callback, true)) + { + UAVCAN_TRACE("Subscriber", "Invalid callback"); + return -ErrInvalidParam; + } + callback_ = callback; + + return BaseType::startAsMessageListener(); + } + + using BaseType::allowAnonymousTransfers; + using BaseType::stop; + using BaseType::getFailureCount; +}; + +} + +#endif // UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/timer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/timer.hpp new file mode 100644 index 0000000000..3973146ff3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/node/timer.hpp @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_NODE_TIMER_HPP_INCLUDED +#define UAVCAN_NODE_TIMER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ + +class UAVCAN_EXPORT TimerBase; + +/** + * Objects of this type will be supplied into timer callbacks. + */ +struct UAVCAN_EXPORT TimerEvent +{ + MonotonicTime scheduled_time; ///< Time when the timer callback was expected to be invoked + MonotonicTime real_time; ///< True time when the timer callback was invoked + + TimerEvent(MonotonicTime arg_scheduled_time, MonotonicTime arg_real_time) + : scheduled_time(arg_scheduled_time) + , real_time(arg_real_time) + { } +}; + +/** + * Inherit this class if you need a timer callback method in your class. + */ +class UAVCAN_EXPORT TimerBase : private DeadlineHandler +{ + MonotonicDuration period_; + + virtual void handleDeadline(MonotonicTime current) override; + +public: + using DeadlineHandler::stop; + using DeadlineHandler::isRunning; + using DeadlineHandler::getDeadline; + using DeadlineHandler::getScheduler; + + explicit TimerBase(INode& node) + : DeadlineHandler(node.getScheduler()) + , period_(MonotonicDuration::getInfinite()) + { } + + /** + * Various ways to start the timer - periodically or once. + * If it is running already, it will be restarted. + * If the deadline is in the past, the event will fire immediately. + * In periodic mode the timer does not accumulate error over time. + */ + void startOneShotWithDeadline(MonotonicTime deadline); + void startOneShotWithDelay(MonotonicDuration delay); + void startPeriodic(MonotonicDuration period); + + /** + * Returns period if the timer is in periodic mode. + * Returns infinite duration if the timer is in one-shot mode or stopped. + */ + MonotonicDuration getPeriod() const { return period_; } + + /** + * Implement this method in your class to receive callbacks. + */ + virtual void handleTimerEvent(const TimerEvent& event) = 0; +}; + +/** + * Wrapper over TimerBase that forwards callbacks into arbitrary handlers, like + * functor objects, member functions or static functions. + * + * Callback must be set before the first event; otherwise the event will generate a fatal error. + * + * Also take a look at @ref MethodBinder<>, which may come useful if C++11 features are not available. + * + * @tparam Callback_ Callback type. Shall accept const reference to TimerEvent as its argument. + */ +template +class UAVCAN_EXPORT TimerEventForwarder : public TimerBase +{ +public: + typedef Callback_ Callback; + +private: + Callback callback_; + + virtual void handleTimerEvent(const TimerEvent& event) + { + if (coerceOrFallback(callback_, true)) + { + callback_(event); + } + else + { + handleFatalError("Invalid timer callback"); + } + } + +public: + explicit TimerEventForwarder(INode& node) + : TimerBase(node) + , callback_() + { } + + TimerEventForwarder(INode& node, const Callback& callback) + : TimerBase(node) + , callback_(callback) + { } + + /** + * Get/set the callback object. + * Callback must be set before the first event happens; otherwise the event will generate a fatal error. + */ + const Callback& getCallback() const { return callback_; } + void setCallback(const Callback& callback) { callback_ = callback; } +}; + + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +/** + * Use this timer in C++11 mode. + * Callback type is std::function<>. + */ +typedef TimerEventForwarder > Timer; + +#endif + +} + +#endif // UAVCAN_NODE_TIMER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/README.md b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/README.md new file mode 100644 index 0000000000..1b29688f36 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/README.md @@ -0,0 +1,13 @@ +High-level protocol logic +========================= + +Classes defined in this directory implement some high-level functions of UAVCAN. + +Since most applications won't use all of them at the same time, their definitions are provided right in the header +files rather than in source files, in order to not pollute the build outputs with unused code (which is also a +requirement for most safety-critical software). + +However, some of the high-level functions that are either always used by the library itself or those that are extremely +likely to be used by the application are defined in .cpp files. + +When adding a new class here, please think twice before putting its definition into a .cpp file. diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp new file mode 100644 index 0000000000..3da0ca790b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements the standard services for data type introspection. + * Once started it does not require any attention from the application. + * The user does not need to deal with it directly - it's started by the node class. + */ +class UAVCAN_EXPORT DataTypeInfoProvider : Noncopyable +{ + typedef MethodBinder GetDataTypeInfoCallback; + + ServiceServer gdti_srv_; + + INode& getNode() { return gdti_srv_.getNode(); } + + static bool isValidDataTypeKind(DataTypeKind kind) + { + return (kind == DataTypeKindMessage) || (kind == DataTypeKindService); + } + + void handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request, + protocol::GetDataTypeInfo::Response& response) + { + /* + * Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID + */ + const DataTypeDescriptor* desc = UAVCAN_NULLPTR; + + if (request.name.empty()) + { + response.id = request.id; // Pre-setting the fields so they have meaningful values even in + response.kind = request.kind; // ...case of failure. + + if (!isValidDataTypeKind(DataTypeKind(request.kind.value))) + { + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i", + static_cast(request.kind.value)); + return; + } + + desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id); + } + else + { + response.name = request.name; + + desc = GlobalDataTypeRegistry::instance().find(request.name.c_str()); + } + + if (desc == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("DataTypeInfoProvider", + "Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'", + static_cast(request.id), static_cast(request.kind.value), request.name.c_str()); + return; + } + + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str()); + + /* + * Filling the response struct + */ + response.signature = desc->getSignature().get(); + response.id = desc->getID().get(); + response.kind.value = desc->getKind(); + response.flags = protocol::GetDataTypeInfo::Response::FLAG_KNOWN; + response.name = desc->getFullName(); + + const Dispatcher& dispatcher = getNode().getDispatcher(); + + if (desc->getKind() == DataTypeKindService) + { + if (dispatcher.hasServer(desc->getID().get())) + { + response.flags |= protocol::GetDataTypeInfo::Response::FLAG_SERVING; + } + } + else if (desc->getKind() == DataTypeKindMessage) + { + if (dispatcher.hasSubscriber(desc->getID().get())) + { + response.flags |= protocol::GetDataTypeInfo::Response::FLAG_SUBSCRIBED; + } + if (dispatcher.hasPublisher(desc->getID().get())) + { + response.flags |= protocol::GetDataTypeInfo::Response::FLAG_PUBLISHING; + } + } + else + { + UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror. + } + } + +public: + explicit DataTypeInfoProvider(INode& node) : + gdti_srv_(node) + { } + + int start() + { + int res = 0; + + res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); + if (res < 0) + { + goto fail; + } + + UAVCAN_ASSERT(res >= 0); + return res; + + fail: + UAVCAN_ASSERT(res < 0); + gdti_srv_.stop(); + return res; + } +}; + +} + +#endif // UAVCAN_PROTOCOL_DATA_TYPE_INFO_PROVIDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp new file mode 100644 index 0000000000..31e2c16a3d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements client-side logic of dynamic node ID allocation procedure. + * + * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined + * by the specification, until a Node ID is granted by the allocator. + * + * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests + * and listening for responses. + * + * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. + * + * Note that this class uses std::rand(), which must be correctly seeded before use. + */ +class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase +{ + typedef MethodBinder&)> + AllocationCallback; + + enum Mode + { + ModeWaitingForTimeSlot, + ModeDelayBeforeFollowup, + NumModes + }; + + Publisher dnida_pub_; + Subscriber dnida_sub_; + + uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize]; + uint8_t size_of_received_unique_id_; + + NodeID preferred_node_id_; + NodeID allocated_node_id_; + NodeID allocator_node_id_; + + void terminate(); + + static MonotonicDuration getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec); + + void restartTimer(const Mode mode); + + virtual void handleTimerEvent(const TimerEvent&) override; + + void handleAllocation(const ReceivedDataStructure& msg); + +public: + typedef protocol::HardwareVersion::FieldTypes::unique_id UniqueID; + + DynamicNodeIDClient(INode& node) + : TimerBase(node) + , dnida_pub_(node) + , dnida_sub_(node) + , size_of_received_unique_id_(0) + { } + + /** + * @param unique_id Unique ID of the local node. Must be the same as in the hardware version struct. + * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if + * the application doesn't have any preference (this is default). + * @param transfer_priority Transfer priority, Normal by default. + * @return Zero on success + * Negative error code on failure + * -ErrLogic if 1. the node is not in passive mode or 2. the client is already started + */ + int start(const UniqueID& unique_id, + const NodeID preferred_node_id = NodeID::Broadcast, + const TransferPriority transfer_priority = TransferPriority::OneHigherThanLowest); + + /** + * Use this method to determine when allocation is complete. + */ + bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); } + + /** + * This method allows to retrieve the node ID that was allocated to the local node. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, a non-unicast node ID will be returned. + */ + NodeID getAllocatedNodeID() const { return allocated_node_id_; } + + /** + * This method allows to retrieve node ID of the allocator that granted our Node ID. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, an non-unicast node ID will be returned. + */ + NodeID getAllocatorNodeID() const { return allocator_node_id_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp new file mode 100644 index 0000000000..cf61e8771a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/abstract_server.hpp @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +class AbstractServer : protected IAllocationRequestHandler + , protected INodeDiscoveryHandler +{ + UniqueID own_unique_id_; + MonotonicTime started_at_; + +protected: + INode& node_; + IEventTracer& tracer_; + AllocationRequestManager allocation_request_manager_; + NodeDiscoverer node_discoverer_; + + AbstractServer(INode& node, + IEventTracer& tracer) : + node_(node), + tracer_(tracer), + allocation_request_manager_(node, tracer, *this), + node_discoverer_(node, tracer, *this) + { } + + const UniqueID& getOwnUniqueID() const { return own_unique_id_; } + + int init(const UniqueID& own_unique_id, const TransferPriority priority) + { + int res = 0; + + own_unique_id_ = own_unique_id; + + res = allocation_request_manager_.init(priority); + if (res < 0) + { + return res; + } + + res = node_discoverer_.init(priority); + if (res < 0) + { + return res; + } + + started_at_ = node_.getMonotonicTime(); + + return 0; + } + +public: + /** + * This can be used to guess if there are any un-allocated dynamic nodes left in the network. + */ + bool guessIfAllDynamicNodesAreAllocated( + const MonotonicDuration& allocation_activity_timeout = + MonotonicDuration::fromMSec(Allocation::MAX_REQUEST_PERIOD_MS * 2), + const MonotonicDuration& min_uptime = MonotonicDuration::fromMSec(6000)) const + { + const MonotonicTime ts = node_.getMonotonicTime(); + + /* + * If uptime is not large enough, the allocator may be unaware about some nodes yet. + */ + const MonotonicDuration uptime = ts - started_at_; + if (uptime < max(allocation_activity_timeout, min_uptime)) + { + return false; + } + + /* + * If there are any undiscovered nodes, assume that allocation is still happening. + */ + if (node_discoverer_.hasUnknownNodes()) + { + return false; + } + + /* + * Lastly, check if there wasn't any allocation messages detected on the bus in the specified amount of time. + */ + const MonotonicDuration since_allocation_activity = + ts - allocation_request_manager_.getTimeOfLastAllocationActivity(); + if (since_allocation_activity < allocation_activity_timeout) + { + return false; + } + + return true; + } + + /** + * This is useful for debugging/testing/monitoring. + */ + const NodeDiscoverer& getNodeDiscoverer() const { return node_discoverer_; } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp new file mode 100644 index 0000000000..a16346b9a0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * The main allocator must implement this interface. + */ +class IAllocationRequestHandler +{ +public: + /** + * Allocation request manager uses this method to detect if it is allowed to publish follow-up responses. + */ + virtual bool canPublishFollowupAllocationResponse() const = 0; + + /** + * This method will be invoked when a new allocation request is received. + */ + virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; + + virtual ~IAllocationRequestHandler() { } +}; + +/** + * This class manages communication with allocation clients. + * Three-stage unique ID exchange is implemented here, as well as response publication. + */ +class AllocationRequestManager +{ + typedef MethodBinder&)> + AllocationCallback; + + const MonotonicDuration stage_timeout_; + + MonotonicTime last_message_timestamp_; + MonotonicTime last_activity_timestamp_; + Allocation::FieldTypes::unique_id current_unique_id_; + + IAllocationRequestHandler& handler_; + IEventTracer& tracer_; + + Subscriber allocation_sub_; + Publisher allocation_pub_; + + enum { InvalidStage = 0 }; + + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + + static uint8_t detectRequestStage(const Allocation& msg) + { + const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + + if ((msg.unique_id.size() != max_bytes_per_request) && + (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) && + (msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD + { + return InvalidStage; + } + if (msg.first_part_of_unique_id) + { + return 1; // Note that CAN FD frames can deliver the unique ID in one stage! + } + if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 3; + } + return InvalidStage; + } + + uint8_t getExpectedStage() const + { + if (current_unique_id_.empty()) + { + return 1; + } + if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) + { + return 3; + } + if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + return InvalidStage; + } + + void publishFollowupAllocationResponse() + { + Allocation msg; + msg.unique_id = current_unique_id_; + UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); + + UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", + unsigned(msg.unique_id.size())); + + trace(TraceAllocationFollowupResponse, msg.unique_id.size()); + + const int res = allocation_pub_.broadcast(msg); + if (res < 0) + { + trace(TraceError, res); + allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast"); + } + } + + void handleAllocation(const ReceivedDataStructure& msg) + { + trace(TraceAllocationActivity, msg.getSrcNodeID().get()); + last_activity_timestamp_ = msg.getMonotonicTimestamp(); + + if (!msg.isAnonymousTransfer()) + { + return; // This is a response from another allocator, ignore + } + + /* + * Reset the expected stage on timeout + */ + if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_)) + { + UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); + current_unique_id_.clear(); + trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec()); + } + + /* + * Checking if request stage matches the expected stage + */ + const uint8_t request_stage = detectRequestStage(msg); + if (request_stage == InvalidStage) + { + trace(TraceAllocationBadRequest, msg.unique_id.size()); + return; // Malformed request - ignore without resetting + } + + const uint8_t expected_stage = getExpectedStage(); + if (expected_stage == InvalidStage) + { + UAVCAN_ASSERT(0); + return; + } + + if (request_stage != expected_stage) + { + trace(TraceAllocationUnexpectedStage, request_stage); + return; // Ignore - stage mismatch + } + + const uint8_t max_expected_bytes = + static_cast(current_unique_id_.capacity() - current_unique_id_.size()); + UAVCAN_ASSERT(max_expected_bytes > 0); + if (msg.unique_id.size() > max_expected_bytes) + { + trace(TraceAllocationBadRequest, msg.unique_id.size()); + return; // Malformed request + } + + /* + * Updating the local state + */ + for (uint8_t i = 0; i < msg.unique_id.size(); i++) + { + current_unique_id_.push_back(msg.unique_id[i]); + } + + trace(TraceAllocationRequestAccepted, current_unique_id_.size()); + + /* + * Proceeding with allocation if possible + * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader. + */ + if (current_unique_id_.size() == current_unique_id_.capacity()) + { + UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d", + int(msg.node_id)); + + UniqueID unique_id; + copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); + current_unique_id_.clear(); + + { + uint64_t event_agrument = 0; + for (uint8_t i = 0; i < 8; i++) + { + event_agrument |= static_cast(unique_id[i]) << (56U - i * 8U); + } + trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); + } + + handler_.handleAllocationRequest(unique_id, msg.node_id); + } + else + { + if (handler_.canPublishFollowupAllocationResponse()) + { + publishFollowupAllocationResponse(); + } + else + { + trace(TraceAllocationFollowupDenied, 0); + current_unique_id_.clear(); + } + } + + /* + * It is super important to update timestamp only if the request has been processed successfully. + */ + last_message_timestamp_ = msg.getMonotonicTimestamp(); + } + +public: + AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler) + : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)) + , handler_(handler) + , tracer_(tracer) + , allocation_sub_(node) + , allocation_pub_(node) + { } + + int init(const TransferPriority priority) + { + int res = allocation_pub_.init(priority); + if (res < 0) + { + return res; + } + allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)); + + res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation)); + if (res < 0) + { + return res; + } + allocation_sub_.allowAnonymousTransfers(); + + return 0; + } + + int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id) + { + Allocation msg; + + msg.unique_id.resize(msg.unique_id.capacity()); + copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); + + msg.node_id = allocated_node_id.get(); + + trace(TraceAllocationResponse, msg.node_id); + last_activity_timestamp_ = allocation_pub_.getNode().getMonotonicTime(); + + return allocation_pub_.broadcast(msg); + } + + /** + * When the last allocation activity was registered. + * This value can be used to heuristically determine whether there are any unallocated nodes left in the network. + */ + MonotonicTime getTimeOfLastAllocationActivity() const { return last_activity_timestamp_; } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp new file mode 100644 index 0000000000..e0f9e5bcae --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED + +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef centralized::Server CentralizedServer; + +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp new file mode 100644 index 0000000000..fea436bfb7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace centralized +{ +/** + * This server is an alternative to @ref DistributedServer with the following differences: + * - It is not distributed, so using it means introducing a single point of failure into the system. + * - It takes less code space and requires less RAM, which makes it suitable for resource-constrained applications. + * + * This version is suitable only for simple non-critical systems. + */ +class Server : public AbstractServer +{ + Storage storage_; + + /* + * Private methods + */ + bool isNodeIDTaken(const NodeID node_id) const + { + return storage_.isNodeIDOccupied(node_id); + } + + void tryPublishAllocationResult(const NodeID node_id, const UniqueID& unique_id) + { + const int res = allocation_request_manager_.broadcastAllocationResponse(unique_id, node_id); + if (res < 0) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("Dynamic allocation response"); + } + } + + /* + * Methods of IAllocationRequestHandler + */ + virtual bool canPublishFollowupAllocationResponse() const override + { + return true; // Because there's only one Centralized server in the system + } + + virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) override + { + const NodeID existing_node_id = storage_.getNodeIDForUniqueID(unique_id); + if (existing_node_id.isValid()) + { + tryPublishAllocationResult(existing_node_id, unique_id); + } + else + { + const NodeID allocated_node_id = + NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); + + if (allocated_node_id.isUnicast()) + { + const int res = storage_.add(allocated_node_id, unique_id); + if (res >= 0) + { + tryPublishAllocationResult(allocated_node_id, unique_id); + } + else + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("CentralizedServer storage add"); + } + } + else + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left"); + } + } + } + + /* + * Methods of INodeDiscoveryHandler + */ + virtual bool canDiscoverNewNodes() const override + { + return true; // Because there's only one Centralized server in the system + } + + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const override + { + return storage_.isNodeIDOccupied(node_id) ? NodeAwarenessKnownAndCommitted : NodeAwarenessUnknown; + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) override + { + if (storage_.isNodeIDOccupied(node_id)) + { + UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that + return; + } + + const int res = storage_.add(node_id, (unique_id_or_null == UAVCAN_NULLPTR) ? UniqueID() : *unique_id_or_null); + if (res < 0) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("CentralizedServer storage add"); + } + } + +public: + Server(INode& node, + IStorageBackend& storage, + IEventTracer& tracer) + : AbstractServer(node, tracer) + , storage_(storage) + { } + + int init(const UniqueID& own_unique_id, + const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + /* + * Initializing storage first, because the next step requires it to be loaded + */ + int res = storage_.init(); + if (res < 0) + { + return res; + } + + /* + * Common logic + */ + res = AbstractServer::init(own_unique_id, priority); + if (res < 0) + { + return res; + } + + /* + * Making sure that the server is started with the same node ID + */ + { + const NodeID stored_own_node_id = storage_.getNodeIDForUniqueID(getOwnUniqueID()); + if (stored_own_node_id.isValid()) + { + if (stored_own_node_id != node_.getNodeID()) + { + return -ErrInvalidConfiguration; + } + } + else + { + res = storage_.add(node_.getNodeID(), getOwnUniqueID()); + if (res < 0) + { + return res; + } + } + } + + return 0; + } + + uint8_t getNumAllocations() const { return storage_.getSize(); } +}; + +} +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp new file mode 100644 index 0000000000..51123a8963 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace centralized +{ +/** + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ +class Storage +{ + typedef BitSet OccupationMask; + typedef Array, ArrayModeStatic, + BitLenToByteLen::Result> + OccupationMaskArray; + + IStorageBackend& storage_; + OccupationMask occupation_mask_; + + static IStorageBackend::String getOccupationMaskKey() { return "occupation_mask"; } + + static OccupationMask maskFromArray(const OccupationMaskArray& array) + { + OccupationMask mask; + for (uint8_t byte = 0; byte < array.size(); byte++) + { + for (uint8_t bit = 0; bit < 8; bit++) + { + mask[byte * 8U + bit] = (array[byte] & (1U << bit)) != 0; + } + } + return mask; + } + + static OccupationMaskArray maskToArray(const OccupationMask& mask) + { + OccupationMaskArray array; + for (uint8_t byte = 0; byte < array.size(); byte++) + { + for (uint8_t bit = 0; bit < 8; bit++) + { + if (mask[byte * 8U + bit]) + { + array[byte] = static_cast(array[byte] | (1U << bit)); + } + } + } + return array; + } + +public: + Storage(IStorageBackend& storage) : + storage_(storage) + { } + + /** + * This method reads only the occupation mask from the storage. + */ + int init() + { + StorageMarshaller io(storage_); + OccupationMaskArray array; + io.get(getOccupationMaskKey(), array); + occupation_mask_ = maskFromArray(array); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the entry was successfully appended. + */ + int add(const NodeID node_id, const UniqueID& unique_id) + { + if (!node_id.isUnicast()) + { + return -ErrInvalidParam; + } + + StorageMarshaller io(storage_); + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + { + uint32_t node_id_int = node_id.get(); + int res = io.setAndGetBack(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id_int); + if (res < 0) + { + return res; + } + if (node_id_int != node_id.get()) + { + return -ErrFailure; + } + } + + // Updating the mask in the storage + OccupationMask new_occupation_mask = occupation_mask_; + new_occupation_mask[node_id.get()] = true; + OccupationMaskArray occupation_array = maskToArray(new_occupation_mask); + + int res = io.setAndGetBack(getOccupationMaskKey(), occupation_array); + if (res < 0) + { + return res; + } + if (occupation_array != maskToArray(new_occupation_mask)) + { + return -ErrFailure; + } + + // Updating the cached mask only if the storage was updated successfully + occupation_mask_ = new_occupation_mask; + + return 0; + } + + /** + * Returns an invalid node ID if there's no such allocation. + */ + NodeID getNodeIDForUniqueID(const UniqueID& unique_id) const + { + StorageMarshaller io(storage_); + uint32_t node_id = 0; + io.get(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id); + return (node_id > 0 && node_id <= NodeID::Max) ? NodeID(static_cast(node_id)) : NodeID(); + } + + bool isNodeIDOccupied(NodeID node_id) const { return occupation_mask_[node_id.get()]; } + + uint8_t getSize() const { return static_cast(occupation_mask_.count()); } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp new file mode 100644 index 0000000000..822f757522 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED + +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef distributed::Server DistributedServer; + +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp new file mode 100644 index 0000000000..b4e573b29d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -0,0 +1,433 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class maintains the cluster state. + */ +class ClusterManager : private TimerBase +{ +public: + enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize }; + +private: + typedef MethodBinder&)> + DiscoveryCallback; + + struct Server + { + NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + Server() + : next_index(0) + , match_index(0) + { } + + void resetIndices(const Log& log) + { + next_index = Log::Index(log.getLastIndex() + 1U); + match_index = 0; + } + }; + + IStorageBackend& storage_; + IEventTracer& tracer_; + const Log& log_; + + Subscriber discovery_sub_; + mutable Publisher discovery_pub_; + + Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there. + + uint8_t cluster_size_; + uint8_t num_known_servers_; + + static IStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; } + + INode& getNode() { return discovery_sub_.getNode(); } + const INode& getNode() const { return discovery_sub_.getNode(); } + + const Server* findServer(NodeID node_id) const { return const_cast(this)->findServer(node_id); } + Server* findServer(NodeID node_id) + { + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + if (servers_[i].node_id == node_id) + { + return &servers_[i]; + } + } + return UAVCAN_NULLPTR; + } + + virtual void handleTimerEvent(const TimerEvent&) + { + UAVCAN_ASSERT(num_known_servers_ < cluster_size_); + + tracer_.onEvent(TraceRaftDiscoveryBroadcast, num_known_servers_); + + /* + * Filling the message + */ + Discovery msg; + msg.configured_cluster_size = cluster_size_; + + msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0 + + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + msg.known_nodes.push_back(servers_[i].node_id.get()); + } + + UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1)); + + /* + * Broadcasting + */ + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Broadcasting Discovery message; known nodes: %d of %d", + int(msg.known_nodes.size()), int(cluster_size_)); + + const int res = discovery_pub_.broadcast(msg); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Discovery broadcst failed: %d", res); + getNode().registerInternalFailure("Raft discovery broadcast"); + } + + /* + * Termination condition + */ + if (isClusterDiscovered()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Discovery broadcasting timer stopped"); + stop(); + } + } + + void handleDiscovery(const ReceivedDataStructure& msg) + { + tracer_.onEvent(TraceRaftDiscoveryReceived, msg.getSrcNodeID().get()); + + /* + * Validating cluster configuration + * If there's a case of misconfiguration, the message will be ignored. + */ + if (msg.configured_cluster_size != cluster_size_) + { + tracer_.onEvent(TraceRaftBadClusterSizeReceived, msg.configured_cluster_size); + getNode().registerInternalFailure("Bad Raft cluster size"); + return; + } + + /* + * Updating the set of known servers + */ + for (uint8_t i = 0; i < msg.known_nodes.size(); i++) + { + if (isClusterDiscovered()) + { + break; + } + + const NodeID node_id(msg.known_nodes[i]); + if (node_id.isUnicast() && !isKnownServer(node_id)) + { + addServer(node_id); + } + } + + /* + * Publishing a new Discovery request if the publishing server needs to learn about more servers. + */ + if (msg.configured_cluster_size > msg.known_nodes.size()) + { + startDiscoveryPublishingTimerIfNotRunning(); + } + } + + void startDiscoveryPublishingTimerIfNotRunning() + { + if (!isRunning()) + { + startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_PERIOD_MS)); + } + } + +public: + enum { ClusterSizeUnknown = 0 }; + + /** + * @param node Needed to publish and subscribe to Discovery message + * @param storage Needed to read the cluster size parameter from the storage + * @param log Needed to initialize nextIndex[] values after elections + */ + ClusterManager(INode& node, IStorageBackend& storage, const Log& log, IEventTracer& tracer) + : TimerBase(node) + , storage_(storage) + , tracer_(tracer) + , log_(log) + , discovery_sub_(node) + , discovery_pub_(node) + , cluster_size_(0) + , num_known_servers_(0) + { } + + /** + * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the + * storage backend using key 'cluster_size'. + * Returns negative error code. + */ + int init(const uint8_t init_cluster_size, const TransferPriority priority) + { + /* + * Figuring out the cluster size + */ + if (init_cluster_size == ClusterSizeUnknown) + { + // Reading from the storage + StorageMarshaller io(storage_); + uint32_t value = 0; + int res = io.get(getStorageKeyForClusterSize(), value); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Cluster size is neither configured nor stored in the storage"); + return res; + } + if ((value == 0) || (value > MaxClusterSize)) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is invalid"); + return -ErrInvalidConfiguration; + } + cluster_size_ = static_cast(value); + } + else + { + if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize)) + { + return -ErrInvalidParam; + } + cluster_size_ = init_cluster_size; + + // Writing the storage + StorageMarshaller io(storage_); + uint32_t value = init_cluster_size; + int res = io.setAndGetBack(getStorageKeyForClusterSize(), value); + if ((res < 0) || (value != init_cluster_size)) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Failed to store cluster size"); + return -ErrFailure; + } + } + + tracer_.onEvent(TraceRaftClusterSizeInited, cluster_size_); + + UAVCAN_ASSERT(cluster_size_ > 0); + UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize); + + /* + * Initializing pub/sub and timer + */ + int res = discovery_pub_.init(priority); + if (res < 0) + { + return res; + } + + res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery)); + if (res < 0) + { + return res; + } + + startDiscoveryPublishingTimerIfNotRunning(); + + /* + * Misc + */ + resetAllServerIndices(); + return 0; + } + + /** + * Adds once server regardless of the discovery logic. + */ + void addServer(NodeID node_id) + { + UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize); + if (!isKnownServer(node_id) && node_id.isUnicast()) + { + tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get()); + servers_[num_known_servers_].node_id = node_id; + servers_[num_known_servers_].resetIndices(log_); + num_known_servers_ = static_cast(num_known_servers_ + 1U); + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * Whether such server has been discovered. + */ + bool isKnownServer(NodeID node_id) const + { + if (node_id == getNode().getNodeID()) + { + return true; + } + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); + if (servers_[i].node_id == node_id) + { + return true; + } + } + return false; + } + + /** + * An invalid node ID will be returned if there's no such server. + * The local server is not listed there. + */ + NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const + { + if (index < num_known_servers_) + { + return servers_[index].node_id; + } + return NodeID(); + } + + /** + * See next_index[] in Raft paper. + */ + Log::Index getServerNextIndex(NodeID server_node_id) const + { + const Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + return s->next_index; + } + UAVCAN_ASSERT(0); + return 0; + } + + void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment) + { + Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + s->next_index = Log::Index(s->next_index + increment); + } + else + { + UAVCAN_ASSERT(0); + } + } + + void decrementServerNextIndex(NodeID server_node_id) + { + Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + s->next_index--; + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * See match_index[] in Raft paper. + */ + Log::Index getServerMatchIndex(NodeID server_node_id) const + { + const Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + return s->match_index; + } + UAVCAN_ASSERT(0); + return 0; + } + + void setServerMatchIndex(NodeID server_node_id, Log::Index match_index) + { + Server* const s = findServer(server_node_id); + if (s != UAVCAN_NULLPTR) + { + s->match_index = match_index; + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This method must be called when the current server becomes leader. + */ + void resetAllServerIndices() + { + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + servers_[i].resetIndices(log_); + } + } + + /** + * Number of known servers can only grow, and it never exceeds the cluster size value. + * This number does not include the local server. + */ + uint8_t getNumKnownServers() const { return num_known_servers_; } + + /** + * Cluster size and quorum size are constant. + */ + uint8_t getClusterSize() const { return cluster_size_; } + uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } + + bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp new file mode 100644 index 0000000000..3ef89b31ee --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -0,0 +1,310 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * Raft log. + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ +class Log +{ +public: + typedef uint8_t Index; + + enum { Capacity = NodeID::Max + 1 }; + +private: + IStorageBackend& storage_; + IEventTracer& tracer_; + Entry entries_[Capacity]; + Index last_index_; // Index zero always contains an empty entry + + static IStorageBackend::String getLastIndexKey() { return "log_last_index"; } + + static IStorageBackend::String makeEntryKey(Index index, const char* postfix) + { + IStorageBackend::String str; + // "log0_foobar" + str += "log"; + str.appendFormatted("%d", int(index)); + str += "_"; + str += postfix; + return str; + } + + int readEntryFromStorage(Index index, Entry& out_entry) + { + const StorageMarshaller io(storage_); + + // Term + if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0) + { + return -ErrFailure; + } + + // Unique ID + if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = 0; + if (io.get(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + if (node_id > NodeID::Max) + { + return -ErrFailure; + } + out_entry.node_id = static_cast(node_id); + + return 0; + } + + int writeEntryToStorage(Index index, const Entry& entry) + { + Entry temp = entry; + + StorageMarshaller io(storage_); + + // Term + if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0) + { + return -ErrFailure; + } + + // Unique ID + if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = entry.node_id; + if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + temp.node_id = static_cast(node_id); + + return (temp == entry) ? 0 : -ErrFailure; + } + + int initEmptyLogStorage() + { + StorageMarshaller io(storage_); + + /* + * Writing the zero entry - it must always be default-initialized + */ + entries_[0] = Entry(); + int res = writeEntryToStorage(0, entries_[0]); + if (res < 0) + { + return res; + } + + /* + * Initializing last index + * Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be + * left in an inconsistent state. + */ + last_index_ = 0; + uint32_t stored_index = 0; + res = io.setAndGetBack(getLastIndexKey(), stored_index); + if (res < 0) + { + return res; + } + if (stored_index != 0) + { + return -ErrFailure; + } + + return 0; + } + +public: + Log(IStorageBackend& storage, IEventTracer& tracer) + : storage_(storage) + , tracer_(tracer) + , last_index_(0) + { } + + int init() + { + StorageMarshaller io(storage_); + + // Reading max index + { + uint32_t value = 0; + if (io.get(getLastIndexKey(), value) < 0) + { + if (storage_.get(getLastIndexKey()).empty()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Initializing empty storage"); + return initEmptyLogStorage(); + } + else + { + // There's some data in the storage, but it cannot be parsed - reporting an error + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read last index"); + return -ErrFailure; + } + } + if (value >= Capacity) + { + return -ErrFailure; + } + last_index_ = Index(value); + } + + tracer_.onEvent(TraceRaftLogLastIndexRestored, last_index_); + + // Restoring log entries - note that index 0 always exists + for (Index index = 0; index <= last_index_; index++) + { + const int result = readEntryFromStorage(index, entries_[index]); + if (result < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read entry at index %u: %d", + unsigned(index), result); + return result; + } + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Restored %u log entries", unsigned(last_index_)); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the entry was successfully appended. + */ + int append(const Entry& entry) + { + if ((last_index_ + 1) >= Capacity) + { + return -ErrLogic; + } + + tracer_.onEvent(TraceRaftLogAppend, last_index_ + 1U); + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + int res = writeEntryToStorage(Index(last_index_ + 1), entry); + if (res < 0) + { + return res; + } + + // Updating the last index + StorageMarshaller io(storage_); + uint32_t new_last_index = last_index_ + 1U; + res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != last_index_ + 1U) + { + return -ErrFailure; + } + entries_[new_last_index] = entry; + last_index_ = Index(new_last_index); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "New entry, index %u, node ID %u, term %u", + unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term)); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the requested operation has been carried out successfully. + */ + int removeEntriesWhereIndexGreaterOrEqual(Index index) + { + UAVCAN_ASSERT(last_index_ < Capacity); + + if (((index) >= Capacity) || (index <= 0)) + { + return -ErrLogic; + } + + uint32_t new_last_index = index - 1U; + + tracer_.onEvent(TraceRaftLogRemove, new_last_index); + + if (new_last_index != last_index_) + { + StorageMarshaller io(storage_); + int res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != index - 1U) + { + return -ErrFailure; + } + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Entries removed, last index %u --> %u", + unsigned(last_index_), unsigned(new_last_index)); + last_index_ = Index(new_last_index); + } + + // Removal operation leaves dangling entries in storage, it's OK + return 0; + } + + int removeEntriesWhereIndexGreater(Index index) + { + return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U)); + } + + /** + * Returns nullptr if there's no such index. + * This method does not use storage IO. + */ + const Entry* getEntryAtIndex(Index index) const + { + UAVCAN_ASSERT(last_index_ < Capacity); + return (index <= last_index_) ? &entries_[index] : UAVCAN_NULLPTR; + } + + Index getLastIndex() const { return last_index_; } + + bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const + { + UAVCAN_ASSERT(last_index_ < Capacity); + // Terms are different - the one with higher term is more up-to-date + if (other_last_term != entries_[last_index_].term) + { + return other_last_term > entries_[last_index_].term; + } + // Terms are equal - longer log wins + return other_last_index >= last_index_; + } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp new file mode 100644 index 0000000000..a1ca44ea79 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -0,0 +1,237 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class is a convenient container for persistent state variables defined by Raft. + * Writes are slow, reads are instantaneous. + */ +class PersistentState +{ + IStorageBackend& storage_; + IEventTracer& tracer_; + + Term current_term_; + NodeID voted_for_; + Log log_; + + static IStorageBackend::String getCurrentTermKey() { return "current_term"; } + static IStorageBackend::String getVotedForKey() { return "voted_for"; } + +public: + PersistentState(IStorageBackend& storage, IEventTracer& tracer) + : storage_(storage) + , tracer_(tracer) + , current_term_(0) + , log_(storage, tracer) + { } + + /** + * Initialization is performed as follows (every step may fail and return an error): + * 1. Log is restored or initialized. + * 2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized + * with zero. + * 3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is + * zero, the value will be initialized with zero. + */ + int init() + { + /* + * Reading log + */ + int res = log_.init(); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res); + return res; + } + + const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); + if (last_entry == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0); + + StorageMarshaller io(storage_); + + /* + * Reading currentTerm + */ + if (storage_.get(getCurrentTermKey()).empty() && log_is_empty) + { + // First initialization + current_term_ = 0; + res = io.setAndGetBack(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to init current term: %d", res); + return res; + } + if (current_term_ != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + res = io.get(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to read current term: %d", res); + return res; + } + } + + tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_); + + if (current_term_ < last_entry->term) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)", + unsigned(current_term_), unsigned(last_entry->term)); + return -ErrLogic; + } + + /* + * Reading votedFor + */ + if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0)) + { + // First initialization + voted_for_ = NodeID(0); + uint32_t stored_voted_for = 0; + res = io.setAndGetBack(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to init votedFor: %d", res); + return res; + } + if (stored_voted_for != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + uint32_t stored_voted_for = 0; + res = io.get(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to read votedFor: %d", res); + return res; + } + if (stored_voted_for > NodeID::Max) + { + return -ErrInvalidConfiguration; + } + voted_for_ = NodeID(uint8_t(stored_voted_for)); + } + + tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get()); + + return 0; + } + + Term getCurrentTerm() const { return current_term_; } + + NodeID getVotedFor() const { return voted_for_; } + bool isVotedForSet() const { return voted_for_.isUnicast(); } + + Log& getLog() { return log_; } + const Log& getLog() const { return log_; } + + /** + * Invokes storage IO. + */ + int setCurrentTerm(Term term) + { + if (term < current_term_) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + tracer_.onEvent(TraceRaftCurrentTermUpdate, term); + + StorageMarshaller io(storage_); + + Term tmp = term; + int res = io.setAndGetBack(getCurrentTermKey(), tmp); + if (res < 0) + { + return res; + } + + if (tmp != term) + { + return -ErrFailure; + } + + current_term_ = term; + return 0; + } + + /** + * Invokes storage IO. + */ + int setVotedFor(NodeID node_id) + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get()); + + StorageMarshaller io(storage_); + + uint32_t tmp = node_id.get(); + int res = io.setAndGetBack(getVotedForKey(), tmp); + if (res < 0) + { + return res; + } + + if (node_id.get() != tmp) + { + return -ErrFailure; + } + + voted_for_ = node_id; + return 0; + } + + int resetVotedFor() { return setVotedFor(NodeID(0)); } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp new file mode 100644 index 0000000000..b446e3ee45 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -0,0 +1,917 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log. + */ +class IRaftLeaderMonitor +{ +public: + /** + * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). + */ + virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0; + + /** + * Invoked by the Raft core when the local node becomes a leader or ceases to be one. + * By default the local node is not leader. + * It is possible to commit to the log right from this method. + */ + virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0; + + virtual ~IRaftLeaderMonitor() { } +}; + +/** + * This class implements log replication and voting. + * It does not implement client-server interaction at all; instead it just exposes a public method for adding + * allocation entries. + * + * Note that this class uses std::rand(), so the RNG must be properly seeded by the application. + * + * Activity registration: + * - persistent state update error + * - switch to candidate (this defines timeout between reelections) + * - newer term in response (also switch to follower) + * - append entries request with term >= currentTerm + * - vote granted + */ +class RaftCore : private TimerBase +{ +public: + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + +private: + typedef MethodBinder&, + ServiceResponseDataStructure&)> + AppendEntriesCallback; + + typedef MethodBinder&)> + AppendEntriesResponseCallback; + + typedef MethodBinder&, + ServiceResponseDataStructure&)> + RequestVoteCallback; + + typedef MethodBinder&)> + RequestVoteResponseCallback; + + struct PendingAppendEntriesFields + { + Log::Index prev_log_index; + Log::Index num_entries; + + PendingAppendEntriesFields() + : prev_log_index(0) + , num_entries(0) + { } + }; + + /* + * Constants + */ + enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 }; + + IEventTracer& tracer_; + IRaftLeaderMonitor& leader_monitor_; + + /* + * States + */ + PersistentState persistent_state_; + ClusterManager cluster_; + Log::Index commit_index_; + + MonotonicTime last_activity_timestamp_; + MonotonicDuration randomized_activity_timeout_; + ServerState server_state_; + + uint8_t next_server_index_; ///< Next server to query AE from + uint8_t num_votes_received_in_this_campaign_; + + PendingAppendEntriesFields pending_append_entries_fields_; + + /* + * Transport + */ + ServiceServer append_entries_srv_; + ServiceClient append_entries_client_; + ServiceServer request_vote_srv_; + ServiceClient request_vote_client_; + + /* + * Methods + */ + void trace(TraceCode event, int64_t argument) { tracer_.onEvent(event, argument); } + + INode& getNode() { return append_entries_srv_.getNode(); } + const INode& getNode() const { return append_entries_srv_.getNode(); } + + void checkInvariants() const + { + // Commit index + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); + + // Term + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) != + UAVCAN_NULLPTR); + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term <= + persistent_state_.getCurrentTerm()); + + // Elections + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() || + persistent_state_.getVotedFor() == getNode().getNodeID()); + UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize()); + + // Transport + UAVCAN_ASSERT(append_entries_client_.getNumPendingCalls() <= 1); + UAVCAN_ASSERT(request_vote_client_.getNumPendingCalls() <= cluster_.getNumKnownServers()); + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !append_entries_client_.hasPendingCalls()); + UAVCAN_ASSERT(server_state_ != ServerStateLeader || !request_vote_client_.hasPendingCalls()); + UAVCAN_ASSERT(server_state_ != ServerStateFollower || + (!append_entries_client_.hasPendingCalls() && !request_vote_client_.hasPendingCalls())); + } + + void registerActivity() + { + last_activity_timestamp_ = getNode().getMonotonicTime(); + + static const int32_t randomization_range_msec = AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS - + AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS; + // coverity[dont_call] + const int32_t random_msec = (std::rand() % randomization_range_msec) + 1; + + randomized_activity_timeout_ = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec); + + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS); + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS); + } + + bool isActivityTimedOut() const + { + return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_); + } + + void handlePersistentStateUpdateError(int error) + { + UAVCAN_ASSERT(error < 0); + trace(TraceRaftPersistStateUpdateError, error); + switchState(ServerStateFollower); + registerActivity(); // Deferring reelections + } + + void updateFollower() + { + if (isActivityTimedOut()) + { + switchState(ServerStateCandidate); + registerActivity(); + } + } + + void updateCandidate() + { + if (num_votes_received_in_this_campaign_ > 0) + { + trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_); + const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Election complete, won: %d", int(won)); + + switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader + } + else + { + // Set votedFor, abort on failure + int res = persistent_state_.setVotedFor(getNode().getNodeID()); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + // Increment current term, abort on failure + res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + num_votes_received_in_this_campaign_ = 1; // Voting for self + + RequestVote::Request req; + req.last_log_index = persistent_state_.getLog().getLastIndex(); + req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term; + req.term = persistent_state_.getCurrentTerm(); + + for (uint8_t i = 0; i < MaxNumFollowers; i++) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + if (!node_id.isUnicast()) + { + break; + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Requesting vote from %d", int(node_id.get())); + trace(TraceRaftVoteRequestInitiation, node_id.get()); + + res = request_vote_client_.call(node_id, req); + if (res < 0) + { + trace(TraceError, res); + } + } + } + } + + void updateLeader() + { + if (append_entries_client_.hasPendingCalls()) + { + append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why + } + + if (cluster_.getClusterSize() > 1) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); + UAVCAN_ASSERT(node_id.isUnicast()); + + next_server_index_++; + if (next_server_index_ >= cluster_.getNumKnownServers()) + { + next_server_index_ = 0; + } + + AppendEntries::Request req; + req.term = persistent_state_.getCurrentTerm(); + req.leader_commit = commit_index_; + + req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); + + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); + if (entry == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + handlePersistentStateUpdateError(-ErrLogic); + return; + } + + req.prev_log_term = entry->term; + + for (Log::Index index = cluster_.getServerNextIndex(node_id); + index <= persistent_state_.getLog().getLastIndex(); + index++) + { + req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); + if (req.entries.size() == req.entries.capacity()) + { + break; + } + } + + pending_append_entries_fields_.num_entries = req.entries.size(); + pending_append_entries_fields_.prev_log_index = req.prev_log_index; + + const int res = append_entries_client_.call(node_id, req); + if (res < 0) + { + trace(TraceRaftAppendEntriesCallFailure, res); + } + } + + propagateCommitIndex(); + } + + void switchState(ServerState new_state) + { + if (server_state_ == new_state) + { + return; + } + + /* + * Logging + */ + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d", + int(server_state_), int(new_state)); + trace(TraceRaftStateSwitch, new_state); + + /* + * Updating the current state + */ + const ServerState old_state = server_state_; + server_state_ = new_state; + + /* + * Resetting specific states + */ + cluster_.resetAllServerIndices(); + + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; + + request_vote_client_.cancelAllCalls(); + append_entries_client_.cancelAllCalls(); + + /* + * Calling the switch handler + * Note that the handler may commit to the log directly + */ + if ((old_state == ServerStateLeader) || + (new_state == ServerStateLeader)) + { + leader_monitor_.handleLocalLeadershipChange(new_state == ServerStateLeader); + } + } + + void tryIncrementCurrentTermFromResponse(Term new_term) + { + trace(TraceRaftNewerTermInResponse, new_term); + const int res = persistent_state_.setCurrentTerm(new_term); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + registerActivity(); // Deferring future elections + switchState(ServerStateFollower); + } + + void propagateCommitIndex() + { + // Objective is to estimate whether we can safely increment commit index value + UAVCAN_ASSERT(server_state_ == ServerStateLeader); + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); + + if (commit_index_ < persistent_state_.getLog().getLastIndex()) + { + /* + * Not all local entries are committed. + * Deciding if it is safe to increment commit index. + */ + uint8_t num_nodes_with_next_log_entry_available = 1; // Local node + for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) + { + const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); + if (match_index > commit_index_) + { + num_nodes_with_next_log_entry_available++; + } + } + + if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize()) + { + commit_index_++; + UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed + trace(TraceRaftNewEntryCommitted, commit_index_); + + // AT THIS POINT ALLOCATION IS COMPLETE + leader_monitor_.handleLogCommitOnLeader(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); + } + } + } + + void handleAppendEntriesRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + checkInvariants(); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + if (cluster_.isClusterDiscovered()) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + else + { + cluster_.addServer(request.getSrcNodeID()); + } + } + + UAVCAN_ASSERT(response.isResponseEnabled()); // This is default + + /* + * Checking if our current state is up to date. + * The request will be ignored if persistent state cannot be updated. + */ + if (request.term > persistent_state_.getCurrentTerm()) + { + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + response.success = false; + + /* + * Step 1 (see Raft paper) + * Reject the request if the leader has stale term number. + */ + if (request.term < persistent_state_.getCurrentTerm()) + { + response.setResponseEnabled(true); + return; + } + + registerActivity(); + switchState(ServerStateFollower); + + /* + * Step 2 + * Reject the request if the assumed log index does not exist on the local node. + */ + const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index); + if (prev_entry == UAVCAN_NULLPTR) + { + response.setResponseEnabled(true); + return; + } + + /* + * Step 3 + * Drop log entries if term number does not match. + * Ignore the request if the persistent state cannot be updated. + */ + if (prev_entry->term != request.prev_log_term) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index); + response.setResponseEnabled(res >= 0); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + return; + } + + /* + * Step 4 + * Update the log with new entries - this will possibly require to rewrite existing entries. + * Ignore the request if the persistent state cannot be updated. + */ + if (request.prev_log_index != persistent_state_.getLog().getLastIndex()) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + + for (uint8_t i = 0; i < request.entries.size(); i++) + { + const int res = persistent_state_.getLog().append(request.entries[i]); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; // Response will not be sent, the server will assume that we're dead + } + } + + /* + * Step 5 + * Update the commit index. + */ + if (request.leader_commit > commit_index_) + { + commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex()); + trace(TraceRaftCommitIndexUpdate, commit_index_); + } + + response.setResponseEnabled(true); + response.success = true; + } + + void handleAppendEntriesResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + checkInvariants(); + + if (!result.isSuccessful()) + { + return; + } + + if (result.getResponse().term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.getResponse().term); + } + else + { + if (result.getResponse().success) + { + cluster_.incrementServerNextIndexBy(result.getCallID().server_node_id, + pending_append_entries_fields_.num_entries); + cluster_.setServerMatchIndex(result.getCallID().server_node_id, + Log::Index(pending_append_entries_fields_.prev_log_index + + pending_append_entries_fields_.num_entries)); + } + else + { + cluster_.decrementServerNextIndex(result.getCallID().server_node_id); + trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get()); + } + } + + pending_append_entries_fields_ = PendingAppendEntriesFields(); + // Rest of the logic is implemented in periodic update handlers. + } + + void handleRequestVoteRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + checkInvariants(); + trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + + UAVCAN_ASSERT(response.isResponseEnabled()); // This is default + + /* + * Checking if our current state is up to date. + * The request will be ignored if persistent state cannot be updated. + */ + if (request.term > persistent_state_.getCurrentTerm()) + { + switchState(ServerStateFollower); // Our term is stale, so we can't serve as leader + + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + + if (request.term < response.term) + { + response.vote_granted = false; + } + else + { + const bool can_vote = !persistent_state_.isVotedForSet() || + (persistent_state_.getVotedFor() == request.getSrcNodeID()); + const bool log_is_up_to_date = + persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term); + + response.vote_granted = can_vote && log_is_up_to_date; + + if (response.vote_granted) + { + switchState(ServerStateFollower); // Avoiding race condition when Candidate + registerActivity(); // This is necessary to avoid excessive elections + + const int res = persistent_state_.setVotedFor(request.getSrcNodeID()); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + } + } + + void handleRequestVoteResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled + checkInvariants(); + + if (!result.isSuccessful()) + { + return; + } + + trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get()); + + if (result.getResponse().term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.getResponse().term); + } + else + { + if (result.getResponse().vote_granted) + { + num_votes_received_in_this_campaign_++; + } + } + // Rest of the logic is implemented in periodic update handlers. + // I'm no fan of asynchronous programming. At all. + } + + virtual void handleTimerEvent(const TimerEvent&) + { + checkInvariants(); + + switch (server_state_) + { + case ServerStateFollower: + { + updateFollower(); + break; + } + case ServerStateCandidate: + { + updateCandidate(); + break; + } + case ServerStateLeader: + { + updateLeader(); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + } + +public: + RaftCore(INode& node, + IStorageBackend& storage, + IEventTracer& tracer, + IRaftLeaderMonitor& leader_monitor) + : TimerBase(node) + , tracer_(tracer) + , leader_monitor_(leader_monitor) + , persistent_state_(storage, tracer) + , cluster_(node, storage, persistent_state_.getLog(), tracer) + , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero + , last_activity_timestamp_(node.getMonotonicTime()) + , randomized_activity_timeout_( + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS)) + , server_state_(ServerStateFollower) + , next_server_index_(0) + , num_votes_received_in_this_campaign_(0) + , append_entries_srv_(node) + , append_entries_client_(node) + , request_vote_srv_(node) + , request_vote_client_(node) + { } + + /** + * Once started, the logic runs in the background until destructor is called. + * @param cluster_size If set, this value will be used and stored in the persistent storage. If not set, + * value from the persistent storage will be used. If not set and there's no such key + * in the persistent storage, initialization will fail. + */ + int init(const uint8_t cluster_size, const TransferPriority priority) + { + /* + * Initializing state variables + */ + server_state_ = ServerStateFollower; + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; + commit_index_ = 0; + + registerActivity(); + + /* + * Initializing internals + */ + int res = persistent_state_.init(); + if (res < 0) + { + return res; + } + + res = cluster_.init(cluster_size, priority); + if (res < 0) + { + return res; + } + + res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest)); + if (res < 0) + { + return res; + } + + res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest)); + if (res < 0) + { + return res; + } + + res = append_entries_client_.init(priority); + if (res < 0) + { + return res; + } + append_entries_client_.setCallback(AppendEntriesResponseCallback(this, + &RaftCore::handleAppendEntriesResponse)); + + res = request_vote_client_.init(priority); + if (res < 0) + { + return res; + } + request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); + + /* + * Initializing timing constants + * Refer to the specification for the formula + */ + const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); + + const MonotonicDuration update_interval = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS / + 2 / max(static_cast(2), num_followers)); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Update interval: %ld msec", static_cast(update_interval.toMSec())); + + append_entries_client_.setRequestTimeout(min(append_entries_client_.getDefaultRequestTimeout(), + update_interval)); + + request_vote_client_.setRequestTimeout(min(request_vote_client_.getDefaultRequestTimeout(), + update_interval)); + + startPeriodic(update_interval); + + trace(TraceRaftCoreInited, update_interval.toUSec()); + + UAVCAN_ASSERT(res >= 0); + return 0; + } + + /** + * This function is mostly needed for testing. + */ + Log::Index getCommitIndex() const { return commit_index_; } + + /** + * This essentially indicates whether the server could replicate log since last allocation. + */ + bool areAllLogEntriesCommitted() const { return commit_index_ == persistent_state_.getLog().getLastIndex(); } + + /** + * Only the leader can call @ref appendLog(). + */ + bool isLeader() const { return server_state_ == ServerStateLeader; } + + /** + * Inserts one entry into log. + * This method will trigger an assertion failure and return error if the current node is not the leader. + * If operation fails, the node may give up its Leader status. + */ + void appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) + { + if (isLeader()) + { + Entry entry; + entry.node_id = node_id.get(); + entry.unique_id = unique_id; + entry.term = persistent_state_.getCurrentTerm(); + + trace(TraceRaftNewLogEntry, entry.node_id); + const int res = persistent_state_.getLog().append(entry); + if (res < 0) + { + handlePersistentStateUpdateError(res); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This class is used to perform log searches. + */ + struct LogEntryInfo + { + Entry entry; + bool committed; + + LogEntryInfo(const Entry& arg_entry, bool arg_committed) + : entry(arg_entry) + , committed(arg_committed) + { } + }; + + /** + * This method is used by the allocator to query existence of certain entries in the Raft log. + * Predicate is a callable of the following prototype: + * bool (const LogEntryInfo& entry) + * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy + * contructor with the last visited entry; otherwise the constructor will not be initialized. + * In this case, lazy constructor is used as boost::optional. + * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. + */ + template + inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const + { + UAVCAN_ASSERT(coerceOrFallback(predicate, true)); + for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index >= 0; index--) + { + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); + UAVCAN_ASSERT(entry != UAVCAN_NULLPTR); + const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_); + if (predicate(info)) + { + LazyConstructor ret; + ret.template construct(info); + return ret; + } + } + return LazyConstructor(); + } + + Log::Index getNumAllocations() const + { + // Remember that index zero contains a special-purpose entry that doesn't count as allocation + return persistent_state_.getLog().getLastIndex(); + } + + /** + * These accessors are needed for debugging, visualization and testing. + */ + const PersistentState& getPersistentState() const { return persistent_state_; } + const ClusterManager& getClusterManager() const { return cluster_; } + MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; } + ServerState getServerState() const { return server_state_; } + MonotonicDuration getUpdateInterval() const { return getPeriod(); } + MonotonicDuration getRandomizedTimeout() const { return randomized_activity_timeout_; } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp new file mode 100644 index 0000000000..52f712553a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -0,0 +1,354 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class implements the top-level allocation logic and server API. + */ +class UAVCAN_EXPORT Server : public AbstractServer + , IRaftLeaderMonitor +{ + struct UniqueIDLogPredicate + { + const UniqueID unique_id; + + UniqueIDLogPredicate(const UniqueID& uid) + : unique_id(uid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.unique_id == unique_id; + } + }; + + struct NodeIDLogPredicate + { + const NodeID node_id; + + NodeIDLogPredicate(const NodeID& nid) + : node_id(nid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.node_id == node_id.get(); + } + }; + + /* + * States + */ + RaftCore raft_core_; + + /* + * Methods of IAllocationRequestHandler + */ + virtual bool canPublishFollowupAllocationResponse() const + { + /* + * The server is allowed to publish follow-up allocation responses only if both conditions are met: + * - The server is leader. + * - The last allocation request has been completed successfully. + * + * Why second condition? Imagine a case when there's two Raft nodes that don't hear each other - A and B, + * both of them are leaders (but only A can commit to the log, B is in a minor partition); then there's a + * client X that can exchange with both leaders, and a client Y that can exchange only with A. Such a + * situation can occur in case of a very unlikely failure of redundant interfaces. + * + * Both clients X and Y initially send a first-stage Allocation request; A responds to Y with a first-stage + * response, whereas B responds to X. Both X and Y will issue a follow-up second-stage requests, which may + * cause A to mix second-stage Allocation requests from different nodes, leading to reception of an invalid + * unique ID. When both leaders receive full unique IDs (A will receive an invalid one, B will receive a valid + * unique ID of X), only A will be able to make a commit, because B is in a minority. Since both clients were + * unable to receive node ID values in this round, they will try again later. + * + * Now, in order to prevent B from disrupting client-server communication second time around, we introduce this + * second restriction: the server cannot exchange with clients as long as its log contains uncommitted entries. + * + * Note that this restriction does not apply to allocation requests sent via CAN FD frames, as in this case + * no follow-up responses are necessary. So only CAN FD can offer reliable Allocation exchange. + */ + return raft_core_.isLeader() && raft_core_.areAllLogEntriesCommitted(); + } + + virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) + { + /* + * Note that it is possible that the local node is not leader. We will still perform the log search + * and try to find the node that requested allocation. If the node is found, response will be sent; + * otherwise the request will be ignored because only leader can add new allocations. + */ + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id)); + + if (result.isConstructed()) + { + if (result->committed) + { + tryPublishAllocationResult(result->entry); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Allocation request served with existing allocation; node ID %d", + int(result->entry.node_id)); + } + else + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Allocation request ignored - allocation exists but not committed yet; node ID %d", + int(result->entry.node_id)); + } + } + else + { + if (raft_core_.isLeader() && !node_discoverer_.hasUnknownNodes()) + { + allocateNewNode(unique_id, preferred_node_id); + } + } + } + + /* + * Methods of INodeDiscoveryHandler + */ + virtual bool canDiscoverNewNodes() const + { + return raft_core_.isLeader(); + } + + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const + { + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); + if (result.isConstructed()) + { + return result->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted; + } + else + { + return NodeAwarenessUnknown; + } + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) + { + if (raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)).isConstructed()) + { + UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that + return; + } + + const UniqueID uid = (unique_id_or_null == UAVCAN_NULLPTR) ? UniqueID() : *unique_id_or_null; + + if (raft_core_.isLeader()) + { + raft_core_.appendLog(uid, node_id); + } + } + + /* + * Methods of IRaftLeaderMonitor + */ + virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry) + { + /* + * Maybe this node did not request allocation at all, we don't care, we publish anyway. + */ + tryPublishAllocationResult(entry); + } + + virtual void handleLocalLeadershipChange(bool local_node_is_leader) + { + if (!local_node_is_leader) + { + return; + } + + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID())); + + if (!result.isConstructed()) + { + raft_core_.appendLog(getOwnUniqueID(), node_.getNodeID()); + } + } + + /* + * Private methods + */ + bool isNodeIDTaken(const NodeID node_id) const + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Testing if node ID %d is taken", int(node_id.get())); + return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); + } + + void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) + { + const NodeID allocated_node_id = + NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); + if (!allocated_node_id.isUnicast()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left"); + return; + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d", + int(allocated_node_id.get())); + raft_core_.appendLog(unique_id, allocated_node_id); + } + + void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) + { + const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); + if (res < 0) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("Dynamic allocation response"); + } + } + +public: + Server(INode& node, + IStorageBackend& storage, + IEventTracer& tracer) + : AbstractServer(node, tracer) + , raft_core_(node, storage, tracer, *this) + { } + + int init(const UniqueID& own_unique_id, + const uint8_t cluster_size = ClusterManager::ClusterSizeUnknown, + const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + /* + * Initializing Raft core first, because the next step requires Log to be loaded + */ + int res = raft_core_.init(cluster_size, priority); + if (res < 0) + { + return res; + } + + /* + * Common logic + */ + res = AbstractServer::init(own_unique_id, priority); + if (res < 0) + { + return res; + } + + /* + * Making sure that the server is started with the same node ID + */ + const LazyConstructor own_log_entry = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID())); + + if (own_log_entry.isConstructed()) + { + if (own_log_entry->entry.unique_id != getOwnUniqueID()) + { + return -ErrInvalidConfiguration; + } + } + + return 0; + } + + Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); } + + /** + * These accessors are needed for debugging, visualization and testing. + */ + const RaftCore& getRaftCore() const { return raft_core_; } +}; + +/** + * This structure represents immediate state of the server. + * It can be used for state visualization and debugging. + */ +struct StateReport +{ + uint8_t cluster_size; + + RaftCore::ServerState state; + + Log::Index last_log_index; + Log::Index commit_index; + + Term last_log_term; + Term current_term; + + NodeID voted_for; + + MonotonicTime last_activity_timestamp; + MonotonicDuration randomized_timeout; + + uint8_t num_unknown_nodes; + + struct FollowerState + { + NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + FollowerState() + : next_index(0) + , match_index(0) + { } + } followers[ClusterManager::MaxClusterSize - 1]; + + StateReport(const Server& s) + : cluster_size (s.getRaftCore().getClusterManager().getClusterSize()) + , state (s.getRaftCore().getServerState()) + , last_log_index (s.getRaftCore().getPersistentState().getLog().getLastIndex()) + , commit_index (s.getRaftCore().getCommitIndex()) + , last_log_term (0) // See below + , current_term (s.getRaftCore().getPersistentState().getCurrentTerm()) + , voted_for (s.getRaftCore().getPersistentState().getVotedFor()) + , last_activity_timestamp(s.getRaftCore().getLastActivityTimestamp()) + , randomized_timeout (s.getRaftCore().getRandomizedTimeout()) + , num_unknown_nodes (s.getNodeDiscoverer().getNumUnknownNodes()) + { + const Entry* const e = s.getRaftCore().getPersistentState().getLog().getEntryAtIndex(last_log_index); + UAVCAN_ASSERT(e != UAVCAN_NULLPTR); + if (e != UAVCAN_NULLPTR) + { + last_log_term = e->term; + } + + for (uint8_t i = 0; i < (cluster_size - 1U); i++) + { + const ClusterManager& mgr = s.getRaftCore().getClusterManager(); + const NodeID node_id = mgr.getRemoteServerNodeIDAtIndex(i); + if (node_id.isUnicast()) + { + followers[i].node_id = node_id; + followers[i].next_index = mgr.getServerNextIndex(node_id); + followers[i].match_index = mgr.getServerMatchIndex(node_id); + } + } + } +}; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp new file mode 100644 index 0000000000..ed3ba2f873 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ + +using namespace ::uavcan::protocol::dynamic_node_id::server; + +/** + * Raft term + */ +typedef StorageType::Type Term; + +} +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp new file mode 100644 index 0000000000..7f56f2a82c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * @ref IEventTracer. + * Event codes cannot be changed, only new ones can be added. + */ +enum UAVCAN_EXPORT TraceCode +{ + // Event name Argument + // 0 + TraceError, // error code (may be negated) + TraceRaftLogLastIndexRestored, // recovered last index value + TraceRaftLogAppend, // index of new entry + TraceRaftLogRemove, // new last index value + TraceRaftCurrentTermRestored, // current term + // 5 + TraceRaftCurrentTermUpdate, // current term + TraceRaftVotedForRestored, // value of votedFor + TraceRaftVotedForUpdate, // value of votedFor + TraceRaftDiscoveryBroadcast, // number of known servers + TraceRaftNewServerDiscovered, // node ID of the new server + // 10 + TraceRaftDiscoveryReceived, // node ID of the sender + TraceRaftClusterSizeInited, // cluster size + TraceRaftBadClusterSizeReceived, // received cluster size + TraceRaftCoreInited, // update interval in usec + TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader + // 15 + Trace0, + TraceRaftNewLogEntry, // node ID value + TraceRaftRequestIgnored, // node ID of the client + TraceRaftVoteRequestReceived, // node ID of the client + TraceRaftVoteRequestSucceeded, // node ID of the server + // 20 + TraceRaftVoteRequestInitiation, // node ID of the server + TraceRaftPersistStateUpdateError, // negative error code + TraceRaftCommitIndexUpdate, // new commit index value + TraceRaftNewerTermInResponse, // new term value + TraceRaftNewEntryCommitted, // new commit index value + // 25 + TraceRaftAppendEntriesCallFailure, // error code (may be negated) + TraceRaftElectionComplete, // number of votes collected + TraceRaftAppendEntriesRespUnsucfl, // node ID of the client + Trace2, + Trace3, + // 30 + TraceAllocationFollowupResponse, // number of unique ID bytes in this response + TraceAllocationFollowupDenied, // reason code (see sources for details) + TraceAllocationFollowupTimeout, // timeout value in microseconds + TraceAllocationBadRequest, // number of unique ID bytes in this request + TraceAllocationUnexpectedStage, // stage number in the request - 1, 2, or 3 + // 35 + TraceAllocationRequestAccepted, // number of bytes of unique ID after request + TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit big endian + TraceAllocationResponse, // allocated node ID + TraceAllocationActivity, // source node ID of the message + Trace12, + // 40 + TraceDiscoveryNewNodeFound, // node ID + TraceDiscoveryCommitCacheUpdated, // node ID marked as committed + TraceDiscoveryNodeFinalized, // node ID in lower 7 bits, bit 8 (256, 0x100) is set if unique ID is known + TraceDiscoveryGetNodeInfoFailure, // node ID + TraceDiscoveryTimerStart, // interval in microseconds + // 45 + TraceDiscoveryTimerStop, // reason code (see sources for details) + TraceDiscoveryGetNodeInfoRequest, // target node ID + TraceDiscoveryNodeRestartDetected, // node ID + TraceDiscoveryNodeRemoved, // node ID + Trace22, + // 50 + + NumTraceCodes +}; + +/** + * This interface allows the application to trace events that happen in the server. + */ +class UAVCAN_EXPORT IEventTracer +{ +public: +#if UAVCAN_TOSTRING + /** + * It is safe to call this function with any argument. + * If the event code is out of range, an assertion failure will be triggered and an error text will be returned. + */ + static const char* getEventName(TraceCode code) + { + // import re;m = lambda s:',\n'.join('"%s"' % x for x in re.findall(r'\ {4}Trace[0-9]*([A-Za-z0-9]*),',s)) + static const char* const Strings[] = + { + "Error", + "RaftLogLastIndexRestored", + "RaftLogAppend", + "RaftLogRemove", + "RaftCurrentTermRestored", + "RaftCurrentTermUpdate", + "RaftVotedForRestored", + "RaftVotedForUpdate", + "RaftDiscoveryBroadcast", + "RaftNewServerDiscovered", + "RaftDiscoveryReceived", + "RaftClusterSizeInited", + "RaftBadClusterSizeReceived", + "RaftCoreInited", + "RaftStateSwitch", + "", + "RaftNewLogEntry", + "RaftRequestIgnored", + "RaftVoteRequestReceived", + "RaftVoteRequestSucceeded", + "RaftVoteRequestInitiation", + "RaftPersistStateUpdateError", + "RaftCommitIndexUpdate", + "RaftNewerTermInResponse", + "RaftNewEntryCommitted", + "RaftAppendEntriesCallFailure", + "RaftElectionComplete", + "RaftAppendEntriesRespUnsucfl", + "", + "", + "AllocationFollowupResponse", + "AllocationFollowupDenied", + "AllocationFollowupTimeout", + "AllocationBadRequest", + "AllocationUnexpectedStage", + "AllocationRequestAccepted", + "AllocationExchangeComplete", + "AllocationResponse", + "AllocationActivity", + "", + "DiscoveryNewNodeFound", + "DiscoveryCommitCacheUpdated", + "DiscoveryNodeFinalized", + "DiscoveryGetNodeInfoFailure", + "DiscoveryTimerStart", + "DiscoveryTimerStop", + "DiscoveryGetNodeInfoRequest", + "DiscoveryNodeRestartDetected", + "DiscoveryNodeRemoved", + "" + }; + uavcan::StaticAssert::check(); + UAVCAN_ASSERT(code < NumTraceCodes); + // coverity[dead_error_line] + return (code < NumTraceCodes) ? Strings[static_cast(code)] : "INVALID_EVENT_CODE"; + } +#endif + + /** + * The server invokes this method every time it believes that a noteworthy event has happened. + * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures + * full backward compatibility. + * @param event_code Event code, see the sources for the enum with values. + * @param event_argument Value associated with the event; its meaning depends on the event code. + */ + virtual void onEvent(TraceCode event_code, int64_t event_argument) = 0; + + virtual ~IEventTracer() { } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp new file mode 100644 index 0000000000..958328ba60 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -0,0 +1,348 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * The allocator must implement this interface. + */ +class INodeDiscoveryHandler +{ +public: + /** + * In order to avoid bus congestion, normally only leader can discover new nodes. + */ + virtual bool canDiscoverNewNodes() const = 0; + + /** + * These values represent the level of awareness of a certain node by the server. + */ + enum NodeAwareness + { + NodeAwarenessUnknown, + NodeAwarenessKnownButNotCommitted, + NodeAwarenessKnownAndCommitted + }; + + /** + * It is OK to do full log traversal in this method, because the unique ID collector will cache the + * result when possible. + */ + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0; + + /** + * This method will be called when a new node responds to GetNodeInfo request. + * If this method fails to register the node, the node will be queried again later and this method will be + * invoked again. + * Unique ID will be UAVCAN_NULLPTR if the node is assumed to not implement the GetNodeInfo service. + */ + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0; + + virtual ~INodeDiscoveryHandler() { } +}; + +/** + * This class listens to NodeStatus messages from other nodes and retrieves their unique ID if they are not + * known to the allocator. + */ +class NodeDiscoverer : TimerBase +{ + typedef MethodBinder&)> + GetNodeInfoResponseCallback; + + typedef MethodBinder&)> + NodeStatusCallback; + + struct NodeData + { + uint32_t last_seen_uptime; + uint8_t num_get_node_info_attempts; + + NodeData() + : last_seen_uptime(0) + , num_get_node_info_attempts(0) + { } + }; + + typedef Map NodeMap; + + /** + * When this number of attempts has been made, the discoverer will give up and assume that the node + * does not implement this service. + */ + enum { MaxAttemptsToGetNodeInfo = 5 }; + + enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) + + /* + * States + */ + INodeDiscoveryHandler& handler_; + IEventTracer& tracer_; + + BitSet committed_node_mask_; ///< Nodes that are marked will not be queried + NodeMap node_map_; + + ServiceClient get_node_info_client_; + Subscriber node_status_sub_; + + /* + * Methods + */ + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + + INode& getNode() { return node_status_sub_.getNode(); } + + void removeNode(const NodeID node_id) + { + node_map_.remove(node_id); + trace(TraceDiscoveryNodeRemoved, node_id.get()); + } + + NodeID pickNextNodeToQuery() const + { + // This essentially means that we pick first available node. Remember that the map is unordered. + const NodeMap::KVPair* const pair = node_map_.getByIndex(0); + return (pair == UAVCAN_NULLPTR) ? NodeID() : pair->key; + } + + bool needToQuery(NodeID node_id) + { + UAVCAN_ASSERT(node_id.isUnicast()); + + /* + * Fast check + */ + if (committed_node_mask_[node_id.get()]) + { + return false; + } + + /* + * Slow check - may involve full log search + */ + const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id); + + if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown) + { + return true; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted) + { + removeNode(node_id); + return false; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted) + { + trace(TraceDiscoveryCommitCacheUpdated, node_id.get()); + committed_node_mask_[node_id.get()] = true; + removeNode(node_id); + return false; + } + else + { + UAVCAN_ASSERT(0); + return false; + } + } + + NodeID pickNextNodeToQueryAndCleanupMap() + { + NodeID node_id; + do + { + node_id = pickNextNodeToQuery(); + if (node_id.isUnicast()) + { + if (needToQuery(node_id)) + { + return node_id; + } + else + { + removeNode(node_id); + } + } + } + while (node_id.isUnicast()); + return NodeID(); + } + + void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) + { + trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == UAVCAN_NULLPTR) ? 0U : 0x100U)); + removeNode(node_id); + /* + * It is paramount to check if the server is still interested to receive this data. + * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with + * duplicate node ID in the log. + */ + if (needToQuery(node_id)) + { + handler_.handleNewNodeDiscovery(unique_id_or_null, node_id); + } + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + if (result.isSuccessful()) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d", + int(result.getCallID().server_node_id.get())); + finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id); + } + else + { + trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get()); + + NodeData* const data = node_map_.access(result.getCallID().server_node_id); + if (data == UAVCAN_NULLPTR) + { + return; // Probably it is a known node now + } + + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", + "GetNodeInfo request to %d has timed out, %d attempts", + int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts)); + data->num_get_node_info_attempts++; + if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo) + { + finalizeNodeDiscovery(UAVCAN_NULLPTR, result.getCallID().server_node_id); + // Warning: data pointer is invalidated now + } + } + } + + void handleTimerEvent(const TimerEvent&) override + { + if (get_node_info_client_.hasPendingCalls()) + { + return; + } + + const NodeID node_id = pickNextNodeToQueryAndCleanupMap(); + if (!node_id.isUnicast()) + { + trace(TraceDiscoveryTimerStop, 0); + stop(); + return; + } + + if (!handler_.canDiscoverNewNodes()) + { + return; // Timer must continue to run in order to not stuck when it unlocks + } + + trace(TraceDiscoveryGetNodeInfoRequest, node_id.get()); + + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d", + int(node_id.get())); + const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request()); + if (res < 0) + { + getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call"); + } + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + if (!needToQuery(msg.getSrcNodeID())) + { + return; + } + + NodeData* data = node_map_.access(msg.getSrcNodeID()); + if (data == UAVCAN_NULLPTR) + { + trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get()); + + data = node_map_.insert(msg.getSrcNodeID(), NodeData()); + if (data == UAVCAN_NULLPTR) + { + getNode().registerInternalFailure("NodeDiscoverer OOM"); + return; + } + } + UAVCAN_ASSERT(data != UAVCAN_NULLPTR); + + if (msg.uptime_sec < data->last_seen_uptime) + { + trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get()); + data->num_get_node_info_attempts = 0; + } + data->last_seen_uptime = msg.uptime_sec; + + if (!isRunning()) + { + startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); + trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); + } + } + +public: + NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler) + : TimerBase(node) + , handler_(handler) + , tracer_(tracer) + , node_map_(node.getAllocator()) + , get_node_info_client_(node) + , node_status_sub_(node) + { } + + int init(const TransferPriority priority) + { + int res = get_node_info_client_.init(priority); + if (res < 0) + { + return res; + } + get_node_info_client_.setCallback( + GetNodeInfoResponseCallback(this, &NodeDiscoverer::handleGetNodeInfoResponse)); + + res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus)); + if (res < 0) + { + return res; + } + + // Note: the timer starts ad-hoc from the node status callback, not here. + + return 0; + } + + /** + * Returns true if there's at least one node with pending GetNodeInfo. + */ + bool hasUnknownNodes() const { return !node_map_.isEmpty(); } + + /** + * Returns number of nodes that are being queried at the moment. + * This method is needed for testing and state visualization. + */ + uint8_t getNumUnknownNodes() const { return static_cast(node_map_.getSize()); } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp new file mode 100644 index 0000000000..42e85926e8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * Node ID allocation logic + */ +template +class NodeIDSelector +{ + typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const; + + const Owner* const owner_; + const IsNodeIDTakenMethod is_node_id_taken_; + +public: + NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken) + : owner_(owner) + , is_node_id_taken_(is_node_id_taken) + { + UAVCAN_ASSERT(owner_ != UAVCAN_NULLPTR); + UAVCAN_ASSERT(is_node_id_taken_ != UAVCAN_NULLPTR); + } + + /** + * Reutrns a default-constructed (invalid) node ID if a free one could not be found. + */ + NodeID findFreeNodeID(const NodeID preferred) const + { + uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; + + // Up + while (candidate <= NodeID::MaxRecommendedForRegularNodes) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; + + // Down + while (candidate > 0) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); + } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp new file mode 100644 index 0000000000..9e2395c92a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * This interface is used by the server to read and write stable storage. + * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 + * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward + * and efficient implementation of storage backends, e.g. based on text files. + * Keys and values may contain only non-whitespace, non-formatting printable characters. + */ +class UAVCAN_EXPORT IStorageBackend +{ +public: + /** + * Maximum length of keys and values. One pair takes twice as much space. + */ + enum { MaxStringLength = 32 }; + + /** + * It is guaranteed that the server will never require more than this number of key/value pairs. + * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. + */ + enum { MaxKeyValuePairs = 512 }; + + /** + * This type is used to exchange data chunks with the backend. + * It doesn't use any dynamic memory; please refer to the Array<> class for details. + */ + typedef MakeString::Type String; + + /** + * Read one value from the storage. + * If such key does not exist, or if read failed, an empty string will be returned. + * This method should not block for more than 50 ms. + */ + virtual String get(const String& key) const = 0; + + /** + * Create or update value for the given key. Empty value should be regarded as a request to delete the key. + * This method should not block for more than 50 ms. + * Failures will be ignored. + */ + virtual void set(const String& key, const String& value) = 0; + + virtual ~IStorageBackend() { } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp new file mode 100644 index 0000000000..83f68e4f2a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * This class extends the storage backend interface with serialization/deserialization functionality. + */ +class StorageMarshaller +{ + IStorageBackend& storage_; + + static uint8_t convertLowerCaseHexCharToNibble(char ch) + { + const uint8_t ret = (ch > '9') ? static_cast(ch - 'a' + 10) : static_cast(ch - '0'); + UAVCAN_ASSERT(ret < 16); + return ret; + } + +public: + StorageMarshaller(IStorageBackend& storage) + : storage_(storage) + { } + + /** + * Turns a unique ID into a hex string that can be used as a key or as a value. + */ + static IStorageBackend::String convertUniqueIDToHex(const UniqueID& key) + { + IStorageBackend::String serialized; + for (uint8_t i = 0; i < UniqueID::MaxSize; i++) + { + serialized.appendFormatted("%02x", key.at(i)); + } + UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2); + return serialized; + } + + /** + * These methods set the value and then immediately read it back. + * 1. Serialize the value. + * 2. Update the value on the backend. + * 3. Call get() with the same value argument. + * The caller then is supposed to check whether the argument has the desired value. + */ + int setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value) + { + IStorageBackend::String serialized; + serialized.appendFormatted("%lu", inout_value); + + UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); + } + + int setAndGetBack(const IStorageBackend::String& key, UniqueID& inout_value) + { + const IStorageBackend::String serialized = convertUniqueIDToHex(inout_value); + + UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); + } + + /** + * Getters simply read and deserialize the value. + * 1. Read the value back from the backend; return false if read fails. + * 2. Deserealize the newly read value; return false if deserialization fails. + * 3. Update the argument with deserialized value. + * 4. Return true. + */ + int get(const IStorageBackend::String& key, uint32_t& out_value) const + { + /* + * Reading the storage + */ + const IStorageBackend::String val = storage_.get(key); + if (val.empty()) + { + return -ErrFailure; + } + + /* + * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. + * The value must contain only numeric characters. + */ + for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if (static_cast(*it) < '0' || static_cast(*it) > '9') + { + return -ErrFailure; + } + } + + if (val.size() > 10) // len(str(0xFFFFFFFF)) + { + return -ErrFailure; + } + + /* + * Conversion is carried out here + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + errno = 0; +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + const unsigned long long x = std::strtoull(val.c_str(), UAVCAN_NULLPTR, 10); +#else + // There was no strtoull() before C++11, so we need to resort to strtoul() + StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check(); + const unsigned long x = std::strtoul(val.c_str(), UAVCAN_NULLPTR, 10); +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + if (errno != 0) + { + return -ErrFailure; + } +#endif + + out_value = static_cast(x); + return 0; + } + + int get(const IStorageBackend::String& key, UniqueID& out_value) const + { + static const uint8_t NumBytes = UniqueID::MaxSize; + + /* + * Reading the storage + */ + IStorageBackend::String val = storage_.get(key); + if (val.size() != NumBytes * 2) + { + return -ErrFailure; + } + + /* + * The value must contain only hexadecimal numbers. + */ + val.convertToLowerCaseASCII(); + for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if ((static_cast(*it) < '0' || static_cast(*it) > '9') && + (static_cast(*it) < 'a' || static_cast(*it) > 'f')) + { + return -ErrFailure; + } + } + + /* + * Conversion is carried out here + */ + IStorageBackend::String::const_iterator it = val.begin(); + + for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++) + { + out_value[byte_index] = + static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) << 4); + out_value[byte_index] = + static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) | out_value[byte_index]); + } + + return 0; + } +}; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp new file mode 100644 index 0000000000..360a93aee6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED + +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +using namespace ::uavcan::protocol::dynamic_node_id; + +/** + * Node Unique ID + */ +typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID; + +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/file_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/file_server.hpp new file mode 100644 index 0000000000..167be48fa9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/file_server.hpp @@ -0,0 +1,321 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +// UAVCAN types +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * The file server backend should implement this interface. + * Note that error codes returned by these methods are defined in uavcan.protocol.file.Error; these are + * not the same as libuavcan-internal error codes defined in uavcan.error.hpp. + */ +class UAVCAN_EXPORT IFileServerBackend +{ + +public: + typedef protocol::file::Path::FieldTypes::path Path; + typedef protocol::file::EntryType EntryType; + typedef protocol::file::Error Error; + + IFileServerBackend::Path root_path_; + IFileServerBackend::Path alt_root_path_; + + /** + * All read operations must return this number of bytes, unless end of file is reached. + */ + enum { ReadSize = protocol::file::Read::Response::FieldTypes::data::MaxSize }; + + /** + * Shortcut for uavcan.protocol.file.Path.SEPARATOR. + */ + static char getPathSeparator() { return static_cast(protocol::file::Path::SEPARATOR); } + + /** + * Set a base path to the files. + */ + void setRootPath(const char * path) + { + if (path) + { + root_path_.clear(); + root_path_ = path; + if (root_path_.back() != getPathSeparator()) + { + root_path_.push_back(getPathSeparator()); + } + } + } + + void setAltRootPath(const char * path) + { + if (path) + { + alt_root_path_.clear(); + alt_root_path_ = path; + if (alt_root_path_.back() != getPathSeparator()) + { + alt_root_path_.push_back(getPathSeparator()); + } + } + } + + /** + * Get a base path to the files. + */ + Path& getRootPath() + { + return root_path_; + } + + /** + * Get a base path to the files. + */ + Path& getAltRootPath() + { + return alt_root_path_; + } + + /** + * Backend for uavcan.protocol.file.GetInfo. + * Refer to uavcan.protocol.file.EntryType for the list of available bit flags. + * Implementation of this method is required. + * On success the method must return zero. + */ + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) = 0; + + /** + * Backend for uavcan.protocol.file.Read. + * Implementation of this method is required. + * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except + * if the end of file is reached. + * On success the method must return zero. + */ + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0; + + // Methods below are optional. + + /** + * Backend for uavcan.protocol.file.Write. + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t write(const Path& path, const uint64_t offset, const uint8_t* buffer, const uint16_t size) + { + (void)path; + (void)offset; + (void)buffer; + (void)size; + return Error::NOT_IMPLEMENTED; + } + + /** + * Backend for uavcan.protocol.file.Delete. ('delete' is a C++ keyword, so 'remove' is used instead) + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t remove(const Path& path) + { + (void)path; + return Error::NOT_IMPLEMENTED; + } + + /** + * Backend for uavcan.protocol.file.GetDirectoryEntryInfo. + * Refer to uavcan.protocol.file.EntryType for the list of available bit flags. + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t getDirectoryEntryInfo(const Path& directory_path, const uint32_t entry_index, + EntryType& out_type, Path& out_entry_full_path) + { + (void)directory_path; + (void)entry_index; + (void)out_type; + (void)out_entry_full_path; + return Error::NOT_IMPLEMENTED; + } + + virtual ~IFileServerBackend() { } +}; + +/** + * Basic file server implements only the following services: + * uavcan.protocol.file.GetInfo + * uavcan.protocol.file.Read + * Also see @ref IFileServerBackend. + */ +class BasicFileServer +{ + typedef MethodBinder + GetInfoCallback; + + typedef MethodBinder + ReadCallback; + + ServiceServer get_info_srv_; + ServiceServer read_srv_; + + void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp) + { + resp.error.value = backend_.getInfo(req.path.path, resp.size, resp.entry_type); + } + + void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp) + { + uint16_t inout_size = resp.data.capacity(); + + resp.data.resize(inout_size); + + resp.error.value = backend_.read(req.path.path, req.offset, resp.data.begin(), inout_size); + + if (resp.error.value != protocol::file::Error::OK) + { + inout_size = 0; + } + + if (inout_size > resp.data.capacity()) + { + UAVCAN_ASSERT(0); + resp.error.value = protocol::file::Error::UNKNOWN_ERROR; + } + else + { + resp.data.resize(inout_size); + } + } + +protected: + IFileServerBackend& backend_; ///< Derived types can use it + +public: + BasicFileServer(INode& node, IFileServerBackend& backend) + : get_info_srv_(node) + , read_srv_(node) + , backend_(backend) + { } + + int start(const char* server_root = UAVCAN_NULLPTR, const char* server_alt_root = UAVCAN_NULLPTR) + { + backend_.setRootPath(server_root); + backend_.setAltRootPath(server_alt_root); + + int res = get_info_srv_.start(GetInfoCallback(this, &BasicFileServer::handleGetInfo)); + if (res < 0) + { + return res; + } + + res = read_srv_.start(ReadCallback(this, &BasicFileServer::handleRead)); + if (res < 0) + { + return res; + } + + return 0; + } +}; + +/** + * Full file server implements all file services: + * uavcan.protocol.file.GetInfo + * uavcan.protocol.file.Read + * uavcan.protocol.file.Write + * uavcan.protocol.file.Delete + * uavcan.protocol.file.GetDirectoryEntryInfo + * Also see @ref IFileServerBackend. + */ +class FileServer : protected BasicFileServer +{ + typedef MethodBinder + WriteCallback; + + typedef MethodBinder + DeleteCallback; + + typedef MethodBinder + GetDirectoryEntryInfoCallback; + + ServiceServer write_srv_; + ServiceServer delete_srv_; + ServiceServer get_directory_entry_info_srv_; + + void handleWrite(const protocol::file::Write::Request& req, protocol::file::Write::Response& resp) + { + resp.error.value = backend_.write(req.path.path, req.offset, req.data.begin(), req.data.size()); + } + + void handleDelete(const protocol::file::Delete::Request& req, protocol::file::Delete::Response& resp) + { + resp.error.value = backend_.remove(req.path.path); + } + + void handleGetDirectoryEntryInfo(const protocol::file::GetDirectoryEntryInfo::Request& req, + protocol::file::GetDirectoryEntryInfo::Response& resp) + { + resp.error.value = backend_.getDirectoryEntryInfo(req.directory_path.path, req.entry_index, + resp.entry_type, resp.entry_full_path.path); + } + + +public: + FileServer(INode& node, IFileServerBackend& backend) + : BasicFileServer(node, backend) + , write_srv_(node) + , delete_srv_(node) + , get_directory_entry_info_srv_(node) + { } + + int start() + { + int res = BasicFileServer::start(); + if (res < 0) + { + return res; + } + + res = write_srv_.start(WriteCallback(this, &FileServer::handleWrite)); + if (res < 0) + { + return res; + } + + res = delete_srv_.start(DeleteCallback(this, &FileServer::handleDelete)); + if (res < 0) + { + return res; + } + + res = get_directory_entry_info_srv_.start( + GetDirectoryEntryInfoCallback(this, &FileServer::handleGetDirectoryEntryInfo)); + if (res < 0) + { + return res; + } + + return 0; + } +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp new file mode 100644 index 0000000000..04de3be0d2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -0,0 +1,472 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +/** + * Application-specific firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class IFirmwareVersionChecker +{ +public: + /** + * This value is limited by the pool block size minus some extra data. Please refer to the Map<> documentation + * for details. If this size is set too high, the compilation will fail in the Map<> template. + */ + enum { MaxFirmwareFilePathLength = 40 }; + + /** + * This type is used to store path to firmware file that the target node will retrieve using the + * service uavcan.protocol.file.Read. Note that the maximum length is limited due to some specifics of + * libuavcan (@ref MaxFirmwareFilePathLength), this is NOT a protocol-level limitation. + */ + typedef MakeString::Type FirmwareFilePath; + + /** + * This method will be invoked when the class obtains a response to GetNodeInfo request. + * + * @param node_id Node ID that this GetNodeInfo response was received from. + * + * @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details. + * + * @param out_firmware_file_path The implementation should return the firmware image path via this argument. + * Note that this path must be reachable via uavcan.protocol.file.Read service. + * Refer to @ref FileServer and @ref BasicFileServer for details. + * + * @return True - the class will begin sending update requests. + * False - the node will be ignored, no request will be sent. + */ + virtual bool shouldRequestFirmwareUpdate(NodeID node_id, const protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) = 0; + + /** + * This method will be invoked when a node responds to the update request with an error. If the request simply + * times out, this method will not be invoked. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting + * is not needed anymore. This method will not be invoked. + * + * @param node_id Node ID that returned this error. + * + * @param error_response Contents of the error response. It contains error code and text. + * + * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be + * initialized with old path, so if the same path needs to be reused, this + * argument should be left unchanged. + * + * @return True - the class will continue sending update requests with new firmware path. + * False - the node will be forgotten, new requests will not be sent. + */ + virtual bool shouldRetryFirmwareUpdate(NodeID node_id, + const protocol::file::BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) = 0; + + /** + * This node is invoked when the node responds to the update request with confirmation. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * Implementation is optional; default one does nothing. + * + * @param node_id Node ID that confirmed the request. + * + * @param response Actual response. + */ + virtual void handleFirmwareUpdateConfirmation(NodeID node_id, + const protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } + + virtual ~IFirmwareVersionChecker() { } +}; + +/** + * This class subscribes to updates from @ref NodeInfoRetriever in order to detect nodes that need firmware + * updates. The decision process of whether a firmware update is needed is relayed to the application via + * @ref IFirmwareVersionChecker. If the application confirms that the update is needed, this class will begin + * sending uavcan.protocol.file.BeginFirmwareUpdate periodically (period is configurable) to every node that + * needs an update in a round-robin fashion. There are the following termination conditions for the periodical + * sending process: + * + * - The node responds with confirmation. In this case the class will forget about the node on the assumption + * that its job is done here. Confirmation will be reported to the application via the interface. + * + * - The node responds with an error, and the error code is not ERROR_IN_PROGRESS. In this case the class will + * request the application via the interface mentioned above about its further actions - either give up or + * retry, possibly with a different firmware. + * + * - The node responds with error ERROR_IN_PROGRESS. In this case the class behaves exactly in the same way as if + * response was successful (because the firmware is alredy being updated, so the goal is fulfilled). + * Confirmation will be reported to the application via the interface. + * + * - The node goes offline or restarts. In this case the node will be immediately forgotten, and the process + * will repeat again later because the node info retriever re-queries GetNodeInfo every time when a node restarts. + * + * Since the target node (i.e. node that is being updated) will try to retrieve the specified firmware file using + * the file services (uavcan.protocol.file.*), the provided firmware path must be reachable for the file server + * (@ref FileServer, @ref BasicFileServer). Normally, an application that serves as UAVCAN firmware update server + * will include at least the following components: + * - this firmware update trigger; + * - dynamic node ID allocation server; + * - file server. + * + * Implementation details: the class uses memory pool to keep the list of nodes that have not responded yet, which + * limits the maximum length of the path to the firmware file, which is covered in @ref IFirmwareVersionChecker. + * To somewhat relieve the maximum path length limitation, the class can be supplied with a common prefix that + * will be prepended to firmware pathes before sending requests. + * Interval at which requests are being sent is configurable, but the default value should cover the needs of + * virtually all use cases (as always). + */ +class FirmwareUpdateTrigger : public INodeInfoListener, + private TimerBase +{ + typedef MethodBinder&)> + BeginFirmwareUpdateResponseCallback; + + typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath; + + enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout. + + struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable + { + enum { DefaultOutput = 0xFFU }; + + const FirmwareUpdateTrigger& owner; + uint8_t output; + + NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner) + : owner(arg_owner) + , output(DefaultOutput) + { } + + bool operator()(const NodeID node_id, const FirmwareFilePath&) + { + if (node_id.get() > owner.last_queried_node_id_ && + !owner.begin_fw_update_client_.hasPendingCallToServer(node_id)) + { + output = min(output, node_id.get()); + } + return false; + } + }; + + /* + * State + */ + ServiceClient begin_fw_update_client_; + + IFirmwareVersionChecker& checker_; + + NodeInfoRetriever* node_info_retriever_; + + Map pending_nodes_; + + MonotonicDuration request_interval_; + + FirmwareFilePath common_path_prefix_; + + mutable uint8_t last_queried_node_id_; + + /* + * Methods of INodeInfoListener + */ + virtual void handleNodeInfoUnavailable(NodeID node_id) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get())); + pending_nodes_.remove(node_id); // For extra paranoia + } + + virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info) + { + FirmwareFilePath firmware_file_path; + const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path); + if (update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s", + int(node_id.get()), firmware_file_path.c_str()); + trySetPendingNode(node_id, firmware_file_path); + } + else + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get())); + pending_nodes_.remove(node_id); + } + } + + virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event) + { + if (event.status.mode == protocol::NodeStatus::MODE_OFFLINE) + { + pending_nodes_.remove(event.node_id); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get())); + } + } + + /* + * Own methods + */ + INode& getNode() { return begin_fw_update_client_.getNode(); } + + void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path) + { + if (UAVCAN_NULLPTR != pending_nodes_.insert(node_id, path)) + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer started"); + } + } + else + { + getNode().registerInternalFailure("FirmwareUpdateTrigger OOM"); + } + } + + NodeID pickNextNodeID() const + { + // We can't do index search because indices are unstable in Map<> + // First try - from the current node up + NextNodeIDSearchPredicate s1(*this); + (void)pending_nodes_.find(s1); + + if (s1.output != NextNodeIDSearchPredicate::DefaultOutput) + { + last_queried_node_id_ = s1.output; + } + else if (last_queried_node_id_ != 0) + { + // Nothing was found, resetting the selector and trying again + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_)); + last_queried_node_id_ = 0; + + NextNodeIDSearchPredicate s2(*this); + (void)pending_nodes_.find(s2); + + if (s2.output != NextNodeIDSearchPredicate::DefaultOutput) + { + last_queried_node_id_ = s2.output; + } + } + else + { + ; // Hopeless + } + UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u", + int(last_queried_node_id_), pending_nodes_.getSize(), + begin_fw_update_client_.getNumPendingCalls()); + return last_queried_node_id_; + } + + void handleBeginFirmwareUpdateResponse(const ServiceCallResult& result) + { + if (!result.isSuccessful()) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry", + int(result.getCallID().server_node_id.get())); + return; + } + + FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id); + if (old_path == UAVCAN_NULLPTR) + { + // The entry has been removed, assuming that it's not needed anymore + return; + } + + const bool confirmed = + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK || + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + + if (confirmed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request", + int(result.getCallID().server_node_id.get())); + pending_nodes_.remove(result.getCallID().server_node_id); + checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse()); + } + else + { + UAVCAN_ASSERT(old_path != UAVCAN_NULLPTR); + UAVCAN_ASSERT(TimerBase::isRunning()); + // We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer + + const bool update_needed = + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path); + + if (!update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry", + int(result.getCallID().server_node_id.get())); + pending_nodes_.remove(result.getCallID().server_node_id); + } + } + } + + virtual void handleTimerEvent(const TimerEvent&) + { + if (pending_nodes_.isEmpty()) + { + TimerBase::stop(); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped"); + return; + } + + const NodeID node_id = pickNextNodeID(); + if (!node_id.isUnicast()) + { + return; + } + + FirmwareFilePath* const path = pending_nodes_.access(node_id); + if (path == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map + return; + } + + protocol::file::BeginFirmwareUpdate::Request req; + + req.source_node_id = getNode().getNodeID().get(); + req.image_file_remote_path.path = path->c_str(); + + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s", + int(node_id.get()), req.image_file_remote_path.path.c_str()); + + const int call_res = begin_fw_update_client_.call(node_id, req); + if (call_res < 0) + { + getNode().registerInternalFailure("FirmwareUpdateTrigger call"); + } + } + +public: + FirmwareUpdateTrigger(INode& node, IFirmwareVersionChecker& checker) + : TimerBase(node) + , begin_fw_update_client_(node) + , checker_(checker) + , node_info_retriever_(UAVCAN_NULLPTR) + , pending_nodes_(node.getAllocator()) + , request_interval_(MonotonicDuration::fromMSec(DefaultRequestIntervalMs)) + , last_queried_node_id_(0) + { } + + ~FirmwareUpdateTrigger() + { + if (node_info_retriever_ != UAVCAN_NULLPTR) + { + node_info_retriever_->removeListener(this); + } + } + + /** + * Starts the object. Once started, it can't be stopped unless destroyed. + * + * @param node_info_retriever The object will register itself against this retriever. + * When the destructor is called, the object will unregister itself. + * + * @param common_path_prefix If set, this path will be prefixed to all firmware pathes provided by the + * application interface. The prefix does not need to end with path separator; + * the last trailing one will be removed (so use '//' if you need '/'). + * By default the prefix is empty. + * + * @return Negative error code. + */ + int start(NodeInfoRetriever& node_info_retriever, + const FirmwareFilePath& arg_common_path_prefix = FirmwareFilePath(), + const FirmwareFilePath& arg_alt_path_prefix = FirmwareFilePath(), + const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + /* + * Configuring the node info retriever + */ + node_info_retriever_ = &node_info_retriever; + + int res = node_info_retriever_->addListener(this); + if (res < 0) + { + return res; + } + + /* + * Initializing the prefix, removing only the last trailing path separator. + */ + common_path_prefix_ = arg_common_path_prefix; + + if (!common_path_prefix_.empty() && + *(common_path_prefix_.end() - 1) == protocol::file::Path::SEPARATOR) + { + common_path_prefix_.resize(uint8_t(common_path_prefix_.size() - 1U)); + } + + /* + * Initializing the client + */ + res = begin_fw_update_client_.init(priority); + if (res < 0) + { + return res; + } + begin_fw_update_client_.setCallback( + BeginFirmwareUpdateResponseCallback(this, &FirmwareUpdateTrigger::handleBeginFirmwareUpdateResponse)); + + // The timer will be started ad-hoc + return 0; + } + + /** + * Interval at which uavcan.protocol.file.BeginFirmwareUpdate requests are being sent. + * Note that default value should be OK for any use case. + */ + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) + { + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) // Restarting with new interval + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This method is mostly needed for testing. + * When triggering is not in progress, the class consumes zero CPU time. + */ + bool isTimerRunning() const { return TimerBase::isRunning(); } + + unsigned getNumPendingNodes() const + { + const unsigned ret = pending_nodes_.getSize(); + UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); + return ret; + } + +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp new file mode 100644 index 0000000000..e64fb47833 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -0,0 +1,262 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Please read the specs to learn how the time synchronization works. + * + * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen. + * + * NOTE: In order for this class to work, the platform driver must implement + * CAN bus TX loopback with both UTC and monotonic timestamping. + * + * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus" + * + * TODO: Enforce max one master per node + */ +class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase +{ + class IfaceMaster + { + Publisher pub_; + MonotonicTime iface_prev_pub_mono_; + UtcTime prev_tx_utc_; + const uint8_t iface_index_; + + public: + IfaceMaster(INode& node, uint8_t iface_index) + : pub_(node) + , iface_index_(iface_index) + { + UAVCAN_ASSERT(iface_index < MaxCanIfaces); + } + + int init(TransferPriority priority) + { + const int res = pub_.init(priority); + if (res >= 0) + { + pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_)); + pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback); + } + return res; + } + + void setTxTimestamp(UtcTime ts) + { + if (ts.isZero()) + { + UAVCAN_ASSERT(0); + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts"); + return; + } + if (!prev_tx_utc_.isZero()) + { + prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict"); + return; + } + prev_tx_utc_ = ts; + } + + int publish(TransferID tid, MonotonicTime current_time) + { + UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback); + UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_)); + + const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; + iface_prev_pub_mono_ = current_time; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS; + + protocol::GlobalTimeSync msg; + msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec(); + prev_tx_utc_ = UtcTime(); + + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", + static_cast(msg.previous_transmission_timestamp_usec), + int(iface_index_), int(tid.get())); + return pub_.broadcast(msg, tid); + } + }; + + INode& node_; + LazyConstructor iface_masters_[MaxCanIfaces]; + MonotonicTime prev_pub_mono_; + DataTypeID dtid_; + bool initialized_; + + virtual void handleLoopbackFrame(const RxFrame& frame) + { + const uint8_t iface = frame.getIfaceIndex(); + if (initialized_ && iface < MaxCanIfaces) + { + if (frame.getDataTypeID() == dtid_ && + frame.getTransferType() == TransferTypeMessageBroadcast && + frame.isStartOfTransfer() && frame.isEndOfTransfer() && + frame.getSrcNodeID() == node_.getNodeID()) + { + iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp()); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + int getNextTransferID(TransferID& tid) + { + const MonotonicDuration max_transfer_interval = + MonotonicDuration::fromMSec(protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS); + + const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast); + const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval; + TransferID* const tid_ptr = + node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (tid_ptr == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + + tid = *tid_ptr; + tid_ptr->increment(); + return 0; + } + +public: + explicit GlobalTimeSyncMaster(INode& node) + : LoopbackFrameListenerBase(node.getDispatcher()) + , node_(node) + , initialized_(false) + { } + + /** + * Merely prepares the class to work, doesn't do anything else. + * Must be called before the master can be used. + * Returns negative error code. + */ + int init(const TransferPriority priority = TransferPriority::OneLowerThanHighest) + { + if (initialized_) + { + return 0; + } + + // Data type ID + const DataTypeDescriptor* const desc = + GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName()); + if (desc == UAVCAN_NULLPTR) + { + return -ErrUnknownDataType; + } + dtid_ = desc->getID(); + + // Iface master array + int res = -ErrLogic; + for (uint8_t i = 0; i < MaxCanIfaces; i++) + { + if (!iface_masters_[i].isConstructed()) + { + iface_masters_[i].construct(node_, i); + } + res = iface_masters_[i]->init(priority); + if (res < 0) + { + break; + } + } + + // Loopback listener + initialized_ = res >= 0; + if (initialized_) + { + LoopbackFrameListenerBase::startListening(); + } + return res; + } + + /** + * Whether the master instance has been initialized. + */ + bool isInitialized() const { return initialized_; } + + /** + * Publishes one sync message. + * + * Every call to this method hints the master to publish the next sync message once. Exact time will + * be obtained from the TX loopback timestamp field. + * + * This method shall be called with a proper interval - refer to the time sync message definition + * for min/max interval values. + * + * Returns negative error code. + */ + int publish() + { + if (!initialized_) + { + const int res = init(); + if (res < 0) + { + return res; + } + } + + /* + * Enforce max frequency + */ + const MonotonicTime current_time = node_.getMonotonicTime(); + { + const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_BROADCASTING_PERIOD_MS) + { + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped"); + return 0; + } + prev_pub_mono_ = current_time; + } + + /* + * Obtain common Transfer ID for all masters + */ + TransferID tid; + { + const int tid_res = getNextTransferID(tid); + if (tid_res < 0) + { + return tid_res; + } + } + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const int res = iface_masters_[i]->publish(tid, current_time); + if (res < 0) + { + return res; + } + } + return 0; + } +}; + +} + +#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp new file mode 100644 index 0000000000..770cd721ad --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -0,0 +1,198 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Please read the specs to learn how the time synchronization works. + * + * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen. + * + * NOTE: In order for this class to work, the platform driver must implement: + * - CAN bus RX UTC timestamping; + * - Clock adjustment method in the system clock interface @ref ISystemClock::adjustUtc(). + * + * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus" + * http://modecs.cs.uni-salzburg.at/results/related_documents/CAN_clock.pdf + */ +class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable +{ + typedef MethodBinder&)> + GlobalTimeSyncCallback; + + Subscriber sub_; + + UtcTime prev_ts_utc_; + MonotonicTime prev_ts_mono_; + MonotonicTime last_adjustment_ts_; + enum State { Update, Adjust } state_; + NodeID master_nid_; + TransferID prev_tid_; + uint8_t prev_iface_index_; + bool suppressed_; + + ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); } + + void adjustFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0); + const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_; + + UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i", + static_cast(adjustment.toUSec()), + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_)); + + if (!suppressed_) + { + getSystemClock().adjustUtc(adjustment); + } + last_adjustment_ts_ = msg.getMonotonicTimestamp(); + state_ = Update; + } + + void updateFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + + prev_ts_utc_ = msg.getUtcTimestamp(); + prev_ts_mono_ = msg.getMonotonicTimestamp(); + master_nid_ = msg.getSrcNodeID(); + prev_iface_index_ = msg.getIfaceIndex(); + prev_tid_ = msg.getTransferID(); + state_ = Adjust; + } + + void processMsg(const ReceivedDataStructure& msg) + { + const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; + UAVCAN_ASSERT(!since_prev_msg.isNegative()); + + const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); + const bool switch_master = msg.getSrcNodeID() < master_nid_; + // TODO: Make configurable + const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS; + + if (switch_master || pub_timeout || needs_init) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", + int(needs_init), int(switch_master), int(pub_timeout)); + updateFromMsg(msg); + } + else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) + { + if (state_ == Adjust) + { + const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; + const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; + const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS; + if (msg_invalid || wrong_tid || wrong_timing) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", + "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", + int(msg_invalid), int(wrong_tid), int(wrong_timing)); + state_ = Update; + } + } + if (state_ == Adjust) + { + adjustFromMsg(msg); + } + else + { + updateFromMsg(msg); + } + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + } + } + + void handleGlobalTimeSync(const ReceivedDataStructure& msg) + { + if (msg.getTransferType() == TransferTypeMessageBroadcast) + { + processMsg(msg); + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType())); + } + } + +public: + explicit GlobalTimeSyncSlave(INode& node) + : sub_(node) + , state_(Update) + , prev_iface_index_(0xFF) + , suppressed_(false) + { } + + /** + * Starts the time sync slave. Once started, it works on its own and does not require any + * attention from the application, other than to handle a clock adjustment request occasionally. + * Returns negative error code. + */ + int start() + { + return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync)); + } + + /** + * Enable or disable the suppressed mode. + * + * In suppressed mode the slave will continue tracking time sync masters in the network, but will not + * perform local clock adjustments. So it's kind of a dry run - all the time sync logic works except + * the local clock will not receive adjustments. + * + * Suppressed mode is useful for nodes that can act as a back-up clock sync masters - as long as the + * node sees a higher priority time sync master in the network, its slave will be NOT suppressed + * in order to sync the local clock with the global master. As soon as all other higher priority + * masters go down, the local node will suppress its time sync slave instance and become a new master. + * + * Suppressed mode is disabled by default. + */ + void suppress(bool suppressed) { suppressed_ = suppressed; } + bool isSuppressed() const { return suppressed_; } + + /** + * If the clock sync slave sees any clock sync masters in the network, it is ACTIVE. + * When the last master times out (PUBLISHER_TIMEOUT), the slave will be INACTIVE. + * Note that immediately after start up the slave will be INACTIVE until it finds a master. + * Please read the specs to learn more. + */ + bool isActive() const + { + const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_; + return !last_adjustment_ts_.isZero() && + (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS); + } + + /** + * Node ID of the master the slave is currently locked on. + * Returns an invalid Node ID if there's no active master. + */ + NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); } + + /** + * Last time when the local clock adjustment was performed. + */ + MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/logger.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/logger.hpp new file mode 100644 index 0000000000..07a6ba6a56 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/logger.hpp @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * External log sink interface. + * External log sink allows the application to install a hook on the logger output. + * This can be used for application-wide logging. + * Please refer to the @ref Logger class docs. + */ +class UAVCAN_EXPORT ILogSink +{ +public: + typedef typename StorageType::Type LogLevel; + + virtual ~ILogSink() { } + + /** + * Logger will not sink messages with a severity level lower than returned by this method. + * Default level is DEBUG. + */ + virtual LogLevel getLogLevel() const { return protocol::debug::LogLevel::DEBUG; } + + /** + * Logger will call this method for every log message which severity level + * is not less than the current level of this sink. + */ + virtual void log(const protocol::debug::LogMessage& message) = 0; +}; + +/** + * Node logging convenience class. + * + * This class is based on the standard UAVCAN message type for logging - uavcan.protocol.debug.LogMessage. + * + * Provides logging methods of different severity; implements two sinks for the log messages: + * - Broadcast via the UAVCAN bus; + * - Sink into the application via @ref ILogSink. + * + * For each sink an individual severity threshold filter can be configured. + */ +class UAVCAN_EXPORT Logger +{ +public: + typedef ILogSink::LogLevel LogLevel; + + /** + * This value is higher than any valid severity value. + * Use it to completely suppress the output. + */ + static LogLevel getLogLevelAboveAll() { return (1U << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1U; } + +private: + enum { DefaultTxTimeoutMs = 2000 }; + + Publisher logmsg_pub_; + protocol::debug::LogMessage msg_buf_; + LogLevel level_; + ILogSink* external_sink_; + + LogLevel getExternalSinkLevel() const + { + return (external_sink_ == UAVCAN_NULLPTR) ? getLogLevelAboveAll() : external_sink_->getLogLevel(); + } + +public: + explicit Logger(INode& node) + : logmsg_pub_(node) + , external_sink_(UAVCAN_NULLPTR) + { + level_ = protocol::debug::LogLevel::ERROR; + setTxTimeout(MonotonicDuration::fromMSec(DefaultTxTimeoutMs)); + UAVCAN_ASSERT(getTxTimeout() == MonotonicDuration::fromMSec(DefaultTxTimeoutMs)); + } + + /** + * Initializes the logger, does not perform any network activity. + * Must be called once before use. + * Returns negative error code. + */ + int init(const TransferPriority priority = TransferPriority::Lowest) + { + const int res = logmsg_pub_.init(priority); + if (res < 0) + { + return res; + } + return 0; + } + + /** + * Logs one message. Please consider using helper methods instead of this one. + * + * The message will be broadcasted via the UAVCAN bus if the severity level of the + * message is >= severity level of the logger. + * + * The message will be reported into the external log sink if the external sink is + * installed and the severity level of the message is >= severity level of the external sink. + * + * Returns negative error code. + */ + int log(const protocol::debug::LogMessage& message) + { + int retval = 0; + if (message.level.value >= getExternalSinkLevel()) + { + external_sink_->log(message); + } + if (message.level.value >= level_) + { + retval = logmsg_pub_.broadcast(message); + } + return retval; + } + + /** + * Severity filter for UAVCAN broadcasting. + * Log message will be broadcasted via the UAVCAN network only if its severity is >= getLevel(). + * This does not affect the external sink. + * Default level is ERROR. + */ + LogLevel getLevel() const { return level_; } + void setLevel(LogLevel level) { level_ = level; } + + /** + * External log sink allows the application to install a hook on the logger output. + * This can be used for application-wide logging. + * Null pointer means that there's no log sink (can be used to remove it). + * By default there's no log sink. + */ + ILogSink* getExternalSink() const { return external_sink_; } + void setExternalSink(ILogSink* sink) { external_sink_ = sink; } + + /** + * Log message broadcast transmission timeout. + * The default value should be acceptable for any use case. + */ + MonotonicDuration getTxTimeout() const { return logmsg_pub_.getTxTimeout(); } + void setTxTimeout(MonotonicDuration val) { logmsg_pub_.setTxTimeout(val); } + + /** + * Helper methods for various severity levels and with formatting support. + * These methods build a formatted log message and pass it into the method @ref log(). + * + * Format string usage is a bit unusual: use "%*" for any argument type, use "%%" to print a percent character. + * No other formating options are supported. Insufficient/extra arguments are ignored. + * + * Example format string: + * "What do you get if you %* %* by %*? %*. Extra arguments: %* %* %%" + * ...with the following arguments: + * "multiply", 6, 9.0F 4.2e1 + * ...will likely produce this (floating point representation is platform dependent): + * "What do you get if you multiply 6 by 9.000000? 42.000000. Extra arguments: %* %* %" + * + * Formatting is not supported in C++03 mode. + * @{ + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + + template + int log(LogLevel level, const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT; + + template + inline int logDebug(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::DEBUG, source, format, args...); + } + + template + inline int logInfo(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::INFO, source, format, args...); + } + + template + inline int logWarning(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::WARNING, source, format, args...); + } + + template + inline int logError(const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::ERROR, source, format, args...); + } + +#else + + int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT + { + #if UAVCAN_EXCEPTIONS + try + #endif + { + if (level >= level_ || level >= getExternalSinkLevel()) + { + msg_buf_.level.value = level; + msg_buf_.source = source; + msg_buf_.text = text; + return log(msg_buf_); + } + return 0; + } + #if UAVCAN_EXCEPTIONS + catch (...) + { + return -ErrFailure; + } + #endif + } + + int logDebug(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::DEBUG, source, text); + } + + int logInfo(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::INFO, source, text); + } + + int logWarning(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::WARNING, source, text); + } + + int logError(const char* source, const char* text) UAVCAN_NOEXCEPT + { + return log(protocol::debug::LogLevel::ERROR, source, text); + } + +#endif + /** + * @} + */ +}; + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +int Logger::log(LogLevel level, const char* source, const char* format, Args... args) UAVCAN_NOEXCEPT +{ +#if UAVCAN_EXCEPTIONS + try +#endif + { + if (level >= level_ || level >= getExternalSinkLevel()) + { + msg_buf_.level.value = level; + msg_buf_.source = source; + msg_buf_.text.clear(); + CharArrayFormatter formatter(msg_buf_.text); + formatter.write(format, args...); + return log(msg_buf_); + } + return 0; + } +#if UAVCAN_EXCEPTIONS + catch (...) + { + return -ErrFailure; + } +#endif +} + +#endif + +} + +#endif // UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_info_retriever.hpp new file mode 100644 index 0000000000..a1549f763c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -0,0 +1,464 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Classes that need to receive GetNodeInfo responses should implement this interface. + */ +class UAVCAN_EXPORT INodeInfoListener +{ +public: + /** + * Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or + * becomes online for the first time. + * @param node_id Node ID of the node + * @param response Node info struct + */ + virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0; + + /** + * Called when the retriever decides that the node does not support the GetNodeInfo service. + * This method will never be called if the number of attempts is unlimited. + */ + virtual void handleNodeInfoUnavailable(NodeID node_id) = 0; + + /** + * This call is routed directly from @ref NodeStatusMonitor. + * Default implementation does nothing. + * @param event Node status change event + */ + virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event) + { + (void)event; + } + + /** + * This call is routed directly from @ref NodeStatusMonitor. + * Default implementation does nothing. + * @param msg Node status message + */ + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + (void)msg; + } + + virtual ~INodeInfoListener() { } +}; + +/** + * This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts. + * It does a number of attempts in case if there's a communication failure before assuming that the node does not + * implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit + * virtually any use case, but they can be overriden if needed - refer to the setter methods below for details. + * + * Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local + * node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo + * service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are + * unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate + * for the end application, the request interval can be overriden via @ref setRequestInterval(). + * + * Following the above explained requirements, the default request interval is defined as follows: + * request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes) + * Which yields 40 ms. + * + * Given default service timeout 1000 ms and the defined above request frequency 40 ms, the maximum number of + * concurrent requests will be: + * max concurrent requests = ceil(1000 [ms] timeout / 40 [ms] request interval) + * Which yields 25 requests. + * + * Keep the above equations in mind when changing the default request interval. + * + * Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never + * exceed one. This is actually the most likely scenario. + * + * Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc. + * + * Events from this class can be routed to many listeners, @ref INodeInfoListener. + */ +class UAVCAN_EXPORT NodeInfoRetriever : public NodeStatusMonitor + , TimerBase +{ +public: + enum { MaxNumRequestAttempts = 254 }; + enum { UnlimitedRequestAttempts = 0 }; + +private: + typedef MethodBinder&)> + GetNodeInfoResponseCallback; + + struct Entry + { + uint32_t uptime_sec; + uint8_t num_attempts_made; + bool request_needed; ///< Always false for unknown nodes + bool updated_since_last_attempt; ///< Always false for unknown nodes + + Entry() + : uptime_sec(0) + , num_attempts_made(0) + , request_needed(false) + , updated_since_last_attempt(false) + { +#if UAVCAN_DEBUG + StaticAssert::check(); +#endif + } + }; + + struct NodeInfoRetrievedHandlerCaller + { + const NodeID node_id; + const protocol::GetNodeInfo::Response& node_info; + + NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info) + : node_id(arg_node_id) + , node_info(arg_node_info) + { } + + bool operator()(INodeInfoListener* key) + { + UAVCAN_ASSERT(key != UAVCAN_NULLPTR); + key->handleNodeInfoRetrieved(node_id, node_info); + return false; + } + }; + + template + struct GenericHandlerCaller + { + void (INodeInfoListener::* const method)(Event); + Event event; + + GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event) + : method(arg_method) + , event(arg_event) + { } + + bool operator()(INodeInfoListener* key) + { + UAVCAN_ASSERT(key != UAVCAN_NULLPTR); + (key->*method)(event); + return false; + } + }; + + enum { DefaultNumRequestAttempts = 16 }; + enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation + + /* + * State + */ + Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Multiset listeners_; + + ServiceClient get_node_info_client_; + + MonotonicDuration request_interval_; + + mutable uint8_t last_picked_node_; + + uint8_t num_attempts_; + + /* + * Methods + */ + const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } + Entry& getEntry(NodeID node_id) + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeInfoRetriever NodeID"); + } + return entries_[node_id.get() - 1]; + } + + void startTimerIfNotRunning() + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str()); + } + } + + NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const + { + out_at_least_one_request_needed = false; + + for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin + { + last_picked_node_++; + if (last_picked_node_ > NodeID::Max) + { + last_picked_node_ = 1; + } + UAVCAN_ASSERT((last_picked_node_ >= 1) && + (last_picked_node_ <= NodeID::Max)); + + const Entry& entry = getEntry(last_picked_node_); + + if (entry.request_needed) + { + out_at_least_one_request_needed = true; + + if (entry.updated_since_last_attempt && + !get_node_info_client_.hasPendingCallToServer(last_picked_node_)) + { + UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_)); + return NodeID(last_picked_node_); + } + } + } + + return NodeID(); // No node could be found + } + + virtual void handleTimerEvent(const TimerEvent&) + { + bool at_least_one_request_needed = false; + const NodeID next = pickNextNodeToQuery(at_least_one_request_needed); + + if (next.isUnicast()) + { + UAVCAN_ASSERT(at_least_one_request_needed); + getEntry(next).updated_since_last_attempt = false; + const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request()); + if (res < 0) + { + get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call"); + } + } + else + { + if (!at_least_one_request_needed) + { + TimerBase::stop(); + UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); + } + } + } + + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) + { + const bool was_offline = !event.was_known || + (event.old_status.mode == protocol::NodeStatus::MODE_OFFLINE); + + const bool offline_now = event.status.mode == protocol::NodeStatus::MODE_OFFLINE; + + if (was_offline || offline_now) + { + Entry& entry = getEntry(event.node_id); + + entry.request_needed = !offline_now; + entry.num_attempts_made = 0; + + UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d", + int(event.node_id.get()), int(entry.request_needed)); + + if (entry.request_needed) + { + startTimerIfNotRunning(); + } + } + + listeners_.forEach( + GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); + } + + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + Entry& entry = getEntry(msg.getSrcNodeID()); + + if (msg.uptime_sec < entry.uptime_sec) + { + entry.request_needed = true; + entry.num_attempts_made = 0; + + startTimerIfNotRunning(); + } + entry.uptime_sec = msg.uptime_sec; + entry.updated_since_last_attempt = true; + + listeners_.forEach(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + Entry& entry = getEntry(result.getCallID().server_node_id); + + if (result.isSuccessful()) + { + /* + * Updating the uptime here allows to properly handle a corner case where the service response arrives + * after the device has restarted and published its new NodeStatus (although it's unlikely to happen). + */ + entry.uptime_sec = result.getResponse().status.uptime_sec; + entry.request_needed = false; + listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); + } + else + { + if (num_attempts_ != UnlimitedRequestAttempts) + { + entry.num_attempts_made++; + if (entry.num_attempts_made >= num_attempts_) + { + entry.request_needed = false; + listeners_.forEach(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.getCallID().server_node_id)); + } + } + } + } + +public: + NodeInfoRetriever(INode& node) + : NodeStatusMonitor(node) + , TimerBase(node) + , listeners_(node.getAllocator()) + , get_node_info_client_(node) + , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec)) + , last_picked_node_(1) + , num_attempts_(DefaultNumRequestAttempts) + { } + + /** + * Starts the retriever. + * Destroy the object to stop it. + * Returns negative error code. + */ + int start(const TransferPriority priority = TransferPriority::OneHigherThanLowest) + { + int res = NodeStatusMonitor::start(); + if (res < 0) + { + return res; + } + + res = get_node_info_client_.init(priority); + if (res < 0) + { + return res; + } + get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this, + &NodeInfoRetriever::handleGetNodeInfoResponse)); + // Note: the timer will be started ad-hoc + return 0; + } + + /** + * This method forces the class to re-request uavcan.protocol.GetNodeInfo from all nodes as if they + * have just appeared in the network. + */ + void invalidateAll() + { + NodeStatusMonitor::forgetAllNodes(); + get_node_info_client_.cancelAllCalls(); + + for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++) + { + entries_[i] = Entry(); + } + // It is not necessary to reset the last picked node index + } + + /** + * Adds one listener. Does nothing if such listener already exists. + * May return -ErrMemory if there's no space to add the listener. + */ + int addListener(INodeInfoListener* listener) + { + if (listener != UAVCAN_NULLPTR) + { + removeListener(listener); + return (UAVCAN_NULLPTR == listeners_.emplace(listener)) ? -ErrMemory : 0; + } + else + { + return -ErrInvalidParam; + } + } + + /** + * Removes the listener. + * If the listener was not registered, nothing will be done. + */ + void removeListener(INodeInfoListener* listener) + { + if (listener != UAVCAN_NULLPTR) + { + listeners_.removeAll(listener); + } + else + { + UAVCAN_ASSERT(0); + } + } + + unsigned getNumListeners() const { return listeners_.getSize(); } + + /** + * Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is + * not implemented. + * Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts. + */ + uint8_t getNumRequestAttempts() const { return num_attempts_; } + void setNumRequestAttempts(const uint8_t num) + { + num_attempts_ = min(static_cast(MaxNumRequestAttempts), num); + } + + /** + * Request interval also implicitly defines the maximum number of concurrent requests. + * Read the class documentation for details. + */ + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) + { + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * These methods are needed mostly for testing. + */ + bool isRetrievingInProgress() const { return TimerBase::isRunning(); } + + uint8_t getNumPendingRequests() const + { + const unsigned num = get_node_info_client_.getNumPendingCalls(); + UAVCAN_ASSERT(num <= 0xFF); + return static_cast(num); + } +}; + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp new file mode 100644 index 0000000000..c915ec6d10 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -0,0 +1,320 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements the core functionality of a network monitor. + * It can be extended by inheritance to add more complex logic, or used directly as is. + */ +class UAVCAN_EXPORT NodeStatusMonitor +{ +public: + struct NodeStatus + { + uint8_t health : 2; + uint8_t mode : 3; + uint8_t sub_mode : 3; + + NodeStatus() : + health(protocol::NodeStatus::HEALTH_CRITICAL), + mode(protocol::NodeStatus::MODE_OFFLINE), + sub_mode(0) + { + StaticAssert::check(); + StaticAssert::check(); + StaticAssert::check(); + } + + bool operator!=(const NodeStatus rhs) const { return !operator==(rhs); } + bool operator==(const NodeStatus rhs) const + { + return std::memcmp(this, &rhs, sizeof(NodeStatus)) == 0; + } + +#if UAVCAN_TOSTRING + std::string toString() const + { + char buf[40]; + (void)snprintf(buf, sizeof(buf), "health=%d mode=%d sub_mode=%d", int(health), int(mode), int(sub_mode)); + return std::string(buf); + } +#endif + }; + + struct NodeStatusChangeEvent + { + NodeID node_id; + NodeStatus status; + NodeStatus old_status; + bool was_known; + + NodeStatusChangeEvent() : + was_known(false) + { } + }; + +private: + enum { TimerPeriodMs100 = 2 }; + + typedef MethodBinder&)> + NodeStatusCallback; + + typedef MethodBinder TimerCallback; + + Subscriber sub_; + + TimerEventForwarder timer_; + + struct Entry + { + NodeStatus status; + int8_t time_since_last_update_ms100; + Entry() : + time_since_last_update_ms100(-1) + { } + }; + + mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Entry& getEntry(NodeID node_id) const + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeStatusMonitor NodeID"); + } + return entries_[node_id.get() - 1]; + } + + void changeNodeStatus(const NodeID node_id, const Entry new_entry_value) + { + Entry& entry = getEntry(node_id); + if (entry.status != new_entry_value.status) + { + NodeStatusChangeEvent event; + event.node_id = node_id; + event.old_status = entry.status; + event.status = new_entry_value.status; + event.was_known = entry.time_since_last_update_ms100 >= 0; + + UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()), + (event.was_known ? "known" : "new"), + event.old_status.toString().c_str(), event.status.toString().c_str()); + + handleNodeStatusChange(event); + } + entry = new_entry_value; + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + Entry new_entry; + new_entry.time_since_last_update_ms100 = 0; + new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1); + new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1); + new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1); + + changeNodeStatus(msg.getSrcNodeID(), new_entry); + + handleNodeStatusMessage(msg); + } + + void handleTimerEvent(const TimerEvent&) + { + const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + Entry& entry = getEntry(i); + if (entry.time_since_last_update_ms100 >= 0 && + entry.status.mode != protocol::NodeStatus::MODE_OFFLINE) + { + entry.time_since_last_update_ms100 = + int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100)); + + if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100) + { + Entry new_entry_value = entry; + new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; + new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE; + changeNodeStatus(i, new_entry_value); + } + } + } + } + +protected: + /** + * Called when a node becomes online, changes status or goes offline. + * Refer to uavcan.protocol.NodeStatus for the offline timeout value. + * Overriding is not required. + */ + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) + { + (void)event; + } + + /** + * Called for every received message uavcan.protocol.NodeStatus after handleNodeStatusChange(), even + * if the status code did not change. + * Overriding is not required. + */ + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + (void)msg; + } + +public: + explicit NodeStatusMonitor(INode& node) + : sub_(node) + , timer_(node) + { } + + virtual ~NodeStatusMonitor() { } + + /** + * Starts the monitor. + * Destroy the object to stop it. + * Returns negative error code. + */ + int start() + { + const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); + if (res >= 0) + { + timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent)); + timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); + } + return res; + } + + /** + * Make the node unknown. + */ + void forgetNode(NodeID node_id) + { + if (node_id.isValid()) + { + Entry& entry = getEntry(node_id); + entry = Entry(); + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * Make all nodes unknown. + */ + void forgetAllNodes() + { + for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++) + { + entries_[i] = Entry(); + } + } + + /** + * Returns status of a given node. + * Unknown nodes are considered offline. + * Complexity O(1). + */ + NodeStatus getNodeStatus(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return NodeStatus(); + } + + const Entry& entry = getEntry(node_id); + if (entry.time_since_last_update_ms100 >= 0) + { + return entry.status; + } + else + { + return NodeStatus(); + } + } + + /** + * Whether the class has observed this node at least once since initialization. + * Complexity O(1). + */ + bool isNodeKnown(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return false; + } + return getEntry(node_id).time_since_last_update_ms100 >= 0; + } + + /** + * This helper method allows to quickly estimate the overall network health. + * Health of the local node is not considered. + * Returns an invalid Node ID value if there's no known nodes in the network. + */ + NodeID findNodeWithWorstHealth() const + { + NodeID nid_with_worst_health; + uint8_t worst_health = protocol::NodeStatus::HEALTH_OK; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + const Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0) + { + if (entry.status.health > worst_health || !nid_with_worst_health.isValid()) + { + nid_with_worst_health = nid; + worst_health = entry.status.health; + } + } + } + + return nid_with_worst_health; + } + + /** + * Calls the operator for every known node. + * Operator signature: + * void (NodeID, NodeStatus) + */ + template + void forEachNode(Operator op) const + { + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + const Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0) + { + op(nid, entry.status); + } + } + } +}; + +} + +#endif // UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_provider.hpp new file mode 100644 index 0000000000..b39319f080 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/node_status_provider.hpp @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This optional interface can be implemented by the user in order to update the node status as necessary, + * immediately before the next NodeStatus message is emitted by @ref NodeStatusProvider. + */ +class IAdHocNodeStatusUpdater +{ +public: + /** + * This method is invoked by the library from @ref NodeStatusProvider from the library's thread immediately + * before the next NodeStatus message is transmitted. The application can implement this method to perform + * node status updates only as necessary. + * The application is expected to invoke the methods of @ref NodeStatusProvider to update the status + * from this method. + * Note that this method is only invoked when publication is happening by the timer event. + * It will NOT be invoked if the following methods are used to trigger node status publication: + * - @ref NodeStatusProvider::startAndPublish() + * - @ref NodeStatusProvider::forcePublish() + */ + virtual void updateNodeStatus() = 0; + + virtual ~IAdHocNodeStatusUpdater() { } +}; + +/** + * Provides the status and basic information about this node to other network participants. + * + * Usually the application does not need to deal with this class directly - it's instantiated by the node class. + * + * Default values: + * - health - OK + * - mode - INITIALIZATION + */ +class UAVCAN_EXPORT NodeStatusProvider : private TimerBase +{ + typedef MethodBinder GetNodeInfoCallback; + + const MonotonicTime creation_timestamp_; + + Publisher node_status_pub_; + ServiceServer gni_srv_; + + protocol::GetNodeInfo::Response node_info_; + + IAdHocNodeStatusUpdater* ad_hoc_status_updater_; + + INode& getNode() { return node_status_pub_.getNode(); } + + bool isNodeInfoInitialized() const; + + int publish(); + + virtual void handleTimerEvent(const TimerEvent&) override; + void handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, protocol::GetNodeInfo::Response& rsp); + +public: + typedef typename StorageType::Type + VendorSpecificStatusCode; + + typedef typename StorageType::Type NodeName; + + explicit NodeStatusProvider(INode& node) + : TimerBase(node) + , creation_timestamp_(node.getMonotonicTime()) + , node_status_pub_(node) + , gni_srv_(node) + , ad_hoc_status_updater_(UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(!creation_timestamp_.isZero()); + + node_info_.status.mode = protocol::NodeStatus::MODE_INITIALIZATION; + + node_info_.status.health = protocol::NodeStatus::HEALTH_OK; + } + + /** + * Starts the provider and immediately broadcasts uavcan.protocol.NodeStatus. + * Returns negative error code. + */ + int startAndPublish(const TransferPriority priority = TransferPriority::Default); + + /** + * Publish the message uavcan.protocol.NodeStatus right now, out of schedule. + * Returns negative error code. + */ + int forcePublish() { return publish(); } + + /** + * Allows to override default publishing rate for uavcan.protocol.NodeStatus. + * Refer to the DSDL definition of uavcan.protocol.NodeStatus to see what is the default rate. + * Doesn't fail; if the value is outside of acceptable range, a closest valid value will be used instead. + */ + void setStatusPublicationPeriod(uavcan::MonotonicDuration period); + uavcan::MonotonicDuration getStatusPublicationPeriod() const; + + /** + * Configure the optional handler that is invoked before every node status message is emitted. + * By default no handler is installed. + * It is allowed to pass a null pointer, that will disable the ad-hoc update feature. + * @ref IAdHocNodeStatusUpdater + */ + void setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater); + IAdHocNodeStatusUpdater* getAdHocNodeStatusUpdater() const { return ad_hoc_status_updater_; } + + /** + * Local node health code control. + */ + uint8_t getHealth() const { return node_info_.status.health; } + void setHealth(uint8_t code); + void setHealthOk() { setHealth(protocol::NodeStatus::HEALTH_OK); } + void setHealthWarning() { setHealth(protocol::NodeStatus::HEALTH_WARNING); } + void setHealthError() { setHealth(protocol::NodeStatus::HEALTH_ERROR); } + void setHealthCritical() { setHealth(protocol::NodeStatus::HEALTH_CRITICAL); } + + /** + * Local node mode code control. + */ + uint8_t getMode() const { return node_info_.status.mode; } + void setMode(uint8_t code); + void setModeOperational() { setMode(protocol::NodeStatus::MODE_OPERATIONAL); } + void setModeInitialization() { setMode(protocol::NodeStatus::MODE_INITIALIZATION); } + void setModeMaintenance() { setMode(protocol::NodeStatus::MODE_MAINTENANCE); } + void setModeSoftwareUpdate() { setMode(protocol::NodeStatus::MODE_SOFTWARE_UPDATE); } + void setModeOffline() { setMode(protocol::NodeStatus::MODE_OFFLINE); } + + /** + * Local node vendor-specific status code control. + */ + void setVendorSpecificStatusCode(VendorSpecificStatusCode code); + VendorSpecificStatusCode getVendorSpecificStatusCode() const + { + return node_info_.status.vendor_specific_status_code; + } + + /** + * Local node name control. + * Can be set only once before the provider is started. + * The provider will refuse to start if the node name is not set. + */ + const NodeName& getName() const { return node_info_.name; } + void setName(const NodeName& name); + + /** + * Node version information. + * Can be set only once before the provider is started. + */ + const protocol::SoftwareVersion& getSoftwareVersion() const { return node_info_.software_version; } + const protocol::HardwareVersion& getHardwareVersion() const { return node_info_.hardware_version; } + void setSoftwareVersion(const protocol::SoftwareVersion& version); + void setHardwareVersion(const protocol::HardwareVersion& version); +}; + +} + +#endif // UAVCAN_PROTOCOL_NODE_STATUS_PROVIDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp new file mode 100644 index 0000000000..28c9b32c1d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Helper for broadcasting the message uavcan.protocol.Panic. + */ +class UAVCAN_EXPORT PanicBroadcaster : private TimerBase +{ + Publisher pub_; + protocol::Panic msg_; + + void publishOnce() + { + const int res = pub_.broadcast(msg_); + if (res < 0) + { + pub_.getNode().registerInternalFailure("Panic pub failed"); + } + } + + virtual void handleTimerEvent(const TimerEvent&) + { + publishOnce(); + } + +public: + explicit PanicBroadcaster(INode& node) + : TimerBase(node) + , pub_(node) + { } + + /** + * This method does not block and can't fail. + * @param short_reason Short ASCII string that describes the reason of the panic, 7 characters max. + * If the string exceeds 7 characters, it will be truncated. + * @param broadcasting_period Broadcasting period. Optional. + * @param priority Transfer priority. Optional. + */ + void panic(const char* short_reason_description, + MonotonicDuration broadcasting_period = MonotonicDuration::fromMSec(100), + const TransferPriority priority = TransferPriority::Default) + { + msg_.reason_text.clear(); + const char* p = short_reason_description; + while (p && *p) + { + if (msg_.reason_text.size() == msg_.reason_text.capacity()) + { + break; + } + msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p)); + p++; + } + + UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str()); + + pub_.setTxTimeout(broadcasting_period); + pub_.setPriority(priority); + + publishOnce(); + startPeriodic(broadcasting_period); + } + + /** + * Stop broadcasting immediately. + */ + void dontPanic() // Where's my towel + { + stop(); + msg_.reason_text.clear(); + } + + bool isPanicking() const { return isRunning(); } + + const typename protocol::Panic::FieldTypes::reason_text& getReason() const + { + return msg_.reason_text; + } +}; + +} + +#endif // UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_listener.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_listener.hpp new file mode 100644 index 0000000000..69a0e71bd7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/panic_listener.hpp @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +/** + * This class implements proper panic detector. + * Refer to uavcan.protocol.Panic for details. + * The listener can be stopped from the callback. + * + * @tparam Callback Possible callback prototypes: + * void (const ReceivedDataStructure&) + * void (const protocol::Panic&) + */ +template < +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + typename Callback = std::function&)> +#else + typename Callback = void (*)(const ReceivedDataStructure&) +#endif + > +class UAVCAN_EXPORT PanicListener : Noncopyable +{ + typedef MethodBinder&)> + PanicMsgCallback; + + Subscriber sub_; + MonotonicTime prev_msg_timestamp_; + Callback callback_; + uint8_t num_subsequent_msgs_; + + void invokeCallback(const ReceivedDataStructure& msg) + { + if (coerceOrFallback(callback_, true)) + { + callback_(msg); + } + else + { + UAVCAN_ASSERT(0); // This is a logic error because normally we shouldn't start with an invalid callback + sub_.getNode().registerInternalFailure("PanicListener invalid callback"); + } + } + + void handleMsg(const ReceivedDataStructure& msg) + { + UAVCAN_TRACE("PanicListener", "Received panic from snid=%i reason=%s", + int(msg.getSrcNodeID().get()), msg.reason_text.c_str()); + if (prev_msg_timestamp_.isZero()) + { + num_subsequent_msgs_ = 1; + prev_msg_timestamp_ = msg.getMonotonicTimestamp(); + } + else + { + const MonotonicDuration diff = msg.getMonotonicTimestamp() - prev_msg_timestamp_; + UAVCAN_ASSERT(diff.isPositive() || diff.isZero()); + if (diff.toMSec() > protocol::Panic::MAX_INTERVAL_MS) + { + num_subsequent_msgs_ = 1; + } + else + { + num_subsequent_msgs_++; + } + prev_msg_timestamp_ = msg.getMonotonicTimestamp(); + if (num_subsequent_msgs_ >= protocol::Panic::MIN_MESSAGES) + { + num_subsequent_msgs_ = protocol::Panic::MIN_MESSAGES; + invokeCallback(msg); // The application can stop us from the callback + } + } + } + +public: + explicit PanicListener(INode& node) + : sub_(node) + , callback_() + , num_subsequent_msgs_(0) + { } + + /** + * Start the listener. + * Once started it does not require further attention. + * Returns negative error code. + */ + int start(const Callback& callback) + { + stop(); + if (!coerceOrFallback(callback, true)) + { + UAVCAN_TRACE("PanicListener", "Invalid callback"); + return -ErrInvalidParam; + } + callback_ = callback; + return sub_.start(PanicMsgCallback(this, &PanicListener::handleMsg)); + } + + void stop() + { + sub_.stop(); + num_subsequent_msgs_ = 0; + prev_msg_timestamp_ = MonotonicTime(); + } +}; + +} + +#endif // UAVCAN_PROTOCOL_PANIC_LISTENER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/param_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/param_server.hpp new file mode 100644 index 0000000000..d3a6651ffb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/param_server.hpp @@ -0,0 +1,192 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Implement this interface in the application to support the standard remote reconfiguration services. + * Refer to @ref ParamServer. + */ +class UAVCAN_EXPORT IParamManager +{ +public: + typedef typename StorageType::Type Name; + typedef typename StorageType::Type Index; + typedef protocol::param::Value Value; + typedef protocol::param::NumericValue NumericValue; + + virtual ~IParamManager() { } + + /** + * Copy the parameter name to @ref out_name if it exists, otherwise do nothing. + */ + virtual void getParamNameByIndex(Index index, Name& out_name) const = 0; + + /** + * Assign by name if exists. + */ + virtual void assignParamValue(const Name& name, const Value& value) = 0; + + /** + * Read by name if exists, otherwise do nothing. + */ + virtual void readParamValue(const Name& name, Value& out_value) const = 0; + + /** + * Read param's default/max/min if available. + * Note that min/max are only applicable to numeric params. + * Implementation is optional. + */ + virtual void readParamDefaultMaxMin(const Name& name, Value& out_default, + NumericValue& out_max, NumericValue& out_min) const + { + (void)name; + (void)out_default; + (void)out_max; + (void)out_min; + } + + /** + * Save all params to non-volatile storage. + * @return Negative if failed. + */ + virtual int saveAllParams() = 0; + + /** + * Clear the non-volatile storage. + * @return Negative if failed. + */ + virtual int eraseAllParams() = 0; +}; + +/** + * Convenience class for supporting the standard configuration services. + * Highly recommended to use. + */ +class UAVCAN_EXPORT ParamServer +{ + typedef MethodBinder GetSetCallback; + + typedef MethodBinder ExecuteOpcodeCallback; + + ServiceServer get_set_srv_; + ServiceServer save_erase_srv_; + IParamManager* manager_; + + void handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out) + { + UAVCAN_ASSERT(manager_ != UAVCAN_NULLPTR); + + // Recover the name from index + if (in.name.empty()) + { + manager_->getParamNameByIndex(in.index, out.name); + UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str()); + } + else + { + out.name = in.name; + } + + if (out.name.empty()) + { + UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index)); + return; + } + + // Assign if needed, read back + if (!in.value.is(protocol::param::Value::Tag::empty)) + { + manager_->assignParamValue(out.name, in.value); + } + manager_->readParamValue(out.name, out.value); + + // Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about + if (!out.value.is(protocol::param::Value::Tag::empty)) + { + manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value); + } + else + { + UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str()); + out.name.clear(); + } + } + + void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in, + protocol::param::ExecuteOpcode::Response& out) + { + UAVCAN_ASSERT(manager_ != UAVCAN_NULLPTR); + + if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE) + { + out.ok = manager_->saveAllParams() >= 0; + } + else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE) + { + out.ok = manager_->eraseAllParams() >= 0; + } + else + { + UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode)); + out.ok = false; + } + } + +public: + explicit ParamServer(INode& node) + : get_set_srv_(node) + , save_erase_srv_(node) + , manager_(UAVCAN_NULLPTR) + { } + + /** + * Starts the parameter server with given param manager instance. + * Returns negative error code. + */ + int start(IParamManager* manager) + { + if (manager == UAVCAN_NULLPTR) + { + return -ErrInvalidParam; + } + manager_ = manager; + + int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet)); + if (res < 0) + { + return res; + } + + res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode)); + if (res < 0) + { + get_set_srv_.stop(); + } + return res; + } + + /** + * @ref IParamManager + */ + IParamManager* getParamManager() const { return manager_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/restart_request_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/restart_request_server.hpp new file mode 100644 index 0000000000..62031fe983 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/restart_request_server.hpp @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Implement this interface in the application to support the standard node restart service. + */ +class UAVCAN_EXPORT IRestartRequestHandler +{ +public: + virtual ~IRestartRequestHandler() { } + + /** + * This method shall do either: + * - restart the local node immediately; + * - initiate the restart procedure to complete it asynchronously; + * - reject the restart request and return false. + * + * If the restart requets was accepted, this method shall either return true or don't return at all. + */ + virtual bool handleRestartRequest(NodeID request_source) = 0; +}; + +/** + * Convenience class for supporting the standard node restart service. + * Highly recommended to use. + */ +class UAVCAN_EXPORT RestartRequestServer : Noncopyable +{ + typedef MethodBinder&, + protocol::RestartNode::Response&) const> RestartNodeCallback; + + ServiceServer srv_; + IRestartRequestHandler* handler_; + + void handleRestartNode(const ReceivedDataStructure& request, + protocol::RestartNode::Response& response) const + { + UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get())); + response.ok = false; + if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER) + { + if (handler_) + { + response.ok = handler_->handleRestartRequest(request.getSrcNodeID()); + } + UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected")); + } + else + { + UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx", + static_cast(request.magic_number)); + } + } + +public: + explicit RestartRequestServer(INode& node) + : srv_(node) + , handler_(UAVCAN_NULLPTR) + { } + + /** + * Restart request handler configuration. + * All restart requests will be explicitly rejected if there's no handler installed. + */ + IRestartRequestHandler* getHandler() const { return handler_; } + void setHandler(IRestartRequestHandler* handler) { handler_ = handler; } + + /** + * Starts the server. + * Returns negative error code. + */ + int start() + { + return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode)); + } +}; + +} + +#endif // UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp new file mode 100644 index 0000000000..34f8aaa0b0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class provides statistics about the transport layer performance on the local node. + * The user's application does not deal with this class directly because it's instantiated by the node class. + */ +class UAVCAN_EXPORT TransportStatsProvider : Noncopyable +{ + typedef MethodBinder + GetTransportStatsCallback; + + ServiceServer srv_; + + void handleGetTransportStats(const protocol::GetTransportStats::Request&, + protocol::GetTransportStats::Response& resp) const + { + const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter(); + resp.transfer_errors = perf.getErrorCount(); + resp.transfers_tx = perf.getTxTransferCount(); + resp.transfers_rx = perf.getRxTransferCount(); + + const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager(); + for (uint8_t i = 0; i < canio.getNumIfaces(); i++) + { + const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i); + protocol::CANIfaceStats stats; + stats.errors = can_perf.errors; + stats.frames_tx = can_perf.frames_tx; + stats.frames_rx = can_perf.frames_rx; + resp.can_iface_stats.push_back(stats); + } + } + +public: + explicit TransportStatsProvider(INode& node) + : srv_(node) + { } + + /** + * Once started, this class requires no further attention. + * Returns negative error code. + */ + int start() + { + return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats)); + } +}; + +} + +#endif // UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/std.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/std.hpp new file mode 100644 index 0000000000..a1e63ea469 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/std.hpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_STD_HPP_INCLUDED +#define UAVCAN_STD_HPP_INCLUDED + +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +# include +# include + +namespace uavcan +{ + +typedef std::uint8_t uint8_t; +typedef std::uint16_t uint16_t; +typedef std::uint32_t uint32_t; +typedef std::uint64_t uint64_t; + +typedef std::int8_t int8_t; +typedef std::int16_t int16_t; +typedef std::int32_t int32_t; +typedef std::int64_t int64_t; + +} + +#else + +# include // Standard integer types from C library +# include // vsnprintf() from the C library + +namespace uavcan +{ + +typedef ::uint8_t uint8_t; +typedef ::uint16_t uint16_t; +typedef ::uint32_t uint32_t; +typedef ::uint64_t uint64_t; + +typedef ::int8_t int8_t; +typedef ::int16_t int16_t; +typedef ::int32_t int32_t; +typedef ::int64_t int64_t; + +} + +#endif + +namespace uavcan +{ +/** + * Wrapper over the standard snprintf(). This wrapper is needed because different standards and different + * implementations of C++ do not agree whether snprintf() should be defined in std:: or in ::. The solution + * is to use 'using namespace std' hack inside the wrapper, so the compiler will be able to pick whatever + * definition is available in the standard library. Alternatively, the user's application can provide a + * custom implementation of uavcan::snprintf(). + */ +#if __GNUC__ +__attribute__ ((format(printf, 3, 4))) +#endif +extern int snprintf(char* out, std::size_t maxlen, const char* format, ...); + +#if !UAVCAN_USE_EXTERNAL_SNPRINTF +inline int snprintf(char* out, std::size_t maxlen, const char* format, ...) +{ + using namespace std; // This way we can pull vsnprintf() either from std:: or from ::. + va_list args; + va_start(args, format); + const int return_value = vsnprintf(out, maxlen, format, args); + va_end(args); + return return_value; +} +#endif + +} + +#endif // UAVCAN_STD_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/time.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/time.hpp new file mode 100644 index 0000000000..40bcb99449 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/time.hpp @@ -0,0 +1,289 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TIME_HPP_INCLUDED +#define UAVCAN_TIME_HPP_INCLUDED + +#include +#include +#include +#include +#include + + +namespace uavcan +{ + +template +class DurationBase +{ + int64_t usec_; + +protected: + ~DurationBase() { } + + DurationBase() + : usec_(0) + { + StaticAssert<(sizeof(D) == 8)>::check(); + } + +public: + static D getInfinite() { return fromUSec(NumericTraits::max()); } + + static D fromUSec(int64_t us) + { + D d; + d.usec_ = us; + return d; + } + static D fromMSec(int64_t ms) { return fromUSec(ms * 1000); } + + int64_t toUSec() const { return usec_; } + int64_t toMSec() const { return usec_ / 1000; } + + D getAbs() const { return D::fromUSec((usec_ < 0) ? (-usec_) : usec_); } + + bool isPositive() const { return usec_ > 0; } + bool isNegative() const { return usec_ < 0; } + bool isZero() const { return usec_ == 0; } + + bool operator==(const D& r) const { return usec_ == r.usec_; } + bool operator!=(const D& r) const { return !operator==(r); } + + bool operator<(const D& r) const { return usec_ < r.usec_; } + bool operator>(const D& r) const { return usec_ > r.usec_; } + bool operator<=(const D& r) const { return usec_ <= r.usec_; } + bool operator>=(const D& r) const { return usec_ >= r.usec_; } + + D operator+(const D& r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check + D operator-(const D& r) const { return fromUSec(usec_ - r.usec_); } // ditto + + D operator-() const { return fromUSec(-usec_); } + + D& operator+=(const D& r) + { + *this = *this + r; + return *static_cast(this); + } + D& operator-=(const D& r) + { + *this = *this - r; + return *static_cast(this); + } + + template + D operator*(Scale scale) const { return fromUSec(usec_ * scale); } + + template + D& operator*=(Scale scale) + { + *this = *this * scale; + return *static_cast(this); + } + + static const unsigned StringBufSize = 32; + void toString(char buf[StringBufSize]) const; ///< Prints time in seconds with microsecond resolution +#if UAVCAN_TOSTRING + std::string toString() const; ///< Prints time in seconds with microsecond resolution +#endif +}; + + +template +class TimeBase +{ + uint64_t usec_; + +protected: + ~TimeBase() { } + + TimeBase() + : usec_(0) + { + StaticAssert<(sizeof(T) == 8)>::check(); + StaticAssert<(sizeof(D) == 8)>::check(); + } + +public: + static T getMax() { return fromUSec(NumericTraits::max()); } + + static T fromUSec(uint64_t us) + { + T d; + d.usec_ = us; + return d; + } + static T fromMSec(uint64_t ms) { return fromUSec(ms * 1000); } + + uint64_t toUSec() const { return usec_; } + uint64_t toMSec() const { return usec_ / 1000; } + + bool isZero() const { return usec_ == 0; } + + bool operator==(const T& r) const { return usec_ == r.usec_; } + bool operator!=(const T& r) const { return !operator==(r); } + + bool operator<(const T& r) const { return usec_ < r.usec_; } + bool operator>(const T& r) const { return usec_ > r.usec_; } + bool operator<=(const T& r) const { return usec_ <= r.usec_; } + bool operator>=(const T& r) const { return usec_ >= r.usec_; } + + T operator+(const D& r) const + { + if (r.isNegative()) + { + if (uint64_t(r.getAbs().toUSec()) > usec_) + { + return fromUSec(0); + } + } + else + { + if (uint64_t(int64_t(usec_) + r.toUSec()) < usec_) + { + return fromUSec(NumericTraits::max()); + } + } + return fromUSec(uint64_t(int64_t(usec_) + r.toUSec())); + } + + T operator-(const D& r) const + { + return *static_cast(this) + (-r); + } + D operator-(const T& r) const + { + return D::fromUSec((usec_ > r.usec_) ? int64_t(usec_ - r.usec_) : -int64_t(r.usec_ - usec_)); + } + + T& operator+=(const D& r) + { + *this = *this + r; + return *static_cast(this); + } + T& operator-=(const D& r) + { + *this = *this - r; + return *static_cast(this); + } + + static const unsigned StringBufSize = 32; + void toString(char buf[StringBufSize]) const; ///< Prints time in seconds with microsecond resolution +#if UAVCAN_TOSTRING + std::string toString() const; ///< Prints time in seconds with microsecond resolution +#endif +}; + +/* + * Monotonic + */ +class UAVCAN_EXPORT MonotonicDuration : public DurationBase { }; + +class UAVCAN_EXPORT MonotonicTime : public TimeBase { }; + +/* + * UTC + */ +class UAVCAN_EXPORT UtcDuration : public DurationBase { }; + +class UAVCAN_EXPORT UtcTime : public TimeBase /// Implicitly convertible to/from uavcan.Timestamp +{ +public: + UtcTime() { } + + UtcTime(const Timestamp& ts) // Implicit + { + operator=(ts); + } + + UtcTime& operator=(const Timestamp& ts) + { + *this = fromUSec(ts.usec); + return *this; + } + + operator Timestamp() const + { + Timestamp ts; + ts.usec = toUSec(); + return ts; + } +}; + +// ---------------------------------------------------------------------------- + +template +const unsigned DurationBase::StringBufSize; + +template +const unsigned TimeBase::StringBufSize; + +template +void DurationBase::toString(char buf[StringBufSize]) const +{ + char* ptr = buf; + if (isNegative()) + { + *ptr++ = '-'; + } + (void)snprintf(ptr, StringBufSize - 1, "%llu.%06lu", + static_cast(getAbs().toUSec() / 1000000L), + static_cast(getAbs().toUSec() % 1000000L)); +} + + +template +void TimeBase::toString(char buf[StringBufSize]) const +{ + (void)snprintf(buf, StringBufSize, "%llu.%06lu", + static_cast(toUSec() / 1000000UL), + static_cast(toUSec() % 1000000UL)); +} + + +#if UAVCAN_TOSTRING + +template +std::string DurationBase::toString() const +{ + char buf[StringBufSize]; + toString(buf); + return std::string(buf); +} + +template +std::string TimeBase::toString() const +{ + char buf[StringBufSize]; + toString(buf); + return std::string(buf); +} + +#endif + + +template +UAVCAN_EXPORT +Stream& operator<<(Stream& s, const DurationBase& d) +{ + char buf[DurationBase::StringBufSize]; + d.toString(buf); + s << buf; + return s; +} + +template +UAVCAN_EXPORT +Stream& operator<<(Stream& s, const TimeBase& t) +{ + char buf[TimeBase::StringBufSize]; + t.toString(buf); + s << buf; + return s; +} + +} + +#endif // UAVCAN_TIME_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp new file mode 100644 index 0000000000..e4a276d1ce --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +/** + * API for transfer buffer users. + */ +class UAVCAN_EXPORT ITransferBuffer +{ +public: + virtual ~ITransferBuffer() { } + + virtual int read(unsigned offset, uint8_t* data, unsigned len) const = 0; + virtual int write(unsigned offset, const uint8_t* data, unsigned len) = 0; +}; + +} + +#endif // UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp new file mode 100644 index 0000000000..22b98809e0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2015 Pavel Kirienko , + * Ilia Sheremet + */ + +#ifndef UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class configures hardware acceptance filters (if this feature is present on the particular CAN driver) to + * preclude reception of irrelevant CAN frames on the hardware level. + * + * Configuration starts by creating an object of class @ref CanAcceptanceFilterConfigurator on the stack. + * By means of computeConfiguration() method the class determines the number of available HW filters and the number + * of listeners. In case if custom configuration required, it is possible to add it through addFilterConfig(). + * Subsequently obtained configurations are then loaded into the CAN driver by calling the applyConfiguration() method. + * If the cumulative number of configurations obtained by computeConfiguration() and addFilterConfig() is higher than + * the number of available HW filters, configurations will be merged automatically in the most efficient way. + * + * Note that if the application adds additional server or subscriber objects after the filters have been configured, + * the configuration procedure will have to be performed again. + * + * The maximum number of CAN acceptance filters is predefined in uavcan/build_config.hpp through a constant + * @ref MaxCanAcceptanceFilters. The algorithm doesn't allow to have higher number of HW filters configurations than + * defined by MaxCanAcceptanceFilters. You can change this value according to the number specified in your CAN driver + * datasheet. + */ +class CanAcceptanceFilterConfigurator +{ +public: + /** + * These arguments defines whether acceptance filter configuration has anonymous messages or not + */ + enum AnonymousMessages + { + AcceptAnonymousMessages, + IgnoreAnonymousMessages + }; + +private: + /** + * Below constants based on UAVCAN transport layer specification. Masks and ID's depends on message + * TypeID, TransferID (RequestNotResponse - for service types, ServiceNotMessage - for all types of messages). + * For more details refer to uavcan.org/CAN_bus_transport_layer_specification. + * For clarity let's represent "i" as Data Type ID and "d" as Destination Node Id + * DefaultFilterMsgMask = 00000 11111111 11111111 10000000 + * DefaultFilterMsgID = 00000 iiiiiiii iiiiiiii 00000000, no need to explicitly define, since MsgID initialized + * as 0. + * DefaultFilterServiceMask = 00000 00000000 01111111 10000000 + * DefaultFilterServiceID = 00000 00000000 0ddddddd 10000000, all Service Response Frames are accepted by + * HW filters. + * DefaultAnonMsgMask = 00000 00000000 00000000 11111111 + * DefaultAnonMsgID = 00000 00000000 00000000 00000000, by default the config is added to accept all anonymous + * frames. In case there are no anonymous messages, invoke computeConfiguration(IgnoreAnonymousMessages). + */ + static const unsigned DefaultFilterMsgMask = 0xFFFF80; + static const unsigned DefaultFilterServiceMask = 0x7F80; + static const unsigned DefaultFilterServiceID = 0x80; + static const unsigned DefaultAnonMsgMask = 0xFF; + static const unsigned DefaultAnonMsgID = 0x0; + + typedef uavcan::Multiset MultisetConfigContainer; + + static CanFilterConfig mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_); + static uint8_t countBits(uint32_t n_); + uint16_t getNumFilters() const; + + /** + * Fills the multiset_configs_ to proceed it with mergeConfigurations() + */ + int16_t loadInputConfiguration(AnonymousMessages load_mode); + + /** + * This method merges several listeners's filter configurations by predetermined algorithm + * if number of available hardware acceptance filters less than number of listeners + */ + int16_t mergeConfigurations(); + + INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher + MultisetConfigContainer multiset_configs_; + uint16_t filters_number_; + +public: + /** + * @param node Libuavcan node whose subscribers/servers/etc will be used to configure the filters. + * + * @param filters_number Allows to override the maximum number of hardware filters to use. + * If set to zero (which is default), the class will obtain the number of available + * filters from the CAN driver via @ref ICanIface::getNumFilters(). + */ + explicit CanAcceptanceFilterConfigurator(INode& node, uint16_t filters_number = 0) + : node_(node) + , multiset_configs_(node.getAllocator()) + , filters_number_(filters_number) + { } + + /** + * This method invokes loadInputConfiguration() and mergeConfigurations() consequently + * in order to comute optimal filter configurations for the current hardware. + * + * It can only be invoked when all of the subscriber and server objects are initialized. + * If new subscriber or server objects are added later, the filters will have to be reconfigured again. + * + * @param mode Either: AcceptAnonymousMessages - the filters will accept all anonymous messages (this is default) + * IgnoreAnonymousMessages - anonymous messages will be ignored + * @return 0 = success, negative for error. + */ + int computeConfiguration(AnonymousMessages mode = AcceptAnonymousMessages); + + /** + * Add an additional filter configuration. + * This method must not be invoked after @ref computeConfiguration(). + * @return 0 = success, negative for error. + */ + int addFilterConfig(const CanFilterConfig& config); + + /** + * This method loads the configuration computed with mergeConfigurations() or explicitly added by addFilterConfig() + * to the CAN driver. Must be called after computeConfiguration() and addFilterConfig(). + * @return 0 = success, negative for error. + */ + int applyConfiguration(); + + /** + * Returns the configuration computed with mergeConfigurations() or added by addFilterConfig(). + * If mergeConfigurations() or addFilterConfig() have not been called yet, an empty configuration will be returned. + */ + const MultisetConfigContainer& getConfiguration() const + { + return multiset_configs_; + } +}; + +/** + * This function is a shortcut for @ref CanAcceptanceFilterConfigurator. + * It allows to compute filter configuration and then apply it in just one step. + * It implements only the most common use case; if you have special requirements, + * use @ref CanAcceptanceFilterConfigurator directly. + * + * @param node Refer to @ref CanAcceptanceFilterConfigurator constructor for explanation. + * @param mode Refer to @ref CanAcceptanceFilterConfigurator::computeConfiguration() for explanation. + * @return non-negative on success, negative error code on error. + */ +static inline int configureCanAcceptanceFilters(INode& node, + CanAcceptanceFilterConfigurator::AnonymousMessages mode + = CanAcceptanceFilterConfigurator::AcceptAnonymousMessages) +{ + CanAcceptanceFilterConfigurator cfger(node); + + const int compute_res = cfger.computeConfiguration(mode); + if (compute_res < 0) + { + return compute_res; + } + + return cfger.applyConfiguration(); +} + +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp new file mode 100644 index 0000000000..11752f0f8a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp @@ -0,0 +1,186 @@ +/* + * CAN bus IO logic. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +struct UAVCAN_EXPORT CanRxFrame : public CanFrame +{ + MonotonicTime ts_mono; + UtcTime ts_utc; + uint8_t iface_index; + + CanRxFrame() + : iface_index(0) + { } + +#if UAVCAN_TOSTRING + std::string toString(StringRepresentation mode = StrTight) const; +#endif +}; + + +class UAVCAN_EXPORT CanTxQueue : Noncopyable +{ +public: + enum Qos { Volatile, Persistent }; + + struct Entry : public LinkedListNode // Not required to be packed - fits the block in any case + { + MonotonicTime deadline; + CanFrame frame; + uint8_t qos; + CanIOFlags flags; + + Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags) + : deadline(arg_deadline) + , frame(arg_frame) + , qos(uint8_t(arg_qos)) + , flags(arg_flags) + { + UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent)); + IsDynamicallyAllocatable::check(); + } + + static void destroy(Entry*& obj, IPoolAllocator& allocator); + + bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; } + + bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const; + bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const; + bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); } + bool qosLowerThan(const Entry& rhs) const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif + }; + +private: + class PriorityInsertionComparator + { + const CanFrame& frm_; + public: + explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { } + bool operator()(const Entry* entry) + { + UAVCAN_ASSERT(entry); + return frm_.priorityHigherThan(entry->frame); + } + }; + + LinkedListRoot queue_; + LimitedPoolAllocator allocator_; + ISystemClock& sysclock_; + uint32_t rejected_frames_cnt_; + + void registerRejectedFrame(); + +public: + CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota) + : allocator_(allocator, allocator_quota) + , sysclock_(sysclock) + , rejected_frames_cnt_(0) + { } + + ~CanTxQueue(); + + void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags); + + Entry* peek(); // Modifier + void remove(Entry*& entry); + const CanFrame* getTopPriorityPendingFrame() const; + + /// The 'or equal' condition is necessary to avoid frame reordering. + bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const; + + uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; } + + bool isEmpty() const { return queue_.isEmpty(); } +}; + + +struct UAVCAN_EXPORT CanIfacePerfCounters +{ + uint64_t frames_tx; + uint64_t frames_rx; + uint64_t errors; + + CanIfacePerfCounters() + : frames_tx(0) + , frames_rx(0) + , errors(0) + { } +}; + + +class UAVCAN_EXPORT CanIOManager : Noncopyable +{ + struct IfaceFrameCounters + { + uint64_t frames_tx; + uint64_t frames_rx; + + IfaceFrameCounters() + : frames_tx(0) + , frames_rx(0) + { } + }; + + ICanDriver& driver_; + ISystemClock& sysclock_; + + LazyConstructor tx_queues_[MaxCanIfaces]; + IfaceFrameCounters counters_[MaxCanIfaces]; + + const uint8_t num_ifaces_; + + int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags); + int sendFromTxQueue(uint8_t iface_index); + int callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces], + MonotonicTime blocking_deadline); + +public: + CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, + std::size_t mem_blocks_per_iface = 0); + + uint8_t getNumIfaces() const { return num_ifaces_; } + + CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const; + + const ICanDriver& getCanDriver() const { return driver_; } + ICanDriver& getCanDriver() { return driver_; } + + uint8_t makePendingTxMask() const; + + /** + * Returns: + * 0 - rejected/timedout/enqueued + * 1+ - sent/received + * negative - failure + */ + int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); + int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags); +}; + +} + +#endif // UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/crc.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/crc.hpp new file mode 100644 index 0000000000..aae2c4d964 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/crc.hpp @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_CRC_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CRC_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +/** + * CRC-16-CCITT + * Initial value: 0xFFFF + * Poly: 0x1021 + * Reverse: no + * Output xor: 0 + * + * import crcmod + * crc = crcmod.predefined.Crc('crc-ccitt-false') + * crc.update('123456789') + * crc.hexdigest() + * '29B1' + */ +class UAVCAN_EXPORT TransferCRC +{ +#if !UAVCAN_TINY + static const uint16_t Table[256]; +#endif + + uint16_t value_; + +public: + enum { NumBytes = 2 }; + + TransferCRC() + : value_(0xFFFFU) + { } + +#if UAVCAN_TINY + void add(uint8_t byte) + { + value_ ^= uint16_t(uint16_t(byte) << 8); + for (uint8_t bit = 8; bit > 0; --bit) + { + if (value_ & 0x8000U) + { + value_ = uint16_t(uint16_t(value_ << 1) ^ 0x1021U); + } + else + { + value_ = uint16_t(value_ << 1); + } + } + } +#else + void add(uint8_t byte) + { + value_ = uint16_t(uint16_t((value_ << 8) ^ Table[uint16_t((value_ >> 8) ^ byte) & 0xFFU]) & 0xFFFFU); + } +#endif + + void add(const uint8_t* bytes, unsigned len) + { + UAVCAN_ASSERT(bytes); + while (len--) + { + add(*bytes++); + } + } + + uint16_t get() const { return value_; } +}; + +} + +#endif // UAVCAN_TRANSPORT_CRC_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/dispatcher.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/dispatcher.hpp new file mode 100644 index 0000000000..53bc2e203f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -0,0 +1,242 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT Dispatcher; + +#if !UAVCAN_TINY +/** + * Inherit this class to receive notifications about all TX CAN frames that were transmitted with the loopback flag. + */ +class UAVCAN_EXPORT LoopbackFrameListenerBase : public LinkedListNode +{ + Dispatcher& dispatcher_; + +protected: + explicit LoopbackFrameListenerBase(Dispatcher& dispatcher) + : dispatcher_(dispatcher) + { } + + virtual ~LoopbackFrameListenerBase() { stopListening(); } + + void startListening(); + void stopListening(); + bool isListening() const; + + Dispatcher& getDispatcher() { return dispatcher_; } + +public: + virtual void handleLoopbackFrame(const RxFrame& frame) = 0; +}; + + +class UAVCAN_EXPORT LoopbackFrameListenerRegistry : Noncopyable +{ + LinkedListRoot listeners_; + +public: + void add(LoopbackFrameListenerBase* listener); + void remove(LoopbackFrameListenerBase* listener); + bool doesExist(const LoopbackFrameListenerBase* listener) const; + unsigned getNumListeners() const { return listeners_.getLength(); } + + void invokeListeners(RxFrame& frame); +}; + +/** + * Implement this interface to receive notifications about all incoming CAN frames, including loopback. + */ +class UAVCAN_EXPORT IRxFrameListener +{ +public: + virtual ~IRxFrameListener() { } + + /** + * Make sure to filter out loopback frames if they are not wanted. + */ + virtual void handleRxFrame(const CanRxFrame& frame, CanIOFlags flags) = 0; +}; +#endif + +/** + * This class performs low-level CAN frame routing. + */ +class UAVCAN_EXPORT Dispatcher : Noncopyable +{ + CanIOManager canio_; + ISystemClock& sysclock_; + OutgoingTransferRegistry outgoing_transfer_reg_; + TransferPerfCounter perf_; + + class ListenerRegistry + { + LinkedListRoot list_; + + class DataTypeIDInsertionComparator + { + const DataTypeID id_; + public: + explicit DataTypeIDInsertionComparator(DataTypeID id) : id_(id) { } + bool operator()(const TransferListener* listener) const + { + UAVCAN_ASSERT(listener); + return id_ > listener->getDataTypeDescriptor().getID(); + } + }; + + public: + enum Mode { UniqueListener, ManyListeners }; + + bool add(TransferListener* listener, Mode mode); + void remove(TransferListener* listener); + bool exists(DataTypeID dtid) const; + void cleanup(MonotonicTime ts); + void handleFrame(const RxFrame& frame); + + unsigned getNumEntries() const { return list_.getLength(); } + + const LinkedListRoot& getList() const { return list_; } + }; + + ListenerRegistry lmsg_; + ListenerRegistry lsrv_req_; + ListenerRegistry lsrv_resp_; + +#if !UAVCAN_TINY + LoopbackFrameListenerRegistry loopback_listeners_; + IRxFrameListener* rx_listener_; +#endif + + NodeID self_node_id_; + bool self_node_id_is_set_; + + void handleFrame(const CanRxFrame& can_frame); + + void handleLoopbackFrame(const CanRxFrame& can_frame); + + void notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags); + +public: + Dispatcher(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock) + : canio_(driver, allocator, sysclock) + , sysclock_(sysclock) + , outgoing_transfer_reg_(allocator) +#if !UAVCAN_TINY + , rx_listener_(UAVCAN_NULLPTR) +#endif + , self_node_id_(NodeID::Broadcast) // Default + , self_node_id_is_set_(false) + { } + + /** + * This version returns strictly when the deadline is reached. + */ + int spin(MonotonicTime deadline); + + /** + * This version does not return until all available frames are processed. + */ + int spinOnce(); + + /** + * Refer to CanIOManager::send() for the parameter description + */ + int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, + CanIOFlags flags, uint8_t iface_mask); + + void cleanup(MonotonicTime ts); + + bool registerMessageListener(TransferListener* listener); + bool registerServiceRequestListener(TransferListener* listener); + bool registerServiceResponseListener(TransferListener* listener); + + void unregisterMessageListener(TransferListener* listener); + void unregisterServiceRequestListener(TransferListener* listener); + void unregisterServiceResponseListener(TransferListener* listener); + + bool hasSubscriber(DataTypeID dtid) const; + bool hasPublisher(DataTypeID dtid) const; + bool hasServer(DataTypeID dtid) const; + + unsigned getNumMessageListeners() const { return lmsg_.getNumEntries(); } + unsigned getNumServiceRequestListeners() const { return lsrv_req_.getNumEntries(); } + unsigned getNumServiceResponseListeners() const { return lsrv_resp_.getNumEntries(); } + + /** + * These methods can be used to retreive lists of messages, service requests and service responses the + * dispatcher is currently listening to. + * Note that the list of service response listeners is very volatile, because a response listener will be + * removed from this list as soon as the corresponding service call is complete. + * @{ + */ + const LinkedListRoot& getListOfMessageListeners() const + { + return lmsg_.getList(); + } + const LinkedListRoot& getListOfServiceRequestListeners() const + { + return lsrv_req_.getList(); + } + const LinkedListRoot& getListOfServiceResponseListeners() const + { + return lsrv_resp_.getList(); + } + /** + * @} + */ + + OutgoingTransferRegistry& getOutgoingTransferRegistry() { return outgoing_transfer_reg_; } + +#if !UAVCAN_TINY + LoopbackFrameListenerRegistry& getLoopbackFrameListenerRegistry() { return loopback_listeners_; } + + IRxFrameListener* getRxFrameListener() const { return rx_listener_; } + void removeRxFrameListener() { rx_listener_ = UAVCAN_NULLPTR; } + void installRxFrameListener(IRxFrameListener* listener) + { + UAVCAN_ASSERT(listener != UAVCAN_NULLPTR); + rx_listener_ = listener; + } +#endif + + /** + * Node ID can be set only once. + * Non-unicast Node ID puts the node into passive mode. + */ + NodeID getNodeID() const { return self_node_id_; } + bool setNodeID(NodeID nid); + + /** + * Refer to the specs to learn more about passive mode. + */ + bool isPassiveMode() const { return !getNodeID().isUnicast(); } + + const ISystemClock& getSystemClock() const { return sysclock_; } + ISystemClock& getSystemClock() { return sysclock_; } + + const CanIOManager& getCanIOManager() const { return canio_; } + CanIOManager& getCanIOManager() { return canio_; } + + const TransferPerfCounter& getTransferPerfCounter() const { return perf_; } + TransferPerfCounter& getTransferPerfCounter() { return perf_; } +}; + +} + +#endif // UAVCAN_TRANSPORT_DISPATCHER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/frame.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/frame.hpp new file mode 100644 index 0000000000..8c95beab89 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/frame.hpp @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED +#define UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT Frame +{ + enum { PayloadCapacity = 7 }; // Will be redefined when CAN FD is available + + uint8_t payload_[PayloadCapacity]; + TransferPriority transfer_priority_; + TransferType transfer_type_; + DataTypeID data_type_id_; + uint_fast8_t payload_len_; + NodeID src_node_id_; + NodeID dst_node_id_; + TransferID transfer_id_; + bool start_of_transfer_; + bool end_of_transfer_; + bool toggle_; + +public: + Frame() : + transfer_type_(TransferType(NumTransferTypes)), // Invalid value + payload_len_(0), + start_of_transfer_(false), + end_of_transfer_(false), + toggle_(false) + { } + + Frame(DataTypeID data_type_id, + TransferType transfer_type, + NodeID src_node_id, + NodeID dst_node_id, + TransferID transfer_id) : + transfer_priority_(TransferPriority::Default), + transfer_type_(transfer_type), + data_type_id_(data_type_id), + payload_len_(0), + src_node_id_(src_node_id), + dst_node_id_(dst_node_id), + transfer_id_(transfer_id), + start_of_transfer_(false), + end_of_transfer_(false), + toggle_(false) + { + UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); + UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type))); + UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); + } + + void setPriority(TransferPriority priority) { transfer_priority_ = priority; } + TransferPriority getPriority() const { return transfer_priority_; } + + /** + * Max payload length depends on the transfer type and frame index. + */ + uint8_t getPayloadCapacity() const { return PayloadCapacity; } + uint8_t setPayload(const uint8_t* data, unsigned len); + + unsigned getPayloadLen() const { return payload_len_; } + const uint8_t* getPayloadPtr() const { return payload_; } + + TransferType getTransferType() const { return transfer_type_; } + DataTypeID getDataTypeID() const { return data_type_id_; } + NodeID getSrcNodeID() const { return src_node_id_; } + NodeID getDstNodeID() const { return dst_node_id_; } + TransferID getTransferID() const { return transfer_id_; } + + void setStartOfTransfer(bool x) { start_of_transfer_ = x; } + void setEndOfTransfer(bool x) { end_of_transfer_ = x; } + + bool isStartOfTransfer() const { return start_of_transfer_; } + bool isEndOfTransfer() const { return end_of_transfer_; } + + void flipToggle() { toggle_ = !toggle_; } + bool getToggle() const { return toggle_; } + + bool parse(const CanFrame& can_frame); + bool compile(CanFrame& can_frame) const; + + bool isValid() const; + + bool operator!=(const Frame& rhs) const { return !operator==(rhs); } + bool operator==(const Frame& rhs) const; + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + + +class UAVCAN_EXPORT RxFrame : public Frame +{ + MonotonicTime ts_mono_; + UtcTime ts_utc_; + uint8_t iface_index_; + +public: + RxFrame() + : iface_index_(0) + { } + + RxFrame(const Frame& frame, MonotonicTime ts_mono, UtcTime ts_utc, uint8_t iface_index) + : ts_mono_(ts_mono) + , ts_utc_(ts_utc) + , iface_index_(iface_index) + { + *static_cast(this) = frame; + } + + bool parse(const CanRxFrame& can_frame); + + /** + * Can't be zero. + */ + MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } + + /** + * Can be zero if not supported by the platform driver. + */ + UtcTime getUtcTimestamp() const { return ts_utc_; } + + uint8_t getIfaceIndex() const { return iface_index_; } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +} + +#endif // UAVCAN_TRANSPORT_FRAME_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp new file mode 100644 index 0000000000..f92636e540 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED +#define UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT OutgoingTransferRegistryKey +{ + DataTypeID data_type_id_; + uint8_t transfer_type_; + NodeID destination_node_id_; ///< Not applicable for message broadcasting + +public: + OutgoingTransferRegistryKey() + : transfer_type_(0xFF) + { } + + OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id) + : data_type_id_(data_type_id) + , transfer_type_(transfer_type) + , destination_node_id_(destination_node_id) + { + UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast()); + /* + * Service response transfers must use the same Transfer ID as matching service request transfer, + * so this registry is not applicable for service response transfers at all. + */ + UAVCAN_ASSERT(transfer_type != TransferTypeServiceResponse); + } + + DataTypeID getDataTypeID() const { return data_type_id_; } + TransferType getTransferType() const { return TransferType(transfer_type_); } + + bool operator==(const OutgoingTransferRegistryKey& rhs) const + { + return + (data_type_id_ == rhs.data_type_id_) && + (transfer_type_ == rhs.transfer_type_) && + (destination_node_id_ == rhs.destination_node_id_); + } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +/** + * Outgoing transfer registry keeps track of Transfer ID values for all currently existing local transfer senders. + * If a local transfer sender was inactive for a sufficiently long time, the outgoing transfer registry will + * remove the respective Transfer ID tracking object. + */ +class UAVCAN_EXPORT OutgoingTransferRegistry : Noncopyable +{ + struct Value + { + MonotonicTime deadline; + TransferID tid; + }; + + class DeadlineExpiredPredicate + { + const MonotonicTime ts_; + + public: + explicit DeadlineExpiredPredicate(MonotonicTime ts) + : ts_(ts) + { } + + bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const + { + (void)key; + UAVCAN_ASSERT(!value.deadline.isZero()); + const bool expired = value.deadline <= ts_; + if (expired) + { + UAVCAN_TRACE("OutgoingTransferRegistry", "Expired %s tid=%i", + key.toString().c_str(), int(value.tid.get())); + } + return expired; + } + }; + + class ExistenceCheckingPredicate + { + const DataTypeID dtid_; + const TransferType tt_; + + public: + ExistenceCheckingPredicate(DataTypeID dtid, TransferType tt) + : dtid_(dtid) + , tt_(tt) + { } + + bool operator()(const OutgoingTransferRegistryKey& key, const Value&) const + { + return dtid_ == key.getDataTypeID() && tt_ == key.getTransferType(); + } + }; + + Map map_; + +public: + static const MonotonicDuration MinEntryLifetime; + + explicit OutgoingTransferRegistry(IPoolAllocator& allocator) + : map_(allocator) + { } + + TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline); + + bool exists(DataTypeID dtid, TransferType tt) const; + + void cleanup(MonotonicTime ts); +}; + +} + +#endif // UAVCAN_TRANSPORT_OUTGOING_TRANSFER_REGISTRY_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/perf_counter.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/perf_counter.hpp new file mode 100644 index 0000000000..e056f3c0b7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/perf_counter.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ + +#if UAVCAN_TINY + +class UAVCAN_EXPORT TransferPerfCounter : Noncopyable +{ +public: + void addTxTransfer() { } + void addRxTransfer() { } + void addError() { } + void addErrors(unsigned) { } + uint64_t getTxTransferCount() const { return 0; } + uint64_t getRxTransferCount() const { return 0; } + uint64_t getErrorCount() const { return 0; } +}; + +#else + +/** + * The class is declared noncopyable for two reasons: + * - to prevent accidental pass-by-value into a mutator + * - to make the addresses of the counters fixed and exposable to the user of the library + */ +class UAVCAN_EXPORT TransferPerfCounter : Noncopyable +{ + uint64_t transfers_tx_; + uint64_t transfers_rx_; + uint64_t errors_; + +public: + TransferPerfCounter() + : transfers_tx_(0) + , transfers_rx_(0) + , errors_(0) + { } + + void addTxTransfer() { transfers_tx_++; } + void addRxTransfer() { transfers_rx_++; } + + void addError() { errors_++; } + + void addErrors(unsigned errors) + { + errors_ += errors; + } + + /** + * Returned references are guaranteed to be valid as long as this instance of Node exists. + * This is enforced by virtue of the class being Noncopyable. + */ + const uint64_t& getTxTransferCount() const { return transfers_tx_; } + const uint64_t& getRxTransferCount() const { return transfers_rx_; } + const uint64_t& getErrorCount() const { return errors_; } +}; + +#endif + +} + +#endif // UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer.hpp new file mode 100644 index 0000000000..7e5d5b8821 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer.hpp @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ + +static const unsigned GuaranteedPayloadLenPerFrame = 7; ///< Guaranteed for all transfers, all CAN standards + +enum TransferType +{ + TransferTypeServiceResponse = 0, + TransferTypeServiceRequest = 1, + TransferTypeMessageBroadcast = 2 +}; + +static const uint8_t NumTransferTypes = 3; + + +class UAVCAN_EXPORT TransferPriority +{ + uint8_t value_; + +public: + static const uint8_t BitLen = 5U; + static const uint8_t NumericallyMax = (1U << BitLen) - 1; + static const uint8_t NumericallyMin = 0; + + /// This priority is used by default + static const TransferPriority Default; + static const TransferPriority MiddleLower; + static const TransferPriority OneHigherThanLowest; + static const TransferPriority OneLowerThanHighest; + static const TransferPriority Lowest; + + TransferPriority() : value_(0xFF) { } + + TransferPriority(uint8_t value) // Implicit + : value_(value) + { + UAVCAN_ASSERT(isValid()); + } + + template + static TransferPriority fromPercent() + { + StaticAssert<(Percent <= 100)>::check(); + enum { Result = ((100U - Percent) * NumericallyMax) / 100U }; + StaticAssert<(Result <= NumericallyMax)>::check(); + StaticAssert<(Result >= NumericallyMin)>::check(); + return TransferPriority(Result); + } + + uint8_t get() const { return value_; } + + bool isValid() const { return value_ < (1U << BitLen); } + + bool operator!=(TransferPriority rhs) const { return value_ != rhs.value_; } + bool operator==(TransferPriority rhs) const { return value_ == rhs.value_; } +}; + + +class UAVCAN_EXPORT TransferID +{ + uint8_t value_; + +public: + static const uint8_t BitLen = 5U; + static const uint8_t Max = (1U << BitLen) - 1U; + static const uint8_t Half = (1U << BitLen) / 2U; + + TransferID() + : value_(0) + { } + + TransferID(uint8_t value) // implicit + : value_(value) + { + value_ &= Max; + UAVCAN_ASSERT(value == value_); + } + + bool operator!=(TransferID rhs) const { return !operator==(rhs); } + bool operator==(TransferID rhs) const { return get() == rhs.get(); } + + void increment() + { + value_ = (value_ + 1) & Max; + } + + uint8_t get() const + { + UAVCAN_ASSERT(value_ <= Max); + return value_; + } + + /** + * Amount of increment() calls to reach rhs value. + */ + int computeForwardDistance(TransferID rhs) const; +}; + + +class UAVCAN_EXPORT NodeID +{ + static const uint8_t ValueBroadcast = 0; + static const uint8_t ValueInvalid = 0xFF; + uint8_t value_; + +public: + static const uint8_t BitLen = 7U; + static const uint8_t Max = (1U << BitLen) - 1U; + static const uint8_t MaxRecommendedForRegularNodes = Max - 2; + static const NodeID Broadcast; + + NodeID() : value_(ValueInvalid) { } + + NodeID(uint8_t value) // Implicit + : value_(value) + { + UAVCAN_ASSERT(isValid()); + } + + uint8_t get() const { return value_; } + + bool isValid() const { return value_ <= Max; } + bool isBroadcast() const { return value_ == ValueBroadcast; } + bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); } + + bool operator!=(NodeID rhs) const { return !operator==(rhs); } + bool operator==(NodeID rhs) const { return value_ == rhs.value_; } + + bool operator<(NodeID rhs) const { return value_ < rhs.value_; } + bool operator>(NodeID rhs) const { return value_ > rhs.value_; } + bool operator<=(NodeID rhs) const { return value_ <= rhs.value_; } + bool operator>=(NodeID rhs) const { return value_ >= rhs.value_; } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_buffer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_buffer.hpp new file mode 100644 index 0000000000..8d7bb873dc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_buffer.hpp @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Standalone static buffer + */ +class StaticTransferBufferImpl : public ITransferBuffer +{ + uint8_t* const data_; + const uint16_t size_; + uint16_t max_write_pos_; + +public: + StaticTransferBufferImpl(uint8_t* buf, uint16_t buf_size) : + data_(buf), + size_(buf_size), + max_write_pos_(0) + { } + + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual int write(unsigned offset, const uint8_t* data, unsigned len) override; + + void reset(); + + uint16_t getSize() const { return size_; } + + uint8_t* getRawPtr() { return data_; } + const uint8_t* getRawPtr() const { return data_; } + + uint16_t getMaxWritePos() const { return max_write_pos_; } + void setMaxWritePos(uint16_t value) { max_write_pos_ = value; } +}; + +template +class UAVCAN_EXPORT StaticTransferBuffer : public StaticTransferBufferImpl +{ + uint8_t buffer_[Size]; +public: + StaticTransferBuffer() : StaticTransferBufferImpl(buffer_, Size) + { + StaticAssert<(Size > 0)>::check(); + } +}; + +/** + * Internal for TransferBufferManager + */ +class UAVCAN_EXPORT TransferBufferManagerKey +{ + NodeID node_id_; + uint8_t transfer_type_; + +public: + TransferBufferManagerKey() + : transfer_type_(TransferType(0)) + { + UAVCAN_ASSERT(isEmpty()); + } + + TransferBufferManagerKey(NodeID node_id, TransferType ttype) + : node_id_(node_id) + , transfer_type_(ttype) + { + UAVCAN_ASSERT(!isEmpty()); + } + + bool operator==(const TransferBufferManagerKey& rhs) const + { + return node_id_ == rhs.node_id_ && transfer_type_ == rhs.transfer_type_; + } + + bool isEmpty() const { return !node_id_.isValid(); } + + NodeID getNodeID() const { return node_id_; } + TransferType getTransferType() const { return TransferType(transfer_type_); } + +#if UAVCAN_TOSTRING + std::string toString() const; +#endif +}; + +/** + * Resizable gather/scatter storage. + * reset() call releases all memory blocks. + * Supports unordered write operations - from higher to lower offsets + */ +class UAVCAN_EXPORT TransferBufferManagerEntry : public ITransferBuffer + , public LinkedListNode +{ + struct Block : LinkedListNode + { + enum { Size = MemPoolBlockSize - sizeof(LinkedListNode) }; + uint8_t data[static_cast(Size)]; + + static Block* instantiate(IPoolAllocator& allocator); + static void destroy(Block*& obj, IPoolAllocator& allocator); + + void read(uint8_t*& outptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_read); + void write(const uint8_t*& inptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_write); + }; + + IPoolAllocator& allocator_; + LinkedListRoot blocks_; // Blocks are ordered from lower to higher buffer offset + uint16_t max_write_pos_; + const uint16_t max_size_; + TransferBufferManagerKey key_; + +public: + TransferBufferManagerEntry(IPoolAllocator& allocator, uint16_t max_size) : + allocator_(allocator), + max_write_pos_(0), + max_size_(max_size) + { + StaticAssert<(Block::Size > 8)>::check(); + IsDynamicallyAllocatable::check(); + IsDynamicallyAllocatable::check(); + } + + virtual ~TransferBufferManagerEntry() { reset(); } + + static TransferBufferManagerEntry* instantiate(IPoolAllocator& allocator, uint16_t max_size); + static void destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator); + + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual int write(unsigned offset, const uint8_t* data, unsigned len) override; + + void reset(const TransferBufferManagerKey& key = TransferBufferManagerKey()); + + const TransferBufferManagerKey& getKey() const { return key_; } + bool isEmpty() const { return key_.isEmpty(); } +}; + +/** + * Buffer manager implementation. + */ +class TransferBufferManager : public Noncopyable +{ + LinkedListRoot buffers_; + IPoolAllocator& allocator_; + const uint16_t max_buf_size_; + + TransferBufferManagerEntry* findFirst(const TransferBufferManagerKey& key); + +public: + TransferBufferManager(uint16_t max_buf_size, IPoolAllocator& allocator) : + allocator_(allocator), + max_buf_size_(max_buf_size) + { } + + ~TransferBufferManager(); + + ITransferBuffer* access(const TransferBufferManagerKey& key); + ITransferBuffer* create(const TransferBufferManagerKey& key); + void remove(const TransferBufferManagerKey& key); + bool isEmpty() const; + + unsigned getNumBuffers() const; +}; + +/** + * Convinience class. + */ +class UAVCAN_EXPORT TransferBufferAccessor +{ + TransferBufferManager& bufmgr_; + const TransferBufferManagerKey key_; + +public: + TransferBufferAccessor(TransferBufferManager& bufmgr, TransferBufferManagerKey key) : + bufmgr_(bufmgr), + key_(key) + { + UAVCAN_ASSERT(!key.isEmpty()); + } + ITransferBuffer* access() { return bufmgr_.access(key_); } + ITransferBuffer* create() { return bufmgr_.create(key_); } + void remove() { bufmgr_.remove(key_); } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_BUFFER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_listener.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_listener.hpp new file mode 100644 index 0000000000..4ccf36a72f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Container for received transfer. + */ +class UAVCAN_EXPORT IncomingTransfer : public ITransferBuffer +{ + MonotonicTime ts_mono_; + UtcTime ts_utc_; + TransferPriority transfer_priority_; + TransferType transfer_type_; + TransferID transfer_id_; + NodeID src_node_id_; + uint8_t iface_index_; + + /// That's a no-op, asserts in debug builds + virtual int write(unsigned offset, const uint8_t* data, unsigned len) override; + +protected: + IncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, TransferPriority transfer_priority, + TransferType transfer_type, TransferID transfer_id, NodeID source_node_id, + uint8_t iface_index) + : ts_mono_(ts_mono) + , ts_utc_(ts_utc) + , transfer_priority_(transfer_priority) + , transfer_type_(transfer_type) + , transfer_id_(transfer_id) + , src_node_id_(source_node_id) + , iface_index_(iface_index) + { } + +public: + /** + * Dispose the payload buffer. Further calls to read() will not be possible. + */ + virtual void release() { } + + /** + * Whether this is a anonymous transfer + */ + virtual bool isAnonymousTransfer() const { return false; } + + MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } + UtcTime getUtcTimestamp() const { return ts_utc_; } + TransferPriority getPriority() const { return transfer_priority_; } + TransferType getTransferType() const { return transfer_type_; } + TransferID getTransferID() const { return transfer_id_; } + NodeID getSrcNodeID() const { return src_node_id_; } + uint8_t getIfaceIndex() const { return iface_index_; } +}; + +/** + * Internal. + */ +class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer +{ + const uint8_t* const payload_; + const uint8_t payload_len_; +public: + explicit SingleFrameIncomingTransfer(const RxFrame& frm); + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual bool isAnonymousTransfer() const override; +}; + +/** + * Internal. + */ +class UAVCAN_EXPORT MultiFrameIncomingTransfer : public IncomingTransfer, Noncopyable +{ + TransferBufferAccessor& buf_acc_; +public: + MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, const RxFrame& last_frame, + TransferBufferAccessor& tba); + virtual int read(unsigned offset, uint8_t* data, unsigned len) const override; + virtual void release() override { buf_acc_.remove(); } +}; + +/** + * Internal, refer to the transport dispatcher class. + */ +class UAVCAN_EXPORT TransferListener : public LinkedListNode +{ + const DataTypeDescriptor& data_type_; + TransferBufferManager bufmgr_; + Map receivers_; + TransferPerfCounter& perf_; + const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant + bool allow_anonymous_transfers_; + + class TimedOutReceiverPredicate + { + const MonotonicTime ts_; + TransferBufferManager& parent_bufmgr_; + + public: + TimedOutReceiverPredicate(MonotonicTime arg_ts, TransferBufferManager& arg_bufmgr) + : ts_(arg_ts) + , parent_bufmgr_(arg_bufmgr) + { } + + bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const; + }; + + bool checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const; + +protected: + void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba); + void handleAnonymousTransferReception(const RxFrame& frame); + + virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0; + +public: + TransferListener(TransferPerfCounter& perf, const DataTypeDescriptor& data_type, + uint16_t max_buffer_size, IPoolAllocator& allocator) + : data_type_(data_type) + , bufmgr_(max_buffer_size, allocator) + , receivers_(allocator) + , perf_(perf) + , crc_base_(data_type.getSignature().toTransferCRC()) + , allow_anonymous_transfers_(false) + { } + + virtual ~TransferListener(); + + const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; } + + /** + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. + */ + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } + + void cleanup(MonotonicTime ts); + + virtual void handleFrame(const RxFrame& frame); +}; + +/** + * This class is used by transfer listener to decide if the frame should be accepted or ignored. + */ +class ITransferAcceptanceFilter +{ +public: + /** + * If it returns false, the frame will be ignored, otherwise accepted. + */ + virtual bool shouldAcceptFrame(const RxFrame& frame) const = 0; + + virtual ~ITransferAcceptanceFilter() { } +}; + +/** + * This class should be derived by callers. + */ +class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener +{ + const ITransferAcceptanceFilter* filter_; + + virtual void handleFrame(const RxFrame& frame) override; + +public: + TransferListenerWithFilter(TransferPerfCounter& perf, const DataTypeDescriptor& data_type, + uint16_t max_buffer_size, IPoolAllocator& allocator) + : TransferListener(perf, data_type, max_buffer_size, allocator) + , filter_(UAVCAN_NULLPTR) + { } + + void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter) + { + filter_ = acceptance_filter; + } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_LISTENER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_receiver.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_receiver.hpp new file mode 100644 index 0000000000..20c4b41eea --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_receiver.hpp @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT TransferReceiver +{ +public: + enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame }; + + static const uint16_t MinTransferIntervalMSec = 1; + static const uint16_t MaxTransferIntervalMSec = 0xFFFF; + static const uint16_t DefaultTransferIntervalMSec = 1000; + static const uint16_t DefaultTidTimeoutMSec = 1000; + + static MonotonicDuration getDefaultTransferInterval() + { + return MonotonicDuration::fromMSec(DefaultTransferIntervalMSec); + } + static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromMSec(MinTransferIntervalMSec); } + static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromMSec(MaxTransferIntervalMSec); } + +private: + enum { IfaceIndexNotSet = MaxCanIfaces }; + + enum { ErrorCntMask = 31 }; + enum { IfaceIndexMask = MaxCanIfaces }; + + MonotonicTime prev_transfer_ts_; + MonotonicTime this_transfer_ts_; + UtcTime first_frame_ts_; + uint16_t transfer_interval_msec_; + uint16_t this_transfer_crc_; + + uint16_t buffer_write_pos_; + + TransferID tid_; // 1 byte field + + // 1 byte aligned bitfields: + uint8_t next_toggle_ : 1; + uint8_t iface_index_ : 2; + mutable uint8_t error_cnt_ : 5; + + bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; } + + bool isMidTransfer() const { return buffer_write_pos_ > 0; } + + MonotonicDuration getIfaceSwitchDelay() const; + MonotonicDuration getTidTimeout() const; + + void registerError() const; + + void updateTransferTimings(); + void prepareForNextTransfer(); + + bool validate(const RxFrame& frame) const; + bool writePayload(const RxFrame& frame, ITransferBuffer& buf); + ResultCode receive(const RxFrame& frame, TransferBufferAccessor& tba); + +public: + TransferReceiver() : + transfer_interval_msec_(DefaultTransferIntervalMSec), + this_transfer_crc_(0), + buffer_write_pos_(0), + next_toggle_(false), + iface_index_(IfaceIndexNotSet), + error_cnt_(0) + { } + + bool isTimedOut(MonotonicTime current_ts) const; + + ResultCode addFrame(const RxFrame& frame, TransferBufferAccessor& tba); + + uint8_t yieldErrorCount(); + + MonotonicTime getLastTransferTimestampMonotonic() const { return prev_transfer_ts_; } + UtcTime getLastTransferTimestampUtc() const { return first_frame_ts_; } + + uint16_t getLastTransferCrc() const { return this_transfer_crc_; } + + MonotonicDuration getInterval() const { return MonotonicDuration::fromMSec(transfer_interval_msec_); } +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_sender.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_sender.hpp new file mode 100644 index 0000000000..462e51ed39 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED +#define UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class UAVCAN_EXPORT TransferSender +{ + const MonotonicDuration max_transfer_interval_; + + Dispatcher& dispatcher_; + + TransferPriority priority_; + CanTxQueue::Qos qos_; + TransferCRC crc_base_; + DataTypeID data_type_id_; + CanIOFlags flags_; + uint8_t iface_mask_; + bool allow_anonymous_transfers_; + + void registerError() const; + +public: + enum { AllIfacesMask = 0xFF }; + + static MonotonicDuration getDefaultMaxTransferInterval() + { + return MonotonicDuration::fromMSec(60 * 1000); + } + + TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, + MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) + : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) + , priority_(TransferPriority::Default) + , qos_(CanTxQueue::Qos()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) + { + init(data_type, qos); + } + + TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) + : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) + , priority_(TransferPriority::Default) + , qos_(CanTxQueue::Qos()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) + { } + + void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos); + + bool isInitialized() const { return data_type_id_ != DataTypeID(); } + + CanIOFlags getCanIOFlags() const { return flags_; } + void setCanIOFlags(CanIOFlags flags) { flags_ = flags; } + + uint8_t getIfaceMask() const { return iface_mask_; } + void setIfaceMask(uint8_t iface_mask) + { + UAVCAN_ASSERT(iface_mask); + iface_mask_ = iface_mask; + } + + TransferPriority getPriority() const { return priority_; } + void setPriority(TransferPriority prio) { priority_ = prio; } + + /** + * Anonymous transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if + * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID). + * By default, this class will return an error if it is asked to send a transfer while the + * node is configured in passive mode. However, if this option is enabled, it will be possible + * to send anonymous transfers from passive mode. + */ + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } + + /** + * Send with explicit Transfer ID. + * Should be used only for service responses, where response TID should match request TID. + */ + int send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, + TransferID tid) const; + + /** + * Send with automatic Transfer ID. + * + * Note that as long as the local node operates in passive mode, the + * flag @ref CanIOFlagAbortOnError will be set implicitly for all outgoing frames. + * + * TID is managed by OutgoingTransferRegistry. + */ + int send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const; +}; + +} + +#endif // UAVCAN_TRANSPORT_TRANSFER_SENDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/uavcan.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/uavcan.hpp new file mode 100644 index 0000000000..9a0139aeb3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/uavcan.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + * + * This header should be included by the user application. + */ + +#ifndef UAVCAN_UAVCAN_HPP_INCLUDED +#define UAVCAN_UAVCAN_HPP_INCLUDED + +#include +#include + +// High-level node logic +#include +#include +#include +#include +#include +#include +#include + +// Util +#include +#include +#include + +#endif // UAVCAN_UAVCAN_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/bitset.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/bitset.hpp new file mode 100644 index 0000000000..3ef06494bb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/bitset.hpp @@ -0,0 +1,189 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_BITSET_HPP_INCLUDED +#define UAVCAN_UTIL_BITSET_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * STL-like bitset + */ +template +class UAVCAN_EXPORT BitSet +{ + enum { NumBytes = (NumBits + 7) / 8 }; + + static std::size_t getByteNum(std::size_t bit_num) { return bit_num / 8; } + + static std::size_t getBitNum(const std::size_t bit_num) { return bit_num % 8; } + + static void validatePos(std::size_t& inout_pos) + { + if (inout_pos >= NumBits) + { + UAVCAN_ASSERT(0); + inout_pos = NumBits - 1; + } + } + + char data_[NumBytes]; + +public: + class Reference + { + friend class BitSet; + + BitSet* const parent_; + const std::size_t bitpos_; + + Reference(BitSet* arg_parent, std::size_t arg_bitpos) + : parent_(arg_parent) + , bitpos_(arg_bitpos) + { } + + public: + Reference& operator=(bool x) + { + parent_->set(bitpos_, x); + return *this; + } + + Reference& operator=(const Reference& x) + { + parent_->set(bitpos_, x); + return *this; + } + + bool operator~() const + { + return !parent_->test(bitpos_); + } + + operator bool() const + { + return parent_->test(bitpos_); + } + }; + + BitSet() + : data_() + { + reset(); + } + + BitSet& reset() + { + std::memset(data_, 0, NumBytes); + return *this; + } + + BitSet& set() + { + std::memset(data_, 0xFF, NumBytes); + return *this; + } + + BitSet& set(std::size_t pos, bool val = true) + { + validatePos(pos); + if (val) + { + data_[getByteNum(pos)] = char(data_[getByteNum(pos)] | (1 << getBitNum(pos))); + } + else + { + data_[getByteNum(pos)] = char(data_[getByteNum(pos)] & ~(1 << getBitNum(pos))); + } + return *this; + } + + bool test(std::size_t pos) const + { + return (data_[getByteNum(pos)] & (1 << getBitNum(pos))) != 0; + } + + bool any() const + { + for (std::size_t i = 0; i < NumBits; ++i) + { + if (test(i)) + { + return true; + } + } + return false; + } + + std::size_t count() const + { + std::size_t retval = 0; + for (std::size_t i = 0; i < NumBits; ++i) + { + retval += test(i) ? 1U : 0U; + } + return retval; + } + + std::size_t size() const { return NumBits; } + + bool operator[](std::size_t pos) const + { + return test(pos); + } + + Reference operator[](std::size_t pos) + { + validatePos(pos); + return Reference(this, pos); + } + + BitSet& operator=(const BitSet & rhs) + { + if (&rhs == this) + { + return *this; + } + for (std::size_t i = 0; i < NumBytes; ++i) + { + data_[i] = rhs.data_[i]; + } + return *this; + } + + bool operator!=(const BitSet& rhs) const { return !operator==(rhs); } + bool operator==(const BitSet& rhs) const + { + for (std::size_t i = 0; i < NumBits; ++i) + { + if (test(i) != rhs.test(i)) + { + return false; + } + } + return true; + } +}; + +template <> class BitSet<0>; ///< Invalid instantiation + + +template +Stream& operator<<(Stream& s, const BitSet& x) +{ + for (std::size_t i = NumBits; i > 0; --i) + { + s << (x.test(i-1) ? "1" : "0"); + } + return s; +} + +} + +#endif // UAVCAN_UTIL_BITSET_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/comparison.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/comparison.hpp new file mode 100644 index 0000000000..587d8625aa --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/comparison.hpp @@ -0,0 +1,271 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_COMPARISON_HPP_INCLUDED +#define UAVCAN_UTIL_COMPARISON_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +/** + * Exact comparison of two floats that suppresses the compiler warnings. + */ +template +UAVCAN_EXPORT +inline bool areFloatsExactlyEqual(const T& left, const T& right) +{ + return (left <= right) && (left >= right); +} + +/** + * This function performs fuzzy comparison of two floating point numbers. + * Type of T can be either float, double or long double. + * For details refer to http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * See also: @ref UAVCAN_FLOAT_COMPARISON_EPSILON_MULT. + */ +template +UAVCAN_EXPORT +inline bool areFloatsClose(T a, T b, const T& absolute_epsilon, const T& relative_epsilon) +{ + // NAN + if (isNaN(a) || isNaN(b)) + { + return false; + } + + // Infinity + if (isInfinity(a) || isInfinity(b)) + { + return areFloatsExactlyEqual(a, b); + } + + // Close numbers near zero + const T diff = std::fabs(a - b); + if (diff <= absolute_epsilon) + { + return true; + } + + // General case + a = std::fabs(a); + b = std::fabs(b); + const T largest = (b > a) ? b : a; + return (diff <= largest * relative_epsilon); +} + +/** + * This namespace contains implementation details for areClose(). + * Don't try this at home. + */ +namespace are_close_impl_ +{ + +struct Applicable { char foo[1]; }; +struct NotApplicable { long long foo[16]; }; + +template +struct HasIsCloseMethod +{ + template struct ConstRef { }; + template struct ByValue { }; + + template static Applicable test(ConstRef*); + template static Applicable test(ByValue*); + + template static NotApplicable test(...); + + enum { Result = sizeof(test(UAVCAN_NULLPTR)) }; +}; + +/// First stage: bool L::isClose(R) +template +UAVCAN_EXPORT +inline bool areCloseImplFirst(const L& left, const R& right, IntToType) +{ + return left.isClose(right); +} + +/// Second stage: bool R::isClose(L) +template +UAVCAN_EXPORT +inline bool areCloseImplSecond(const L& left, const R& right, IntToType) +{ + return right.isClose(left); +} + +/// Second stage: L == R +template +UAVCAN_EXPORT +inline bool areCloseImplSecond(const L& left, const R& right, IntToType) +{ + return left == right; +} + +/// First stage: select either L == R or bool R::isClose(L) +template +UAVCAN_EXPORT +inline bool areCloseImplFirst(const L& left, const R& right, IntToType) +{ + return are_close_impl_::areCloseImplSecond(left, right, + IntToType::Result>()); +} + +} // namespace are_close_impl_ + +/** + * Generic fuzzy comparison function. + * + * This function properly handles floating point comparison, including mixed floating point type comparison, + * e.g. float with long double. + * + * Two objects of types A and B will be fuzzy comparable if either method is defined: + * - bool A::isClose(const B&) const + * - bool A::isClose(B) const + * - bool B::isClose(const A&) const + * - bool B::isClose(A) const + * + * Call areClose(A, B) will be dispatched as follows: + * + * - If A and B are both floating point types (float, double, long double) - possibly different - the call will be + * dispatched to @ref areFloatsClose(). If A and B are different types, value of the larger type will be coerced + * to the smaller type, e.g. areClose(long double, float) --> areClose(float, float). + * + * - If A defines isClose() that accepts B, the call will be dispatched there. + * + * - If B defines isClose() that accepts A, the call will be dispatched there (A/B swapped). + * + * - Last resort is A == B. + * + * Alternatively, a custom behavior can be implemented via template specialization. + * + * See also: @ref UAVCAN_FLOAT_COMPARISON_EPSILON_MULT. + * + * Examples: + * areClose(1.0, 1.0F) --> true + * areClose(1.0, 1.0F + std::numeric_limits::epsilon()) --> true + * areClose(1.0, 1.1) --> false + * areClose("123", std::string("123")) --> true (using std::string's operator ==) + * areClose(inf, inf) --> true + * areClose(inf, -inf) --> false + * areClose(nan, nan) --> false (any comparison with nan returns false) + * areClose(123, "123") --> compilation error: operator == is not defined + */ +template +UAVCAN_EXPORT +inline bool areClose(const L& left, const R& right) +{ + return are_close_impl_::areCloseImplFirst(left, right, + IntToType::Result>()); +} + +/* + * Float comparison specializations + */ +template <> +UAVCAN_EXPORT +inline bool areClose(const float& left, const float& right) +{ + return areFloatsClose(left, right, NumericTraits::epsilon(), + NumericTraits::epsilon() * FloatComparisonEpsilonMult); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const double& left, const double& right) +{ + return areFloatsClose(left, right, NumericTraits::epsilon(), + NumericTraits::epsilon() * FloatComparisonEpsilonMult); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const long double& left, const long double& right) +{ + return areFloatsClose(left, right, NumericTraits::epsilon(), + NumericTraits::epsilon() * FloatComparisonEpsilonMult); +} + +/* + * Mixed floating type comparison - coercing larger type to smaller type + */ +template <> +UAVCAN_EXPORT +inline bool areClose(const float& left, const double& right) +{ + return areClose(left, static_cast(right)); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const double& left, const float& right) +{ + return areClose(static_cast(left), right); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const float& left, const long double& right) +{ + return areClose(left, static_cast(right)); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const long double& left, const float& right) +{ + return areClose(static_cast(left), right); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const double& left, const long double& right) +{ + return areClose(left, static_cast(right)); +} + +template <> +UAVCAN_EXPORT +inline bool areClose(const long double& left, const double& right) +{ + return areClose(static_cast(left), right); +} + +/** + * Comparison against zero. + * Helps to compare a floating point number against zero if the exact type is unknown. + * For non-floating point types performs exact comparison against integer zero. + */ +template +UAVCAN_EXPORT +inline bool isCloseToZero(const T& x) +{ + return x == 0; +} + +template <> +UAVCAN_EXPORT +inline bool isCloseToZero(const float& x) +{ + return areClose(x, static_cast(0.0F)); +} + +template <> +UAVCAN_EXPORT +inline bool isCloseToZero(const double& x) +{ + return areClose(x, static_cast(0.0)); +} + +template <> +UAVCAN_EXPORT +inline bool isCloseToZero(const long double& x) +{ + return areClose(x, static_cast(0.0L)); +} + +} + +#endif // UAVCAN_UTIL_COMPARISON_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/lazy_constructor.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/lazy_constructor.hpp new file mode 100644 index 0000000000..c2076701de --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -0,0 +1,182 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED +#define UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED + +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * This class allows to postpone the object contruction. + * It statically allocates a pool of memory of just enough size to fit the object being constructed. + * Later call to construct<>() calls the constructor of the object. + * The object will be destroyed automatically when the container class destructor is called. + * The memory pool is aligned at the size of the largest primitive type (long double or long long int). + */ +template +class UAVCAN_EXPORT LazyConstructor +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + struct + { + alignas(T) unsigned char pool[sizeof(T)]; + } data_; +#else + union + { + unsigned char pool[sizeof(T)]; + long double _aligner1; + long long _aligner2; + } data_; +#endif + + T* ptr_; + + void ensureConstructed() const + { + if (!ptr_) + { + handleFatalError("LazyConstructor"); + } + } + + void ensureNotConstructed() const + { + if (ptr_) + { + handleFatalError("LazyConstructor"); + } + } + +public: + LazyConstructor() + : ptr_(UAVCAN_NULLPTR) + { + fill(data_.pool, data_.pool + sizeof(T), uint8_t(0)); + } + + LazyConstructor(const LazyConstructor& rhs) // Implicit + : ptr_(UAVCAN_NULLPTR) + { + fill(data_.pool, data_.pool + sizeof(T), uint8_t(0)); + if (rhs) + { + construct(*rhs); // Invoke copy constructor + } + } + + ~LazyConstructor() { destroy(); } + + LazyConstructor& operator=(const LazyConstructor& rhs) + { + destroy(); + if (rhs) + { + construct(*rhs); // Invoke copy constructor + } + return *this; + } + + bool isConstructed() const { return ptr_ != UAVCAN_NULLPTR; } + + operator T*() const { return ptr_; } + + const T* operator->() const { ensureConstructed(); return ptr_; } + T* operator->() { ensureConstructed(); return ptr_; } + + const T& operator*() const { ensureConstructed(); return *ptr_; } + T& operator*() { ensureConstructed(); return *ptr_; } + + void destroy() + { + if (ptr_) + { + ptr_->~T(); + } + ptr_ = UAVCAN_NULLPTR; + fill(data_.pool, data_.pool + sizeof(T), uint8_t(0)); + } + + void construct() + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(); + } + +// MAX_ARGS = 6 +// TEMPLATE = ''' +// template <%s> +// void construct(%s) +// { +// ensureNotConstructed(); +// ptr_ = new (static_cast(data_.pool)) T(%s); +// }''' +// for nargs in range(1, MAX_ARGS + 1): +// nums = [(x + 1) for x in range(nargs)] +// l1 = ['typename P%d' % x for x in nums] +// l2 = ['typename ParameterType::Type p%d' % (x, x) for x in nums] +// l3 = ['p%d' % x for x in nums] +// print(TEMPLATE % (', '.join(l1), ', '.join(l2), ', '.join(l3))) + + template + void construct(typename ParameterType::Type p1) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5); + } + + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5, typename ParameterType::Type p6) + { + ensureNotConstructed(); + ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5, p6); + } +}; + +} + +#endif // UAVCAN_UTIL_LAZY_CONSTRUCTOR_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/linked_list.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/linked_list.hpp new file mode 100644 index 0000000000..915eb9c7e9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/linked_list.hpp @@ -0,0 +1,176 @@ +/* + * Singly-linked list. + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED +#define UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Classes that are supposed to be linked-listed should derive this. + */ +template +class UAVCAN_EXPORT LinkedListNode : Noncopyable +{ + T* next_; + +protected: + LinkedListNode() + : next_(UAVCAN_NULLPTR) + { } + + ~LinkedListNode() { } + +public: + T* getNextListNode() const { return next_; } + + void setNextListNode(T* node) + { + next_ = node; + } +}; + +/** + * Linked list root. + */ +template +class UAVCAN_EXPORT LinkedListRoot : Noncopyable +{ + T* root_; + +public: + LinkedListRoot() + : root_(UAVCAN_NULLPTR) + { } + + T* get() const { return root_; } + bool isEmpty() const { return get() == UAVCAN_NULLPTR; } + + /** + * Complexity: O(N) + */ + unsigned getLength() const; + + /** + * Inserts the node to the beginning of the list. + * If the node is already present in the list, it will be relocated to the beginning. + * Complexity: O(N) + */ + void insert(T* node); + + /** + * Inserts the node immediately before the node X where predicate(X) returns true. + * If the node is already present in the list, it can be relocated to a new position. + * Complexity: O(2N) (calls remove()) + */ + template + void insertBefore(T* node, Predicate predicate); + + /** + * Removes only the first occurence of the node. + * Complexity: O(N) + */ + void remove(const T* node); +}; + +// ---------------------------------------------------------------------------- + +/* + * LinkedListRoot<> + */ +template +unsigned LinkedListRoot::getLength() const +{ + T* node = root_; + unsigned cnt = 0; + while (node) + { + cnt++; + node = node->getNextListNode(); + } + return cnt; +} + +template +void LinkedListRoot::insert(T* node) +{ + if (node == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return; + } + remove(node); // Making sure there will be no loops + node->setNextListNode(root_); + root_ = node; +} + +template +template +void LinkedListRoot::insertBefore(T* node, Predicate predicate) +{ + if (node == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return; + } + + remove(node); + + if (root_ == UAVCAN_NULLPTR || predicate(root_)) + { + node->setNextListNode(root_); + root_ = node; + } + else + { + T* p = root_; + while (p->getNextListNode()) + { + if (predicate(p->getNextListNode())) + { + break; + } + p = p->getNextListNode(); + } + node->setNextListNode(p->getNextListNode()); + p->setNextListNode(node); + } +} + +template +void LinkedListRoot::remove(const T* node) +{ + if (root_ == UAVCAN_NULLPTR || node == UAVCAN_NULLPTR) + { + return; + } + + if (root_ == node) + { + root_ = root_->getNextListNode(); + } + else + { + T* p = root_; + while (p->getNextListNode()) + { + if (p->getNextListNode() == node) + { + p->setNextListNode(p->getNextListNode()->getNextListNode()); + break; + } + p = p->getNextListNode(); + } + } +} + +} + +#endif // UAVCAN_UTIL_LINKED_LIST_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/map.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/map.hpp new file mode 100644 index 0000000000..59ce72da74 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/map.hpp @@ -0,0 +1,388 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MAP_HPP_INCLUDED +#define UAVCAN_UTIL_MAP_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Slow but memory efficient KV container. + * + * KV pairs will be allocated in the node's memory pool. + * + * Please be aware that this container does not perform any speed optimizations to minimize memory footprint, + * so the complexity of most operations is O(N). + * + * Type requirements: + * Both key and value must be copyable, assignable and default constructible. + * Key must implement a comparison operator. + * Key's default constructor must initialize the object into invalid state. + * Size of Key + Value + padding must not exceed MemPoolBlockSize. + */ +template +class UAVCAN_EXPORT Map : Noncopyable +{ +public: + struct KVPair + { + Value value; // Key and value are swapped because this may allow to reduce padding (depending on types) + Key key; + + KVPair() : + value(), + key() + { } + + KVPair(const Key& arg_key, const Value& arg_value) : + value(arg_value), + key(arg_key) + { } + + bool match(const Key& rhs) const { return rhs == key; } + }; + +private: + struct KVGroup : LinkedListNode + { + enum { NumKV = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(KVPair) }; + KVPair kvs[NumKV]; + + KVGroup() + { + StaticAssert<(static_cast(NumKV) > 0)>::check(); + IsDynamicallyAllocatable::check(); + } + + static KVGroup* instantiate(IPoolAllocator& allocator) + { + void* const praw = allocator.allocate(sizeof(KVGroup)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) KVGroup(); + } + + static void destroy(KVGroup*& obj, IPoolAllocator& allocator) + { + if (obj != UAVCAN_NULLPTR) + { + obj->~KVGroup(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } + } + + KVPair* find(const Key& key) + { + for (unsigned i = 0; i < static_cast(NumKV); i++) + { + if (kvs[i].match(key)) + { + return kvs + i; + } + } + return UAVCAN_NULLPTR; + } + }; + + LinkedListRoot list_; + IPoolAllocator& allocator_; + + KVPair* findKey(const Key& key); + + void compact(); + + struct YesPredicate + { + bool operator()(const Key&, const Value&) const { return true; } + }; + +public: + Map(IPoolAllocator& allocator) : + allocator_(allocator) + { + UAVCAN_ASSERT(Key() == Key()); + } + + ~Map() + { + clear(); + } + + /** + * Returns null pointer if there's no such entry. + */ + Value* access(const Key& key); + + /** + * If entry with the same key already exists, it will be replaced + */ + Value* insert(const Key& key, const Value& value); + + /** + * Does nothing if there's no such entry. + */ + void remove(const Key& key); + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (Key& key, Value& value) + */ + template + void removeAllWhere(Predicate predicate); + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const Key& key, const Value& value) + */ + template + const Key* find(Predicate predicate) const; + + /** + * Removes all items. + */ + void clear(); + + /** + * Returns a key-value pair located at the specified position from the beginning. + * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. + * If index is greater than or equal the number of pairs, null pointer will be returned. + */ + KVPair* getByIndex(unsigned index); + const KVPair* getByIndex(unsigned index) const; + + /** + * Complexity is O(1). + */ + bool isEmpty() const { return find(YesPredicate()) == UAVCAN_NULLPTR; } + + /** + * Complexity is O(N). + */ + unsigned getSize() const; +}; + +// ---------------------------------------------------------------------------- + +/* + * Map<> + */ +template +typename Map::KVPair* Map::findKey(const Key& key) +{ + KVGroup* p = list_.get(); + while (p) + { + KVPair* const kv = p->find(key); + if (kv) + { + return kv; + } + p = p->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +template +void Map::compact() +{ + KVGroup* p = list_.get(); + while (p) + { + KVGroup* const next = p->getNextListNode(); + bool remove_this = true; + for (int i = 0; i < KVGroup::NumKV; i++) + { + if (!p->kvs[i].match(Key())) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + KVGroup::destroy(p, allocator_); + } + p = next; + } +} + +template +Value* Map::access(const Key& key) +{ + UAVCAN_ASSERT(!(key == Key())); + KVPair* const kv = findKey(key); + return kv ? &kv->value : UAVCAN_NULLPTR; +} + +template +Value* Map::insert(const Key& key, const Value& value) +{ + UAVCAN_ASSERT(!(key == Key())); + remove(key); + + KVPair* const kv = findKey(Key()); + if (kv) + { + *kv = KVPair(key, value); + return &kv->value; + } + + KVGroup* const kvg = KVGroup::instantiate(allocator_); + if (kvg == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + list_.insert(kvg); + kvg->kvs[0] = KVPair(key, value); + return &kvg->kvs[0].value; +} + +template +void Map::remove(const Key& key) +{ + UAVCAN_ASSERT(!(key == Key())); + KVPair* const kv = findKey(key); + if (kv) + { + *kv = KVPair(); + compact(); + } +} + +template +template +void Map::removeAllWhere(Predicate predicate) +{ + unsigned num_removed = 0; + + KVGroup* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + KVGroup* const next_group = p->getNextListNode(); + + for (int i = 0; i < KVGroup::NumKV; i++) + { + const KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + if (predicate(kv->key, kv->value)) + { + num_removed++; + p->kvs[i] = KVPair(); + } + } + } + + p = next_group; + } + + if (num_removed > 0) + { + compact(); + } +} + +template +template +const Key* Map::find(Predicate predicate) const +{ + KVGroup* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + KVGroup* const next_group = p->getNextListNode(); + + for (int i = 0; i < KVGroup::NumKV; i++) + { + const KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + if (predicate(kv->key, kv->value)) + { + return &p->kvs[i].key; + } + } + } + + p = next_group; + } + return UAVCAN_NULLPTR; +} + +template +void Map::clear() +{ + removeAllWhere(YesPredicate()); +} + +template +typename Map::KVPair* Map::getByIndex(unsigned index) +{ + // Slowly crawling through the dynamic storage + KVGroup* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + KVGroup* const next_group = p->getNextListNode(); + + for (int i = 0; i < KVGroup::NumKV; i++) + { + KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + if (index == 0) + { + return kv; + } + index--; + } + } + + p = next_group; + } + + return UAVCAN_NULLPTR; +} + +template +const typename Map::KVPair* Map::getByIndex(unsigned index) const +{ + return const_cast*>(this)->getByIndex(index); +} + +template +unsigned Map::getSize() const +{ + unsigned num = 0; + KVGroup* p = list_.get(); + while (p) + { + for (int i = 0; i < KVGroup::NumKV; i++) + { + const KVPair* const kv = p->kvs + i; + if (!kv->match(Key())) + { + num++; + } + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // UAVCAN_UTIL_MAP_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/method_binder.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/method_binder.hpp new file mode 100644 index 0000000000..f3bbf424b4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/method_binder.hpp @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED +#define UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED + +#include +#include +#include + +namespace uavcan +{ +/** + * Use this to call member functions as callbacks in C++03 mode. + * + * In C++11 or newer you don't need it because you can use std::function<>/std::bind<> instead. + */ +template +class UAVCAN_EXPORT MethodBinder +{ + ObjectPtr obj_; + MemFunPtr fun_; + + void validateBeforeCall() const + { + if (!operator bool()) + { + handleFatalError("Null binder"); + } + } + +public: + MethodBinder() + : obj_() + , fun_() + { } + + MethodBinder(ObjectPtr o, MemFunPtr f) + : obj_(o) + , fun_(f) + { } + + /** + * Returns true if the binder is initialized (doesn't contain null pointers). + */ + operator bool() const + { + return coerceOrFallback(obj_, true) && coerceOrFallback(fun_, true); + } + + /** + * Will raise a fatal error if either method pointer or object pointer are null. + */ + void operator()() + { + validateBeforeCall(); + (obj_->*fun_)(); + } + + /** + * Will raise a fatal error if either method pointer or object pointer are null. + */ + template + void operator()(Par1& p1) + { + validateBeforeCall(); + (obj_->*fun_)(p1); + } + + /** + * Will raise a fatal error if either method pointer or object pointer are null. + */ + template + void operator()(Par1& p1, Par2& p2) + { + validateBeforeCall(); + (obj_->*fun_)(p1, p2); + } +}; + +} + +#endif // UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/multiset.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/multiset.hpp new file mode 100644 index 0000000000..5813466c62 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/multiset.hpp @@ -0,0 +1,478 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED +#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * Slow but memory efficient unordered multiset. Unlike Map<>, this container does not move objects, so + * they don't have to be copyable. + * + * Items will be allocated in the node's memory pool. + */ +template +class UAVCAN_EXPORT Multiset : Noncopyable +{ + struct Item : ::uavcan::Noncopyable + { + T* ptr; + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + alignas(T) unsigned char pool[sizeof(T)]; ///< Memory efficient version +#else + union + { + unsigned char pool[sizeof(T)]; + /* + * Such alignment does not guarantee safety for all types (only for libuavcan internal ones); + * however, increasing it is too memory inefficient. So it is recommended to use C++11, where + * this issue is resolved with alignas() (see above). + */ + void* _aligner1_; + long long _aligner2_; + }; +#endif + + Item() + : ptr(UAVCAN_NULLPTR) + { + fill_n(pool, sizeof(pool), static_cast(0)); + } + + ~Item() { destroy(); } + + bool isConstructed() const { return ptr != UAVCAN_NULLPTR; } + + void destroy() + { + if (ptr != UAVCAN_NULLPTR) + { + ptr->~T(); + ptr = UAVCAN_NULLPTR; + fill_n(pool, sizeof(pool), static_cast(0)); + } + } + }; + +private: + struct Chunk : LinkedListNode + { + enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; + Item items[NumItems]; + + Chunk() + { + StaticAssert<(static_cast(NumItems) > 0)>::check(); + IsDynamicallyAllocatable::check(); + UAVCAN_ASSERT(!items[0].isConstructed()); + } + + static Chunk* instantiate(IPoolAllocator& allocator) + { + void* const praw = allocator.allocate(sizeof(Chunk)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) Chunk(); + } + + static void destroy(Chunk*& obj, IPoolAllocator& allocator) + { + if (obj != UAVCAN_NULLPTR) + { + obj->~Chunk(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } + } + + Item* findFreeSlot() + { + for (unsigned i = 0; i < static_cast(NumItems); i++) + { + if (!items[i].isConstructed()) + { + return items + i; + } + } + return UAVCAN_NULLPTR; + } + }; + + /* + * Data + */ + LinkedListRoot list_; + IPoolAllocator& allocator_; + + /* + * Methods + */ + Item* findOrCreateFreeSlot(); + + void compact(); + + enum RemoveStrategy { RemoveOne, RemoveAll }; + + template + void removeWhere(Predicate predicate, RemoveStrategy strategy); + + struct YesPredicate + { + bool operator()(const T&) const { return true; } + }; + + struct IndexPredicate : ::uavcan::Noncopyable + { + unsigned index; + IndexPredicate(unsigned target_index) + : index(target_index) + { } + + bool operator()(const T&) + { + return index-- == 0; + } + }; + + struct ComparingPredicate + { + const T& reference; + + ComparingPredicate(const T& ref) + : reference(ref) + { } + + bool operator()(const T& sample) + { + return reference == sample; + } + }; + + template + struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable + { + Operator oper; + + OperatorToFalsePredicateAdapter(Operator o) + : oper(o) + { } + + bool operator()(T& item) + { + oper(item); + return false; + } + + bool operator()(const T& item) const + { + oper(item); + return false; + } + }; + +public: + Multiset(IPoolAllocator& allocator) + : allocator_(allocator) + { } + + ~Multiset() + { + clear(); + } + + /** + * Creates one item in-place and returns a pointer to it. + * If creation fails due to lack of memory, UAVCAN_NULLPTR will be returned. + * Complexity is O(N). + */ + T* emplace() + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(); + return item->ptr; + } + + template + T* emplace(P1 p1) + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(p1); + return item->ptr; + } + + template + T* emplace(P1 p1, P2 p2) + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(p1, p2); + return item->ptr; + } + + template + T* emplace(P1 p1, P2 p2, P3 p3) + { + Item* const item = findOrCreateFreeSlot(); + if (item == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_ASSERT(item->ptr == UAVCAN_NULLPTR); + item->ptr = new (item->pool) T(p1, p2, p3); + return item->ptr; + } + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (T& item) + */ + template + void removeAllWhere(Predicate predicate) { removeWhere(predicate, RemoveAll); } + + template + void removeFirstWhere(Predicate predicate) { removeWhere(predicate, RemoveOne); } + + void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); } + + void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); } + + void clear() { removeAllWhere(YesPredicate()); } + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const T& item) + */ + template + T* find(Predicate predicate); + + template + const T* find(Predicate predicate) const + { + return const_cast(this)->find(predicate); + } + + /** + * Calls Operator for each item of the set. + * Operator prototype: + * void (T& item) + * void (const T& item) - const overload + */ + template + void forEach(Operator oper) + { + OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + + template + void forEach(Operator oper) const + { + const OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + + /** + * Returns an item located at the specified position from the beginning. + * Note that addition and removal operations invalidate indices. + * If index is greater than or equal the number of items, null pointer will be returned. + * Complexity is O(N). + */ + T* getByIndex(unsigned index) + { + IndexPredicate predicate(index); + return find(predicate); + } + + const T* getByIndex(unsigned index) const + { + return const_cast(this)->getByIndex(index); + } + + /** + * Complexity is O(1). + */ + bool isEmpty() const { return find(YesPredicate()) == UAVCAN_NULLPTR; } + + /** + * Counts number of items stored. + * Best case complexity is O(N). + */ + unsigned getSize() const; +}; + +// ---------------------------------------------------------------------------- + +/* + * Multiset<> + */ +template +typename Multiset::Item* Multiset::findOrCreateFreeSlot() +{ + // Search + { + Chunk* p = list_.get(); + while (p) + { + Item* const dyn = p->findFreeSlot(); + if (dyn != UAVCAN_NULLPTR) + { + return dyn; + } + p = p->getNextListNode(); + } + } + + // Create new chunk + Chunk* const chunk = Chunk::instantiate(allocator_); + if (chunk == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + list_.insert(chunk); + return &chunk->items[0]; +} + +template +void Multiset::compact() +{ + Chunk* p = list_.get(); + while (p) + { + Chunk* const next = p->getNextListNode(); + bool remove_this = true; + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i].isConstructed()) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + Chunk::destroy(p, allocator_); + } + p = next; + } +} + +template +template +void Multiset::removeWhere(Predicate predicate, const RemoveStrategy strategy) +{ + unsigned num_removed = 0; + + Chunk* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified + + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } + + for (int i = 0; i < Chunk::NumItems; i++) + { + Item& item = p->items[i]; + if (item.isConstructed()) + { + if (predicate(*item.ptr)) + { + num_removed++; + item.destroy(); + if (strategy == RemoveOne) + { + break; + } + } + } + } + + p = next_chunk; + } + + if (num_removed > 0) + { + compact(); + } +} + +template +template +T* Multiset::find(Predicate predicate) +{ + Chunk* p = list_.get(); + while (p != UAVCAN_NULLPTR) + { + Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified + + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i].isConstructed()) + { + if (predicate(*p->items[i].ptr)) + { + return p->items[i].ptr; + } + } + } + + p = next_chunk; + } + return UAVCAN_NULLPTR; +} + +template +unsigned Multiset::getSize() const +{ + unsigned num = 0; + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + num += p->items[i].isConstructed() ? 1U : 0U; + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/placement_new.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/placement_new.hpp new file mode 100644 index 0000000000..8c13308111 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/placement_new.hpp @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED +#define UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED + +#include +#include + +/* + * Some embedded C++ implementations don't implement the placement new operator. + * Define UAVCAN_IMPLEMENT_PLACEMENT_NEW to apply this workaround. + */ + +#ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW +# error UAVCAN_IMPLEMENT_PLACEMENT_NEW +#endif + +#if UAVCAN_IMPLEMENT_PLACEMENT_NEW + +inline void* operator new (std::size_t, void* ptr) throw() +{ + return ptr; +} +inline void* operator new[](std::size_t, void* ptr) throw() +{ + return ptr; +} + +inline void operator delete (void*, void*) throw() { } +inline void operator delete[](void*, void*) throw() { } + +#else +# include +#endif + +#endif // UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/templates.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/templates.hpp new file mode 100644 index 0000000000..00abb7c678 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/util/templates.hpp @@ -0,0 +1,557 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED +#define UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED + +#include +#include +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# include // cfloat may not be available +#else +# include // C++11 mode assumes that all standard headers are available +#endif + +namespace uavcan +{ +/** + * Usage: + * StaticAssert::check(); + */ +template +struct UAVCAN_EXPORT StaticAssert; + +template <> +struct UAVCAN_EXPORT StaticAssert +{ + static void check() { } +}; + +/** + * Usage: + * ShowIntegerAsError::foobar(); + */ +template struct ShowIntegerAsError; + +/** + * Prevents copying when inherited + */ +class UAVCAN_EXPORT Noncopyable +{ + Noncopyable(const Noncopyable&); + Noncopyable& operator=(const Noncopyable&); +protected: + Noncopyable() { } + ~Noncopyable() { } +}; + +/** + * Compile time conditions + */ +template +struct UAVCAN_EXPORT EnableIf { }; + +template +struct UAVCAN_EXPORT EnableIf +{ + typedef T Type; +}; + +/** + * Lightweight type categorization. + */ +template +struct UAVCAN_EXPORT EnableIfType +{ + typedef R Type; +}; + +/** + * Compile-time type selection (Alexandrescu) + */ +template +struct UAVCAN_EXPORT Select; + +template +struct UAVCAN_EXPORT Select +{ + typedef TrueType Result; +}; + +template +struct UAVCAN_EXPORT Select +{ + typedef FalseType Result; +}; + +/** + * Checks if two identifiers refer to the same type. + */ +template +struct IsSameType +{ + enum { Result = 0 }; +}; + +template +struct IsSameType +{ + enum { Result = 1 }; +}; + +/** + * Remove reference as in + */ +template struct RemoveReference { typedef T Type; }; +template struct RemoveReference { typedef T Type; }; +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 +template struct RemoveReference { typedef T Type; }; +#endif + +/** + * Parameter types + */ +template struct ParameterType { typedef const U& Type; }; +template struct ParameterType { typedef U& Type; }; +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 +template struct ParameterType { typedef U&& Type; }; +#endif + +/** + * Value types + */ +template struct UAVCAN_EXPORT BooleanType { }; +typedef BooleanType TrueType; +typedef BooleanType FalseType; + +template struct IntToType { }; + +/** + * Relations + */ +template +class UAVCAN_EXPORT IsImplicitlyConvertibleFromTo +{ + template static U returner(); + + struct True_ { char x[2]; }; + struct False_ { }; + + static True_ test(const T2 &); + static False_ test(...); + +public: + enum { Result = sizeof(True_) == sizeof(IsImplicitlyConvertibleFromTo::test(returner())) }; +}; + +/** + * coerceOrFallback(From) + * coerceOrFallback(From, To) + * @{ + */ +template +struct UAVCAN_EXPORT CoerceOrFallbackImpl +{ + static To impl(const From& from, const To&, TrueType) { return To(from); } + static To impl(const From&, const To& default_, FalseType) { return default_; } +}; + +/** + * If possible, performs an implicit cast from the type From to the type To. + * If the cast is not possible, returns default_ of type To. + */ +template +UAVCAN_EXPORT +To coerceOrFallback(const From& from, const To& default_) +{ + return CoerceOrFallbackImpl::impl(from, default_, + BooleanType::Result>()); +} + +/** + * If possible, performs an implicit cast from the type From to the type To. + * If the cast is not possible, returns a default constructed object of the type To. + */ +template +UAVCAN_EXPORT +To coerceOrFallback(const From& from) +{ + return CoerceOrFallbackImpl::impl(from, To(), + BooleanType::Result>()); +} +/** + * @} + */ + +/** + * Selects smaller value + */ +template +struct EnumMin +{ + enum { Result = (A < B) ? A : B }; +}; + +/** + * Selects larger value + */ +template +struct EnumMax +{ + enum { Result = (A > B) ? A : B }; +}; + +/** + * Compile time square root for integers. + * Useful for operations on square matrices. + */ +template struct UAVCAN_EXPORT CompileTimeIntSqrt; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<1> { enum { Result = 1 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<4> { enum { Result = 2 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<9> { enum { Result = 3 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<16> { enum { Result = 4 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<25> { enum { Result = 5 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<36> { enum { Result = 6 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<49> { enum { Result = 7 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<64> { enum { Result = 8 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<81> { enum { Result = 9 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<100> { enum { Result = 10 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<121> { enum { Result = 11 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<144> { enum { Result = 12 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<169> { enum { Result = 13 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<196> { enum { Result = 14 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<225> { enum { Result = 15 }; }; +template <> struct UAVCAN_EXPORT CompileTimeIntSqrt<256> { enum { Result = 16 }; }; + +/** + * Replacement for std::copy(..) + */ +template +UAVCAN_EXPORT +OutputIt copy(InputIt first, InputIt last, OutputIt result) +{ + while (first != last) + { + *result = *first; + ++first; + ++result; + } + return result; +} + +/** + * Replacement for std::fill(..) + */ +template +UAVCAN_EXPORT +void fill(ForwardIt first, ForwardIt last, const T& value) +{ + while (first != last) + { + *first = value; + ++first; + } +} + +/** + * Replacement for std::fill_n(..) + */ +template +UAVCAN_EXPORT +void fill_n(OutputIt first, std::size_t n, const T& value) +{ + while (n--) + { + *first++ = value; + } +} + +/** + * Replacement for std::min(..) + */ +template +UAVCAN_EXPORT +const T& min(const T& a, const T& b) +{ + return (b < a) ? b : a; +} + +/** + * Replacement for std::max(..) + */ +template +UAVCAN_EXPORT +const T& max(const T& a, const T& b) +{ + return (a < b) ? b : a; +} + +/** + * Replacement for std::lexicographical_compare(..) + */ +template +UAVCAN_EXPORT +bool lexicographical_compare(InputIt1 first1, InputIt1 last1, InputIt2 first2, InputIt2 last2) +{ + while ((first1 != last1) && (first2 != last2)) + { + if (*first1 < *first2) + { + return true; + } + if (*first2 < *first1) + { + return false; + } + ++first1; + ++first2; + } + return (first1 == last1) && (first2 != last2); +} + +/** + * Replacement for std::equal(..) + */ +template +UAVCAN_EXPORT +bool equal(InputIt1 first1, InputIt1 last1, InputIt2 first2) +{ + while (first1 != last1) + { + if (*first1 != *first2) + { + return false; + } + ++first1; + ++first2; + } + return true; +} + +/** + * Numeric traits, like std::numeric_limits<> + */ +template +struct UAVCAN_EXPORT NumericTraits; + +/// bool +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static bool max() { return true; } + static bool min() { return false; } +}; + +/// char +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static char max() { return CHAR_MAX; } + static char min() { return CHAR_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static signed char max() { return SCHAR_MAX; } + static signed char min() { return SCHAR_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned char max() { return UCHAR_MAX; } + static unsigned char min() { return 0; } +}; + +/// short +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static short max() { return SHRT_MAX; } + static short min() { return SHRT_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned short max() { return USHRT_MAX; } + static unsigned short min() { return 0; } +}; + +/// int +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static int max() { return INT_MAX; } + static int min() { return INT_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned int max() { return UINT_MAX; } + static unsigned int min() { return 0; } +}; + +/// long +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static long max() { return LONG_MAX; } + static long min() { return LONG_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned long max() { return ULONG_MAX; } + static unsigned long min() { return 0; } +}; + +/// long long +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 1 }; + static long long max() { return LLONG_MAX; } + static long long min() { return LLONG_MIN; } +}; +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 0 }; + enum { IsInteger = 1 }; + static unsigned long long max() { return ULLONG_MAX; } + static unsigned long long min() { return 0; } +}; + +/// float +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 0 }; + static float max() { return FLT_MAX; } + static float min() { return FLT_MIN; } + static float infinity() { return INFINITY; } + static float epsilon() { return FLT_EPSILON; } +}; + +/// double +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 0 }; + static double max() { return DBL_MAX; } + static double min() { return DBL_MIN; } + static double infinity() { return static_cast(INFINITY) * static_cast(INFINITY); } + static double epsilon() { return DBL_EPSILON; } +}; + +#if defined(LDBL_MAX) && defined(LDBL_MIN) && defined(LDBL_EPSILON) +/// long double +template <> +struct UAVCAN_EXPORT NumericTraits +{ + enum { IsSigned = 1 }; + enum { IsInteger = 0 }; + static long double max() { return LDBL_MAX; } + static long double min() { return LDBL_MIN; } + static long double infinity() { return static_cast(INFINITY) * static_cast(INFINITY); } + static long double epsilon() { return LDBL_EPSILON; } +}; +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# undef isnan +# undef isinf +# undef signbit +#endif + +/** + * Replacement for std::isnan(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool isNaN(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::isnan(arg); +#else + // coverity[same_on_both_sides : FALSE] + // cppcheck-suppress duplicateExpression + return !(arg <= arg); +#endif +} + +/** + * Replacement for std::isinf(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool isInfinity(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::isinf(arg); +#else + return (arg >= NumericTraits::infinity()) || (arg <= -NumericTraits::infinity()); +#endif +} + +/** + * Replacement for std::isfinite(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool isFinite(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::isfinite(arg); +#else + return !isNaN(arg) && !isInfinity(arg); +#endif +} + +/** + * Replacement for std::signbit(). + * Note that direct float comparison (==, !=) is intentionally avoided. + */ +template +inline bool getSignBit(T arg) +{ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + return std::signbit(arg); +#else + // coverity[divide_by_zero : FALSE] + return arg < T(0) || (((arg <= T(0)) && (arg >= T(0))) && (T(1) / arg < T(0))); +#endif +} + +} + +#endif // UAVCAN_UTIL_TEMPLATES_HPP_INCLUDED diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/driver/uc_can.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/driver/uc_can.cpp new file mode 100644 index 0000000000..739740343b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/driver/uc_can.cpp @@ -0,0 +1,118 @@ +/* + * CAN bus driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +const uint32_t CanFrame::MaskStdID; +const uint32_t CanFrame::MaskExtID; +const uint32_t CanFrame::FlagEFF; +const uint32_t CanFrame::FlagRTR; +const uint32_t CanFrame::FlagERR; +const uint8_t CanFrame::MaxDataLen; + +bool CanFrame::priorityHigherThan(const CanFrame& rhs) const +{ + const uint32_t clean_id = id & MaskExtID; + const uint32_t rhs_clean_id = rhs.id & MaskExtID; + + /* + * STD vs EXT - if 11 most significant bits are the same, EXT loses. + */ + const bool ext = id & FlagEFF; + const bool rhs_ext = rhs.id & FlagEFF; + if (ext != rhs_ext) + { + const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id; + const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id; + if (arb11 != rhs_arb11) + { + return arb11 < rhs_arb11; + } + else + { + return rhs_ext; + } + } + + /* + * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses. + */ + const bool rtr = id & FlagRTR; + const bool rhs_rtr = rhs.id & FlagRTR; + if (clean_id == rhs_clean_id && rtr != rhs_rtr) + { + return rhs_rtr; + } + + /* + * Plain ID arbitration - greater value loses. + */ + return clean_id < rhs_clean_id; +} + +#if UAVCAN_TOSTRING +std::string CanFrame::toString(StringRepresentation mode) const +{ + UAVCAN_ASSERT(mode == StrTight || mode == StrAligned); + + static const unsigned AsciiColumnOffset = 36U; + + char buf[50]; + char* wpos = buf; + char* const epos = buf + sizeof(buf); + fill(buf, buf + sizeof(buf), '\0'); + + if (id & FlagEFF) + { + wpos += snprintf(wpos, unsigned(epos - wpos), "0x%08x ", unsigned(id & MaskExtID)); + } + else + { + const char* const fmt = (mode == StrAligned) ? " 0x%03x " : "0x%03x "; + wpos += snprintf(wpos, unsigned(epos - wpos), fmt, unsigned(id & MaskStdID)); + } + + if (id & FlagRTR) + { + wpos += snprintf(wpos, unsigned(epos - wpos), " RTR"); + } + else if (id & FlagERR) + { + wpos += snprintf(wpos, unsigned(epos - wpos), " ERR"); + } + else + { + for (int dlen = 0; dlen < dlc; dlen++) // hex bytes + { + wpos += snprintf(wpos, unsigned(epos - wpos), " %02x", unsigned(data[dlen])); + } + + while ((mode == StrAligned) && (wpos < buf + AsciiColumnOffset)) // alignment + { + *wpos++ = ' '; + } + + wpos += snprintf(wpos, unsigned(epos - wpos), " \'"); // ascii + for (int dlen = 0; dlen < dlc; dlen++) + { + uint8_t ch = data[dlen]; + if (ch < 0x20 || ch > 0x7E) + { + ch = '.'; + } + wpos += snprintf(wpos, unsigned(epos - wpos), "%c", ch); + } + wpos += snprintf(wpos, unsigned(epos - wpos), "\'"); + } + (void)wpos; + return std::string(buf); +} +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_array_copy.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_array_copy.cpp new file mode 100644 index 0000000000..5657655a45 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_array_copy.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ +void bitarrayCopy(const unsigned char* src, std::size_t src_offset, std::size_t src_len, + unsigned char* dst, std::size_t dst_offset) +{ + /* + * Should never be called on a zero-length buffer. The caller will also ensure that the bit + * offsets never exceed one byte. + */ + + UAVCAN_ASSERT(src_len > 0U); + UAVCAN_ASSERT(src_offset < 8U && dst_offset < 8U); + + const std::size_t last_bit = src_offset + src_len; + while (last_bit - src_offset) + { + const uint8_t src_bit_offset = src_offset % 8U; + const uint8_t dst_bit_offset = dst_offset % 8U; + + // The number of bits to copy + const uint8_t max_offset = uavcan::max(src_bit_offset, dst_bit_offset); + const std::size_t copy_bits = uavcan::min(last_bit - src_offset, std::size_t(8U - max_offset)); + + /* + * The mask indicating which bits of dest to update: + * dst_byte_offset copy_bits write_mask + * 0 8 11111111 + * 0 7 11111110 + * ... + * 0 1 10000000 + * ... + * 4 4 00001111 + * 4 3 00001110 + * 4 2 00001100 + * 4 1 00001000 + * ... + * 7 1 00000001 + */ + const uint8_t write_mask = uint8_t(uint8_t(0xFF00U >> copy_bits) >> dst_bit_offset); + + // The value to be extracted from src, shifted into the dst location + const uint8_t src_data = uint8_t((src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset); + + dst[dst_offset / 8U] = uint8_t((dst[dst_offset / 8U] & ~write_mask) | (src_data & write_mask)); + + src_offset += copy_bits; + dst_offset += copy_bits; + } +} +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_stream.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_stream.cpp new file mode 100644 index 0000000000..fba0153fb2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_bit_stream.cpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +const unsigned BitStream::MaxBytesPerRW; +const unsigned BitStream::MaxBitsPerRW; + +int BitStream::write(const uint8_t* bytes, const unsigned bitlen) +{ + // Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_) + uint8_t tmp[MaxBytesPerRW + 1]; + + // Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write() + const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); + UAVCAN_ASSERT(MaxBytesPerRW >= bytelen); + tmp[0] = tmp[bytelen - 1] = 0; + + fill(tmp, tmp + bytelen, uint8_t(0)); + copyBitArrayAlignedToUnaligned(bytes, bitlen, tmp, bit_offset_ % 8); + + const unsigned new_bit_offset = bit_offset_ + bitlen; + + // Bitcopy algorithm resets skipped bits in the first byte. Restore them back. + tmp[0] |= byte_cache_; + + // (new_bit_offset % 8 == 0) means that this write was perfectly aligned. + byte_cache_ = uint8_t((new_bit_offset % 8) ? tmp[bytelen - 1] : 0); + + /* + * Dump the data into the destination buffer. + * Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value + * within the next write() operation. + */ + const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen); + if (write_res < 0) + { + return write_res; + } + if (static_cast(write_res) < bytelen) + { + return ResultOutOfBuffer; + } + + bit_offset_ = new_bit_offset; + return ResultOk; +} + +int BitStream::read(uint8_t* bytes, const unsigned bitlen) +{ + uint8_t tmp[MaxBytesPerRW + 1]; + + const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); + UAVCAN_ASSERT(MaxBytesPerRW >= bytelen); + + const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen); + if (read_res < 0) + { + return read_res; + } + if (static_cast(read_res) < bytelen) + { + return ResultOutOfBuffer; + } + + fill(bytes, bytes + bitlenToBytelen(bitlen), uint8_t(0)); + copyBitArrayUnalignedToAligned(tmp, bit_offset_ % 8, bitlen, bytes); + bit_offset_ += bitlen; + return ResultOk; +} + +#if UAVCAN_TOSTRING +std::string BitStream::toString() const +{ + std::string out; + out.reserve(128); + + for (unsigned offset = 0; true; offset++) + { + uint8_t byte = 0; + if (1 != buf_.read(offset, &byte, 1U)) + { + break; + } + for (int i = 7; i >= 0; i--) // Most significant goes first + { + out += (byte & (1 << i)) ? '1' : '0'; + } + out += ' '; + } + if (out.length() > 0) + { + (void)out.erase(out.length() - 1, 1); + } + return out; +} +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_float_spec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_float_spec.cpp new file mode 100644 index 0000000000..267e0a6bc9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_float_spec.cpp @@ -0,0 +1,89 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +#if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION + +union Fp32 +{ + uint32_t u; + float f; +}; + +/* + * IEEE754Converter + */ +uint16_t IEEE754Converter::nativeIeeeToHalf(float value) +{ + /* + * https://gist.github.com/rygorous/2156668 + * Public domain, by Fabian "ryg" Giesen + */ + const Fp32 f32infty = { 255U << 23 }; + const Fp32 f16infty = { 31U << 23 }; + const Fp32 magic = { 15U << 23 }; + const uint32_t sign_mask = 0x80000000U; + const uint32_t round_mask = ~0xFFFU; + + Fp32 in; + uint16_t out; + + in.f = value; + + uint32_t sign = in.u & sign_mask; + in.u ^= sign; + + if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */ + { + /* NaN->sNaN and Inf->Inf */ + out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U; + } + else /* (De)normalized number or zero */ + { + in.u &= round_mask; + in.f *= magic.f; + in.u -= round_mask; + if (in.u > f16infty.u) + { + in.u = f16infty.u; /* Clamp to signed infinity if overflowed */ + } + + out = uint16_t(in.u >> 13); /* Take the bits! */ + } + + out = uint16_t(out | (sign >> 16)); + + return out; +} + +float IEEE754Converter::halfToNativeIeee(uint16_t value) +{ + /* + * https://gist.github.com/rygorous/2144712 + * Public domain, by Fabian "ryg" Giesen + */ + const Fp32 magic = { (254U - 15U) << 23 }; + const Fp32 was_infnan = { (127U + 16U) << 23 }; + Fp32 out; + + out.u = (value & 0x7FFFU) << 13; /* exponent/mantissa bits */ + out.f *= magic.f; /* exponent adjust */ + if (out.f >= was_infnan.f) /* make sure Inf/NaN survive */ + { + out.u |= 255U << 23; + } + out.u |= (value & 0x8000U) << 16; /* sign bit */ + + return out.f; +} + +#endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_scalar_codec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_scalar_codec.cpp new file mode 100644 index 0000000000..1a7485698c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/marshal/uc_scalar_codec.cpp @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +void ScalarCodec::swapByteOrder(uint8_t* const bytes, const unsigned len) +{ + UAVCAN_ASSERT(bytes); + for (unsigned i = 0, j = len - 1; i < j; i++, j--) + { + const uint8_t c = bytes[i]; + bytes[i] = bytes[j]; + bytes[j] = c; + } +} + +int ScalarCodec::encodeBytesImpl(uint8_t* const bytes, const unsigned bitlen) +{ + UAVCAN_ASSERT(bytes); + // Underlying stream class assumes that more significant bits have lower index, so we need to shift some. + if (bitlen % 8) + { + bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] << ((8 - (bitlen % 8)) & 7)); + } + return stream_.write(bytes, bitlen); +} + +int ScalarCodec::decodeBytesImpl(uint8_t* const bytes, const unsigned bitlen) +{ + UAVCAN_ASSERT(bytes); + const int read_res = stream_.read(bytes, bitlen); + if (read_res > 0) + { + if (bitlen % 8) + { + bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] >> ((8 - (bitlen % 8)) & 7)); // As in encode(), vice versa + } + } + return read_res; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_publisher.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_publisher.cpp new file mode 100644 index 0000000000..2c42cfb0ff --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_publisher.cpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +bool GenericPublisherBase::isInited() const +{ + return sender_.isInitialized(); +} + +int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos) +{ + if (isInited()) + { + return 0; + } + + GlobalDataTypeRegistry::instance().freeze(); + + const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname); + if (descr == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname); + return -ErrUnknownDataType; + } + + sender_.init(*descr, qos); + + return 0; +} + +MonotonicTime GenericPublisherBase::getTxDeadline() const +{ + return node_.getMonotonicTime() + tx_timeout_; +} + +int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline) +{ + if (tid) + { + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id, *tid); + } + else + { + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id); + } +} + +void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout) +{ + tx_timeout = max(tx_timeout, getMinTxTimeout()); + tx_timeout = min(tx_timeout, getMaxTxTimeout()); + tx_timeout_ = tx_timeout; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_subscriber.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_subscriber.cpp new file mode 100644 index 0000000000..6f5adb66ab --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_generic_subscriber.cpp @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +int GenericSubscriberBase::genericStart(TransferListener* listener, + bool (Dispatcher::*registration_method)(TransferListener*)) +{ + if (listener == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + stop(listener); + if (!(node_.getDispatcher().*registration_method)(listener)) + { + UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener"); + return -ErrInvalidTransferListener; + } + return 0; +} + +void GenericSubscriberBase::stop(TransferListener* listener) +{ + if (listener != UAVCAN_NULLPTR) + { + node_.getDispatcher().unregisterMessageListener(listener); + node_.getDispatcher().unregisterServiceRequestListener(listener); + node_.getDispatcher().unregisterServiceResponseListener(listener); + } +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_global_data_type_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_global_data_type_registry.cpp new file mode 100644 index 0000000000..7586201e2b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_global_data_type_registry.cpp @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ + +GlobalDataTypeRegistry::List* GlobalDataTypeRegistry::selectList(DataTypeKind kind) const +{ + if (kind == DataTypeKindMessage) + { + return &msgs_; + } + else if (kind == DataTypeKindService) + { + return &srvs_; + } + else + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } +} + +GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::remove(Entry* dtd) +{ + if (!dtd) + { + UAVCAN_ASSERT(0); + return RegistrationResultInvalidParams; + } + if (isFrozen()) + { + return RegistrationResultFrozen; + } + + List* list = selectList(dtd->descriptor.getKind()); + if (!list) + { + return RegistrationResultInvalidParams; + } + + list->remove(dtd); // If this call came from regist<>(), that would be enough + Entry* p = list->get(); // But anyway + while (p) + { + Entry* const next = p->getNextListNode(); + if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName())) + { + list->remove(p); + } + p = next; + } + return RegistrationResultOk; +} + +GlobalDataTypeRegistry::RegistrationResult GlobalDataTypeRegistry::registImpl(Entry* dtd) +{ + if (!dtd || !dtd->descriptor.isValid()) + { + UAVCAN_ASSERT(0); + return RegistrationResultInvalidParams; + } + if (isFrozen()) + { + return RegistrationResultFrozen; + } + + List* list = selectList(dtd->descriptor.getKind()); + if (!list) + { + return RegistrationResultInvalidParams; + } + + { // Collision check + Entry* p = list->get(); + while (p) + { + if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision + { + return RegistrationResultCollision; + } + if (!std::strncmp(p->descriptor.getFullName(), dtd->descriptor.getFullName(), + DataTypeDescriptor::MaxFullNameLen)) // Name collision + { + return RegistrationResultCollision; + } + p = p->getNextListNode(); + } + } +#if UAVCAN_DEBUG + const unsigned len_before = list->getLength(); +#endif + list->insertBefore(dtd, EntryInsertionComparator(dtd)); + +#if UAVCAN_DEBUG + { // List integrity check + const unsigned len_after = list->getLength(); + if ((len_before + 1) != len_after) + { + UAVCAN_ASSERT(0); + std::abort(); + } + } + { // Order check + Entry* p = list->get(); + int id = -1; + while (p) + { + if (id >= p->descriptor.getID().get()) + { + UAVCAN_ASSERT(0); + std::abort(); + } + id = p->descriptor.getID().get(); + p = p->getNextListNode(); + } + } +#endif + return RegistrationResultOk; +} + +GlobalDataTypeRegistry& GlobalDataTypeRegistry::instance() +{ + static GlobalDataTypeRegistry singleton; + return singleton; +} + +void GlobalDataTypeRegistry::freeze() +{ + if (!frozen_) + { + frozen_ = true; + UAVCAN_TRACE("GlobalDataTypeRegistry", "Frozen; num msgs: %u, num srvs: %u", + getNumMessageTypes(), getNumServiceTypes()); + } +} + +const DataTypeDescriptor* GlobalDataTypeRegistry::find(const char* name) const +{ + const DataTypeDescriptor* desc = find(DataTypeKindMessage, name); + if (desc == UAVCAN_NULLPTR) + { + desc = find(DataTypeKindService, name); + } + return desc; +} + +const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const char* name) const +{ + if (!name) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + const List* list = selectList(kind); + if (!list) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + Entry* p = list->get(); + while (p) + { + if (p->descriptor.match(kind, name)) + { + return &p->descriptor; + } + p = p->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, DataTypeID dtid) const +{ + const List* list = selectList(kind); + if (!list) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + Entry* p = list->get(); + while (p) + { + if (p->descriptor.match(kind, dtid)) + { + return &p->descriptor; + } + p = p->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_scheduler.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_scheduler.cpp new file mode 100644 index 0000000000..529d94ccc6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_scheduler.cpp @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * MonotonicDeadlineHandler + */ +void DeadlineHandler::startWithDeadline(MonotonicTime deadline) +{ + UAVCAN_ASSERT(!deadline.isZero()); + stop(); + deadline_ = deadline; + scheduler_.getDeadlineScheduler().add(this); +} + +void DeadlineHandler::startWithDelay(MonotonicDuration delay) +{ + startWithDeadline(scheduler_.getMonotonicTime() + delay); +} + +void DeadlineHandler::stop() +{ + scheduler_.getDeadlineScheduler().remove(this); +} + +bool DeadlineHandler::isRunning() const +{ + return scheduler_.getDeadlineScheduler().doesExist(this); +} + +/* + * MonotonicDeadlineScheduler + */ +struct MonotonicDeadlineHandlerInsertionComparator +{ + const MonotonicTime ts; + explicit MonotonicDeadlineHandlerInsertionComparator(MonotonicTime arg_ts) : ts(arg_ts) { } + bool operator()(const DeadlineHandler* t) const + { + return t->getDeadline() > ts; + } +}; + +void DeadlineScheduler::add(DeadlineHandler* mdh) +{ + UAVCAN_ASSERT(mdh); + handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getDeadline())); +} + +void DeadlineScheduler::remove(DeadlineHandler* mdh) +{ + UAVCAN_ASSERT(mdh); + handlers_.remove(mdh); +} + +bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const +{ + UAVCAN_ASSERT(mdh); + const DeadlineHandler* p = handlers_.get(); +#if UAVCAN_DEBUG + MonotonicTime prev_deadline; +#endif + while (p) + { +#if UAVCAN_DEBUG + if (prev_deadline > p->getDeadline()) // Self check + { + std::abort(); + } + prev_deadline = p->getDeadline(); +#endif + if (p == mdh) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock) +{ + while (true) + { + DeadlineHandler* const mdh = handlers_.get(); + if (!mdh) + { + return sysclock.getMonotonic(); + } +#if UAVCAN_DEBUG + if (mdh->getNextListNode()) // Order check + { + UAVCAN_ASSERT(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline()); + } +#endif + + const MonotonicTime ts = sysclock.getMonotonic(); + if (ts < mdh->getDeadline()) + { + return ts; + } + + handlers_.remove(mdh); + mdh->handleDeadline(ts); // This handler can be re-registered immediately + } + UAVCAN_ASSERT(0); + return MonotonicTime(); +} + +MonotonicTime DeadlineScheduler::getEarliestDeadline() const +{ + const DeadlineHandler* const mdh = handlers_.get(); + if (mdh) + { + return mdh->getDeadline(); + } + return MonotonicTime::getMax(); +} + +/* + * Scheduler + */ +MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const +{ + const MonotonicTime earliest = min(deadline_scheduler_.getEarliestDeadline(), spin_deadline); + const MonotonicTime ts = getMonotonicTime(); + if (earliest > ts) + { + if (earliest - ts > deadline_resolution_) + { + return ts + deadline_resolution_; + } + } + return earliest; +} + +void Scheduler::pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin) +{ + // cleanup will be performed less frequently if the stack handles more frames per second + const MonotonicTime deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1); + if (mono_ts > deadline) + { + //UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin); + prev_cleanup_ts_ = mono_ts; + dispatcher_.cleanup(mono_ts); + } +} + +int Scheduler::spin(MonotonicTime deadline) +{ + if (inside_spin_) // Preventing recursive calls + { + UAVCAN_ASSERT(0); + return -ErrRecursiveCall; + } + InsideSpinSetter iss(*this); + UAVCAN_ASSERT(inside_spin_); + + int retval = 0; + while (true) + { + const MonotonicTime dl = computeDispatcherSpinDeadline(deadline); + retval = dispatcher_.spin(dl); + if (retval < 0) + { + break; + } + + const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock()); + pollCleanup(ts, unsigned(retval)); + if (ts >= deadline) + { + break; + } + } + + return retval; +} + +int Scheduler::spinOnce() +{ + if (inside_spin_) // Preventing recursive calls + { + UAVCAN_ASSERT(0); + return -ErrRecursiveCall; + } + InsideSpinSetter iss(*this); + UAVCAN_ASSERT(inside_spin_); + + const int retval = dispatcher_.spinOnce(); + if (retval < 0) + { + return retval; + } + + const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock()); + pollCleanup(ts, unsigned(retval)); + + return retval; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_service_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_service_client.cpp new file mode 100644 index 0000000000..cfe14555b0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_service_client.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ +/* + * ServiceClientBase::CallState + */ +void ServiceClientBase::CallState::handleDeadline(MonotonicTime) +{ + UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s", + int(id_.server_node_id.get()), int(id_.transfer_id.get()), + (owner_.data_type_descriptor_ == UAVCAN_NULLPTR) ? "???" : owner_.data_type_descriptor_->getFullName()); + /* + * What we're doing here is relaying execution from this call stack to a different one. + * We need it because call registry cannot release memory from this callback, because this will destroy the + * object method of which we're executing now. + */ + UAVCAN_ASSERT(timed_out_ == false); + timed_out_ = true; + owner_.generateDeadlineImmediately(); + UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback"); +} + +/* + * ServiceClientBase + */ +int ServiceClientBase::prepareToCall(INode& node, + const char* dtname, + NodeID server_node_id, + ServiceCallID& out_call_id) +{ + /* + * Making sure we're not going to get transport error because of invalid input data + */ + if (!server_node_id.isUnicast() || (server_node_id == node.getNodeID())) + { + UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID"); + return -ErrInvalidParam; + } + out_call_id.server_node_id = server_node_id; + + /* + * Determining the Data Type ID + */ + if (data_type_descriptor_ == UAVCAN_NULLPTR) + { + GlobalDataTypeRegistry::instance().freeze(); + data_type_descriptor_ = GlobalDataTypeRegistry::instance().find(DataTypeKindService, dtname); + if (data_type_descriptor_ == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", dtname); + return -ErrUnknownDataType; + } + UAVCAN_TRACE("ServiceClient", "Data type descriptor inited: %s", data_type_descriptor_->toString().c_str()); + } + UAVCAN_ASSERT(data_type_descriptor_ != UAVCAN_NULLPTR); + + /* + * Determining the Transfer ID + */ + const OutgoingTransferRegistryKey otr_key(data_type_descriptor_->getID(), + TransferTypeServiceRequest, server_node_id); + const MonotonicTime otr_deadline = node.getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval(); + TransferID* const otr_tid = + node.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (!otr_tid) + { + UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str()); + return -ErrMemory; + } + out_call_id.transfer_id = *otr_tid; + otr_tid->increment(); + + return 0; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_timer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_timer.cpp new file mode 100644 index 0000000000..0230198d86 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/node/uc_timer.cpp @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ +/* + * TimerBase + */ +void TimerBase::handleDeadline(MonotonicTime current) +{ + UAVCAN_ASSERT(!isRunning()); + + const MonotonicTime scheduled_time = getDeadline(); + + if (period_ < MonotonicDuration::getInfinite()) + { + startWithDeadline(scheduled_time + period_); + } + + // Application can re-register the timer with different params, it's OK + handleTimerEvent(TimerEvent(scheduled_time, current)); +} + +void TimerBase::startOneShotWithDeadline(MonotonicTime deadline) +{ + stop(); + period_ = MonotonicDuration::getInfinite(); + DeadlineHandler::startWithDeadline(deadline); +} + +void TimerBase::startOneShotWithDelay(MonotonicDuration delay) +{ + stop(); + period_ = MonotonicDuration::getInfinite(); + DeadlineHandler::startWithDelay(delay); +} + +void TimerBase::startPeriodic(MonotonicDuration period) +{ + UAVCAN_ASSERT(period < MonotonicDuration::getInfinite()); + stop(); + period_ = period; + DeadlineHandler::startWithDelay(period); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp new file mode 100644 index 0000000000..2889cbadee --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +void DynamicNodeIDClient::terminate() +{ + UAVCAN_TRACE("DynamicNodeIDClient", "Client terminated"); + stop(); + dnida_sub_.stop(); +} + +MonotonicDuration DynamicNodeIDClient::getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec) +{ + UAVCAN_ASSERT(upper_bound_msec > lower_bound_msec); + // coverity[dont_call] + return MonotonicDuration::fromMSec(lower_bound_msec + + static_cast(std::rand()) % (upper_bound_msec - lower_bound_msec)); +} + +void DynamicNodeIDClient::restartTimer(const Mode mode) +{ + UAVCAN_ASSERT(mode < NumModes); + UAVCAN_ASSERT((mode == ModeWaitingForTimeSlot) == (size_of_received_unique_id_ == 0)); + + const MonotonicDuration delay = (mode == ModeWaitingForTimeSlot) ? + getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_REQUEST_PERIOD_MS, + protocol::dynamic_node_id::Allocation::MAX_REQUEST_PERIOD_MS) : + getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_FOLLOWUP_DELAY_MS, + protocol::dynamic_node_id::Allocation::MAX_FOLLOWUP_DELAY_MS); + + startOneShotWithDelay(delay); + + UAVCAN_TRACE("DynamicNodeIDClient", "Restart mode %d, delay %d ms", + static_cast(mode), static_cast(delay.toMSec())); +} + +void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) +{ + UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(size_of_received_unique_id_ < protocol::dynamic_node_id::Allocation::FieldTypes::unique_id::MaxSize); + + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + /* + * Filling the message. + */ + protocol::dynamic_node_id::Allocation tx; + tx.node_id = preferred_node_id_.get(); + tx.first_part_of_unique_id = (size_of_received_unique_id_ == 0); + + const uint8_t size_of_unique_id_in_request = + min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, + static_cast(tx.unique_id.capacity() - size_of_received_unique_id_)); + + tx.unique_id.resize(size_of_unique_id_in_request); + copy(unique_id_ + size_of_received_unique_id_, + unique_id_ + size_of_received_unique_id_ + size_of_unique_id_in_request, + tx.unique_id.begin()); + + UAVCAN_ASSERT(equal(tx.unique_id.begin(), tx.unique_id.end(), unique_id_ + size_of_received_unique_id_)); + + /* + * Resetting the state - this way we can continue with a first stage request on the next attempt. + */ + size_of_received_unique_id_ = 0; + restartTimer(ModeWaitingForTimeSlot); + + /* + * Broadcasting the message. + */ + UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting; preferred ID %d, size of UID %d", + static_cast(preferred_node_id_.get()), + static_cast(tx.unique_id.size())); + const int res = dnida_pub_.broadcast(tx); + if (res < 0) + { + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); + } +} + +void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure& msg) +{ + UAVCAN_ASSERT(preferred_node_id_.isValid()); + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation message from %d, %d bytes of unique ID, node ID %d", + static_cast(msg.getSrcNodeID().get()), static_cast(msg.unique_id.size()), + static_cast(msg.node_id)); + + /* + * Switching to passive state by default; will switch to active state if response matches. + */ + size_of_received_unique_id_ = 0; + restartTimer(ModeWaitingForTimeSlot); + + /* + * Filtering out anonymous and invalid messages. + */ + const bool full_response = (msg.unique_id.size() == msg.unique_id.capacity()); + + if (msg.isAnonymousTransfer() || + msg.unique_id.empty() || + (full_response && (msg.node_id == 0))) + { + UAVCAN_TRACE("DynamicNodeIDClient", "Message from %d ignored", static_cast(msg.getSrcNodeID().get())); + return; + } + + /* + * If matches, either switch to active mode or complete the allocation. + */ + if (equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) + { + if (full_response) + { + allocated_node_id_ = msg.node_id; + allocator_node_id_ = msg.getSrcNodeID(); + terminate(); + UAVCAN_ASSERT(isAllocationComplete()); + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d", + static_cast(allocated_node_id_.get()), static_cast(allocator_node_id_.get())); + } + else + { + size_of_received_unique_id_ = msg.unique_id.size(); + restartTimer(ModeDelayBeforeFollowup); + } + } +} + +int DynamicNodeIDClient::start(const UniqueID& unique_id, + const NodeID preferred_node_id, + const TransferPriority transfer_priority) +{ + terminate(); + + // Allocation is not possible if node ID is already set + if (dnida_pub_.getNode().getNodeID().isUnicast()) + { + return -ErrLogic; + } + + // Unique ID initialization & validation + copy(unique_id.begin(), unique_id.end(), unique_id_); + bool unique_id_is_zero = true; + for (uint8_t i = 0; i < sizeof(unique_id_); i++) + { + if (unique_id_[i] != 0) + { + unique_id_is_zero = false; + break; + } + } + + if (unique_id_is_zero) + { + return -ErrInvalidParam; + } + + if (!preferred_node_id.isValid()) // Only broadcast and unicast are allowed + { + return -ErrInvalidParam; + } + + // Initializing the fields + preferred_node_id_ = preferred_node_id; + allocated_node_id_ = NodeID(); + allocator_node_id_ = NodeID(); + UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(!allocated_node_id_.isValid()); + UAVCAN_ASSERT(!allocator_node_id_.isValid()); + + // Initializing node objects - Rule A + int res = dnida_pub_.init(); + if (res < 0) + { + return res; + } + dnida_pub_.allowAnonymousTransfers(); + dnida_pub_.setPriority(transfer_priority); + + res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation)); + if (res < 0) + { + return res; + } + dnida_sub_.allowAnonymousTransfers(); + + restartTimer(ModeWaitingForTimeSlot); + + return 0; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_node_status_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_node_status_provider.cpp new file mode 100644 index 0000000000..a24034716c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/protocol/uc_node_status_provider.cpp @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +bool NodeStatusProvider::isNodeInfoInitialized() const +{ + // Hardware/Software versions are not required + return !node_info_.name.empty(); +} + +int NodeStatusProvider::publish() +{ + const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_; + UAVCAN_ASSERT(uptime.isPositive()); + node_info_.status.uptime_sec = uint32_t(uptime.toMSec() / 1000); + + UAVCAN_ASSERT(node_info_.status.health <= protocol::NodeStatus::FieldTypes::health::max()); + + return node_status_pub_.broadcast(node_info_.status); +} + +void NodeStatusProvider::handleTimerEvent(const TimerEvent&) +{ + if (getNode().isPassiveMode()) + { + UAVCAN_TRACE("NodeStatusProvider", "NodeStatus pub skipped - passive mode"); + } + else + { + if (ad_hoc_status_updater_ != UAVCAN_NULLPTR) + { + ad_hoc_status_updater_->updateNodeStatus(); + } + + const int res = publish(); + if (res < 0) + { + getNode().registerInternalFailure("NodeStatus pub failed"); + } + } +} + +void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, + protocol::GetNodeInfo::Response& rsp) +{ + UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request"); + UAVCAN_ASSERT(isNodeInfoInitialized()); + rsp = node_info_; +} + +int NodeStatusProvider::startAndPublish(const TransferPriority priority) +{ + if (!isNodeInfoInitialized()) + { + UAVCAN_TRACE("NodeStatusProvider", "Node info was not initialized"); + return -ErrNotInited; + } + + int res = -1; + + node_status_pub_.setPriority(priority); + + if (!getNode().isPassiveMode()) + { + res = publish(); + if (res < 0) // Initial broadcast + { + goto fail; + } + } + + res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest)); + if (res < 0) + { + goto fail; + } + + setStatusPublicationPeriod(MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS)); + + return res; + +fail: + UAVCAN_ASSERT(res < 0); + gni_srv_.stop(); + TimerBase::stop(); + return res; +} + +void NodeStatusProvider::setStatusPublicationPeriod(uavcan::MonotonicDuration period) +{ + const MonotonicDuration maximum = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS); + const MonotonicDuration minimum = MonotonicDuration::fromMSec(protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS); + + period = min(period, maximum); + period = max(period, minimum); + TimerBase::startPeriodic(period); + + const MonotonicDuration tx_timeout = period - MonotonicDuration::fromUSec(period.toUSec() / 20); + node_status_pub_.setTxTimeout(tx_timeout); + + UAVCAN_TRACE("NodeStatusProvider", "Status pub period: %s, TX timeout: %s", + period.toString().c_str(), node_status_pub_.getTxTimeout().toString().c_str()); +} + +uavcan::MonotonicDuration NodeStatusProvider::getStatusPublicationPeriod() const +{ + return TimerBase::getPeriod(); +} + +void NodeStatusProvider::setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater) +{ + ad_hoc_status_updater_ = updater; // Can be nullptr, that's okay +} + +void NodeStatusProvider::setHealth(uint8_t code) +{ + node_info_.status.health = code; +} + +void NodeStatusProvider::setMode(uint8_t code) +{ + node_info_.status.mode = code; +} + +void NodeStatusProvider::setVendorSpecificStatusCode(VendorSpecificStatusCode code) +{ + node_info_.status.vendor_specific_status_code = code; +} + +void NodeStatusProvider::setName(const NodeName& name) +{ + if (node_info_.name.empty()) + { + node_info_.name = name; + } +} + +void NodeStatusProvider::setSoftwareVersion(const protocol::SoftwareVersion& version) +{ + if (node_info_.software_version == protocol::SoftwareVersion()) + { + node_info_.software_version = version; + } +} + +void NodeStatusProvider::setHardwareVersion(const protocol::HardwareVersion& version) +{ + if (node_info_.hardware_version == protocol::HardwareVersion()) + { + node_info_.hardware_version = version; + } +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp new file mode 100644 index 0000000000..e468b3ca85 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -0,0 +1,256 @@ +/* + * Copyright (C) 2015 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +namespace uavcan +{ +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceID; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID; + +int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode) +{ + multiset_configs_.clear(); + + if (load_mode == AcceptAnonymousMessages) + { + CanFilterConfig anon_frame_cfg; + anon_frame_cfg.id = DefaultAnonMsgID | CanFrame::FlagEFF; + anon_frame_cfg.mask = DefaultAnonMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(anon_frame_cfg) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + } + + CanFilterConfig service_cfg; + service_cfg.id = DefaultFilterServiceID; + service_cfg.id |= (static_cast(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF; + service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(service_cfg) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + + const TransferListener* p = node_.getDispatcher().getListOfMessageListeners().get(); + while (p != UAVCAN_NULLPTR) + { + CanFilterConfig cfg; + cfg.id = (static_cast(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF; + cfg.mask = DefaultFilterMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(cfg) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + p = p->getNextListNode(); + } + + if (multiset_configs_.getSize() == 0) + { + return -ErrLogic; + } + + return 0; +} + +int16_t CanAcceptanceFilterConfigurator::mergeConfigurations() +{ + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + UAVCAN_ASSERT(multiset_configs_.getSize() != 0); + + while (acceptance_filters_number < multiset_configs_.getSize()) + { + uint16_t i_rank = 0, j_rank = 0; + uint8_t best_rank = 0; + + const uint16_t multiset_array_size = static_cast(multiset_configs_.getSize()); + + for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++) + { + for (uint16_t j_ind = static_cast(i_ind + 1); j_ind < multiset_array_size; j_ind++) + { + CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind), + *multiset_configs_.getByIndex(j_ind)); + uint8_t rank = countBits(temp_config.mask); + if (rank > best_rank) + { + best_rank = rank; + i_rank = i_ind; + j_rank = j_ind; + } + } + } + + *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank), + *multiset_configs_.getByIndex(j_rank)); + multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank)); + } + + UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize()); + + return 0; +} + +int CanAcceptanceFilterConfigurator::applyConfiguration(void) +{ + CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters]; + unsigned int filter_array_size = multiset_configs_.getSize(); + + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + if (filter_array_size > acceptance_filters_number) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()"); + mergeConfigurations(); + filter_array_size = multiset_configs_.getSize(); + } + + if (filter_array_size > MaxCanAcceptanceFilters) + { + UAVCAN_ASSERT(0); + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrLogic; + } + + for (uint16_t i = 0; i < filter_array_size; i++) + { + CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i); + + filter_conf_array[i] = temp_filter_config; + } + +#if UAVCAN_DEBUG + for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) + { + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %u", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", i, + multiset_configs_.getByIndex(i)->mask); + } +#endif + + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + ICanIface* iface = can_driver.getIface(i); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrDriver; + } + int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), + static_cast(filter_array_size)); + if (num < 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrDriver; + } + } + + return 0; +} + +int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode) +{ + if (getNumFilters() == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + int16_t fill_array_error = loadInputConfiguration(mode); + if (fill_array_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()"); + return fill_array_error; + } + + int16_t merge_configurations_error = mergeConfigurations(); + if (merge_configurations_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration"); + return merge_configurations_error; + } + + return 0; +} + +uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const +{ + if (filters_number_ == 0) + { + static const uint16_t InvalidOut = 0xFFFF; + uint16_t out = InvalidOut; + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const ICanIface* iface = can_driver.getIface(i); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + out = 0; + break; + } + const uint16_t num = iface->getNumFilters(); + out = min(out, num); + if (out > MaxCanAcceptanceFilters) + { + out = MaxCanAcceptanceFilters; + } + } + + return (out == InvalidOut) ? 0 : out; + } + else + { + return filters_number_; + } +} + +int CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config) +{ + if (multiset_configs_.emplace(config) == UAVCAN_NULLPTR) + { + return -ErrMemory; + } + + return 0; +} + +CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_) +{ + CanFilterConfig temp_arr; + temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id); + temp_arr.id = a_.id & temp_arr.mask; + + return temp_arr; +} + +uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_) +{ + uint8_t c_; // c accumulates the total bits set in v + for (c_ = 0; n_; c_++) + { + n_ &= n_ - 1; // clear the least significant bit set + } + + return c_; +} +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp new file mode 100644 index 0000000000..bcca883477 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp @@ -0,0 +1,509 @@ +/* + * CAN bus IO logic. + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * CanRxFrame + */ +#if UAVCAN_TOSTRING +std::string CanRxFrame::toString(StringRepresentation mode) const +{ + std::string out = CanFrame::toString(mode); + out.reserve(128); + out += " ts_m=" + ts_mono.toString(); + out += " ts_utc=" + ts_utc.toString(); + out += " iface="; + out += char('0' + iface_index); + return out; +} +#endif + +/* + * CanTxQueue::Entry + */ +void CanTxQueue::Entry::destroy(Entry*& obj, IPoolAllocator& allocator) +{ + if (obj != UAVCAN_NULLPTR) + { + obj->~Entry(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } +} + +bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const +{ + if (qos != rhs_qos) + { + return qos > rhs_qos; + } + return frame.priorityHigherThan(rhs_frame); +} + +bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const +{ + if (qos != rhs_qos) + { + return qos < rhs_qos; + } + return frame.priorityLowerThan(rhs_frame); +} + +#if UAVCAN_TOSTRING +std::string CanTxQueue::Entry::toString() const +{ + std::string str_qos; + switch (qos) + { + case Volatile: + { + str_qos = " "; + break; + } + case Persistent: + { + str_qos = " "; + break; + } + default: + { + UAVCAN_ASSERT(0); + str_qos = " "; + break; + } + } + return str_qos + frame.toString(); +} +#endif + +/* + * CanTxQueue + */ +CanTxQueue::~CanTxQueue() +{ + Entry* p = queue_.get(); + while (p) + { + Entry* const next = p->getNextListNode(); + remove(p); + p = next; + } +} + +void CanTxQueue::registerRejectedFrame() +{ + if (rejected_frames_cnt_ < NumericTraits::max()) + { + rejected_frames_cnt_++; + } +} + +void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags) +{ + const MonotonicTime timestamp = sysclock_.getMonotonic(); + + if (timestamp >= tx_deadline) + { + UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired"); + registerRejectedFrame(); + return; + } + + void* praw = allocator_.allocate(sizeof(Entry)); + if (praw == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup"); + // No memory left in the pool, so we try to remove expired frames + Entry* p = queue_.get(); + while (p) + { + Entry* const next = p->getNextListNode(); + if (p->isExpired(timestamp)) + { + UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str()); + registerRejectedFrame(); + remove(p); + } + p = next; + } + praw = allocator_.allocate(sizeof(Entry)); // Try again + } + + if (praw == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration"); + registerRejectedFrame(); + + // Find a frame with lowest QoS + Entry* p = queue_.get(); + if (p == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("CanTxQueue", "Push rejected: Nothing to replace"); + return; + } + Entry* lowestqos = p; + while (p) + { + if (lowestqos->qosHigherThan(*p)) + { + lowestqos = p; + } + p = p->getNextListNode(); + } + // Note that frame with *equal* QoS will be replaced too. + if (lowestqos->qosHigherThan(frame, qos)) // Frame that we want to transmit has lowest QoS + { + UAVCAN_TRACE("CanTxQueue", "Push rejected: low QoS"); + return; // What a loser. + } + UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str()); + remove(lowestqos); + praw = allocator_.allocate(sizeof(Entry)); // Try again + } + + if (praw == UAVCAN_NULLPTR) + { + return; // Seems that there is no memory at all. + } + Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags); + UAVCAN_ASSERT(entry); + queue_.insertBefore(entry, PriorityInsertionComparator(frame)); +} + +CanTxQueue::Entry* CanTxQueue::peek() +{ + const MonotonicTime timestamp = sysclock_.getMonotonic(); + Entry* p = queue_.get(); + while (p) + { + if (p->isExpired(timestamp)) + { + UAVCAN_TRACE("CanTxQueue", "Peek: Expired %s", p->toString().c_str()); + Entry* const next = p->getNextListNode(); + registerRejectedFrame(); + remove(p); + p = next; + } + else + { + return p; + } + } + return UAVCAN_NULLPTR; +} + +void CanTxQueue::remove(Entry*& entry) +{ + if (entry == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return; + } + queue_.remove(entry); + Entry::destroy(entry, allocator_); +} + +const CanFrame* CanTxQueue::getTopPriorityPendingFrame() const +{ + return (queue_.get() == UAVCAN_NULLPTR) ? UAVCAN_NULLPTR : &queue_.get()->frame; +} + +bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const +{ + const Entry* entry = queue_.get(); + if (entry == UAVCAN_NULLPTR) + { + return false; + } + return !rhs_frame.priorityHigherThan(entry->frame); +} + +/* + * CanIOManager + */ +int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) +{ + UAVCAN_ASSERT(iface_index < MaxCanIfaces); + ICanIface* const iface = driver_.getIface(iface_index); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // Nonexistent interface + return -ErrLogic; + } + const int res = iface->send(frame, tx_deadline, flags); + if (res != 1) + { + UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s", + res, iface_index, frame.toString().c_str()); + } + if (res > 0) + { + counters_[iface_index].frames_tx += unsigned(res); + } + return res; +} + +int CanIOManager::sendFromTxQueue(uint8_t iface_index) +{ + UAVCAN_ASSERT(iface_index < MaxCanIfaces); + CanTxQueue::Entry* entry = tx_queues_[iface_index]->peek(); + if (entry == UAVCAN_NULLPTR) + { + return 0; + } + const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags); + if (res > 0) + { + tx_queues_[iface_index]->remove(entry); + } + return res; +} + +int CanIOManager::callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces], + MonotonicTime blocking_deadline) +{ + const CanSelectMasks in_masks = inout_masks; + + const int res = driver_.select(inout_masks, pending_tx, blocking_deadline); + if (res < 0) + { + return -ErrDriver; + } + + inout_masks.read &= in_masks.read; // Driver is not required to clean the masks + inout_masks.write &= in_masks.write; + return res; +} + +CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, + std::size_t mem_blocks_per_iface) + : driver_(driver) + , sysclock_(sysclock) + , num_ifaces_(driver.getNumIfaces()) +{ + if (num_ifaces_ < 1 || num_ifaces_ > MaxCanIfaces) + { + handleFatalError("Num ifaces"); + } + + if (mem_blocks_per_iface == 0) + { + mem_blocks_per_iface = allocator.getBlockCapacity() / (num_ifaces_ + 1U) + 1U; + } + UAVCAN_TRACE("CanIOManager", "Memory blocks per iface: %u, total: %u", + unsigned(mem_blocks_per_iface), unsigned(allocator.getBlockCapacity())); + + for (int i = 0; i < num_ifaces_; i++) + { + tx_queues_[i].construct + (allocator, sysclock, mem_blocks_per_iface); + } +} + +uint8_t CanIOManager::makePendingTxMask() const +{ + uint8_t write_mask = 0; + for (uint8_t i = 0; i < getNumIfaces(); i++) + { + if (!tx_queues_[i]->isEmpty()) + { + write_mask = uint8_t(write_mask | (1 << i)); + } + } + return write_mask; +} + +CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const +{ + ICanIface* const iface = driver_.getIface(iface_index); + if (iface == UAVCAN_NULLPTR || iface_index >= MaxCanIfaces) + { + UAVCAN_ASSERT(0); + return CanIfacePerfCounters(); + } + CanIfacePerfCounters cnt; + cnt.errors = iface->getErrorCount() + tx_queues_[iface_index]->getRejectedFrameCount(); + cnt.frames_rx = counters_[iface_index].frames_rx; + cnt.frames_tx = counters_[iface_index].frames_tx; + return cnt; +} + +int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) +{ + const uint8_t num_ifaces = getNumIfaces(); + const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1); + iface_mask &= all_ifaces_mask; + + if (blocking_deadline > tx_deadline) + { + blocking_deadline = tx_deadline; + } + + int retval = 0; + + while (true) // Somebody please refactor this. + { + if (iface_mask == 0) + { + break; + } + + CanSelectMasks masks; + masks.write = iface_mask | makePendingTxMask(); + { + // Building the list of next pending frames per iface. + // The driver will give them a scrutinizing look before deciding whether he wants to accept them. + const CanFrame* pending_tx[MaxCanIfaces] = {}; + for (int i = 0; i < num_ifaces; i++) + { + CanTxQueue& q = *tx_queues_[i]; + if (iface_mask & (1 << i)) // I hate myself so much right now. + { + pending_tx[i] = q.topPriorityHigherOrEqual(frame) ? q.getTopPriorityPendingFrame() : &frame; + } + else + { + pending_tx[i] = q.getTopPriorityPendingFrame(); + } + } + + const int select_res = callSelect(masks, pending_tx, blocking_deadline); + if (select_res < 0) + { + return -ErrDriver; + } + UAVCAN_ASSERT(masks.read == 0); + } + + // Transmission + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (masks.write & (1 << i)) + { + int res = 0; + if (iface_mask & (1 << i)) + { + if (tx_queues_[i]->topPriorityHigherOrEqual(frame)) + { + res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired) + } + if (res <= 0) + { + res = sendToIface(i, frame, tx_deadline, flags); + if (res > 0) + { + iface_mask &= uint8_t(~(1 << i)); // Mark transmitted + } + } + } + else + { + res = sendFromTxQueue(i); + } + if (res > 0) + { + retval++; + } + } + } + + // Timeout. Enqueue the frame if wasn't transmitted and leave. + const bool timed_out = sysclock_.getMonotonic() >= blocking_deadline; + if (masks.write == 0 || timed_out) + { + if (!timed_out) + { + UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again"); + continue; + } + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (iface_mask & (1 << i)) + { + tx_queues_[i]->push(frame, tx_deadline, qos, flags); + } + } + break; + } + } + return retval; +} + +int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags) +{ + const uint8_t num_ifaces = getNumIfaces(); + + while (true) + { + CanSelectMasks masks; + masks.write = makePendingTxMask(); + masks.read = uint8_t((1 << num_ifaces) - 1); + { + const CanFrame* pending_tx[MaxCanIfaces] = {}; + for (int i = 0; i < num_ifaces; i++) // Dear compiler, kindly unroll this. Thanks. + { + pending_tx[i] = tx_queues_[i]->getTopPriorityPendingFrame(); + } + + const int select_res = callSelect(masks, pending_tx, blocking_deadline); + if (select_res < 0) + { + return -ErrDriver; + } + } + + // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (masks.write & (1 << i)) + { + (void)sendFromTxQueue(i); // It may fail, we don't care. Requested operation was receive, not send. + } + } + + // Read + for (uint8_t i = 0; i < num_ifaces; i++) + { + if (masks.read & (1 << i)) + { + ICanIface* const iface = driver_.getIface(i); + if (iface == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); // Nonexistent interface + continue; + } + + const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags); + if (res == 0) + { + UAVCAN_ASSERT(0); // select() reported that iface has pending RX frames, but receive() returned none + continue; + } + out_frame.iface_index = i; + + if ((res > 0) && !(out_flags & CanIOFlagLoopback)) + { + counters_[i].frames_rx += 1; + } + return (res < 0) ? -ErrDriver : res; + } + } + + // Timeout checked in the last order - this way we can operate with expired deadline: + if (sysclock_.getMonotonic() >= blocking_deadline) + { + break; + } + } + return 0; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_crc.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_crc.cpp new file mode 100644 index 0000000000..a854c4ca5d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_crc.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +#if !UAVCAN_TINY + +// print ', '.join(map(lambda x: '%04x' % x, map(lambda x: int(x, 0), c.crc_ccitt_tab))) +const uint16_t TransferCRC::Table[256] = +{ + 0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U, + 0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU, + 0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U, + 0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU, + 0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U, + 0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU, + 0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U, + 0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU, + 0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U, + 0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU, + 0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U, + 0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU, + 0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U, + 0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U, + 0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U, + 0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U, + 0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU, + 0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U, + 0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU, + 0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U, + 0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU, + 0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U, + 0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU, + 0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U, + 0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU, + 0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U, + 0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU, + 0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U, + 0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U, + 0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U, + 0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U, + 0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U +}; + +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_dispatcher.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_dispatcher.cpp new file mode 100644 index 0000000000..f304689bc4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_dispatcher.cpp @@ -0,0 +1,385 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +#if !UAVCAN_TINY +/* + * LoopbackFrameListenerBase + */ +void LoopbackFrameListenerBase::startListening() +{ + dispatcher_.getLoopbackFrameListenerRegistry().add(this); +} + +void LoopbackFrameListenerBase::stopListening() +{ + dispatcher_.getLoopbackFrameListenerRegistry().remove(this); +} + +bool LoopbackFrameListenerBase::isListening() const +{ + return dispatcher_.getLoopbackFrameListenerRegistry().doesExist(this); +} + +/* + * LoopbackFrameListenerRegistry + */ +void LoopbackFrameListenerRegistry::add(LoopbackFrameListenerBase* listener) +{ + UAVCAN_ASSERT(listener); + listeners_.insert(listener); +} + +void LoopbackFrameListenerRegistry::remove(LoopbackFrameListenerBase* listener) +{ + UAVCAN_ASSERT(listener); + listeners_.remove(listener); +} + +bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* listener) const +{ + UAVCAN_ASSERT(listener); + const LoopbackFrameListenerBase* p = listeners_.get(); + while (p) + { + if (p == listener) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +void LoopbackFrameListenerRegistry::invokeListeners(RxFrame& frame) +{ + LoopbackFrameListenerBase* p = listeners_.get(); + while (p) + { + LoopbackFrameListenerBase* const next = p->getNextListNode(); + p->handleLoopbackFrame(frame); // p may be modified + p = next; + } +} +#endif + +/* + * Dispatcher::ListenerRegister + */ +bool Dispatcher::ListenerRegistry::add(TransferListener* listener, Mode mode) +{ + if (mode == UniqueListener) + { + TransferListener* p = list_.get(); + while (p) + { + if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID()) + { + return false; + } + p = p->getNextListNode(); + } + } + // Objective is to arrange entries by Data Type ID in ascending order from root. + list_.insertBefore(listener, DataTypeIDInsertionComparator(listener->getDataTypeDescriptor().getID())); + return true; +} + +void Dispatcher::ListenerRegistry::remove(TransferListener* listener) +{ + list_.remove(listener); +} + +bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const +{ + TransferListener* p = list_.get(); + while (p) + { + if (p->getDataTypeDescriptor().getID() == dtid) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +void Dispatcher::ListenerRegistry::cleanup(MonotonicTime ts) +{ + TransferListener* p = list_.get(); + while (p) + { + TransferListener* const next = p->getNextListNode(); + p->cleanup(ts); // p may be modified + p = next; + } +} + +void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame) +{ + TransferListener* p = list_.get(); + while (p) + { + TransferListener* const next = p->getNextListNode(); + if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID()) + { + p->handleFrame(frame); // p may be modified + } + else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id! + { + break; + } + else + { + ; // Nothing to do with this one + } + p = next; + } +} + +/* + * Dispatcher + */ +void Dispatcher::handleFrame(const CanRxFrame& can_frame) +{ + RxFrame frame; + if (!frame.parse(can_frame)) + { + // This is not counted as a transport error + UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str()); + return; + } + + if ((frame.getDstNodeID() != NodeID::Broadcast) && + (frame.getDstNodeID() != getNodeID())) + { + return; + } + + switch (frame.getTransferType()) + { + case TransferTypeMessageBroadcast: + { + lmsg_.handleFrame(frame); + break; + } + case TransferTypeServiceRequest: + { + lsrv_req_.handleFrame(frame); + break; + } + case TransferTypeServiceResponse: + { + lsrv_resp_.handleFrame(frame); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } +} + +#if UAVCAN_TINY +void Dispatcher::handleLoopbackFrame(const CanRxFrame&) +{ +} + +void Dispatcher::notifyRxFrameListener(const CanRxFrame&, CanIOFlags) +{ +} +#else +void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame) +{ + RxFrame frame; + if (!frame.parse(can_frame)) + { + UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str()); + UAVCAN_ASSERT(0); // No way! + return; + } + UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID()); + loopback_listeners_.invokeListeners(frame); +} + +void Dispatcher::notifyRxFrameListener(const CanRxFrame& can_frame, CanIOFlags flags) +{ + if (rx_listener_ != UAVCAN_NULLPTR) + { + rx_listener_->handleRxFrame(can_frame, flags); + } +} +#endif + +int Dispatcher::spin(MonotonicTime deadline) +{ + int num_frames_processed = 0; + do + { + CanIOFlags flags = 0; + CanRxFrame frame; + const int res = canio_.receive(frame, deadline, flags); + if (res < 0) + { + return res; + } + if (res > 0) + { + if (flags & CanIOFlagLoopback) + { + handleLoopbackFrame(frame); + } + else + { + num_frames_processed++; + handleFrame(frame); + } + notifyRxFrameListener(frame, flags); + } + } + while (sysclock_.getMonotonic() < deadline); + + return num_frames_processed; +} + +int Dispatcher::spinOnce() +{ + int num_frames_processed = 0; + + while (true) + { + CanIOFlags flags = 0; + CanRxFrame frame; + const int res = canio_.receive(frame, MonotonicTime(), flags); + if (res < 0) + { + return res; + } + else if (res > 0) + { + if (flags & CanIOFlagLoopback) + { + handleLoopbackFrame(frame); + } + else + { + num_frames_processed++; + handleFrame(frame); + } + notifyRxFrameListener(frame, flags); + } + else + { + break; // No frames left + } + } + + return num_frames_processed; +} + +int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) +{ + if (frame.getSrcNodeID() != getNodeID()) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + CanFrame can_frame; + if (!frame.compile(can_frame)) + { + UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str()); + UAVCAN_ASSERT(0); + return -ErrLogic; + } + return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags); +} + +void Dispatcher::cleanup(MonotonicTime ts) +{ + outgoing_transfer_reg_.cleanup(ts); + lmsg_.cleanup(ts); + lsrv_req_.cleanup(ts); + lsrv_resp_.cleanup(ts); +} + +bool Dispatcher::registerMessageListener(TransferListener* listener) +{ + if (listener->getDataTypeDescriptor().getKind() != DataTypeKindMessage) + { + UAVCAN_ASSERT(0); + return false; + } + return lmsg_.add(listener, ListenerRegistry::ManyListeners); // Multiple subscribers are OK +} + +bool Dispatcher::registerServiceRequestListener(TransferListener* listener) +{ + if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService) + { + UAVCAN_ASSERT(0); + return false; + } + return lsrv_req_.add(listener, ListenerRegistry::UniqueListener); // Only one server per data type +} + +bool Dispatcher::registerServiceResponseListener(TransferListener* listener) +{ + if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService) + { + UAVCAN_ASSERT(0); + return false; + } + return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv +} + +void Dispatcher::unregisterMessageListener(TransferListener* listener) +{ + lmsg_.remove(listener); +} + +void Dispatcher::unregisterServiceRequestListener(TransferListener* listener) +{ + lsrv_req_.remove(listener); +} + +void Dispatcher::unregisterServiceResponseListener(TransferListener* listener) +{ + lsrv_resp_.remove(listener); +} + +bool Dispatcher::hasSubscriber(DataTypeID dtid) const +{ + return lmsg_.exists(dtid); +} + +bool Dispatcher::hasPublisher(DataTypeID dtid) const +{ + return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast); +} + +bool Dispatcher::hasServer(DataTypeID dtid) const +{ + return lsrv_req_.exists(dtid); +} + +bool Dispatcher::setNodeID(NodeID nid) +{ + if (!self_node_id_is_set_) + { + self_node_id_ = nid; + self_node_id_is_set_ = true; + return true; + } + return false; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_frame.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_frame.cpp new file mode 100644 index 0000000000..d97c34f091 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_frame.cpp @@ -0,0 +1,334 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Frame + */ +uint8_t Frame::setPayload(const uint8_t* data, unsigned len) +{ + const uint8_t maxlen = getPayloadCapacity(); + len = min(unsigned(maxlen), len); + (void)copy(data, data + len, payload_); + payload_len_ = uint_fast8_t(len); + return static_cast(len); +} + +template +inline static uint32_t bitunpack(uint32_t val) +{ + StaticAssert<(OFFSET >= 0)>::check(); + StaticAssert<(WIDTH > 0)>::check(); + StaticAssert<((OFFSET + WIDTH) <= 29)>::check(); + return (val >> OFFSET) & ((1UL << WIDTH) - 1); +} + +bool Frame::parse(const CanFrame& can_frame) +{ + if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended()) + { + UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__); + return false; + } + + if (can_frame.dlc > sizeof(can_frame.data)) + { + UAVCAN_ASSERT(0); // This is not a protocol error, so UAVCAN_ASSERT() is ok + return false; + } + + if (can_frame.dlc < 1) + { + UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__); + return false; + } + + /* + * CAN ID parsing + */ + const uint32_t id = can_frame.id & CanFrame::MaskExtID; + + transfer_priority_ = static_cast(bitunpack<24, 5>(id)); + src_node_id_ = static_cast(bitunpack<0, 7>(id)); + + const bool service_not_message = bitunpack<7, 1>(id) != 0U; + if (service_not_message) + { + const bool request_not_response = bitunpack<15, 1>(id) != 0U; + transfer_type_ = request_not_response ? TransferTypeServiceRequest : TransferTypeServiceResponse; + + dst_node_id_ = static_cast(bitunpack<8, 7>(id)); + data_type_id_ = static_cast(bitunpack<16, 8>(id)); + } + else + { + transfer_type_ = TransferTypeMessageBroadcast; + dst_node_id_ = NodeID::Broadcast; + + data_type_id_ = static_cast(bitunpack<8, 16>(id)); + + if (src_node_id_.isBroadcast()) + { + // Removing the discriminator + data_type_id_ = static_cast(data_type_id_.get() & 3U); + } + } + + /* + * CAN payload parsing + */ + payload_len_ = static_cast(can_frame.dlc - 1U); + (void)copy(can_frame.data, can_frame.data + payload_len_, payload_); + + const uint8_t tail = can_frame.data[can_frame.dlc - 1U]; + + start_of_transfer_ = (tail & (1U << 7)) != 0; + end_of_transfer_ = (tail & (1U << 6)) != 0; + toggle_ = (tail & (1U << 5)) != 0; + + transfer_id_ = tail & TransferID::Max; + + return isValid(); +} + +template +inline static uint32_t bitpack(uint32_t field) +{ + StaticAssert<(OFFSET >= 0)>::check(); + StaticAssert<(WIDTH > 0)>::check(); + StaticAssert<((OFFSET + WIDTH) <= 29)>::check(); + UAVCAN_ASSERT((field & ((1UL << WIDTH) - 1)) == field); + return uint32_t((field & ((1UL << WIDTH) - 1)) << OFFSET); +} + +bool Frame::compile(CanFrame& out_can_frame) const +{ + if (!isValid()) + { + UAVCAN_ASSERT(0); // This is an application error, so we need to maximize it. + return false; + } + + /* + * CAN ID field + */ + out_can_frame.id = CanFrame::FlagEFF | + bitpack<0, 7>(src_node_id_.get()) | + bitpack<24, 5>(transfer_priority_.get()); + + if (transfer_type_ == TransferTypeMessageBroadcast) + { + out_can_frame.id |= + bitpack<7, 1>(0U) | + bitpack<8, 16>(data_type_id_.get()); + } + else + { + const bool request_not_response = transfer_type_ == TransferTypeServiceRequest; + out_can_frame.id |= + bitpack<7, 1>(1U) | + bitpack<8, 7>(dst_node_id_.get()) | + bitpack<15, 1>(request_not_response ? 1U : 0U) | + bitpack<16, 8>(data_type_id_.get()); + } + + /* + * Payload + */ + uint8_t tail = transfer_id_.get(); + if (start_of_transfer_) + { + tail |= (1U << 7); + } + if (end_of_transfer_) + { + tail |= (1U << 6); + } + if (toggle_) + { + tail |= (1U << 5); + } + + UAVCAN_ASSERT(payload_len_ < sizeof(static_cast(UAVCAN_NULLPTR)->data)); + + out_can_frame.dlc = static_cast(payload_len_); + (void)copy(payload_, payload_ + payload_len_, out_can_frame.data); + + out_can_frame.data[out_can_frame.dlc] = tail; + out_can_frame.dlc++; + + /* + * Discriminator + */ + if (src_node_id_.isBroadcast()) + { + TransferCRC crc; + crc.add(out_can_frame.data, out_can_frame.dlc); + out_can_frame.id |= bitpack<10, 14>(crc.get() & ((1U << 14) - 1U)); + } + + return true; +} + +bool Frame::isValid() const +{ + /* + * Toggle + */ + if (start_of_transfer_ && toggle_) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Node ID + */ + if (!src_node_id_.isValid() || !dst_node_id_.isValid()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + if (src_node_id_.isUnicast() && (src_node_id_ == dst_node_id_)) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Transfer type + */ + if (transfer_type_ >= NumTransferTypes) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + // Anonymous transfers + if (src_node_id_.isBroadcast() && + (!start_of_transfer_ || !end_of_transfer_ || (transfer_type_ != TransferTypeMessageBroadcast))) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Payload + */ + if (payload_len_ > getPayloadCapacity()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Data type ID + */ + if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_))) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + /* + * Priority + */ + if (!transfer_priority_.isValid()) + { + UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__); + return false; + } + + return true; +} + +bool Frame::operator==(const Frame& rhs) const +{ + return + (transfer_priority_ == rhs.transfer_priority_) && + (transfer_type_ == rhs.transfer_type_) && + (data_type_id_ == rhs.data_type_id_) && + (src_node_id_ == rhs.src_node_id_) && + (dst_node_id_ == rhs.dst_node_id_) && + (transfer_id_ == rhs.transfer_id_) && + (toggle_ == rhs.toggle_) && + (start_of_transfer_ == rhs.start_of_transfer_) && + (end_of_transfer_ == rhs.end_of_transfer_) && + (payload_len_ == rhs.payload_len_) && + equal(payload_, payload_ + payload_len_, rhs.payload_); +} + +#if UAVCAN_TOSTRING +std::string Frame::toString() const +{ + static const int BUFLEN = 100; + char buf[BUFLEN]; + int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d sot=%d eot=%d togl=%d tid=%d payload=[", + int(transfer_priority_.get()), int(data_type_id_.get()), int(transfer_type_), + int(src_node_id_.get()), int(dst_node_id_.get()), + int(start_of_transfer_), int(end_of_transfer_), int(toggle_), int(transfer_id_.get())); + + for (unsigned i = 0; i < payload_len_; i++) + { + // Coverity Scan complains about payload_ being not default initialized. This is OK. + // coverity[read_parm_fld] + ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), "%02x", payload_[i]); + if ((i + 1) < payload_len_) + { + ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), " "); + } + } + (void)snprintf(buf + ofs, unsigned(BUFLEN - ofs), "]"); + return std::string(buf); +} +#endif + +/** + * RxFrame + */ +bool RxFrame::parse(const CanRxFrame& can_frame) +{ + if (!Frame::parse(can_frame)) + { + return false; + } + if (can_frame.ts_mono.isZero()) // Monotonic timestamps are mandatory. + { + UAVCAN_ASSERT(0); // If it is not set, it's a driver failure. + return false; + } + ts_mono_ = can_frame.ts_mono; + ts_utc_ = can_frame.ts_utc; + iface_index_ = can_frame.iface_index; + return true; +} + +#if UAVCAN_TOSTRING +std::string RxFrame::toString() const +{ + std::string out = Frame::toString(); + out.reserve(128); + out += " ts_m=" + ts_mono_.toString(); + out += " ts_utc=" + ts_utc_.toString(); + out += " iface="; + out += char('0' + iface_index_); + return out; +} +#endif + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp new file mode 100644 index 0000000000..fc49f65aa3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_outgoing_transfer_registry.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ +/* + * OutgoingTransferRegistryKey + */ +#if UAVCAN_TOSTRING +std::string OutgoingTransferRegistryKey::toString() const +{ + char buf[40]; + (void)snprintf(buf, sizeof(buf), "dtid=%u tt=%u dnid=%u", + int(data_type_id_.get()), int(transfer_type_), int(destination_node_id_.get())); + return std::string(buf); +} +#endif + +/* + * OutgoingTransferRegistry + */ +const MonotonicDuration OutgoingTransferRegistry::MinEntryLifetime = MonotonicDuration::fromMSec(2000); + +TransferID* OutgoingTransferRegistry::accessOrCreate(const OutgoingTransferRegistryKey& key, + MonotonicTime new_deadline) +{ + UAVCAN_ASSERT(!new_deadline.isZero()); + Value* p = map_.access(key); + if (p == UAVCAN_NULLPTR) + { + p = map_.insert(key, Value()); + if (p == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + UAVCAN_TRACE("OutgoingTransferRegistry", "Created %s", key.toString().c_str()); + } + p->deadline = new_deadline; + return &p->tid; +} + +bool OutgoingTransferRegistry::exists(DataTypeID dtid, TransferType tt) const +{ + return UAVCAN_NULLPTR != map_.find(ExistenceCheckingPredicate(dtid, tt)); +} + +void OutgoingTransferRegistry::cleanup(MonotonicTime ts) +{ + map_.removeAllWhere(DeadlineExpiredPredicate(ts)); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer.cpp new file mode 100644 index 0000000000..58778ef168 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/** + * TransferPriority + */ +const uint8_t TransferPriority::BitLen; +const uint8_t TransferPriority::NumericallyMax; +const uint8_t TransferPriority::NumericallyMin; + +const TransferPriority TransferPriority::Default((1U << BitLen) / 2); +const TransferPriority TransferPriority::MiddleLower((1U << BitLen) / 2 + (1U << BitLen) / 4); +const TransferPriority TransferPriority::OneHigherThanLowest(NumericallyMax - 1); +const TransferPriority TransferPriority::OneLowerThanHighest(NumericallyMin + 1); +const TransferPriority TransferPriority::Lowest(NumericallyMax); + +/** + * TransferID + */ +const uint8_t TransferID::BitLen; +const uint8_t TransferID::Max; +const uint8_t TransferID::Half; + +/** + * NodeID + */ +const uint8_t NodeID::ValueBroadcast; +const uint8_t NodeID::ValueInvalid; +const uint8_t NodeID::BitLen; +const uint8_t NodeID::Max; +const uint8_t NodeID::MaxRecommendedForRegularNodes; +const NodeID NodeID::Broadcast(ValueBroadcast); + +/** + * TransferID + */ +int TransferID::computeForwardDistance(TransferID rhs) const +{ + int d = int(rhs.get()) - int(get()); + if (d < 0) + { + d += 1 << BitLen; + } + + UAVCAN_ASSERT(((get() + d) & Max) == rhs.get()); + return d; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_buffer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_buffer.cpp new file mode 100644 index 0000000000..5e670cf0eb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_buffer.cpp @@ -0,0 +1,362 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * StaticTransferBufferImpl + */ +int StaticTransferBufferImpl::read(unsigned offset, uint8_t* data, unsigned len) const +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= max_write_pos_) + { + return 0; + } + if ((offset + len) > max_write_pos_) + { + len = max_write_pos_ - offset; + } + UAVCAN_ASSERT((offset + len) <= max_write_pos_); + (void)copy(data_ + offset, data_ + offset + len, data); + return int(len); +} + +int StaticTransferBufferImpl::write(unsigned offset, const uint8_t* data, unsigned len) +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= size_) + { + return 0; + } + if ((offset + len) > size_) + { + len = size_ - offset; + } + UAVCAN_ASSERT((offset + len) <= size_); + (void)copy(data, data + len, data_ + offset); + max_write_pos_ = max(uint16_t(offset + len), uint16_t(max_write_pos_)); + return int(len); +} + +void StaticTransferBufferImpl::reset() +{ + max_write_pos_ = 0; +#if UAVCAN_DEBUG + fill(data_, data_ + size_, uint8_t(0)); +#endif +} + +/* + * TransferBufferManagerKey + */ +#if UAVCAN_TOSTRING +std::string TransferBufferManagerKey::toString() const +{ + char buf[24]; + (void)snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_)); + return std::string(buf); +} +#endif + +/* + * DynamicTransferBuffer::Block + */ +TransferBufferManagerEntry::Block* +TransferBufferManagerEntry::Block::instantiate(IPoolAllocator& allocator) +{ + void* const praw = allocator.allocate(sizeof(Block)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) Block; +} + +void TransferBufferManagerEntry::Block::destroy(Block*& obj, IPoolAllocator& allocator) +{ + if (obj != UAVCAN_NULLPTR) + { + obj->~Block(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } +} + +void TransferBufferManagerEntry::Block::read(uint8_t*& outptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_read) +{ + UAVCAN_ASSERT(outptr); + for (unsigned i = 0; (i < Block::Size) && (left_to_read > 0); i++, total_offset++) + { + if (total_offset >= target_offset) + { + *outptr++ = data[i]; + left_to_read--; + } + } +} + +void TransferBufferManagerEntry::Block::write(const uint8_t*& inptr, unsigned target_offset, + unsigned& total_offset, unsigned& left_to_write) +{ + UAVCAN_ASSERT(inptr); + for (unsigned i = 0; (i < Block::Size) && (left_to_write > 0); i++, total_offset++) + { + if (total_offset >= target_offset) + { + data[i] = *inptr++; + left_to_write--; + } + } +} + +/* + * DynamicTransferBuffer + */ +TransferBufferManagerEntry* TransferBufferManagerEntry::instantiate(IPoolAllocator& allocator, + uint16_t max_size) +{ + void* const praw = allocator.allocate(sizeof(TransferBufferManagerEntry)); + if (praw == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; + } + return new (praw) TransferBufferManagerEntry(allocator, max_size); +} + +void TransferBufferManagerEntry::destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator) +{ + if (obj != UAVCAN_NULLPTR) + { + obj->~TransferBufferManagerEntry(); + allocator.deallocate(obj); + obj = UAVCAN_NULLPTR; + } +} + +int TransferBufferManagerEntry::read(unsigned offset, uint8_t* data, unsigned len) const +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= max_write_pos_) + { + return 0; + } + if ((offset + len) > max_write_pos_) + { + len = max_write_pos_ - offset; + } + UAVCAN_ASSERT((offset + len) <= max_write_pos_); + + // This shall be optimized. + unsigned total_offset = 0; + unsigned left_to_read = len; + uint8_t* outptr = data; + Block* p = blocks_.get(); + while (p) + { + p->read(outptr, offset, total_offset, left_to_read); + if (left_to_read == 0) + { + break; + } + p = p->getNextListNode(); + } + + UAVCAN_ASSERT(left_to_read == 0); + return int(len); +} + +int TransferBufferManagerEntry::write(unsigned offset, const uint8_t* data, unsigned len) +{ + if (!data) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + if (offset >= max_size_) + { + return 0; + } + if ((offset + len) > max_size_) + { + len = max_size_ - offset; + } + UAVCAN_ASSERT((offset + len) <= max_size_); + + unsigned total_offset = 0; + unsigned left_to_write = len; + const uint8_t* inptr = data; + Block* p = blocks_.get(); + Block* last_written_block = UAVCAN_NULLPTR; + + // First we need to write the part that is already allocated + while (p) + { + last_written_block = p; + p->write(inptr, offset, total_offset, left_to_write); + if (left_to_write == 0) + { + break; + } + p = p->getNextListNode(); + } + + // Then we need to append new chunks until all data is written + while (left_to_write > 0) + { + // cppcheck-suppress nullPointer + UAVCAN_ASSERT(p == UAVCAN_NULLPTR); + + // Allocating the chunk + Block* new_block = Block::instantiate(allocator_); + if (new_block == UAVCAN_NULLPTR) + { + break; // We're in deep shit. + } + // Appending the chain with the new block + if (last_written_block != UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(last_written_block->getNextListNode() == UAVCAN_NULLPTR); // Because it is last in the chain + last_written_block->setNextListNode(new_block); + new_block->setNextListNode(UAVCAN_NULLPTR); + } + else + { + blocks_.insert(new_block); + } + last_written_block = new_block; + + // Writing the data + new_block->write(inptr, offset, total_offset, left_to_write); + } + + UAVCAN_ASSERT(len >= left_to_write); + const unsigned actually_written = len - left_to_write; + max_write_pos_ = max(uint16_t(offset + actually_written), uint16_t(max_write_pos_)); + return int(actually_written); +} + +void TransferBufferManagerEntry::reset(const TransferBufferManagerKey& key) +{ + key_ = key; + max_write_pos_ = 0; + Block* p = blocks_.get(); + while (p) + { + Block* const next = p->getNextListNode(); + blocks_.remove(p); + Block::destroy(p, allocator_); + p = next; + } +} + +/* + * TransferBufferManager + */ +TransferBufferManagerEntry* TransferBufferManager::findFirst(const TransferBufferManagerKey& key) +{ + TransferBufferManagerEntry* dyn = buffers_.get(); + while (dyn) + { + UAVCAN_ASSERT(!dyn->isEmpty()); + if (dyn->getKey() == key) + { + return dyn; + } + dyn = dyn->getNextListNode(); + } + return UAVCAN_NULLPTR; +} + +TransferBufferManager::~TransferBufferManager() +{ + TransferBufferManagerEntry* dyn = buffers_.get(); + while (dyn) + { + TransferBufferManagerEntry* const next = dyn->getNextListNode(); + buffers_.remove(dyn); + TransferBufferManagerEntry::destroy(dyn, allocator_); + dyn = next; + } +} + +ITransferBuffer* TransferBufferManager::access(const TransferBufferManagerKey& key) +{ + if (key.isEmpty()) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + return findFirst(key); +} + +ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key) +{ + if (key.isEmpty()) + { + UAVCAN_ASSERT(0); + return UAVCAN_NULLPTR; + } + remove(key); + + TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_); + if (tbme == UAVCAN_NULLPTR) + { + return UAVCAN_NULLPTR; // Epic fail. + } + + buffers_.insert(tbme); + + UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str()); + + if (tbme != UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(tbme->isEmpty()); + tbme->reset(key); + } + return tbme; +} + +void TransferBufferManager::remove(const TransferBufferManagerKey& key) +{ + UAVCAN_ASSERT(!key.isEmpty()); + + TransferBufferManagerEntry* dyn = findFirst(key); + if (dyn != UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferBufferManager", "Buffer deleted, %s", key.toString().c_str()); + buffers_.remove(dyn); + TransferBufferManagerEntry::destroy(dyn, allocator_); + } +} + +bool TransferBufferManager::isEmpty() const +{ + return getNumBuffers() == 0; +} + +unsigned TransferBufferManager::getNumBuffers() const +{ + return buffers_.getLength(); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_listener.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_listener.cpp new file mode 100644 index 0000000000..304e67126a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_listener.cpp @@ -0,0 +1,258 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ +/* + * IncomingTransfer + */ +int IncomingTransfer::write(unsigned, const uint8_t*, unsigned) +{ + UAVCAN_ASSERT(0); // Incoming transfer container is read-only + return -ErrLogic; +} + +/* + * SingleFrameIncomingTransfer + */ +SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm) + : IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getPriority(), + frm.getTransferType(), frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex()) + , payload_(frm.getPayloadPtr()) + , payload_len_(uint8_t(frm.getPayloadLen())) +{ + UAVCAN_ASSERT(frm.isValid()); +} + +int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const +{ + if (data == UAVCAN_NULLPTR) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + if (offset >= payload_len_) + { + return 0; + } + if ((offset + len) > payload_len_) + { + len = payload_len_ - offset; + } + UAVCAN_ASSERT((offset + len) <= payload_len_); + (void)copy(payload_ + offset, payload_ + offset + len, data); + return int(len); +} + +bool SingleFrameIncomingTransfer::isAnonymousTransfer() const +{ + return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast(); +} + +/* + * MultiFrameIncomingTransfer + */ +MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, + const RxFrame& last_frame, TransferBufferAccessor& tba) + : IncomingTransfer(ts_mono, ts_utc, last_frame.getPriority(), last_frame.getTransferType(), + last_frame.getTransferID(), last_frame.getSrcNodeID(), last_frame.getIfaceIndex()) + , buf_acc_(tba) +{ + UAVCAN_ASSERT(last_frame.isValid()); + UAVCAN_ASSERT(last_frame.isEndOfTransfer()); +} + +int MultiFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const +{ + const ITransferBuffer* const tbb = const_cast(buf_acc_).access(); + if (tbb == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer"); + return -ErrLogic; + } + return tbb->read(offset, data, len); +} + +/* + * TransferListener::TimedOutReceiverPredicate + */ +bool TransferListener::TimedOutReceiverPredicate::operator()(const TransferBufferManagerKey& key, + const TransferReceiver& value) const +{ + if (value.isTimedOut(ts_)) + { + UAVCAN_TRACE("TransferListener", "Timed out receiver: %s", key.toString().c_str()); + /* + * TransferReceivers do not own their buffers - this helps the Map<> container to copy them + * around quickly and safely (using default assignment operator). Downside is that we need to + * destroy the buffers manually. + * Maybe it is not good that the predicate has side effects, but I ran out of better ideas. + */ + parent_bufmgr_.remove(key); + return true; + } + return false; +} + +/* + * TransferListener + */ +bool TransferListener::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const +{ + TransferCRC crc = crc_base_; + unsigned offset = 0; + while (true) + { + uint8_t buf[16]; + const int res = tbb.read(offset, buf, sizeof(buf)); + if (res < 0) + { + UAVCAN_TRACE("TransferListener", "Failed to check CRC: Buffer read failure %i", res); + return false; + } + if (res == 0) + { + break; + } + offset += unsigned(res); + crc.add(buf, unsigned(res)); + } + if (crc.get() != compare_with) + { + UAVCAN_TRACE("TransferListener", "CRC mismatch, expected=0x%04x, got=0x%04x", + int(compare_with), int(crc.get())); + return false; + } + return true; +} + +void TransferListener::handleReception(TransferReceiver& receiver, const RxFrame& frame, + TransferBufferAccessor& tba) +{ + switch (receiver.addFrame(frame, tba)) + { + case TransferReceiver::ResultNotComplete: + { + perf_.addErrors(receiver.yieldErrorCount()); + break; + } + case TransferReceiver::ResultSingleFrame: + { + perf_.addRxTransfer(); + SingleFrameIncomingTransfer it(frame); + handleIncomingTransfer(it); + break; + } + case TransferReceiver::ResultComplete: + { + perf_.addRxTransfer(); + const ITransferBuffer* tbb = tba.access(); + if (tbb == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferListener", "Buffer access failure, last frame: %s", frame.toString().c_str()); + break; + } + if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb)) + { + UAVCAN_TRACE("TransferListener", "CRC error, last frame: %s", frame.toString().c_str()); + break; + } + MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(), + receiver.getLastTransferTimestampUtc(), frame, tba); + handleIncomingTransfer(it); + it.release(); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } +} + +void TransferListener::handleAnonymousTransferReception(const RxFrame& frame) +{ + if (allow_anonymous_transfers_) + { + perf_.addRxTransfer(); + SingleFrameIncomingTransfer it(frame); + handleIncomingTransfer(it); + } +} + +TransferListener::~TransferListener() +{ + // Map must be cleared before bufmgr is destroyed + receivers_.clear(); +} + +void TransferListener::cleanup(MonotonicTime ts) +{ + receivers_.removeAllWhere(TimedOutReceiverPredicate(ts, bufmgr_)); + UAVCAN_ASSERT(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1); +} + +void TransferListener::handleFrame(const RxFrame& frame) +{ + if (frame.getSrcNodeID().isUnicast()) // Normal transfer + { + const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); + + TransferReceiver* recv = receivers_.access(key); + if (recv == UAVCAN_NULLPTR) + { + if (!frame.isStartOfTransfer()) + { + return; + } + + TransferReceiver new_recv; + recv = receivers_.insert(key, new_recv); + if (recv == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); + return; + } + } + TransferBufferAccessor tba(bufmgr_, key); + handleReception(*recv, frame, tba); + } + else if (frame.getSrcNodeID().isBroadcast() && + frame.isStartOfTransfer() && + frame.isEndOfTransfer() && + frame.getDstNodeID().isBroadcast()) // Anonymous transfer + { + handleAnonymousTransferReception(frame); + } + else + { + UAVCAN_TRACE("TransferListener", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame + } +} + +/* + * TransferListenerWithFilter + */ +void TransferListenerWithFilter::handleFrame(const RxFrame& frame) +{ + if (filter_ != UAVCAN_NULLPTR) + { + if (filter_->shouldAcceptFrame(frame)) + { + TransferListener::handleFrame(frame); + } + } + else + { + UAVCAN_ASSERT(0); + } +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_receiver.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_receiver.cpp new file mode 100644 index 0000000000..d60e92861d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_receiver.cpp @@ -0,0 +1,248 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +const uint16_t TransferReceiver::MinTransferIntervalMSec; +const uint16_t TransferReceiver::MaxTransferIntervalMSec; +const uint16_t TransferReceiver::DefaultTransferIntervalMSec; +const uint16_t TransferReceiver::DefaultTidTimeoutMSec; + +MonotonicDuration TransferReceiver::getIfaceSwitchDelay() const +{ + return MonotonicDuration::fromMSec(transfer_interval_msec_); +} + +MonotonicDuration TransferReceiver::getTidTimeout() const +{ + return MonotonicDuration::fromMSec(DefaultTidTimeoutMSec); +} + +void TransferReceiver::registerError() const +{ + error_cnt_ = static_cast(error_cnt_ + 1) & ErrorCntMask; +} + +void TransferReceiver::updateTransferTimings() +{ + UAVCAN_ASSERT(!this_transfer_ts_.isZero()); + + const MonotonicTime prev_prev_ts = prev_transfer_ts_; + prev_transfer_ts_ = this_transfer_ts_; + + if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts)) + { + uint64_t interval_msec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toMSec()); + interval_msec = min(interval_msec, uint64_t(MaxTransferIntervalMSec)); + interval_msec = max(interval_msec, uint64_t(MinTransferIntervalMSec)); + transfer_interval_msec_ = static_cast((uint64_t(transfer_interval_msec_) * 7U + interval_msec) / 8U); + } +} + +void TransferReceiver::prepareForNextTransfer() +{ + tid_.increment(); + next_toggle_ = false; + buffer_write_pos_ = 0; +} + +bool TransferReceiver::validate(const RxFrame& frame) const +{ + if (iface_index_ != frame.getIfaceIndex()) + { + return false; + } + if (frame.isStartOfTransfer() && !frame.isEndOfTransfer() && (frame.getPayloadLen() < TransferCRC::NumBytes)) + { + UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str()); + registerError(); + return false; + } + if (frame.isStartOfTransfer() && frame.getToggle()) + { + UAVCAN_TRACE("TransferReceiver", "Toggle bit is not cleared, %s", frame.toString().c_str()); + registerError(); + return false; + } + if (frame.isStartOfTransfer() && isMidTransfer()) + { + UAVCAN_TRACE("TransferReceiver", "Unexpected start of transfer, %s", frame.toString().c_str()); + registerError(); + } + if (frame.getToggle() != next_toggle_) + { + UAVCAN_TRACE("TransferReceiver", "Unexpected toggle bit (not %i), %s", + int(next_toggle_), frame.toString().c_str()); + registerError(); + return false; + } + if (frame.getTransferID() != tid_) + { + UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str()); + registerError(); + return false; + } + return true; +} + +bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf) +{ + const uint8_t* const payload = frame.getPayloadPtr(); + const unsigned payload_len = frame.getPayloadLen(); + + if (frame.isStartOfTransfer()) // First frame contains CRC, we need to extract it now + { + if (frame.getPayloadLen() < TransferCRC::NumBytes) + { + return false; // Must have been validated earlier though. I think I'm paranoid. + } + this_transfer_crc_ = static_cast(payload[0] & 0xFF); + this_transfer_crc_ |= static_cast(static_cast(payload[1] & 0xFF) << 8); // Little endian. + + const unsigned effective_payload_len = payload_len - TransferCRC::NumBytes; + const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len); + const bool success = res == static_cast(effective_payload_len); + if (success) + { + buffer_write_pos_ = static_cast(buffer_write_pos_ + effective_payload_len); + } + return success; + } + else + { + const int res = buf.write(buffer_write_pos_, payload, payload_len); + const bool success = res == static_cast(payload_len); + if (success) + { + buffer_write_pos_ = static_cast(buffer_write_pos_ + payload_len); + } + return success; + } +} + +TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba) +{ + // Transfer timestamps are derived from the first frame + if (frame.isStartOfTransfer()) + { + this_transfer_ts_ = frame.getMonotonicTimestamp(); + first_frame_ts_ = frame.getUtcTimestamp(); + } + + if (frame.isStartOfTransfer() && frame.isEndOfTransfer()) + { + tba.remove(); + updateTransferTimings(); + prepareForNextTransfer(); + this_transfer_crc_ = 0; // SFT has no CRC + return ResultSingleFrame; + } + + // Payload write + ITransferBuffer* buf = tba.access(); + if (buf == UAVCAN_NULLPTR) + { + buf = tba.create(); + } + if (buf == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str()); + prepareForNextTransfer(); + registerError(); + return ResultNotComplete; + } + if (!writePayload(frame, *buf)) + { + UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str()); + tba.remove(); + prepareForNextTransfer(); + registerError(); + return ResultNotComplete; + } + next_toggle_ = !next_toggle_; + + if (frame.isEndOfTransfer()) + { + updateTransferTimings(); + prepareForNextTransfer(); + return ResultComplete; + } + return ResultNotComplete; +} + +bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const +{ + return (current_ts - this_transfer_ts_) > getTidTimeout(); +} + +TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba) +{ + if ((frame.getMonotonicTimestamp().isZero()) || + (frame.getMonotonicTimestamp() < prev_transfer_ts_) || + (frame.getMonotonicTimestamp() < this_transfer_ts_)) + { + UAVCAN_TRACE("TransferReceiver", "Invalid frame, %s", frame.toString().c_str()); + return ResultNotComplete; + } + + const bool not_initialized = !isInitialized(); + const bool tid_timed_out = isTimedOut(frame.getMonotonicTimestamp()); + const bool same_iface = frame.getIfaceIndex() == iface_index_; + const bool first_frame = frame.isStartOfTransfer(); + const bool non_wrapped_tid = tid_.computeForwardDistance(frame.getTransferID()) < TransferID::Half; + const bool not_previous_tid = frame.getTransferID().computeForwardDistance(tid_) > 1; + const bool iface_switch_allowed = (frame.getMonotonicTimestamp() - this_transfer_ts_) > getIfaceSwitchDelay(); + + // FSM, the hard way + const bool need_restart = + (not_initialized) || + (tid_timed_out) || + (same_iface && first_frame && not_previous_tid) || + (iface_switch_allowed && first_frame && non_wrapped_tid); + + if (need_restart) + { + if (!not_initialized && (tid_ != frame.getTransferID())) + { + registerError(); + } + UAVCAN_TRACE("TransferReceiver", "Restart [ni=%d, isa=%d, tt=%d, si=%d, ff=%d, nwtid=%d, nptid=%d, tid=%d], %s", + int(not_initialized), int(iface_switch_allowed), int(tid_timed_out), int(same_iface), + int(first_frame), int(non_wrapped_tid), int(not_previous_tid), int(tid_.get()), + frame.toString().c_str()); + tba.remove(); + iface_index_ = frame.getIfaceIndex() & IfaceIndexMask; + tid_ = frame.getTransferID(); + next_toggle_ = false; + buffer_write_pos_ = 0; + this_transfer_crc_ = 0; + if (!first_frame) + { + tid_.increment(); + return ResultNotComplete; + } + } + + if (!validate(frame)) + { + return ResultNotComplete; + } + return receive(frame, tba); +} + +uint8_t TransferReceiver::yieldErrorCount() +{ + const uint8_t ret = error_cnt_; + error_cnt_ = 0; + return ret; +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_sender.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_sender.cpp new file mode 100644 index 0000000000..f5d69d9b9e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_transfer_sender.cpp @@ -0,0 +1,173 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +namespace uavcan +{ + +void TransferSender::registerError() const +{ + dispatcher_.getTransferPerfCounter().addError(); +} + +void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos) +{ + UAVCAN_ASSERT(!isInitialized()); + + qos_ = qos; + data_type_id_ = dtid.getID(); + crc_base_ = dtid.getSignature().toTransferCRC(); +} + +int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, + TransferID tid) const +{ + Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, tid); + + frame.setPriority(priority_); + frame.setStartOfTransfer(true); + + UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str()); + + /* + * Checking if we're allowed to send. + * In passive mode we can send only anonymous transfers, if they are enabled. + */ + if (dispatcher_.isPassiveMode()) + { + const bool allow = allow_anonymous_transfers_ && + (transfer_type == TransferTypeMessageBroadcast) && + (int(payload_len) <= frame.getPayloadCapacity()); + if (!allow) + { + return -ErrPassiveMode; + } + } + + dispatcher_.getTransferPerfCounter().addTxTransfer(); + + /* + * Sending frames + */ + if (frame.getPayloadCapacity() >= payload_len) // Single Frame Transfer + { + const int res = frame.setPayload(payload, payload_len); + if (res != int(payload_len)) + { + UAVCAN_ASSERT(0); + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res); + registerError(); + return (res < 0) ? res : -ErrLogic; + } + + frame.setEndOfTransfer(true); + UAVCAN_ASSERT(frame.isStartOfTransfer() && frame.isEndOfTransfer() && !frame.getToggle()); + + const CanIOFlags flags = frame.getSrcNodeID().isUnicast() ? flags_ : (flags_ | CanIOFlagAbortOnError); + + return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags, iface_mask_); + } + else // Multi Frame Transfer + { + UAVCAN_ASSERT(!dispatcher_.isPassiveMode()); + UAVCAN_ASSERT(frame.getSrcNodeID().isUnicast()); + + int offset = 0; + { + TransferCRC crc = crc_base_; + crc.add(payload, payload_len); + + static const int BUFLEN = sizeof(static_cast(0)->data); + uint8_t buf[BUFLEN]; + + buf[0] = uint8_t(crc.get() & 0xFFU); // Transfer CRC, little endian + buf[1] = uint8_t((crc.get() >> 8) & 0xFF); + (void)copy(payload, payload + BUFLEN - 2, buf + 2); + + const int write_res = frame.setPayload(buf, BUFLEN); + if (write_res < 2) + { + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res); + registerError(); + return write_res; + } + offset = write_res - 2; + UAVCAN_ASSERT(int(payload_len) > offset); + } + + int num_sent = 0; + + while (true) + { + const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_); + if (send_res < 0) + { + registerError(); + return send_res; + } + + num_sent++; + if (frame.isEndOfTransfer()) + { + return num_sent; // Number of frames transmitted + } + + frame.setStartOfTransfer(false); + frame.flipToggle(); + + UAVCAN_ASSERT(offset >= 0); + const int write_res = frame.setPayload(payload + offset, payload_len - unsigned(offset)); + if (write_res < 0) + { + UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res); + registerError(); + return write_res; + } + + offset += write_res; + UAVCAN_ASSERT(offset <= int(payload_len)); + if (offset >= int(payload_len)) + { + frame.setEndOfTransfer(true); + } + } + } + + UAVCAN_ASSERT(0); + return -ErrLogic; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable. +} + +int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, + MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const +{ + /* + * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped? + */ + const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id); + + UAVCAN_ASSERT(!tx_deadline.isZero()); + const MonotonicTime otr_deadline = tx_deadline + max(max_transfer_interval_ * 2, + OutgoingTransferRegistry::MinEntryLifetime); + + TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (tid == UAVCAN_NULLPTR) + { + UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i", + int(data_type_id_.get()), int(transfer_type)); + return -ErrMemory; + } + + const TransferID this_tid = tid->get(); + tid->increment(); + + return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type, + dst_node_id, this_tid); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_data_type.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_data_type.cpp new file mode 100644 index 0000000000..5f605d60ff --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_data_type.cpp @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +namespace uavcan +{ +/* + * DataTypeID + */ +const uint16_t DataTypeID::MaxServiceDataTypeIDValue; +const uint16_t DataTypeID::MaxMessageDataTypeIDValue; +const uint16_t DataTypeID::MaxPossibleDataTypeIDValue; + +DataTypeID DataTypeID::getMaxValueForDataTypeKind(const DataTypeKind dtkind) +{ + if (dtkind == DataTypeKindService) + { + return MaxServiceDataTypeIDValue; + } + else if (dtkind == DataTypeKindMessage) + { + return MaxMessageDataTypeIDValue; + } + else + { + UAVCAN_ASSERT(0); + return DataTypeID(0); + } +} + +/* + * DataTypeSignatureCRC + */ +DataTypeSignatureCRC DataTypeSignatureCRC::extend(uint64_t crc) +{ + DataTypeSignatureCRC ret; + ret.crc_ = crc ^ 0xFFFFFFFFFFFFFFFF; + return ret; +} + +void DataTypeSignatureCRC::add(uint8_t byte) +{ + static const uint64_t Poly = 0x42F0E1EBA9EA3693; + crc_ ^= uint64_t(byte) << 56; + for (int i = 0; i < 8; i++) + { + crc_ = (crc_ & (uint64_t(1) << 63)) ? (crc_ << 1) ^ Poly : crc_ << 1; + } +} + +void DataTypeSignatureCRC::add(const uint8_t* bytes, unsigned len) +{ + UAVCAN_ASSERT(bytes); + while (len--) + { + add(*bytes++); + } +} + +/* + * DataTypeSignature + */ +void DataTypeSignature::mixin64(uint64_t x) +{ + DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_); + for (int i = 0; i < 64; i += 8) // LSB first + { + crc.add((x >> i) & 0xFF); + } + value_ = crc.get(); +} + +void DataTypeSignature::extend(DataTypeSignature dts) +{ + const uint64_t y = value_; + mixin64(dts.get()); + mixin64(y); +} + +TransferCRC DataTypeSignature::toTransferCRC() const +{ + TransferCRC tcrc; + for (int i = 0; i < 64; i += 8) // LSB first + { + tcrc.add((value_ >> i) & 0xFF); + } + return tcrc; +} + +/* + * DataTypeDescriptor + */ +const unsigned DataTypeDescriptor::MaxFullNameLen; + +bool DataTypeDescriptor::isValid() const +{ + return id_.isValidForDataTypeKind(kind_) && + (full_name_ != UAVCAN_NULLPTR) && + (*full_name_ != '\0'); +} + +bool DataTypeDescriptor::match(DataTypeKind kind, const char* name) const +{ + return (kind_ == kind) && !std::strncmp(full_name_, name, MaxFullNameLen); +} + +bool DataTypeDescriptor::match(DataTypeKind kind, DataTypeID id) const +{ + return (kind_ == kind) && (id_ == id); +} + +#if UAVCAN_TOSTRING +std::string DataTypeDescriptor::toString() const +{ + char kindch = '?'; + switch (kind_) + { + case DataTypeKindMessage: + { + kindch = 'm'; + break; + } + case DataTypeKindService: + { + kindch = 's'; + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + + char buf[128]; + (void)snprintf(buf, sizeof(buf), "%s:%u%c:%016llx", + full_name_, static_cast(id_.get()), kindch, + static_cast(signature_.get())); + return std::string(buf); +} +#endif + +bool DataTypeDescriptor::operator==(const DataTypeDescriptor& rhs) const +{ + return + (kind_ == rhs.kind_) && + (id_ == rhs.id_) && + (signature_ == rhs.signature_) && + !std::strncmp(full_name_, rhs.full_name_, MaxFullNameLen); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp new file mode 100644 index 0000000000..e806d5de9e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ +/* + * LimitedPoolAllocator + */ +void* LimitedPoolAllocator::allocate(std::size_t size) +{ + if (used_blocks_ < max_blocks_) + { + used_blocks_++; + return allocator_.allocate(size); + } + else + { + return UAVCAN_NULLPTR; + } +} + +void LimitedPoolAllocator::deallocate(const void* ptr) +{ + allocator_.deallocate(ptr); + + UAVCAN_ASSERT(used_blocks_ > 0); + if (used_blocks_ > 0) + { + used_blocks_--; + } +} + +uint16_t LimitedPoolAllocator::getBlockCapacity() const +{ + return min(max_blocks_, allocator_.getBlockCapacity()); +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_error.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_error.cpp new file mode 100644 index 0000000000..59b1aad88b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_error.cpp @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +#ifndef UAVCAN_EXCEPTIONS +# error UAVCAN_EXCEPTIONS +#endif + +#if UAVCAN_EXCEPTIONS +# include +#endif + +namespace uavcan +{ + +void handleFatalError(const char* msg) +{ +#if UAVCAN_EXCEPTIONS + throw std::runtime_error(msg); +#else + (void)msg; + UAVCAN_ASSERT(0); + std::abort(); +#endif +} + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/clock.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/clock.hpp new file mode 100644 index 0000000000..623321bae7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/clock.hpp @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2014 + */ + +#pragma once + +#include +#include +#include +#include + +class SystemClockMock : public uavcan::ISystemClock +{ +public: + mutable uint64_t monotonic; + mutable uint64_t utc; + uavcan::UtcDuration last_adjustment; + uint64_t monotonic_auto_advance; + bool preserve_utc; + + SystemClockMock(uint64_t initial = 0) + : monotonic(initial) + , utc(initial) + , monotonic_auto_advance(0) + , preserve_utc(false) + { } + + void advance(uint64_t usec) const + { + monotonic += usec; + if (!preserve_utc) + { + utc += usec; + } + } + + virtual uavcan::MonotonicTime getMonotonic() const + { + assert(this); + const uint64_t res = monotonic; + advance(monotonic_auto_advance); + return uavcan::MonotonicTime::fromUSec(res); + } + + virtual uavcan::UtcTime getUtc() const + { + assert(this); + return uavcan::UtcTime::fromUSec(utc); + } + + virtual void adjustUtc(uavcan::UtcDuration adjustment) + { + assert(this); + const uint64_t prev_utc = utc; + utc = uint64_t(int64_t(utc) + adjustment.toUSec()); + last_adjustment = adjustment; + std::cout << "Clock adjustment " << prev_utc << " --> " << utc << std::endl; + } +}; + + +class SystemClockDriver : public uavcan::ISystemClock +{ +public: + uavcan::UtcDuration utc_adjustment; + + virtual uavcan::MonotonicTime getMonotonic() const + { + struct timespec ts; + const int ret = clock_gettime(CLOCK_MONOTONIC, &ts); + if (ret != 0) + { + assert(0); + return uavcan::MonotonicTime(); + } + return uavcan::MonotonicTime::fromUSec(uint64_t(int64_t(ts.tv_sec) * 1000000L + int64_t(ts.tv_nsec / 1000L))); + } + + virtual uavcan::UtcTime getUtc() const + { + struct timeval tv; + const int ret = gettimeofday(&tv, UAVCAN_NULLPTR); + if (ret != 0) + { + assert(0); + return uavcan::UtcTime(); + } + return uavcan::UtcTime::fromUSec(uint64_t(int64_t(tv.tv_sec) * 1000000L + tv.tv_usec)) + utc_adjustment; + } + + virtual void adjustUtc(uavcan::UtcDuration adjustment) + { + utc_adjustment += adjustment; + } +}; + +inline uavcan::MonotonicTime tsMono(uint64_t usec) { return uavcan::MonotonicTime::fromUSec(usec); } +inline uavcan::UtcTime tsUtc(uint64_t usec) { return uavcan::UtcTime::fromUSec(usec); } + +inline uavcan::MonotonicDuration durMono(int64_t usec) { return uavcan::MonotonicDuration::fromUSec(usec); } + +template +static bool areTimestampsClose(const T& a, const T& b, int64_t precision_usec = 100000) +{ + return (a - b).getAbs().toUSec() < precision_usec; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/data_type.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/data_type.cpp new file mode 100644 index 0000000000..a95bdbb32f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/data_type.cpp @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(DataTypeSignatureCRC, Correctness) +{ + uavcan::DataTypeSignatureCRC crc; + + ASSERT_EQ(0xFFFFFFFFFFFFFFFF ^ 0xFFFFFFFFFFFFFFFF, crc.get()); + + crc.add('1'); + crc.add('2'); + crc.add('3'); + crc.add(reinterpret_cast("456789"), 6); + + ASSERT_EQ(0x62EC59E3F1A4F00A, crc.get()); +} + + +TEST(DataTypeSignatureCRC, Extension) +{ + uavcan::DataTypeSignatureCRC crc1; + + crc1.add('1'); + crc1.add('2'); + crc1.add('3'); + + uavcan::DataTypeSignatureCRC crc2 = uavcan::DataTypeSignatureCRC::extend(crc1.get()); + + crc2.add(reinterpret_cast("456789"), 6); + + ASSERT_EQ(0x62EC59E3F1A4F00A, crc2.get()); +} + + +TEST(DataTypeSignature, Correctness) +{ + using uavcan::DataTypeSignature; + using uavcan::DataTypeSignatureCRC; + + DataTypeSignature signature; + ASSERT_EQ(0, signature.get()); + + /* + * First extension + */ + signature.extend(DataTypeSignature(0x123456789abcdef0)); + + DataTypeSignatureCRC crc; + crc.add(0xF0); + crc.add(0xDE); + crc.add(0xBC); + crc.add(0x9A); + crc.add(0x78); + crc.add(0x56); + crc.add(0x34); + crc.add(0x12); + for (int i = 0; i < 8; i++) + { + crc.add(0); + } + + ASSERT_EQ(crc.get(), signature.get()); + + const uint64_t old_signature = signature.get(); + + /* + * Second extension + */ + signature.extend(DataTypeSignature(0xfedcba9876543210)); + crc.add(0x10); + crc.add(0x32); + crc.add(0x54); + crc.add(0x76); + crc.add(0x98); + crc.add(0xba); + crc.add(0xdc); + crc.add(0xfe); + for (int i = 0; i < 64; i += 8) + { + crc.add((old_signature >> i) & 0xFF); + } + + ASSERT_EQ(crc.get(), signature.get()); + + /* + * Comparison + */ + ASSERT_TRUE(signature == DataTypeSignature(signature.get())); + ASSERT_FALSE(signature == DataTypeSignature()); + ASSERT_FALSE(signature != DataTypeSignature(signature.get())); + ASSERT_TRUE(signature != DataTypeSignature()); +} + + +TEST(DataTypeDescriptor, ToString) +{ + uavcan::DataTypeDescriptor desc; + ASSERT_EQ(":65535s:0000000000000000", desc.toString()); + + desc = uavcan::DataTypeDescriptor(uavcan::DataTypeKindMessage, 123, + uavcan::DataTypeSignature(0xdeadbeef1234), "Bar"); + ASSERT_EQ("Bar:123m:0000deadbeef1234", desc.toString()); + + // Max length - 80 chars + desc = uavcan::DataTypeDescriptor(uavcan::DataTypeKindMessage, 1023, uavcan::DataTypeSignature(0xdeadbeef12345678), + "sirius_cybernetics_corporation.marvin.model_a.LongDataTypeName123456789abcdefghi"); + ASSERT_EQ("sirius_cybernetics_corporation.marvin.model_a.LongDataTypeName123456789abcdefghi:1023m:deadbeef12345678", + desc.toString()); +} + + +TEST(DataTypeDescriptor, Match) +{ + uavcan::DataTypeDescriptor desc(uavcan::DataTypeKindMessage, 123, + uavcan::DataTypeSignature(0xdeadbeef1234), "namespace.TypeName"); + ASSERT_TRUE(desc.match(uavcan::DataTypeKindMessage, "namespace.TypeName")); + ASSERT_FALSE(desc.match(uavcan::DataTypeKindMessage, "boo")); + ASSERT_FALSE(desc.match(uavcan::DataTypeKindService, "namespace.TypeName")); +} + + +TEST(DataTypeID, Basic) +{ + uavcan::DataTypeID id; + + ASSERT_EQ(0xFFFF, id.get()); + ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindService)); + + id = 123; + uavcan::DataTypeID id2 = 255; + + ASSERT_EQ(123, id.get()); + ASSERT_EQ(255, id2.get()); + + ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindService)); + ASSERT_TRUE(id2.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_TRUE(id2.isValidForDataTypeKind(uavcan::DataTypeKindService)); + + ASSERT_TRUE(id < id2); + ASSERT_TRUE(id <= id2); + ASSERT_TRUE(id2 > id); + ASSERT_TRUE(id2 >= id); + ASSERT_TRUE(id != id2); + + id = id2; + ASSERT_FALSE(id < id2); + ASSERT_TRUE(id <= id2); + ASSERT_FALSE(id2 > id); + ASSERT_TRUE(id2 >= id); + ASSERT_TRUE(id == id2); + + id = 1024; + ASSERT_TRUE(id.isValidForDataTypeKind(uavcan::DataTypeKindMessage)); + ASSERT_FALSE(id.isValidForDataTypeKind(uavcan::DataTypeKindService)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_1.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_1.cpp new file mode 100644 index 0000000000..593b9ade9a --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_1.cpp @@ -0,0 +1,18 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +/* + * Objective of this test is to make sure that the generated types are being linked correcly. + * This test requests the address of some static const member variables to make sure that there + * will be no duplicated symbol linking errors. + */ +TEST(DsdlConst1, Timestamp) +{ + using uavcan::Timestamp; + + std::cout << &Timestamp::UNKNOWN << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_2.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_2.cpp new file mode 100644 index 0000000000..477db1fa75 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_const_2.cpp @@ -0,0 +1,14 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(DsdlConst2, Timestamp) +{ + using uavcan::Timestamp; + + std::cout << &Timestamp::UNKNOWN << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_test.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_test.cpp new file mode 100644 index 0000000000..fdaaac4859 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_test.cpp @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +TEST(Dsdl, EmptyServices) +{ + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + root_ns_b::ServiceWithEmptyRequest::Request req; + ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Request::encode(req, sc_wr)); + ASSERT_EQ("", bs_wr.toString()); + + root_ns_b::ServiceWithEmptyRequest::Response resp; + ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Response::encode(resp, sc_wr)); + ASSERT_EQ("", bs_wr.toString()); + + resp.covariance.push_back(-2); + resp.covariance.push_back(65504); + root_ns_b::ServiceWithEmptyRequest::Response::encode(resp, sc_wr); + ASSERT_EQ("00000000 11000000 11111111 01111011", bs_wr.toString()); + + resp.covariance.push_back(42); + resp.covariance[0] = 999; + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(1, root_ns_b::ServiceWithEmptyRequest::Response::decode(resp, sc_rd)); + + ASSERT_EQ(2, resp.covariance.size()); + ASSERT_EQ(-2, resp.covariance[0]); + ASSERT_EQ(65504, resp.covariance[1]); +} + + +TEST(Dsdl, Signature) +{ + ASSERT_EQ(0xe74617107a34aa9c, root_ns_a::EmptyService::getDataTypeSignature().get()); + ASSERT_STREQ("root_ns_a.EmptyService", root_ns_a::EmptyService::getDataTypeFullName()); + ASSERT_EQ(uavcan::DataTypeKindService, root_ns_a::EmptyService::DataTypeKind); + + ASSERT_EQ(0x99604d7066e0d713, root_ns_a::NestedMessage::getDataTypeSignature().get()); // Computed manually + ASSERT_STREQ("root_ns_a.NestedMessage", root_ns_a::NestedMessage::getDataTypeFullName()); + ASSERT_EQ(uavcan::DataTypeKindMessage, root_ns_a::NestedMessage::DataTypeKind); +} + + +TEST(Dsdl, Operators) +{ + { + root_ns_a::EmptyService::Request a, b; + ASSERT_TRUE(a == b); + ASSERT_FALSE(a != b); + } + { + root_ns_a::NestedMessage c, d; + ASSERT_TRUE(c == d); + ASSERT_FALSE(c != d); + + c.field = 1; + ASSERT_FALSE(c == d); + ASSERT_TRUE(c != d); + } +} + + +TEST(Dsdl, CloseComparison) +{ + root_ns_a::A first, second; + + ASSERT_TRUE(first == second); + + first.vector[1].vector[1] = std::numeric_limits::epsilon(); + ASSERT_TRUE(first.isClose(second)); // Still close + ASSERT_FALSE(first == second); // But not exactly + + first.vector[1].vector[1] = std::numeric_limits::epsilon(); + ASSERT_FALSE(first.isClose(second)); // Nope + ASSERT_FALSE(first == second); // Ditto +} + +/* + * This test assumes that it will be executed before other GDTR tests; otherwise it fails. + * TODO: Probably it needs to be called directly from main() + */ +//TEST(Dsdl, Registration) +//{ +// using uavcan::GlobalDataTypeRegistry; +// /* +// * Descriptors +// */ +// const uavcan::DataTypeDescriptor* desc = UAVCAN_NULLPTR; +// +// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "root_ns_a.EmptyMessage"); +// ASSERT_TRUE(desc); +// ASSERT_EQ(root_ns_a::EmptyMessage::DefaultDataTypeID, desc->getID()); +// ASSERT_EQ(root_ns_a::EmptyMessage::DataTypeKind, desc->getKind()); +// ASSERT_EQ(root_ns_a::EmptyMessage::getDataTypeSignature(), desc->getSignature()); +// ASSERT_STREQ(root_ns_a::EmptyMessage::getDataTypeFullName(), desc->getFullName()); +// +// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "root_ns_a.EmptyService"); +// ASSERT_TRUE(desc); +// ASSERT_EQ(root_ns_a::EmptyService::DefaultDataTypeID, desc->getID()); +// ASSERT_EQ(root_ns_a::EmptyService::DataTypeKind, desc->getKind()); +// ASSERT_EQ(root_ns_a::EmptyService::getDataTypeSignature(), desc->getSignature()); +// ASSERT_STREQ(root_ns_a::EmptyService::getDataTypeFullName(), desc->getFullName()); +// +// desc = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "root_ns_a.ReportBackSoldier"); +// ASSERT_TRUE(desc); +// ASSERT_EQ(root_ns_a::ReportBackSoldier::DefaultDataTypeID, desc->getID()); +// ASSERT_EQ(root_ns_a::ReportBackSoldier::DataTypeKind, desc->getKind()); +// ASSERT_EQ(root_ns_a::ReportBackSoldier::getDataTypeSignature(), desc->getSignature()); +// ASSERT_STREQ(root_ns_a::ReportBackSoldier::getDataTypeFullName(), desc->getFullName()); +// +// /* +// * Mask +// */ +// uavcan::DataTypeIDMask mask; +// +// GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindMessage, mask); +// ASSERT_TRUE(mask[8]); +// mask[8] = false; +// +// GlobalDataTypeRegistry::instance().getDataTypeIDMask(uavcan::DataTypeKindService, mask); +// ASSERT_TRUE(mask[1]); +// ASSERT_TRUE(mask[3]); +// mask[1] = mask[3] = false; +// +// /* +// * Reset +// */ +// GlobalDataTypeRegistry::instance().reset(); +// ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen()); +//} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp new file mode 100644 index 0000000000..079ab96e29 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/dsdl_uavcan_compilability.cpp @@ -0,0 +1,292 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +template +static bool validateYaml(const T& obj, const std::string& reference) +{ + uavcan::OStream::instance() << "Validating YAML:\n" << obj << "\n" << uavcan::OStream::endl; + + std::ostringstream os; + os << obj; + if (os.str() == reference) + { + return true; + } + else + { + std::cout << "INVALID YAML:\n" + << "EXPECTED:\n" + << "===\n" + << reference + << "\n===\n" + << "ACTUAL:\n" + << "\n===\n" + << os.str() + << "\n===\n" << std::endl; + return false; + } +} + +TEST(Dsdl, Streaming) +{ + EXPECT_TRUE(validateYaml(uavcan::protocol::GetNodeInfo::Response(), + "status: \n" + " uptime_sec: 0\n" + " health: 0\n" + " mode: 0\n" + " sub_mode: 0\n" + " vendor_specific_status_code: 0\n" + "software_version: \n" + " major: 0\n" + " minor: 0\n" + " optional_field_flags: 0\n" + " vcs_commit: 0\n" + " image_crc: 0\n" + "hardware_version: \n" + " major: 0\n" + " minor: 0\n" + " unique_id: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + " certificate_of_authenticity: \"\"\n" + "name: \"\"")); + + root_ns_a::Deep ps; + ps.a.resize(1); + EXPECT_TRUE(validateYaml(ps, + "c: 0\n" + "str: \"\"\n" + "a: \n" + " - \n" + " scalar: 0\n" + " vector: \n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + "b: \n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]\n" + " - \n" + " vector: [0, 0]\n" + " bools: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]")); +} + + +template +static bool encodeDecodeValidate(const T& obj, const std::string& reference_bit_string) +{ + uavcan::StaticTransferBuffer<256> buf; + + { + uavcan::BitStream bits(buf); + uavcan::ScalarCodec codec(bits); + /* + * Coding + */ + if (0 > T::encode(obj, codec)) + { + std::cout << "Failed to encode" << std::endl; + return false; + } + + /* + * Validating the encoded bitstream + */ + const std::string result = bits.toString(); + if (result != reference_bit_string) + { + std::cout << "ENCODED VALUE DOESN'T MATCH THE REFERENCE:\nEXPECTED:\n" + << reference_bit_string << "\nACTUAL:\n" + << result << std::endl; + return false; + } + } + + /* + * Decoding back and comparing + */ + uavcan::BitStream bits(buf); + uavcan::ScalarCodec codec(bits); + + T decoded; + + if (0 > T::decode(decoded, codec)) + { + std::cout << "Failed to decode" << std::endl; + return false; + } + + if (!decoded.isClose(obj)) + { + std::cout << "DECODED OBJECT DOESN'T MATCH THE REFERENCE:\nEXPECTED:\n" + << obj << "\nACTUAL:\n" + << decoded << std::endl; + return false; + } + return true; +} + + +TEST(Dsdl, Union) +{ + using root_ns_a::UnionTest; + using root_ns_a::NestedInUnion; + + ASSERT_EQ(3, UnionTest::MinBitLen); + ASSERT_EQ(16, UnionTest::MaxBitLen); + ASSERT_EQ(13, NestedInUnion::MinBitLen); + ASSERT_EQ(13, NestedInUnion::MaxBitLen); + + UnionTest s; + + EXPECT_TRUE(validateYaml(s, "z: ")); + encodeDecodeValidate(s, "00000000"); + + s.to() = 16; + EXPECT_TRUE(validateYaml(s, "a: 16")); + EXPECT_TRUE(encodeDecodeValidate(s, "00110000")); + + s.to() = 31; + EXPECT_TRUE(validateYaml(s, "b: 31")); + EXPECT_TRUE(encodeDecodeValidate(s, "01011111")); + + s.to() = 256; + EXPECT_TRUE(validateYaml(s, "c: 256")); + EXPECT_TRUE(encodeDecodeValidate(s, "01100000 00000001")); + + s.to().push_back(true); + s.to().push_back(false); + s.to().push_back(true); + s.to().push_back(true); + s.to().push_back(false); + s.to().push_back(false); + s.to().push_back(true); + s.to().push_back(true); + s.to().push_back(true); + ASSERT_EQ(9, s.to().size()); + EXPECT_TRUE(validateYaml(s, "d: [1, 0, 1, 1, 0, 0, 1, 1, 1]")); + EXPECT_TRUE(encodeDecodeValidate(s, "10010011 01100111")); + + s.to().array[0] = 0; + s.to().array[1] = 1; + s.to().array[2] = 2; + s.to().array[3] = 3; + EXPECT_TRUE(validateYaml(s, "e: \n array: [0, 1, 2, 3]")); + EXPECT_TRUE(encodeDecodeValidate(s, "10100000 11011000")); +} + + +TEST(Dsdl, UnionTagWidth) +{ + using root_ns_a::UnionTest4; + + ASSERT_EQ(2, UnionTest4::MinBitLen); + ASSERT_EQ(8, UnionTest4::MaxBitLen); + + UnionTest4 s; + + { + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, UnionTest4::encode(s, sc_wr)); + ASSERT_EQ("00000000", bs_wr.toString()); + } + + { + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + s.to() = 1U << 5U; // 32, 0b100000 + + ASSERT_EQ(1, UnionTest4::encode(s, sc_wr)); + ASSERT_EQ("10100000", bs_wr.toString()); + } +} + + +TEST(Dsdl, ParamGetSetRequestUnion) +{ + uavcan::protocol::param::GetSet::Request req; + + req.index = 8191; + req.name = "123"; // 49, 50, 51 // 00110001, 00110010, 00110011 + EXPECT_TRUE(encodeDecodeValidate(req, "11111111 11111000 00110001 00110010 00110011")); + + req.value.to() = "abc"; // 01100001, 01100010, 01100011 + EXPECT_TRUE(encodeDecodeValidate(req, + "11111111 11111100 " // Index, Union tag + "00000011 " // Array length + "01100001 01100010 01100011 " // Payload + "00110001 00110010 00110011")); // Name + + EXPECT_TRUE(validateYaml(req, + "index: 8191\n" + "value: \n" + " string_value: \"abc\"\n" + "name: \"123\"")); + + req.value.to() = 1; + EXPECT_TRUE(encodeDecodeValidate(req, + "11111111 11111001 " // Index, Union tag + "00000001 00000000 00000000 00000000 00000000 00000000 00000000 00000000 " // Payload + "00110001 00110010 00110011")); // Name +} + + +TEST(Dsdl, ParamGetSetResponseUnion) +{ + uavcan::protocol::param::GetSet::Response res; + + res.value.to() = "abc"; + res.default_value.to(); // Empty + res.name = "123"; + EXPECT_TRUE(encodeDecodeValidate(res, + "00000100 " // Value union tag + "00000011 " // Value array length + "01100001 01100010 01100011 " // Value array payload + "00000100 " // Default union tag + "00000000 " // Default array length + "00000000 " // Max value tag + "00000000 " // Min value tag + "00110001 00110010 00110011")); // Name + + res.value.to() = true; + res.default_value.to(); // False + res.name = "123"; + EXPECT_TRUE(encodeDecodeValidate(res, + "00000011 " // Value union tag + "00000001 " // Value + "00000011 " // Default union tag + "00000000 " // Default value + "00000000 " // Max value tag + "00000000 " // Min value tag + "00110001 00110010 00110011")); // Name +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan new file mode 100644 index 0000000000..ed97d539c0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/128.EmptyService.uavcan @@ -0,0 +1 @@ +--- diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan new file mode 100644 index 0000000000..ba13284492 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/129.ReportBackSoldier.uavcan @@ -0,0 +1,9 @@ +bool TRUE = true +bool NOT_TRUE = false * true +uint64 BIG = (1 << 64) - 1 +float64 FLOAT_CONSTANT = 3.14 +uint8[<128] string_request +--- +float64 FLOAT_CONSTANT = 1.79769313486231570815e+308 +root_ns_b.SuperIntelligentShadeOfBlue blue +uint8[<128] string_response diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan new file mode 100644 index 0000000000..01297a9041 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/20000.MavlinkMessage.uavcan @@ -0,0 +1,10 @@ +# +# This thing is only needed for testing +# + +uint8 seq +uint8 sysid +uint8 compid +uint8 msgid + +uint8[<256] payload diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan new file mode 100644 index 0000000000..8d1c8b69c3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/32768.EmptyMessage.uavcan @@ -0,0 +1 @@ + diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan new file mode 100644 index 0000000000..5bca3a34d2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan @@ -0,0 +1,3 @@ +uint8[<=64] string_request +--- +uint8[<=64] string_response diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/A.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/A.uavcan new file mode 100644 index 0000000000..9f69a474e8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/A.uavcan @@ -0,0 +1,8 @@ +# +# Nested type. +# Energy and capacity are expressed in watt hours (Joules are infeasible because of range limitations). +# Unknown values should be represented as NAN. +# + +float32 scalar +B[2] vector diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/B.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/B.uavcan new file mode 100644 index 0000000000..fd41dbd0c4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/B.uavcan @@ -0,0 +1,2 @@ +float64[2] vector +bool[16] bools diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan new file mode 100644 index 0000000000..425e9d26c8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Deep.uavcan @@ -0,0 +1,8 @@ +# +# Info for all battery packs of the primary power supply. +# + +bool c +uint8[<20] str +A[<2] a +B[2] b diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Empty.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/Empty.uavcan new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan new file mode 100644 index 0000000000..ca6785ed66 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedInUnion.uavcan @@ -0,0 +1,4 @@ +# This should be 13 bits long +void2 # 2 bits +uint2[4] array # 8 bits +void3 # 3 bits diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan new file mode 100644 index 0000000000..d4875f15f2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/NestedMessage.uavcan @@ -0,0 +1,5 @@ + +float32 VALUE = 2 +( 2 *2) +bool BOOLEAN = true + false +int2 field +EmptyMessage empty diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan new file mode 100644 index 0000000000..b143a2735f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest.uavcan @@ -0,0 +1,8 @@ +# Max bit len 16 bits +@union # 3 bits +Empty z +uint5 a +uint5 b # Conflicting with a +uint13 c +bool[<=9] d # 4 bit length field + 9 bit payload +NestedInUnion e diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest4.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest4.uavcan new file mode 100644 index 0000000000..05bfb5c98c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_a/UnionTest4.uavcan @@ -0,0 +1,6 @@ +# A union of four items; tag 2 bits wide, total length 1 byte +@union # 2 bits +Empty first # Tag value 0 +uint5 second # Tag value 1 +uint6 third # Tag value 2 +int2 fourth # Tag value 3 diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan new file mode 100644 index 0000000000..731fe88f85 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyRequest.uavcan @@ -0,0 +1,2 @@ +--- +float16[<=9] covariance diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan new file mode 100644 index 0000000000..875562aa2b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/ServiceWithEmptyResponse.uavcan @@ -0,0 +1,2 @@ +float16[<=9] covariance +--- diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan new file mode 100644 index 0000000000..2badbc9ee2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/SuperIntelligentShadeOfBlue.uavcan @@ -0,0 +1,2 @@ +float16[<32] array_f16 +root_ns_a.NestedMessage[3] nested_message diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/T.uavcan b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/T.uavcan new file mode 100644 index 0000000000..31a901612e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dsdl_test/root_ns_b/T.uavcan @@ -0,0 +1,2 @@ +bool T = true +bool F = false diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/dynamic_memory.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/dynamic_memory.cpp new file mode 100644 index 0000000000..2a4b17b9dc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/dynamic_memory.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +TEST(DynamicMemory, Basic) +{ + uavcan::PoolAllocator<128, 32> pool32; + EXPECT_EQ(4, pool32.getNumFreeBlocks()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + const void* ptr1 = pool32.allocate(16); + ASSERT_TRUE(ptr1); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + EXPECT_FALSE(pool32.allocate(120)); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + pool32.deallocate(ptr1); + EXPECT_EQ(0, pool32.getNumUsedBlocks()); + EXPECT_EQ(1, pool32.getPeakNumUsedBlocks()); +} + +TEST(DynamicMemory, OutOfMemory) +{ + uavcan::PoolAllocator<64, 32> pool32; + + EXPECT_EQ(2, pool32.getNumFreeBlocks()); + EXPECT_EQ(0, pool32.getNumUsedBlocks()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + + const void* ptr1 = pool32.allocate(32); + ASSERT_TRUE(ptr1); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + EXPECT_EQ(1, pool32.getPeakNumUsedBlocks()); + + const void* ptr2 = pool32.allocate(32); + ASSERT_TRUE(ptr2); + EXPECT_EQ(2, pool32.getNumUsedBlocks()); + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); + + ASSERT_FALSE(pool32.allocate(32)); // No free blocks left --> UAVCAN_NULLPTR + EXPECT_EQ(2, pool32.getNumUsedBlocks()); + EXPECT_EQ(0, pool32.getNumFreeBlocks()); + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); +} + +TEST(DynamicMemory, LimitedPoolAllocator) +{ + uavcan::PoolAllocator<128, 32> pool32; + uavcan::LimitedPoolAllocator lim(pool32, 2); + + EXPECT_EQ(2, lim.getBlockCapacity()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + + const void* ptr1 = lim.allocate(1); + const void* ptr2 = lim.allocate(1); + const void* ptr3 = lim.allocate(1); + + EXPECT_TRUE(ptr1); + EXPECT_TRUE(ptr2); + EXPECT_FALSE(ptr3); + + lim.deallocate(ptr2); + const void* ptr4 = lim.allocate(1); + lim.deallocate(ptr1); + const void* ptr5 = lim.allocate(1); + const void* ptr6 = lim.allocate(1); + + EXPECT_TRUE(ptr4); + EXPECT_TRUE(ptr5); + EXPECT_FALSE(ptr6); + + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/helpers/heap_based_pool_allocator.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/helpers/heap_based_pool_allocator.cpp new file mode 100644 index 0000000000..52dbb364b0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/helpers/heap_based_pool_allocator.cpp @@ -0,0 +1,193 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#ifdef __linux__ +#include + +#else +#include +#endif + +TEST(HeapBasedPoolAllocator, Basic) +{ +#ifdef __linux__ + std::cout << ">>> HEAP BEFORE:" << std::endl; + malloc_stats(); +#endif + + uavcan::HeapBasedPoolAllocator al(0xEEEE); + + ASSERT_EQ(0, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + + ASSERT_EQ(0xEEEE, al.getBlockCapacity()); + ASSERT_EQ(0xFFFF, al.getBlockCapacityHardLimit()); + + void* a = al.allocate(10); + void* b = al.allocate(10); + void* c = al.allocate(10); + void* d = al.allocate(10); + + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(4, al.getNumAllocatedBlocks()); + + al.deallocate(a); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(3, al.getNumAllocatedBlocks()); + + al.deallocate(b); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(2, al.getNumAllocatedBlocks()); + + al.deallocate(c); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(1, al.getNumAllocatedBlocks()); + + a = al.allocate(10); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(2, al.getNumAllocatedBlocks()); + ASSERT_EQ(c, a); + + al.deallocate(a); + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(1, al.getNumAllocatedBlocks()); + + al.shrink(); + ASSERT_EQ(1, al.getNumReservedBlocks()); + ASSERT_EQ(1, al.getNumAllocatedBlocks()); + + al.deallocate(d); + ASSERT_EQ(1, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + + al.shrink(); + ASSERT_EQ(0, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + +#ifdef __linux__ + std::cout << ">>> HEAP AFTER:" << std::endl; + malloc_stats(); +#endif +} + + +TEST(HeapBasedPoolAllocator, Limits) +{ + uavcan::HeapBasedPoolAllocator al(2); + + ASSERT_EQ(2, al.getBlockCapacity()); + ASSERT_EQ(4, al.getBlockCapacityHardLimit()); + + ASSERT_EQ(0, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); + + void* a = al.allocate(10); + void* b = al.allocate(10); + void* c = al.allocate(10); + void* d = al.allocate(10); + + ASSERT_TRUE(a); + ASSERT_TRUE(b); + ASSERT_TRUE(c); + ASSERT_TRUE(d); + + ASSERT_FALSE(al.allocate(10)); + + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(4, al.getNumAllocatedBlocks()); + + al.deallocate(a); + al.deallocate(b); + al.deallocate(c); + al.deallocate(d); + + ASSERT_EQ(4, al.getNumReservedBlocks()); + ASSERT_EQ(0, al.getNumAllocatedBlocks()); +} + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +#include +#include + +struct RaiiSynchronizer +{ + static std::mutex mutex; + std::lock_guard guard{mutex}; +}; + +std::mutex RaiiSynchronizer::mutex; + +TEST(HeapBasedPoolAllocator, Concurrency) +{ +#ifdef __linux__ + std::cout << ">>> HEAP BEFORE:" << std::endl; + malloc_stats(); + +#endif + uavcan::HeapBasedPoolAllocator al(1000); + + ASSERT_EQ(1000, al.getBlockCapacity()); + ASSERT_EQ(2000, al.getBlockCapacityHardLimit()); + + volatile bool terminate = false; + + /* + * Starting the testing threads + */ + std::thread threads[3]; + + for (auto& x : threads) + { + x = std::thread([&al, &terminate]() + { + while (!terminate) + { + auto a = al.allocate(1); + auto b = al.allocate(1); + auto c = al.allocate(1); + al.deallocate(al.allocate(1)); + al.deallocate(a); + al.deallocate(b); + al.deallocate(c); + } + }); + } + + /* + * Running the threads for some time, then terminating + */ + std::this_thread::sleep_for(std::chrono::seconds(1)); + + terminate = true; + std::cout << "Terminating workers..." << std::endl; + + for (auto& x : threads) + { + x.join(); + } + std::cout << "All workers joined" << std::endl; + + /* + * Now, there must not be any leaked memory, because the worker threads deallocate everything before completion. + */ + std::cout << "Allocated: " << al.getNumAllocatedBlocks() << std::endl; + std::cout << "Reserved: " << al.getNumReservedBlocks() << std::endl; + +#ifdef __linux__ + std::cout << ">>> HEAP BEFORE SHRINK:" << std::endl; + malloc_stats(); + +#endif + al.shrink(); + +#ifdef __linux__ + std::cout << ">>> HEAP AFTER SHRINK:" << std::endl; + malloc_stats(); +#endif +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/array.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/array.cpp new file mode 100644 index 0000000000..d8e4d6f83b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/array.cpp @@ -0,0 +1,1419 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wfloat-equal" +# pragma GCC diagnostic ignored "-Wdouble-promotion" +#endif + +#include +#include +#include + +using uavcan::Array; +using uavcan::ArrayModeDynamic; +using uavcan::ArrayModeStatic; +using uavcan::IntegerSpec; +using uavcan::FloatSpec; +using uavcan::SignednessSigned; +using uavcan::SignednessUnsigned; +using uavcan::CastModeSaturate; +using uavcan::CastModeTruncate; + +struct CustomType +{ + typedef uavcan::IntegerSpec<8, uavcan::SignednessSigned, uavcan::CastModeTruncate> A; + typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> B; + // Dynamic array of max len 5 --> 3 bits for len, 5 bits for data --> 1 byte max len + typedef uavcan::Array, + uavcan::ArrayModeDynamic, 5> C; + + enum { MinBitLen = A::MinBitLen + B::MinBitLen + C::MinBitLen }; + enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen + C::MaxBitLen }; + + typename uavcan::StorageType::Type a; + typename uavcan::StorageType::Type b; + typename uavcan::StorageType::Type c; + + CustomType() + : a() + , b() + , c() + { } + + bool operator==(const CustomType& rhs) const + { + return a == rhs.a && + uavcan::areFloatsExactlyEqual(b, rhs.b) && + c == rhs.c; + } + + static int encode(const CustomType& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::encode(obj.b, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = C::encode(obj.c, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } + + static int decode(CustomType& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::decode(obj.b, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = C::decode(obj.c, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } +}; + + +TEST(Array, Basic) +{ + typedef Array, ArrayModeStatic, 4> A1; + typedef Array, ArrayModeStatic, 2> A2; + typedef Array A3; + + A1 a1; + A2 a2; + A3 a3; + + ASSERT_EQ(1, A3::ValueType::C::RawValueType::BitLen); + + ASSERT_EQ(8 * 4, A1::MaxBitLen); + ASSERT_EQ(16 * 2, A2::MaxBitLen); + ASSERT_EQ((8 + 16 + 5 + 3) * 2, A3::MaxBitLen); + + /* + * Zero initialization check + */ + ASSERT_FALSE(a1.empty()); + for (A1::const_iterator it = a1.begin(); it != a1.end(); ++it) + { + ASSERT_EQ(0, *it); + } + + ASSERT_FALSE(a2.empty()); + for (A2::const_iterator it = a2.begin(); it != a2.end(); ++it) + { + ASSERT_EQ(0, *it); + } + + for (A3::const_iterator it = a3.begin(); it != a3.end(); ++it) + { + ASSERT_EQ(0, it->a); + ASSERT_EQ(0, it->b); + ASSERT_EQ(0, it->c.size()); + ASSERT_TRUE(it->c.empty()); + } + + /* + * Modification with known values; array lengths are hard coded. + */ + for (uint8_t i = 0; i < 4; i++) + { + a1.at(i) = int8_t(i); + } + for (uint8_t i = 0; i < 2; i++) + { + a2.at(i) = i; + } + for (uint8_t i = 0; i < 2; i++) + { + a3[i].a = int8_t(i); + a3[i].b = i; + for (uint8_t i2 = 0; i2 < 5; i2++) + { + a3[i].c.push_back(i2 & 1); + } + ASSERT_EQ(5, a3[i].c.size()); + ASSERT_FALSE(a3[i].c.empty()); + } + + /* + * Representation check + * Note that TAO in A3 is not possible because A3::C has less than one byte per item + */ + uavcan::StaticTransferBuffer<16> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, A1::encode(a1, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A2::encode(a2, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ(0, A3::encode(a3, sc_wr, uavcan::TailArrayOptEnabled)); // Out of buffer space + + static const std::string Reference = + "00000000 00000001 00000010 00000011 " // A1 (0, 1, 2, 3) + "00000000 00000000 00000000 00111100 " // A2 (0, 1) + "00000000 00000000 00000000 10101010 " // A3[0] (0, 0, bool[5]) + "00000001 00000000 00111100 10101010"; // A3[1] (1, 1, bool[5]) + + ASSERT_EQ(Reference, bs_wr.toString()); + + /* + * Read back + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + A1 a1_; + A2 a2_; + A3 a3_; + + ASSERT_EQ(1, A1::decode(a1_, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A2::decode(a2_, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ(a1_, a1); + ASSERT_EQ(a2_, a2); + ASSERT_EQ(a3_, a3); + + for (uint8_t i = 0; i < 4; i++) + { + ASSERT_EQ(a1[i], a1_[i]); + } + for (uint8_t i = 0; i < 2; i++) + { + ASSERT_EQ(a2[i], a2_[i]); + } + for (uint8_t i = 0; i < 2; i++) + { + ASSERT_EQ(a3[i].a, a3_[i].a); + ASSERT_EQ(a3[i].b, a3_[i].b); + ASSERT_EQ(a3[i].c, a3_[i].c); + } + + ASSERT_EQ(0, A3::decode(a3_, sc_rd, uavcan::TailArrayOptEnabled)); // Out of buffer space + + /* + * STL compatibility + */ + std::vector v1; + v1.push_back(0); + v1.push_back(1); + v1.push_back(2); + v1.push_back(3); + + ASSERT_TRUE(a1 == v1); + ASSERT_FALSE(a1 != v1); + ASSERT_TRUE(v1 == a1); + ASSERT_FALSE(v1 != a1); + ASSERT_FALSE(a1 < v1); + + v1[0] = 9000; + ASSERT_FALSE(a1 == v1); + ASSERT_TRUE(a1 != v1); + ASSERT_TRUE(a1 < v1); + + ASSERT_EQ(0, a1.front()); + ASSERT_EQ(3, a1.back()); + + // Boolean vector + std::vector v2; + v2.push_back(false); + v2.push_back(true); + v2.push_back(false); + v2.push_back(true); + v2.push_back(false); + + ASSERT_TRUE(a3[0].c == v2); + ASSERT_FALSE(a3[0].c == v1); + ASSERT_FALSE(a3[0].c != v2); + ASSERT_TRUE(a3[0].c != v1); + + v2[0] = true; + ASSERT_TRUE(a3[0].c != v2); + ASSERT_FALSE(a3[0].c == v2); +} + + +TEST(Array, Dynamic) +{ + typedef Array, ArrayModeDynamic, 5> A; + typedef Array, ArrayModeDynamic, 255> B; + + A a; + B b; + B b2; + + ASSERT_EQ(3 + 5, A::MaxBitLen); + ASSERT_EQ(8 + 255 * 8, B::MaxBitLen); + + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + ASSERT_TRUE(b2.empty()); + + { + uavcan::StaticTransferBuffer<16> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ("000" "00000 000" "00000", bs_wr.toString()); // Last array was optimized away completely + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled)); + + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + ASSERT_TRUE(b2.empty()); + } + + a.push_back(true); + a.push_back(false); + a.push_back(true); + a.push_back(false); + a.push_back(true); + + b.push_back(42); + b.push_back(-42); + + b2.push_back(123); + b2.push_back(72); + + { + uavcan::StaticTransferBuffer<16> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::encode(b2, sc_wr, uavcan::TailArrayOptEnabled)); // No length field + + // A B len B[0] B[1] B2[0] B2[1] + ASSERT_EQ("10110101 00000010 00101010 11010110 01111011 01001000", bs_wr.toString()); + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + a.clear(); + b.clear(); + b2.clear(); + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + ASSERT_TRUE(b2.empty()); + + ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b, sc_rd, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, B::decode(b2, sc_rd, uavcan::TailArrayOptEnabled)); + + ASSERT_EQ(5, a.size()); + ASSERT_EQ(2, b.size()); + ASSERT_EQ(2, b2.size()); + + ASSERT_TRUE(a[0]); + ASSERT_FALSE(a[1]); + ASSERT_TRUE(a[2]); + ASSERT_FALSE(a[3]); + ASSERT_TRUE(a[4]); + + ASSERT_EQ(42, b[0]); + ASSERT_EQ(-42, b[1]); + + ASSERT_EQ(123, b2[0]); + ASSERT_EQ(72, b2[1]); + } + + ASSERT_FALSE(a == b); + ASSERT_FALSE(b == a); + ASSERT_TRUE(a != b); + ASSERT_TRUE(b != a); + + a.resize(0); + b.resize(0); + ASSERT_TRUE(a.empty()); + ASSERT_TRUE(b.empty()); + + a.resize(5, true); + b.resize(255, 72); + ASSERT_EQ(5, a.size()); + ASSERT_EQ(255, b.size()); + + for (uint8_t i = 0; i < 5; i++) + { + ASSERT_TRUE(a[i]); + } + for (uint8_t i = 0; i < 255; i++) + { + ASSERT_EQ(72, b[i]); + } +} + + +template +struct CustomType2 +{ + typedef uavcan::FloatSpec<16, uavcan::CastModeSaturate> A; + + enum { MinBitLen = A::MinBitLen + B::MinBitLen }; + enum { MaxBitLen = A::MaxBitLen + B::MaxBitLen }; + + typename uavcan::StorageType::Type a; + typename uavcan::StorageType::Type b; + + CustomType2() + : a() + , b() + { } + + bool operator==(const CustomType2& rhs) const + { + return uavcan::areFloatsExactlyEqual(a, rhs.a) && + b == rhs.b; + } + + static int encode(const CustomType2& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::encode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::encode(obj.b, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } + + static int decode(CustomType2& obj, uavcan::ScalarCodec& codec, + uavcan::TailArrayOptimizationMode tao_mode = uavcan::TailArrayOptEnabled) + { + int res = 0; + res = A::decode(obj.a, codec, uavcan::TailArrayOptDisabled); + if (res <= 0) + { + return res; + } + res = B::decode(obj.b, codec, tao_mode); + if (res <= 0) + { + return res; + } + return 1; + } +}; + + +template +static std::string runEncodeDecode(const typename uavcan::StorageType::Type& value, + const uavcan::TailArrayOptimizationMode tao_mode) +{ + uavcan::StaticTransferBuffer<(T::MaxBitLen + 7) / 8> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + EXPECT_EQ(1, T::encode(value, sc_wr, tao_mode)); + + typename uavcan::StorageType::Type value2 = typename uavcan::StorageType::Type(); + // Decode multiple times to make sure that the decoded type is being correctly de-initialized + for (int i = 0; i < 3; i++) + { + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + EXPECT_EQ(1, T::decode(value2, sc_rd, tao_mode)); + EXPECT_EQ(value, value2); + } + return bs_wr.toString(); +} + + +TEST(Array, TailArrayOptimization) +{ + typedef Array, ArrayModeDynamic, 5> OneBitArray; + typedef Array, ArrayModeDynamic, 255> EightBitArray; + typedef CustomType2 > A; + typedef CustomType2 > B; + typedef CustomType2 C; + + A a; + B b; + C c; + + /* + * Empty + */ + // a LSB a MSB b len + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + // a LSB a MSB b len + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(b, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(b, uavcan::TailArrayOptDisabled)); + + // a LSB a MSB + ASSERT_EQ("00000000 00000000", runEncodeDecode(c, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000000", runEncodeDecode(c, uavcan::TailArrayOptDisabled)); + + /* + * A + */ + a.b.resize(2); + a.b[0].push_back(true); + a.b[0].push_back(false); + // a.b[1] remains empty + // a LSB a MSB b len b: len(2), 1, 0, len(0) + ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000 00000000 00000010 01010000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + /* + * B + */ + b.b.resize(3); + b.b[0].push_back(42); + b.b[0].push_back(72); + // b.b[1] remains empty + b.b[2].push_back(123); + b.b[2].push_back(99); + // a LSB a MSB b len b[0]len 42 72 b[1]len 123 99 (b[2] len optimized out) + ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 01111011 01100011", + runEncodeDecode(b, uavcan::TailArrayOptEnabled)); + // Same as above, but b[2] len is present v here v + ASSERT_EQ("00000000 00000000 00000011 00000010 00101010 01001000 00000000 00000010 01111011 01100011", + runEncodeDecode(b, uavcan::TailArrayOptDisabled)); + + /* + * C + */ + c.a = 1; + c.b.push_back(1); + c.b.push_back(2); + c.b.push_back(3); + // a LSB a MSB 1 2 3 + ASSERT_EQ("00000000 00111100 00000001 00000010 00000011", + runEncodeDecode(c, uavcan::TailArrayOptEnabled)); + // a LSB a MSB b len 1 2 3 + ASSERT_EQ("00000000 00111100 00000011 00000001 00000010 00000011", + runEncodeDecode(c, uavcan::TailArrayOptDisabled)); +} + + +TEST(Array, TailArrayOptimizationErrors) +{ + typedef Array, ArrayModeDynamic, 5> A; + + A a; + ASSERT_TRUE(a.empty()); + ASSERT_EQ("", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("00000000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + // Correct decode/encode + a.push_back(1); + a.push_back(126); + a.push_back(5); + ASSERT_FALSE(a.empty()); + ASSERT_EQ("00000001 01111110 00000101", runEncodeDecode(a, uavcan::TailArrayOptEnabled)); + ASSERT_EQ("01100000 00101111 11000000 10100000", runEncodeDecode(a, uavcan::TailArrayOptDisabled)); + + // Invalid decode - length field is out of range + uavcan::StaticTransferBuffer<7> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, sc_wr.encode<3>(uint8_t(6))); // Length - more than 5 items, error + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(42))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(72))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(126))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(1))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(2))); + ASSERT_EQ(1, sc_wr.encode<8>(uint8_t(3))); // Out of range - only 5 items allowed + + // 197 73 15 192 32 ... + ASSERT_EQ("11000101 01001001 00001111 11000000 00100000 01000000 01100000", bs_wr.toString()); + + { + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + A a2; + a2.push_back(56); // Garbage + ASSERT_EQ(1, a2.size()); + // Will fail - declared length is more than 5 items + ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptDisabled)); + // Must be cleared + ASSERT_TRUE(a2.empty()); + } + { + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + A a2; + a2.push_back(56); // Garbage + ASSERT_EQ(1, a2.size()); + // Will fail - no length field, but the stream is too long + ASSERT_GT(0, A::decode(a2, sc_rd, uavcan::TailArrayOptEnabled)); + // Will contain some garbage + ASSERT_EQ(5, a2.size()); + // Interpreted stream - see the values above + ASSERT_EQ(197, a2[0]); + ASSERT_EQ(73, a2[1]); + ASSERT_EQ(15, a2[2]); + ASSERT_EQ(192, a2[3]); + ASSERT_EQ(32, a2[4]); + } +} + + +TEST(Array, DynamicEncodeDecodeErrors) +{ + typedef CustomType2, + ArrayModeDynamic, 255>, + ArrayModeDynamic, 255> > A; + A a; + a.b.resize(2); + a.b[0].push_back(55); + a.b[0].push_back(66); + { + uavcan::StaticTransferBuffer<4> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptEnabled)); // Not enough buffer space + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); + } + { + uavcan::StaticTransferBuffer<4> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); // Not enough buffer space + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptDisabled)); + } +} + + +TEST(Array, StaticEncodeDecodeErrors) +{ + typedef CustomType2, + ArrayModeStatic, 2>, + ArrayModeStatic, 2> > A; + A a; + a.a = 1.0; + a.b[0][0] = 0x11; + a.b[0][1] = 0x22; + a.b[1][0] = 0x33; + a.b[1][1] = 0x44; + { // Just enough buffer space - 6 bytes + uavcan::StaticTransferBuffer<6> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(1, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + + ASSERT_EQ("00000000 00111100 00010001 00100010 00110011 01000100", bs_wr.toString()); + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(1, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); + } + { // Not enough space + uavcan::StaticTransferBuffer<5> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + ASSERT_EQ(0, A::encode(a, sc_wr, uavcan::TailArrayOptDisabled)); + + ASSERT_EQ("00000000 00111100 00010001 00100010 00110011", bs_wr.toString()); + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + ASSERT_EQ(0, A::decode(a, sc_rd, uavcan::TailArrayOptEnabled)); + } +} + + +TEST(Array, Copyability) +{ + typedef Array, ArrayModeDynamic, 5> OneBitArray; + typedef Array, ArrayModeDynamic, 255> EightBitArray; + typedef Array A; + typedef Array B; + typedef EightBitArray C; + + A a; + B b; + C c; + + A a2 = a; + B b2 = b; + C c2 = c; + + ASSERT_TRUE(a == a2); + ASSERT_TRUE(b == b2); + ASSERT_TRUE(c == c2); + + a.push_back(OneBitArray()); + b.push_back(EightBitArray()); + c.push_back(42); + + ASSERT_TRUE(a != a2); + ASSERT_TRUE(b != b2); + ASSERT_TRUE(c != c2); + + a2 = a; + b2 = b; + c2 = c; + + ASSERT_TRUE(a2 == a); + ASSERT_TRUE(b2 == b); + ASSERT_TRUE(c2 == c); +} + + +TEST(Array, Appending) +{ + typedef Array, ArrayModeDynamic, 2> A; + typedef Array, ArrayModeDynamic, 257> B; + A a; + B b; + + a.push_back(1); + a.push_back(2); + a += b; + ASSERT_EQ(2, a.size()); + ASSERT_EQ(1, a[0]); + ASSERT_EQ(2, a[1]); + + b += a; + ASSERT_TRUE(b == a); + b += a; + ASSERT_EQ(4, b.size()); + ASSERT_EQ(1, b[0]); + ASSERT_EQ(2, b[1]); + ASSERT_EQ(1, b[2]); + ASSERT_EQ(2, b[3]); +} + + +TEST(Array, Strings) +{ + typedef Array, ArrayModeDynamic, 32> A8; + typedef Array, ArrayModeDynamic, 32> A7; + + A8 a8; + A8 a8_2; + A7 a7; + + ASSERT_TRUE(a8 == a7); + // cppcheck-suppress duplicateExpression + ASSERT_TRUE(a8 == a8); + // cppcheck-suppress duplicateExpression + ASSERT_TRUE(a7 == a7); + ASSERT_TRUE(a8 == ""); + ASSERT_TRUE(a7 == ""); + + a8 = "Hello world!"; + a7 = "123"; + ASSERT_TRUE(a8 == "Hello world!"); + ASSERT_TRUE(a7 == "123"); + + a8 = "Our sun is dying."; + a7 = "456"; + ASSERT_TRUE("Our sun is dying." == a8); + ASSERT_TRUE("456" == a7); + + a8 += " 123456"; + a8 += "-789"; + ASSERT_TRUE("Our sun is dying. 123456-789" == a8); + + ASSERT_TRUE(a8_2 == ""); + ASSERT_TRUE(a8_2.empty()); + ASSERT_TRUE(a8_2 != a8); + a8_2 = a8; + ASSERT_TRUE(a8_2 == "Our sun is dying. 123456-789"); + ASSERT_TRUE(a8_2 == a8); + + /* + * c_str() + */ + ASSERT_STREQ("", A8().c_str()); + ASSERT_STREQ("", A7().c_str()); + ASSERT_STREQ("Our sun is dying. 123456-789", a8_2.c_str()); + ASSERT_STREQ("Our sun is dying. 123456-789", a8.c_str()); + ASSERT_STREQ("456", a7.c_str()); + + /* + * String constructor + */ + A8 a8_3("123"); + A7 a7_3 = "456"; + ASSERT_EQ(3, a8_3.size()); + ASSERT_EQ(3, a7_3.size()); + ASSERT_STREQ("123", a8_3.c_str()); + ASSERT_STREQ("456", a7_3.c_str()); +} + + +TEST(Array, AppendFormatted) +{ + typedef Array, ArrayModeDynamic, 45> A8; + + A8 a; + + ASSERT_TRUE("" == a); + + a.appendFormatted("%4.1f", 12.3); // 4 + a += " "; // 1 + a.appendFormatted("%li", -123456789L); // 10 + a.appendFormatted("%s", " TOTAL PERSPECTIVE VORTEX "); // 26 + a.appendFormatted("0x%X", 0xDEADBEEF); // 10 --> 4 + + ASSERT_STREQ("12.3 -123456789 TOTAL PERSPECTIVE VORTEX 0xDE", a.c_str()); +} + + +TEST(Array, FlatStreaming) +{ + typedef Array, ArrayModeDynamic, 32> A8D; + typedef Array, ArrayModeDynamic, 16> AF16D; + typedef Array, ArrayModeStatic, 3> AF16S; + + A8D a1; + a1 = "12\n3\x44\xa5\xde\xad\x79"; + uavcan::YamlStreamer::stream(std::cout, a1, 0); + std::cout << std::endl; + + A8D a2; + a2 = "Hello"; + uavcan::YamlStreamer::stream(std::cout, a2, 0); + std::cout << std::endl; + + AF16D af16d1; + af16d1.push_back(1.23F); + af16d1.push_back(4.56F); + uavcan::YamlStreamer::stream(std::cout, af16d1, 0); + std::cout << std::endl; + + AF16D af16d2; + uavcan::YamlStreamer::stream(std::cout, af16d2, 0); + std::cout << std::endl; + + AF16S af16s; + uavcan::YamlStreamer::stream(std::cout, af16s, 0); + std::cout << std::endl; +} + + +TEST(Array, MultidimensionalStreaming) +{ + typedef Array, ArrayModeDynamic, 16> Float16Array; + typedef Array TwoDimensional; + typedef Array ThreeDimensional; + + ThreeDimensional threedee; + threedee.resize(3); + for (uint8_t x = 0; x < threedee.size(); x++) + { + threedee[x].resize(3); + for (uint8_t y = 0; y < threedee[x].size(); y++) + { + threedee[x][y].resize(3); + for (uint8_t z = 0; z < threedee[x][y].size(); z++) + { + threedee[x][y][z] = 1.0F / (float(x + y + z) + 1.0F); + } + } + } + + uavcan::YamlStreamer::stream(std::cout, threedee, 0); + std::cout << std::endl; +} + + +TEST(Array, SquareMatrixPacking) +{ + Array, ArrayModeDynamic, 9> m3x3s; + Array, ArrayModeDynamic, 4> m2x2f; + Array, ArrayModeDynamic, 36> m6x6d; + + // NAN will be reduced to empty array + { + const double nans3x3[] = + { + NAN, NAN, NAN, + NAN, NAN, NAN, + NAN, NAN, NAN + }; + m3x3s.packSquareMatrix(nans3x3); + ASSERT_EQ(0, m3x3s.size()); + + // Empty array will be decoded as zero matrix + double nans3x3_out[9]; + m3x3s.unpackSquareMatrix(nans3x3_out); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_DOUBLE_EQ(0, nans3x3_out[i]); + } + } + { + std::vector empty; + m3x3s.packSquareMatrix(empty); + ASSERT_EQ(0, m3x3s.size()); + + empty.resize(9); + m3x3s.unpackSquareMatrix(empty); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_DOUBLE_EQ(0, empty.at(i)); + } + } + + // Scalar matrix will be reduced to a single value + { + std::vector scalar2x2(4); + scalar2x2[0] = scalar2x2[3] = 3.14F; + m2x2f.packSquareMatrix(scalar2x2); + ASSERT_EQ(1, m2x2f.size()); + ASSERT_FLOAT_EQ(3.14F, m2x2f[0]); + + m2x2f.unpackSquareMatrix(scalar2x2); + const float reference[] = + { + 3.14F, 0.0F, + 0.0F, 3.14F + }; + ASSERT_TRUE(std::equal(scalar2x2.begin(), scalar2x2.end(), reference)); + } + { + const float scalar6x6[] = + { + -18, 0, 0, 0, 0, 0, + 0, -18, 0, 0, 0, 0, + 0, 0, -18, 0, 0, 0, + 0, 0, 0, -18, 0, 0, + 0, 0, 0, 0, -18, 0, + 0, 0, 0, 0, 0, -18 + }; + m6x6d.packSquareMatrix(scalar6x6); + ASSERT_EQ(1, m6x6d.size()); + ASSERT_DOUBLE_EQ(-18, m6x6d[0]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar6x6)); + } + + // Diagonal matrix will be reduced to an array of length Width + { + const float diagonal6x6[] = + { + 1, 0, 0, 0, 0, 0, + 0, -2, 0, 0, 0, 0, + 0, 0, 3, 0, 0, 0, + 0, 0, 0, -4, 0, 0, + 0, 0, 0, 0, 5, 0, + 0, 0, 0, 0, 0, -6 + }; + m6x6d.packSquareMatrix(diagonal6x6); + ASSERT_EQ(6, m6x6d.size()); + ASSERT_DOUBLE_EQ(1, m6x6d[0]); + ASSERT_DOUBLE_EQ(-2, m6x6d[1]); + ASSERT_DOUBLE_EQ(3, m6x6d[2]); + ASSERT_DOUBLE_EQ(-4, m6x6d[3]); + ASSERT_DOUBLE_EQ(5, m6x6d[4]); + ASSERT_DOUBLE_EQ(-6, m6x6d[5]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6)); + } + + // A matrix filled with random values will not be compressed + { + std::vector full3x3(9); + for (uint8_t i = 0; i < 9; i++) + { + full3x3[i] = float(i); + } + m3x3s.packSquareMatrix(full3x3); + ASSERT_EQ(9, m3x3s.size()); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(float(i), m3x3s[i]); + } + + long output[9]; + m3x3s.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(full3x3.begin(), full3x3.end(), output)); + } + + // This will be represented as diagonal - NANs are exceptional + { + const double scalarnan3x3[] = + { + NAN, 0, 0, + 0, NAN, 0, + 0, 0, NAN + }; + m3x3s.packSquareMatrix(scalarnan3x3); + ASSERT_EQ(3, m3x3s.size()); + ASSERT_FALSE(m3x3s[0] <= m3x3s[0]); // NAN + ASSERT_FALSE(m3x3s[1] <= m3x3s[1]); // NAN + ASSERT_FALSE(m3x3s[2] <= m3x3s[2]); // NAN + + float output[9]; + m3x3s.unpackSquareMatrix(output); + ASSERT_FALSE(output[0] <= output[0]); // NAN + ASSERT_EQ(0, output[1]); + ASSERT_EQ(0, output[2]); + ASSERT_EQ(0, output[3]); + ASSERT_FALSE(output[4] <= output[4]); // NAN + ASSERT_EQ(0, output[5]); + ASSERT_EQ(0, output[6]); + ASSERT_EQ(0, output[7]); + ASSERT_FALSE(output[8] <= output[8]); // NAN + } + + // This is a full matrix too (notice the NAN) + { + const float full2x2[] = + { + 1, NAN, + 0, -2 + }; + m2x2f.packSquareMatrix(full2x2); + ASSERT_EQ(4, m2x2f.size()); + ASSERT_FLOAT_EQ(1, m2x2f[0]); + ASSERT_FALSE(m2x2f[1] <= m2x2f[1]); // NAN + ASSERT_FLOAT_EQ(0, m2x2f[2]); + ASSERT_FLOAT_EQ(-2, m2x2f[3]); + + float output[4]; + m2x2f.unpackSquareMatrix(output); + ASSERT_EQ(1, output[0]); + ASSERT_FALSE(output[1] <= output[1]); // NAN + ASSERT_EQ(0, output[2]); + ASSERT_EQ(-2, output[3]); + } + + // Zero matrix will be represented as scalar matrix + { + const float zero2x2[] = + { + 0, 0, + 0, 0 + }; + m2x2f.packSquareMatrix(zero2x2); + ASSERT_EQ(1, m2x2f.size()); + ASSERT_FLOAT_EQ(0, m2x2f[0]); + } + + // Symmetric matrix will contain only upper-right triangle + { + const float sym2x2[] = + { + 1, 2, + 2, 1 + }; + m2x2f.packSquareMatrix(sym2x2); + ASSERT_EQ(3, m2x2f.size()); + + float sym2x2_out[4]; + m2x2f.unpackSquareMatrix(sym2x2_out); + ASSERT_FLOAT_EQ(1, sym2x2_out[0]); + ASSERT_FLOAT_EQ(2, sym2x2_out[1]); + ASSERT_FLOAT_EQ(2, sym2x2_out[2]); + ASSERT_FLOAT_EQ(1, sym2x2_out[3]); + } + { + const float sym3x3[] = + { + 1, 2, 3, + 2, 4, 5, + 3, 5, 6 + }; + m3x3s.packSquareMatrix(sym3x3); + ASSERT_EQ(6, m3x3s.size()); + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(4, m3x3s[3]); + ASSERT_EQ(5, m3x3s[4]); + ASSERT_EQ(6, m3x3s[5]); + + float sym3x3_out[9]; + m3x3s.unpackSquareMatrix(sym3x3_out); + + for (int i = 0; i < 9; i++) + { + ASSERT_FLOAT_EQ(sym3x3[i], sym3x3_out[i]); + } + } + { + const double sym6x6[] = + { + 1, 2, 3, 4, 5, 6, + 2, 7, 8, 9, 10, 11, + 3, 8, 12, 13, 14, 15, + 4, 9, 13, 16, 17, 18, + 5, 10, 14, 17, 19, 20, + 6, 11, 15, 18, 20, 21 + }; + m6x6d.packSquareMatrix(sym6x6); + ASSERT_EQ(21, m6x6d.size()); + for (uavcan::uint8_t i = 0; i < 21; i++) + { + ASSERT_DOUBLE_EQ(double(i + 1), m6x6d[i]); + } + + double sym6x6_out[36]; + m6x6d.unpackSquareMatrix(sym6x6_out); + + for (int i = 0; i < 36; i++) + { + ASSERT_DOUBLE_EQ(sym6x6[i], sym6x6_out[i]); + } + } +} + + +TEST(Array, FuzzySquareMatrixPacking) +{ + Array, ArrayModeDynamic, 36> m6x6d; + + // Diagonal matrix will be reduced to an array of length Width + { + float diagonal6x6[] = + { + 1, 0, 0, 0, 0, 0, + 0, -2, 0, 0, 0, 0, + 0, 0, 3, 0, 0, 0, + 0, 0, 0, -4, 0, 0, + 0, 0, 0, 0, 5, 0, + 0, 0, 0, 0, 0, -6 + }; + + // Some almost-zeroes + diagonal6x6[1] = std::numeric_limits::epsilon(); + diagonal6x6[4] = -std::numeric_limits::epsilon(); + diagonal6x6[34] = -std::numeric_limits::epsilon(); + + m6x6d.packSquareMatrix(diagonal6x6); + ASSERT_EQ(6, m6x6d.size()); + ASSERT_DOUBLE_EQ(1, m6x6d[0]); + ASSERT_DOUBLE_EQ(-2, m6x6d[1]); + ASSERT_DOUBLE_EQ(3, m6x6d[2]); + ASSERT_DOUBLE_EQ(-4, m6x6d[3]); + ASSERT_DOUBLE_EQ(5, m6x6d[4]); + ASSERT_DOUBLE_EQ(-6, m6x6d[5]); + + std::vector output(36); + m6x6d.unpackSquareMatrix(output); + + // This comparison will fail because epsilons + ASSERT_FALSE(std::equal(output.begin(), output.end(), diagonal6x6)); + + // This comparison will be ok + ASSERT_TRUE(std::equal(output.begin(), output.end(), diagonal6x6, &uavcan::areClose)); + } +} + + +TEST(Array, SquareMatrixPackingIntegers) +{ + Array, ArrayModeDynamic, 9> m3x3int; + { + const long scalar[] = + { + 42, 0, 0, + 0, 42, 0, + 0, 0, 42 + }; + m3x3int.packSquareMatrix(scalar); + ASSERT_EQ(1, m3x3int.size()); + ASSERT_EQ(42, m3x3int[0]); + + std::vector output(9); + m3x3int.unpackSquareMatrix(output); + ASSERT_TRUE(std::equal(output.begin(), output.end(), scalar)); + } + { + std::vector diagonal(9); + diagonal[0] = 6; + diagonal[4] = -57; + diagonal[8] = 1139; + m3x3int.packSquareMatrix(diagonal); + ASSERT_EQ(3, m3x3int.size()); + ASSERT_EQ(6, m3x3int[0]); + ASSERT_EQ(-57, m3x3int[1]); + ASSERT_EQ(1139, m3x3int[2]); + } + { + std::vector full(9); + for (uint8_t i = 0; i < 9; i++) + { + full[i] = i; + } + m3x3int.packSquareMatrix(full); + ASSERT_EQ(9, m3x3int.size()); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_EQ(i, m3x3int[i]); + } + } +} + +#if UAVCAN_EXCEPTIONS +TEST(Array, SquareMatrixPackingErrors) +{ + Array, ArrayModeDynamic, 9> m3x3s; + + std::vector ill_formed_row_major(8); + ASSERT_THROW(m3x3s.packSquareMatrix(ill_formed_row_major), std::out_of_range); + + ASSERT_THROW(m3x3s.unpackSquareMatrix(ill_formed_row_major), std::out_of_range); +} +#endif + +TEST(Array, SquareMatrixPackingInPlace) +{ + Array, ArrayModeDynamic, 9> m3x3s; + + // Will do nothing - matrix is empty + m3x3s.packSquareMatrix(); + ASSERT_TRUE(m3x3s.empty()); + + // Will fill with zeros - matrix is empty + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + for (uint8_t i = 0; i < 9; i++) + { + ASSERT_EQ(0, m3x3s[i]); + } + + // Fill an unpackaple matrix + m3x3s.clear(); + m3x3s.push_back(11); + m3x3s.push_back(12); + m3x3s.push_back(13); + +#if UAVCAN_EXCEPTIONS + // Shall throw - matrix is ill-formed + ASSERT_THROW(m3x3s.packSquareMatrix(), std::out_of_range); +#endif + + m3x3s.push_back(21); + m3x3s.push_back(22); + m3x3s.push_back(23); + m3x3s.push_back(31); + m3x3s.push_back(32); + m3x3s.push_back(33); + + // Will pack/unpack successfully + ASSERT_EQ(9, m3x3s.size()); + m3x3s.packSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + + // Make sure it was unpacked properly + ASSERT_EQ(11, m3x3s[0]); + ASSERT_EQ(12, m3x3s[1]); + ASSERT_EQ(13, m3x3s[2]); + ASSERT_EQ(21, m3x3s[3]); + ASSERT_EQ(22, m3x3s[4]); + ASSERT_EQ(23, m3x3s[5]); + ASSERT_EQ(31, m3x3s[6]); + ASSERT_EQ(32, m3x3s[7]); + ASSERT_EQ(33, m3x3s[8]); + + // Try again with a scalar matrix + m3x3s.clear(); + for (unsigned i = 0; i < 9; i++) + { + const bool diagonal = (i == 0) || (i == 4) || (i == 8); + m3x3s.push_back(diagonal ? 123 : 0); + } + + ASSERT_EQ(9, m3x3s.size()); + m3x3s.packSquareMatrix(); + ASSERT_EQ(1, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + + for (uint8_t i = 0; i < 9; i++) + { + const bool diagonal = (i == 0) || (i == 4) || (i == 8); + ASSERT_EQ((diagonal ? 123 : 0), m3x3s[i]); + } + + // Try again with symmetric matrix + /* + * Full matrix: + * 1 2 3 + * 2 4 5 + * 3 5 6 + * Compressed triangle: + * 1 2 3 + * 4 5 + * 6 + */ + m3x3s.clear(); + m3x3s.push_back(1); + m3x3s.push_back(2); + m3x3s.push_back(3); + m3x3s.push_back(4); + m3x3s.push_back(5); + m3x3s.push_back(6); + // Unpacking + ASSERT_EQ(6, m3x3s.size()); + m3x3s.unpackSquareMatrix(); + ASSERT_EQ(9, m3x3s.size()); + // Validating + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(2, m3x3s[3]); + ASSERT_EQ(4, m3x3s[4]); + ASSERT_EQ(5, m3x3s[5]); + ASSERT_EQ(3, m3x3s[6]); + ASSERT_EQ(5, m3x3s[7]); + ASSERT_EQ(6, m3x3s[8]); + // Packing back + m3x3s.packSquareMatrix(); + ASSERT_EQ(6, m3x3s.size()); + // Validating + ASSERT_EQ(1, m3x3s[0]); + ASSERT_EQ(2, m3x3s[1]); + ASSERT_EQ(3, m3x3s[2]); + ASSERT_EQ(4, m3x3s[3]); + ASSERT_EQ(5, m3x3s[4]); + ASSERT_EQ(6, m3x3s[5]); +} + +TEST(Array, FuzzyComparison) +{ + typedef Array, ArrayModeStatic, 2>, + ArrayModeStatic, 2>, + ArrayModeStatic, 2> ArrayStatic32; + + typedef Array, ArrayModeDynamic, 2>, + ArrayModeDynamic, 2>, + ArrayModeDynamic, 2> ArrayDynamic64; + + ArrayStatic32 array_s32; + + ArrayDynamic64 array_d64; + + array_d64.resize(2); + array_d64[0].resize(2); + array_d64[1].resize(2); + array_d64[0][0].resize(2); + array_d64[0][1].resize(2); + array_d64[1][0].resize(2); + array_d64[1][1].resize(2); + + std::cout << "One:"; + uavcan::YamlStreamer::stream(std::cout, array_s32, 0); + std::cout << std::endl << "------"; + uavcan::YamlStreamer::stream(std::cout, array_d64, 0); + std::cout << std::endl; + + // Both are equal right now + ASSERT_TRUE(array_d64 == array_s32); + ASSERT_TRUE(array_d64.isClose(array_s32)); + ASSERT_TRUE(array_s32.isClose(array_d64)); + + // Slightly modifying - still close enough + array_s32[0][0][0] = 123.456F + uavcan::NumericTraits::epsilon() * 123.0F; + array_s32[0][0][1] = uavcan::NumericTraits::infinity(); + array_s32[0][1][0] = uavcan::NumericTraits::epsilon(); + array_s32[0][1][1] = -uavcan::NumericTraits::epsilon(); + + array_d64[0][0][0] = 123.456; + array_d64[0][0][1] = uavcan::NumericTraits::infinity(); + array_d64[0][1][0] = -uavcan::NumericTraits::epsilon(); // Note that the sign is inverted + array_d64[0][1][1] = uavcan::NumericTraits::epsilon(); + + std::cout << "Two:"; + uavcan::YamlStreamer::stream(std::cout, array_s32, 0); + std::cout << std::endl << "------"; + uavcan::YamlStreamer::stream(std::cout, array_d64, 0); + std::cout << std::endl; + + // They are close bot not exactly equal + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_TRUE(array_d64.isClose(array_s32)); + ASSERT_TRUE(array_s32.isClose(array_d64)); + + // Not close + array_d64[0][0][0] = 123.457; + + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_FALSE(array_d64.isClose(array_s32)); + ASSERT_FALSE(array_s32.isClose(array_d64)); + + // Values are close, but lengths differ + array_d64[0][0][0] = 123.456; + + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_TRUE(array_d64.isClose(array_s32)); + ASSERT_TRUE(array_s32.isClose(array_d64)); + + array_d64[0][0].resize(1); + + ASSERT_FALSE(array_d64 == array_s32); + ASSERT_FALSE(array_d64.isClose(array_s32)); + ASSERT_FALSE(array_s32.isClose(array_d64)); + + std::cout << "Three:"; + uavcan::YamlStreamer::stream(std::cout, array_s32, 0); + std::cout << std::endl << "------"; + uavcan::YamlStreamer::stream(std::cout, array_d64, 0); + std::cout << std::endl; +} + +TEST(Array, CaseConversion) +{ + Array, ArrayModeDynamic, 30> str; + + str.convertToLowerCaseASCII(); + str.convertToUpperCaseASCII(); + + ASSERT_STREQ("", str.c_str()); + + str = "Hello World!"; + + ASSERT_STREQ("Hello World!", str.c_str()); + str.convertToLowerCaseASCII(); + ASSERT_STREQ("hello world!", str.c_str()); + str.convertToUpperCaseASCII(); + ASSERT_STREQ("HELLO WORLD!", str.c_str()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/bit_stream.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/bit_stream.cpp new file mode 100644 index 0000000000..85ad2010fc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/bit_stream.cpp @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(BitStream, ToString) +{ + { + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ("", bs.toString()); + ASSERT_EQ("", bs.toString()); + } + { + const uint8_t data[] = {0xad}; // 10101101 + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 8)); // all 8 + ASSERT_EQ("10101101", bs.toString()); + } + { + const uint8_t data[] = {0xad, 0xbe}; // 10101101 10111110 + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 16)); // all 16 + ASSERT_EQ("10101101 10111110", bs.toString()); + } + { + const uint8_t data[] = {0xad, 0xbe, 0xfc}; // 10101101 10111110 11111100 + uavcan::StaticTransferBuffer<8> buf; + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 20)); // 10101101 10111110 1111 + ASSERT_EQ("10101101 10111110 11110000", bs.toString()); + } +} + + +TEST(BitStream, BitOrderSimple) +{ + /* + * a = 1010 + * b = 1011 + * c = 1100 + * d = 1101 + * e = 1110 + * f = 1111 + */ + uavcan::StaticTransferBuffer<32> buf; + { // Write + const uint8_t data[] = {0xad, 0xbe}; // adbe + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data, 12)); // adb0 + ASSERT_EQ("10101101 10110000", bs.toString()); // adb0 + } + { // Read + uavcan::BitStream bs(buf); + ASSERT_EQ("10101101 10110000", bs.toString()); // Same data + uint8_t data[] = {0xFF, 0xFF}; // Uninitialized + ASSERT_EQ(1, bs.read(data, 12)); + ASSERT_EQ(0xad, data[0]); + ASSERT_EQ(0xb0, data[1]); + } +} + + +TEST(BitStream, BitOrderComplex) +{ + static const std::string REFERENCE = + "10101101 10111111 11101111 01010110 11011111 01000100 10001101 00010101 10011110 00100110 10101111 00110111 10111100 00000100"; + + uavcan::StaticTransferBuffer<32> buf; + { // Write + const uint8_t data1[] = {0xad, 0xbe}; // 10101101 10111110 + const uint8_t data2[] = {0xfc}; // 11111100 + const uint8_t data3[] = {0xde, 0xad, 0xbe, 0xef}; // 11011110 10101101 10111110 11101111 + const uint8_t data4[] = {0x12, 0x34, 0x56, 0x78, // 00010010 00110100 01010110 01111000 + 0x9a, 0xbc, 0xde, 0xf0}; // 10011010 10111100 11011110 11110000 + + uavcan::BitStream bs(buf); + ASSERT_EQ(1, bs.write(data1, 11)); // 10101101 101 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data2, 6)); // 11111 1 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data3, 25)); // 1101111 01010110 11011111 01 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data4, 64)); // all 64, total 42 + 64 = 106 + std::cout << bs.toString() << std::endl; + ASSERT_EQ(1, bs.write(data4, 4)); // 0001 + std::cout << bs.toString() << std::endl; + + std::cout << "Reference:\n" << REFERENCE << std::endl; + + ASSERT_EQ(REFERENCE, bs.toString()); + } + { // Read back in the same order + uint8_t data[8]; + std::fill(data, data + sizeof(data), 0xA5); // Filling with garbage + uavcan::BitStream bs(buf); + ASSERT_EQ(REFERENCE, bs.toString()); + + ASSERT_EQ(1, bs.read(data, 11)); // 10101101 10100000 + ASSERT_EQ(0xad, data[0]); + ASSERT_EQ(0xa0, data[1]); + + ASSERT_EQ(1, bs.read(data, 6)); // 11111100 + ASSERT_EQ(0xfc, data[0]); + + ASSERT_EQ(1, bs.read(data, 25)); // 11011110 10101101 10111110 10000000 + ASSERT_EQ(0xde, data[0]); + ASSERT_EQ(0xad, data[1]); + ASSERT_EQ(0xbe, data[2]); + ASSERT_EQ(0x80, data[3]); + + ASSERT_EQ(1, bs.read(data, 64)); // Data - see above + ASSERT_EQ(0x12, data[0]); + ASSERT_EQ(0x34, data[1]); + ASSERT_EQ(0x56, data[2]); + ASSERT_EQ(0x78, data[3]); + ASSERT_EQ(0x9a, data[4]); + ASSERT_EQ(0xbc, data[5]); + ASSERT_EQ(0xde, data[6]); + ASSERT_EQ(0xf0, data[7]); + } +} + + +TEST(BitStream, BitByBit) +{ + static const int NUM_BYTES = 1024; + uavcan::StaticTransferBuffer buf; + uavcan::BitStream bs_wr(buf); + + std::string binary_string; + unsigned counter = 0; + for (int byte = 0; byte < NUM_BYTES; byte++) + { + for (int bit = 0; bit < 8; bit++, counter++) + { + const bool value = counter % 3 == 0; + binary_string.push_back(value ? '1' : '0'); + const uint8_t data[] = { uint8_t(value << 7) }; + ASSERT_EQ(1, bs_wr.write(data, 1)); + } + binary_string.push_back(' '); + } + binary_string.erase(binary_string.length() - 1, 1); + + /* + * Currently we have no free buffer space, so next write() must fail + */ + const uint8_t dummy_data_wr[] = { 0xFF }; + ASSERT_EQ(0, bs_wr.write(dummy_data_wr, 1)); + + /* + * Bitstream content validation + */ +// std::cout << bs.toString() << std::endl; +// std::cout << "Reference:\n" << binary_string << std::endl; + ASSERT_EQ(binary_string, bs_wr.toString()); + + /* + * Read back + */ + uavcan::BitStream bs_rd(buf); + counter = 0; + for (int byte = 0; byte < NUM_BYTES; byte++) + { + for (int bit = 0; bit < 8; bit++, counter++) + { + const bool value = counter % 3 == 0; + uint8_t data[1]; + ASSERT_EQ(1, bs_rd.read(data, 1)); + if (value) + { + ASSERT_EQ(0x80, data[0]); + } + else + { + ASSERT_EQ(0, data[0]); + } + } + } + + /* + * Making sure that reading out of buffer range will fail with error + */ + uint8_t dummy_data_rd[] = { 0xFF }; + ASSERT_EQ(0, bs_wr.read(dummy_data_rd, 1)); + ASSERT_EQ(0xFF, dummy_data_rd[0]); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/char_array_formatter.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/char_array_formatter.cpp new file mode 100644 index 0000000000..134d870392 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/char_array_formatter.cpp @@ -0,0 +1,80 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +using uavcan::Array; +using uavcan::ArrayModeDynamic; +using uavcan::ArrayModeStatic; +using uavcan::IntegerSpec; +using uavcan::SignednessSigned; +using uavcan::SignednessUnsigned; +using uavcan::CastModeSaturate; +using uavcan::CastModeTruncate; +using uavcan::CharArrayFormatter; + +TEST(CharArrayFormatter, Basic) +{ + typedef Array, ArrayModeDynamic, 45> A8; + A8 a; + + CharArrayFormatter f(a); + ASSERT_STREQ("", f.getArray().c_str()); + + f.write("Don't %s.", "Panic"); + ASSERT_STREQ("Don't Panic.", f.getArray().c_str()); + + f.write(" abc%idef ", 123); + ASSERT_STREQ("Don't Panic. abc123def ", f.getArray().c_str()); + + f.write("%g", 0.0); + ASSERT_STREQ("Don't Panic. abc123def 0", f.getArray().c_str()); + + a.clear(); + ASSERT_STREQ("", f.getArray().c_str()); + + f.write("123456789"); + ASSERT_STREQ("123456789", f.getArray().c_str()); +} + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +TEST(CharArrayFormatter, Hardcore) +{ + typedef Array, ArrayModeDynamic, 150> A8; + A8 a; + + CharArrayFormatter f(a); + + f.write( + "%% char='%*' double='%*' long='%*' unsigned long='%*' int='%s' long double='%*' bool='%*' const char='%*' %%", + '%', -12.3456, -123456789123456789L, 987654321, -123456789, 0.000000001L, true, "Don't Panic."); + + static const std::string Reference = + "% char='%' double='-12.3456' long='-123456789123456789' unsigned long='987654321' int='-123456789' " + "long double='1e-09' bool='1' const char='Don't Pani"; // few chars truncated! + + ASSERT_STREQ(Reference.c_str(), f.getArray().c_str()); + + a.clear(); + + f.write(""); + ASSERT_STREQ("", f.getArray().c_str()); + + f.write("%%"); // Nothing to format --> "%%" is not expanded + ASSERT_STREQ("%%", f.getArray().c_str()); + + f.write("%*", "Test", 123, true); // Extra args ignored + ASSERT_STREQ("%%Test", f.getArray().c_str()); + + f.write("%% %* %* %% %*", true); // Insufficient args are OK; second "%%" is not expanded + ASSERT_STREQ("%%Test% 1 %* %% %*", f.getArray().c_str()); +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/float_spec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/float_spec.cpp new file mode 100644 index 0000000000..eacc0522fe --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/float_spec.cpp @@ -0,0 +1,184 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + + +TEST(FloatSpec, Sizes) +{ + uavcan::StaticAssert::Type) == 4>::check(); + uavcan::StaticAssert::Type) == 4>::check(); + uavcan::StaticAssert::Type) == 8>::check(); + uavcan::StaticAssert::Type) >= 10>::check(); +} + +TEST(FloatSpec, Limits) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<32, CastModeTruncate> F32T; + typedef FloatSpec<64, CastModeSaturate> F64S; + + ASSERT_FALSE(F16S::IsExactRepresentation); + ASSERT_FLOAT_EQ(65504.0F, F16S::max()); + ASSERT_FLOAT_EQ(9.77e-04F, F16S::epsilon()); + + ASSERT_TRUE(F32T::IsExactRepresentation); + ASSERT_FLOAT_EQ(std::numeric_limits::max(), F32T::max()); + ASSERT_FLOAT_EQ(std::numeric_limits::epsilon(), F32T::epsilon()); + + ASSERT_TRUE(F64S::IsExactRepresentation); + ASSERT_DOUBLE_EQ(std::numeric_limits::max(), F64S::max()); + ASSERT_DOUBLE_EQ(std::numeric_limits::epsilon(), F64S::epsilon()); +} + +TEST(FloatSpec, Basic) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + using uavcan::StorageType; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + typedef FloatSpec<32, CastModeSaturate> F32S; + typedef FloatSpec<32, CastModeTruncate> F32T; + typedef FloatSpec<64, CastModeSaturate> F64S; + typedef FloatSpec<64, CastModeTruncate> F64T; + + static const long double Values[] = + { + 0.0, + 1.0, + M_PI, + 123, + -123, + 99999, + -999999, + std::numeric_limits::max(), + -std::numeric_limits::max(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + static const int NumValues = int(sizeof(Values) / sizeof(Values[0])); + + static const long double ValuesF16S[] = + { + 0.0, + 1.0, + 3.140625, + 123, + -123, + F16S::max(), + -F16S::max(), + F16S::max(), + -F16S::max(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + static const long double ValuesF16T[] = + { + 0.0, + 1.0, + 3.140625, + 123, + -123, + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), + nanl("") + }; + + /* + * Writing + */ + uavcan::StaticTransferBuffer buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + for (int i = 0; i < NumValues; i++) + { + ASSERT_EQ(1, F16S::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16T::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F32S::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F32T::encode(float(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F64S::encode(double(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F64T::encode(double(Values[i]), sc_wr, uavcan::TailArrayOptDisabled)); + } + + ASSERT_EQ(0, F16S::encode(0, sc_wr, uavcan::TailArrayOptDisabled)); // Out of buffer space now + + /* + * Reading + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + +#define CHECK(FloatType, expected_value) \ + do { \ + StorageType::Type var = StorageType::Type(); \ + ASSERT_EQ(1, FloatType::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); \ + if (!std::isnan(expected_value)) { \ + ASSERT_DOUBLE_EQ(expected_value, var); } \ + else { \ + ASSERT_EQ(!!std::isnan(expected_value), !!std::isnan(var)); } \ + } while (0) + + for (int i = 0; i < NumValues; i++) + { + CHECK(F16S, float(ValuesF16S[i])); + CHECK(F16T, float(ValuesF16T[i])); + CHECK(F32S, float(Values[i])); + CHECK(F32T, float(Values[i])); + CHECK(F64S, double(Values[i])); + CHECK(F64T, double(Values[i])); + } + +#undef CHECK +} + +TEST(FloatSpec, Float16Representation) +{ + using uavcan::FloatSpec; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef FloatSpec<16, CastModeSaturate> F16S; + typedef FloatSpec<16, CastModeTruncate> F16T; + + uavcan::StaticTransferBuffer<2 * 6> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, F16S::encode(0.0, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16S::encode(1.0, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16S::encode(-2.0, sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, F16T::encode(999999, sc_wr, uavcan::TailArrayOptDisabled)); // +inf + ASSERT_EQ(1, F16S::encode(-999999, sc_wr, uavcan::TailArrayOptDisabled)); // -max + ASSERT_EQ(1, F16S::encode(float(nan("")), sc_wr, uavcan::TailArrayOptDisabled)); // nan + + ASSERT_EQ(0, F16S::encode(0, sc_wr, uavcan::TailArrayOptDisabled)); // Out of buffer space now + + // Keep in mind that this is LITTLE ENDIAN representation + static const std::string Reference = + "00000000 00000000 " // 0.0 + "00000000 00111100 " // 1.0 + "00000000 11000000 " // -2.0 + "00000000 01111100 " // +inf + "11111111 11111011 " // -max + "11111111 01111111"; // nan + + ASSERT_EQ(Reference, bs_wr.toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/integer_spec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/integer_spec.cpp new file mode 100644 index 0000000000..abda00a2e7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/integer_spec.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(IntegerSpec, Limits) +{ + using uavcan::IntegerSpec; + using uavcan::SignednessSigned; + using uavcan::SignednessUnsigned; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + + typedef IntegerSpec<8, SignednessUnsigned, CastModeSaturate> UInt8; + typedef IntegerSpec<4, SignednessSigned, CastModeTruncate> SInt4; + typedef IntegerSpec<32, SignednessUnsigned, CastModeTruncate> UInt32; + typedef IntegerSpec<40, SignednessUnsigned, CastModeSaturate> UInt40; + typedef IntegerSpec<64, SignednessUnsigned, CastModeTruncate> UInt64; + typedef IntegerSpec<64, SignednessSigned, CastModeSaturate> SInt64; + typedef IntegerSpec<63, SignednessUnsigned, CastModeSaturate> UInt63; + + ASSERT_EQ(255, UInt8::max()); + ASSERT_EQ(0, UInt8::min()); + + ASSERT_EQ(7, SInt4::max()); + ASSERT_EQ(-8, SInt4::min()); + + ASSERT_EQ(0xFFFFFFFF, UInt32::max()); + ASSERT_EQ(0, UInt32::min()); + + ASSERT_EQ(0xFFFFFFFFFF, UInt40::max()); + ASSERT_EQ(0, UInt40::min()); + + ASSERT_EQ(0xFFFFFFFFFFFFFFFF, UInt64::max()); + ASSERT_EQ(0, UInt64::min()); + + ASSERT_EQ(0x7FFFFFFFFFFFFFFF, SInt64::max()); + ASSERT_EQ(-0x8000000000000000, SInt64::min()); + + ASSERT_EQ(0x7FFFFFFFFFFFFFFF, UInt63::max()); + ASSERT_EQ(0, UInt63::min()); + + ASSERT_EQ(SInt64::max(), UInt63::max()); +} + + +TEST(IntegerSpec, Basic) +{ + using uavcan::IntegerSpec; + using uavcan::SignednessSigned; + using uavcan::SignednessUnsigned; + using uavcan::CastModeSaturate; + using uavcan::CastModeTruncate; + using uavcan::StorageType; + + uavcan::StaticTransferBuffer<100> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + typedef IntegerSpec<8, SignednessUnsigned, CastModeSaturate> UInt8S; + typedef IntegerSpec<4, SignednessSigned, CastModeTruncate> SInt4T; + typedef IntegerSpec<32, SignednessUnsigned, CastModeTruncate> UInt32T; + typedef IntegerSpec<40, SignednessUnsigned, CastModeSaturate> UInt40S; + typedef IntegerSpec<64, SignednessUnsigned, CastModeTruncate> UInt64T; + typedef IntegerSpec<58, SignednessSigned, CastModeSaturate> SInt58S; + typedef IntegerSpec<63, SignednessUnsigned, CastModeSaturate> UInt63S; + typedef IntegerSpec<10, SignednessSigned, CastModeSaturate> SInt10S; + typedef IntegerSpec<1, SignednessUnsigned, CastModeSaturate> UInt1S; + + ASSERT_EQ(1, UInt8S::encode(UInt8S::StorageType(123), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, SInt4T::encode(SInt4T::StorageType(-0x44), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt32T::encode(UInt32T::StorageType(0xFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt40S::encode(UInt40S::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt64T::encode(UInt64T::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, SInt58S::encode(SInt58S::StorageType(0xFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt63S::encode(UInt63S::StorageType(0xFFFFFFFFFFFFFFFF), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, SInt10S::encode(SInt10S::StorageType(-30000), sc_wr, uavcan::TailArrayOptDisabled)); + ASSERT_EQ(1, UInt1S::encode(UInt1S::StorageType(42), sc_wr, uavcan::TailArrayOptDisabled)); + + std::cout << bs_wr.toString() << std::endl; + + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + +#define CHECK(IntType, expected_value) \ + do { \ + StorageType::Type var = StorageType::Type(); \ + ASSERT_EQ(1, IntType::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); \ + ASSERT_EQ(expected_value, var); \ + } while (0) + + CHECK(UInt8S, 123); + CHECK(SInt4T, -4); + CHECK(UInt32T, 0xFFFFFFFF); + CHECK(UInt40S, 0xFFFFFFFFFF); + CHECK(UInt64T, 0xFFFFFFFFFFFFFFFF); + CHECK(SInt58S, 0x1FFFFFFFFFFFFFF); + CHECK(UInt63S, 0x7FFFFFFFFFFFFFFF); + CHECK(SInt10S, -512); + CHECK(UInt1S, 1); + +#undef CHECK + + StorageType::Type var; + ASSERT_EQ(0, UInt1S::decode(var, sc_rd, uavcan::TailArrayOptDisabled)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/scalar_codec.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/scalar_codec.cpp new file mode 100644 index 0000000000..e9012f71b5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/scalar_codec.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + + +TEST(ScalarCodec, Basic) +{ + uavcan::StaticTransferBuffer<38> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + { + uint64_t u64 = 0; + ASSERT_EQ(0, sc_wr.decode<64>(u64)); // Out of buffer space + } + + /* + * Encoding some variables + */ + ASSERT_EQ(1, sc_wr.encode<12>(uint16_t(0xbeda))); // --> 0xeda + ASSERT_EQ(1, sc_wr.encode<1>(uint8_t(1))); + ASSERT_EQ(1, sc_wr.encode<1>(uint16_t(0))); + ASSERT_EQ(1, sc_wr.encode<4>(uint8_t(8))); + ASSERT_EQ(1, sc_wr.encode<32>(uint32_t(0xdeadbeef))); + ASSERT_EQ(1, sc_wr.encode<3>(int8_t(-1))); + ASSERT_EQ(1, sc_wr.encode<4>(int8_t(-6))); + ASSERT_EQ(1, sc_wr.encode<20>(int32_t(-123456))); + ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits::min())); + ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits::max())); + ASSERT_EQ(1, sc_wr.encode<15>(int16_t(-1))); + ASSERT_EQ(1, sc_wr.encode<2>(int16_t(-1))); + ASSERT_EQ(1, sc_wr.encode<16>(std::numeric_limits::min())); + ASSERT_EQ(1, sc_wr.encode<64>(std::numeric_limits::max())); // Total 302 bit (38 bytes) + + ASSERT_EQ(0, sc_wr.encode<64>(std::numeric_limits::min())); // Out of buffer space + + /* + * Decoding back + */ + uavcan::BitStream bs_rd(buf); + uavcan::ScalarCodec sc_rd(bs_rd); + + uint8_t u8 = 0; + int8_t i8 = 0; + uint16_t u16 = 0; + int16_t i16 = 0; + uint32_t u32 = 0; + int32_t i32 = 0; + uint64_t u64 = 0; + int64_t i64 = 0; + +#define CHECK(bitlen, variable, expected_value) \ + do { \ + ASSERT_EQ(1, sc_rd.decode(variable)); \ + ASSERT_EQ(expected_value, variable); \ + } while (0) + + CHECK(12, u16, 0xeda); + CHECK(1, u8, 1); + CHECK(1, u16, 0); + CHECK(4, u8, 8); + CHECK(32, u32, 0xdeadbeef); + CHECK(3, i8, -1); + CHECK(4, i8, -6); + CHECK(20, i32, -123456); + CHECK(64, i64, std::numeric_limits::min()); + CHECK(64, i64, std::numeric_limits::max()); + CHECK(15, i16, -1); + CHECK(2, i8, -1); + CHECK(16, i16, std::numeric_limits::min()); + CHECK(64, u64, std::numeric_limits::max()); + +#undef CHECK + + ASSERT_EQ(0, sc_rd.decode<64>(u64)); // Out of buffer space +} + +TEST(ScalarCodec, RepresentationCorrectness) +{ + uavcan::StaticTransferBuffer<4> buf; + uavcan::BitStream bs_wr(buf); + uavcan::ScalarCodec sc_wr(bs_wr); + + ASSERT_EQ(1, sc_wr.encode<12>(uint16_t(0xbeda))); // --> 0xeda + ASSERT_EQ(1, sc_wr.encode<3>(int8_t(-1))); + ASSERT_EQ(1, sc_wr.encode<4>(int8_t(-5))); + ASSERT_EQ(1, sc_wr.encode<2>(int16_t(-1))); + ASSERT_EQ(1, sc_wr.encode<4>(uint8_t(0x88))); // --> 8 + + // This representation was carefully crafted and triple checked: + static const std::string REFERENCE = "11011010 11101111 01111100 00000000"; + ASSERT_EQ(REFERENCE, bs_wr.toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/type_util.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/type_util.cpp new file mode 100644 index 0000000000..e35b730e52 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/marshal/type_util.cpp @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(MarshalTypeUtil, IntegerBitLen) +{ + using uavcan::IntegerBitLen; + + ASSERT_EQ(0, IntegerBitLen<0>::Result); + ASSERT_EQ(1, IntegerBitLen<1>::Result); + ASSERT_EQ(6, IntegerBitLen<42>::Result); + ASSERT_EQ(8, IntegerBitLen<232>::Result); + ASSERT_EQ(32, IntegerBitLen<0x81234567>::Result); +} + + +TEST(MarshalTypeUtil, BitLenToByteLen) +{ + using uavcan::BitLenToByteLen; + + ASSERT_EQ(2, BitLenToByteLen<16>::Result); + ASSERT_EQ(1, BitLenToByteLen<8>::Result); + ASSERT_EQ(1, BitLenToByteLen<7>::Result); + ASSERT_EQ(1, BitLenToByteLen<1>::Result); + ASSERT_EQ(2, BitLenToByteLen<9>::Result); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/global_data_type_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/global_data_type_registry.cpp new file mode 100644 index 0000000000..f81b290285 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/global_data_type_registry.cpp @@ -0,0 +1,168 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace +{ + +struct DataTypeAMessage +{ + enum { DefaultDataTypeID = 0 }; + enum { DataTypeKind = uavcan::DataTypeKindMessage }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(123); } + static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; } +}; + +struct DataTypeAService +{ + enum { DefaultDataTypeID = 0 }; + enum { DataTypeKind = uavcan::DataTypeKindService }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(789); } + static const char* getDataTypeFullName() { return "my_namespace.DataTypeA"; } +}; + +struct DataTypeB +{ + enum { DefaultDataTypeID = 42 }; + enum { DataTypeKind = uavcan::DataTypeKindMessage }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(456); } + static const char* getDataTypeFullName() { return "my_namespace.DataTypeB"; } +}; + +struct DataTypeC +{ + enum { DefaultDataTypeID = 1023 }; + enum { DataTypeKind = uavcan::DataTypeKindMessage }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(654); } + static const char* getDataTypeFullName() { return "foo.DataTypeC"; } +}; + +struct DataTypeD +{ + enum { DefaultDataTypeID = 43 }; + enum { DataTypeKind = uavcan::DataTypeKindService }; + static uavcan::DataTypeSignature getDataTypeSignature() { return uavcan::DataTypeSignature(987); } + static const char* getDataTypeFullName() { return "foo.DataTypeD"; } +}; + +template +uavcan::DataTypeDescriptor extractDescriptor(uint16_t dtid = Type::DefaultDataTypeID) +{ + return uavcan::DataTypeDescriptor(uavcan::DataTypeKind(Type::DataTypeKind), dtid, + Type::getDataTypeSignature(), Type::getDataTypeFullName()); +} + +} + + +TEST(GlobalDataTypeRegistry, Basic) +{ + using uavcan::GlobalDataTypeRegistry; + using uavcan::DataTypeSignature; + + GlobalDataTypeRegistry::instance().reset(); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen()); + ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * Static registrations + */ + uavcan::DefaultDataTypeRegistrator reg_DataTypeAMessage; + uavcan::DefaultDataTypeRegistrator reg_DataTypeB; + + ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(0, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * Runtime registrations + */ + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType( + DataTypeAService::DefaultDataTypeID)); + + ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * Runtime re-registration + */ + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType(147)); + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType(741)); + + ASSERT_EQ(2, GlobalDataTypeRegistry::instance().getNumMessageTypes()); + ASSERT_EQ(1, GlobalDataTypeRegistry::instance().getNumServiceTypes()); + + /* + * These types will be necessary for the aggregate signature test + */ + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultCollision, + GlobalDataTypeRegistry::instance().registerDataType(741)); // ID COLLISION + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultOk, + GlobalDataTypeRegistry::instance().registerDataType(DataTypeC::DefaultDataTypeID)); + uavcan::DefaultDataTypeRegistrator reg_DataTypeD; + + /* + * Frozen state + */ + GlobalDataTypeRegistry::instance().freeze(); + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen, + GlobalDataTypeRegistry::instance().registerDataType(555)); // Rejected + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen, + GlobalDataTypeRegistry::instance().registerDataType(999)); // Rejected + + ASSERT_EQ(GlobalDataTypeRegistry::RegistrationResultFrozen, + GlobalDataTypeRegistry::instance().registerDataType(888)); // Rejected + + /* + * Searching + */ + const uavcan::DataTypeDescriptor* pdtd = UAVCAN_NULLPTR; + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, "Nonexistent")); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find("Nonexistent")); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, 987)); + // Asking for service, but this is a message: + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, "my_namespace.DataTypeB")); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, 42)); + + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, + "my_namespace.DataTypeB"))); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find("my_namespace.DataTypeB"))); + ASSERT_EQ(extractDescriptor(741), *pdtd); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, 741))); + ASSERT_EQ(extractDescriptor(741), *pdtd); + + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, + "my_namespace.DataTypeA"))); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find("my_namespace.DataTypeA"))); + ASSERT_EQ(extractDescriptor(), *pdtd); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindMessage, uavcan::DataTypeID(0)))); + ASSERT_EQ(extractDescriptor(), *pdtd); + + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, + "my_namespace.DataTypeA"))); + ASSERT_EQ(extractDescriptor(147), *pdtd); + ASSERT_TRUE((pdtd = GlobalDataTypeRegistry::instance().find(uavcan::DataTypeKindService, 147))); + ASSERT_EQ(extractDescriptor(147), *pdtd); +} + + +TEST(GlobalDataTypeRegistry, Reset) +{ + using uavcan::GlobalDataTypeRegistry; + + /* + * Since we're dealing with singleton, we need to reset it for other tests to use + */ + ASSERT_TRUE(GlobalDataTypeRegistry::instance().isFrozen()); + GlobalDataTypeRegistry::instance().reset(); + ASSERT_FALSE(GlobalDataTypeRegistry::instance().isFrozen()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/node.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/node.cpp new file mode 100644 index 0000000000..cc4a4ff031 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/node.cpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include // Compilability test +#include +#include "test_node.hpp" +#include "../protocol/helpers.hpp" + +static void registerTypes() +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + uavcan::DefaultDataTypeRegistrator _reg8; +} + + +TEST(Node, Basic) +{ + registerTypes(); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 0; + swver.minor = 1; + swver.vcs_commit = 0xDEADBEEF; + + std::cout << "sizeof(uavcan::Node<0>): " << sizeof(uavcan::Node<0>) << std::endl; + + /* + * uavcan::Node + */ + uavcan::Node<1024> node1(nodes.can_a, nodes.clock_a); + node1.setName("com.example"); + node1.setNodeID(1); + node1.setSoftwareVersion(swver); + + /* + * Companion test node + */ + uavcan::Node<1024> node2(nodes.can_b, nodes.clock_b); + node2.setName("foobar"); + node2.setNodeID(2); + node2.setSoftwareVersion(swver); + + BackgroundSpinner bgspinner(node2, node1); + bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + + uavcan::NodeStatusMonitor node_status_monitor(node2); + ASSERT_LE(0, node_status_monitor.start()); + + /* + * Init the second node - network is empty + */ + ASSERT_LE(0, node2.start()); + ASSERT_FALSE(node_status_monitor.findNodeWithWorstHealth().isValid()); + + /* + * Init the first node + */ + ASSERT_FALSE(node1.isStarted()); + ASSERT_EQ(-uavcan::ErrNotInited, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node1.start()); + ASSERT_TRUE(node1.isStarted()); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(2000))); + + ASSERT_EQ(1, node_status_monitor.findNodeWithWorstHealth().get()); + + /* + * Some logging + */ + SubscriberWithCollector log_sub(node2); + ASSERT_LE(0, log_sub.start()); + + node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + node1.logInfo("test", "6 * 9 = 42"); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20))); + + ASSERT_TRUE(log_sub.collector.msg.get()); + std::cout << *log_sub.collector.msg << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/publisher.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/publisher.cpp new file mode 100644 index 0000000000..f3332a4e0e --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/publisher.cpp @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + + +TEST(Publisher, Basic) +{ + SystemClockMock clock_mock(100); + CanDriverMock can_driver(2, clock_mock); + TestNode node(can_driver, clock_mock, 1); + + uavcan::Publisher publisher(node); + + ASSERT_FALSE(publisher.getTransferSender().isInitialized()); + + std::cout << + "sizeof(uavcan::Publisher): " << + sizeof(uavcan::Publisher) << std::endl; + + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + /* + * Message layout: + * uint8 seq + * uint8 sysid + * uint8 compid + * uint8 msgid + * uint8[<256] payload + */ + root_ns_a::MavlinkMessage msg; + msg.seq = 0x42; + msg.sysid = 0x72; + msg.compid = 0x08; + msg.msgid = 0xa5; + msg.payload = "Msg"; + + const uint8_t expected_transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'}; + const uint64_t tx_timeout_usec = uint64_t(publisher.getDefaultTxTimeout().toUSec()); + + /* + * Broadcast + */ + { + ASSERT_LT(0, publisher.broadcast(msg)); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame expected_frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node.getNodeID(), uavcan::NodeID::Broadcast, 0); + expected_frame.setPayload(expected_transfer_payload, 7); + expected_frame.setStartOfTransfer(true); + expected_frame.setEndOfTransfer(true); + + uavcan::CanFrame expected_can_frame; + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + + // Second shot - checking the transfer ID + publisher.setPriority(10); + ASSERT_LT(0, publisher.broadcast(msg)); + + expected_frame = uavcan::Frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, + uavcan::TransferTypeMessageBroadcast, + node.getNodeID(), uavcan::NodeID::Broadcast, 1); + expected_frame.setStartOfTransfer(true); + expected_frame.setEndOfTransfer(true); + expected_frame.setPayload(expected_transfer_payload, 7); + expected_frame.setPriority(10); + ASSERT_TRUE(expected_frame.compile(expected_can_frame)); + + ASSERT_TRUE(can_driver.ifaces[0].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[1].matchAndPopTx(expected_can_frame, tx_timeout_usec + 100)); + ASSERT_TRUE(can_driver.ifaces[0].tx.empty()); + ASSERT_TRUE(can_driver.ifaces[1].tx.empty()); + } + + clock_mock.advance(1000); + + /* + * Misc + */ + ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen()); + ASSERT_TRUE(publisher.getTransferSender().isInitialized()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/scheduler.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/scheduler.cpp new file mode 100644 index 0000000000..6e8fad2864 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/scheduler.cpp @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + +#if !defined(UAVCAN_CPP11) || !defined(UAVCAN_CPP_VERSION) +# error UAVCAN_CPP_VERSION +#endif + +struct TimerCallCounter +{ + std::vector events_a; + std::vector events_b; + + void callA(const uavcan::TimerEvent& ev) { events_a.push_back(ev); } + void callB(const uavcan::TimerEvent& ev) { events_b.push_back(ev); } + + typedef uavcan::MethodBinder Binder; + + Binder bindA() { return Binder(this, &TimerCallCounter::callA); } + Binder bindB() { return Binder(this, &TimerCallCounter::callB); } +}; + +/* + * This test can fail on a non real time system. That's kinda sad but okay. + */ +TEST(Scheduler, Timers) +{ + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + /* + * Registration + */ + { + TimerCallCounter tcc; + uavcan::TimerEventForwarder a(node, tcc.bindA()); + uavcan::TimerEventForwarder b(node, tcc.bindB()); + + ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + const uavcan::MonotonicTime start_ts = clock_driver.getMonotonic(); + + a.startOneShotWithDeadline(start_ts + durMono(100000)); + b.startPeriodic(durMono(1000)); + + ASSERT_EQ(2, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + /* + * Spinning + */ + ASSERT_EQ(0, node.spin(start_ts + durMono(1000000))); + + ASSERT_EQ(1, tcc.events_a.size()); + ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, start_ts + durMono(100000))); + ASSERT_TRUE(areTimestampsClose(tcc.events_a[0].scheduled_time, tcc.events_a[0].real_time)); + + ASSERT_LT(900, tcc.events_b.size()); + ASSERT_GT(1100, tcc.events_b.size()); + { + uavcan::MonotonicTime next_expected_deadline = start_ts + durMono(1000); + for (unsigned i = 0; i < tcc.events_b.size(); i++) + { + ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, next_expected_deadline)); + ASSERT_TRUE(areTimestampsClose(tcc.events_b[i].scheduled_time, tcc.events_b[i].real_time)); + next_expected_deadline += durMono(1000); + } + } + + /* + * Deinitialization + */ + ASSERT_EQ(1, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + ASSERT_FALSE(a.isRunning()); + ASSERT_EQ(uavcan::MonotonicDuration::getInfinite(), a.getPeriod()); + + ASSERT_TRUE(b.isRunning()); + ASSERT_EQ(1000, b.getPeriod().toUSec()); + } + + ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); // Both timers were destroyed by now + ASSERT_EQ(0, node.spin(durMono(1000))); // Spin some more without timers +} + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +TEST(Scheduler, TimerCpp11) +{ + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + int count = 0; + + uavcan::Timer tm(node, [&count](const uavcan::TimerEvent&) { count++; }); + + ASSERT_EQ(0, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + tm.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(1, node.getScheduler().getDeadlineScheduler().getNumHandlers()); + + ASSERT_EQ(0, node.spin(uavcan::MonotonicDuration::fromMSec(100))); + + std::cout << count << std::endl; + ASSERT_LE(5, count); + ASSERT_GE(15, count); +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_client.cpp new file mode 100644 index 0000000000..28014eecc3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_client.cpp @@ -0,0 +1,454 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "test_node.hpp" + + +template +struct ServiceCallResultHandler +{ + typedef typename uavcan::ServiceCallResult::Status StatusType; + StatusType last_status; + uavcan::NodeID last_server_node_id; + typename DataType::Response last_response; + std::queue responses; + + void handleResponse(const uavcan::ServiceCallResult& result) + { + std::cout << result << std::endl; + last_status = result.getStatus(); + last_response = result.getResponse(); + last_server_node_id = result.getCallID().server_node_id; + responses.push(result.getResponse()); + } + + bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const + { + if (status == last_status && + server_node_id == last_server_node_id && + response == last_response) + { + return true; + } + else + { + std::cout << "MISMATCH: status=" << int(last_status) << ", last_server_node_id=" + << int(last_server_node_id.get()) << ", last response:\n" << last_response << std::endl; + return false; + } + } + + typedef uavcan::MethodBinder&)> Binder; + + Binder bind() { return Binder(this, &ServiceCallResultHandler::handleResponse); } +}; + + +static void stringServiceServerCallback(const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& rsp) +{ + rsp.string_response = "Request string: "; + rsp.string_response += req.string_request; +} + +static void rejectingStringServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& rsp) +{ + rsp.string_response = "Request string: "; + rsp.string_response += req.string_request; + ASSERT_TRUE(rsp.isResponseEnabled()); + rsp.setResponseEnabled(false); + ASSERT_FALSE(rsp.isResponseEnabled()); +} + +static void emptyServiceServerCallback(const uavcan::ReceivedDataStructure&, + uavcan::ServiceResponseDataStructure&) +{ + // Nothing to do - the service is empty +} + + +TEST(ServiceClient, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(stringServiceServerCallback)); + + { + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client1(nodes.b); + ClientType client2(nodes.b); + ClientType client3(nodes.b); + + ASSERT_EQ(0, client1.getNumPendingCalls()); + ASSERT_EQ(0, client2.getNumPendingCalls()); + ASSERT_EQ(0, client3.getNumPendingCalls()); + + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + + client1.setCallback(handler.bind()); + client2.setCallback(client1.getCallback()); + client3.setCallback(client1.getCallback()); + client3.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners()); + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening! + + root_ns_a::StringService::Request request; + request.string_request = "Hello world"; + + std::cout << "!!! Calling!" << std::endl; + + ASSERT_LT(0, client1.call(1, request)); // OK + ASSERT_LT(0, client1.call(1, request)); // OK - second request + ASSERT_LT(0, client2.call(1, request)); // OK + ASSERT_LT(0, client3.call(99, request)); // Will timeout! + ASSERT_LT(0, client3.call(1, request)); // OK - second request + + ASSERT_TRUE(client1.hasPendingCallToServer(1)); + ASSERT_TRUE(client2.hasPendingCallToServer(1)); + ASSERT_TRUE(client3.hasPendingCallToServer(99)); + ASSERT_TRUE(client3.hasPendingCallToServer(1)); + + std::cout << "!!! Spinning!" << std::endl; + + ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! + + ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); + + ASSERT_EQ(2, client1.getNumPendingCalls()); + ASSERT_EQ(1, client2.getNumPendingCalls()); + ASSERT_EQ(2, client3.getNumPendingCalls()); + + ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); + + std::cout << "!!! Spin finished!" << std::endl; + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening! + + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); + + ASSERT_EQ(0, client1.getNumPendingCalls()); + ASSERT_EQ(0, client2.getNumPendingCalls()); + ASSERT_EQ(1, client3.getNumPendingCalls()); // The one addressed to 99 is still waiting + + // Validating + root_ns_a::StringService::Response expected_response; + expected_response.string_response = "Request string: Hello world"; + ASSERT_TRUE(handler.match(ResultType::Success, 1, expected_response)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_FALSE(client3.hasPendingCalls()); + + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :( + + // Validating + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); + + // Stray request + ASSERT_LT(0, client3.call(99, request)); // Will timeout! + ASSERT_TRUE(client3.hasPendingCalls()); + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); + } + + // All destroyed - nobody listening + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); +} + + +TEST(ServiceClient, Rejection) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(rejectingStringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client1(nodes.b); + client1.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + client1.setCallback(handler.bind()); + + root_ns_a::StringService::Request request; + request.string_request = "Hello world"; + + ASSERT_LT(0, client1.call(1, request)); + + ASSERT_EQ(uavcan::NodeID(1), client1.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client1.getCallIDByIndex(1).server_node_id); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); + ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client1.hasPendingCallToServer(1)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 1, root_ns_a::StringService::Response())); +} + + +TEST(ServiceClient, ConcurrentCalls) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(stringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + /* + * Initializing + */ + ClientType client(nodes.b); + + ASSERT_EQ(0, client.getNumPendingCalls()); + + client.setCallback(handler.bind()); + + ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners()); + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening! + + ASSERT_FALSE(client.hasPendingCalls()); + ASSERT_EQ(0, client.getNumPendingCalls()); + + /* + * Calling ten requests, the last one will be cancelled + * Note that there will be non-unique transfer ID values; the client must handle that correctly + */ + uavcan::ServiceCallID last_call_id; + for (int i = 0; i < 10; i++) + { + std::ostringstream os; + os << i; + root_ns_a::StringService::Request request; + request.string_request = os.str().c_str(); + ASSERT_LT(0, client.call(1, request, last_call_id)); + } + + ASSERT_LT(0, client.call(99, root_ns_a::StringService::Request())); // Will timeout in 1000 ms + + client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_LT(0, client.call(88, root_ns_a::StringService::Request())); // Will timeout in 100 ms + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(12, client.getNumPendingCalls()); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening + + /* + * Cancelling one + */ + client.cancelCall(last_call_id); + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(11, client.getNumPendingCalls()); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening + + /* + * Spinning + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(2, client.getNumPendingCalls()); // Two still pending + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening + + /* + * Validating the ones that didn't timeout + */ + root_ns_a::StringService::Response last_response; + for (int i = 0; i < 9; i++) + { + std::ostringstream os; + os << "Request string: " << i; + last_response.string_response = os.str().c_str(); + + ASSERT_FALSE(handler.responses.empty()); + + ASSERT_STREQ(last_response.string_response.c_str(), handler.responses.front().string_response.c_str()); + + handler.responses.pop(); + } + + ASSERT_TRUE(handler.responses.empty()); + + /* + * Validating the 100 ms timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_TRUE(client.hasPendingCalls()); + ASSERT_EQ(1, client.getNumPendingCalls()); // One dropped + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, root_ns_a::StringService::Response())); + + /* + * Validating the 1000 ms timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + + ASSERT_FALSE(client.hasPendingCalls()); + ASSERT_EQ(0, client.getNumPendingCalls()); // All finished + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Not listening + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); +} + + +TEST(ServiceClient, Empty) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(emptyServiceServerCallback)); + + { + // Caller + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client(nodes.b); + + client.setCallback(handler.bind()); + + root_ns_a::EmptyService::Request request; + + ASSERT_LT(0, client.call(1, request)); // OK + } + + // All destroyed - nobody listening + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); +} + + +TEST(ServiceClient, Priority) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Initializing + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + ClientType client(nodes.b); + client.setCallback(handler.bind()); + + // Calling + root_ns_a::EmptyService::Request request; + + client.setPriority(0); + ASSERT_LT(0, client.call(1, request)); // OK + + client.setPriority(10); + ASSERT_LT(0, client.call(1, request)); // OK + + client.setPriority(20); + ASSERT_LT(0, client.call(1, request)); // OK + + client.setPriority(31); + ASSERT_LT(0, client.call(1, request)); // OK + + // Validating + ASSERT_EQ(4, nodes.can_a.read_queue.size()); + + uavcan::Frame frame; + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(0, frame.getPriority().get()); + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(10, frame.getPriority().get()); + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(20, frame.getPriority().get()); + + ASSERT_TRUE(frame.parse(nodes.can_a.read_queue.front())); + nodes.can_a.read_queue.pop(); + ASSERT_EQ(31, frame.getPriority().get()); +} + + +TEST(ServiceClient, Sizes) +{ + using namespace uavcan; + + std::cout << "GetDataTypeInfo server: " << + sizeof(ServiceServer) << std::endl; + + std::cout << "RestartNode server: " << + sizeof(ServiceServer) << std::endl; + + std::cout << "GetDataTypeInfo client: " << + sizeof(ServiceClient) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_server.cpp new file mode 100644 index 0000000000..3d1efcbe9b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/service_server.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + + +struct StringServerImpl +{ + const char* string_response; + + StringServerImpl(const char* string_response) : string_response(string_response) { } + + void handleRequest(const uavcan::ReceivedDataStructure& request, + root_ns_a::StringService::Response& response) + { + std::cout << request << std::endl; + response.string_response = request.string_request; + response.string_response += " --> "; + response.string_response += string_response; + std::cout << response << std::endl; + } + + typedef uavcan::MethodBinder&, + root_ns_a::StringService::Response&)> Binder; + + Binder bind() { return Binder(this, &StringServerImpl::handleRequest); } +}; + + +struct EmptyServerImpl +{ + void handleRequest(const uavcan::ReceivedDataStructure& request, + root_ns_a::EmptyService::Response&) + { + std::cout << request << std::endl; + } + + typedef uavcan::MethodBinder&, + root_ns_a::EmptyService::Response&)> Binder; + + Binder bind() { return Binder(this, &EmptyServerImpl::handleRequest); } +}; + + +TEST(ServiceServer, Basic) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + StringServerImpl impl("456"); + + { + uavcan::ServiceServer server(node); + + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); + server.start(impl.bind()); + ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); + + /* + * Request frames + */ + for (uint8_t i = 0; i < 2; i++) + { + uavcan::Frame frame(root_ns_a::StringService::DefaultDataTypeID, uavcan::TransferTypeServiceRequest, + uavcan::NodeID(uint8_t(i + 0x10)), 1, i); + + const uint8_t req[] = {'r', 'e', 'q', uint8_t(i + '0')}; + frame.setPayload(req, sizeof(req)); + + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + frame.setPriority(i); + + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + can_driver.ifaces[0].pushRx(rx_frame); + } + + node.spin(clock_driver.getMonotonic() + uavcan::MonotonicDuration::fromUSec(10000)); + + /* + * Responses (MFT) + */ + ASSERT_EQ(4, can_driver.ifaces[0].tx.size()); + for (int i = 0; i < 2; i++) + { + char payloads[2][8]; + std::snprintf(payloads[0], 8, "req%i ", i); + std::snprintf(payloads[1], 8, "--> 456"); + + // First frame + uavcan::Frame fr; + ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame())); + std::cout << fr.toString() << std::endl; + ASSERT_FALSE(std::strncmp(payloads[0], reinterpret_cast(fr.getPayloadPtr() + 2), 5)); // No CRC + + ASSERT_EQ(i, fr.getTransferID().get()); + ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType()); + ASSERT_EQ(i + 0x10, fr.getDstNodeID().get()); + ASSERT_EQ(i, fr.getPriority().get()); + + // Second frame + ASSERT_TRUE(fr.parse(can_driver.ifaces[0].popTxFrame())); + std::cout << fr.toString() << std::endl; + // cppcheck-suppress arrayIndexOutOfBounds + ASSERT_FALSE(std::strncmp(payloads[1], reinterpret_cast(fr.getPayloadPtr()), 7)); + + ASSERT_EQ(i, fr.getTransferID().get()); + ASSERT_EQ(uavcan::TransferTypeServiceResponse, fr.getTransferType()); + ASSERT_EQ(i + 0x10, fr.getDstNodeID().get()); + } + + ASSERT_EQ(0, server.getRequestFailureCount()); + ASSERT_EQ(0, server.getResponseFailureCount()); + + ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); + } + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); +} + + +TEST(ServiceServer, Empty) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + EmptyServerImpl impl; + + uavcan::ServiceServer server(node); + + std::cout << "sizeof(ServiceServer): " + << sizeof(uavcan::ServiceServer) << std::endl; + + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); + ASSERT_GE(0, server.start(impl.bind())); + ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/sub_node.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/sub_node.cpp new file mode 100644 index 0000000000..3cb04df8dd --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/sub_node.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "test_node.hpp" +#include "../protocol/helpers.hpp" + +static void registerTypes() +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + uavcan::DefaultDataTypeRegistrator _reg8; +} + + +TEST(SubNode, Basic) +{ + registerTypes(); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 0; + swver.minor = 1; + swver.vcs_commit = 0xDEADBEEF; + + std::cout << "sizeof(uavcan::SubNode<0>): " << sizeof(uavcan::SubNode<0>) << std::endl; + + /* + * uavcan::Node + */ + uavcan::Node<1024> node1(nodes.can_a, nodes.clock_a); + node1.setName("com.example"); + node1.setNodeID(1); + node1.setSoftwareVersion(swver); + + /* + * uavcan::SubNode + */ + uavcan::SubNode<1024> node2(nodes.can_b, nodes.clock_b); + + BackgroundSpinner bgspinner(node2, node1); + bgspinner.startPeriodic(uavcan::MonotonicDuration::fromMSec(10)); + + uavcan::NodeStatusMonitor node_status_monitor(node2); + ASSERT_LE(0, node_status_monitor.start()); + + /* + * Init the first node + */ + ASSERT_FALSE(node1.isStarted()); + ASSERT_EQ(-uavcan::ErrNotInited, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node1.start()); + ASSERT_TRUE(node1.isStarted()); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(2000))); + + ASSERT_EQ(1, node_status_monitor.findNodeWithWorstHealth().get()); + + /* + * Some logging + */ + SubscriberWithCollector log_sub(node2); + ASSERT_LE(0, log_sub.start()); + + node1.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + node1.logInfo("test", "6 * 9 = 42"); + + ASSERT_LE(0, node1.spin(uavcan::MonotonicDuration::fromMSec(20))); + ASSERT_LE(0, node2.spin(uavcan::MonotonicDuration::fromMSec(20))); + + ASSERT_TRUE(log_sub.collector.msg.get()); + std::cout << *log_sub.collector.msg << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/subscriber.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/subscriber.cpp new file mode 100644 index 0000000000..40be485af9 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/subscriber.cpp @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "../clock.hpp" +#include "../transport/can/can.hpp" +#include "test_node.hpp" + + +template +struct SubscriptionListener +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructure; + + struct ReceivedDataStructureCopy + { + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::uint8_t iface_index; + DataType msg; + + ReceivedDataStructureCopy(const ReceivedDataStructure& s) + : ts_monotonic(s.getMonotonicTimestamp()) + , ts_utc(s.getUtcTimestamp()) + , transfer_type(s.getTransferType()) + , transfer_id(s.getTransferID()) + , src_node_id(s.getSrcNodeID()) + , iface_index(s.getIfaceIndex()) + , msg(s) + { } + }; + + std::vector simple; + std::vector extended; + + void receiveExtended(ReceivedDataStructure& msg) + { + extended.push_back(msg); + } + + void receiveSimple(DataType& msg) + { + simple.push_back(msg); + } + + typedef SubscriptionListener SelfType; + typedef uavcan::MethodBinder ExtendedBinder; + typedef uavcan::MethodBinder SimpleBinder; + + ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); } + SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); } +}; + + +TEST(Subscriber, Basic) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + typedef SubscriptionListener Listener; + + uavcan::Subscriber sub_extended(node); + uavcan::Subscriber sub_extended2(node); // Not used + uavcan::Subscriber sub_simple(node); + uavcan::Subscriber sub_simple2(node); // Not used + + std::cout << + "sizeof(uavcan::Subscriber): " << + sizeof(uavcan::Subscriber) << std::endl; + + // Null binder - will fail + ASSERT_EQ(-uavcan::ErrInvalidParam, sub_extended.start(Listener::ExtendedBinder(UAVCAN_NULLPTR, UAVCAN_NULLPTR))); + + Listener listener; + + /* + * Message layout: + * uint8 seq + * uint8 sysid + * uint8 compid + * uint8 msgid + * uint8[<256] payload + */ + root_ns_a::MavlinkMessage expected_msg; + expected_msg.seq = 0x42; + expected_msg.sysid = 0x72; + expected_msg.compid = 0x08; + expected_msg.msgid = 0xa5; + expected_msg.payload = "Msg"; + + const uint8_t transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'}; + + /* + * RxFrame generation + */ + std::vector rx_frames; + for (uint8_t i = 0; i < 4; i++) + { + uavcan::TransferType tt = uavcan::TransferTypeMessageBroadcast; + uavcan::NodeID dni = (tt == uavcan::TransferTypeMessageBroadcast) ? + uavcan::NodeID::Broadcast : node.getDispatcher().getNodeID(); + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, tt, uavcan::NodeID(uint8_t(i + 100)), + dni, i); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + frame.setPayload(transfer_payload, 7); + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + rx_frames.push_back(rx_frame); + } + + /* + * Reception + */ + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); + + ASSERT_EQ(0, sub_extended.start(listener.bindExtended())); + ASSERT_EQ(0, sub_extended2.start(listener.bindExtended())); + ASSERT_EQ(0, sub_simple.start(listener.bindSimple())); + ASSERT_EQ(0, sub_simple2.start(listener.bindSimple())); + + ASSERT_EQ(4, node.getDispatcher().getNumMessageListeners()); + + sub_extended2.stop(); // These are not used - making sure they aren't receiving anything + sub_simple2.stop(); + + ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners()); + + for (unsigned i = 0; i < rx_frames.size(); i++) + { + can_driver.ifaces[0].pushRx(rx_frames[i]); + can_driver.ifaces[1].pushRx(rx_frames[i]); + } + + ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000))); + + /* + * Validation + */ + ASSERT_EQ(listener.extended.size(), rx_frames.size()); + for (unsigned i = 0; i < rx_frames.size(); i++) + { + const Listener::ReceivedDataStructureCopy s = listener.extended.at(i); + ASSERT_TRUE(s.msg == expected_msg); + ASSERT_EQ(rx_frames[i].getSrcNodeID(), s.src_node_id); + ASSERT_EQ(rx_frames[i].getTransferID(), s.transfer_id); + ASSERT_EQ(rx_frames[i].getTransferType(), s.transfer_type); + ASSERT_EQ(rx_frames[i].getMonotonicTimestamp(), s.ts_monotonic); + ASSERT_EQ(rx_frames[i].getIfaceIndex(), s.iface_index); + } + + ASSERT_EQ(listener.simple.size(), rx_frames.size()); + for (unsigned i = 0; i < rx_frames.size(); i++) + { + ASSERT_TRUE(listener.simple.at(i) == expected_msg); + } + + ASSERT_EQ(0, sub_extended.getFailureCount()); + ASSERT_EQ(0, sub_simple.getFailureCount()); + + /* + * Unregistration + */ + ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners()); + + sub_extended.stop(); + sub_extended2.stop(); + sub_simple.stop(); + sub_simple2.stop(); + + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); +} + + +static void panickingSink(const uavcan::ReceivedDataStructure&) +{ + FAIL() << "I just went mad"; +} + + +TEST(Subscriber, FailureCount) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + { + uavcan::Subscriber sub(node); + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); + sub.start(panickingSink); + ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners()); + + ASSERT_EQ(0, sub.getFailureCount()); + + for (uint8_t i = 0; i < 4; i++) + { + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + // No payload - broken transfer + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + can_driver.ifaces[0].pushRx(rx_frame); + can_driver.ifaces[1].pushRx(rx_frame); + } + + ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000))); + + ASSERT_EQ(4, sub.getFailureCount()); + + ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners()); // Still there + } + ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners()); // Removed +} + + +TEST(Subscriber, SingleFrameTransfer) +{ + // Manual type registration - we can't rely on the GDTR state + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(2, clock_driver); + TestNode node(can_driver, clock_driver, 1); + + typedef SubscriptionListener Listener; + + uavcan::Subscriber sub(node); + + std::cout << + "sizeof(uavcan::Subscriber): " << + sizeof(uavcan::Subscriber) << std::endl; + + Listener listener; + + sub.start(listener.bindSimple()); + + for (uint8_t i = 0; i < 4; i++) + { + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + // No payload - message is empty + uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0); + can_driver.ifaces[0].pushRx(rx_frame); + can_driver.ifaces[1].pushRx(rx_frame); + } + + ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000))); + + ASSERT_EQ(0, sub.getFailureCount()); + + ASSERT_EQ(4, listener.simple.size()); + for (unsigned i = 0; i < 4; i++) + { + ASSERT_TRUE(listener.simple.at(i) == root_ns_a::EmptyMessage()); + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node.hpp new file mode 100644 index 0000000000..a8e6fc84fb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node.hpp @@ -0,0 +1,268 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include +#include "../transport/can/can.hpp" +#include +#include + +struct TestNode : public uavcan::INode +{ + /* + * This class used to use the simple pool allocator instead: + * uavcan::PoolAllocator pool; + * It has been replaced because unlike the simple allocator, heap-based one is not tested as extensively. + * Moreover, heap based allocator prints and error message upon destruction if some memory has not been freed. + */ + uavcan::HeapBasedPoolAllocator pool; + uavcan::Scheduler scheduler; + uint64_t internal_failure_count; + + TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id) : + pool(1024), + scheduler(can_driver, pool, clock_driver), + internal_failure_count(0) + { + setNodeID(self_node_id); + } + + virtual void registerInternalFailure(const char* msg) + { + std::cout << "TestNode internal failure: " << msg << std::endl; + internal_failure_count++; + } + + virtual uavcan::IPoolAllocator& getAllocator() { return pool; } + virtual uavcan::Scheduler& getScheduler() { return scheduler; } + virtual const uavcan::Scheduler& getScheduler() const { return scheduler; } +}; + + +struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface +{ + uavcan::ISystemClock& clock; + std::set others; + std::queue read_queue; + std::queue loopback_queue; + uint64_t error_count; + + PairableCanDriver(uavcan::ISystemClock& clock) + : clock(clock) + , error_count(0) + { } + + void linkTogether(PairableCanDriver* with) + { + this->others.insert(with); + with->others.insert(this); + others.erase(this); + } + + virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) + { + return (iface_index == 0) ? this : UAVCAN_NULLPTR; + } + virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const + { + return (iface_index == 0) ? this : UAVCAN_NULLPTR; + } + + virtual uavcan::uint8_t getNumIfaces() const { return 1; } + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline) + { + if (inout_masks.read == 1) + { + inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0; + } + if (inout_masks.read || inout_masks.write) + { + return 1; + } + while (clock.getMonotonic() < blocking_deadline) + { + usleep(1000); + } + return 0; + } + + virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags) + { + assert(!others.empty()); + for (std::set::iterator it = others.begin(); it != others.end(); ++it) + { + (*it)->read_queue.push(frame); + } + if (flags & uavcan::CanIOFlagLoopback) + { + loopback_queue.push(frame); + } + return 1; + } + + virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) + { + out_flags = 0; + if (loopback_queue.empty()) + { + assert(read_queue.size()); + out_frame = read_queue.front(); + read_queue.pop(); + } + else + { + out_flags |= uavcan::CanIOFlagLoopback; + out_frame = loopback_queue.front(); + loopback_queue.pop(); + } + out_ts_monotonic = clock.getMonotonic(); + out_ts_utc = clock.getUtc(); + return 1; + } + + void pushRxToAllIfaces(const uavcan::CanFrame& can_frame) + { + read_queue.push(can_frame); + } + + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; } + virtual uavcan::uint16_t getNumFilters() const { return 0; } + virtual uavcan::uint64_t getErrorCount() const { return error_count; } +}; + + +template +struct InterlinkedTestNodes +{ + ClockType clock_a; + ClockType clock_b; + PairableCanDriver can_a; + PairableCanDriver can_b; + TestNode a; + TestNode b; + + InterlinkedTestNodes(uavcan::NodeID nid_first, uavcan::NodeID nid_second) + : can_a(clock_a) + , can_b(clock_b) + , a(can_a, clock_a, nid_first) + , b(can_b, clock_b, nid_second) + { + can_a.linkTogether(&can_b); + } + + InterlinkedTestNodes() + : can_a(clock_a) + , can_b(clock_b) + , a(can_a, clock_a, 1) + , b(can_b, clock_b, 2) + { + can_a.linkTogether(&can_b); + } + + int spinBoth(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins2 = unsigned(duration.toMSec() / 2); + nspins2 = nspins2 ? nspins2 : 1; + while (nspins2 --> 0) + { + int ret = a.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + ret = b.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + return 0; + } +}; + + +typedef InterlinkedTestNodes InterlinkedTestNodesWithSysClock; +typedef InterlinkedTestNodes InterlinkedTestNodesWithClockMock; + + +template +struct TestNetwork +{ + struct NodeEnvironment + { + SystemClockDriver clock; + PairableCanDriver can_driver; + TestNode node; + + NodeEnvironment(uavcan::NodeID node_id) + : can_driver(clock) + , node(can_driver, clock, node_id) + { } + }; + + std::unique_ptr nodes[NumNodes]; + + TestNetwork(uavcan::uint8_t first_node_id = 1) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + nodes[i].reset(new NodeEnvironment(uint8_t(first_node_id + i))); + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + for (uavcan::uint8_t k = 0; k < NumNodes; k++) + { + nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver); + } + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + assert(nodes[i]->can_driver.others.size() == (NumNodes - 1)); + } + } + + int spinAll(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins = unsigned(duration.toMSec() / NumNodes); + nspins = nspins ? nspins : 1; + while (nspins --> 0) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + } + return 0; + } + + TestNode& operator[](unsigned index) + { + if (index >= NumNodes) + { + throw std::out_of_range("No such test node"); + } + return nodes[index]->node; + } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node_test.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 0000000000..b40357be21 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/node/test_node_test.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "test_node.hpp" + + +TEST(TestNode, TestNetwork) +{ + TestNetwork<4> nwk; + + uavcan::CanFrame frame; + for (uint8_t i = 0; i < 8; i++) + { + frame.data[i] = i; + } + frame.id = 1234U; + + ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags())); + + for (int i = 1; i < 4; i++) + { + uavcan::CanFrame rx; + uavcan::MonotonicTime ts_mono; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags)); + + ASSERT_TRUE(rx == frame); + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/data_type_info_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/data_type_info_provider.cpp new file mode 100644 index 0000000000..75e704b86c --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/data_type_info_provider.cpp @@ -0,0 +1,172 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "helpers.hpp" + +using uavcan::protocol::GetDataTypeInfo; +using uavcan::protocol::NodeStatus; +using uavcan::protocol::DataTypeKind; +using uavcan::ServiceCallResult; +using uavcan::DataTypeInfoProvider; +using uavcan::GlobalDataTypeRegistry; +using uavcan::DefaultDataTypeRegistrator; +using uavcan::MonotonicDuration; + +template +static bool validateDataTypeInfoResponse(const std::unique_ptr::Result>& resp, + unsigned flags) +{ + if (!resp.get()) + { + std::cout << "Null response" << std::endl; + return false; + } + if (!resp->isSuccessful()) + { + std::cout << "Request was not successful" << std::endl; + return false; + } + if (resp->getResponse().name != DataType::getDataTypeFullName()) + { + std::cout << "Type name mismatch: '" + << resp->getResponse().name.c_str() << "' '" + << DataType::getDataTypeFullName() << "'" << std::endl; + return false; + } + if (DataType::getDataTypeSignature().get() != resp->getResponse().signature) + { + std::cout << "Signature mismatch" << std::endl; + return false; + } + if (resp->getResponse().flags != flags) + { + std::cout << "Mask mismatch" << std::endl; + return false; + } + if (resp->getResponse().kind.value != DataType::DataTypeKind) + { + std::cout << "Kind mismatch" << std::endl; + return false; + } + if (resp->getResponse().id != DataType::DefaultDataTypeID) + { + std::cout << "DTID mismatch" << std::endl; + return false; + } + return true; +} + + +TEST(DataTypeInfoProvider, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + DataTypeInfoProvider dtip(nodes.a); + + GlobalDataTypeRegistry::instance().reset(); + DefaultDataTypeRegistrator _reg1; + DefaultDataTypeRegistrator _reg3; + + ASSERT_LE(0, dtip.start()); + + ServiceClientWithCollector gdti_cln(nodes.b); + + /* + * GetDataTypeInfo request for GetDataTypeInfo + */ + GetDataTypeInfo::Request gdti_request; + gdti_request.id = GetDataTypeInfo::DefaultDataTypeID; + gdti_request.kind.value = DataTypeKind::SERVICE; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN | + GetDataTypeInfo::Response::FLAG_SERVING)); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + + /* + * GetDataTypeInfo request for GetDataTypeInfo by name + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = 999; // Intentionally wrong + gdti_request.kind.value = DataTypeKind::MESSAGE; // Intentionally wrong + gdti_request.name = "uavcan.protocol.GetDataTypeInfo"; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN | + GetDataTypeInfo::Response::FLAG_SERVING)); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + + /* + * GetDataTypeInfo request for NodeStatus - not used yet + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = NodeStatus::DefaultDataTypeID; + gdti_request.kind.value = DataTypeKind::MESSAGE; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN)); + + /* + * Starting publisher and subscriber for NodeStatus, requesting info again + */ + uavcan::Publisher ns_pub(nodes.a); + SubscriberWithCollector ns_sub(nodes.a); + + ASSERT_LE(0, ns_pub.broadcast(NodeStatus())); + ASSERT_LE(0, ns_sub.start()); + + // Request again + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::FLAG_KNOWN | + GetDataTypeInfo::Response::FLAG_PUBLISHING | + GetDataTypeInfo::Response::FLAG_SUBSCRIBED)); + + /* + * Requesting a non-existent type + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = 20000; + gdti_request.kind.value = 3; // INVALID VALUE + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(gdti_cln.collector.result.get()); + ASSERT_TRUE(gdti_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().flags); + ASSERT_TRUE(gdti_cln.collector.result->getResponse().name.empty()); // Empty name + ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->getResponse().id); + ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->getResponse().kind.value); + + /* + * Requesting a non-existent type by name + */ + gdti_request = GetDataTypeInfo::Request(); + gdti_request.id = 999; // Intentionally wrong + gdti_request.kind.value = 3; // Intentionally wrong + gdti_request.name = "uavcan.equipment.gnss.Fix"; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(gdti_cln.collector.result.get()); + ASSERT_TRUE(gdti_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get()); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().flags); + ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->getResponse().name); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().id); + ASSERT_EQ(0, gdti_cln.collector.result->getResponse().kind.value); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_client.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_client.cpp new file mode 100644 index 0000000000..eff92c6d72 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_client.cpp @@ -0,0 +1,179 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(DynamicNodeIDClient, Basic) +{ + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + + /* + * Client initialization + */ + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id)); // Empty hardware version is not allowed + + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(unique_id, uavcan::NodeID())); + + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, dnidac.start(unique_id, PreferredNodeID)); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Subscriber (server emulation) + */ + SubscriberWithCollector dynid_sub(nodes.a); + ASSERT_LE(0, dynid_sub.start()); + dynid_sub.subscriber.allowAnonymousTransfers(); + + /* + * Monitoring requests + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + unique_id.begin())); + dynid_sub.collector.msg.reset(); + + // Second - rate is no lower than 0.5 Hz + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + dynid_sub.collector.msg.reset(); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Publisher (server emulation) + */ + uavcan::Publisher dynid_pub(nodes.a); + ASSERT_LE(0, dynid_pub.init()); + + /* + * Sending some some Allocation messages - the timer will keep restarting + */ + for (int i = 0; i < 10; i++) + { + uavcan::protocol::dynamic_node_id::Allocation msg; // Contents of the message doesn't matter + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + /* + * Responding with partially matching unique ID - the client will respond with second-stage request immediately + */ + const uint8_t BytesPerRequest = uavcan::protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(BytesPerRequest); + uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest, msg.unique_id.begin()); + + std::cout << "First-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + unique_id.begin() + BytesPerRequest)); + dynid_sub.collector.msg.reset(); + } + + /* + * Responding with second-stage offer, expecting the last request back + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(BytesPerRequest * 2); + uavcan::copy(unique_id.begin(), unique_id.begin() + BytesPerRequest * 2, msg.unique_id.begin()); + + std::cout << "Second-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + unique_id.begin() + BytesPerRequest * 2)); + dynid_sub.collector.msg.reset(); + } + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Now we have full unique ID for this client received, and it is possible to grant allocation + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(16); + msg.node_id = 72; + uavcan::copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID()); + ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID()); + ASSERT_TRUE(dnidac.isAllocationComplete()); +} + + +TEST(DynamicNodeIDClient, NonPassiveMode) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::DynamicNodeIDClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrLogic, dnidac.start(unique_id)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp new file mode 100644 index 0000000000..07894133dc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -0,0 +1,117 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "event_tracer.hpp" +#include "../helpers.hpp" + + +using uavcan::dynamic_node_id_server::UniqueID; + +class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler +{ + std::vector > requests_; + +public: + bool can_followup; + + AllocationRequestHandler() : can_followup(false) { } + + virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + requests_.push_back(std::pair(unique_id, preferred_node_id)); + } + + virtual bool canPublishFollowupAllocationResponse() const + { + return can_followup; + } + + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + if (requests_.empty()) + { + std::cout << "No pending requests" << std::endl; + return false; + } + + const std::pair pair = requests_.at(requests_.size() - 1U); + requests_.pop_back(); + + if (pair.first != unique_id) + { + std::cout << "Unique ID mismatch" << std::endl; + return false; + } + + if (pair.second != preferred_node_id) + { + std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")" + << std::endl; + return false; + } + + return true; + } + + void reset() { requests_.clear(); } +}; + + +TEST(dynamic_node_id_server_AllocationRequestManager, Basic) +{ + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDClient client(nodes.b); + + /* + * Client initialization + */ + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(unique_id, PreferredNodeID)); + + /* + * Request manager initialization + */ + EventTracer tracer; + AllocationRequestHandler handler; + handler.can_followup = true; + + AllocationRequestManager manager(nodes.a, tracer, handler); + + ASSERT_LE(0, manager.init(uavcan::TransferPriority::OneHigherThanLowest)); + + /* + * Allocation + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(handler.matchAndPopLastRequest(unique_id, PreferredNodeID)); + + ASSERT_LE(0, manager.broadcastAllocationResponse(unique_id, PreferredNodeID)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + /* + * Checking the client + */ + ASSERT_TRUE(client.isAllocationComplete()); + + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp new file mode 100644 index 0000000000..932bf61c06 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "../../helpers.hpp" +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +using uavcan::dynamic_node_id_server::UniqueID; + + +TEST(dynamic_node_id_server_centralized_Server, Basic) +{ + using namespace uavcan::dynamic_node_id_server; + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + EventTracer tracer; + MemoryStorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + UniqueID own_unique_id; + own_unique_id[0] = 0xAA; + own_unique_id[3] = 0xCC; + own_unique_id[7] = 0xEE; + own_unique_id[9] = 0xBB; + + /* + * Server + */ + uavcan::dynamic_node_id_server::CentralizedServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(own_unique_id)); + + ASSERT_EQ(1, server.getNumAllocations()); // Server's own node ID + + /* + * Client + */ + uavcan::DynamicNodeIDClient client(nodes.b); + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(unique_id, PreferredNodeID)); + + /* + * Fire + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); + + ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client +} + + +TEST(dynamic_node_id_server_centralized, ObjectSizes) +{ + using namespace uavcan::dynamic_node_id_server; + std::cout << "centralized::Storage: " << sizeof(centralized::Storage) << std::endl; + std::cout << "centralized::Server: " << sizeof(centralized::Server) << std::endl; + std::cout << "NodeDiscoverer: " << sizeof(NodeDiscoverer) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp new file mode 100644 index 0000000000..c41115653d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_centralized_Storage, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::centralized; + using namespace uavcan::dynamic_node_id_server; + + // No data in the storage - initializing empty + { + MemoryStorageBackend storage; + Storage stor(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, stor.init()); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_EQ(0, stor.getSize()); + + ASSERT_FALSE(stor.isNodeIDOccupied(1)); + ASSERT_FALSE(stor.isNodeIDOccupied(0)); + } + // Nonempty storage, many items + { + MemoryStorageBackend storage; + Storage stor(storage); + + storage.set("occupation_mask", "0e000000000000000000000000000000"); // node ID 1, 2, 3 + ASSERT_LE(0, stor.init()); // OK + + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ(3, stor.getSize()); + + ASSERT_TRUE(stor.isNodeIDOccupied(1)); + ASSERT_TRUE(stor.isNodeIDOccupied(2)); + ASSERT_TRUE(stor.isNodeIDOccupied(3)); + ASSERT_FALSE(stor.isNodeIDOccupied(0)); + ASSERT_FALSE(stor.isNodeIDOccupied(4)); + + UniqueID uid_1; + uid_1[0] = 1; + + UniqueID uid_2; + uid_2[0] = 2; + + UniqueID uid_3; + uid_3[0] = 3; + + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_1).isValid()); + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_2).isValid()); + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_3).isValid()); + ASSERT_FALSE(stor.isNodeIDOccupied(127)); + + storage.set("01000000000000000000000000000000", "1"); + storage.set("02000000000000000000000000000000", "2"); + storage.set("03000000000000000000000000000000", "3"); + + ASSERT_EQ(1, stor.getNodeIDForUniqueID(uid_1).get()); + ASSERT_EQ(2, stor.getNodeIDForUniqueID(uid_2).get()); + ASSERT_EQ(3, stor.getNodeIDForUniqueID(uid_3).get()); + ASSERT_FALSE(stor.isNodeIDOccupied(127)); + } +} + + +TEST(dynamic_node_id_server_centralized_Storage, Basic) +{ + using namespace uavcan::dynamic_node_id_server::centralized; + + MemoryStorageBackend storage; + Storage stor(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, stor.init()); + storage.print(); + ASSERT_EQ(0, storage.getNumKeys()); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + uavcan::dynamic_node_id_server::UniqueID unique_id; + unique_id[0] = 1; + ASSERT_LE(0, stor.add(1, unique_id)); + + ASSERT_EQ("02000000000000000000000000000000", storage.get("occupation_mask")); + ASSERT_EQ("1", storage.get("01000000000000000000000000000000")); + + ASSERT_EQ(2, storage.getNumKeys()); + ASSERT_EQ(1, stor.getSize()); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(2, storage.getNumKeys()); + + unique_id[0] = 2; + ASSERT_GT(0, stor.add(2, unique_id)); + + ASSERT_EQ(2, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, stor.getSize()); + + /* + * Adding a lot of entries + */ + storage.failOnSetCalls(false); + + uavcan::NodeID node_id(2); + while (stor.getSize() < 127) + { + ASSERT_LE(0, stor.add(node_id, unique_id)); + + ASSERT_TRUE(stor.getNodeIDForUniqueID(unique_id).isValid()); + ASSERT_TRUE(stor.isNodeIDOccupied(node_id)); + + node_id = uint8_t(uavcan::min(node_id.get() + 1U, 127U)); + unique_id[0] = uint8_t(unique_id[0] + 1U); + } + + storage.print(); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp new file mode 100644 index 0000000000..4b13b34dc7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp @@ -0,0 +1,262 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +TEST(dynamic_node_id_server_ClusterManager, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + const unsigned MaxClusterSize = + uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + + /* + * Simple initialization + */ + { + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + // Too big + ASSERT_GT(0, mgr.init(MaxClusterSize + 1, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + ASSERT_LE(0, mgr.init(5, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ("5", storage.get("cluster_size")); + + // Testing other states + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_EQ(5, mgr.getClusterSize()); + ASSERT_EQ(3, mgr.getQuorumSize()); + ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid()); + } + /* + * Recovery from the storage + */ + { + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + // Not configured + ASSERT_GT(0, mgr.init(0, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + storage.set("cluster_size", "5"); + ASSERT_LE(0, mgr.init(0, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(1, storage.getNumKeys()); + } +} + + +TEST(dynamic_node_id_server_ClusterManager, OneServer) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(1, uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_TRUE(mgr.isClusterDiscovered()); + + ASSERT_EQ(0, nodes.a.internal_failure_count); + + /* + * Broadcasting discovery with wrong cluster size, it will be reported as internal failure + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 2; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(1, nodes.a.internal_failure_count); + + /* + * Discovery rate limiting test + */ + ASSERT_FALSE(sub.collector.msg.get()); + + msg = uavcan::protocol::dynamic_node_id::server::Discovery(); + msg.configured_cluster_size = 1; // Correct value + ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + // Rinse repeat + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); +} + + +TEST(dynamic_node_id_server_ClusterManager, ThreeServers) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(3, uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_FALSE(mgr.isClusterDiscovered()); + + /* + * Discovery publishing rate check + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + /* + * Discovering other nodes + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 3; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(2, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + sub.collector.msg.reset(); + + ASSERT_FALSE(mgr.isClusterDiscovered()); + + // This will complete the discovery + msg.known_nodes.push_back(127U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(3, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + ASSERT_EQ(127, sub.collector.msg->known_nodes[2]); + sub.collector.msg.reset(); + + // Making sure discovery is now terminated + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(sub.collector.msg.get()); + + /* + * Checking Raft states + */ + ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0)); + ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1)); + ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2)); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); + + mgr.setServerMatchIndex(2, 10); + ASSERT_EQ(10, mgr.getServerMatchIndex(2)); + + mgr.incrementServerNextIndexBy(2, 5); + ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2)); + mgr.decrementServerNextIndex(2); + ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2)); + + mgr.resetAllServerIndices(); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp new file mode 100644 index 0000000000..e5d7339410 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp @@ -0,0 +1,243 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry + + +TEST(dynamic_node_id_server_Log, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + // No log data in the storage - initializing empty log + { + MemoryStorageBackend storage; + uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + } + // Nonempty storage, one item + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + } + // Nonempty storage, broken data + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "foobar"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "128"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "128"); // Bad value (127 max) + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(4, storage.getNumKeys()); + } + // Nonempty storage, many items + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "1"); // 2 items - 0, 1 + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + storage.set("log1_term", "1"); + storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef"); + storage.set("log1_node_id", "127"); + + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(7, storage.getNumKeys()); + ASSERT_EQ(1, log.getLastIndex()); + + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + + ASSERT_EQ(1, log.getEntryAtIndex(1)->term); + ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id); + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid; + uid[0] = 0x01; + uid[1] = 0x23; + uid[2] = 0x45; + uid[3] = 0x67; + uid[4] = 0x89; + uid[5] = 0xab; + uid[6] = 0xcd; + uid[7] = 0xef; + uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8); + ASSERT_EQ(uid, log.getEntryAtIndex(1)->unique_id); + } +} + + +TEST(dynamic_node_id_server_Log, Append) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + storage.print(); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + + /* + * Entry at the index 0 always exists, and it's always zero-initialized. + */ + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("log0_term")); + ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id")); + ASSERT_EQ("0", storage.get("log0_node_id")); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("log1_term")); + ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id")); + ASSERT_EQ("1", storage.get("log1_node_id")); + + ASSERT_EQ(1, log.getLastIndex()); + ASSERT_TRUE(entry == *log.getEntryAtIndex(1)); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(7, storage.getNumKeys()); + + entry.term = 2; + entry.node_id = 2; + entry.unique_id[0] = 2; + ASSERT_GT(0, log.append(entry)); + + ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, log.getLastIndex()); + + /* + * Making sure append() fails when the log is full + */ + storage.failOnSetCalls(false); + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, log.append(entry)); // Failing because full + + storage.print(); +} + + +TEST(dynamic_node_id_server_Log, Remove) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + + /* + * Filling the log fully + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + /* + * Removal will fail as the storage is failing to update + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + /* + * Now removal must work + */ + storage.failOnSetCalls(false); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); + ASSERT_EQ(59, log.getLastIndex()); + ASSERT_EQ("59", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30)); + ASSERT_EQ(30, log.getLastIndex()); + ASSERT_EQ("30", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1)); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ("0", storage.get("log_last_index")); + + storage.print(); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp new file mode 100644 index 0000000000..a47fa81585 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../event_tracer.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_PersistentState, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + /* + * First initialization + */ + { + MemoryStorageBackend storage; + PersistentState pers(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - only empty log is recovered + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + PersistentState pers(storage, tracer); + + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - log and current term are recovered + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + storage.set("current_term", "1"); + + PersistentState pers(storage, tracer); + + ASSERT_GT(0, pers.init()); // Fails because current term is not zero + + storage.set("current_term", "0"); + + ASSERT_LE(0, pers.init()); // OK now + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Full recovery + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + } + ASSERT_LE(4, storage.getNumKeys()); + + PersistentState pers(storage, tracer); + + ASSERT_GT(0, pers.init()); // Fails because log is not empty + + storage.set("current_term", "0"); + storage.set("voted_for", "0"); + ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm + + storage.set("current_term", "1"); // OK + storage.set("voted_for", "128"); // Invalid value + ASSERT_GT(0, pers.init()); // Fails because of bad votedFor + + storage.set("voted_for", "0"); // OK now + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } +} + + +TEST(dynamic_node_id_server_PersistentState, Basic) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + PersistentState pers(storage, tracer); + + /* + * Initializing + */ + ASSERT_LE(0, pers.init()); + + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Inserting some log entries + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, pers.getLog().append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing current term + */ + ASSERT_EQ(0, pers.getCurrentTerm()); + ASSERT_LE(0, pers.setCurrentTerm(2)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing votedFor + */ + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(0)); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_TRUE(pers.isVotedForSet()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + ASSERT_LE(0, pers.resetVotedFor()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ("0", storage.get("voted_for")); + + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_TRUE(pers.isVotedForSet()); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Handling errors + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(2, pers.getCurrentTerm()); + ASSERT_GT(0, pers.setCurrentTerm(7893)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_GT(0, pers.setVotedFor(78)); + ASSERT_EQ(45, pers.getVotedFor().get()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Final checks + */ + ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp new file mode 100644 index 0000000000..e09fadda52 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -0,0 +1,193 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include "../event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +using uavcan::dynamic_node_id_server::UniqueID; + + +class CommitHandler : public uavcan::dynamic_node_id_server::distributed::IRaftLeaderMonitor +{ + const std::string id_; + + virtual void handleLogCommitOnLeader(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + { + std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; + } + + virtual void handleLocalLeadershipChange(bool local_node_is_leader) + { + std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + } + +public: + CommitHandler(const std::string& id) : id_(id) { } +}; + + +TEST(dynamic_node_id_server_RaftCore, Basic) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + EventTracer tracer_a("a"); + EventTracer tracer_b("b"); + MemoryStorageBackend storage_a; + MemoryStorageBackend storage_b; + CommitHandler commit_handler_a("a"); + CommitHandler commit_handler_b("b"); + + InterlinkedTestNodesWithSysClock nodes; + + std::unique_ptr raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + std::unique_ptr raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b)); + + /* + * Initialization + */ + ASSERT_LE(0, raft_a->init(2, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_LE(0, raft_b->init(2, uavcan::TransferPriority::OneHigherThanLowest)); + + /* + * Running and trying not to fall + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); + + // Either must become a leader + ASSERT_TRUE(raft_a->isLeader() || raft_b->isLeader()); + + ASSERT_EQ(0, raft_a->getCommitIndex()); + ASSERT_EQ(0, raft_b->getCommitIndex()); + + /* + * Adding some stuff + */ + Entry::FieldTypes::unique_id unique_id; + uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); + + (raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); + + /* + * Terminating the leader + */ + raft_a.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); + + /* + * Reinitializing the leader - current Follower will become the new Leader + */ + storage_a.reset(); + + raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + ASSERT_LE(0, raft_a->init(2, uavcan::TransferPriority::OneHigherThanLowest)); + ASSERT_EQ(0, raft_a->getCommitIndex()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); + + ASSERT_FALSE(raft_a->isLeader()); + ASSERT_TRUE(raft_b->isLeader()); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); +} + + +TEST(dynamic_node_id_server_Server, Basic) +{ + using namespace uavcan::dynamic_node_id_server; + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + + EventTracer tracer; + MemoryStorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + UniqueID own_unique_id; + own_unique_id[0] = 0xAA; + own_unique_id[3] = 0xCC; + own_unique_id[7] = 0xEE; + own_unique_id[9] = 0xBB; + + /* + * Server + */ + DistributedServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(own_unique_id, 1)); + + ASSERT_EQ(0, server.getNumAllocations()); + + /* + * Client + */ + uavcan::DynamicNodeIDClient client(nodes.b); + uavcan::protocol::HardwareVersion::FieldTypes::unique_id unique_id; + for (uavcan::uint8_t i = 0; i < unique_id.size(); i++) + { + unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(unique_id, PreferredNodeID)); + + /* + * Fire + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); + + ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client +} + + +TEST(dynamic_node_id_server, ObjectSizes) +{ + using namespace uavcan; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server; + + std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl; + std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl; + std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl; + std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl; + std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl; + std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl; + + std::cout << "ServiceServer: " << sizeof(ServiceServer) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "ServiceServer: " << sizeof(ServiceServer) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event.cpp new file mode 100644 index 0000000000..7405f5d850 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "event_tracer.hpp" + + +TEST(dynamic_node_id_server_EventTracer, EventCodeToString) +{ + using namespace uavcan::dynamic_node_id_server; + + // Simply checking some error codes + ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError)); + ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); + ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived)); + ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected)); + ASSERT_STREQ("AllocationUnexpectedStage", IEventTracer::getEventName(TraceAllocationUnexpectedStage)); +} + + +TEST(dynamic_node_id_server_EventTracer, EnvironmentSelfTest) +{ + using namespace uavcan::dynamic_node_id_server; + + EventTracer tracer; + + ASSERT_EQ(0, tracer.getNumEvents()); + + tracer.onEvent(TraceRaftAppendEntriesCallFailure, 123); + ASSERT_EQ(1, tracer.getNumEvents()); + tracer.onEvent(TraceRaftAppendEntriesCallFailure, -456); + ASSERT_EQ(2, tracer.getNumEvents()); + tracer.onEvent(TraceError, -0xFFFFFFFFFFFFFFFLL); + ASSERT_EQ(3, tracer.getNumEvents()); + + ASSERT_EQ(0, tracer.countEvents(TraceAllocationActivity)); + ASSERT_EQ(2, tracer.countEvents(TraceRaftAppendEntriesCallFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceError)); + + ASSERT_EQ(-456, tracer.getLastEventArgumentOrFail(TraceRaftAppendEntriesCallFailure)); + + ASSERT_EQ(-0xFFFFFFFFFFFFFFFLL, tracer.getLastEventArgumentOrFail(TraceError)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp new file mode 100644 index 0000000000..b2c30ce6ab --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include "../../clock.hpp" + + +class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer +{ + struct EventLogEntry + { + const uavcan::dynamic_node_id_server::TraceCode code; + const uavcan::int64_t argument; + + EventLogEntry(uavcan::dynamic_node_id_server::TraceCode arg_code, uavcan::int64_t arg_argument) + : code(arg_code) + , argument(arg_argument) + { } + }; + + const std::string id_; + const uavcan::MonotonicTime startup_ts_; + std::list event_log_; + +public: + EventTracer() : + startup_ts_(SystemClockDriver().getMonotonic()) + { } + + EventTracer(const std::string& id) : + id_(id), + startup_ts_(SystemClockDriver().getMonotonic()) + { } + + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + const uavcan::MonotonicDuration ts = SystemClockDriver().getMonotonic() - startup_ts_; + std::cout << "EVENT [" << id_ << "]\t" << ts.toString() << "\t" + << int(code) << "\t" << getEventName(code) << "\t" << argument << std::endl; + event_log_.push_back(EventLogEntry(code, argument)); + } + + unsigned countEvents(const uavcan::dynamic_node_id_server::TraceCode code) const + { + unsigned count = 0; + for (std::list::const_iterator it = event_log_.begin(); it != event_log_.end(); ++it) + { + count += (it->code == code) ? 1U : 0U; + } + return count; + } + + uavcan::int64_t getLastEventArgumentOrFail(const uavcan::dynamic_node_id_server::TraceCode code) const + { + for (std::list::const_reverse_iterator it = event_log_.rbegin(); it != event_log_.rend(); ++it) + { + if (it->code == code) + { + return it->argument; + } + } + + std::cout << "No such event in the event log, code " << int(code) << ", log length " << event_log_.size() + << std::endl; + + throw std::runtime_error("EventTracer::getLastEventArgumentOrFail()"); + } + + unsigned getNumEvents() const { return static_cast(event_log_.size()); } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp new file mode 100644 index 0000000000..e1da5b02ba --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +class GetNodeInfoMockServer +{ + typedef uavcan::MethodBinder&, + uavcan::protocol::GetNodeInfo::Response&) const> + GetNodeInfoCallback; + + uavcan::ServiceServer server_; + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::protocol::GetNodeInfo::Response& res) const + { + res = response; + std::cout << "GET NODE INFO:\nREQUEST\n" << req << "RESPONSE\n" << res << std::endl; + } + +public: + uavcan::protocol::GetNodeInfo::Response response; + + GetNodeInfoMockServer(uavcan::INode& node) : server_(node) { } + + int start() { return server_.start(GetNodeInfoCallback(this, &GetNodeInfoMockServer::handleRequest)); } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp new file mode 100644 index 0000000000..516c33e994 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include + +class MemoryStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend +{ + typedef std::map Container; + Container container_; + + bool fail_; + +public: + MemoryStorageBackend() + : fail_(false) + { } + + virtual String get(const String& key) const + { + const Container::const_iterator it = container_.find(key); + if (it == container_.end()) + { + return String(); + } + return it->second; + } + + virtual void set(const String& key, const String& value) + { + if (!fail_) + { + container_[key] = value; + } + } + + void failOnSetCalls(bool really) { fail_ = really; } + + void reset() { container_.clear(); } + + unsigned getNumKeys() const { return unsigned(container_.size()); } + + void print() const + { + for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it) + { + std::cout << it->first.c_str() << "\t" << it->second.c_str() << std::endl; + } + } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp new file mode 100644 index 0000000000..64df33baec --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include "event_tracer.hpp" +#include "get_node_info_mock_server.hpp" +#include "../helpers.hpp" + +using namespace uavcan::dynamic_node_id_server; + + +class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler +{ +public: + struct NodeInfo + { + UniqueID unique_id; + uavcan::NodeID node_id; + bool committed; + + NodeInfo() : committed(false) { } + }; + + bool can_discover; + std::vector nodes; + + NodeDiscoveryHandler() : can_discover(false) { } + + virtual bool canDiscoverNewNodes() const + { + return can_discover; + } + + virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const + { + const NodeInfo* const ni = const_cast(this)->findNode(node_id); + if (ni == UAVCAN_NULLPTR) + { + return NodeAwarenessUnknown; + } + return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted; + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id) + { + NodeInfo info; + if (unique_id_or_null != UAVCAN_NULLPTR) + { + info.unique_id = *unique_id_or_null; + } + info.node_id = node_id; + nodes.push_back(info); + } + + NodeInfo* findNode(const UniqueID& unique_id) + { + for (unsigned i = 0; i < nodes.size(); i++) + { + if (nodes.at(i).unique_id == unique_id) + { + return &nodes.at(i); + } + } + return UAVCAN_NULLPTR; + } + + NodeInfo* findNode(const uavcan::NodeID& node_id) + { + for (unsigned i = 0; i < nodes.size(); i++) + { + if (nodes.at(i).node_id == node_id) + { + return &nodes.at(i); + } + } + return UAVCAN_NULLPTR; + } +}; + + +TEST(dynamic_node_id_server_NodeDiscoverer, Basic) +{ + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + EventTracer tracer; + InterlinkedTestNodesWithSysClock nodes; + NodeDiscoveryHandler handler; + + NodeDiscoverer disc(nodes.a, tracer, handler); + + /* + * Initialization + */ + ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus, discovery is disabled + */ + std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl; + handler.can_discover = false; + + uavcan::Publisher node_status_pub(nodes.b); + ASSERT_LE(0, node_status_pub.init()); + + uavcan::protocol::NodeStatus node_status; + node_status.uptime_sec = 0; + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); // The timer runs as long as there are unknown nodes + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled! + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving + */ + std::cout << "!!! Enabling discovery" << std::endl; + handler.can_discover = true; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1150)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus + */ + std::cout << "!!! Publishing NodeStatus" << std::endl; + + node_status.uptime_sec += 5U; + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1250)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized + */ + std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl; + + GetNodeInfoMockServer get_node_info_server(nodes.b); + get_node_info_server.response.hardware_version.unique_id[0] = 123; // Arbitrary data + get_node_info_server.response.hardware_version.unique_id[6] = 213; + get_node_info_server.response.hardware_version.unique_id[14] = 52; + ASSERT_LE(0, get_node_info_server.start()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Checking the results + */ + ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id)); + ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get()); +} + + +TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) +{ + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + EventTracer tracer; + InterlinkedTestNodesWithSysClock nodes; + NodeDiscoveryHandler handler; + + NodeDiscoverer disc(nodes.a, tracer, handler); + + /* + * Initialization + */ + ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest)); + + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus once to trigger querying + * Querying for 2 seconds, no responses will be sent (there's no server) + */ + handler.can_discover = true; + + uavcan::Publisher node_status_pub(nodes.b); + ASSERT_LE(0, node_status_pub.init()); + + uavcan::protocol::NodeStatus node_status; + node_status.uptime_sec = 10; // Nonzero + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Emulating node restart + */ + node_status.uptime_sec = 9; // Less than previous + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Waiting for timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Checking the results + */ + ASSERT_TRUE(handler.findNode(UniqueID())); + ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get()); +} + + +TEST(dynamic_node_id_server_NodeDiscoverer, Sizes) +{ + using namespace uavcan; + + std::cout << "BitSet: " << sizeof(BitSet) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "protocol::GetNodeInfo::Response: " << sizeof(protocol::GetNodeInfo::Response) << std::endl; + std::cout << "Subscriber: " << sizeof(Subscriber) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp new file mode 100644 index 0000000000..53be558259 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "memory_storage_backend.hpp" + +TEST(dynamic_node_id_server_StorageMarshaller, Basic) +{ + MemoryStorageBackend st; + + uavcan::dynamic_node_id_server::StorageMarshaller marshaler(st); + + uavcan::dynamic_node_id_server::IStorageBackend::String key; + + /* + * uint32 + */ + uint32_t u32 = 0; + + key = "foo"; + u32 = 0; + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0, u32); + + key = "bar"; + u32 = 0xFFFFFFFF; + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + ASSERT_LE(0, marshaler.get(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + + key = "foo"; + ASSERT_LE(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + key = "the_cake_is_a_lie"; + ASSERT_GT(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + /* + * uint8[16] + */ + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id array; + + key = "a"; + // Set zero + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(0, array[i]); + } + + // Make sure this will not be interpreted as uint32 + ASSERT_GT(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + // Set pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i + 1); + } + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(i + 1, array[i]); + } + + // Set another pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i | (i << 4)); + } + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(uint8_t(i | (i << 4)), array[i]); + } + + // Make sure uint32 cannot be interpreted as an array + key = "foo"; + ASSERT_GT(0, marshaler.get(key, array)); + + // Nonexistent key + key = "the_cake_is_a_lie"; + ASSERT_GT(0, marshaler.get(key, array)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/file_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/file_server.cpp new file mode 100644 index 0000000000..be970b4d04 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/file_server.cpp @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +class TestFileServerBackend : public uavcan::IFileServerBackend +{ +public: + static const std::string file_name; + static const std::string file_data; + + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) + { + if (path == file_name) + { + out_size = uint16_t(file_data.length()); + out_type.flags |= EntryType::FLAG_FILE; + out_type.flags |= EntryType::FLAG_READABLE; + return 0; + } + else + { + return Error::NOT_FOUND; + } + } + + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) + { + if (path == file_name) + { + if (offset < file_data.length()) + { + inout_size = uint16_t(file_data.length() - offset); + std::memcpy(out_buffer, file_data.c_str() + offset, inout_size); + } + else + { + inout_size = 0; + } + return 0; + } + else + { + return Error::NOT_FOUND; + } + } +}; + +const std::string TestFileServerBackend::file_name = "test"; +const std::string TestFileServerBackend::file_data = "123456789"; + +TEST(BasicFileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + TestFileServerBackend backend; + + uavcan::BasicFileServer serv(nodes.a, backend); + std::cout << "sizeof(uavcan::BasicFileServer): " << sizeof(uavcan::BasicFileServer) << std::endl; + + ASSERT_LE(0, serv.start()); + + /* + * GetInfo, existing file + */ + { + ServiceClientWithCollector get_info(nodes.b); + + GetInfo::Request get_info_req; + get_info_req.path.path = "test"; + + ASSERT_LE(0, get_info.call(1, get_info_req)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(get_info.collector.result.get()); + ASSERT_TRUE(get_info.collector.result->isSuccessful()); + + ASSERT_EQ(0, get_info.collector.result->getResponse().error.value); + ASSERT_EQ(9, get_info.collector.result->getResponse().size); + ASSERT_EQ(EntryType::FLAG_FILE | EntryType::FLAG_READABLE, + get_info.collector.result->getResponse().entry_type.flags); + } + + /* + * Read, existing file + */ + { + ServiceClientWithCollector read(nodes.b); + + Read::Request read_req; + read_req.path.path = "test"; + + ASSERT_LE(0, read.call(1, read_req)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(read.collector.result.get()); + ASSERT_TRUE(read.collector.result->isSuccessful()); + + ASSERT_EQ("123456789", read.collector.result->getResponse().data); + ASSERT_EQ(0, read.collector.result->getResponse().error.value); + } +} + + +TEST(FileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + + InterlinkedTestNodesWithSysClock nodes; + + TestFileServerBackend backend; + + uavcan::FileServer serv(nodes.a, backend); + std::cout << "sizeof(uavcan::FileServer): " << sizeof(uavcan::FileServer) << std::endl; + + ASSERT_LE(0, serv.start()); + + // TODO TEST +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/firmware_update_trigger.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/firmware_update_trigger.cpp new file mode 100644 index 0000000000..a17e92c442 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -0,0 +1,336 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +using namespace uavcan::protocol::file; + +struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + unsigned should_request_cnt; + unsigned should_retry_cnt; + unsigned confirmation_cnt; + + std::string firmware_path; + + int retry_quota; + std::string expected_node_name_to_update; + + BeginFirmwareUpdate::Response last_error_response; + + FirmwareVersionChecker() + : should_request_cnt(0) + , should_retry_cnt(0) + , confirmation_cnt(0) + , retry_quota(0) + { } + + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + should_request_cnt++; + std::cout << "REQUEST? " << int(node_id.get()) << "\n" << node_info << std::endl; + out_firmware_file_path = firmware_path.c_str(); + return node_info.name == expected_node_name_to_update; + } + + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + last_error_response = error_response; + std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; + should_retry_cnt++; + + EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str()); + + if (retry_quota > 0) + { + retry_quota--; + return true; + } + else + { + return false; + } + } + + virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& response) + { + confirmation_cnt++; + std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl; + } +}; + +struct BeginFirmwareUpdateServer +{ + uint8_t response_error_code; + + BeginFirmwareUpdateServer() : response_error_code(0) { } + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& res) const + { + std::cout << "REQUEST\n" << req << std::endl; + res.error = response_error_code; + res.optional_error_message = "foobar"; + } + + typedef uavcan::MethodBinder&, + uavcan::ServiceResponseDataStructure&) const> Callback; + + Callback makeCallback() { return Callback(this, &BeginFirmwareUpdateServer::handleRequest); } +}; + + +TEST(FirmwareUpdateTrigger, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + InterlinkedTestNodesWithSysClock nodes; + + FirmwareVersionChecker checker; + + uavcan::NodeInfoRetriever node_info_retriever(nodes.a); // On the same node + + uavcan::FirmwareUpdateTrigger trigger(nodes.a, checker); + std::cout << "sizeof(uavcan::FirmwareUpdateTrigger): " << sizeof(uavcan::FirmwareUpdateTrigger) << std::endl; + + std::unique_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); // Other node + + /* + * Initializing + */ + ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/")); + + ASSERT_LE(0, node_info_retriever.start()); + ASSERT_EQ(1, node_info_retriever.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Updating one node + * The server that can confirm the request is not running yet + */ + checker.firmware_path = "firmware_path"; + checker.expected_node_name_to_update = "Ivan"; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Starting the firmware update server that returns an error + * The checker will instruct the trigger to repeat + */ + uavcan::ServiceServer srv(nodes.b); + BeginFirmwareUpdateServer srv_impl; + + ASSERT_LE(0, srv.start(srv_impl.makeCallback())); + + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Trying again, this time with ERROR_IN_PROGRESS + */ + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.retry_quota = 0; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Restarting the node info provider + * Now it doesn't need an update + */ + provider.reset(new uavcan::NodeStatusProvider(nodes.b)); + + provider->setName("Dmitry"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(2, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Final checks + */ + ASSERT_EQ(0, nodes.a.internal_failure_count); + ASSERT_EQ(0, nodes.b.internal_failure_count); +} + + +TEST(FirmwareUpdateTrigger, MultiNode) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + TestNetwork<5> nodes; + + // The trigger node + FirmwareVersionChecker checker; + uavcan::NodeInfoRetriever node_info_retriever(nodes[0]); + uavcan::FirmwareUpdateTrigger trigger(nodes[0], checker); + + // The client nodes + std::unique_ptr provider_a(new uavcan::NodeStatusProvider(nodes[1])); + std::unique_ptr provider_b(new uavcan::NodeStatusProvider(nodes[2])); + std::unique_ptr provider_c(new uavcan::NodeStatusProvider(nodes[3])); + std::unique_ptr provider_d(new uavcan::NodeStatusProvider(nodes[4])); + + uavcan::protocol::HardwareVersion hwver; + + /* + * Initializing + */ + ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/")); + + ASSERT_LE(0, node_info_retriever.start()); + ASSERT_EQ(1, node_info_retriever.getNumListeners()); + + hwver.unique_id[0] = 0xAA; + provider_a->setHardwareVersion(hwver); + provider_a->setName("Victor"); + ASSERT_LE(0, provider_a->startAndPublish()); + + hwver.unique_id[0] = 0xBB; + provider_b->setHardwareVersion(hwver); + provider_b->setName("Victor"); + ASSERT_LE(0, provider_b->startAndPublish()); + + hwver.unique_id[0] = 0xCC; + provider_c->setHardwareVersion(hwver); + provider_c->setName("Alexey"); + ASSERT_LE(0, provider_c->startAndPublish()); + + hwver.unique_id[0] = 0xDD; + provider_d->setHardwareVersion(hwver); + provider_d->setName("Victor"); + ASSERT_LE(0, provider_d->startAndPublish()); + + checker.expected_node_name_to_update = "Victor"; // Victors will get updated, others will not + checker.firmware_path = "abc"; + + /* + * Running - 3 will timout, 1 will be ignored + */ + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(3, trigger.getNumPendingNodes()); + + ASSERT_EQ(4, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + /* + * Initializing the BeginFirmwareUpdate servers + */ + uavcan::ServiceServer srv_a(nodes[1]); + uavcan::ServiceServer srv_b(nodes[2]); + uavcan::ServiceServer srv_c(nodes[3]); + uavcan::ServiceServer srv_d(nodes[4]); + + BeginFirmwareUpdateServer srv_a_impl; + BeginFirmwareUpdateServer srv_b_impl; + BeginFirmwareUpdateServer srv_c_impl; + BeginFirmwareUpdateServer srv_d_impl; + + ASSERT_LE(0, srv_a.start(srv_a_impl.makeCallback())); + ASSERT_LE(0, srv_b.start(srv_b_impl.makeCallback())); + ASSERT_LE(0, srv_c.start(srv_c_impl.makeCallback())); + ASSERT_LE(0, srv_d.start(srv_d_impl.makeCallback())); + + srv_a_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry + srv_b_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry + srv_c_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // ignore (see below) + srv_d_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_OK; // OK + + /* + * Spinning, now we're getting some errors + * This also checks correctness of the round-robin selector + */ + checker.retry_quota = 2; + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200)); // Two will retry, one drop, one confirm + + ASSERT_TRUE(trigger.isTimerRunning()); + + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed now + + EXPECT_EQ(4, checker.should_request_cnt); + EXPECT_EQ(4, checker.should_retry_cnt); + EXPECT_EQ(1, checker.confirmation_cnt); + + /* + * Waiting for the timer to stop + */ + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_FALSE(trigger.isTimerRunning()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_master.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_master.cpp new file mode 100644 index 0000000000..bed4d908dc --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_master.cpp @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +struct GlobalTimeSyncMasterTestNode +{ + SystemClockDriver clock; + PairableCanDriver can; + TestNode node; + + GlobalTimeSyncMasterTestNode(uavcan::NodeID nid) + : can(clock) + , node(can, clock, nid) + { } +}; + +struct GlobalTimeSyncTestNetwork +{ + GlobalTimeSyncMasterTestNode slave; + GlobalTimeSyncMasterTestNode master_low; + GlobalTimeSyncMasterTestNode master_high; + + GlobalTimeSyncTestNetwork() + : slave(64) + , master_low(120) + , master_high(8) + { + slave.can.others.insert(&master_low.can); + master_low.can.others.insert(&slave.can); + master_high.can.others.insert(&slave.can); + } + + void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9)) + { + assert(!duration.isNegative()); + unsigned nspins3 = unsigned(duration.toMSec() / 3); + nspins3 = nspins3 ? nspins3 : 2; + while (nspins3 --> 0) + { + ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1))); + ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1))); + ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1))); + } + } +}; + +TEST(GlobalTimeSyncMaster, Basic) +{ + GlobalTimeSyncTestNetwork nwk; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::GlobalTimeSyncSlave slave(nwk.slave.node); + uavcan::GlobalTimeSyncMaster master_low(nwk.master_low.node); + uavcan::GlobalTimeSyncMaster master_high(nwk.master_high.node); + + ASSERT_FALSE(master_low.isInitialized()); + + ASSERT_LE(0, slave.start()); + ASSERT_LE(0, master_low.init()); + ASSERT_LE(0, master_high.init()); + + ASSERT_TRUE(master_low.isInitialized()); + ASSERT_FALSE(slave.isActive()); + + /* + * Simple synchronization + */ + ASSERT_LE(0, master_low.publish()); // Update + nwk.spinAll(); + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Adjustment + nwk.spinAll(); + + // Synchronization complete. + ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc())); + ASSERT_TRUE(slave.isActive()); + ASSERT_EQ(nwk.master_low.node.getNodeID(), slave.getMasterNodeID()); + + /* + * Moving clocks forward and re-syncing with another master + */ + static const uavcan::UtcDuration OneDay = uavcan::UtcDuration::fromMSec(24 * 3600 * 1000); + nwk.master_high.clock.utc_adjustment = OneDay; + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Update from the old master + nwk.spinAll(); + + ASSERT_LE(0, master_high.publish()); // Update from the new master + nwk.spinAll(); + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Adjustment from the old master (ignored now) + ASSERT_LE(0, master_high.publish()); // Adjustment from the new master (accepted) + nwk.spinAll(); + + // Synchronization complete. + ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc())); + ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc())); + ASSERT_TRUE(slave.isActive()); + ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID()); + + /* + * Frequent calls to publish() + */ + ASSERT_LE(0, master_low.publish()); // Dropped + ASSERT_LE(0, master_low.publish()); // Dropped + ASSERT_LE(0, master_low.publish()); // Dropped + + ASSERT_TRUE(nwk.slave.can.read_queue.empty()); + + usleep(400000); + ASSERT_LE(0, master_low.publish()); // Accepted + ASSERT_FALSE(nwk.slave.can.read_queue.empty()); + + nwk.spinAll(); + + // Synchronization did not change + ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc())); + ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc())); + ASSERT_TRUE(slave.isActive()); + ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_slave.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_slave.cpp new file mode 100644 index 0000000000..0ac4dda883 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/global_time_sync_slave.cpp @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + + +TEST(GlobalTimeSyncSlave, Basic) +{ + InterlinkedTestNodesWithClockMock nodes(64, 65); + + SystemClockMock& slave_clock = nodes.clock_a; + SystemClockMock& master_clock = nodes.clock_b; + + slave_clock.advance(1000000); + master_clock.advance(1000000); + + master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000; + master_clock.preserve_utc = slave_clock.preserve_utc = true; + slave_clock.utc = 0; // Not set yet + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::GlobalTimeSyncSlave gtss(nodes.a); + uavcan::Publisher gts_pub(nodes.b); + + ASSERT_LE(0, gtss.start()); + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); + + /* + * Empty broadcast + * The slave must only register the timestamp and adjust nothing + */ + uavcan::protocol::GlobalTimeSync gts; + gts.previous_transmission_timestamp_usec = 0; + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(0, slave_clock.utc); + ASSERT_EQ(1000000, master_clock.utc); + std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl; + std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl; + + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); + + /* + * Follow-up broadcast with proper time + * Slave must adjust now + */ + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(1000000, slave_clock.utc); + ASSERT_EQ(1000000, master_clock.utc); + std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl; + std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl; + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID()); + + /* + * Next follow-up, slave is synchronized now + * Will update + */ + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(2000000, slave_clock.utc); + ASSERT_EQ(2000000, master_clock.utc); + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID()); + + /* + * Next follow-up, slave is synchronized now + * Will adjust + */ + gts_pub.broadcast(gts); + gts.previous_transmission_timestamp_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(3000000, slave_clock.utc); + ASSERT_EQ(3000000, master_clock.utc); + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + ASSERT_EQ(4000000, slave_clock.utc); + ASSERT_EQ(4000000, master_clock.utc); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID()); + + /* + * Another master + * This one has higher priority, so it will be preferred + */ + SystemClockMock master2_clock(100); + master2_clock.monotonic_auto_advance = 1000; + master2_clock.preserve_utc = true; + PairableCanDriver master2_can(master2_clock); + master2_can.others.insert(&nodes.can_a); + TestNode master2_node(master2_can, master2_clock, 8); + + uavcan::Publisher gts_pub2(master2_node); + + /* + * Update step, no adjustment yet + */ + gts.previous_transmission_timestamp_usec = 0; + gts_pub2.broadcast(gts); + gts.previous_transmission_timestamp_usec = master2_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(4000000, slave_clock.utc); + ASSERT_EQ(100, master2_clock.utc); + + master2_clock.utc += 1000000; + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID()); + + /* + * Adjustment + */ + gts_pub2.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID()); + + /* + * Another master will be ignored now + */ + gts.previous_transmission_timestamp_usec = 99999999; + // Update + gts_pub.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + // Adjust + gts_pub.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID()); + + /* + * Timeout + */ + slave_clock.advance(100000000); + + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); +} + + +#if !defined(BYTE_ORDER) || !defined(LITTLE_ENDIAN) || (BYTE_ORDER != LITTLE_ENDIAN) +# error "This test cannot be executed on this platform" +#endif + +static uavcan::Frame makeSyncMsg(uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid) +{ + uavcan::Frame frame(uavcan::protocol::GlobalTimeSync::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + snid, uavcan::NodeID::Broadcast, tid); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + EXPECT_EQ(7, frame.setPayload(reinterpret_cast(&usec), 7)); // Assuming little endian!!! + return frame; +} + +static void broadcastSyncMsg(CanIfaceMock& iface, uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid) +{ + const uavcan::Frame frame = makeSyncMsg(usec, snid, tid); + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + iface.pushRx(can_frame); +} + + +TEST(GlobalTimeSyncSlave, Validation) +{ + SystemClockMock slave_clock; + slave_clock.monotonic = 1000000; + slave_clock.preserve_utc = true; + + CanDriverMock slave_can(3, slave_clock); + for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++) + { + slave_can.ifaces.at(i).enable_utc_timestamping = true; + } + + TestNode node(slave_can, slave_clock, 64); + + uavcan::GlobalTimeSyncSlave gtss(node); + uavcan::Publisher gts_pub(node); + + ASSERT_LE(0, gtss.start()); + ASSERT_FALSE(gtss.isActive()); + ASSERT_FALSE(gtss.getMasterNodeID().isValid()); + ASSERT_EQ(0, slave_clock.utc); + + /* + * Update/adjust/update + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(1000, slave_clock.utc); + + broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 2); // Update + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_EQ(1000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; + + /* + * TID jump simulates a frame loss + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 4); // Adjustment skipped - expected TID 3, update instead + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(1000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; + + /* + * Valid adjustment - continuing from TID 4 + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 5); // Slave UTC was 1000, master reports 3000 --> shift ahead + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 5); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(3000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; + + /* + * Update, then very long delay with correct TID + */ + broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 6); // Valid update, slave UTC is 3000 + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 6); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + slave_clock.monotonic += 5000000; + + broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 7); // Adjustment skipped + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 7); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 8); // Valid adjustment now + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 8); + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(5000, slave_clock.utc); + std::cout << slave_clock.monotonic << std::endl; +} + + +TEST(GlobalTimeSyncSlave, Suppression) +{ + SystemClockMock slave_clock; + slave_clock.monotonic = 1000000; + slave_clock.preserve_utc = true; + + CanDriverMock slave_can(3, slave_clock); + for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++) + { + slave_can.ifaces.at(i).enable_utc_timestamping = true; + } + + TestNode node(slave_can, slave_clock, 64); + + uavcan::GlobalTimeSyncSlave gtss(node); + uavcan::Publisher gts_pub(node); + + ASSERT_LE(0, gtss.start()); + ASSERT_EQ(0, slave_clock.utc); + + gtss.suppress(true); + + broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead + broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(gtss.isActive()); + ASSERT_EQ(8, gtss.getMasterNodeID().get()); + ASSERT_EQ(0, slave_clock.utc); // The clock shall not be asjusted +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/helpers.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/helpers.hpp new file mode 100644 index 0000000000..e399b483b5 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/helpers.hpp @@ -0,0 +1,159 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include "../node/test_node.hpp" + + +template +class SubscriptionCollector : uavcan::Noncopyable +{ + void handler(const DataType& msg) + { + this->msg.reset(new DataType(msg)); + } + +public: + std::unique_ptr msg; + + typedef uavcan::MethodBinder Binder; + + Binder bind() { return Binder(this, &SubscriptionCollector::handler); } +}; + + +template +struct SubscriberWithCollector +{ + typedef SubscriptionCollector Collector; + typedef uavcan::Subscriber Subscriber; + + Collector collector; + Subscriber subscriber; + + SubscriberWithCollector(uavcan::INode& node) + : subscriber(node) + { } + + int start() { return subscriber.start(collector.bind()); } +}; + + +template +class ServiceCallResultCollector : uavcan::Noncopyable +{ + typedef uavcan::ServiceCallResult ServiceCallResult; + +public: + class Result + { + const typename ServiceCallResult::Status status_; + uavcan::ServiceCallID call_id_; + typename DataType::Response response_; + + public: + Result(typename ServiceCallResult::Status arg_status, + uavcan::ServiceCallID arg_call_id, + const typename DataType::Response& arg_response) + : status_(arg_status) + , call_id_(arg_call_id) + , response_(arg_response) + { } + + bool isSuccessful() const { return status_ == ServiceCallResult::Success; } + + typename ServiceCallResult::Status getStatus() const { return status_; } + + uavcan::ServiceCallID getCallID() const { return call_id_; } + + const typename DataType::Response& getResponse() const { return response_; } + typename DataType::Response& getResponse() { return response_; } + }; + +private: + void handler(const uavcan::ServiceCallResult& tmp_result) + { + std::cout << tmp_result << std::endl; + result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse())); + } + +public: + std::unique_ptr result; + + typedef uavcan::MethodBinder&)> + Binder; + + Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); } +}; + + +template +struct ServiceClientWithCollector +{ + typedef ServiceCallResultCollector Collector; + typedef uavcan::ServiceClient ServiceClient; + + Collector collector; + ServiceClient client; + + ServiceClientWithCollector(uavcan::INode& node) + : client(node) + { } + + int call(uavcan::NodeID node_id, const typename DataType::Request& request) + { + client.setCallback(collector.bind()); + return client.call(node_id, request); + } +}; + + +struct BackgroundSpinner : uavcan::TimerBase +{ + uavcan::INode& spinning_node; + + BackgroundSpinner(uavcan::INode& spinning_node, uavcan::INode& running_node) + : uavcan::TimerBase(running_node) + , spinning_node(spinning_node) + { } + + virtual void handleTimerEvent(const uavcan::TimerEvent&) + { + spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1)); + } +}; + +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id, + const MessageType& message, uavcan::TransferID tid) +{ + uavcan::StaticTransferBuffer<100> buffer; + uavcan::BitStream bitstream(buffer); + uavcan::ScalarCodec codec(bitstream); + + // Manual message publication + ASSERT_LT(0, MessageType::encode(message, codec)); + ASSERT_GE(8, buffer.getMaxWritePos()); + + // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node_id, uavcan::NodeID::Broadcast, tid); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + + ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos())); + + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + + can.pushRxToAllIfaces(can_frame); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/logger.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/logger.cpp new file mode 100644 index 0000000000..1d38cec64b --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/logger.cpp @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +struct LogSink : public uavcan::ILogSink +{ + std::queue msgs; + LogLevel level; + + LogSink() + : level(uavcan::protocol::debug::LogLevel::ERROR) + { } + + LogLevel getLogLevel() const { return level; } + + void log(const uavcan::protocol::debug::LogMessage& message) + { + msgs.push(message); + std::cout << message << std::endl; + } + + uavcan::protocol::debug::LogMessage pop() + { + if (msgs.empty()) + { + std::cout << "LogSink is empty" << std::endl; + std::abort(); + } + const uavcan::protocol::debug::LogMessage m = msgs.front(); + msgs.pop(); + return m; + } + + bool popMatchByLevelAndText(int level, const std::string& source, const std::string& text) + { + if (msgs.empty()) + { + std::cout << "LogSink is empty" << std::endl; + return false; + } + const uavcan::protocol::debug::LogMessage m = pop(); + return + level == m.level.value && + source == m.source && + text == m.text; + } +}; + + +TEST(Logger, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::Logger logger(nodes.a); + + ASSERT_EQ(uavcan::protocol::debug::LogLevel::ERROR, logger.getLevel()); + + LogSink sink; + + // Will fail - types are not registered + uavcan::GlobalDataTypeRegistry::instance().reset(); + ASSERT_GT(0, logger.logError("foo", "Error (fail - type is not registered)")); + ASSERT_EQ(0, logger.logDebug("foo", "Debug (ignored - low logging level)")); + + ASSERT_FALSE(logger.getExternalSink()); + logger.setExternalSink(&sink); + ASSERT_EQ(&sink, logger.getExternalSink()); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + SubscriberWithCollector log_sub(nodes.b); + ASSERT_LE(0, log_sub.start()); + + // Sink test + ASSERT_EQ(0, logger.logDebug("foo", "Debug (ignored due to low logging level)")); + ASSERT_TRUE(sink.msgs.empty()); + + sink.level = uavcan::protocol::debug::LogLevel::DEBUG; + ASSERT_EQ(0, logger.logDebug("foo", "Debug (sink only)")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::DEBUG, "foo", "Debug (sink only)")); + + ASSERT_LE(0, logger.logError("foo", "Error")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(log_sub.collector.msg.get()); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::ERROR); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Error"); + + logger.setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + ASSERT_LE(0, logger.logWarning("foo", "Warning")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::WARNING); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Warning"); + + ASSERT_LE(0, logger.logInfo("foo", "Info")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::INFO); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Info"); + + ASSERT_LE(0, logger.logDebug("foo", "Debug")); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::DEBUG); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "Debug"); + + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::ERROR, "foo", "Error")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::WARNING, "foo", "Warning")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::INFO, "foo", "Info")); + ASSERT_TRUE(sink.popMatchByLevelAndText(uavcan::protocol::debug::LogLevel::DEBUG, "foo", "Debug")); +} + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +TEST(Logger, Cpp11Formatting) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::Logger logger(nodes.a); + logger.setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + SubscriberWithCollector log_sub(nodes.b); + ASSERT_LE(0, log_sub.start()); + + ASSERT_LE(0, logger.logWarning("foo", "char='%*', %* is %*", '$', "double", 12.34)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(log_sub.collector.msg.get()); + ASSERT_EQ(log_sub.collector.msg->level.value, uavcan::protocol::debug::LogLevel::WARNING); + ASSERT_EQ(log_sub.collector.msg->source, "foo"); + ASSERT_EQ(log_sub.collector.msg->text, "char='$', double is 12.34"); +} + +#endif diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_info_retriever.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_info_retriever.cpp new file mode 100644 index 0000000000..f0f1b5a9b4 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_info_retriever.cpp @@ -0,0 +1,291 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include "helpers.hpp" + +static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id, + uavcan::uint32_t uptime_sec, uavcan::TransferID tid) +{ + uavcan::protocol::NodeStatus msg; + msg.health = uavcan::protocol::NodeStatus::HEALTH_OK; + msg.mode = uavcan::protocol::NodeStatus::MODE_OPERATIONAL; + msg.uptime_sec = uptime_sec; + emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); +} + + +struct NodeInfoListener : public uavcan::INodeInfoListener +{ + std::unique_ptr last_node_info; + uavcan::NodeID last_node_id; + unsigned status_message_cnt; + unsigned status_change_cnt; + unsigned info_unavailable_cnt; + + NodeInfoListener() + : status_message_cnt(0) + , status_change_cnt(0) + , info_unavailable_cnt(0) + { } + + virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info) + { + last_node_id = node_id; + std::cout << node_info << std::endl; + last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info)); + } + + virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id) + { + std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl; + last_node_id = node_id; + info_unavailable_cnt++; + } + + virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event) + { + std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: " + << event.old_status.toString() << " --> " << event.status.toString() << std::endl; + status_change_cnt++; + } + + virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) + { + std::cout << msg << std::endl; + status_message_cnt++; + } +}; + + +TEST(NodeInfoRetriever, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + std::unique_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.removeListener(&listener); // Does nothing + retr.addListener(&listener); + retr.addListener(&listener); + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default + + /* + * Waiting for discovery + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(retr.isRetrievingInProgress()); + + ASSERT_EQ(2, listener.status_message_cnt); + ASSERT_EQ(1, listener.status_change_cnt); + ASSERT_EQ(0, listener.info_unavailable_cnt); + ASSERT_TRUE(listener.last_node_info.get()); + ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id); + ASSERT_EQ("Ivan", listener.last_node_info->name); + ASSERT_TRUE(hwver == listener.last_node_info->hardware_version); + + provider.reset(); // Moving the provider out of the way; its entry will timeout meanwhile + + /* + * Declaring a bunch of different nodes that don't support GetNodeInfo + */ + ASSERT_FALSE(retr.isRetrievingInProgress()); + + retr.setNumRequestAttempts(3); + + uavcan::TransferID tid; + + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 10, tid); // Reset + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + EXPECT_EQ(11, listener.status_message_cnt); + EXPECT_EQ(5, listener.status_change_cnt); // node 2 online/offline + 3 test nodes above + EXPECT_EQ(2, listener.info_unavailable_cnt); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); // Still one because two went offline + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 12, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_FALSE(retr.isRetrievingInProgress()); // Out of attempts, stopping + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(13, listener.status_message_cnt); + EXPECT_EQ(7, listener.status_change_cnt); // node 2 online/offline + 2 test nodes above online/offline + 1 + EXPECT_EQ(3, listener.info_unavailable_cnt); + + /* + * Forcing the class to forget everything + */ + std::cout << "Invalidation" << std::endl; + + retr.invalidateAll(); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 60, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 60, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 60, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + ASSERT_EQ(3, retr.getNumPendingRequests()); +} + + +TEST(NodeInfoRetriever, MaxConcurrentRequests) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + ASSERT_EQ(40, retr.getRequestInterval().toMSec()); + + const unsigned MaxPendingRequests = 26; // See class docs + const unsigned MinPendingRequestsAtFullLoad = 12; + + /* + * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit. + */ + for (uint8_t node_id = 1U; node_id <= 127U; node_id++) + { + publishNodeStatus(nodes.can_a, node_id, 0, uavcan::TransferID()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + for (int i = 0; i < 8; i++) // Approximate + { + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35)); + std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl; + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_LT(0, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + ASSERT_EQ(0, retr.getNumPendingRequests()); + ASSERT_FALSE(retr.isRetrievingInProgress()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_monitor.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_monitor.cpp new file mode 100644 index 0000000000..cbe9193cb3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_monitor.cpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, + uavcan::uint8_t health, uavcan::uint8_t mode, + uavcan::uint32_t uptime_sec, uavcan::TransferID tid) +{ + uavcan::protocol::NodeStatus msg; + msg.health = health; + msg.mode = mode; + msg.uptime_sec = uptime_sec; + emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); +} + + +static void shortSpin(TestNode& node) +{ + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); +} + + +TEST(NodeStatusMonitor, Basic) +{ + using uavcan::protocol::NodeStatus; + using uavcan::NodeID; + + SystemClockMock clock_mock(100); + clock_mock.monotonic_auto_advance = 1000; + + CanDriverMock can(2, clock_mock); + + TestNode node(can, clock_mock, 64); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::NodeStatusMonitor nsm(node); + ASSERT_LE(0, nsm.start()); + + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10))); + + /* + * Empty NSM, no nodes were registered yet + */ + ASSERT_FALSE(nsm.findNodeWithWorstHealth().isValid()); + + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(123))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode); + + /* + * Some new status messages + */ + publishNodeStatus(can, 10, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 12, 0); + shortSpin(node); + ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstHealth()); + + publishNodeStatus(can, 9, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_INITIALIZATION, 0, 0); + shortSpin(node); + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth()); + + publishNodeStatus(can, 11, NodeStatus::HEALTH_CRITICAL, NodeStatus::MODE_MAINTENANCE, 999, 0); + shortSpin(node); + ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth()); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + /* + * Timeout + */ + std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl; + + clock_mock.advance(500000); + shortSpin(node); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + clock_mock.advance(500000); + shortSpin(node); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + clock_mock.advance(500000); + shortSpin(node); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + /* + * Will timeout now + */ + clock_mock.advance(4000000); + shortSpin(node); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + /* + * Recovering one node, adding two extra + */ + publishNodeStatus(can, 11, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 999, 1); + shortSpin(node); + + publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1); + shortSpin(node); + + publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1); + shortSpin(node); + + /* + * Making sure OFFLINE is still worst status + */ + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth()); + + /* + * Final validation + */ + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health); + + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health); + + /* + * Forgetting + */ + nsm.forgetNode(127); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(127)).health); + + nsm.forgetNode(9); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(9)).health); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_provider.cpp new file mode 100644 index 0000000000..2d57db3959 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/node_status_provider.cpp @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +struct AdHocNodeStatusUpdater : public uavcan::IAdHocNodeStatusUpdater +{ + uavcan::uint64_t invokations; + + AdHocNodeStatusUpdater() : invokations(0) { } + + virtual void updateNodeStatus() + { + invokations++; + } +}; + + +TEST(NodeStatusProvider, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeStatusProvider nsp(nodes.a); + + /* + * Initialization + */ + uavcan::protocol::HardwareVersion hwver; + hwver.major = 3; + hwver.minor = 14; + + uavcan::protocol::SoftwareVersion swver; + swver.major = 2; + swver.minor = 18; + swver.vcs_commit = 0x600DF00D; + + nsp.setHardwareVersion(hwver); + nsp.setSoftwareVersion(swver); + + ASSERT_TRUE(nsp.getName().empty()); + nsp.setName("superluminal_communication_unit"); + ASSERT_STREQ("superluminal_communication_unit", nsp.getName().c_str()); + + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_OK, nsp.getHealth()); + ASSERT_EQ(uavcan::protocol::NodeStatus::MODE_INITIALIZATION, nsp.getMode()); + nsp.setHealthError(); + nsp.setModeOperational(); + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, nsp.getHealth()); + ASSERT_EQ(uavcan::protocol::NodeStatus::MODE_OPERATIONAL, nsp.getMode()); + + // Will fail - types are not registered + uavcan::GlobalDataTypeRegistry::instance().reset(); + ASSERT_GT(0, nsp.startAndPublish()); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + ASSERT_LE(0, nsp.startAndPublish()); + + // Checking the publishing rate settings + ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS), + nsp.getStatusPublicationPeriod()); + + nsp.setStatusPublicationPeriod(uavcan::MonotonicDuration()); + ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS), + nsp.getStatusPublicationPeriod()); + + nsp.setStatusPublicationPeriod(uavcan::MonotonicDuration::fromMSec(3600 * 1000 * 24)); + ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS), + nsp.getStatusPublicationPeriod()); + + AdHocNodeStatusUpdater ad_hoc; + ASSERT_EQ(UAVCAN_NULLPTR, nsp.getAdHocNodeStatusUpdater()); + nsp.setAdHocNodeStatusUpdater(&ad_hoc); + ASSERT_EQ(&ad_hoc, nsp.getAdHocNodeStatusUpdater()); + + /* + * Initial status publication + */ + SubscriberWithCollector status_sub(nodes.b); + + ASSERT_LE(0, status_sub.start()); + ASSERT_FALSE(status_sub.collector.msg.get()); // No data yet + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(status_sub.collector.msg.get()); // Was published at startup + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, status_sub.collector.msg->health); + ASSERT_EQ(0, status_sub.collector.msg->vendor_specific_status_code); + ASSERT_GE(1, status_sub.collector.msg->uptime_sec); + + ASSERT_EQ(0, ad_hoc.invokations); // Not invoked from startAndPublish() + + /* + * Altering the vendor-specific status code, forcePublish()-ing it and checking the result + */ + ASSERT_EQ(0, nsp.getVendorSpecificStatusCode()); + nsp.setVendorSpecificStatusCode(1234); + ASSERT_EQ(1234, nsp.getVendorSpecificStatusCode()); + + ASSERT_LE(0, nsp.forcePublish()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_ERROR, status_sub.collector.msg->health); + ASSERT_EQ(1234, status_sub.collector.msg->vendor_specific_status_code); + ASSERT_GE(1, status_sub.collector.msg->uptime_sec); + + ASSERT_EQ(0, ad_hoc.invokations); // Not invoked from forcePublish() + + /* + * Explicit node info request + */ + ServiceClientWithCollector gni_cln(nodes.b); + + nsp.setHealthCritical(); + + ASSERT_FALSE(gni_cln.collector.result.get()); // No data yet + ASSERT_LE(0, gni_cln.call(1, uavcan::protocol::GetNodeInfo::Request())); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(gni_cln.collector.result.get()); // Response must have been delivered + + ASSERT_TRUE(gni_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gni_cln.collector.result->getCallID().server_node_id.get()); + + ASSERT_EQ(uavcan::protocol::NodeStatus::HEALTH_CRITICAL, + gni_cln.collector.result->getResponse().status.health); + + ASSERT_TRUE(hwver == gni_cln.collector.result->getResponse().hardware_version); + ASSERT_TRUE(swver == gni_cln.collector.result->getResponse().software_version); + + ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->getResponse().name); + + ASSERT_EQ(0, ad_hoc.invokations); // No timer-triggered publications happened yet + + /* + * Timer triggered publication + */ + EXPECT_EQ(3, nodes.a.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + + nodes.spinBoth(nsp.getStatusPublicationPeriod()); + + EXPECT_EQ(1, ad_hoc.invokations); // No timer-triggered publications happened yet + EXPECT_EQ(4, nodes.a.getDispatcher().getTransferPerfCounter().getTxTransferCount()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_broadcaster.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_broadcaster.cpp new file mode 100644 index 0000000000..edf9ba09e8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_broadcaster.cpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(PanicBroadcaster, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::PanicBroadcaster panicker(nodes.a); + + SubscriberWithCollector sub(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, sub.start()); + + panicker.panic("I lost my towel!"); // Only the first 7 chars allowed + + ASSERT_STREQ("I lost ", panicker.getReason().c_str()); + ASSERT_TRUE(panicker.isPanicking()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_STREQ("I lost ", sub.collector.msg->reason_text.c_str()); + sub.collector.msg.reset(); + + panicker.dontPanic(); + ASSERT_FALSE(panicker.isPanicking()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(300)); + ASSERT_FALSE(sub.collector.msg.get()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_listener.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_listener.cpp new file mode 100644 index 0000000000..0f30bfb701 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/panic_listener.cpp @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + + +struct PanicHandler +{ + uavcan::protocol::Panic msg; + + PanicHandler() : msg() { } + + void handle(const uavcan::protocol::Panic& msg) + { + std::cout << msg << std::endl; + this->msg = msg; + } + + typedef uavcan::MethodBinder Binder; + + Binder bind() { return Binder(this, &PanicHandler::handle); } +}; + + +TEST(PanicListener, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::PanicListener pl(nodes.a); + uavcan::PanicBroadcaster pbr(nodes.b); + PanicHandler handler; + ASSERT_LE(0, pl.start(handler.bind())); + + pbr.panic("PANIC!!!"); + ASSERT_TRUE(handler.msg == uavcan::protocol::Panic()); // One message published, panic is not registered yet + + pbr.dontPanic(); + ASSERT_FALSE(pbr.isPanicking()); + std::cout << "Not panicking" << std::endl; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); // Will reset + ASSERT_TRUE(handler.msg == uavcan::protocol::Panic()); + + pbr.panic("PANIC2!!!"); // Message text doesn't matter + ASSERT_TRUE(pbr.isPanicking()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_STREQ("PANIC2!", handler.msg.reason_text.c_str()); // Registered, only 7 chars preserved +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/param_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/param_server.cpp new file mode 100644 index 0000000000..94abbfafa2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/param_server.cpp @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +struct ParamServerTestManager : public uavcan::IParamManager +{ + typedef std::map KeyValue; + KeyValue kv; + + virtual void getParamNameByIndex(Index index, Name& out_name) const + { + Index current_idx = 0; + for (KeyValue::const_iterator it = kv.begin(); it != kv.end(); ++it, ++current_idx) + { + if (current_idx == index) + { + out_name = it->first.c_str(); + break; + } + } + } + + virtual void assignParamValue(const Name& name, const Value& value) + { + assert(!name.empty()); + std::cout << "ASSIGN [" << name.c_str() << "]\n" << value << "\n---" << std::endl; + KeyValue::iterator it = kv.find(name.c_str()); + if (it != kv.end()) + { + if (value.is(Value::Tag::boolean_value)) + { + assert(value.getTag() == Value::Tag::boolean_value); + it->second = double(value.boolean_value); + } + else if (value.is(Value::Tag::integer_value)) + { + assert(value.getTag() == Value::Tag::integer_value); + it->second = double(value.integer_value); + } + else if (value.is(Value::Tag::real_value)) + { + assert(value.getTag() == Value::Tag::real_value); + it->second = double(value.real_value); + } + else if (value.is(Value::Tag::string_value)) + { + assert(value.getTag() == Value::Tag::string_value); + it->second = std::atof(value.string_value.c_str()); + } + else + { + assert(0); + } + } + } + + virtual void readParamValue(const Name& name, Value& out_value) const + { + assert(!name.empty()); + KeyValue::const_iterator it = kv.find(name.c_str()); + if (it != kv.end()) + { + out_value.to() = float(it->second); + assert(out_value.getTag() == Value::Tag::real_value); + } + std::cout << "READ [" << name.c_str() << "]\n" << out_value << "\n---" << std::endl; + } + + virtual int saveAllParams() + { + std::cout << "SAVE" << std::endl; + return 0; + } + + virtual int eraseAllParams() + { + std::cout << "ERASE" << std::endl; + return 0; + } +}; + + +template +static void doCall(Client& client, const Message& request, InterlinkedTestNodesWithSysClock& nodes) +{ + ASSERT_LE(0, client.call(1, request)); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + ASSERT_TRUE(client.collector.result.get()); + ASSERT_TRUE(client.collector.result->isSuccessful()); +} + + +TEST(ParamServer, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::ParamServer server(nodes.a); + + ParamServerTestManager mgr; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + ASSERT_LE(0, server.start(&mgr)); + + ServiceClientWithCollector get_set_cln(nodes.b); + ServiceClientWithCollector save_erase_cln(nodes.b); + + /* + * Save/erase + */ + uavcan::protocol::param::ExecuteOpcode::Request save_erase_rq; + save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + doCall(save_erase_cln, save_erase_rq, nodes); + ASSERT_TRUE(save_erase_cln.collector.result.get()); + ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok); + + save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; + doCall(save_erase_cln, save_erase_rq, nodes); + ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok); + + // Invalid opcode + save_erase_rq.opcode = 0xFF; + doCall(save_erase_cln, save_erase_rq, nodes); + ASSERT_FALSE(save_erase_cln.collector.result->getResponse().ok); + + /* + * Get/set + */ + uavcan::protocol::param::GetSet::Request get_set_rq; + get_set_rq.name = "nonexistent_parameter"; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_TRUE(get_set_cln.collector.result.get()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty()); + + // No such variable, shall return empty name/value + get_set_rq.index = 0; + get_set_rq.name.clear(); + get_set_rq.value.to() = 0xDEADBEEF; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.is(uavcan::protocol::param::Value::Tag::empty)); + + mgr.kv["foobar"] = 123.456; // New param + + // Get by name + get_set_rq = uavcan::protocol::param::GetSet::Request(); + get_set_rq.name = "foobar"; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.is(uavcan::protocol::param::Value::Tag::real_value)); + ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->getResponse().value. + to()); + + // Set by index + get_set_rq = uavcan::protocol::param::GetSet::Request(); + get_set_rq.index = 0; + get_set_rq.value.to() = "424242"; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value. + to()); + + // Get by index + get_set_rq = uavcan::protocol::param::GetSet::Request(); + get_set_rq.index = 0; + doCall(get_set_cln, get_set_rq, nodes); + ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str()); + ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value. + to()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/restart_request_server.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/restart_request_server.cpp new file mode 100644 index 0000000000..8229ed8ce6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/restart_request_server.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +struct RestartHandler : public uavcan::IRestartRequestHandler +{ + bool accept; + + bool handleRestartRequest(uavcan::NodeID request_source) + { + std::cout << "Restart request from " << int(request_source.get()) << " will be " + << (accept ? "accepted" : "rejected") << std::endl; + return accept; + } +}; + + +TEST(RestartRequestServer, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::RestartRequestServer rrs(nodes.a); + + ServiceClientWithCollector rrs_cln(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, rrs.start()); + + uavcan::protocol::RestartNode::Request request; + request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER; + + /* + * Rejected - handler was not set + */ + ASSERT_LE(0, rrs_cln.call(1, request)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result.get()); + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); + + /* + * Accepted + */ + RestartHandler handler; + handler.accept = true; + rrs.setHandler(&handler); + + ASSERT_LE(0, rrs_cln.call(1, request)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_TRUE(rrs_cln.collector.result->getResponse().ok); + + /* + * Rejected by handler + */ + handler.accept = false; + + ASSERT_LE(0, rrs_cln.call(1, request)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); + + /* + * Rejected because of invalid magic number + */ + handler.accept = true; + + ASSERT_LE(0, rrs_cln.call(1, uavcan::protocol::RestartNode::Request())); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_TRUE(rrs_cln.collector.result->isSuccessful()); + ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/transport_stats_provider.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/transport_stats_provider.cpp new file mode 100644 index 0000000000..dc107b3501 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/protocol/transport_stats_provider.cpp @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(TransportStatsProvider, Basic) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::TransportStatsProvider tsp(nodes.a); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(0, tsp.start()); + + ServiceClientWithCollector tsp_cln(nodes.b); + + /* + * First request + */ + ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request())); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(tsp_cln.collector.result.get()); + ASSERT_TRUE(tsp_cln.collector.result->isSuccessful()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); + + /* + * Second request + */ + ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request())); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(tsp_cln.collector.result.get()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_rx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_tx); + ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + ASSERT_EQ(2, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); + ASSERT_EQ(6, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); + + /* + * Sending a malformed frame, it must be registered as tranfer error + */ + uavcan::Frame frame(uavcan::protocol::GetTransportStats::DefaultDataTypeID, uavcan::TransferTypeServiceRequest, + 2, 1, 1); + frame.setStartOfTransfer(true); + frame.setEndOfTransfer(true); + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + nodes.can_a.read_queue.push(can_frame); + + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + /* + * Introducing a CAN driver error + */ + nodes.can_a.error_count = 72; + + /* + * Third request + */ + ASSERT_LE(0, tsp_cln.call(1, uavcan::protocol::GetTransportStats::Request())); + ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10))); + + ASSERT_TRUE(tsp_cln.collector.result.get()); + EXPECT_EQ(1, tsp_cln.collector.result->getResponse().transfer_errors); // That broken frame + EXPECT_EQ(3, tsp_cln.collector.result->getResponse().transfers_rx); + EXPECT_EQ(2, tsp_cln.collector.result->getResponse().transfers_tx); + EXPECT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size()); + EXPECT_EQ(72, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors); + EXPECT_EQ(4, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx); // Same here + EXPECT_EQ(12, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/test_main.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/test_main.cpp new file mode 100644 index 0000000000..37afc97aae --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/test_main.cpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include + +static void sigsegv_handler(int sig) +{ + const int BacktraceSize = 32; + void* array[BacktraceSize]; + const int size = backtrace(array, BacktraceSize); + + std::fprintf(stderr, "SIGNAL %d RECEIVED; STACKTRACE:\n", sig); + backtrace_symbols_fd(array, size, STDERR_FILENO); + std::exit(1); +} + +int main(int argc, char **argv) +{ + signal(SIGSEGV, sigsegv_handler); + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif +#if UAVCAN_CPP_VERSION == UAVCAN_CPP11 + std::cout << "C++11" << std::endl; +#elif UAVCAN_CPP_VERSION == UAVCAN_CPP03 + std::cout << "C++03" << std::endl; +#else +# error UAVCAN_CPP_VERSION +#endif + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/time.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/time.cpp new file mode 100644 index 0000000000..18b3facd80 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/time.cpp @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(Time, Monotonic) +{ + using uavcan::MonotonicDuration; + using uavcan::MonotonicTime; + + MonotonicTime m1; + MonotonicTime m2 = MonotonicTime::fromMSec(1); + MonotonicDuration md1 = m2 - m1; // 1000 + MonotonicDuration md2 = m1 - m2; // -1000 + + ASSERT_EQ(0, m1.toUSec()); + ASSERT_EQ(1000, m2.toUSec()); + ASSERT_EQ(1000, md1.toUSec()); + ASSERT_EQ(-1000, md2.toUSec()); + + ASSERT_LT(m1, m2); + ASSERT_LE(m1, m2); + ASSERT_NE(m1, m2); + ASSERT_TRUE(m1.isZero()); + ASSERT_FALSE(m2.isZero()); + + ASSERT_GT(md1, md2); + ASSERT_GE(md1, md2); + ASSERT_NE(md1, md2); + ASSERT_FALSE(md1.isZero()); + ASSERT_TRUE(md1.isPositive()); + ASSERT_TRUE(md2.isNegative()); + + ASSERT_EQ(0, (md1 + md2).toUSec()); + ASSERT_EQ(2000, (md1 - md2).toUSec()); + + md1 *= 2; // 2000 + ASSERT_EQ(2000, md1.toUSec()); + + md2 += md1; // md2 = -1000 + 2000 + ASSERT_EQ(1000, md2.toUSec()); + + ASSERT_EQ(-1000, (-md2).toUSec()); + + /* + * To string + */ + ASSERT_EQ("0.000000", m1.toString()); + ASSERT_EQ("0.001000", m2.toString()); + + ASSERT_EQ("0.002000", md1.toString()); + ASSERT_EQ("-0.001000", (-md2).toString()); + + ASSERT_EQ("1001.000001", MonotonicTime::fromUSec(1001000001).toString()); + ASSERT_EQ("-1001.000001", MonotonicDuration::fromUSec(-1001000001).toString()); +} + + +TEST(Time, Utc) +{ + using uavcan::UtcDuration; + using uavcan::UtcTime; + using uavcan::Timestamp; + + Timestamp ts; + ts.usec = 9000; + + UtcTime u1(ts); + ASSERT_EQ(9000, u1.toUSec()); + + ts.usec *= 2; + u1 = ts; + ASSERT_EQ(18000, u1.toUSec()); + + ts = UtcTime::fromUSec(12345678900); + ASSERT_EQ(12345678900, ts.usec); + + /* + * To string + */ + ASSERT_EQ("0.018000", u1.toString()); + ASSERT_EQ("12345.678900", UtcTime(ts).toString()); +} + + +TEST(Time, Overflow) +{ + using uavcan::MonotonicDuration; + using uavcan::MonotonicTime; + + MonotonicTime max_4 = MonotonicTime::fromUSec(MonotonicTime::getMax().toUSec() / 4); + MonotonicDuration max_4_duration = max_4 - MonotonicTime(); + + std::cout << max_4 << std::endl; + ASSERT_EQ(max_4_duration.toUSec(), max_4.toUSec()); + + MonotonicTime max = (((max_4 + max_4_duration) + max_4_duration) + max_4_duration) + max_4_duration; + ASSERT_EQ(max, MonotonicTime::getMax()); // Must not overflow + + MonotonicTime min; + min -= max_4_duration; + ASSERT_EQ(min, MonotonicTime()); // Must not underflow +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can.hpp new file mode 100644 index 0000000000..41563a1ac8 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can.hpp @@ -0,0 +1,282 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include "../../clock.hpp" + + +class CanIfaceMock : public uavcan::ICanIface +{ +public: + struct FrameWithTime + { + uavcan::CanFrame frame; + uavcan::MonotonicTime time; + uavcan::UtcTime time_utc; + uavcan::CanIOFlags flags; + + FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time) + : frame(frame) + , time(time) + , flags(0) + { } + + FrameWithTime(const uavcan::CanFrame& frame, uavcan::MonotonicTime time, uavcan::UtcTime time_utc) + : frame(frame) + , time(time) + , time_utc(time_utc) + , flags(0) + { } + + FrameWithTime(const uavcan::CanFrame& frame, uint64_t time_usec) + : frame(frame) + , time(uavcan::MonotonicTime::fromUSec(time_usec)) + , flags(0) + { } + }; + + std::queue tx; ///< Queue of outgoing frames (bus <-- library) + std::queue rx; ///< Queue of incoming frames (bus --> library) + std::queue loopback; ///< Loopback + bool writeable; + bool tx_failure; + bool rx_failure; + uint64_t num_errors; + uavcan::ISystemClock& iclock; + bool enable_utc_timestamping; + uavcan::CanFrame pending_tx; + + CanIfaceMock(uavcan::ISystemClock& iclock) + : writeable(true) + , tx_failure(false) + , rx_failure(false) + , num_errors(0) + , iclock(iclock) + , enable_utc_timestamping(false) + { } + + void pushRx(const uavcan::CanFrame& frame) + { + rx.push(FrameWithTime(frame, iclock.getMonotonic())); + } + + void pushRx(const uavcan::RxFrame& frame) + { + uavcan::CanFrame can_frame; + EXPECT_TRUE(frame.compile(can_frame)); + rx.push(FrameWithTime(can_frame, frame.getMonotonicTimestamp(), frame.getUtcTimestamp())); + } + + bool matchAndPopTx(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline) + { + if (tx.empty()) + { + std::cout << "Tx buffer is empty" << std::endl; + return false; + } + const FrameWithTime frame_time = tx.front(); + tx.pop(); + return (frame_time.frame == frame) && (frame_time.time == tx_deadline); + } + + bool matchPendingTx(const uavcan::CanFrame& frame) const + { + if (pending_tx != frame) + { + std::cout << "Pending TX mismatch: \n" + << " Expected: " << frame.toString(uavcan::CanFrame::StrAligned) << "\n" + << " Actual: " << pending_tx.toString(uavcan::CanFrame::StrAligned) << std::endl; + } + return pending_tx == frame; + } + + bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline_usec) + { + return matchAndPopTx(frame, uavcan::MonotonicTime::fromUSec(tx_deadline_usec)); + } + + uavcan::CanFrame popTxFrame() + { + if (tx.empty()) + { + std::cout << "Tx buffer is empty" << std::endl; + std::abort(); + } + const FrameWithTime frame_time = tx.front(); + tx.pop(); + return frame_time.frame; + } + + virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) + { + assert(this); + EXPECT_TRUE(writeable); // Shall never be called when not writeable + if (tx_failure) + { + return -1; + } + if (!writeable) + { + return 0; + } + tx.push(FrameWithTime(frame, tx_deadline)); + tx.back().flags = flags; + if (flags & uavcan::CanIOFlagLoopback) + { + loopback.push(FrameWithTime(frame, iclock.getMonotonic())); + } + return 1; + } + + virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) + { + assert(this); + if (loopback.empty()) + { + EXPECT_TRUE(rx.size()); // Shall never be called when not readable + if (rx_failure) + { + return -1; + } + if (rx.empty()) + { + return 0; + } + const FrameWithTime frame = rx.front(); + rx.pop(); + out_frame = frame.frame; + out_ts_monotonic = frame.time; + out_ts_utc = frame.time_utc; + out_flags = frame.flags; + } + else + { + out_flags |= uavcan::CanIOFlagLoopback; + const FrameWithTime frame = loopback.front(); + loopback.pop(); + out_frame = frame.frame; + out_ts_monotonic = frame.time; + out_ts_utc = frame.time_utc; + } + + // Let's just all pretend that this code is autogenerated, instead of being carefully designed by a human. + if (out_ts_utc.isZero()) + { + out_ts_utc = enable_utc_timestamping ? iclock.getUtc() : uavcan::UtcTime(); + } + return 1; + } + + // cppcheck-suppress unusedFunction + // cppcheck-suppress functionConst + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return 0; } + // cppcheck-suppress unusedFunction + virtual uavcan::uint16_t getNumFilters() const { return 4; } // decrease number of HW_filters from 9 to 4 + virtual uavcan::uint64_t getErrorCount() const { return num_errors; } +}; + +class CanDriverMock : public uavcan::ICanDriver +{ +public: + std::vector ifaces; + uavcan::ISystemClock& iclock; + bool select_failure; + + CanDriverMock(unsigned num_ifaces, uavcan::ISystemClock& iclock) + : ifaces(num_ifaces, CanIfaceMock(iclock)) + , iclock(iclock) + , select_failure(false) + { } + + void pushRxToAllIfaces(const uavcan::CanFrame& can_frame) + { + for (uint8_t i = 0; i < getNumIfaces(); i++) + { + ifaces.at(i).pushRx(can_frame); + } + } + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime deadline) + { + assert(this); + //std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl; + + for (unsigned i = 0; i < ifaces.size(); i++) + { + ifaces.at(i).pending_tx = (pending_tx[i] == UAVCAN_NULLPTR) ? uavcan::CanFrame() : *pending_tx[i]; + } + + if (select_failure) + { + return -1; + } + + const uavcan::uint8_t valid_iface_mask = uavcan::uint8_t((1 << getNumIfaces()) - 1); + EXPECT_FALSE(inout_masks.write & ~valid_iface_mask); + EXPECT_FALSE(inout_masks.read & ~valid_iface_mask); + + uavcan::uint8_t out_write_mask = 0; + uavcan::uint8_t out_read_mask = 0; + for (unsigned i = 0; i < getNumIfaces(); i++) + { + const uavcan::uint8_t mask = uavcan::uint8_t(1 << i); + if ((inout_masks.write & mask) && ifaces.at(i).writeable) + { + out_write_mask |= mask; + } + if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size())) + { + out_read_mask |= mask; + } + } + inout_masks.write = out_write_mask; + inout_masks.read = out_read_mask; + if ((out_write_mask | out_read_mask) == 0) + { + const uavcan::MonotonicTime ts = iclock.getMonotonic(); + const uavcan::MonotonicDuration diff = deadline - ts; + SystemClockMock* const mock = dynamic_cast(&iclock); + if (mock) + { + if (diff.isPositive()) + { + mock->advance(uint64_t(diff.toUSec())); // Emulating timeout + } + } + else + { + if (diff.isPositive()) + { + usleep(unsigned(diff.toUSec())); + } + } + return 0; + } + return 1; // This value is not being checked anyway, it just has to be greater than zero + } + + virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) { return &ifaces.at(iface_index); } + virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const { return &ifaces.at(iface_index); } + virtual uavcan::uint8_t getNumIfaces() const { return uavcan::uint8_t(ifaces.size()); } +}; + +enum FrameType { STD, EXT }; +inline uavcan::CanFrame makeCanFrame(uint32_t id, const std::string& str_data, FrameType type) +{ + id |= (type == EXT) ? uavcan::CanFrame::FlagEFF : 0; + return uavcan::CanFrame(id, reinterpret_cast(str_data.c_str()), uavcan::uint8_t(str_data.length())); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can_driver.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can_driver.cpp new file mode 100644 index 0000000000..c6fcfccfeb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/can_driver.cpp @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "can.hpp" +#include + +TEST(CanFrame, FrameProperties) +{ + EXPECT_TRUE(makeCanFrame(0, "", EXT).isExtended()); + EXPECT_FALSE(makeCanFrame(0, "", STD).isExtended()); + EXPECT_FALSE(makeCanFrame(0, "", EXT).isRemoteTransmissionRequest()); + EXPECT_FALSE(makeCanFrame(0, "", STD).isRemoteTransmissionRequest()); + + uavcan::CanFrame frame = makeCanFrame(123, "", STD); + frame.id |= uavcan::CanFrame::FlagRTR; + EXPECT_TRUE(frame.isRemoteTransmissionRequest()); + EXPECT_FALSE(frame.isErrorFrame()); + frame.id |= uavcan::CanFrame::FlagERR; + EXPECT_TRUE(frame.isErrorFrame()); +} + +TEST(CanFrame, Arbitration) +{ + using uavcan::CanFrame; + + CanFrame a; + CanFrame b; + + /* + * Simple + */ + a.id = 123; + b.id = 122; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = 123 | CanFrame::FlagEFF; + b.id = 122 | CanFrame::FlagEFF; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = 8; + b.id = 8; + ASSERT_FALSE(a.priorityLowerThan(b)); + ASSERT_FALSE(b.priorityHigherThan(a)); + + /* + * EXT vs STD + */ + a.id = 1000; // 1000 + b.id = 2000 | CanFrame::FlagEFF; // 2000 >> 18, wins + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = 0x400; + b.id = 0x10000000 | CanFrame::FlagEFF; // (0x400 << 18), 11 most significant bits are the same --> EFF loses + ASSERT_TRUE(a.priorityHigherThan(b)); + ASSERT_TRUE(b.priorityLowerThan(a)); + + /* + * RTR vs Data + */ + a.id = 123 | CanFrame::FlagRTR; // On the same ID, RTR loses + b.id = 123; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); + + a.id = CanFrame::MaskStdID | CanFrame::FlagRTR; // RTR is STD, so it wins + b.id = CanFrame::MaskExtID | CanFrame::FlagEFF; // Lowest possible priority for data frame + ASSERT_TRUE(a.priorityHigherThan(b)); + ASSERT_TRUE(b.priorityLowerThan(a)); + + a.id = 123 | CanFrame::FlagRTR; // Both RTR arbitrate as usually + b.id = 122 | CanFrame::FlagRTR; + ASSERT_TRUE(a.priorityLowerThan(b)); + ASSERT_TRUE(b.priorityHigherThan(a)); +} + +TEST(CanFrame, ToString) +{ + uavcan::CanFrame frame = makeCanFrame(123, "\x01\x02\x03\x04" "1234", EXT); + EXPECT_EQ("0x0000007b 01 02 03 04 31 32 33 34 '....1234'", frame.toString()); + EXPECT_TRUE(frame.toString() == frame.toString(uavcan::CanFrame::StrAligned)); + + frame = makeCanFrame(123, "z", EXT); + EXPECT_EQ("0x0000007b 7a 'z'", frame.toString(uavcan::CanFrame::StrAligned)); + EXPECT_EQ("0x0000007b 7a 'z'", frame.toString()); + + EXPECT_EQ(" 0x141 61 62 63 64 aa bb cc dd 'abcd....'", + makeCanFrame(321, "abcd" "\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::StrAligned)); + + EXPECT_EQ(" 0x100 ''", + makeCanFrame(256, "", STD).toString(uavcan::CanFrame::StrAligned)); + + EXPECT_EQ("0x100 ''", + makeCanFrame(256, "", STD).toString()); + + EXPECT_EQ("0x141 61 62 63 64 aa bb cc dd 'abcd....'", + makeCanFrame(321, "abcd" "\xaa\xbb\xcc\xdd", STD).toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/iface_mock.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/iface_mock.cpp new file mode 100644 index 0000000000..b9b9cf4368 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/iface_mock.cpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "can.hpp" + +TEST(CanDriverMock, Basic) +{ + using uavcan::CanFrame; + using uavcan::CanSelectMasks; + + SystemClockMock clockmock; + CanDriverMock driver(3, clockmock); + + const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { }; + + ASSERT_EQ(3, driver.getNumIfaces()); + + // All WR, no RD + CanSelectMasks masks; + masks.write = 7; + masks.read = 7; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(7, masks.write); + EXPECT_EQ(0, masks.read); + + for (unsigned i = 0; i < 3; i++) + { + driver.ifaces.at(i).writeable = false; + } + + // No WR, no RD + masks.write = 7; + masks.read = 7; + EXPECT_EQ(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(0, masks.write); + EXPECT_EQ(0, masks.read); + EXPECT_EQ(100, clockmock.monotonic); + EXPECT_EQ(100, clockmock.utc); + + // No WR, #1 RD + const CanFrame fr1 = makeCanFrame(123, "foo", EXT); + driver.ifaces.at(1).pushRx(fr1); + masks.write = 7; + masks.read = 6; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(0, masks.write); + EXPECT_EQ(2, masks.read); + CanFrame fr2; + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc, flags)); + EXPECT_EQ(0, flags); + EXPECT_EQ(fr1, fr2); + EXPECT_EQ(100, ts_monotonic.toUSec()); + EXPECT_EQ(0, ts_utc.toUSec()); + + // #0 WR, #1 RD, Select failure + driver.ifaces.at(0).writeable = true; + driver.select_failure = true; + masks.write = 1; + masks.read = 7; + EXPECT_EQ(-1, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(1, masks.write); // Leaving masks unchanged - the library must ignore them + EXPECT_EQ(7, masks.read); +} + +TEST(CanDriverMock, Loopback) +{ + using uavcan::CanFrame; + using uavcan::CanSelectMasks; + + SystemClockMock clockmock; + CanDriverMock driver(1, clockmock); + + const uavcan::CanFrame* pending_tx[uavcan::MaxCanIfaces] = { }; + + CanSelectMasks masks; + masks.write = 1; + masks.read = 1; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(1, masks.write); + EXPECT_EQ(0, masks.read); + + clockmock.advance(200); + + CanFrame fr1; + fr1.id = 123 | CanFrame::FlagEFF; + EXPECT_EQ(1, driver.getIface(0)->send(fr1, uavcan::MonotonicTime::fromUSec(10000), uavcan::CanIOFlagLoopback)); + + masks.write = 0; + masks.read = 1; + EXPECT_LT(0, driver.select(masks, pending_tx, uavcan::MonotonicTime::fromUSec(100))); + EXPECT_EQ(0, masks.write); + EXPECT_EQ(1, masks.read); + + CanFrame fr2; + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + EXPECT_EQ(1, driver.getIface(0)->receive(fr2, ts_monotonic, ts_utc, flags)); + EXPECT_EQ(uavcan::CanIOFlagLoopback, flags); + EXPECT_EQ(fr1, fr2); + EXPECT_EQ(200, ts_monotonic.toUSec()); + EXPECT_EQ(0, ts_utc.toUSec()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/io.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/io.cpp new file mode 100644 index 0000000000..5a046973c3 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/io.cpp @@ -0,0 +1,386 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "can.hpp" + +static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame, + uint64_t timestamp_usec, int iface_index) +{ + if (static_cast(rxframe) != frame) + { + std::cout << "Frame mismatch:\n" + << " " << rxframe.toString(uavcan::CanFrame::StrAligned) << "\n" + << " " << frame.toString(uavcan::CanFrame::StrAligned) << std::endl; + } + return (static_cast(rxframe) == frame) && + (rxframe.ts_mono == uavcan::MonotonicTime::fromUSec(timestamp_usec)) && + (rxframe.iface_index == iface_index); +} + +TEST(CanIOManager, Reception) +{ + // Memory + uavcan::PoolAllocator pool; + + // Platform interface + SystemClockMock clockmock; + CanDriverMock driver(2, clockmock); + + // IO Manager + uavcan::CanIOManager iomgr(driver, pool, clockmock); + ASSERT_EQ(2, iomgr.getNumIfaces()); + + /* + * Empty, will time out + */ + uavcan::CanRxFrame frame; + uavcan::CanIOFlags flags = uavcan::CanIOFlags(); + EXPECT_EQ(0, iomgr.receive(frame, tsMono(100), flags)); + EXPECT_EQ(0, flags); + EXPECT_EQ(100, clockmock.monotonic); + EXPECT_EQ(100, clockmock.utc); + + /* + * Non empty from multiple ifaces + */ + const uavcan::CanFrame frames[2][3] = { + { makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) }, + { makeCanFrame(6341, "b0", EXT), makeCanFrame(196, "b1", STD), makeCanFrame(73, "b2", EXT) }, + }; + + clockmock.advance(10); + driver.ifaces.at(0).pushRx(frames[0][0]); // Timestamp 110 + driver.ifaces.at(1).pushRx(frames[1][0]); + clockmock.advance(10); + driver.ifaces.at(0).pushRx(frames[0][1]); // Timestamp 120 + driver.ifaces.at(1).pushRx(frames[1][1]); + clockmock.advance(10); + driver.ifaces.at(0).pushRx(frames[0][2]); // Timestamp 130 + driver.ifaces.at(1).pushRx(frames[1][2]); + clockmock.advance(10); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1)); + EXPECT_EQ(0, flags); + + EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); // Will time out + EXPECT_EQ(0, flags); + + /* + * Perf counters + */ + driver.select_failure = true; + EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + + driver.select_failure = false; + driver.ifaces.at(1).pushRx(frames[0][0]); + driver.ifaces.at(1).rx_failure = true; + EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); + + driver.ifaces.at(0).num_errors = 9000; + driver.ifaces.at(1).num_errors = 100500; + EXPECT_EQ(9000, iomgr.getIfacePerfCounters(0).errors); + EXPECT_EQ(100500, iomgr.getIfacePerfCounters(1).errors); + + EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_rx); + EXPECT_EQ(3, iomgr.getIfacePerfCounters(1).frames_rx); + + EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_tx); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx); +} + +TEST(CanIOManager, Transmission) +{ + using uavcan::CanIOManager; + using uavcan::CanTxQueue; + + // Memory + uavcan::PoolAllocator pool; + + // Platform interface + SystemClockMock clockmock; + CanDriverMock driver(2, clockmock); + + // IO Manager + CanIOManager iomgr(driver, pool, clockmock, 9999); + ASSERT_EQ(2, iomgr.getNumIfaces()); + + const int ALL_IFACES_MASK = 3; + + const uavcan::CanFrame frames[] = { + makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD) + }; + + uavcan::CanIOFlags flags = uavcan::CanIOFlags(); + + /* + * Simple transmission + */ + EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100)); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent, flags)); // To #1 only + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200)); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame())); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); + + EXPECT_EQ(0, clockmock.monotonic); + EXPECT_EQ(0, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).errors); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors); + + /* + * TX Queue basics + */ + EXPECT_EQ(0, pool.getNumUsedBlocks()); + + // Sending to both, #0 blocked + driver.ifaces.at(0).writeable = false; + EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201)); + EXPECT_EQ(200, clockmock.monotonic); + EXPECT_EQ(200, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(1, pool.getNumUsedBlocks()); // One frame went into TX queue, and will expire soon + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // This one will persist + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // This will drop off on the second select() + + // Sending to both, both blocked + driver.ifaces.at(1).writeable = false; + EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Still 0 + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); // 1!! + + // Sending to #0, both blocked + EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent, flags)); + EXPECT_EQ(400, clockmock.monotonic); + EXPECT_EQ(400, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(4, pool.getNumUsedBlocks()); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); + + // At this time TX queues are containing the following data: + // iface 0: frames[0] (EXPIRED), frames[1], frames[2] + // iface 1: frames[1] + + // Sending to #1, both writeable + driver.ifaces.at(0).writeable = true; + driver.ifaces.at(1).writeable = true; + // One frame per each iface will be sent: + EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent, flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777)); // Note that frame[0] on iface #0 has expired + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); // Expired but still will be reported + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + // Calling receive() to flush the rest two frames + uavcan::CanRxFrame dummy_rx_frame; + EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0), flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777)); + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[1])); + + // Final checks + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(0, pool.getNumUsedBlocks()); // Make sure the memory was properly released + EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors); // This is because of expired frame[0] + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).errors); + + /* + * TX Queue updates from receive() call + */ + driver.ifaces.at(0).writeable = false; + driver.ifaces.at(1).writeable = false; + + // Sending 5 frames, one will be rejected + EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); + EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent, flags)); + // One frame kicked here: + EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + // State checks + EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full + EXPECT_EQ(1200, clockmock.monotonic); + EXPECT_EQ(1200, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + + // Preparing the driver mock for receive() call + driver.ifaces.at(0).writeable = true; + driver.ifaces.at(1).writeable = true; + const uavcan::CanFrame rx_frames[] = { makeCanFrame(123, "rx0", STD), makeCanFrame(321, "rx1", EXT) }; + driver.ifaces.at(0).pushRx(rx_frames[0]); + driver.ifaces.at(1).pushRx(rx_frames[1]); + + // This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK) + uavcan::CanRxFrame rx_frame; + EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags)); // Non-blocking + EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333)); + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[1])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags)); + EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE) + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[2])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[2])); + + // State checks + EXPECT_EQ(0, pool.getNumUsedBlocks()); // TX queue is empty + EXPECT_EQ(1200, clockmock.monotonic); + EXPECT_EQ(1200, clockmock.utc); + EXPECT_TRUE(driver.ifaces.at(0).tx.empty()); + EXPECT_TRUE(driver.ifaces.at(1).tx.empty()); + EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).errors); + EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).errors); // This is because of rejected frame[1] + + /* + * Error handling + */ + // Select failure + driver.select_failure = true; + EXPECT_EQ(-uavcan::ErrDriver, iomgr.receive(rx_frame, tsMono(2000), flags)); + EXPECT_EQ(-uavcan::ErrDriver, + iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); + EXPECT_EQ(1200, clockmock.monotonic); + EXPECT_EQ(1200, clockmock.utc); + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + // Transmission failure + driver.select_failure = false; + driver.ifaces.at(0).writeable = true; + driver.ifaces.at(1).writeable = true; + driver.ifaces.at(0).tx_failure = true; + driver.ifaces.at(1).tx_failure = true; + // Non-blocking - return < 0 + EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(frames[0])); + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(frames[0])); + + ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered + + // Failure removed - transmission shall proceed + driver.ifaces.at(0).tx_failure = false; + driver.ifaces.at(1).tx_failure = false; + EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500), flags)); + EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200)); + EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200)); + EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted + ASSERT_EQ(0, flags); + EXPECT_TRUE(driver.ifaces.at(0).matchPendingTx(uavcan::CanFrame())); // Last call will be receive-only, + EXPECT_TRUE(driver.ifaces.at(1).matchPendingTx(uavcan::CanFrame())); // hence empty TX + + /* + * Perf counters + */ + EXPECT_EQ(1, iomgr.getIfacePerfCounters(0).frames_rx); + EXPECT_EQ(1, iomgr.getIfacePerfCounters(1).frames_rx); + + EXPECT_EQ(6, iomgr.getIfacePerfCounters(0).frames_tx); + EXPECT_EQ(8, iomgr.getIfacePerfCounters(1).frames_tx); +} + +TEST(CanIOManager, Loopback) +{ + using uavcan::CanIOManager; + using uavcan::CanTxQueue; + using uavcan::CanFrame; + using uavcan::CanRxFrame; + + // Memory + uavcan::PoolAllocator pool; + + // Platform interface + SystemClockMock clockmock; + CanDriverMock driver(2, clockmock); + + // IO Manager + CanIOManager iomgr(driver, pool, clockmock); + ASSERT_EQ(2, iomgr.getNumIfaces()); + + CanFrame fr1; + fr1.id = 123 | CanFrame::FlagEFF; + + CanFrame fr2; + fr2.id = 456 | CanFrame::FlagEFF; + + CanRxFrame rfr1; + CanRxFrame rfr2; + + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback)); + ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags)); + ASSERT_EQ(uavcan::CanIOFlagLoopback, flags); + ASSERT_TRUE(rfr1 == fr1); + + flags = 0; + ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback)); + ASSERT_EQ(1, iomgr.send(fr2, tsMono(1000), tsMono(0), 1, CanTxQueue::Persistent, uavcan::CanIOFlagLoopback)); + ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags)); + ASSERT_EQ(uavcan::CanIOFlagLoopback, flags); + ASSERT_LE(0, iomgr.receive(rfr2, tsMono(100), flags)); + ASSERT_EQ(uavcan::CanIOFlagLoopback, flags); + ASSERT_TRUE(rfr1 == fr1); + ASSERT_TRUE(rfr2 == fr2); + + /* + * Perf counters + * Loopback frames are not registered as RX + */ + EXPECT_EQ(0, iomgr.getIfacePerfCounters(0).frames_rx); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_rx); + + EXPECT_EQ(3, iomgr.getIfacePerfCounters(0).frames_tx); + EXPECT_EQ(0, iomgr.getIfacePerfCounters(1).frames_tx); +} + +TEST(CanIOManager, Size) +{ + std::cout << sizeof(uavcan::CanIOManager) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/tx_queue.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/tx_queue.cpp new file mode 100644 index 0000000000..da5aa58bb2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can/tx_queue.cpp @@ -0,0 +1,216 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "can.hpp" + + +static int getQueueLength(uavcan::CanTxQueue& queue) +{ + const uavcan::CanTxQueue::Entry* p = queue.peek(); + int length = 0; + while (p) + { + length++; + p = p->getNextListNode(); + } + return length; +} + +static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame) +{ + const uavcan::CanTxQueue::Entry* p = queue.peek(); + while (p) + { + if (frame == p->frame) + { + return true; + } + p = p->getNextListNode(); + } + return false; +} + +TEST(CanTxQueue, Qos) +{ + const uavcan::CanIOFlags flags = 0; + uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags); + uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags); + + EXPECT_FALSE(e1.qosHigherThan(e2)); + EXPECT_FALSE(e2.qosHigherThan(e1)); + EXPECT_FALSE(e1.qosLowerThan(e2)); + EXPECT_FALSE(e2.qosLowerThan(e1)); + + e2.qos = uavcan::CanTxQueue::Persistent; + + EXPECT_FALSE(e1.qosHigherThan(e2)); + EXPECT_TRUE(e2.qosHigherThan(e1)); + EXPECT_TRUE(e1.qosLowerThan(e2)); + EXPECT_FALSE(e2.qosLowerThan(e1)); + + e1.qos = uavcan::CanTxQueue::Persistent; + e1.frame.id -= 1; + + EXPECT_TRUE(e1.qosHigherThan(e2)); + EXPECT_FALSE(e2.qosHigherThan(e1)); + EXPECT_FALSE(e1.qosLowerThan(e2)); + EXPECT_TRUE(e2.qosLowerThan(e1)); +} + +TEST(CanTxQueue, TxQueue) +{ + using uavcan::CanTxQueue; + using uavcan::CanFrame; + + ASSERT_GE(40, sizeof(CanTxQueue::Entry)); // should be true for any platforms, though not required + + uavcan::PoolAllocator<40 * 4, 40> pool; + + SystemClockMock clockmock; + + CanTxQueue queue(pool, clockmock, 99999); + EXPECT_TRUE(queue.isEmpty()); + + const uavcan::CanIOFlags flags = 0; + + // Descending priority + const CanFrame f0 = makeCanFrame(0, "f0", EXT); + const CanFrame f1 = makeCanFrame(10, "f1", EXT); + const CanFrame f2 = makeCanFrame(20, "f2", EXT); + const CanFrame f3 = makeCanFrame(100, "f3", EXT); + const CanFrame f4 = makeCanFrame(10000, "f4", EXT); + const CanFrame f5 = makeCanFrame(99999, "f5", EXT); + const CanFrame f5a = makeCanFrame(99999, "f5a", EXT); + const CanFrame f6 = makeCanFrame(999999, "f6", EXT); + + /* + * Priority insertion + */ + queue.push(f4, tsMono(100), CanTxQueue::Persistent, flags); + EXPECT_FALSE(queue.isEmpty()); + EXPECT_EQ(1, pool.getNumUsedBlocks()); + EXPECT_EQ(f4, queue.peek()->frame); + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5)); + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal + EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3)); + + queue.push(f3, tsMono(200), CanTxQueue::Persistent, flags); + EXPECT_EQ(f3, queue.peek()->frame); + + queue.push(f0, tsMono(300), CanTxQueue::Volatile, flags); + EXPECT_EQ(f0, queue.peek()->frame); + + queue.push(f1, tsMono(400), CanTxQueue::Volatile, flags); + EXPECT_EQ(f0, queue.peek()->frame); // Still f0, since it is highest + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1)); + + // Out of free memory now + + EXPECT_EQ(0, queue.getRejectedFrameCount()); + EXPECT_EQ(4, getQueueLength(queue)); + EXPECT_TRUE(isInQueue(queue, f0)); + EXPECT_TRUE(isInQueue(queue, f1)); + EXPECT_TRUE(isInQueue(queue, f3)); + EXPECT_TRUE(isInQueue(queue, f4)); + + const CanTxQueue::Entry* p = queue.peek(); + while (p) + { + std::cout << p->toString() << std::endl; + p = p->getNextListNode(); + } + + /* + * QoS + */ + EXPECT_FALSE(isInQueue(queue, f2)); + queue.push(f2, tsMono(100), CanTxQueue::Volatile, flags); // Non preempting, will be rejected + EXPECT_FALSE(isInQueue(queue, f2)); + + queue.push(f2, tsMono(500), CanTxQueue::Persistent, flags); // Will override f1 (f3 and f4 are presistent) + EXPECT_TRUE(isInQueue(queue, f2)); + EXPECT_FALSE(isInQueue(queue, f1)); + EXPECT_EQ(4, getQueueLength(queue)); + EXPECT_EQ(2, queue.getRejectedFrameCount()); + EXPECT_EQ(f0, queue.peek()->frame); // Check the priority + + queue.push(f5, tsMono(600), CanTxQueue::Persistent, flags); // Will override f0 (rest are presistent) + EXPECT_TRUE(isInQueue(queue, f5)); + EXPECT_FALSE(isInQueue(queue, f0)); + EXPECT_EQ(f2, queue.peek()->frame); // Check the priority + + // No volatile frames left now + + queue.push(f5a, tsMono(700), CanTxQueue::Persistent, flags); // Will override f5 (same frame, same QoS) + EXPECT_TRUE(isInQueue(queue, f5a)); + EXPECT_FALSE(isInQueue(queue, f5)); + + queue.push(f6, tsMono(700), CanTxQueue::Persistent, flags); // Will be rejected (lowest QoS) + EXPECT_FALSE(isInQueue(queue, f6)); + + EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0)); + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f2)); // Equal + EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5a)); + EXPECT_EQ(4, getQueueLength(queue)); + EXPECT_EQ(4, pool.getNumUsedBlocks()); + EXPECT_EQ(5, queue.getRejectedFrameCount()); + EXPECT_TRUE(isInQueue(queue, f2)); + EXPECT_TRUE(isInQueue(queue, f3)); + EXPECT_TRUE(isInQueue(queue, f4)); + EXPECT_TRUE(isInQueue(queue, f5a)); + EXPECT_EQ(f2, queue.peek()->frame); // Check the priority + + /* + * Expiration + */ + clockmock.monotonic = 101; + queue.push(f0, tsMono(800), CanTxQueue::Volatile, flags); // Will replace f4 which is expired now + EXPECT_TRUE(isInQueue(queue, f0)); + EXPECT_FALSE(isInQueue(queue, f4)); + EXPECT_EQ(6, queue.getRejectedFrameCount()); + + clockmock.monotonic = 1001; + queue.push(f5, tsMono(2000), CanTxQueue::Volatile, flags); // Entire queue is expired + EXPECT_TRUE(isInQueue(queue, f5)); + EXPECT_EQ(1, getQueueLength(queue)); // Just one entry left - f5 + EXPECT_EQ(1, pool.getNumUsedBlocks()); // Make sure there is no leaks + EXPECT_EQ(10, queue.getRejectedFrameCount()); + + queue.push(f0, tsMono(1000), CanTxQueue::Persistent, flags); // This entry is already expired + EXPECT_EQ(1, getQueueLength(queue)); + EXPECT_EQ(1, pool.getNumUsedBlocks()); + EXPECT_EQ(11, queue.getRejectedFrameCount()); + + /* + * Removing + */ + queue.push(f4, tsMono(5000), CanTxQueue::Volatile, flags); + EXPECT_EQ(2, getQueueLength(queue)); + EXPECT_TRUE(isInQueue(queue, f4)); + EXPECT_EQ(f4, queue.peek()->frame); + + CanTxQueue::Entry* entry = queue.peek(); + EXPECT_TRUE(entry); + queue.remove(entry); + EXPECT_FALSE(entry); + + EXPECT_FALSE(isInQueue(queue, f4)); + EXPECT_TRUE(isInQueue(queue, f5)); + + entry = queue.peek(); + EXPECT_TRUE(entry); + queue.remove(entry); + EXPECT_FALSE(entry); + + EXPECT_FALSE(isInQueue(queue, f5)); + + EXPECT_EQ(0, getQueueLength(queue)); // Final state checks + EXPECT_EQ(0, pool.getNumUsedBlocks()); + EXPECT_EQ(11, queue.getRejectedFrameCount()); + EXPECT_FALSE(queue.peek()); + EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can_acceptance_filter_configurator.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can_acceptance_filter_configurator.cpp new file mode 100644 index 0000000000..1d3f3fc6a7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/can_acceptance_filter_configurator.cpp @@ -0,0 +1,219 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +#include +#include "../node/test_node.hpp" +#include "uavcan/node/subscriber.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +struct SubscriptionListener +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructure; + + struct ReceivedDataStructureCopy + { + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::uint8_t iface_index; + DataType msg; + + ReceivedDataStructureCopy(const ReceivedDataStructure& s) : + ts_monotonic(s.getMonotonicTimestamp()), + ts_utc(s.getUtcTimestamp()), + transfer_type(s.getTransferType()), + transfer_id(s.getTransferID()), + src_node_id(s.getSrcNodeID()), + iface_index(s.getIfaceIndex()), + msg(s) + { } + }; + + std::vector simple; + std::vector extended; + + void receiveExtended(ReceivedDataStructure& msg) + { + extended.push_back(msg); + } + + void receiveSimple(DataType& msg) + { + simple.push_back(msg); + } + + typedef SubscriptionListener SelfType; + typedef uavcan::MethodBinder ExtendedBinder; + typedef uavcan::MethodBinder SimpleBinder; + + ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); } + SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); } +}; + +static void writeServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp) +{ + std::cout << req << std::endl; + rsp.error = rsp.ERROR_UNKNOWN; +} + +TEST(CanAcceptanceFilter, Basic_test) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 24); + + uavcan::Subscriber::ExtendedBinder> sub_1(node); + uavcan::Subscriber::ExtendedBinder> sub_2(node); + uavcan::Subscriber::ExtendedBinder> sub_3(node); + uavcan::Subscriber::ExtendedBinder> sub_4(node); + uavcan::Subscriber::ExtendedBinder> sub_5(node); + uavcan::Subscriber::ExtendedBinder> sub_6(node); + uavcan::Subscriber::ExtendedBinder> sub_6_1(node); + uavcan::ServiceServer server(node); + + SubscriptionListener listener_1; + SubscriptionListener listener_2; + SubscriptionListener listener_3; + SubscriptionListener listener_4; + SubscriptionListener listener_5; + SubscriptionListener listener_6; + + sub_1.start(listener_1.bindExtended()); + sub_2.start(listener_2.bindExtended()); + sub_3.start(listener_3.bindExtended()); + sub_4.start(listener_4.bindExtended()); + sub_5.start(listener_5.bindExtended()); + sub_6.start(listener_6.bindExtended()); + sub_6_1.start(listener_6.bindExtended()); + server.start(writeServiceServerCallback); + std::cout << "Subscribers are initialized." << std::endl; + + uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10); + + int configure_filters_assert = anon_test_configuration.computeConfiguration(); + ASSERT_EQ(configure_filters_assert, 0); + std::cout << "Filters are calculated with anonymous configuration." << std::endl; + + const auto& configure_array = anon_test_configuration.getConfiguration(); + uint32_t configure_array_size = configure_array.getSize(); + std::cout << "Number of configurations after first time computeConfiguration() invoked: " + << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 9); + + std::cout << "Adding two additional configurations ... " << std::endl; + uavcan::CanFilterConfig aux_config_1, aux_config_2; + aux_config_1.id = 911; + aux_config_1.mask = 1488; + aux_config_2.id = 999999; + aux_config_2.mask = 849128412; + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1); + ASSERT_EQ(configure_filters_assert, 0); + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2); + ASSERT_EQ(configure_filters_assert, 0); + configure_array_size = configure_array.getSize(); + std::cout << "New configuration anon_container size: " << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 11); + + std::cout << "Applying configuration ... " << std::endl; + configure_filters_assert = anon_test_configuration.applyConfiguration(); + ASSERT_EQ(configure_filters_assert, 0); + std::cout << "Filters are configured." << std::endl; + configure_array_size = configure_array.getSize(); + std::cout << "Final configuration anon_container size: " << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 10); + + for (uint16_t i = 0; i + */ + +#include +#include +#include +#include "transfer_test_helpers.hpp" +#include "can/can.hpp" +#include + + +class DispatcherTransferEmulator : public IncomingTransferEmulatorBase +{ + CanDriverMock& target_; + +public: + DispatcherTransferEmulator(CanDriverMock& target, uavcan::NodeID dst_node_id = 127) + : IncomingTransferEmulatorBase(dst_node_id) + , target_(target) + { } + + void sendOneFrame(const uavcan::RxFrame& frame) + { + CanIfaceMock* const iface = static_cast(target_.getIface(frame.getIfaceIndex())); + EXPECT_TRUE(iface); + if (iface) + { + iface->pushRx(frame); + } + } +}; + + +struct RxFrameListener : public uavcan::IRxFrameListener +{ + std::vector rx_frames; + + virtual void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags) + { + std::cout << "RX frame [flags=" << flags << "]: " << frame.toString() << std::endl; + if ((flags & uavcan::CanIOFlagLoopback) == 0) + { + rx_frames.push_back(frame); + } + } +}; + + +static const uavcan::NodeID SELF_NODE_ID(64); + + +TEST(Dispatcher, Reception) +{ + uavcan::PoolAllocator pool; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, pool, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once + ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID)); + ASSERT_EQ(SELF_NODE_ID, dispatcher.getNodeID()); + + DispatcherTransferEmulator emulator(driver, SELF_NODE_ID); + + /* + * RX listener + */ + RxFrameListener rx_listener; + ASSERT_FALSE(dispatcher.getRxFrameListener()); + dispatcher.installRxFrameListener(&rx_listener); + ASSERT_TRUE(dispatcher.getRxFrameListener()); + ASSERT_TRUE(rx_listener.rx_frames.empty()); + + /* + * Test environment + */ + static const uavcan::DataTypeDescriptor TYPES[4] = + { + makeDataType(uavcan::DataTypeKindMessage, 1), + makeDataType(uavcan::DataTypeKindMessage, 2), + makeDataType(uavcan::DataTypeKindService, 1), + makeDataType(uavcan::DataTypeKindService, 1) + }; + + typedef std::unique_ptr TestListenerPtr; + static const int MaxBufSize = 512; + static const int NumSubscribers = 6; + TestListenerPtr subscribers[NumSubscribers] = + { + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[0], MaxBufSize, pool)), // msg // Two similar + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[1], MaxBufSize, pool)), // msg + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[2], MaxBufSize, pool)), // srv + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)), // srv + TestListenerPtr(new TestListener(dispatcher.getTransferPerfCounter(), TYPES[3], MaxBufSize, pool)) // srv // Repeat again + }; + + static const std::string DATA[6] = + { + "Yes, man is mortal, but that would be only half the trouble. " + "The worst of it is that he's sometimes unexpectedly mortal - there's the trick!", + + "In fact, I'm beginning to fear that this confusion will go on for a long time. " + "And all because he writes down what I said incorrectly.", + + "I had the pleasure of meeting that young man at the Patriarch's Ponds. " + "He almost drove me mad myself, proving to me that I don't exist. But you do believe that it is really I?", + + "He was a dreamer, a thinker, a speculative philosopher... or, as his wife would have it, an idiot.", + + "The only way to get ideas for stories is to drink way too much coffee and buy a desk that doesn't " + "collapse when you beat your head against it", + + "" + }; + + for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++) + { + std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl; + } + + const Transfer transfers[8] = + { + emulator.makeTransfer(0, uavcan::TransferTypeMessageBroadcast, 10, DATA[0], TYPES[0]), + emulator.makeTransfer(5, uavcan::TransferTypeMessageBroadcast, 11, DATA[1], TYPES[1]), + emulator.makeTransfer(10, uavcan::TransferTypeServiceRequest, 12, DATA[2], TYPES[2]), + emulator.makeTransfer(15, uavcan::TransferTypeServiceResponse, 13, DATA[4], TYPES[3]), + emulator.makeTransfer(20, uavcan::TransferTypeMessageBroadcast, 14, DATA[3], TYPES[0]), + emulator.makeTransfer(25, uavcan::TransferTypeMessageBroadcast, 15, DATA[5], TYPES[1]), + // Wrongly addressed: + emulator.makeTransfer(29, uavcan::TransferTypeServiceResponse, 10, DATA[0], TYPES[3], 100), + emulator.makeTransfer(31, uavcan::TransferTypeServiceRequest, 11, DATA[4], TYPES[2], 101) + }; + + /* + * Registration + */ + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_FALSE(dispatcher.hasSubscriber(subscribers[i]->getDataTypeDescriptor().getID())); + ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID())); + ASSERT_FALSE(dispatcher.hasServer(subscribers[i]->getDataTypeDescriptor().getID())); + } + + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[0].get())); + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[1].get())); + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[2].get())); + ASSERT_TRUE(dispatcher.registerServiceRequestListener(subscribers[3].get())); + ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[4].get())); + ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[5].get())); + + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID())); + } + + // Subscribers + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[0]->getDataTypeDescriptor().getID())); + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[1]->getDataTypeDescriptor().getID())); + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[2]->getDataTypeDescriptor().getID())); + + // Servers + ASSERT_TRUE(dispatcher.hasServer(subscribers[3]->getDataTypeDescriptor().getID())); + + /* + * Sending the transfers + */ + // Multiple service request listeners are not allowed + ASSERT_FALSE(dispatcher.registerServiceRequestListener(subscribers[3].get())); + + // Item count validation + ASSERT_EQ(3, dispatcher.getNumMessageListeners()); + ASSERT_EQ(1, dispatcher.getNumServiceRequestListeners()); + ASSERT_EQ(2, dispatcher.getNumServiceResponseListeners()); + + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_TRUE(subscribers[i]->isEmpty()); + } + + emulator.send(transfers); + emulator.send(transfers); // Just for fun, they will be ignored anyway. + + while (true) + { + const int res = dispatcher.spinOnce(); + ASSERT_LE(0, res); + clockmock.advance(100); + if (res == 0) + { + break; + } + } + + /* + * Matching. + */ + ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[4])); + ASSERT_TRUE(subscribers[0]->matchAndPop(transfers[0])); + + ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[4])); + ASSERT_TRUE(subscribers[1]->matchAndPop(transfers[0])); + + ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[5])); + ASSERT_TRUE(subscribers[2]->matchAndPop(transfers[1])); + + ASSERT_TRUE(subscribers[3]->matchAndPop(transfers[2])); + + ASSERT_TRUE(subscribers[4]->matchAndPop(transfers[3])); + + ASSERT_TRUE(subscribers[5]->matchAndPop(transfers[3])); + + for (int i = 0; i < NumSubscribers; i++) + { + ASSERT_TRUE(subscribers[i]->isEmpty()); + } + + /* + * Unregistering all transfers + */ + dispatcher.unregisterMessageListener(subscribers[0].get()); + dispatcher.unregisterMessageListener(subscribers[1].get()); + dispatcher.unregisterMessageListener(subscribers[2].get()); + dispatcher.unregisterServiceRequestListener(subscribers[3].get()); + dispatcher.unregisterServiceResponseListener(subscribers[4].get()); + dispatcher.unregisterServiceResponseListener(subscribers[5].get()); + + ASSERT_EQ(0, dispatcher.getNumMessageListeners()); + ASSERT_EQ(0, dispatcher.getNumServiceRequestListeners()); + ASSERT_EQ(0, dispatcher.getNumServiceResponseListeners()); + + /* + * Perf counters + */ + EXPECT_LT(0, dispatcher.getTransferPerfCounter().getErrorCount()); // Repeated transfers + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(9, dispatcher.getTransferPerfCounter().getRxTransferCount()); + + /* + * RX listener + */ + std::cout << "Num received frames: " << rx_listener.rx_frames.size() << std::endl; + ASSERT_EQ(292, rx_listener.rx_frames.size()); +} + + +TEST(Dispatcher, Transmission) +{ + uavcan::PoolAllocator pool; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, pool, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once + ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID)); + + /* + * RX listener + */ + RxFrameListener rx_listener; + dispatcher.installRxFrameListener(&rx_listener); + + /* + * Transmission + */ + static const uavcan::MonotonicTime TX_DEADLINE = tsMono(123456); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame frame(123, uavcan::TransferTypeServiceRequest, SELF_NODE_ID, 2, 0); + frame.setPayload(reinterpret_cast("123"), 3); + + ASSERT_FALSE(dispatcher.hasPublisher(123)); + ASSERT_FALSE(dispatcher.hasPublisher(456)); + const uavcan::OutgoingTransferRegistryKey otr_key(123, uavcan::TransferTypeMessageBroadcast, 0); + ASSERT_TRUE(dispatcher.getOutgoingTransferRegistry().accessOrCreate(otr_key, + uavcan::MonotonicTime::fromMSec(1000000))); + ASSERT_TRUE(dispatcher.hasPublisher(123)); + ASSERT_FALSE(dispatcher.hasPublisher(456)); + + ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0, 0xFF)); + + /* + * Validation + */ + uavcan::CanFrame expected_can_frame; + ASSERT_TRUE(frame.compile(expected_can_frame)); + + ASSERT_TRUE(driver.ifaces.at(0).matchAndPopTx(expected_can_frame, TX_DEADLINE)); + ASSERT_TRUE(driver.ifaces.at(1).matchAndPopTx(expected_can_frame, TX_DEADLINE)); + + ASSERT_TRUE(driver.ifaces.at(0).tx.empty()); + ASSERT_TRUE(driver.ifaces.at(1).tx.empty()); + + /* + * Perf counters - all empty because dispatcher itself does not count TX transfers + */ + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); + + /* + * RX listener + */ + ASSERT_TRUE(rx_listener.rx_frames.empty()); +} + + +TEST(Dispatcher, Spin) +{ + NullAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); // Can be set only once + ASSERT_FALSE(dispatcher.setNodeID(SELF_NODE_ID)); + + clockmock.monotonic_auto_advance = 100; + + ASSERT_EQ(100, clockmock.monotonic); + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + ASSERT_LE(1000, clockmock.monotonic); + ASSERT_EQ(0, dispatcher.spinOnce()); + ASSERT_LE(1000, clockmock.monotonic); + ASSERT_EQ(0, dispatcher.spin(tsMono(1100))); + ASSERT_LE(1100, clockmock.monotonic); +} + + +struct DispatcherTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase +{ + uavcan::RxFrame last_frame; + unsigned count; + + DispatcherTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher) + : uavcan::LoopbackFrameListenerBase(dispatcher) + , count(0) + { } + + using uavcan::LoopbackFrameListenerBase::startListening; + using uavcan::LoopbackFrameListenerBase::isListening; + + void handleLoopbackFrame(const uavcan::RxFrame& frame) + { + std::cout << "DispatcherTestLoopbackFrameListener: " << frame.toString() << std::endl; + last_frame = frame; + count++; + } +}; + +TEST(Dispatcher, Loopback) +{ + NullAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); + + { + DispatcherTestLoopbackFrameListener listener(dispatcher); + ASSERT_FALSE(listener.isListening()); + listener.startListening(); + ASSERT_TRUE(listener.isListening()); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame frame(123, uavcan::TransferTypeServiceResponse, SELF_NODE_ID, 2, 0); + frame.setPayload(reinterpret_cast("123"), 3); + + ASSERT_TRUE(listener.last_frame == uavcan::RxFrame()); + + ASSERT_LE(0, dispatcher.send(frame, tsMono(1000), tsMono(0), uavcan::CanTxQueue::Persistent, + uavcan::CanIOFlagLoopback, 0xFF)); + + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + + ASSERT_TRUE(listener.last_frame != uavcan::RxFrame()); + ASSERT_TRUE(listener.last_frame == frame); + ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); // Last iface + ASSERT_EQ(2, listener.count); + + ASSERT_EQ(1, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners()); + } + ASSERT_EQ(0, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/frame.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/frame.cpp new file mode 100644 index 0000000000..522ad42f9d --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/frame.cpp @@ -0,0 +1,389 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "../clock.hpp" +#include "can/can.hpp" + + +TEST(Frame, MessageParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + /* + * Priority + * Message Type ID + * Service Not Message + * Source Node ID + */ + const uint32_t can_id = + (16 << 24) | // Priority + (20000 << 8) | // Message Type ID + (0 << 7) | // Service Not Message + (42 << 0); // Source Node ID + + const std::string payload_string = "hello\xD4"; // SET = 110, TID = 20 + + /* + * Parse + */ + // Invalid CAN frames + ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, reinterpret_cast(""), 0))); + ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD))); + + // Valid + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(20), frame.getTransferID()); + EXPECT_TRUE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_FALSE(frame.getToggle()); + EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID()); + EXPECT_TRUE(frame.getDstNodeID().isBroadcast()); + EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType()); + EXPECT_EQ(20000, frame.getDataTypeID().get()); + EXPECT_EQ(16, frame.getPriority().get()); + + EXPECT_EQ(payload_string.length() - 1, frame.getPayloadLen()); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + payload_string.begin())); + + std::cout << frame.toString() << std::endl; + + /* + * Compile + */ + CanFrame can_frame; + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + ASSERT_TRUE(frame.compile(can_frame)); + ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT)); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + std::cout << can_frame.toString() << std::endl; + /* + * FUN FACT: comparison of uint8_t with char may fail on the character 0xD4 (depending on the locale), + * because it will be considered a Unicode character. Hence, we do reinterpret_cast<>. + */ + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, + reinterpret_cast(&payload_string[0]))); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Frame, ServiceParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + /* + * Priority + * Service Type ID + * Request Not Response + * Destination Node ID + * Service Not Message + * Source Node ID + */ + const uint32_t can_id = + (31 << 24) | // Priority + (200 << 16) | // Service Type ID + (1 << 15) | // Request Not Response + (0x42 << 8) | // Destination Node ID + (1 << 7) | // Service Not Message + (42 << 0); // Source Node ID + + const std::string payload_string = "hello\x6a"; // SET = 011, TID = 10 + + /* + * Parse + */ + // Invalid CAN frames + ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, reinterpret_cast(""), 0))); + ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD))); + + // Valid + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(10), frame.getTransferID()); + EXPECT_FALSE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_TRUE(frame.getToggle()); + EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID()); + EXPECT_EQ(uavcan::NodeID(0x42), frame.getDstNodeID()); + EXPECT_EQ(uavcan::TransferTypeServiceRequest, frame.getTransferType()); + EXPECT_EQ(200, frame.getDataTypeID().get()); + EXPECT_EQ(31, frame.getPriority().get()); + + EXPECT_EQ(payload_string.length(), frame.getPayloadLen() + 1); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + reinterpret_cast(&payload_string[0]))); + + std::cout << frame.toString() << std::endl; + + /* + * Compile + */ + CanFrame can_frame; + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + ASSERT_TRUE(frame.compile(can_frame)); + ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT)); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, + reinterpret_cast(&payload_string[0]))); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Frame, AnonymousParseCompile) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::TransferID; + using uavcan::TransferType; + + Frame frame; + + /* + * Priority + * Discriminator + * Message Type ID + * Service Not Message + * Source Node ID + */ + const uint32_t can_id = + (16383 << 10) | // Discriminator + (1 << 8); // Message Type ID + + const std::string payload_string = "hello\xd4"; // SET = 110, TID = 20 + + uavcan::TransferCRC payload_crc; + payload_crc.add(reinterpret_cast(payload_string.c_str()), unsigned(payload_string.length())); + + /* + * Parse + */ + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + EXPECT_EQ(TransferID(20), frame.getTransferID()); + EXPECT_TRUE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_FALSE(frame.getToggle()); + EXPECT_TRUE(frame.getSrcNodeID().isBroadcast()); + EXPECT_TRUE(frame.getDstNodeID().isBroadcast()); + EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType()); + EXPECT_EQ(1, frame.getDataTypeID().get()); + EXPECT_EQ(0, frame.getPriority().get()); + + EXPECT_EQ(payload_string.length() - 1, frame.getPayloadLen()); + EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(), + reinterpret_cast(&payload_string[0]))); + + std::cout << frame.toString() << std::endl; + + /* + * Compile + */ + const uint32_t DiscriminatorMask = 0x00FFFC00; + const uint32_t NoDiscriminatorMask = 0xFF0003FF; + + CanFrame can_frame; + ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT))); + + ASSERT_TRUE(frame.compile(can_frame)); + ASSERT_EQ(can_id & NoDiscriminatorMask & uavcan::CanFrame::MaskExtID, + can_frame.id & NoDiscriminatorMask & uavcan::CanFrame::MaskExtID); + + EXPECT_EQ(payload_string.length(), can_frame.dlc); + EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, + reinterpret_cast(&payload_string[0]))); + + EXPECT_EQ((can_frame.id & DiscriminatorMask & uavcan::CanFrame::MaskExtID) >> 10, payload_crc.get() & 16383); + + /* + * Comparison + */ + ASSERT_FALSE(Frame() == frame); + ASSERT_TRUE(Frame() != frame); + frame = Frame(); + ASSERT_TRUE(Frame() == frame); + ASSERT_FALSE(Frame() != frame); +} + + +TEST(Frame, FrameParsing) +{ + using uavcan::Frame; + using uavcan::CanFrame; + using uavcan::NodeID; + using uavcan::TransferID; + + CanFrame can; + Frame frame; + ASSERT_FALSE(frame.parse(can)); + + for (unsigned i = 0; i < sizeof(CanFrame::data); i++) + { + can.data[i] = uint8_t(i | (i << 4)); + } + + /* + * Message CAN ID fields and offsets: + * 24 Priority + * 8 Message Type ID + * 7 Service Not Message (0) + * 0 Source Node ID + * + * Service CAN ID fields and offsets: + * 24 Priority + * 16 Service Type ID + * 15 Request Not Response + * 8 Destination Node ID + * 7 Service Not Message (1) + * 0 Source Node ID + */ + + /* + * SFT message broadcast + */ + can.id = CanFrame::FlagEFF | + (2 << 24) | + (456 << 8) | + (0 << 7) | + (42 << 0); + + can.data[7] = 0xcf; // SET=110, TID=0 + + ASSERT_FALSE(frame.parse(can)); + can.dlc = 8; + + ASSERT_TRUE(frame.parse(can)); + EXPECT_TRUE(frame.isStartOfTransfer()); + EXPECT_TRUE(frame.isEndOfTransfer()); + EXPECT_FALSE(frame.getToggle()); + ASSERT_EQ(2, frame.getPriority().get()); + ASSERT_EQ(NodeID(42), frame.getSrcNodeID()); + ASSERT_EQ(NodeID::Broadcast, frame.getDstNodeID()); + ASSERT_EQ(456, frame.getDataTypeID().get()); + ASSERT_EQ(TransferID(15), frame.getTransferID()); + ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType()); + + // TODO: test service frames + // TODO: test malformed frames +} + + +TEST(Frame, RxFrameParse) +{ + using uavcan::Frame; + using uavcan::RxFrame; + using uavcan::CanFrame; + using uavcan::CanRxFrame; + + CanRxFrame can_rx_frame; + RxFrame rx_frame; + + // Failure + ASSERT_FALSE(rx_frame.parse(can_rx_frame)); + + // Valid + can_rx_frame.ts_mono = uavcan::MonotonicTime::fromUSec(1); // Zero is not allowed + can_rx_frame.id = CanFrame::FlagEFF | + (2 << 24) | + (456 << 8) | + (0 << 7) | + (42 << 0); + + ASSERT_FALSE(rx_frame.parse(can_rx_frame)); + + can_rx_frame.data[0] = 0xc0; // SOT, EOT + can_rx_frame.dlc = 1; + + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(1, rx_frame.getMonotonicTimestamp().toUSec()); + ASSERT_EQ(0, rx_frame.getIfaceIndex()); + + can_rx_frame.ts_mono = tsMono(123); + can_rx_frame.iface_index = 2; + + Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0); + ASSERT_TRUE(frame.compile(can_rx_frame)); + + ASSERT_TRUE(rx_frame.parse(can_rx_frame)); + ASSERT_EQ(123, rx_frame.getMonotonicTimestamp().toUSec()); + ASSERT_EQ(2, rx_frame.getIfaceIndex()); + ASSERT_EQ(456, rx_frame.getDataTypeID().get()); + ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType()); +} + + +TEST(Frame, FrameToString) +{ + using uavcan::Frame; + using uavcan::RxFrame; + + // RX frame default + RxFrame rx_frame; + EXPECT_EQ("prio=255 dtid=65535 tt=3 snid=255 dnid=255 sot=0 eot=0 togl=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", + rx_frame.toString()); + + // RX frame max len + rx_frame = RxFrame(Frame(uavcan::DataTypeID::MaxPossibleDataTypeIDValue, uavcan::TransferTypeMessageBroadcast, + uavcan::NodeID::Max, 0, uavcan::TransferID::Max), + uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3); + + uint8_t data[8]; + for (unsigned i = 0; i < sizeof(data); i++) + { + data[i] = uint8_t(i); + } + rx_frame.setPayload(data, sizeof(data)); + + rx_frame.setStartOfTransfer(true); + rx_frame.setEndOfTransfer(true); + rx_frame.flipToggle(); + rx_frame.setPriority(uavcan::TransferPriority::NumericallyMax); + + EXPECT_EQ("prio=31 dtid=65535 tt=2 snid=127 dnid=0 sot=1 eot=1 togl=1 tid=31 payload=[00 01 02 03 04 05 06] " + "ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3", + rx_frame.toString()); + + // Plain frame default + Frame frame; + EXPECT_EQ("prio=255 dtid=65535 tt=3 snid=255 dnid=255 sot=0 eot=0 togl=0 tid=0 payload=[]", frame.toString()); + + // Plain frame max len + frame = rx_frame; + EXPECT_EQ("prio=31 dtid=65535 tt=2 snid=127 dnid=0 sot=1 eot=1 togl=1 tid=31 payload=[00 01 02 03 04 05 06]", + frame.toString()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/incoming_transfer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/incoming_transfer.cpp new file mode 100644 index 0000000000..3e1f55bc58 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/incoming_transfer.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "transfer_test_helpers.hpp" + + +static uavcan::RxFrame makeFrame() +{ + uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0), + tsMono(123), tsUtc(456), 0); + uint8_t data[8]; + for (uint8_t i = 0; i < sizeof(data); i++) + { + data[i] = i; + } + frame.setPayload(data, sizeof(data)); + frame.setEndOfTransfer(true); + return frame; +} + + +static bool match(const uavcan::IncomingTransfer& it, const uavcan::RxFrame& frame, + const uint8_t* payload, unsigned payload_len) +{ + // Fields extracted from the frame struct + EXPECT_EQ(it.getMonotonicTimestamp(), frame.getMonotonicTimestamp()); + EXPECT_EQ(it.getUtcTimestamp(), frame.getUtcTimestamp()); + EXPECT_EQ(it.getSrcNodeID(), frame.getSrcNodeID()); + EXPECT_EQ(it.getTransferID(), frame.getTransferID()); + EXPECT_EQ(it.getTransferType(), frame.getTransferType()); + + // Payload comparison + static const unsigned BUFLEN = 1024; + uint8_t buf_reference[BUFLEN], buf_actual[BUFLEN]; + + if (payload_len > BUFLEN) + { + std::cout << "match(): Payload is too long" << std::endl; + exit(1); + } + + std::fill(buf_reference, buf_reference + BUFLEN, 0); + std::fill(buf_actual, buf_actual + BUFLEN, 0); + std::copy(payload, payload + payload_len, buf_reference); + + EXPECT_EQ(payload_len, it.read(0, buf_actual, payload_len * 3)); + EXPECT_EQ(0, it.read(payload_len, buf_actual, payload_len * 3)); + + return std::equal(buf_reference, buf_reference + BUFLEN, buf_actual); +} + + +TEST(SingleFrameIncomingTransfer, Basic) +{ + using uavcan::RxFrame; + using uavcan::SingleFrameIncomingTransfer; + + const RxFrame frame = makeFrame(); + SingleFrameIncomingTransfer it(frame); + + ASSERT_TRUE(match(it, frame, frame.getPayloadPtr(), frame.getPayloadLen())); +} + + +TEST(MultiFrameIncomingTransfer, Basic) +{ + using uavcan::RxFrame; + using uavcan::MultiFrameIncomingTransfer; + + uavcan::PoolAllocator poolmgr; + uavcan::TransferBufferManager bufmgr(256, poolmgr); + + const RxFrame frame = makeFrame(); + uavcan::TransferBufferManagerKey bufmgr_key(frame.getSrcNodeID(), frame.getTransferType()); + uavcan::TransferBufferAccessor tba(bufmgr, bufmgr_key); + + MultiFrameIncomingTransfer it(frame.getMonotonicTimestamp(), frame.getUtcTimestamp(), frame, tba); + + /* + * Empty read must fail + */ + uint8_t data_byte = 0; + ASSERT_GT(0, it.read(0, &data_byte, 1)); // Error - no such buffer + + /* + * Filling the test data + */ + const std::string data = "123Hello world"; + const uint8_t* const data_ptr = reinterpret_cast(data.c_str()); + ASSERT_FALSE(bufmgr.access(bufmgr_key)); + ASSERT_TRUE(bufmgr.create(bufmgr_key)); + ASSERT_EQ(data.length(), bufmgr.access(bufmgr_key)->write(0, data_ptr, unsigned(data.length()))); + + /* + * Check + */ + ASSERT_TRUE(match(it, frame, data_ptr, unsigned(data.length()))); + + /* + * Buffer release + */ + ASSERT_TRUE(bufmgr.access(bufmgr_key)); + it.release(); + ASSERT_FALSE(bufmgr.access(bufmgr_key)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/outgoing_transfer_registry.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/outgoing_transfer_registry.cpp new file mode 100644 index 0000000000..393a0abb7f --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/outgoing_transfer_registry.cpp @@ -0,0 +1,90 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "transfer_test_helpers.hpp" + + +TEST(OutgoingTransferRegistry, Basic) +{ + using uavcan::OutgoingTransferRegistryKey; + uavcan::PoolAllocator poolmgr; + uavcan::OutgoingTransferRegistry otr(poolmgr); + + otr.cleanup(tsMono(1000)); + + static const int NUM_KEYS = 5; + const OutgoingTransferRegistryKey keys[NUM_KEYS] = + { + OutgoingTransferRegistryKey(123, uavcan::TransferTypeServiceRequest, 42), + OutgoingTransferRegistryKey(321, uavcan::TransferTypeMessageBroadcast, 0), + OutgoingTransferRegistryKey(213, uavcan::TransferTypeServiceRequest, 2), + OutgoingTransferRegistryKey(312, uavcan::TransferTypeServiceRequest, 4), + OutgoingTransferRegistryKey(456, uavcan::TransferTypeServiceRequest, 2) + }; + + ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[2], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get()); + ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // OOM + + /* + * Incrementing a little + */ + otr.accessOrCreate(keys[0], tsMono(2000000))->increment(); + otr.accessOrCreate(keys[0], tsMono(4000000))->increment(); + otr.accessOrCreate(keys[0], tsMono(3000000))->increment(); + ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get()); + + otr.accessOrCreate(keys[2], tsMono(2000000))->increment(); + otr.accessOrCreate(keys[2], tsMono(3000000))->increment(); + ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get()); + + otr.accessOrCreate(keys[3], tsMono(9000000))->increment(); + ASSERT_EQ(1, otr.accessOrCreate(keys[3], tsMono(4000000))->get()); + + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(4000000))->get()); + + ASSERT_FALSE(otr.accessOrCreate(keys[4], tsMono(1000000))); // Still OOM + + /* + * Checking existence + * Exist: 0, 1, 2, 3 + * Does not exist: 4 + */ + ASSERT_TRUE(otr.exists(keys[1].getDataTypeID(), keys[1].getTransferType())); + ASSERT_TRUE(otr.exists(keys[0].getDataTypeID(), keys[0].getTransferType())); + ASSERT_TRUE(otr.exists(keys[3].getDataTypeID(), keys[3].getTransferType())); + ASSERT_TRUE(otr.exists(keys[2].getDataTypeID(), keys[2].getTransferType())); + + ASSERT_FALSE(otr.exists(keys[1].getDataTypeID(), keys[2].getTransferType())); // Invalid combination + ASSERT_FALSE(otr.exists(keys[0].getDataTypeID(), keys[1].getTransferType())); // Invalid combination + ASSERT_FALSE(otr.exists(keys[4].getDataTypeID(), keys[4].getTransferType())); // Plain missing + + /* + * Cleaning up + */ + otr.cleanup(tsMono(4000001)); // Kills 1, 3 + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get()); + otr.accessOrCreate(keys[1], tsMono(5000000))->increment(); + otr.accessOrCreate(keys[3], tsMono(5000000))->increment(); + + ASSERT_EQ(3, otr.accessOrCreate(keys[0], tsMono(5000000))->get()); + ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(6000000))->get()); + + otr.cleanup(tsMono(5000001)); // Kills 1, 3 (He needs a bath, Jud. He stinks of the ground you buried him in.), 0 + ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[1], tsMono(1000000))->get()); + ASSERT_EQ(0, otr.accessOrCreate(keys[3], tsMono(1000000))->get()); + + ASSERT_EQ(2, otr.accessOrCreate(keys[2], tsMono(1000000))->get()); + + otr.cleanup(tsMono(5000001)); // Frees some memory for 4 + ASSERT_EQ(0, otr.accessOrCreate(keys[0], tsMono(1000000))->get()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer.cpp new file mode 100644 index 0000000000..db630b7770 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer.cpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +TEST(Transfer, TransferID) +{ + using uavcan::TransferID; + + // Tests below are based on this assumption + ASSERT_EQ(32, 1 << TransferID::BitLen); + + /* + * forwardDistance() + */ + EXPECT_EQ(0, TransferID(0).computeForwardDistance(0)); + EXPECT_EQ(1, TransferID(0).computeForwardDistance(1)); + EXPECT_EQ(7, TransferID(0).computeForwardDistance(7)); + + EXPECT_EQ(0, TransferID(7).computeForwardDistance(7)); + EXPECT_EQ(31,TransferID(31).computeForwardDistance(30)); + EXPECT_EQ(1, TransferID(31).computeForwardDistance(0)); + + EXPECT_EQ(30,TransferID(7).computeForwardDistance(5)); + EXPECT_EQ(5, TransferID(0).computeForwardDistance(5)); + + /* + * Misc + */ + EXPECT_TRUE(TransferID(2) == TransferID(2)); + EXPECT_FALSE(TransferID(2) != TransferID(2)); + EXPECT_FALSE(TransferID(2) == TransferID(0)); + EXPECT_TRUE(TransferID(2) != TransferID(0)); + + TransferID tid; + for (int i = 0; i < 999; i++) + { + ASSERT_EQ(i & ((1 << TransferID::BitLen) - 1), tid.get()); + const TransferID copy = tid; + tid.increment(); + ASSERT_EQ(1, copy.computeForwardDistance(tid)); + ASSERT_EQ(31, tid.computeForwardDistance(copy)); + ASSERT_EQ(0, tid.computeForwardDistance(tid)); + } +} + + +TEST(Transfer, NodeID) +{ + uavcan::NodeID nid1(1); + uavcan::NodeID nid127(127); + uavcan::NodeID nid0(0); + uavcan::NodeID nidx; + + ASSERT_TRUE(nid1.isUnicast()); + ASSERT_FALSE(nid1.isBroadcast()); + ASSERT_TRUE(nid1.isValid()); + + ASSERT_TRUE(nid127.isUnicast()); + ASSERT_FALSE(nid127.isBroadcast()); + ASSERT_TRUE(nid127.isValid()); + + ASSERT_FALSE(nid0.isUnicast()); + ASSERT_TRUE(nid0.isBroadcast()); + ASSERT_TRUE(nid0.isValid()); + + ASSERT_FALSE(nidx.isUnicast()); + ASSERT_FALSE(nidx.isBroadcast()); + ASSERT_FALSE(nidx.isValid()); + + /* + * Comparison operators + */ + ASSERT_TRUE(nid1 < nid127); + ASSERT_TRUE(nid1 <= nid127); + ASSERT_TRUE(nid0 < nid1); + ASSERT_TRUE(nid0 <= nid1); + + ASSERT_FALSE(nid1 > nid127); + ASSERT_FALSE(nid1 >= nid127); + ASSERT_FALSE(nid0 > nid1); + ASSERT_FALSE(nid0 >= nid1); + + ASSERT_FALSE(nid1 > uavcan::NodeID(1)); + ASSERT_TRUE(nid1 >= uavcan::NodeID(1)); + + ASSERT_FALSE(nid1 == nid127); + ASSERT_TRUE(nid127 == uavcan::NodeID(127)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_buffer.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_buffer.cpp new file mode 100644 index 0000000000..1a4ac031fb --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_buffer.cpp @@ -0,0 +1,318 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include + +static const std::string TEST_DATA = + "It was like this: I asked myself one day this question - what if Napoleon, for instance, had happened to be in my " + "place, and if he had not had Toulon nor Egypt nor the passage of Mont Blanc to begin his career with, but " + "instead of all those picturesque and monumental things, there had simply been some ridiculous old hag, a " + "pawnbroker, who had to be murdered too to get money from her trunk (for his career, you understand). " + "Well, would he have brought himself to that if there had been no other means?"; + +template +static bool allEqual(const T (&a)[Size]) +{ + unsigned n = Size; + while ((--n > 0) && (a[n] == a[0])) { } + return n == 0; +} + +template +static void fill(T (&a)[Size], R value) +{ + for (unsigned i = 0; i < Size; i++) + { + a[i] = T(value); + } +} + +static bool matchAgainst(const std::string& data, const uavcan::ITransferBuffer& tbb, + unsigned offset = 0, int len = -1) +{ + uint8_t local_buffer[1024]; + fill(local_buffer, 0); + assert((len < 0) || (sizeof(local_buffer) >= static_cast(len))); + + if (len < 0) + { + const int res = tbb.read(offset, local_buffer, sizeof(local_buffer)); + if (res < 0) + { + std::cout << "matchAgainst(): res " << res << std::endl; + return false; + } + len = res; + } + else + { + const int res = tbb.read(offset, local_buffer, unsigned(len)); + if (res != len) + { + std::cout << "matchAgainst(): res " << res << " expected " << len << std::endl; + return false; + } + } + const bool equals = std::equal(local_buffer, local_buffer + len, data.begin() + offset); + if (!equals) + { + std::cout << "local_buffer:\n\t" << local_buffer << std::endl; + std::cout << "test_data:\n\t" << std::string(data.begin() + offset, data.begin() + offset + len) << std::endl; + } + return equals; +} + +static bool matchAgainstTestData(const uavcan::ITransferBuffer& tbb, unsigned offset, int len = -1) +{ + return matchAgainst(TEST_DATA, tbb, offset, len); +} + +TEST(TransferBuffer, TestDataValidation) +{ + ASSERT_LE(4, TEST_DATA.length() / uavcan::MemPoolBlockSize); + uint8_t local_buffer[50]; + std::copy(TEST_DATA.begin(), TEST_DATA.begin() + sizeof(local_buffer), local_buffer); + ASSERT_FALSE(allEqual(local_buffer)); +} + +static const int TEST_BUFFER_SIZE = 200; + +TEST(StaticTransferBuffer, Basic) +{ + using uavcan::StaticTransferBuffer; + StaticTransferBuffer buf; + + uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; + const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); + + // Empty reads + fill(local_buffer, 0xA5); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(999, local_buffer, 0)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Bulk write + ASSERT_EQ(TEST_BUFFER_SIZE, buf.write(0, test_data_ptr, unsigned(TEST_DATA.length()))); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4)); + + // Reset + fill(local_buffer, 0xA5); + buf.reset(); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Random write + ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 12, 21)); + + ASSERT_EQ(12, buf.write(0, test_data_ptr, 12)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + + ASSERT_EQ(0, buf.write(21, test_data_ptr + 21, 0)); + ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, 999)); + ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); +} + + +TEST(TransferBufferManagerEntry, Basic) +{ + using uavcan::TransferBufferManagerEntry; + + static const int MAX_SIZE = TEST_BUFFER_SIZE; + static const int POOL_BLOCKS = 8; + uavcan::PoolAllocator pool; + + TransferBufferManagerEntry buf(pool, MAX_SIZE); + + uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; + const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); + + // Empty reads + fill(local_buffer, 0xA5); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(999, local_buffer, 0)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Bulk write + ASSERT_EQ(MAX_SIZE, buf.write(0, test_data_ptr, unsigned(TEST_DATA.length()))); + + ASSERT_LT(0, pool.getNumUsedBlocks()); // Making sure some memory was used + + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4)); + + // Reset + fill(local_buffer, 0xA5); + buf.reset(); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_TRUE(allEqual(local_buffer)); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + + // Random write + ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 12, 21)); + + ASSERT_EQ(60, buf.write(TEST_BUFFER_SIZE - 60, test_data_ptr + TEST_BUFFER_SIZE - 60, 60)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE - 60)); + + // Now we have two empty regions: empty-data-empty-data + + ASSERT_EQ(0, buf.write(0, test_data_ptr, 0)); + ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, TEST_BUFFER_SIZE - 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21)); + + // Now: empty-data-data-data + + ASSERT_EQ(21, buf.write(0, test_data_ptr, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + + // Destroying the object; memory should be released + ASSERT_LT(0, pool.getNumUsedBlocks()); + buf.~TransferBufferManagerEntry(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +static const std::string MGR_TEST_DATA[4] = +{ + "I thought you would cry out again \'don\'t speak of it, leave off.\'\" Raskolnikov gave a laugh, but rather a " + "forced one. \"What, silence again?\" he asked a minute later. \"We must talk about something, you know. ", + + "It would be interesting for me to know how you would decide a certain \'problem\' as Lebeziatnikov would say.\" " + "(He was beginning to lose the thread.) \"No, really, I am serious. Imagine, Sonia, that you had known all ", + + "Luzhin\'s intentions beforehand. Known, that is, for a fact, that they would be the ruin of Katerina Ivanovna " + "and the children and yourself thrown in--since you don\'t count yourself for anything--Polenka too... for ", + + "she\'ll go the same way. Well, if suddenly it all depended on your decision whether he or they should go on " + "living, that is whether Luzhin should go on living and doing wicked things, or Katerina Ivanovna should die? " + "How would you decide which of them was to die? I ask you?" +}; + +static const int MGR_MAX_BUFFER_SIZE = 100; + +TEST(TransferBufferManager, TestDataValidation) +{ + for (unsigned i = 0; i < sizeof(MGR_TEST_DATA) / sizeof(MGR_TEST_DATA[0]); i++) + { + ASSERT_LT(MGR_MAX_BUFFER_SIZE, MGR_TEST_DATA[i].length()); + } +} + + +static int fillTestData(const std::string& data, uavcan::ITransferBuffer* tbb) +{ + return tbb->write(0, reinterpret_cast(data.c_str()), unsigned(data.length())); +} + +TEST(TransferBufferManager, Basic) +{ + using uavcan::TransferBufferManager; + using uavcan::TransferBufferManagerKey; + using uavcan::ITransferBuffer; + + static const int POOL_BLOCKS = 100; + uavcan::PoolAllocator pool; + + std::unique_ptr mgr(new TransferBufferManager(MGR_MAX_BUFFER_SIZE, pool)); + + // Empty + ASSERT_FALSE(mgr->access(TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast))); + ASSERT_FALSE(mgr->access(TransferBufferManagerKey(127, uavcan::TransferTypeServiceRequest))); + + ITransferBuffer* tbb = UAVCAN_NULLPTR; + + const TransferBufferManagerKey keys[5] = + { + TransferBufferManagerKey(0, uavcan::TransferTypeServiceRequest), + TransferBufferManagerKey(1, uavcan::TransferTypeMessageBroadcast), + TransferBufferManagerKey(2, uavcan::TransferTypeServiceRequest), + TransferBufferManagerKey(127, uavcan::TransferTypeServiceResponse), + TransferBufferManagerKey(64, uavcan::TransferTypeMessageBroadcast) + }; + + ASSERT_TRUE((tbb = mgr->create(keys[0]))); + ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[0], tbb)); + ASSERT_EQ(1, mgr->getNumBuffers()); + + ASSERT_TRUE((tbb = mgr->create(keys[1]))); + ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[1], tbb)); + ASSERT_EQ(2, mgr->getNumBuffers()); + ASSERT_LT(2, pool.getNumUsedBlocks()); + + ASSERT_TRUE((tbb = mgr->create(keys[2]))); + ASSERT_EQ(MGR_MAX_BUFFER_SIZE, fillTestData(MGR_TEST_DATA[2], tbb)); + ASSERT_EQ(3, mgr->getNumBuffers()); + + std::cout << "TransferBufferManager - Basic: Pool usage: " << pool.getNumUsedBlocks() << std::endl; + + ASSERT_TRUE((tbb = mgr->create(keys[3]))); + + ASSERT_LT(0, fillTestData(MGR_TEST_DATA[3], tbb)); + ASSERT_EQ(4, mgr->getNumBuffers()); + + // Making sure all buffers contain proper data + ASSERT_TRUE((tbb = mgr->access(keys[0]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[0], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[1]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[1], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[2]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[2], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[3]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[3], *tbb)); + + mgr->remove(keys[1]); + ASSERT_FALSE(mgr->access(keys[1])); + ASSERT_EQ(3, mgr->getNumBuffers()); + ASSERT_LT(0, pool.getNumFreeBlocks()); + + mgr->remove(keys[0]); + ASSERT_FALSE(mgr->access(keys[0])); + ASSERT_EQ(2, mgr->getNumBuffers()); + + // At this time we have the following NodeID: 2, 127 + ASSERT_TRUE((tbb = mgr->access(keys[2]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[2], *tbb)); + + ASSERT_TRUE((tbb = mgr->access(keys[3]))); + ASSERT_TRUE(matchAgainst(MGR_TEST_DATA[3], *tbb)); + + // These were deleted: 0, 1; 3 is still there + ASSERT_FALSE(mgr->access(keys[1])); + ASSERT_FALSE(mgr->access(keys[0])); + ASSERT_TRUE(mgr->access(keys[3])); + + // Filling the memory again in order to check the destruction below + ASSERT_TRUE((tbb = mgr->create(keys[1]))); + ASSERT_LT(0, fillTestData(MGR_TEST_DATA[1], tbb)); + + // Deleting the object; all memory must be freed + ASSERT_NE(0, pool.getNumUsedBlocks()); + mgr.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_listener.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_listener.cpp new file mode 100644 index 0000000000..cf3806c7f0 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_listener.cpp @@ -0,0 +1,268 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "transfer_test_helpers.hpp" +#include "../clock.hpp" + + +class TransferListenerEmulator : public IncomingTransferEmulatorBase +{ + uavcan::TransferListener& target_; + const uavcan::DataTypeDescriptor data_type_; + +public: + TransferListenerEmulator(uavcan::TransferListener& target, const uavcan::DataTypeDescriptor& type, + uavcan::NodeID dst_node_id = 127) + : IncomingTransferEmulatorBase(dst_node_id) + , target_(target) + , data_type_(type) + { } + + void sendOneFrame(const uavcan::RxFrame& frame) { target_.handleFrame(frame); } + + Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type, + uint8_t source_node_id, const std::string& payload) + { + return IncomingTransferEmulatorBase::makeTransfer(priority, transfer_type, source_node_id, payload, data_type_); + } +}; + + +TEST(TransferListener, BasicMFT) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator pool; + + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 256, pool); + + /* + * Test data + */ + static const std::string DATA[] = + { + "Build a man a fire, and he'll be warm for a day. " + "Set a man on fire, and he'll be warm for the rest of his life.", + + "123456789", + + "In the beginning there was nothing, which exploded.", + + "The USSR, which they'd begun to renovate and improve at about the time when Tatarsky decided to " + "change his profession, improved so much that it ceased to exist", + + "BEWARE JET BLAST" + }; + + for (unsigned i = 0; i < sizeof(DATA) / sizeof(DATA[0]); i++) + { + std::cout << "Size of test data chunk " << i << ": " << DATA[i].length() << std::endl; + } + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 1, DATA[0]), + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 2, DATA[1]), // Same NID + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 3, DATA[2]), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 4, DATA[3]), + emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 5, DATA[4]), + }; + + /* + * Sending concurrently + * Expected reception order: 1, 4, 2, 0, 3 + */ + emulator.send(transfers); + + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[4])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[2])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[0])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, CrcFailure) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 256, poolmgr); // Static buffer only, 2 entries + + /* + * Generating transfers with damaged payload (CRC is not valid) + */ + TransferListenerEmulator emulator(subscriber, type); + const Transfer tr_mft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 42, "123456789abcdefghik"); + const Transfer tr_sft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 11, "abcd"); + + std::vector ser_mft = serializeTransfer(tr_mft); + std::vector ser_sft = serializeTransfer(tr_sft); + + ASSERT_TRUE(ser_mft.size() > 1); + ASSERT_TRUE(ser_sft.size() == 1); + + const_cast(ser_mft[1].getPayloadPtr())[1] = uint8_t(~ser_mft[1].getPayloadPtr()[1]); // CRC invalid now + const_cast(ser_sft[0].getPayloadPtr())[2] = uint8_t(~ser_sft[0].getPayloadPtr()[2]); // no CRC here + + /* + * Sending and making sure that MFT was not received, but SFT was. + */ + std::vector > sers; + sers.push_back(ser_mft); + sers.push_back(ser_sft); + sers.push_back(ser_mft); // Ignored + sers.push_back(ser_sft); // Ignored + + emulator.send(sers); + + Transfer tr_sft_damaged = tr_sft; + tr_sft_damaged.payload[2] = char(~tr_sft.payload[2]); // Damaging the data similarly, so that it can be matched + + ASSERT_TRUE(subscriber.matchAndPop(tr_sft_damaged)); + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, BasicSFT) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 0, poolmgr); // Max buf size is 0, i.e. SFT-only + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 1, "123"), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 1, "456"), // Same NID + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, ""), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 3, "abc"), + emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 4, ""), + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "foo"), // Same as 2, not ignored + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "123456789abc"), // Same as 2, not SFT - ignore + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 2, "bar"), // Same as 2, not ignored + }; + + emulator.send(transfers); + + ASSERT_TRUE(subscriber.matchAndPop(transfers[0])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[2])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[4])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[5])); + ASSERT_TRUE(subscriber.matchAndPop(transfers[7])); + + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, Cleanup) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 256, poolmgr); + + /* + * Generating transfers + */ + TransferListenerEmulator emulator(subscriber, type); + const Transfer tr_mft = emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 42, "123456789abcdefghik"); + const Transfer tr_sft = emulator.makeTransfer(16, uavcan::TransferTypeServiceResponse, 11, "abcd"); + + const std::vector ser_mft = serializeTransfer(tr_mft); + const std::vector ser_sft = serializeTransfer(tr_sft); + + ASSERT_TRUE(ser_mft.size() > 1); + ASSERT_TRUE(ser_sft.size() == 1); + + const std::vector ser_mft_begin(ser_mft.begin(), ser_mft.begin() + 1); + + /* + * Sending the first part and SFT + */ + std::vector > sers; + sers.push_back(ser_mft_begin); // Only the first part + sers.push_back(ser_sft); + + emulator.send(sers); + + ASSERT_TRUE(subscriber.matchAndPop(tr_sft)); + ASSERT_TRUE(subscriber.isEmpty()); + + /* + * Cleanup with huge timestamp value will remove all entries + */ + static_cast(subscriber).cleanup(tsMono(100000000)); + + /* + * Sending the same transfers again - they will be accepted since registres were cleared + */ + sers.clear(); + sers.push_back(ser_mft); // Complete transfer + sers.push_back(ser_sft); + + emulator.send(sers); + + ASSERT_TRUE(subscriber.matchAndPop(tr_sft)); + ASSERT_TRUE(subscriber.matchAndPop(tr_mft)); + ASSERT_TRUE(subscriber.isEmpty()); +} + + +TEST(TransferListener, AnonymousTransfers) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + static const int NUM_POOL_BLOCKS = 100; + uavcan::PoolAllocator poolmgr; + uavcan::TransferPerfCounter perf; + TestListener subscriber(perf, type, 0, poolmgr); + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(16, uavcan::TransferTypeServiceRequest, 0, "1234567"), // Invalid - not broadcast + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "1234567"), // Valid + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "12345678"), // Invalid - not SFT + emulator.makeTransfer(16, uavcan::TransferTypeMessageBroadcast, 0, "") // Valid + }; + + emulator.send(transfers); + + // Nothing will be received, because anonymous transfers are disabled by default + ASSERT_TRUE(subscriber.isEmpty()); + + subscriber.allowAnonymousTransfers(); + + // Re-send everything again + emulator.send(transfers); + + // Now the anonymous transfers are enabled + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); // Only SFT broadcast will be accepted + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + + ASSERT_TRUE(subscriber.isEmpty()); +} + +TEST(TransferListener, Sizes) +{ + using namespace uavcan; + + std::cout << "sizeof(TransferListener): " << sizeof(TransferListener) << std::endl; +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_receiver.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_receiver.cpp new file mode 100644 index 0000000000..dd7e85d1d2 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_receiver.cpp @@ -0,0 +1,561 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "../clock.hpp" +#include "transfer_test_helpers.hpp" + +/* + * Beware! + * The code you're about to look at desperately needs some cleaning. + */ + +enum SotEotToggle +{ + SET000 = 0, + SET001 = 1, + SET010 = 2, + SET011 = 3, + SET100 = 4, + SET101 = 5, // Illegal + SET110 = 6, + SET111 = 7 // Illegal +}; + +struct RxFrameGenerator +{ + static const uavcan::TransferBufferManagerKey DEFAULT_KEY; + enum { TARGET_NODE_ID = 126 }; + + uint16_t data_type_id; + uavcan::TransferBufferManagerKey bufmgr_key; + + RxFrameGenerator(uint16_t data_type_id, const uavcan::TransferBufferManagerKey& bufmgr_key = DEFAULT_KEY) + : data_type_id(data_type_id) + , bufmgr_key(bufmgr_key) + { } + + /// iface_index, data, set, transfer_id, ts_monotonic [, ts_utc] + uavcan::RxFrame operator()(uint8_t iface_index, const std::string& data, SotEotToggle set, + uint8_t transfer_id, uint64_t ts_monotonic, uint64_t ts_utc = 0) + { + const uavcan::NodeID dst_nid = + (bufmgr_key.getTransferType() == uavcan::TransferTypeMessageBroadcast) ? + uavcan::NodeID::Broadcast : TARGET_NODE_ID; + + uavcan::Frame frame(data_type_id, bufmgr_key.getTransferType(), bufmgr_key.getNodeID(), + dst_nid, transfer_id); + + frame.setStartOfTransfer((set & (1 << 2)) != 0); + frame.setEndOfTransfer((set & (1 << 1)) != 0); + + if ((set & (1 << 0)) != 0) + { + frame.flipToggle(); + } + + EXPECT_EQ(data.length(), + frame.setPayload(reinterpret_cast(data.c_str()), unsigned(data.length()))); + + uavcan::RxFrame output(frame, uavcan::MonotonicTime::fromUSec(ts_monotonic), + uavcan::UtcTime::fromUSec(ts_utc), iface_index); + //std::cout << "Generated frame: " << output.toString() << std::endl; + + return output; + } +}; + +const uavcan::TransferBufferManagerKey RxFrameGenerator::DEFAULT_KEY(42, uavcan::TransferTypeMessageBroadcast); + + +template +struct Context +{ + uavcan::PoolAllocator pool; + uavcan::TransferReceiver receiver; // Must be default constructible and copyable + uavcan::TransferBufferManager bufmgr; + + Context() : + bufmgr(BufSize, pool) + { } + + ~Context() + { + // We need to destroy the receiver before its buffer manager + receiver = uavcan::TransferReceiver(); + } +}; + + +static bool matchBufferContent(const uavcan::ITransferBuffer* tbb, const std::string& content) +{ + uint8_t data[1024]; + std::fill(data, data + sizeof(data), 0); + if (content.length() > sizeof(data)) + { + std::cerr << "matchBufferContent(): Content is too long" << std::endl; + std::exit(1); + } + tbb->read(0, data, unsigned(content.length())); + if (std::equal(content.begin(), content.end(), data)) + { + return true; + } + std::cout << "Buffer content mismatch:" + << "\n\tExpected: " << content + << "\n\tActually: " << reinterpret_cast(data) + << std::endl; + return false; +} + + +#define CHECK_NOT_COMPLETE(x) ASSERT_EQ(uavcan::TransferReceiver::ResultNotComplete, (x)) +#define CHECK_COMPLETE(x) ASSERT_EQ(uavcan::TransferReceiver::ResultComplete, (x)) +#define CHECK_SINGLE_FRAME(x) ASSERT_EQ(uavcan::TransferReceiver::ResultSingleFrame, (x)) + +TEST(TransferReceiver, Basic) +{ + using uavcan::TransferReceiver; + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + std::cout << "sizeof(TransferReceiver): " << sizeof(TransferReceiver) << std::endl; + + /* + * Empty + */ + ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec()); + + /* + * Single frame transfer with zero ts, must be ignored + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "Foo", SET110, 0, 0), bk)); + ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic().toUSec()); + + /* + * Valid compound transfer + * Args: iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "\x34\x12" "34567", SET100, 0, 100), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "foo", SET011, 0, 200), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567foo")); + ASSERT_EQ(0x1234, rcv.getLastTransferCrc()); + ASSERT_EQ(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); // Not initialized yet + ASSERT_EQ(100, rcv.getLastTransferTimestampMonotonic().toUSec()); + + /* + * Compound transfer mixed with invalid frames; buffer was not released explicitly + * Args: iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET100, 0, 300), bk)); // Previous TID, rejected + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "rty", SET100, 0, 300), bk)); // Previous TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "\x9a\x78" "34567", SET100, 1, 1000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET100, 1, 1100), bk)); // Old toggle + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET000, 1, 1100), bk)); // Old toggle + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "abcdefg", SET001, 1, 1200), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "4567891", SET001, 2, 1300), bk)); // Next TID, but not SOT + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET010, 1, 1300), bk)); // Wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", SET001, 1, 1300), bk)); // Unexpected toggle + CHECK_COMPLETE( rcv.addFrame(gen(0, "", SET010, 1, 1300), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcdefg")); + ASSERT_EQ(0x789A, rcv.getLastTransferCrc()); + ASSERT_GE(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); + ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_FALSE(rcv.isTimedOut(tsMono(1000))); + ASSERT_FALSE(rcv.isTimedOut(tsMono(5000))); + ASSERT_TRUE(rcv.isTimedOut(tsMono(60000000))); + + /* + * Single-frame transfers + * Args: iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET110, 1, 2000), bk)); // Previous TID + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 2, 2100), bk)); // Wrong iface + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 2, 2200), bk)); + + ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer must be removed + ASSERT_GT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_EQ(2200, rcv.getLastTransferTimestampMonotonic().toUSec()); + + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 3, 2500), bk)); + ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic().toUSec()); + + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 0, 3000), bk)); + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 1, 3100), bk)); + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 3, 3200), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 0, 3300), bk)); // Old TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 2, 3400), bk)); // Old TID, wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", SET110, 3, 3500), bk)); // Old TID, wrong iface + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", SET110, 4, 3600), bk)); + ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + /* + * Timeouts + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 1, 5000), bk)); // Wrong iface - ignored + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", SET110, 6, 1500000), bk)); // Accepted due to iface timeout + ASSERT_EQ(1500000, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET110, 7, 1500100), bk)); // Ignored - old iface 0 + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", SET110, 7, 1500100), bk)); + ASSERT_EQ(1500100, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", SET110, 7, 1500100), bk)); // Old TID + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 7, 100000000), bk)); // Accepted - global timeout + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", SET110, 8, 100000100), bk)); + ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic().toUSec()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + ASSERT_TRUE(rcv.isTimedOut(tsMono(900000000))); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "34567", SET100, 0, 900000000), bk)); // Global timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 0, 900000100), bk)); // Wrong iface + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", SET011, 0, 900000200), bk)); // Wrong iface + CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", SET011, 0, 900000200), bk)); + + ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + ASSERT_FALSE(rcv.isTimedOut(tsMono(1000))); + ASSERT_FALSE(rcv.isTimedOut(tsMono(900000300))); + ASSERT_TRUE(rcv.isTimedOut(tsMono(9990000000))); + + ASSERT_LT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); + ASSERT_GE(TransferReceiver::getMaxTransferInterval(), rcv.getInterval()); + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwe")); + ASSERT_EQ(0x5678, rcv.getLastTransferCrc()); + + /* + * Destruction + */ + ASSERT_TRUE(bufmgr.access(gen.bufmgr_key)); + context.receiver.~TransferReceiver(); // TransferReceiver does not own the buffer, it must not be released! + ASSERT_TRUE(bufmgr.access(gen.bufmgr_key)); // Making sure that the buffer is still there +} + + +TEST(TransferReceiver, OutOfBufferSpace_32bytes) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + /* + * Simple transfer, maximum buffer length + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 1, 100000000), bk)); // 5 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 1, 100000100), bk)); // 12 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET000, 1, 100000200), bk)); // 19 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 1, 100000300), bk)); // 26 + CHECK_COMPLETE( rcv.addFrame(gen(1, "123456", SET010, 1, 100000400), bk)); // 32 + + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567123456712345671234567123456")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); // CRC from "12", which is 0x3231 in little endian + + /* + * Transfer longer than available buffer space + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 2, 100001000), bk)); // 5 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 2, 100001100), bk)); // 12 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET000, 2, 100001200), bk)); // 19 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 2, 100001200), bk)); // 26 + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET010, 2, 100001300), bk)); // 33 // EOT, ignored - lost sync + + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); // Timestamp will not be overriden + ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed + + ASSERT_EQ(1, rcv.yieldErrorCount()); + ASSERT_EQ(0, rcv.yieldErrorCount()); +} + + +TEST(TransferReceiver, OutOfOrderFrames) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 7, 100000000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET000, 7, 100000100), bk)); // Out of order + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 7, 100000200), bk)); // Out of order + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 7, 100000300), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET011, 7, 100000200), bk)); // Out of order + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 7, 100000400), bk)); + + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + ASSERT_EQ(3, rcv.yieldErrorCount()); + ASSERT_EQ(0, rcv.yieldErrorCount()); +} + + +TEST(TransferReceiver, IntervalMeasurement) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + static const int INTERVAL = 1000; + uavcan::TransferID tid; + uint64_t timestamp = 100000000; + + for (int i = 0; i < 1000; i++) + { + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, tid.get(), timestamp), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, tid.get(), timestamp), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, tid.get(), timestamp), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + ASSERT_EQ(timestamp, rcv.getLastTransferTimestampMonotonic().toUSec()); + + timestamp += uint64_t(INTERVAL); + tid.increment(); + } + + ASSERT_EQ(INTERVAL, rcv.getInterval().toUSec()); +} + + +TEST(TransferReceiver, Restart) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + /* + * This transfer looks complete, but must be ignored because of large delay after the first frame + * Args: iface_index, data, set, transfer_id, ts_monotonic [, ts_utc] + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET100, 0, 100), bk)); // Begin + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 0, 100000100), bk)); // Continue 100 sec later, expired + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET010, 0, 100000200), bk)); // Ignored + + /* + * Begins immediately after, encounters a delay 0.9 sec but completes OK + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 0, 100000300), bk)); // Begin + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET001, 0, 100900300), bk)); // 0.9 sec later + CHECK_COMPLETE( rcv.addFrame(gen(1, "1234567", SET010, 0, 100900400), bk)); // OK nevertheless + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + + /* + * Begins OK, gets a timeout, switches to another iface + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET100, 1, 103000500), bk)); // Begin + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET001, 1, 105000500), bk)); // 2 sec later, timeout + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 1, 105000600), bk)); // Same TID, another iface - ignore + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 2, 105000700), bk)); // Not first frame - ignore + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 2, 105000800), bk)); // First, another iface - restart + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 1, 105000600), bk)); // Old iface - ignore + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET001, 2, 105000900), bk)); // Continuing + CHECK_COMPLETE( rcv.addFrame(gen(0, "1234567", SET010, 2, 105000910), bk)); // Done + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + ASSERT_EQ(4, rcv.yieldErrorCount()); + ASSERT_EQ(0, rcv.yieldErrorCount()); +} + + +TEST(TransferReceiver, UtcTransferTimestamping) +{ + Context<32> context; + RxFrameGenerator gen(789); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + + /* + * Zero UTC timestamp must be preserved + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 0, 1, 0), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 0, 2, 0), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 0, 3, 0), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(1, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(0, rcv.getLastTransferTimestampUtc().toUSec()); + + /* + * Non-zero UTC timestamp + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "1234567", SET100, 1, 4, 123), bk)); // This UTC is going to be preserved + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwertyu", SET001, 1, 5, 0), bk)); // Following are ignored + CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", SET010, 1, 6, 42), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(4, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(123, rcv.getLastTransferTimestampUtc().toUSec()); + + /* + * Single-frame transfers + * iface_index, data, set, transfer_id, ts_monotonic + */ + CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "abc", SET110, 2, 10, 100000000), bk)); // Exact value is irrelevant + ASSERT_EQ(10, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(100000000, rcv.getLastTransferTimestampUtc().toUSec()); + + /* + * Restart recovery + */ + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 1, 100000000, 800000000), bk)); + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwertyu", SET001, 1, 100000001, 300000000), bk)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET010, 1, 100000002, 900000000), bk)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567qwertyuabcd")); + ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + ASSERT_EQ(800000000, rcv.getLastTransferTimestampUtc().toUSec()); +} + + +TEST(TransferReceiver, HeaderParsing) +{ + static const std::string SFT_PAYLOAD = "1234567"; + + uavcan::TransferID tid; + + /* + * Broadcast + */ + { + Context<32> context; + RxFrameGenerator gen(123); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + + /* + * MFT, message broadcasting + */ + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast); + uavcan::TransferBufferAccessor bk1(context.bufmgr, gen.bufmgr_key); + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), 1), bk1)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET011, tid.get(), 2), bk1)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + tid.increment(); + bk1.remove(); + } + + /* + * SFT, message broadcasting + */ + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast); + uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key); + + const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), 1000); + + CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk)); + ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero + + // All bytes are payload, zero overhead + ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr())); + + tid.increment(); + } + } + + /* + * Unicast + */ + { + Context<32> context; + RxFrameGenerator gen(123); + uavcan::TransferReceiver& rcv = context.receiver; + uavcan::TransferBufferManager& bufmgr = context.bufmgr; + + static const uavcan::TransferType ADDRESSED_TRANSFER_TYPES[2] = + { + uavcan::TransferTypeServiceRequest, + uavcan::TransferTypeServiceResponse + }; + + /* + * MFT, service request/response + */ + for (unsigned i = 0; i < (sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++) + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]); + uavcan::TransferBufferAccessor bk2(context.bufmgr, gen.bufmgr_key); + + const uint64_t ts_monotonic = i + 10; + + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), ts_monotonic), bk2)); + CHECK_COMPLETE( rcv.addFrame(gen(0, "abcd", SET011, tid.get(), ts_monotonic), bk2)); + + ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd")); + ASSERT_EQ(0x3231, rcv.getLastTransferCrc()); + + tid.increment(); + bk2.remove(); + } + + /* + * SFT, message unicast, service request/response + */ + for (unsigned i = 0; i < int(sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++) + { + gen.bufmgr_key = + uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]); + uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key); + + const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), i + 10000U); + + CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk)); + ASSERT_EQ(0x0000, rcv.getLastTransferCrc()); // Default value - zero + + // First byte must be ignored + ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr())); + + tid.increment(); + } + } + + +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_sender.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_sender.cpp new file mode 100644 index 0000000000..ff27b6fe98 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_sender.cpp @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "transfer_test_helpers.hpp" +#include "can/can.hpp" +#include + +static int sendOne(uavcan::TransferSender& sender, const std::string& data, + uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, + uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id) +{ + return sender.send(reinterpret_cast(data.c_str()), unsigned(data.length()), + uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline), + uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id); +} + +static int sendOne(uavcan::TransferSender& sender, const std::string& data, + uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline, + uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid) +{ + return sender.send(reinterpret_cast(data.c_str()), unsigned(data.length()), + uavcan::MonotonicTime::fromUSec(monotonic_tx_deadline), + uavcan::MonotonicTime::fromUSec(monotonic_blocking_deadline), transfer_type, dst_node_id, tid); +} + + +TEST(TransferSender, Basic) +{ + uavcan::PoolAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + static const uavcan::NodeID TX_NODE_ID(64); + static const uavcan::NodeID RX_NODE_ID(65); + uavcan::Dispatcher dispatcher_tx(driver, poolmgr, clockmock); + uavcan::Dispatcher dispatcher_rx(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher_tx.setNodeID(TX_NODE_ID)); + ASSERT_TRUE(dispatcher_rx.setNodeID(RX_NODE_ID)); + + /* + * Test environment + */ + static const uavcan::DataTypeDescriptor TYPES[2] = + { + makeDataType(uavcan::DataTypeKindMessage, 1), + makeDataType(uavcan::DataTypeKindService, 1) + }; + + uavcan::TransferSender senders[2] = + { + uavcan::TransferSender(dispatcher_tx, TYPES[0], uavcan::CanTxQueue::Volatile), + uavcan::TransferSender(dispatcher_tx, TYPES[1], uavcan::CanTxQueue::Persistent) + }; + + static const std::string DATA[4] = + { + "Don't panic.", + + "The ships hung in the sky in much the same way that bricks don't.", + + "Would it save you a lot of time if I just gave up and went mad now?", + + "If there's anything more important than my ego around, I want it caught and shot now." + }; + + /* + * Transmission + */ + static const uint64_t TX_DEADLINE = 1000000; + + // Low priority + senders[0].setPriority(20); + sendOne(senders[0], DATA[0], TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + sendOne(senders[0], DATA[1], TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + // High priority + senders[0].setPriority(10); + sendOne(senders[0], "123", TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + sendOne(senders[0], "456", TX_DEADLINE, 0, uavcan::TransferTypeMessageBroadcast, 0); + + senders[1].setPriority(15); + sendOne(senders[1], DATA[2], TX_DEADLINE, 0, uavcan::TransferTypeServiceRequest, RX_NODE_ID); + sendOne(senders[1], DATA[3], TX_DEADLINE, 0, uavcan::TransferTypeServiceResponse, RX_NODE_ID, 1); + sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TransferTypeServiceRequest, RX_NODE_ID); + sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TransferTypeServiceResponse, RX_NODE_ID, 2); + + using namespace uavcan; + static const Transfer TRANSFERS[8] = + { + Transfer(TX_DEADLINE, 0, 20, TransferTypeMessageBroadcast, 0, TX_NODE_ID, 0, DATA[0], TYPES[0]), + Transfer(TX_DEADLINE, 0, 20, TransferTypeMessageBroadcast, 1, TX_NODE_ID, 0, DATA[1], TYPES[0]), + Transfer(TX_DEADLINE, 0, 10, TransferTypeMessageBroadcast, 2, TX_NODE_ID, 0, "123", TYPES[0]), + Transfer(TX_DEADLINE, 0, 10, TransferTypeMessageBroadcast, 3, TX_NODE_ID, 0, "456", TYPES[0]), + + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceRequest, 0, TX_NODE_ID, RX_NODE_ID, DATA[2], TYPES[1]), + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceResponse, 1, TX_NODE_ID, RX_NODE_ID, DATA[3], TYPES[1]), + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceRequest, 1, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]), + Transfer(TX_DEADLINE, 0, 15, TransferTypeServiceResponse, 2, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]) + }; + + /* + * Making sure that the abort flag is not used. + */ + ASSERT_EQ(0, driver.ifaces.at(0).tx.front().flags); + + /* + * Receiving on the other side. + */ + for (uint8_t i = 0; i < driver.getNumIfaces(); i++) // Moving the frames from TX to RX side + { + CanIfaceMock& iface = driver.ifaces.at(i); + std::cout << "Num frames: " << iface.tx.size() << std::endl; + while (!iface.tx.empty()) + { + CanIfaceMock::FrameWithTime ft = iface.tx.front(); + iface.tx.pop(); + iface.rx.push(ft); + } + } + + TestListener sub_msg(dispatcher_rx.getTransferPerfCounter(), TYPES[0], 512, poolmgr); + TestListener sub_srv_req(dispatcher_rx.getTransferPerfCounter(), TYPES[1], 512, poolmgr); + TestListener sub_srv_resp(dispatcher_rx.getTransferPerfCounter(), TYPES[1], 512, poolmgr); + + dispatcher_rx.registerMessageListener(&sub_msg); + dispatcher_rx.registerServiceRequestListener(&sub_srv_req); + dispatcher_rx.registerServiceResponseListener(&sub_srv_resp); + + while (true) + { + const int res = dispatcher_rx.spin(tsMono(0)); + ASSERT_LE(0, res); + clockmock.advance(100); + if (res == 0) + { + break; + } + } + + /* + * Validation + */ + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[0])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[1])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[2])); + ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[3])); + + ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[4])); + ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[6])); + + ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[5])); + ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[7])); + + /* + * Perf counters + */ + EXPECT_EQ(0, dispatcher_tx.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(8, dispatcher_tx.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher_tx.getTransferPerfCounter().getRxTransferCount()); + + EXPECT_EQ(0, dispatcher_rx.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(0, dispatcher_rx.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(8, dispatcher_rx.getTransferPerfCounter().getRxTransferCount()); +} + + +struct TransferSenderTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase +{ + uavcan::RxFrame last_frame; + unsigned count; + + TransferSenderTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher) + : uavcan::LoopbackFrameListenerBase(dispatcher) + , count(0) + { + startListening(); + } + + void handleLoopbackFrame(const uavcan::RxFrame& frame) + { + last_frame = frame; + count++; + } +}; + +TEST(TransferSender, Loopback) +{ + uavcan::PoolAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + static const uavcan::NodeID TX_NODE_ID(64); + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + ASSERT_TRUE(dispatcher.setNodeID(TX_NODE_ID)); + + uavcan::DataTypeDescriptor desc = makeDataType(uavcan::DataTypeKindMessage, 1, "Foobar"); + + uavcan::TransferSender sender(dispatcher, desc, uavcan::CanTxQueue::Volatile); + + sender.setCanIOFlags(uavcan::CanIOFlagLoopback); + ASSERT_EQ(uavcan::CanIOFlagLoopback, sender.getCanIOFlags()); + + sender.setIfaceMask(2); + ASSERT_EQ(2, sender.getIfaceMask()); + + TransferSenderTestLoopbackFrameListener listener(dispatcher); + + ASSERT_LE(0, sender.send(reinterpret_cast("123"), 3, tsMono(1000), tsMono(0), + uavcan::TransferTypeMessageBroadcast, 0)); + + ASSERT_EQ(0, listener.count); + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + ASSERT_EQ(1, listener.count); + ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); + ASSERT_EQ(3, listener.last_frame.getPayloadLen()); + ASSERT_TRUE(TX_NODE_ID == listener.last_frame.getSrcNodeID()); + ASSERT_TRUE(listener.last_frame.isEndOfTransfer()); + + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); +} + +TEST(TransferSender, PassiveMode) +{ + uavcan::PoolAllocator poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock); + + uavcan::TransferSender sender(dispatcher, makeDataType(uavcan::DataTypeKindMessage, 123), + uavcan::CanTxQueue::Volatile); + + static const uint8_t Payload[] = {1, 2, 3, 4, 5}; + + // By default, sending in passive mode is not enabled + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // Overriding the default + sender.allowAnonymousTransfers(); + + // OK, now we can broadcast in any mode + ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // ...but not unicast or anything else + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeServiceRequest, uavcan::NodeID(42))); + + // Making sure the abort flag is set + ASSERT_FALSE(driver.ifaces.at(0).tx.empty()); + ASSERT_EQ(uavcan::CanIOFlagAbortOnError, driver.ifaces.at(0).tx.front().flags); + + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.cpp new file mode 100644 index 0000000000..e3da982602 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.cpp @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include "transfer_test_helpers.hpp" +#include "../clock.hpp" + + +TEST(TransferTestHelpers, Transfer) +{ + uavcan::PoolAllocator pool; + + uavcan::TransferBufferManager mgr(128, pool); + uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageBroadcast)); + + uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0), + uavcan::MonotonicTime(), uavcan::UtcTime(), 0); + frame.setEndOfTransfer(true); + uavcan::MultiFrameIncomingTransfer mfit(tsMono(10), tsUtc(1000), frame, tba); + + // Filling the buffer with data + static const std::string TEST_DATA = "Kaneda! What do you see? Kaneda! What do you see? Kaneda! Kaneda!!!"; + ASSERT_TRUE(tba.create()); + ASSERT_EQ(TEST_DATA.length(), tba.access()->write(0, reinterpret_cast(TEST_DATA.c_str()), + unsigned(TEST_DATA.length()))); + + // Reading back + const Transfer transfer(mfit, uavcan::DataTypeDescriptor()); + ASSERT_EQ(TEST_DATA, transfer.payload); +} + + +TEST(TransferTestHelpers, MFTSerialization) +{ + uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "Foo"); + + static const std::string DATA = "To go wrong in one's own way is better than to go right in someone else's."; + const Transfer transfer(1, 100000, 10, + uavcan::TransferTypeServiceRequest, 2, 42, 127, DATA, type); + + const std::vector ser = serializeTransfer(transfer); + + std::cout << "Serialized transfer:\n"; + for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) + { + std::cout << "\t" << it->toString() << "\n"; + } + + for (std::vector::const_iterator it = ser.begin(); it != ser.end(); ++it) + { + std::cout << "\t'"; + for (unsigned i = 0; i < it->getPayloadLen(); i++) + { + uint8_t ch = it->getPayloadPtr()[i]; + if (ch < 0x20 || ch > 0x7E) + { + ch = '.'; + } + std::cout << static_cast(ch); + } + std::cout << "'\n"; + } + std::cout << std::flush; +} + + +TEST(TransferTestHelpers, SFTSerialization) +{ + uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "Foo"); + + { + const Transfer transfer(1, 100000, 10, + uavcan::TransferTypeMessageBroadcast, 7, 42, 0, "Nvrfrgt", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, 11, + uavcan::TransferTypeServiceRequest, 7, 42, 127, "7-chars", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, 12, + uavcan::TransferTypeMessageBroadcast, 7, 42, 0, "", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } + { + const Transfer transfer(1, 100000, 13, + uavcan::TransferTypeServiceResponse, 7, 42, 127, "", type); + const std::vector ser = serializeTransfer(transfer); + ASSERT_EQ(1, ser.size()); + std::cout << "Serialized transfer:\n\t" << ser[0].toString() << "\n"; + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.hpp b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.hpp new file mode 100644 index 0000000000..14d6ccc8b1 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/transport/transfer_test_helpers.hpp @@ -0,0 +1,320 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * UAVCAN transfer representation used in various tests. + */ +struct Transfer +{ + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferPriority priority; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::NodeID dst_node_id; + uavcan::DataTypeDescriptor data_type; + std::string payload; + + Transfer(const uavcan::IncomingTransfer& tr, const uavcan::DataTypeDescriptor& data_type) + : ts_monotonic(tr.getMonotonicTimestamp()) + , ts_utc(tr.getUtcTimestamp()) + , priority(tr.getPriority()) + , transfer_type(tr.getTransferType()) + , transfer_id(tr.getTransferID()) + , src_node_id(tr.getSrcNodeID()) + , dst_node_id() // default is invalid + , data_type(data_type) + { + unsigned offset = 0; + while (true) + { + uint8_t buf[256]; + int res = tr.read(offset, buf, sizeof(buf)); + if (res < 0) + { + std::cout << "IncomingTransferContainer: read failure " << res << std::endl; + exit(1); + } + if (res == 0) + { + break; + } + payload += std::string(reinterpret_cast(buf), unsigned(res)); + offset += unsigned(res); + } + } + + Transfer(uavcan::MonotonicTime ts_monotonic, uavcan::UtcTime ts_utc, uavcan::TransferPriority priority, + uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, + uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type) + : ts_monotonic(ts_monotonic) + , ts_utc(ts_utc) + , priority(priority) + , transfer_type(transfer_type) + , transfer_id(transfer_id) + , src_node_id(src_node_id) + , dst_node_id(dst_node_id) + , data_type(data_type) + , payload(payload) + { } + + Transfer(uint64_t ts_monotonic, uint64_t ts_utc, uavcan::TransferPriority priority, + uavcan::TransferType transfer_type, uavcan::TransferID transfer_id, uavcan::NodeID src_node_id, + uavcan::NodeID dst_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& data_type) + : ts_monotonic(uavcan::MonotonicTime::fromUSec(ts_monotonic)) + , ts_utc(uavcan::UtcTime::fromUSec(ts_utc)) + , priority(priority) + , transfer_type(transfer_type) + , transfer_id(transfer_id) + , src_node_id(src_node_id) + , dst_node_id(dst_node_id) + , data_type(data_type) + , payload(payload) + { } + + bool operator==(const Transfer& rhs) const + { + return + (ts_monotonic == rhs.ts_monotonic) && + ((!ts_utc.isZero() && !rhs.ts_utc.isZero()) ? (ts_utc == rhs.ts_utc) : true) && + (priority == rhs.priority) && + (transfer_type == rhs.transfer_type) && + (transfer_id == rhs.transfer_id) && + (src_node_id == rhs.src_node_id) && + ((dst_node_id.isValid() && rhs.dst_node_id.isValid()) ? (dst_node_id == rhs.dst_node_id) : true) && + (data_type == rhs.data_type) && + (payload == rhs.payload); + } + + std::string toString() const + { + std::ostringstream os; + os << "ts_m=" << ts_monotonic + << " ts_utc=" << ts_utc + << " prio=" << int(priority.get()) + << " tt=" << int(transfer_type) + << " tid=" << int(transfer_id.get()) + << " snid=" << int(src_node_id.get()) + << " dnid=" << int(dst_node_id.get()) + << " dtid=" << int(data_type.getID().get()) + << "\n\t'" << payload << "'"; + return os.str(); + } +}; + +/** + * This subscriber accepts any types of transfers - this makes testing easier. + * In reality, uavcan::TransferListener should accept only specific transfer types + * which are dispatched/filtered by uavcan::Dispatcher. + */ +class TestListener : public uavcan::TransferListener +{ + typedef uavcan::TransferListener Base; + + std::queue transfers_; + +public: + TestListener(uavcan::TransferPerfCounter& perf, const uavcan::DataTypeDescriptor& data_type, + uavcan::uint16_t max_buffer_size, uavcan::IPoolAllocator& allocator) + : Base(perf, data_type, max_buffer_size, allocator) + { } + + void handleIncomingTransfer(uavcan::IncomingTransfer& transfer) + { + const Transfer rx(transfer, Base::getDataTypeDescriptor()); + transfers_.push(rx); + std::cout << "Received transfer: " << rx.toString() << std::endl; + + const bool single_frame = dynamic_cast(&transfer) != UAVCAN_NULLPTR; + + const bool anonymous = single_frame && + transfer.getSrcNodeID().isBroadcast() && + (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast); + + ASSERT_EQ(anonymous, transfer.isAnonymousTransfer()); + } + + bool matchAndPop(const Transfer& reference) + { + if (transfers_.empty()) + { + std::cout << "No received transfers" << std::endl; + return false; + } + + const Transfer tr = transfers_.front(); + transfers_.pop(); + + const bool res = (tr == reference); + if (!res) + { + std::cout << "TestSubscriber: Transfer mismatch:\n" + << "Expected: " << reference.toString() << "\n" + << "Received: " << tr.toString() << std::endl; + } + return res; + } + + unsigned getNumReceivedTransfers() const { return static_cast(transfers_.size()); } + bool isEmpty() const { return transfers_.empty(); } +}; + + +namespace +{ + +std::vector serializeTransfer(const Transfer& transfer) +{ + const bool need_crc = transfer.payload.length() > (sizeof(uavcan::CanFrame::data) - 1); + + std::vector raw_payload; + if (need_crc) + { + uavcan::TransferCRC payload_crc = transfer.data_type.getSignature().toTransferCRC(); + payload_crc.add(reinterpret_cast(transfer.payload.c_str()), uint16_t(transfer.payload.length())); + // Little endian + raw_payload.push_back(uint8_t(payload_crc.get() & 0xFF)); + raw_payload.push_back(uint8_t((payload_crc.get() >> 8) & 0xFF)); + } + raw_payload.insert(raw_payload.end(), transfer.payload.begin(), transfer.payload.end()); + + std::vector output; + unsigned offset = 0; + uavcan::MonotonicTime ts_monotonic = transfer.ts_monotonic; + uavcan::UtcTime ts_utc = transfer.ts_utc; + + uavcan::Frame frm(transfer.data_type.getID(), transfer.transfer_type, transfer.src_node_id, + transfer.dst_node_id, transfer.transfer_id); + frm.setStartOfTransfer(true); + frm.setPriority(transfer.priority); + + while (true) + { + const int bytes_left = int(raw_payload.size()) - int(offset); + EXPECT_TRUE(bytes_left >= 0); + + const int spres = frm.setPayload(&*(raw_payload.begin() + offset), unsigned(bytes_left)); + if (spres < 0) + { + std::cerr << ">_<" << std::endl; + std::exit(1); + } + if (spres == bytes_left) + { + frm.setEndOfTransfer(true); + } + + offset += unsigned(spres); + + const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0); + ts_monotonic += uavcan::MonotonicDuration::fromUSec(1); + ts_utc += uavcan::UtcDuration::fromUSec(1); + + output.push_back(rxfrm); + if (frm.isEndOfTransfer()) + { + break; + } + + frm.setStartOfTransfer(false); + frm.flipToggle(); + } + return output; +} + +inline uavcan::DataTypeDescriptor makeDataType(uavcan::DataTypeKind kind, uint16_t id, const char* name = "") +{ + const uavcan::DataTypeSignature signature((uint64_t(kind) << 16) | uint16_t(id << 8) | uint16_t(id & 0xFF)); + return uavcan::DataTypeDescriptor(kind, id, signature, name); +} + +} + + +class IncomingTransferEmulatorBase +{ + uavcan::MonotonicTime ts_; + uavcan::TransferID tid_; + uavcan::NodeID dst_node_id_; + +public: + IncomingTransferEmulatorBase(uavcan::NodeID dst_node_id) + : dst_node_id_(dst_node_id) + { } + + virtual ~IncomingTransferEmulatorBase() { } + + Transfer makeTransfer(uavcan::TransferPriority priority, uavcan::TransferType transfer_type, + uint8_t source_node_id, const std::string& payload, const uavcan::DataTypeDescriptor& type, + uavcan::NodeID dst_node_id_override = uavcan::NodeID()) + { + ts_ += uavcan::MonotonicDuration::fromUSec(100); + const uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(ts_.toUSec() + 1000000000ul); + const uavcan::NodeID dst_node_id = (transfer_type == uavcan::TransferTypeMessageBroadcast) ? + uavcan::NodeID::Broadcast : + (dst_node_id_override.isValid() ? dst_node_id_override : dst_node_id_); + const Transfer tr(ts_, utc, priority, transfer_type, tid_, source_node_id, dst_node_id, payload, type); + tid_.increment(); + return tr; + } + + virtual void sendOneFrame(const uavcan::RxFrame& frame) = 0; + + void send(const std::vector >& sers) + { + unsigned index = 0; + while (true) + { + // Sending all transfers concurrently + bool all_empty = true; + for (std::vector >::const_iterator it = sers.begin(); it != sers.end(); ++it) + { + if (it->size() <= index) + { + continue; + } + all_empty = false; + std::cout << "Incoming Transfer Emulator: Sending: " << it->at(index).toString() << std::endl; + sendOneFrame(it->at(index)); + } + index++; + if (all_empty) + { + break; + } + } + } + + void send(const Transfer* transfers, unsigned num_transfers) + { + std::vector > sers; + while (num_transfers--) + { + sers.push_back(serializeTransfer(*transfers++)); + } + send(sers); + } + + template void send(const Transfer (&transfers)[SIZE]) { send(transfers, SIZE); } +}; + +/** + * Zero allocator - always fails + */ +class NullAllocator : public uavcan::IPoolAllocator +{ +public: + virtual void* allocate(std::size_t) { return UAVCAN_NULLPTR; } + virtual void deallocate(const void*) { } + virtual uint16_t getBlockCapacity() const { return 0; } +}; diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/comparison.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/comparison.cpp new file mode 100644 index 0000000000..10ef7042d6 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/comparison.cpp @@ -0,0 +1,163 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +TEST(Comparison, Basic) +{ + // Basic same type floats + ASSERT_TRUE(uavcan::areClose(0.1, 0.1)); + ASSERT_TRUE(uavcan::areClose(0.1F, 0.1F)); + ASSERT_TRUE(uavcan::areClose(0.1L, 0.1L)); + + // Basic mixed type floats + ASSERT_TRUE(uavcan::areClose(0.1F, 0.1)); + ASSERT_TRUE(uavcan::areClose(0.1, 0.1F)); + ASSERT_TRUE(uavcan::areClose(0.1F, 0.1L)); + ASSERT_TRUE(uavcan::areClose(0.1L, 0.1F)); + ASSERT_TRUE(uavcan::areClose(0.1, 0.1L)); + ASSERT_TRUE(uavcan::areClose(0.1L, 0.1)); + + // Basic floats + ASSERT_TRUE(uavcan::areClose(0x07, '\x07')); + ASSERT_TRUE(uavcan::areClose(123456789LL, 123456789)); + ASSERT_TRUE(uavcan::areClose("123", std::string("123"))); + + // Non-equality + ASSERT_FALSE(uavcan::areClose(-0.1, 0.1)); + ASSERT_FALSE(uavcan::areClose(-0.1F, 0.0L)); + ASSERT_FALSE(uavcan::areClose("123", std::string("12"))); + ASSERT_FALSE(uavcan::areClose(0x07L, '\0')); +} + +TEST(Comparison, FloatSpecialCase) +{ + ASSERT_FALSE(uavcan::areClose(0.1, std::numeric_limits::infinity())); + + ASSERT_TRUE(uavcan::areClose(std::numeric_limits::infinity(), + std::numeric_limits::infinity())); + + ASSERT_FALSE(uavcan::areClose(std::numeric_limits::infinity(), -std::numeric_limits::infinity())); + + ASSERT_FALSE(uavcan::areClose(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN())); +} + +TEST(Comparison, FloatULP) +{ + ASSERT_FALSE(uavcan::areClose(0.100000000000000001L, 0.1L)); + ASSERT_TRUE( uavcan::areClose(0.100000000000000001, 0.1L)); + ASSERT_TRUE( uavcan::areClose(0.100000000000000001F, 0.1L)); + + // Near zero + ASSERT_TRUE( uavcan::areClose(0.0F, std::numeric_limits::epsilon())); + ASSERT_TRUE( uavcan::areClose(0.0F, -std::numeric_limits::epsilon())); + ASSERT_FALSE(uavcan::areClose(0.0F, std::numeric_limits::epsilon() * 2)); +} + +TEST(Comparison, BruteforceValidation) +{ + const std::streamsize default_precision = std::cout.precision(); + std::cout.precision(20); + + float x = -uavcan::NumericTraits::max(); + + while (x < uavcan::NumericTraits::max()) + { + const float y1 = x + std::abs(x) * (uavcan::NumericTraits::epsilon() * 2); // Still equal + const float y2 = x + uavcan::max(std::abs(x), 1.F) * (uavcan::NumericTraits::epsilon() * 20); // Nope + + if (!uavcan::areClose(y1, x)) + { + std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl; + ASSERT_TRUE(false); + } + if (uavcan::areClose(y2, x)) + { + std::cout << "y1=" << y1 << " y2=" << y2 << " x=" << x << std::endl; + ASSERT_TRUE(false); + } + + x = y2; + } + + std::cout.precision(default_precision); +} + + +struct B +{ + long double b; + B(long double val = 0.0L) : b(val) { } +}; + +struct A +{ + float a; + explicit A(float val = 0.0F) : a(val) { } + + bool isClose(A rhs) const + { + std::cout << "bool A::isClose(A) --> " << rhs.a << std::endl; + return uavcan::areClose(a, rhs.a); + } + + bool isClose(const B& rhs) const + { + std::cout << "bool A::isClose(const B&) --> " << rhs.b << std::endl; + return uavcan::areClose(a, rhs.b); + } +}; + +struct C +{ + long long c; + explicit C(long long val = 0.0L) : c(val) { } + + bool operator==(B rhs) const + { + std::cout << "bool C::operator==(B) --> " << rhs.b << std::endl; + return c == static_cast(rhs.b); + } +}; + +TEST(Comparison, IsCloseMethod) +{ + B b; + A a; + C c; + + std::cout << 1 << std::endl; + ASSERT_TRUE(uavcan::areClose(a, b)); // Fuzzy + ASSERT_TRUE(uavcan::areClose(a, A())); // Fuzzy + ASSERT_TRUE(uavcan::areClose(b, a)); // Fuzzy, reverse application + ASSERT_TRUE(uavcan::areClose(c, b)); // Exact + + std::cout << 2 << std::endl; + + a.a = uavcan::NumericTraits::epsilon(); + + ASSERT_TRUE(uavcan::areClose(a, b)); + ASSERT_TRUE(uavcan::areClose(b, a)); + ASSERT_TRUE(a.isClose(b)); + ASSERT_TRUE(a.isClose(A())); + ASSERT_TRUE(uavcan::areClose(A(), a)); + + std::cout << 3 << std::endl; + + a.a = 1e-5F; + + ASSERT_FALSE(uavcan::areClose(a, b)); + ASSERT_FALSE(uavcan::areClose(b, a)); + ASSERT_FALSE(uavcan::areClose(A(), a)); + + std::cout << 4 << std::endl; + + b.b = 1.1L; + c.c = 1; + + ASSERT_TRUE(uavcan::areClose(c, b)); // Round to integer + ASSERT_TRUE(uavcan::areClose(c, 1.0L)); // Implicit cast to B + ASSERT_FALSE(uavcan::areClose(c, 0.0L)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/lazy_constructor.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/lazy_constructor.cpp new file mode 100644 index 0000000000..778a5c0674 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/lazy_constructor.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + + +TEST(LazyConstructor, Basic) +{ + using ::uavcan::LazyConstructor; + + LazyConstructor a; + LazyConstructor b; + + ASSERT_FALSE(a); + ASSERT_FALSE(b.isConstructed()); + + /* + * Construction + */ + a.destroy(); // no-op + a.construct(); + b.construct("Hello world"); + + ASSERT_TRUE(a); + ASSERT_TRUE(b.isConstructed()); + + ASSERT_NE(*a, *b); + ASSERT_STRNE(a->c_str(), b->c_str()); + + ASSERT_EQ(*a, ""); + ASSERT_EQ(*b, "Hello world"); + + /* + * Copying + */ + a = b; // Assignment operator performs destruction and immediate copy construction + ASSERT_EQ(*a, *b); + ASSERT_EQ(*a, "Hello world"); + + LazyConstructor c(a); // Copy constructor call is forwarded to std::string + + ASSERT_EQ(*c, *a); + + *a = "123"; + ASSERT_NE(*c, *a); + ASSERT_EQ(*c, *b); + + *c = "456"; + ASSERT_NE(*a, *c); + ASSERT_NE(*b, *a); + ASSERT_NE(*c, *b); + + /* + * Destruction + */ + ASSERT_TRUE(c); + c.destroy(); + ASSERT_FALSE(c); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/linked_list.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/linked_list.cpp new file mode 100644 index 0000000000..8910bb8684 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/linked_list.cpp @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +struct ListItem : uavcan::LinkedListNode +{ + int value; + + ListItem(int value = 0) + : value(value) + { } + + struct GreaterThanComparator + { + const int compare_with; + + GreaterThanComparator(int compare_with) + : compare_with(compare_with) + { } + + bool operator()(const ListItem* item) const + { + return item->value > compare_with; + } + }; + + void insort(uavcan::LinkedListRoot& root) + { + root.insertBefore(this, GreaterThanComparator(value)); + } +}; + +TEST(LinkedList, Basic) +{ + uavcan::LinkedListRoot root; + + /* + * Insert/remove + */ + EXPECT_EQ(0, root.getLength()); + + ListItem item1; + root.insert(&item1); + root.insert(&item1); // Insert twice - second will be ignored + EXPECT_EQ(1, root.getLength()); + + root.remove(&item1); + root.remove(&item1); + EXPECT_EQ(0, root.getLength()); + + ListItem items[3]; + root.insert(&item1); + root.insert(items + 0); + root.insert(items + 1); + root.insert(items + 2); + EXPECT_EQ(4, root.getLength()); + + /* + * Order persistence + */ + items[0].value = 10; + items[1].value = 11; + items[2].value = 12; + const int expected_values[] = {12, 11, 10, 0}; + ListItem* node = root.get(); + for (int i = 0; i < 4; i++) + { + EXPECT_EQ(expected_values[i], node->value); + node = node->getNextListNode(); + } + + root.remove(items + 0); + root.remove(items + 2); + root.remove(items + 2); + EXPECT_EQ(2, root.getLength()); + + const int expected_values2[] = {11, 0}; + node = root.get(); + for (int i = 0; i < 2; i++) + { + EXPECT_EQ(expected_values2[i], node->value); + node = node->getNextListNode(); + } + + root.insert(items + 2); + EXPECT_EQ(3, root.getLength()); + EXPECT_EQ(12, root.get()->value); + + /* + * Emptying + */ + root.remove(&item1); + root.remove(items + 0); + root.remove(items + 1); + root.remove(items + 2); + EXPECT_EQ(0, root.getLength()); +} + +TEST(LinkedList, Sorting) +{ + uavcan::LinkedListRoot root; + ListItem items[6]; + for (int i = 0; i < 6; i++) + { + items[i].value = i; + } + + EXPECT_EQ(0, root.getLength()); + + items[2].insort(root); + EXPECT_EQ(1, root.getLength()); + + items[2].insort(root); // Making sure the duplicate will be removed + EXPECT_EQ(1, root.getLength()); + + items[3].insort(root); + + items[0].insort(root); + + items[4].insort(root); + EXPECT_EQ(4, root.getLength()); + + items[1].insort(root); + EXPECT_EQ(5, root.getLength()); + + items[1].insort(root); // Another duplicate + EXPECT_EQ(5, root.getLength()); + + items[5].insort(root); + + EXPECT_EQ(6, root.getLength()); + + int prev_val = -100500; + const ListItem* item = root.get(); + while (item) + { + //std::cout << item->value << std::endl; + EXPECT_LT(prev_val, item->value); + prev_val = item->value; + item = item->getNextListNode(); + } +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/map.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/map.cpp new file mode 100644 index 0000000000..89c2faf816 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/map.cpp @@ -0,0 +1,224 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include + + +/* + * TODO: This one test has been temporarily disabled because it is not compatible with newer versions of libstdc++ + * that ship with newer versions of GCC. The problem is that std::string has become too large to fit into a 64-byte + * large memory block. This should be fixed in the future. + */ +#if 0 +static std::string toString(long x) +{ + char buf[80]; + std::snprintf(buf, sizeof(buf), "%li", x); + return std::string(buf); +} + +static bool oddValuePredicate(const std::string& key, const std::string& value) +{ + EXPECT_FALSE(key.empty()); + EXPECT_FALSE(value.empty()); + const int num = atoi(value.c_str()); + return num & 1; +} + +struct KeyFindPredicate +{ + const std::string target; + KeyFindPredicate(std::string target) : target(target) { } + bool operator()(const std::string& key, const std::string&) const { return key == target; } +}; + +struct ValueFindPredicate +{ + const std::string target; + ValueFindPredicate(std::string target) : target(target) { } + bool operator()(const std::string&, const std::string& value) const { return value == target; } +}; + + +TEST(Map, Basic) +{ + using uavcan::Map; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Map MapType; + std::unique_ptr map(new MapType(pool)); + + // Empty + ASSERT_FALSE(map->access("hi")); + map->remove("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(map->getByIndex(0)); + ASSERT_FALSE(map->getByIndex(1)); + ASSERT_FALSE(map->getByIndex(10000)); + + // Insertion + ASSERT_EQ("a", *map->insert("1", "a")); + ASSERT_EQ("b", *map->insert("2", "b")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_EQ(2, map->getSize()); + + // Ordering + ASSERT_TRUE(map->getByIndex(0)->match("1")); + ASSERT_TRUE(map->getByIndex(1)->match("2")); + + // Insertion + ASSERT_EQ("c", *map->insert("3", "c")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + + ASSERT_EQ("d", *map->insert("4", "d")); + ASSERT_EQ(2, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block + ASSERT_EQ(4, map->getSize()); + + // Making sure everything is here + ASSERT_EQ("a", *map->access("1")); + ASSERT_EQ("b", *map->access("2")); + ASSERT_EQ("c", *map->access("3")); + ASSERT_EQ("d", *map->access("4")); + ASSERT_FALSE(map->access("hi")); + + // Modifying existing entries + *map->access("1") = "A"; + *map->access("2") = "B"; + *map->access("3") = "C"; + *map->access("4") = "D"; + ASSERT_EQ("A", *map->access("1")); + ASSERT_EQ("B", *map->access("2")); + ASSERT_EQ("C", *map->access("3")); + ASSERT_EQ("D", *map->access("4")); + + // Finding some keys + ASSERT_EQ("1", *map->find(KeyFindPredicate("1"))); + ASSERT_EQ("2", *map->find(KeyFindPredicate("2"))); + ASSERT_EQ("3", *map->find(KeyFindPredicate("3"))); + ASSERT_EQ("4", *map->find(KeyFindPredicate("4"))); + ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_key"))); + + // Finding some values + ASSERT_EQ("1", *map->find(ValueFindPredicate("A"))); + ASSERT_EQ("2", *map->find(ValueFindPredicate("B"))); + ASSERT_EQ("3", *map->find(ValueFindPredicate("C"))); + ASSERT_EQ("4", *map->find(ValueFindPredicate("D"))); + ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_value"))); + + // Removing one + map->remove("1"); // One of dynamics now migrates to the static storage + map->remove("foo"); // There's no such thing anyway + ASSERT_EQ(2, pool.getNumUsedBlocks()); + ASSERT_EQ(3, map->getSize()); + + ASSERT_FALSE(map->access("1")); + ASSERT_EQ("B", *map->access("2")); + ASSERT_EQ("C", *map->access("3")); + ASSERT_EQ("D", *map->access("4")); + + // Removing another + map->remove("2"); + ASSERT_EQ(2, map->getSize()); + ASSERT_EQ(2, pool.getNumUsedBlocks()); + + ASSERT_FALSE(map->access("1")); + ASSERT_FALSE(map->access("2")); + ASSERT_EQ("C", *map->access("3")); + ASSERT_EQ("D", *map->access("4")); + + // Adding some new + unsigned max_key_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string key = toString(i); + const std::string value = toString(i); + std::string* res = map->insert(key, value); // Will override some from the above + if (res == UAVCAN_NULLPTR) + { + ASSERT_LT(2, i); + break; + } + else + { + ASSERT_EQ(value, *res); + } + max_key_integer = unsigned(i); + } + std::cout << "Max key/value: " << max_key_integer << std::endl; + ASSERT_LT(4, max_key_integer); + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(map->insert("nonexistent", "value")); + ASSERT_FALSE(map->access("nonexistent")); + ASSERT_FALSE(map->access("value")); + + // Removing odd values - nearly half of them + map->removeAllWhere(oddValuePredicate); + + // Making sure there's no odd values left + for (unsigned kv_int = 0; kv_int <= max_key_integer; kv_int++) + { + const std::string* val = map->access(toString(kv_int)); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Making sure the memory will be released + map.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} +#endif + + +TEST(Map, PrimitiveKey) +{ + using uavcan::Map; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Map MapType; + std::unique_ptr map(new MapType(pool)); + + // Empty + ASSERT_FALSE(map->access(1)); + map->remove(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, map->getSize()); + ASSERT_FALSE(map->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *map->insert(1, 1)); + ASSERT_EQ(1, map->getSize()); + ASSERT_EQ(2, *map->insert(2, 2)); + ASSERT_EQ(2, map->getSize()); + ASSERT_EQ(3, *map->insert(3, 3)); + ASSERT_EQ(4, *map->insert(4, 4)); + ASSERT_EQ(4, map->getSize()); + + // Ordering + ASSERT_TRUE(map->getByIndex(0)->match(1)); + ASSERT_TRUE(map->getByIndex(1)->match(2)); + ASSERT_TRUE(map->getByIndex(2)->match(3)); + ASSERT_TRUE(map->getByIndex(3)->match(4)); + ASSERT_FALSE(map->getByIndex(5)); + ASSERT_FALSE(map->getByIndex(1000)); +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/multiset.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/multiset.cpp new file mode 100644 index 0000000000..c3f227f865 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/multiset.cpp @@ -0,0 +1,267 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#if __GNUC__ +# pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant" +#endif + +#include +#include +#include +#include +#include + + +static std::string toString(long x) +{ + char buf[80]; + std::snprintf(buf, sizeof(buf), "%li", x); + return std::string(buf); +} + +static bool oddValuePredicate(const std::string& value) +{ + EXPECT_FALSE(value.empty()); + const int num = atoi(value.c_str()); + return num & 1; +} + +struct FindPredicate +{ + const std::string target; + FindPredicate(const std::string& target) : target(target) { } + bool operator()(const std::string& value) const { return value == target; } +}; + +struct NoncopyableWithCounter : uavcan::Noncopyable +{ + static int num_objects; + long long value; + + NoncopyableWithCounter() : value(0) { num_objects++; } + NoncopyableWithCounter(long long x) : value(x) { num_objects++; } + ~NoncopyableWithCounter() { num_objects--; } + + static bool isNegative(const NoncopyableWithCounter& val) + { + return val.value < 0; + } + + bool operator==(const NoncopyableWithCounter& ref) const { return ref.value == value; } +}; + +int NoncopyableWithCounter::num_objects = 0; + +template +struct SummationOperator : uavcan::Noncopyable +{ + T accumulator; + SummationOperator() : accumulator() { } + void operator()(const T& x) { accumulator += x; } +}; + +struct ClearingOperator +{ + template + void operator()(T& x) const { x = T(); } +}; + + +TEST(Multiset, Basic) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 4; + uavcan::PoolAllocator pool; + + typedef Multiset MultisetType; + std::unique_ptr mset(new MultisetType(pool)); + + typedef SummationOperator StringConcatenationOperator; + + // Empty + mset->removeFirst("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(mset->getByIndex(0)); + ASSERT_FALSE(mset->getByIndex(1)); + ASSERT_FALSE(mset->getByIndex(10000)); + + // Static addion + ASSERT_EQ("1", *mset->emplace("1")); + ASSERT_EQ("2", *mset->emplace("2")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + ASSERT_EQ(2, mset->getSize()); + + { + StringConcatenationOperator op; + mset->forEach(op); + ASSERT_EQ(2, op.accumulator.size()); + } + + // Dynamic addition + ASSERT_EQ("3", *mset->emplace("3")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + + ASSERT_EQ("4", *mset->emplace("4")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + ASSERT_EQ(4, mset->getSize()); + + ASSERT_FALSE(mset->getByIndex(100)); + ASSERT_FALSE(mset->getByIndex(4)); + + // Finding some items + ASSERT_EQ("1", *mset->find(FindPredicate("1"))); + ASSERT_EQ("2", *mset->find(FindPredicate("2"))); + ASSERT_EQ("3", *mset->find(FindPredicate("3"))); + ASSERT_EQ("4", *mset->find(FindPredicate("4"))); + ASSERT_FALSE(mset->find(FindPredicate("nonexistent"))); + + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator: " << op.accumulator << std::endl; + ASSERT_EQ(4, op.accumulator.size()); + } + + // Removing some + mset->removeFirst("1"); + mset->removeFirst("foo"); // There's no such thing anyway + mset->removeFirst("2"); + + // Adding some new items + unsigned max_value_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string value = toString(i); + std::string* res = mset->emplace(value); // Will NOT override above + if (res == UAVCAN_NULLPTR) + { + ASSERT_LT(1, i); + break; + } + else + { + ASSERT_EQ(value, *res); + } + max_value_integer = unsigned(i); + } + std::cout << "Max value: " << max_value_integer << std::endl; + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(mset->emplace("nonexistent")); + + // Removing odd values - nearly half of them + mset->removeAllWhere(oddValuePredicate); + + // Making sure there's no odd values left + for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++) + { + const std::string* val = mset->find(FindPredicate(toString(kv_int))); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Clearing all strings + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator before clearing: " << op.accumulator << std::endl; + } + mset->forEach(ClearingOperator()); + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator after clearing: " << op.accumulator << std::endl; + ASSERT_TRUE(op.accumulator.empty()); + } + + // Making sure the memory will be released + mset.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +TEST(Multiset, PrimitiveKey) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Multiset MultisetType; + std::unique_ptr mset(new MultisetType(pool)); + + // Empty + mset->removeFirst(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getSize()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *mset->emplace(1)); + ASSERT_EQ(1, mset->getSize()); + ASSERT_EQ(2, *mset->emplace(2)); + ASSERT_EQ(2, mset->getSize()); + ASSERT_EQ(3, *mset->emplace(3)); + ASSERT_EQ(4, *mset->emplace(4)); + ASSERT_EQ(4, mset->getSize()); + + // Summation and clearing + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(1 + 2 + 3 + 4, summation_operator.accumulator); + } + mset->forEach(ClearingOperator()); + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(0, summation_operator.accumulator); + } +} + + +TEST(Multiset, NoncopyableWithCounter) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + + typedef Multiset MultisetType; + std::unique_ptr mset(new MultisetType(pool)); + + ASSERT_EQ(0, NoncopyableWithCounter::num_objects); + ASSERT_EQ(0, mset->emplace()->value); + ASSERT_EQ(1, NoncopyableWithCounter::num_objects); + ASSERT_EQ(123, mset->emplace(123)->value); + ASSERT_EQ(2, NoncopyableWithCounter::num_objects); + ASSERT_EQ(-456, mset->emplace(-456)->value); + ASSERT_EQ(3, NoncopyableWithCounter::num_objects); + ASSERT_EQ(456, mset->emplace(456)->value); + ASSERT_EQ(4, NoncopyableWithCounter::num_objects); + ASSERT_EQ(-789, mset->emplace(-789)->value); + ASSERT_EQ(5, NoncopyableWithCounter::num_objects); + + mset->removeFirst(NoncopyableWithCounter(0)); + ASSERT_EQ(4, NoncopyableWithCounter::num_objects); + + mset->removeFirstWhere(&NoncopyableWithCounter::isNegative); + ASSERT_EQ(3, NoncopyableWithCounter::num_objects); + + mset->removeAllWhere(&NoncopyableWithCounter::isNegative); + ASSERT_EQ(2, NoncopyableWithCounter::num_objects); // Only 1 and 2 are left + + mset.reset(); + + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, NoncopyableWithCounter::num_objects); // All destroyed +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/test/util/templates.cpp b/src/drivers/uavcan/libdronecan/libuavcan/test/util/templates.cpp new file mode 100644 index 0000000000..dc234dd7f7 --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/test/util/templates.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +struct NonConvertible { }; + +struct ConvertibleToBool +{ + const bool value; + ConvertibleToBool(bool value) : value(value) { } + operator bool() const { return value; } + bool operator!() const { return !value; } +}; + +struct NonDefaultConstructible +{ + NonDefaultConstructible(int) { } +}; + +TEST(Util, CoerceOrFallback) +{ + using uavcan::coerceOrFallback; + + ASSERT_FALSE(coerceOrFallback(NonConvertible())); + ASSERT_TRUE(coerceOrFallback(NonConvertible(), true)); + + ASSERT_EQ(0, coerceOrFallback(NonConvertible())); + ASSERT_EQ(9000, coerceOrFallback(NonConvertible(), 9000)); + + ASSERT_TRUE(coerceOrFallback(ConvertibleToBool(true))); + ASSERT_TRUE(coerceOrFallback(ConvertibleToBool(true), false)); + ASSERT_FALSE(coerceOrFallback(ConvertibleToBool(false))); + ASSERT_FALSE(coerceOrFallback(ConvertibleToBool(false), true)); + ASSERT_EQ(1, coerceOrFallback(ConvertibleToBool(true))); + ASSERT_EQ(0, coerceOrFallback(ConvertibleToBool(false), -100)); + + //coerceOrFallback(ConvertibleToBool(true)); // Will fail to compile + coerceOrFallback(NonConvertible(), NonDefaultConstructible(64)); +} + +TEST(Util, FloatClassification) +{ + // NAN + ASSERT_TRUE(uavcan::isNaN(std::numeric_limits::quiet_NaN())); + ASSERT_FALSE(uavcan::isNaN(std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::isNaN(std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::isNaN(123.456)); + + // INF + ASSERT_TRUE(uavcan::isInfinity(std::numeric_limits::infinity())); + ASSERT_TRUE(uavcan::isInfinity(-std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::isInfinity(std::numeric_limits::quiet_NaN())); + ASSERT_FALSE(uavcan::isInfinity(-0.1L)); + + // Signbit + ASSERT_FALSE(uavcan::getSignBit(12)); + ASSERT_TRUE(uavcan::getSignBit(-std::numeric_limits::infinity())); + ASSERT_FALSE(uavcan::getSignBit(std::numeric_limits::infinity())); + ASSERT_TRUE(uavcan::getSignBit(-0.0)); // Negative zero +} diff --git a/src/drivers/uavcan/libdronecan/libuavcan/tools/coverity_scan_model.cpp b/src/drivers/uavcan/libdronecan/libuavcan/tools/coverity_scan_model.cpp new file mode 100644 index 0000000000..e66bb785fa --- /dev/null +++ b/src/drivers/uavcan/libdronecan/libuavcan/tools/coverity_scan_model.cpp @@ -0,0 +1,45 @@ +/* + * Coverity Scan model. + * + * - A model file can't import any header files. + * - Therefore only some built-in primitives like int, char and void are + * available but not wchar_t, NULL etc. + * - Modeling doesn't need full structs and typedefs. Rudimentary structs + * and similar types are sufficient. + * - An uninitialized local pointer is not an error. It signifies that the + * variable could be either NULL or have some data. + * + * Coverity Scan doesn't pick up modifications automatically. The model file + * must be uploaded by an admin in the analysis settings of + * https://scan.coverity.com/projects/1513 + */ + +namespace std +{ + typedef unsigned long size_t; +} + +namespace uavcan +{ + +void handleFatalError(const char* msg) +{ + __coverity_panic__(); +} + +template +class PoolAllocator +{ +public: + void* allocate(std::size_t size) + { + return __coverity_alloc__(size); + } + + void deallocate(const void* ptr) + { + __coverity_free__(ptr); + } +}; + +} diff --git a/src/drivers/uavcan/libuavcan b/src/drivers/uavcan/libuavcan deleted file mode 160000 index dce2d4e7d8..0000000000 --- a/src/drivers/uavcan/libuavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit dce2d4e7d8f41348e571481cd2e4788ac8af900d diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index e62b29d5ea..2127817fd0 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -104,7 +104,6 @@ UavcanAirspeedBridge::ias_sub_cb(const report.timestamp = hrt_absolute_time(); report.indicated_airspeed_m_s = msg.indicated_airspeed; report.true_airspeed_m_s = _last_tas_m_s; - report.air_temperature_celsius = _last_outside_air_temp_k + atmosphere::kAbsoluteNullCelsius; publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index db88941a56..7fa9e843f7 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -58,6 +59,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), + _sub_gnss_heading(node), _pub_moving_baseline_data(node), _pub_rtcm_stream(node), _channel_using_fix2(new bool[_max_channels]) @@ -100,6 +102,12 @@ UavcanGnssBridge::init() return res; } + res = _sub_gnss_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssBridge::gnss_relative_sub_cb)); + + if (res < 0) { + PX4_WARN("GNSS relative sub failed %i", res); + return res; + } // UAVCAN_PUB_RTCM int32_t uavcan_pub_rtcm = 0; @@ -295,6 +303,7 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _rel_heading_valid = msg.reported_heading_acc_available; + _rel_heading = math::radians(msg.reported_heading_deg); + _rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg); +} template void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &msg, uint8_t fix_type, @@ -463,9 +480,21 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.vdop = msg.pdop; } - report.heading = heading; - report.heading_offset = heading_offset; - report.heading_accuracy = heading_accuracy; + // Use heading from RelPosHeading message if available and we have RTK Fixed solution. + if (_rel_heading_valid && (fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) { + report.heading = _rel_heading; + report.heading_offset = NAN; + report.heading_accuracy = _rel_heading_accuracy; + + _rel_heading = NAN; + _rel_heading_accuracy = NAN; + _rel_heading_valid = false; + + } else { + report.heading = heading; + report.heading_offset = heading_offset; + report.heading_accuracy = heading_accuracy; + } report.noise_per_ms = noise_per_ms; report.jamming_indicator = jamming_indicator; diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index f2f28eb5bc..f8205c3b38 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -82,6 +83,7 @@ private: void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); + void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg); template void process_fixx(const uavcan::ReceivedDataStructure &msg, @@ -113,11 +115,16 @@ private: void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)> TimerCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + RelPosHeadingCbBinder; + uavcan::INode &_node; uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; + uavcan::Subscriber _sub_gnss_heading; uavcan::Publisher _pub_moving_baseline_data; uavcan::Publisher _pub_rtcm_stream; @@ -137,6 +144,10 @@ private: bool _publish_rtcm_stream{false}; bool _publish_moving_baseline_data{false}; + float _rel_heading_accuracy{NAN}; + float _rel_heading{NAN}; + bool _rel_heading_valid{false}; + perf_counter_t _rtcm_stream_pub_perf{nullptr}; perf_counter_t _moving_baseline_data_pub_perf{nullptr}; }; diff --git a/src/drivers/uavcan/uavcan_drivers/posix/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/posix/CMakeLists.txt new file mode 100644 index 0000000000..c7335e837b --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/CMakeLists.txt @@ -0,0 +1,12 @@ +# +# Copyright (C) 2015 Pavel Kirienko +# + +cmake_minimum_required(VERSION 3.9) + +project(libuavcan_posix) + +# +# Library (header only) +# +install(DIRECTORY include/uavcan_posix DESTINATION include) diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include.mk b/src/drivers/uavcan/uavcan_drivers/posix/include.mk new file mode 100644 index 0000000000..84ad8e1744 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include.mk @@ -0,0 +1,7 @@ +# +# Copyright (C) 2015 David Sidrane +# + +LIBUAVCAN_POSIX_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_POSIX_INC := $(LIBUAVCAN_POSIX_DIR)include/ diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp new file mode 100644 index 0000000000..b58a40f34e --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -0,0 +1,473 @@ +/**************************************************************************** +* +* Copyright (c) 2015, 2021 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class BasicFileServerBackend : public uavcan::IFileServerBackend +{ + enum { FilePermissions = 438 }; ///< 0o666 + +protected: + + class FDCacheBase + { + public: + FDCacheBase() { } + virtual ~FDCacheBase() { } + + virtual int open(const char *path, int oflags) + { + using namespace std; + + return ::open(path, oflags); + } + + virtual int close(int fd, bool done = true) + { + (void)done; + using namespace std; + + return ::close(fd); + } + + virtual void init() { } + }; + + FDCacheBase fallback_; + + class FDCache : public FDCacheBase, protected uavcan::TimerBase + { + /// Age in Seconds an entry will stay in the cache if not accessed. + enum { MaxAgeSeconds = 7 }; + + /// Rate in Seconds that the cache will be flushed of stale entries. + enum { GarbageCollectionSeconds = 60 }; + + IFileServerBackend::Path &alt_root_path_; + IFileServerBackend::Path &root_path_; + + class FDCacheItem : uavcan::Noncopyable + { + friend FDCache; + + FDCacheItem *next_; + std::time_t last_access_; + const int fd_; + const int oflags_; + const char *const path_; + + public: + enum { InvalidFD = -1 }; + + FDCacheItem() : + next_(UAVCAN_NULLPTR), + last_access_(0), + fd_(InvalidFD), + oflags_(0), + path_(UAVCAN_NULLPTR) + { } + + FDCacheItem(int fd, const char *path, int oflags) : + next_(UAVCAN_NULLPTR), + last_access_(0), + fd_(fd), + oflags_(oflags), + path_(::strndup(path, uavcan::protocol::file::Path::FieldTypes::path::MaxSize)) + { } + + ~FDCacheItem() + { + using namespace std; + + if (valid()) { + ::free(const_cast(path_)); + } + } + + bool valid() const + { + return path_ != UAVCAN_NULLPTR; + } + + int getFD() const + { + return fd_; + } + + std::time_t getAccess() const + { + return last_access_; + } + + std::time_t acessed() + { + using namespace std; + last_access_ = time(UAVCAN_NULLPTR); + return getAccess(); + } + + void expire() + { + last_access_ = 0; + } + + bool expired() const + { + using namespace std; + return 0 == last_access_ || (time(UAVCAN_NULLPTR) - last_access_) > MaxAgeSeconds; + } + + bool equals(const char *path, int oflags) const + { + using namespace std; + return oflags_ == oflags && 0 == ::strcmp(path, path_); + } + + bool equals(int fd) const + { + return fd_ == fd; + } + }; + + FDCacheItem *head_; + + FDCacheItem *find(const char *path, int oflags) + { + for (FDCacheItem *pi = head_; pi; pi = pi->next_) { + if (pi->equals(path, oflags)) { + return pi; + } + } + + return UAVCAN_NULLPTR; + } + + FDCacheItem *find(int fd) + { + for (FDCacheItem *pi = head_; pi; pi = pi->next_) { + if (pi->equals(fd)) { + return pi; + } + } + + return UAVCAN_NULLPTR; + } + + FDCacheItem *add(FDCacheItem *pi) + { + pi->next_ = head_; + head_ = pi; + pi->acessed(); + return pi; + } + + void removeExpired(FDCacheItem **pi) + { + while (*pi) { + if ((*pi)->expired()) { + FDCacheItem *next = (*pi)->next_; + (void)FDCacheBase::close((*pi)->fd_); + delete (*pi); + *pi = next; + continue; + } + + pi = &(*pi)->next_; + } + } + + void remove(FDCacheItem *pi, bool done) + { + if (done) { + pi->expire(); + } + + removeExpired(&head_); + } + + void clear() + { + FDCacheItem *tmp; + + for (FDCacheItem *pi = head_; pi; pi = tmp) { + tmp = pi->next_; + (void)FDCacheBase::close(pi->fd_); + delete pi; + } + } + + /* Removed stale entries. In the normal case a node will read the + * complete contents of a file and the read of the last block will + * cause the method remove() to be invoked with done true. Thereby + * flushing the entry from the cache. But if the node does not + * stay the course of the read, it may leave a dangling entry. + * This call back handles the garbage collection. + */ + virtual void handleTimerEvent(const uavcan::TimerEvent &) + { + removeExpired(&head_); + } + + public: + FDCache(uavcan::INode &node, IFileServerBackend::Path &root_path, IFileServerBackend::Path &alt_root_path) : + TimerBase(node), + alt_root_path_(alt_root_path), + root_path_(root_path), + head_(UAVCAN_NULLPTR) + { } + + virtual ~FDCache() + { + stop(); + clear(); + } + + virtual void init() + { + startPeriodic(uavcan::MonotonicDuration::fromMSec(GarbageCollectionSeconds * 1000)); + } + + virtual int open(const char *path, int oflags) + { + int fd = FDCacheItem::InvalidFD; + + FDCacheItem *pi = find(path, oflags); + + if (pi != UAVCAN_NULLPTR) { + pi->acessed(); + + } else { + Path vpath = root_path_.c_str(); + vpath += path; + + fd = FDCacheBase::open(vpath.c_str(), oflags); + + if (fd < 0) { + vpath = alt_root_path_.c_str(); + vpath += path; + fd = FDCacheBase::open(vpath.c_str(), oflags); + } + + if (fd < 0) { + return fd; + } + + /* Allocate and clone path */ + + pi = new FDCacheItem(fd, path, oflags); + + /* Allocation worked but check clone */ + + if (pi && !pi->valid()) { + /* Allocation worked but clone or path failed */ + delete pi; + pi = UAVCAN_NULLPTR; + } + + if (pi == UAVCAN_NULLPTR) { + /* + * If allocation fails no harm just can not cache it + * return open fd + */ + return fd; + } + + /* add new */ + add(pi); + } + + return pi->getFD(); + } + + virtual int close(int fd, bool done) + { + FDCacheItem *pi = find(fd); + + if (pi == UAVCAN_NULLPTR) { + /* + * If not found just close it + */ + return FDCacheBase::close(fd); + } + + remove(pi, done); + return 0; + } + }; + + FDCacheBase *fdcache_; + uavcan::INode &node_; + + FDCacheBase &getFDCache() + { + if (fdcache_ == UAVCAN_NULLPTR) { + fdcache_ = new FDCache(node_, getRootPath(), getAltRootPath()); + + if (fdcache_ == UAVCAN_NULLPTR) { + fdcache_ = &fallback_; + } + + fdcache_->init(); + } + + return *fdcache_; + } + + /** + * Back-end for uavcan.protocol.file.GetInfo. + * Implementation of this method is required. + * On success the method must return zero. + */ + virtual uavcan::int16_t getInfo(const Path &path, uavcan::uint64_t &out_size, EntryType &out_type) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + if (path.size() > 0) { + + using namespace std; + + struct stat sb; + + Path vpath = getRootPath().c_str(); + vpath += path; + + rv = stat(vpath.c_str(), &sb); + + if (rv < 0) { + vpath = getAltRootPath().c_str(); + vpath += path; + rv = stat(vpath.c_str(), &sb); + } + + if (rv < 0) { + rv = errno; + + } else { + rv = 0; + out_size = sb.st_size; + out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE; + + if (S_ISDIR(sb.st_mode)) { + out_type.flags |= uavcan::protocol::file::EntryType::FLAG_DIRECTORY; + + } else if (S_ISREG(sb.st_mode)) { + out_type.flags |= uavcan::protocol::file::EntryType::FLAG_FILE; + } + + // TODO Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + } + } + + return rv; + } + + /** + * Back-end for uavcan.protocol.file.Read. + * Implementation of this method is required. + * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except + * if the end of file is reached. + * On success the method must return zero. + */ + virtual uavcan::int16_t read(const Path &path, const uavcan::uint64_t offset, uavcan::uint8_t *out_buffer, + uavcan::uint16_t &inout_size) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + if (path.size() > 0 && inout_size != 0) { + using namespace std; + + FDCacheBase &cache = getFDCache(); + + int fd = cache.open(path.c_str(), O_RDONLY); + + if (fd < 0) { + rv = -errno; + + } else { + ssize_t total_read = 0; + + rv = ::lseek(fd, offset, SEEK_SET); + + if (rv < 0) { + rv = -errno; + + } else { + rv = 0; + ssize_t remaining = inout_size; + ssize_t nread = 0; + + do { + nread = ::read(fd, &out_buffer[total_read], remaining); + + if (nread < 0) { + rv = errno; + + } else { + remaining -= nread, + total_read += nread; + } + } while (nread > 0 && remaining > 0); + } + + (void)cache.close(fd, rv != 0 || total_read != inout_size); + inout_size = total_read; + } + } + + return rv; + } + +public: + BasicFileServerBackend(uavcan::INode &node) : + fdcache_(UAVCAN_NULLPTR), + node_(node) + { } + + ~BasicFileServerBackend() + { + if (fdcache_ != &fallback_) { + delete fdcache_; + fdcache_ = UAVCAN_NULLPTR; + } + } +}; + +#if __GNUC__ +/// Typo fix in a backwards-compatible way (only for GCC projects). Will be removed someday. +typedef BasicFileServerBackend +BasicFileSeverBackend // Missing 'r' +__attribute__((deprecated)); +#endif + +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp new file mode 100644 index 0000000000..fe6129318d --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED +#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan_posix +{ +namespace dynamic_node_id_server +{ +/** + * This interface implements a POSIX compliant file based IEventTracer interface + */ +class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer +{ + /** + * Maximum length of full path to log file + */ + enum { MaxPathLength = 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString path_; + +protected: + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + using namespace std; + + timespec ts = timespec(); // If clock_gettime() fails, zero time will be used + (void)clock_gettime(CLOCK_REALTIME, &ts); + + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions); + + if (fd >= 0) { + const int FormatBufferLength = 63; + char buffer[FormatBufferLength + 1]; + ssize_t remaining = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", + static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), + static_cast(code), static_cast(argument)); + + ssize_t total_written = 0; + ssize_t written = 0; + + do { + written = write(fd, &buffer[total_written], remaining); + + if (written > 0) { + total_written += written; + remaining -= written; + } + } while (written > 0 && remaining > 0); + + (void)close(fd); + } + } + +public: + /** + * Initializes the file based event tracer. + */ + int init(const PathString &path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) { + rv = 0; + path_ = path.c_str(); + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); + + if (fd >= 0) { + (void)close(fd); + } + } + + return rv; + } +}; +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp new file mode 100644 index 0000000000..1989cc98da --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -0,0 +1,158 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_posix +{ +namespace dynamic_node_id_server +{ +/** + * This interface implements a POSIX compliant IStorageBackend interface + */ +class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend +{ + /** + * Maximum length of full path including / and key max + */ + enum { MaxPathLength = 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString base_path; + +protected: + virtual String get(const String &key) const + { + using namespace std; + PathString path = base_path.c_str(); + path += key; + String value; + int fd = open(path.c_str(), O_RDONLY); + + if (fd >= 0) { + char buffer[MaxStringLength + 1]; + (void)memset(buffer, 0, sizeof(buffer)); + ssize_t remaining = MaxStringLength; + ssize_t total_read = 0; + ssize_t nread = 0; + + do { + nread = ::read(fd, &buffer[total_read], remaining); + + if (nread > 0) { + remaining -= nread, + total_read += nread; + } + } while (nread > 0 && remaining > 0); + + (void)close(fd); + + if (total_read > 0) { + for (int i = 0; i < total_read; i++) { + if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r') { + buffer[i] = '\0'; + break; + } + } + + value = buffer; + } + } + + return value; + } + + virtual void set(const String &key, const String &value) + { + using namespace std; + PathString path = base_path.c_str(); + path += key; + int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions); + + if (fd >= 0) { + ssize_t remaining = value.size(); + ssize_t total_written = 0; + ssize_t written = 0; + + do { + written = write(fd, &value.c_str()[total_written], remaining); + + if (written > 0) { + total_written += written; + remaining -= written; + } + } while (written > 0 && remaining > 0); + + (void)fsync(fd); + (void)close(fd); + } + } + +public: + /** + * Initializes the file based backend storage by passing a path to + * the directory where the key named files will be stored. + * The return value should be 0 on success. + * If it is -ErrInvalidConfiguration then the the path name is too long to + * accommodate the trailing slash and max key length. + */ + int init(const PathString &path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) { + base_path = path.c_str(); + + if (base_path.back() == '/') { + base_path.pop_back(); + } + + rv = 0; + struct stat sb; + + if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) { + // coverity[toctou] + rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + + if (rv >= 0) { + base_path.push_back('/'); + + if ((base_path.size() + MaxStringLength) > MaxPathLength) { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + + return rv; + } +}; +} +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp new file mode 100644 index 0000000000..ddea2bbef5 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -0,0 +1,319 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#include + +// TODO Get rid of the macro +#if !defined(DIRENT_ISFILE) && defined(DT_REG) +# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) +#endif + +#define ALT_APD_SIGNATURE 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + enum { FilePermissions = 438 }; ///< 0o666 + + enum { MaxBasePathLength = 128 }; + + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::Type BasePathString; + + /** + * Maximum length of full path including / the file name + */ + enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; + + /** + * This type is used internally for the full path to file + */ + typedef uavcan::MakeString::Type PathString; + + BasePathString base_path_; + BasePathString alt_base_path_; + + static void addSlash(BasePathString &path) + { + if (path.back() != getPathSeparator()) { + path.push_back(getPathSeparator()); + } + } + + static void removeSlash(BasePathString &path) + { + if (path.back() == getPathSeparator()) { + path.pop_back(); + } + } + + void setFirmwareBasePath(const char *path) + { + base_path_ = path; + } + + void setFirmwareAltBasePath(const char *path) + { + alt_base_path_ = path; + } + +protected: + /** + * This method will be invoked when the class obtains a response to GetNodeInfo request. + * + * @param node_id Node ID that this GetNodeInfo response was received from. + * + * @param node_info Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details. + * + * @param out_firmware_file_path The implementation should return the firmware image path via this argument. + * Note that this path must be reachable via uavcan.protocol.file.Read service. + * Refer to @ref FileServer and @ref BasicFileServer for details. + * + * @return True - the class will begin sending update requests. + * False - the node will be ignored, no request will be sent. + */ + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID, + const uavcan::protocol::GetNodeInfo::Response &node_info, + FirmwareFilePath &out_firmware_file_path) + { + using namespace std; + + /* This is a work around for two issues. + * 1) FirmwareFilePath is 40 + * 2) OK using is using 32 for max file names. + * + * So for the file: + * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin + * +---ufw + * +- board_id.bin + */ + bool rv = false; + uint16_t board_id = (node_info.hardware_version.major << 8) + node_info.hardware_version.minor; + char bin_file_name[MaxBasePathLength + 1]; + int n = snprintf(bin_file_name, sizeof(bin_file_name), "%u.bin", board_id); + + if (n > 0 && n < (int)sizeof(bin_file_name) - 2) { + char bin_file_path[MaxBasePathLength + 1]; + + // Look on Primary location + + snprintf(bin_file_path, sizeof(bin_file_path), "%s%s", + getFirmwareBasePath().c_str(), bin_file_name); + + // We have a file path, is it a valid image + + AppDescriptor descriptor{0}; + + bool found = getFileInfo(bin_file_path, descriptor) == 0; + + if (!found && !getFirmwareAltBasePath().empty()) { + snprintf(bin_file_name, sizeof(bin_file_name), "%u.bin", board_id); + snprintf(bin_file_path, sizeof(bin_file_path), "%s/%s", + getFirmwareAltBasePath().c_str(), bin_file_name); + + found = getFileInfo(bin_file_path, descriptor) == 0; + } + + if (found && (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || + descriptor.image_crc != node_info.software_version.image_crc)) { + rv = true; + out_firmware_file_path = bin_file_name; + } + } + + return rv; + } + + /** + * This method will be invoked when a node responds to the update request with an error. If the request simply + * times out, this method will not be invoked. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting + * is not needed anymore. This method will not be invoked. + * + * @param node_id Node ID that returned this error. + * + * @param error_response Contents of the error response. It contains error code and text. + * + * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be + * initialized with old path, so if the same path needs to be reused, this + * argument should be left unchanged. + * + * @return True - the class will continue sending update requests with new firmware path. + * False - the node will be forgotten, new requests will not be sent. + */ + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID, + const uavcan::protocol::file::BeginFirmwareUpdate::Response &, + FirmwareFilePath &) + { + // TODO: Limit the number of attempts per node + return true; + } + +public: + struct AppDescriptor { + uavcan::uint8_t signature[sizeof(uavcan::uint64_t)]; + union { + uavcan::uint64_t image_crc; + uavcan::uint32_t crc32_block1; + uavcan::uint32_t crc32_block2; + }; + uavcan::uint32_t image_size; + uavcan::uint32_t vcs_commit; + uavcan::uint8_t major_version; + uavcan::uint8_t minor_version; + uavcan::uint16_t board_id; + uavcan::uint8_t reserved[ 3 + 3 + 2]; + }; + + static int getFileInfo(const char *path, AppDescriptor &descriptor, int limit = 0) + { + using namespace std; + + const unsigned MaxChunk = 512 / sizeof(uint64_t); + + // Make sure this does not present as a valid descriptor + struct { + union { + uavcan::uint64_t l; + uavcan::uint8_t b[sizeof(uint64_t)] {ALT_APD_SIGNATURE}; + } signature; + uavcan::uint8_t zeropad[sizeof(AppDescriptor) - sizeof(uint64_t)] {0}; + } s; + + int rv = -ENOENT; + uint64_t chunk[MaxChunk]; + int fd = open(path, O_RDONLY); + + if (fd >= 0) { + AppDescriptor *pdescriptor = UAVCAN_NULLPTR; + + while (pdescriptor == UAVCAN_NULLPTR && limit >= 0) { + int len = read(fd, chunk, sizeof(chunk)); + + if (len == 0) { + break; + } + + if (len < 0) { + rv = -errno; + goto out_close; + } + + uint64_t *p = &chunk[0]; + + if (limit > 0) { + limit -= sizeof(chunk); + } + + do { + if (*p == s.signature.l) { + pdescriptor = reinterpret_cast(p); // FIXME TODO This breaks strict aliasing + descriptor = *pdescriptor; + rv = 0; + break; + } + } while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]); + } + +out_close: + (void)close(fd); + } + + return rv; + } + + const BasePathString &getFirmwareBasePath() const { return base_path_; } + + const BasePathString &getFirmwareAltBasePath() const { return alt_base_path_; } + + static char getPathSeparator() + { + return static_cast(uavcan::protocol::file::Path::SEPARATOR); + } + + /** + * Creates the Directories were the files will be stored + * + * This is directory structure is in support of a workaround + * for the issues that FirmwareFilePath is 40 + * + * It creates a path structure: + * +---(base_path) <----------- Files are here. + */ + + int createFwPaths(const char *base_path, const char *alt_base_path = nullptr) + { + using namespace std; + int rv = -uavcan::ErrInvalidParam; + + if (alt_base_path) { + const int len = strlen(alt_base_path); + + if (len > 0 && len < base_path_.MaxSize) { + setFirmwareAltBasePath(alt_base_path); + + } else { + return rv; + } + } + + if (base_path) { + const int len = strlen(base_path); + + if (len > 0 && len < base_path_.MaxSize) { + setFirmwareBasePath(base_path); + removeSlash(base_path_); + const char *path = getFirmwareBasePath().c_str(); + + rv = 0; + struct stat sb; + + if (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode)) { + rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); + } + + addSlash(base_path_); + } + } + + return rv; + } + + const char *getFirmwarePath() const + { + return getFirmwareBasePath().c_str(); + } + + const char *getAltFirmwarePath() const + { + return getFirmwareAltBasePath().c_str(); + } +}; +} + +#endif // Include guard diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 87f3110cac..ad39a943a7 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -705,11 +705,12 @@ UavcanNode::Run() if (can_init_res < 0) { PX4_ERR("CAN driver init failed %i", can_init_res); + + } else { + _instance->init(node_id, can->driver.updateEvent()); + + _node_init = true; } - - _instance->init(node_id, can->driver.updateEvent()); - - _node_init = true; } pthread_mutex_lock(&_node_mutex); @@ -1072,8 +1073,6 @@ void UavcanMixingInterfaceServo::Run() void UavcanNode::print_info() { - (void)pthread_mutex_lock(&_node_mutex); - // Memory status printf("Pool allocator status:\n"); printf("\tCapacity hard/soft: %" PRIu16 "/%" PRIu16 " blocks\n", @@ -1111,15 +1110,29 @@ UavcanNode::print_info() printf("\n"); #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) - printf("ESC outputs:\n"); - _mixing_interface_esc.mixingOutput().printStatus(); - printf("Servo outputs:\n"); - _mixing_interface_servo.mixingOutput().printStatus(); + // Print esc status if at least one channel is enabled + for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { + if (_mixing_interface_esc.mixingOutput().isFunctionSet(i)) { + printf("ESC outputs:\n"); + _mixing_interface_esc.mixingOutput().printStatus(); + printf("\n"); + break; + } + } + + // Print servo status if at least one channel is enabled + for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { + if (_mixing_interface_servo.mixingOutput().isFunctionSet(i)) { + printf("Servo outputs:\n"); + _mixing_interface_servo.mixingOutput().printStatus(); + printf("\n"); + break; + } + } + #endif - printf("\n"); - // Sensor bridges for (const auto &br : _sensor_bridges) { printf("Sensor '%s':\n", br->get_name()); @@ -1139,8 +1152,6 @@ UavcanNode::print_info() perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - - (void)pthread_mutex_unlock(&_node_mutex); } void diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index edd2c41ea4..4e9253fbf6 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -85,7 +85,7 @@ PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000); * @unit m * @group UAVCAN */ -PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f); +PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.0f); /** * UAVCAN rangefinder maximum range @@ -95,7 +95,7 @@ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f); * @unit m * @group UAVCAN */ -PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 200.0f); +PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 999.0f); /** * UAVCAN fuel tank maximum capacity diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 21366ad064..d170bde437 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -31,10 +31,13 @@ # ############################################################################ -set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan) -set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) +set(LIBDRONECAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan) +set(LIBDRONECAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers) -px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR}) +set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/libdronecan/dsdl") + +px4_add_git_submodule(TARGET git_uavcan_dsdl PATH ${DSDLC_DIR}) +px4_add_git_submodule(TARGET git_uavcan_pydronecan PATH ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/pydronecan) set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") @@ -89,25 +92,24 @@ add_compile_options( -Wno-address-of-packed-member ) set(CMAKE_WARN_DEPRECATED OFF CACHE BOOL "" FORCE) # silence libuavcan deprecation warning for now (TODO: fix and remove) -add_subdirectory(${LIBUAVCAN_DIR} libuavcan EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR} libdronecan EXCLUDE_FROM_ALL) add_dependencies(uavcan prebuild_targets) # driver -add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) +add_subdirectory(${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL) target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated ) # generated DSDL -set(DSDLC_DIR "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl") set(DSDLC_INPUTS - "${LIBUAVCAN_DIR}/dsdl/ardupilot" - "${LIBUAVCAN_DIR}/dsdl/com" - "${LIBUAVCAN_DIR}/dsdl/cuav" - "${LIBUAVCAN_DIR}/dsdl/dronecan" - "${LIBUAVCAN_DIR}/dsdl/uavcan" + "${DSDLC_DIR}/ardupilot" + "${DSDLC_DIR}/com" + "${DSDLC_DIR}/cuav" + "${DSDLC_DIR}/dronecan" + "${DSDLC_DIR}/uavcan" ) set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") @@ -118,7 +120,7 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS}) endforeach(DSDLC_INPUT) add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp COMMAND - ${PYTHON_EXECUTABLE} ${LIBUAVCAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc + ${PYTHON_EXECUTABLE} ${LIBDRONECAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc --outdir ${DSDLC_OUTPUT} ${DSDLC_INPUTS} #--verbose COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp @@ -135,10 +137,10 @@ px4_add_module( #-DDEBUG_BUILD INCLUDES ${DSDLC_OUTPUT} - ${LIBUAVCAN_DIR}/libuavcan/include - ${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated - ${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include - ${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include + ${LIBDRONECAN_DIR}/libuavcan/include + ${LIBDRONECAN_DIR}/libuavcan/include/dsdlc_generated + ${LIBDRONECAN_DIR}/libuavcan/posix/include + ${LIBDRONECAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include SRCS allocator.hpp uavcan_driver.hpp @@ -153,7 +155,9 @@ px4_add_module( drivers_bootloaders version conversion - git_uavcan + + git_uavcan_dsdl + git_uavcan_pydronecan uavcan_${UAVCAN_DRIVER}_driver # within libuavcan diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 129bd32c21..2ad053d9a4 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -100,10 +100,10 @@ public: } // reading_type - if (dist.current_distance >= dist.max_distance) { + if (dist.current_distance > dist.max_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; - } else if (dist.current_distance <= dist.min_distance) { + } else if (dist.current_distance < dist.min_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; } else if (dist.signal_quality != 0) { diff --git a/src/drivers/uavcannode/Publishers/SafetyButton.hpp b/src/drivers/uavcannode/Publishers/SafetyButton.hpp index 136cd27c45..4634acc887 100644 --- a/src/drivers/uavcannode/Publishers/SafetyButton.hpp +++ b/src/drivers/uavcannode/Publishers/SafetyButton.hpp @@ -75,7 +75,8 @@ public: if (safety_button.triggered) { ardupilot::indication::Button Button{}; Button.button = ardupilot::indication::Button::BUTTON_SAFETY; - Button.press_time = UINT8_MAX; + // NOTE: Ardupilot checks that the press time is exactly 10, PX4 checks >= 10 + Button.press_time = 10; uavcan::Publisher::broadcast(Button); } } diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt index 57a5f4af21..b6e3d8a3c2 100644 --- a/src/drivers/voxl2_io/CMakeLists.txt +++ b/src/drivers/voxl2_io/CMakeLists.txt @@ -37,15 +37,12 @@ px4_add_module( SRCS voxl2_io_crc16.c voxl2_io_crc16.h - voxl2_io_serial.cpp - voxl2_io_serial.hpp voxl2_io_packet.c voxl2_io_packet.h voxl2_io_packet_types.h voxl2_io.cpp voxl2_io.hpp DEPENDS - rc px4_work_queue mixer_module MODULE_CONFIG diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 021305e955..86dc48ca58 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -1,12 +1,12 @@ module_name: VOXL2 IO Output actuator_output: - config_parameters: - - param: 'VOXL2_IO_MIN' - label: 'PWM min value' - - param: 'VOXL2_IO_MAX' - label: 'PWM max value' +# config_parameters: +# - param: 'VOXL2_IO_MIN' +# label: 'PWM min value' +# - param: 'VOXL2_IO_MAX' +# label: 'PWM max value' output_groups: - param_prefix: VOXL2_IO group_label: 'PWMs' channel_label: 'PWM Channel' - num_channels: 4 + num_channels: 8 diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 352b474906..3461c99a4b 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -33,8 +33,6 @@ #include "voxl2_io.hpp" -#include - Voxl2IO::Voxl2IO() : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)), @@ -43,69 +41,81 @@ Voxl2IO::Voxl2IO() : _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) { _mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS); - _uart_port = new Voxl2IoSerial(); - voxl2_io_packet_init(&_sbus_packet); + voxl2_io_packet_init(&_voxl2_io_packet); + + //set low rate scheduling interval to 200hz so that RC can be updated even if all actuators are disabled + //otherwise, the default low rate scheduling interval is 300ms and RC packets are delayed and lost + _mixing_output.setLowrateSchedulingInterval(5_ms); } Voxl2IO::~Voxl2IO() { - /* make sure servos are off */ - stop_all_pwms(); - - if (_uart_port) { - _uart_port->uart_close(); - _uart_port = nullptr; - } + _uart_port.close(); perf_free(_cycle_perf); perf_free(_output_update_perf); } - int Voxl2IO::init() { + PX4_INFO("VOXL2_IO: Driver starting"); + int ret = PX4_OK; - /* Open serial port in this thread */ - if (!_uart_port->is_open()) { - if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) { - /* Send PWM config to M0065... pwm_min and pwm_max */ - PX4_INFO("Opened UART connection to M0065 device on port %s", _device); - - } else { - PX4_ERR("Failed opening device"); - return PX4_ERROR; - } - } - - /* Verify connectivity and protocol version number */ - if (get_version_info() < 0) { - PX4_ERR("Failed to detect voxl2_io protocol version."); - return PX4_ERROR; - - } else { - if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION - && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) { - PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - - } else { - PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - return PX4_ERROR; - } - } - - /* Getting initial parameter values */ + // Getting initial parameter values ret = update_params(); if (ret != OK) { + PX4_ERR("VOXL2_IO: Failed to update params during init"); return ret; } - /* Send PWM MIN/MAX to M0065 */ - update_pwm_config(); + // Print initial param values + print_params(); - ScheduleOnInterval(_current_update_interval); - // ScheduleNow(); + PX4_INFO("VOXL2_IO: "); + + // Open serial port + if (!_uart_port.isOpen()) { + PX4_INFO("VOXL2_IO: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); + + // Configure UART port + if (! _uart_port.setPort(_device)) { + PX4_ERR("Error configuring serial device on port %s", _device); + return -1; + } + + if (! _uart_port.setBaudrate(_parameters.baud_rate)) { + PX4_ERR("Error setting baudrate to %d on %s", (int) _parameters.baud_rate, _device); + return -1; + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart_port.open()) { + PX4_ERR("Error opening serial device %s", _device); + return -1; + } + } + + // Detect M0065 board + ret = get_version_info(); + + if (ret != 0) { + PX4_ERR("VOXL2_IO: Could not detect the board"); + PX4_ERR("VOXL2_IO: Driver initialization failed. Exiting"); + + if (_uart_port.open()) { + PX4_INFO("VOXL2_IO: Closing uart port"); + _uart_port.close(); + } + + return -1; + } + + //ScheduleOnInterval(_current_update_interval); + ScheduleNow(); + + PX4_INFO("VOXL2_IO: Driver initialization succeeded"); return ret; } @@ -117,11 +127,10 @@ int Voxl2IO::update_params() ret = load_params(&_parameters); if (ret == PX4_OK) { - _mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED); + _mixing_output.setAllDisarmedValues(_parameters.pwm_dis); _mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE); - _mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN); - _mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX); - _pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min; + _mixing_output.setAllMinValues(_parameters.pwm_min); + _mixing_output.setAllMaxValues(_parameters.pwm_max); } return ret; @@ -130,196 +139,159 @@ int Voxl2IO::update_params() int Voxl2IO::load_params(voxl2_io_params_t *params) { int ret = PX4_OK; - int32_t max = params->pwm_max; - int32_t min = params->pwm_min; // initialize out for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { params->function_map[i] = (int)OutputFunction::Disabled; } - /* UART config, PWM mode, and RC protocol*/ + // UART config, PWM mode, and RC protocol param_get(param_find("VOXL2_IO_BAUD"), ¶ms->baud_rate); - param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); + //param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); - /* PWM min, max, and failsafe values*/ + // PWM min, max, and failsafe values param_get(param_find("VOXL2_IO_MIN"), ¶ms->pwm_min); param_get(param_find("VOXL2_IO_MAX"), ¶ms->pwm_max); + param_get(param_find("VOXL2_IO_DIS"), ¶ms->pwm_dis); + param_get(param_find("VOXL2_IO_CMIN"), ¶ms->pwm_cal_min); + param_get(param_find("VOXL2_IO_CMAX"), ¶ms->pwm_cal_max); - /* PWM output functions */ + // PWM output functions + //0: disabled, 1: constant min, 2: constant max + //101-112: motors, 201-208: servos, 402: RC Roll, 403: RC Pitch, 404: RC Throttle, + //405: RC Yaw, 406: RC Flaps, 407-412: RC AUX 1-6, 420-422: Gimbal RPY param_get(param_find("VOXL2_IO_FUNC1"), ¶ms->function_map[0]); param_get(param_find("VOXL2_IO_FUNC2"), ¶ms->function_map[1]); param_get(param_find("VOXL2_IO_FUNC3"), ¶ms->function_map[2]); param_get(param_find("VOXL2_IO_FUNC4"), ¶ms->function_map[3]); + param_get(param_find("VOXL2_IO_FUNC5"), ¶ms->function_map[4]); + param_get(param_find("VOXL2_IO_FUNC6"), ¶ms->function_map[5]); + param_get(param_find("VOXL2_IO_FUNC7"), ¶ms->function_map[6]); + param_get(param_find("VOXL2_IO_FUNC8"), ¶ms->function_map[7]); - /* Validate PWM min and max values */ + // Validate PWM min and max values if (params->pwm_min > params->pwm_max) { - PX4_ERR("Invalid parameter VOXL2_IO_MIN. Please verify parameters."); + PX4_ERR("VOXL2_IO: Invalid parameter VOXL2_IO_MIN. Please verify parameters."); params->pwm_min = 0; ret = PX4_ERROR; } - if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) { - PX4_INFO("Updating PWM params load_params"); - update_pwm_config(); - } - return ret; } -void Voxl2IO::update_pwm_config() -{ - Command cmd; - uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast((_parameters.pwm_min & 0xFF00) >> 8), static_cast(_parameters.pwm_min & 0xFF), - static_cast((_parameters.pwm_max & 0xFF00) >> 8), static_cast(_parameters.pwm_max & 0xFF) - }; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE, cmd.buf, - sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send config packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } -} - int Voxl2IO::get_version_info() { - int res = 0 ; - int header = -1 ; - int info_packet = -1; - int read_retries = 3; - int read_succeeded = 0; + PX4_INFO("VOXL2_IO: Detecting M0065 board..."); + voxl2_io_packet_init(&_voxl2_io_packet); + Command cmd; + cmd.len = voxl2_io_create_extended_version_request_packet(0, cmd.buf, sizeof(cmd.buf)); - /* Request protocol version info from M0065 */ - cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE); + int retries_left = _board_detect_retries; + bool got_response = false; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); + while ((got_response == false) && (retries_left > 0)) { + retries_left--; + + //send the version request command to the board + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Could not write version request packet to UART port"); + return -1; + } - } else { _bytes_sent += cmd.len; _packets_sent++; - } - /* Read response */ - px4_usleep(10000); - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + hrt_abstime t_request = hrt_absolute_time(); + hrt_abstime t_timeout = 50000; //50ms timeout for version info response - while (read_retries) { - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) { - info_packet = index; - break; - } - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; + //wait for the response to come back + while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) { + px4_usleep(500); //sleep a bit while waiting for the board to respond + + int nread = _uart_port.read(_read_buf, sizeof(_read_buf)); + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + hrt_abstime response_time = hrt_elapsed_time(&t_request); + //PX4_INFO("got packet of length %i",ret); + _packets_received++; + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(VOXL2_IO_EXTENDED_VERSION_INFO)) { + VOXL2_IO_EXTENDED_VERSION_INFO ver; + memcpy(&ver, _voxl2_io_packet.buffer, packet_size); + + PX4_INFO("VOXL2_IO: \tVOXL2_IO ID: %i", ver.id); + PX4_INFO("VOXL2_IO: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version).c_str()); + + uint8_t *u = &ver.unique_id[0]; + PX4_INFO("VOXL2_IO: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + + PX4_INFO("VOXL2_IO: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("VOXL2_IO: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_INFO("VOXL2_IO: \tReply time : %uus", (uint32_t)response_time); + + //we requested response from ID 0, so it should match + if (ver.id != 0) { + PX4_ERR("VOXL2_IO: Invalid id: %d", ver.id); + } + + //check HW (board version) + else if (ver.hw_version != VOXL2_IO_HW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid HW version : %d (expected %d)", ver.hw_version, VOXL2_IO_HW_VERSION); + return -1; + } + + //check firmware version running on the board + else if (ver.sw_version != VOXL2_IO_SW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid FW version : %d (expected %d)", ver.sw_version, VOXL2_IO_SW_VERSION); + return -1; + + } else { + got_response = true; + memcpy(&_version_info, &ver, sizeof(_version_info)); //store the version info only if it is valid + } + } } } - /* Try again in a bit if packet header not present yet... */ - if (header == -1 || info_packet == -1) { - if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); } - - if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) { - if (_debug) { - PX4_ERR("Error parsing version info packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - break; - - } else { - memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO)); - read_succeeded = 1; + //break out of the loop waiting for a response + if (got_response) { break; } + } - } else { - read_retries--; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } + if (!got_response) { + PX4_ERR("VOXL2_IO: Board version info response timeout (%d retries left)", retries_left); } } - if (! read_succeeded) { - return -EIO; - } - - return 0; + return (got_response == true ? 0 : -1); } bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], unsigned num_outputs, unsigned num_control_groups_updated) { - /* Stop Mixer while ESCs are being calibrated */ + // Stop Mixer while ESCs are being calibrated if (_outputs_disabled) { return 0; } + //PX4_INFO("VOXL2_IO: Mixer output: %u, %u, %u, %u", outputs[0], outputs[1], outputs[2], outputs[3]); + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) //So, if Run() is blocked by a custom command, this function will not be called until Run is running again - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int32_t _fb_idx = -1; + uint32_t output_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) { - PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); + PX4_ERR("VOXL2_IO: Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); return false; } @@ -333,43 +305,20 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _pwm_on = true; } - if (!_pwm_on || stop_motors) { - _rate_req[i] = 0; + //Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right? + if (0) { //(!_pwm_on || stop_motors) { + output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns } else { - _rate_req[i] = outputs[i]; + output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns } - - _pwm_values[i] = _rate_req[i]; } Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); + cmd.len = voxl2_io_create_hires_pwm_packet(output_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_pwm_on && _debug) { - PX4_INFO("Mixer outputs"); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5], - outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11], - outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17] - ); - - // Debug messages for PWM 400Hz values sent to M0065 - uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1); - uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2); - uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3); - uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4); - PX4_INFO(""); - } - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Failed to send packet"); return false; } else { @@ -377,40 +326,39 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _packets_sent++; } + //if (_pwm_on && _debug){ + if (_debug) { + PX4_INFO("VOXL2_IO: Mixer outputs: [%u, %u, %u, %u, %u, %u, %u, %u]", + outputs[0], outputs[1], outputs[2], outputs[3], + outputs[4], outputs[5], outputs[6], outputs[7]); + } + perf_count(_output_update_perf); return true; } -int Voxl2IO::flush_uart_rx() -{ - while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} - - return 0; -} - static bool valid_port(int port) { - if (port == 2 || port == 6 || port == 7) { return true; } + if (port == 2 || port == 6 || port == 7) { + return true; + } return false; } +/* int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) { for (int i = 0; i < len; i++) { - int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet); + int16_t ret = voxl2_io_packet_process_char(buf[i], &_voxl2_io_packet); if (ret > 0) { - uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet); - uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet); + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { return 0; - - } else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) { - return 0; - } else { return -1; } @@ -418,36 +366,31 @@ int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) } else { //parser error switch (ret) { case VOXL2_IO_ERROR_BAD_CHECKSUM: - if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet checksum"); break; case VOXL2_IO_ERROR_BAD_LENGTH: - if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet length"); break; case VOXL2_IO_ERROR_BAD_HEADER: - if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet header"); break; case VOXL2_IO_NO_PACKET: - // if(_pwm_on) PX4_WARN("NO packet"); + // if(_pwm_on) PX4_WARN("VOXL2_IO: NO packet"); break; default: - if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: Unknown error: %i", ret); break; } - - return ret; } } return 0; } +*/ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -455,6 +398,8 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, unsigned frame_drops, int rssi, input_rc_s &input_rc) { // fill rc_in struct for publishing + memset(&input_rc, 0, sizeof(input_rc)); //zero out the struct first + input_rc.channel_count = raw_rc_count_local; if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { @@ -474,9 +419,9 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _raw_rc_values[i] = UINT16_MAX; } - input_rc.timestamp = now; + input_rc.timestamp = now; input_rc.timestamp_last_signal = input_rc.timestamp; - input_rc.rc_ppm_frame_length = 0; + input_rc.rc_ppm_frame_length = 0; /* fake rssi if no value was provided */ if (rssi == -1) { @@ -494,131 +439,98 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _sbus_frame_drops++; } - input_rc.rc_failsafe = failsafe; - input_rc.rc_lost = input_rc.rc_failsafe; - input_rc.rc_lost_frame_count = _sbus_frame_drops; + input_rc.rssi_dbm = 0.0f; + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = input_rc.rc_failsafe; + input_rc.rc_lost_frame_count = _sbus_frame_drops; input_rc.rc_total_frame_count = ++_sbus_total_frames; } -int Voxl2IO::receive_sbus() + +int Voxl2IO::parse_sbus_packet(uint8_t *raw_data, uint32_t data_len) { - int res = 0; - int header = -1; - int read_retries = 3; - int read_succeeded = 0; - voxl2_io_packet_init(&_sbus_packet); + input_rc_s input_rc; + uint16_t num_values; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); + hrt_abstime t_now = hrt_absolute_time(); - while (read_retries) { - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; - break; - } - } + bool rc_updated = sbus_parse(t_now, raw_data, data_len, _raw_rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - /* Try again in a bit if packet header not present yet... */ - if (header == -1) { - if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); } + if (rc_updated) { + //if (_pwm_on && _debug){ + if (_debug) { + //PX4_INFO("VOXL2_IO: Decoded packet, header pos: %i", header); + PX4_INFO("VOXL2_IO: [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], + _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], + _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], + _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], + _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], + _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] + ); - read_retries--; - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) { - if (_pwm_on && _debug) { - PX4_ERR("Error parsing QC RAW SBUS packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - break; - } - - input_rc_s input_rc; - uint16_t num_values; - bool sbus_failsafe = false; - bool sbus_frame_drop = false; - uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); - hrt_abstime now = hrt_absolute_time(); - bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values, - &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - - if (rc_updated) { - if (_pwm_on && _debug) { - PX4_INFO("Decoded packet, header pos: %i", header); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], - _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], - _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], - _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], - _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], - _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] - ); - } - - input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; - fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); - - if (!input_rc.rc_lost && !input_rc.rc_failsafe) { - _rc_last_valid = input_rc.timestamp; - } - - input_rc.timestamp_last_signal = _rc_last_valid; - _rc_pub.publish(input_rc); - - _bytes_received += res; - _packets_received++; - read_succeeded = 1; - break; - - } else if (_pwm_on && _debug) { - PX4_ERR("Failed to decode SBUS packet, header pos: %i", header); - - if (sbus_frame_drop) { - PX4_WARN("SBUS frame dropped"); - } - - PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); + if (sbus_frame_drop) { + PX4_WARN("VOXL2_IO: SBUS frame dropped"); } } - read_retries--; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + fill_rc_in(num_values, _raw_rc_values, t_now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); + + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + _rc_last_valid_time = input_rc.timestamp; + } + + input_rc.timestamp_last_signal = _rc_last_valid_time; + _rc_pub.publish(input_rc); + + if (_rc_mode == RC_MODE::SCAN) { + PX4_INFO("VOXL2_IO: Detected VOXL2 IO SBUS RC"); + _rc_mode = RC_MODE::SBUS; + } } - if (! read_succeeded) { - _new_packet = false; - return -EIO; + return 0; +} + + +int Voxl2IO::receive_uart_packets() +{ + int nread = _uart_port.read(_read_buf, READ_BUF_SIZE); + + if (nread > 0) { + if (_debug) { + PX4_INFO("VOXL2_IO: receive_uart_packets read %d bytes", nread); + } + + _bytes_received += nread; + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + _packets_received++; + + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { + + //parse SBUS packet only if configured to do so + if ((_rc_mode == RC_MODE::SCAN) || (_rc_mode == RC_MODE::SBUS)) { + parse_sbus_packet(&_voxl2_io_packet.buffer[SBUS_PAYLOAD], SBUS_FRAME_SIZE); + } + } + + //parse other packets (future use) + } + } } - _new_packet = true; return 0; } @@ -635,26 +547,14 @@ void Voxl2IO::Run() perf_begin(_cycle_perf); - /* Handle RC */ - if (_rc_mode == RC_MODE::SCAN) { - if (receive_sbus() == PX4_OK) { - PX4_INFO("Found M0065 SBUS RC."); - _rc_mode = RC_MODE::SBUS; - } // Add more cases here for other protocols in the future.. - - } else if (_rc_mode == RC_MODE::SBUS) { - receive_sbus(); - } - - /* Only update outputs if we have new values from RC */ - if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) { - _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module - _new_packet = false; - } + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update PWM status if armed or if disarmed PWM values are set */ _pwm_on = _mixing_output.armed().armed; + //receive packets from voxl_io board + receive_uart_packets(); + /* check for parameter updates */ if (!_pwm_on && _parameter_update_sub.updated()) { /* clear update */ @@ -665,48 +565,6 @@ void Voxl2IO::Run() update_params(); } - /* Don't process commands if pwm on */ - if (!_pwm_on) { - if (_current_cmd.valid()) { - PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us); - flush_uart_rx(); - - do { - PX4_INFO("CMDs left %d", _current_cmd.repeats); - - if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { - if (_current_cmd.repeats == 0) { - _current_cmd.clear(); - } - - - } else { - _bytes_sent += _current_cmd.len; - _packets_sent++; - - if (_current_cmd.retries == 0) { - _current_cmd.clear(); - PX4_ERR("Failed to send command, errno: %i", errno); - - } else { - _current_cmd.retries--; - PX4_ERR("Failed to send command, errno: %i", errno); - } - } - - px4_usleep(_current_cmd.repeat_delay_us); - } while (_current_cmd.repeats-- > 0); - - } else { - Command *new_cmd = _pending_cmd.load(); - - if (new_cmd) { - _current_cmd = *new_cmd; - _pending_cmd.store(nullptr); - } - } - } - /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ _mixing_output.updateSubscriptions(true); perf_end(_cycle_perf); @@ -728,17 +586,17 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'v': - PX4_INFO("Verbose mode enabled"); + PX4_INFO("VOXL2_IO: Verbose mode enabled"); get_instance()->_debug = true; break; case 'd': - PX4_INFO("M0065 PWM outputs disabled"); + PX4_INFO("VOXL2_IO: M0065 PWM outputs disabled"); get_instance()->_outputs_disabled = true; break; case 'e': - PX4_INFO("M0065 using external RC"); + PX4_INFO("VOXL2_IO: M0065 using external RC"); get_instance()->_rc_mode = RC_MODE::EXTERNAL; break; @@ -747,7 +605,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) snprintf(get_instance()->_device, 2, "%s", myoptarg); } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + PX4_ERR("VOXL2_IO: Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); _object.store(nullptr); _task_id = -1; return PX4_ERROR; @@ -756,7 +614,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) break; default: - print_usage("Unknown command, parsing flags"); + print_usage("VOXL2_IO: Unknown command, parsing flags"); break; } } @@ -766,7 +624,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } } else { - PX4_ERR("alloc failed"); + PX4_ERR("VOXL2_IO: alloc failed"); } _object.store(nullptr); @@ -776,280 +634,131 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } -bool Voxl2IO::stop_all_pwms() -{ - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _fb_idx = 0; - - Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); - return false; - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } - - return true; -} - -int Voxl2IO::send_cmd_thread_safe(Command *cmd) -{ - cmd->id = _cmd_id++; - _pending_cmd.store(cmd); - - /* wait until main thread processed it */ - while (_pending_cmd.load()) { - px4_usleep(1000); - } - - return 0; -} - int Voxl2IO::calibrate_escs() { - /* Disable outputs so Mixer isn't being a PITA while we calibrate */ + // Disable outputs so Mixer isn't being a PITA while we calibrate _outputs_disabled = true; Command cmd; - int32_t fb_idx = -1; - uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0}; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf, - sizeof(cmd.buf)); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet"); - _outputs_disabled = false; - return -1; + PX4_INFO("VOXL2_IO: PWM ESC calibration is starting!"); + + // Give user 10 seconds to plug in PWM cable for ESCs + PX4_INFO("VOXL2_IO: MIN PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: MAX PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_max); + PX4_INFO("VOXL2_IO:"); + PX4_INFO("VOXL2_IO: Connect your ESCs! (Calibration will start in ~10 seconds)"); + + px4_usleep(10000000); + + uint32_t max_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint32_t min_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + + for (int idx = 0; idx < VOXL2_IO_OUTPUT_CHANNELS; idx++) { + //only send out calibration pulse if the actuator is a motor + if ((_parameters.function_map[idx] >= 101) && (_parameters.function_map[idx] <= 112)) { + max_pwm_cmds[idx] = _parameters.pwm_cal_max * MIXER_OUTPUT_TO_CMD_SCALE; + min_pwm_cmds[idx] = _parameters.pwm_cal_min * MIXER_OUTPUT_TO_CMD_SCALE; + } } - /* Give user 10 seconds to plug in PWM cable for ESCs */ - PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)"); - hrt_abstime start_cal = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_cal) < 10000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled max pwms: %u %u %u %u %u %u %u %u", + max_pwm_cmds[0], max_pwm_cmds[1], max_pwm_cmds[2], max_pwm_cmds[3], + max_pwm_cmds[4], max_pwm_cmds[5], max_pwm_cmds[6], max_pwm_cmds[7]); } - /* PWM MAX 3 seconds */ - PX4_INFO("Writing PWM MAX for 3 seconds!"); - int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX}; + hrt_abstime start; - if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); } + // Send PWM max every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MAX (%dus) for 5 seconds!", _parameters.pwm_cal_max); + cmd.len = voxl2_io_create_hires_pwm_packet(max_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - int16_t led_cmd[4] {0, 0, 0, 0}; - cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MAX packet"); + _outputs_disabled = false; + return -1; + } - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet"); - _outputs_disabled = false; - return -1; - - } else { - cmd.clear(); + px4_usleep(2500); } - hrt_abstime start_pwm_max = hrt_absolute_time(); + // Send PWM min every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MIN (%dus) for 5 seconds!", _parameters.pwm_cal_min); - while (hrt_elapsed_time(&start_pwm_max) < 3000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled min pwms: %u %u %u %u %u %u %u %u", + min_pwm_cmds[0], min_pwm_cmds[1], min_pwm_cmds[2], min_pwm_cmds[3], + min_pwm_cmds[4], min_pwm_cmds[5], min_pwm_cmds[6], min_pwm_cmds[7]); } - /* PWM MIN 4 seconds */ - PX4_INFO("Writing PWM MIN for 4 seconds!"); - int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN}; + cmd.len = voxl2_io_create_hires_pwm_packet(min_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); } - cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet"); - _outputs_disabled = false; - return -1; + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MIN packet"); + _outputs_disabled = false; + return -1; + } + + px4_usleep(2500); } - hrt_abstime start_pwm_min = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_pwm_min) < 4000000) { - continue; - } - - PX4_INFO("ESC Calibration complete"); + PX4_INFO("VOXL2_IO: Waiting 5 seconds to finish the calibration (no PWM output)"); + px4_usleep(5000000); + PX4_INFO("VOXL2_IO: ESC Calibration complete"); _outputs_disabled = false; return 0; } int Voxl2IO::custom_command(int argc, char *argv[]) { - int myoptind = 0; - int ch; - const char *myoptarg = nullptr; - - Command cmd; - uint8_t output_channel = 0xF; - int16_t rate = 0; - - uint32_t repeat_count = 100; - uint32_t repeat_delay_us = 10000; - const char *verb = argv[argc - 1]; - if ((strcmp(verb, "pwm")) == 0 && argc < 3) { - return print_usage("pwm command: missing args"); - - } else if (argc < 1) { + if (argc < 1) { return print_usage("unknown command: missing args"); } - PX4_INFO("Executing the following command: %s", verb); + PX4_INFO("VOXL2_IO: Executing the following command: %s", verb); - /* start the FMU if not running */ + /* start if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { return Voxl2IO::task_spawn(argc, argv); } - } - if (!strcmp(verb, "status")) { - if (!is_running()) { - PX4_INFO("Not running"); - return -1; - } - - return get_instance()->print_status(); + PX4_INFO("VOXL2_IO: Already running"); + return 0; } if (!is_running()) { - PX4_INFO("Not running"); + PX4_INFO("VOXL2_IO: Not running"); return -1; } + if (!strcmp(verb, "status")) { + return get_instance()->print_status(); + } + + if (!strcmp(verb, "calibrate_escs")) { if (get_instance()->_outputs_disabled) { - PX4_WARN("Can't calibrate ESCs while outputs are disabled."); + PX4_WARN("VOXL2_IO: Can't calibrate ESCs while outputs are disabled."); return -1; } return get_instance()->calibrate_escs(); } - while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'c': - output_channel = atoi(myoptarg); - - if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) { - char reason[50]; - sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1); - print_usage(reason); - return 0; - } - - break; - - case 'n': - repeat_count = atoi(myoptarg); - - if (repeat_count < 1) { - print_usage("bad repeat_count"); - return 0; - } - - break; - - case 't': - repeat_delay_us = atoi(myoptarg); - - if (repeat_delay_us < 1) { - print_usage("bad repeat delay"); - return 0; - } - - break; - - case 'r': - rate = atoi(myoptarg); - break; - - case 'p': - if (valid_port(atoi(myoptarg))) { - snprintf(get_instance()->_device, 2, "%s", myoptarg); - - } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); - return 0; - } - - break; - - default: - print_usage("Unknown command, parsing flags"); - return 0; - } - } - - if (!strcmp(verb, "pwm")) { - PX4_INFO("Output channel: %i", output_channel); - PX4_INFO("Repeat count: %i", repeat_count); - PX4_INFO("Repeat delay (us): %i", repeat_delay_us); - PX4_INFO("Rate: %i", rate); - - if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) { - PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate); - int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t id_fb = 0; - - if (output_channel == - 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'. - rate_req[0] = rate; - rate_req[1] = rate; - rate_req[2] = rate; - rate_req[3] = rate; - - } else { - rate_req[output_channel] = rate; - id_fb = output_channel; - } - - cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3], - 0, 0, 0, 0, - id_fb, cmd.buf, sizeof(cmd.buf)); - - cmd.response = false; - cmd.repeats = repeat_count; - cmd.resp_delay_us = 1000; - cmd.repeat_delay_us = repeat_delay_us; - cmd.print_feedback = false; - - PX4_INFO("feedback id debug: %i", id_fb); - PX4_INFO("Sending UART M0065 power command %i", rate); - - if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet: stop PWMs"); - return -1; - - } else { - get_instance()->_bytes_sent += cmd.len; - get_instance()->_packets_sent++; - } - - } else { - print_usage("Invalid Output Channel, use 0-3"); - return 0; - } + if (!strcmp(verb, "enable_debug")) { + get_instance()->_debug = true; } return print_usage("unknown custom command"); @@ -1057,22 +766,20 @@ int Voxl2IO::custom_command(int argc, char *argv[]) int Voxl2IO::print_status() { - PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); - PX4_INFO("PWM Rate: 400 Hz"); // Only support 400 Hz for now - PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no"); - PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version); - PX4_INFO("RC Type: SBUS"); // Only support SBUS through M0065 for now - PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes"); - PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames); - PX4_INFO("UART port: %s", _device); - PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); - PX4_INFO("Packets sent: %" PRIu32, _packets_sent); - PX4_INFO(""); - PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]); - PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]); - PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]); - PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + //PX4_INFO("VOXL2_IO: Max update rate: %u Hz", 1000000/_current_update_interval); + //PX4_INFO("VOXL2_IO: PWM Rate: 400 Hz"); // Only support 400 Hz for now + PX4_INFO("VOXL2_IO: Outputs on : %s", _pwm_on ? "yes" : "no"); + PX4_INFO("VOXL2_IO: SW version : %u", _version_info.sw_version); + PX4_INFO("VOXL2_IO: HW version : %u: %s", _version_info.hw_version, + board_id_to_name(_version_info.hw_version).c_str()); + PX4_INFO("VOXL2_IO: RC Type : SBUS"); // Only support SBUS through M0065 for now + PX4_INFO("VOXL2_IO: RC Connected : %s", hrt_absolute_time() - _rc_last_valid_time > 500000 ? "no" : "yes"); + PX4_INFO("VOXL2_IO: RC Packets Rxd : %" PRIu16, _sbus_total_frames); + PX4_INFO("VOXL2_IO: UART port : %s", _device); + PX4_INFO("VOXL2_IO: UART open : %s", _uart_port.open() ? "yes" : "no"); + PX4_INFO("VOXL2_IO: Packets sent : %" PRIu32, _packets_sent); + PX4_INFO("VOXL2_IO: "); + print_params(); perf_print_counter(_cycle_perf); perf_print_counter(_output_update_perf); @@ -1103,17 +810,49 @@ px4io driver is used for main ones. PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); - PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false); - PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); - PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); - PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("enable_debug", "Enables driver debugging"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } +std::string Voxl2IO::board_id_to_name(int board_id) +{ + switch(board_id){ + case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)"); + case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"); + case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"); + case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)"); + case 35: return std::string("ModalAi I/O Expander (M0065)"); + case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)"); + case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)"); + case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)"); + case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)"); + case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)"); + case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)"); + case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)"); + default: return std::string("Unknown Board"); + } +} + +void Voxl2IO::print_params() +{ + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_BAUD : %" PRId32, _parameters.baud_rate); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC1 : %" PRId32, _parameters.function_map[0]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC2 : %" PRId32, _parameters.function_map[1]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC3 : %" PRId32, _parameters.function_map[2]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC4 : %" PRId32, _parameters.function_map[3]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC5 : %" PRId32, _parameters.function_map[4]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC6 : %" PRId32, _parameters.function_map[5]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC7 : %" PRId32, _parameters.function_map[6]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC8 : %" PRId32, _parameters.function_map[7]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_DIS : %" PRId32, _parameters.pwm_dis); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MIN : %" PRId32, _parameters.pwm_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MAX : %" PRId32, _parameters.pwm_max); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMIN : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMAX : %" PRId32, _parameters.pwm_cal_max); +} + extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]); int voxl2_io_main(int argc, char *argv[]) diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp index 6ac46251bc..b3458456a0 100644 --- a/src/drivers/voxl2_io/voxl2_io.hpp +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -49,16 +50,17 @@ #include #include #include +#include #include #include #include #include -#include "voxl2_io_serial.hpp" - #include "voxl2_io_packet.h" #include "voxl2_io_packet_types.h" +using namespace device; + using namespace time_literals; class Voxl2IO final : public ModuleBase, public OutputModuleInterface @@ -85,8 +87,8 @@ public: virtual int init(); - void update_pwm_config(); int get_version_info(); + void print_params(); struct Command { uint16_t id = 0; @@ -105,8 +107,8 @@ public: void clear() { len = 0; } }; - int send_cmd_thread_safe(Command *cmd); - int receive_sbus(); + int receive_uart_packets(); + int parse_sbus_packet(uint8_t *raw_data, uint32_t data_len); void fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -114,53 +116,42 @@ public: unsigned frame_drops, int rssi, input_rc_s &input_rc); private: void Run() override; - bool stop_all_pwms(); /* PWM Parameters */ - static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off - static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes - static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1; - static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; - static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4; - static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0; - - static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200; - static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000; - - static constexpr uint16_t DISARMED_VALUE = 0; - - static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0; - static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800; - static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0; - static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0; - - static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000; - static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000; - static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900; - static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks + static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; + static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 8; + static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = + UINT16_MAX; //this will tell mixer to use the default failsafe depending on the function /* SBUS */ static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30; static constexpr uint16_t SBUS_PAYLOAD = 3; + //packet packing function accepts pulse width in nanoseconds + //assuming the mixer outputs in microseconds, need to convert + static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 1000; //standard pwm + //static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 125; //oneshot125 + /* M0065 version info */ - static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6; - static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1; - static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35; - VOXL2_IO_VERSION_INFO _version_info; + static constexpr uint16_t VOXL2_IO_SW_VERSION = 2; + static constexpr uint16_t VOXL2_IO_HW_VERSION = 35; //this is the version of the HW + int _board_detect_retries{3}; + VOXL2_IO_EXTENDED_VERSION_INFO _version_info; /* Module update interval */ static constexpr unsigned _current_update_interval{4000}; // 250 Hz typedef struct { - int32_t config{VOXL2_IO_CONFIG}; int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD}; - int32_t pwm_min{VOXL2_IO_DEFAULT_MIN}; - int32_t pwm_max{VOXL2_IO_DEFAULT_MAX}; - int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; - int32_t param_rc_input_proto{0}; + int32_t pwm_min{0}; + int32_t pwm_max{0}; + int32_t pwm_dis{0}; + int32_t pwm_cal_min{0}; + int32_t pwm_cal_max{0}; + //int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; + //int32_t param_rc_input_proto{0}; int32_t param_rc_rssi_pwm_chan{0}; - int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0, 0, 0, 0, 0}; int32_t verbose_logging{0}; } voxl2_io_params_t; voxl2_io_params_t _parameters; @@ -179,20 +170,18 @@ private: SCAN } _rc_mode{RC_MODE::SCAN}; - /* QUP7, VOXL2 J19, /dev/slpi-uart-7*/ char _device[10] {VOXL2_IO_DEFAULT_PORT}; - Voxl2IoSerial *_uart_port; + Serial _uart_port{}; /* Mixer output */ MixingOutput _mixing_output; /* RC input */ - VOXL2_IOPacket _sbus_packet; - uint64_t _rc_last_valid; + VOXL2_IOPacket _voxl2_io_packet; + uint64_t _rc_last_valid_time; uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX}; - unsigned _sbus_frame_drops{0}; - uint16_t _sbus_total_frames{0}; - bool _new_packet{false}; + unsigned int _sbus_frame_drops{0}; + uint32_t _sbus_total_frames{0}; /* Publications */ uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; @@ -201,17 +190,12 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; bool _pwm_on{false}; - int32_t _pwm_fullscale{0}; - int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; bool _outputs_disabled{false}; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; bool _debug{false}; - uint16_t _cmd_id{0}; - Command _current_cmd; - px4::atomic _pending_cmd{nullptr}; static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; @@ -220,9 +204,8 @@ private: uint32_t _packets_sent{0}; uint32_t _packets_received{0}; - int parse_response(uint8_t *buf, uint8_t len); int load_params(voxl2_io_params_t *params); int update_params(); - int flush_uart_rx(); int calibrate_escs(); + std::string board_id_to_name(int board_id); }; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c index 3de26acbfe..99340fa974 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.c +++ b/src/drivers/voxl2_io/voxl2_io_packet.c @@ -150,6 +150,32 @@ int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); } +typedef struct { + uint8_t command_type; + //uint8_t channel_offset; + uint16_t vals[8]; //could be 1,2,4,6,8 +} __attribute__((__packed__)) pwm_hires_cmd_t; + +pwm_hires_cmd_t hires_cmd; + +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size) +{ + if (cmd_cnt > 8) { + return -1; + } + + hires_cmd.command_type = 0; + //hires_cmd.channel_offset = 0; + + //resolution of commands in the packet is 0.05us = 50ns + for (uint32_t idx = 0; idx < cmd_cnt; idx++) { + hires_cmd.vals[idx] = pwm_val_ns[idx] / 50; + } + + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_cmd, (cmd_cnt * 2 + 1), out, + out_size); +} + int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) { uint16_t packet_size = size + 5; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h index 7e6237017f..1c4d242814 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.h +++ b/src/drivers/voxl2_io/voxl2_io_packet.h @@ -213,6 +213,9 @@ int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, in uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, uint8_t *out, uint16_t out_size); +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size); + + // Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) // Return value is the length of generated packet (if positive), otherwise error code int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h index 150982eca4..12766fb0ef 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet_types.h +++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h @@ -46,6 +46,7 @@ #define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3 #define VOXL2_IO_PACKET_TYPE_STEP_CMD 4 #define VOXL2_IO_PACKET_TYPE_LED_CMD 5 +#define VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD 6 #define VOXL2_IO_PACKET_TYPE_RESET_CMD 10 #define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11 #define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12 diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c index 704cca989e..ac99723b8d 100644 --- a/src/drivers/voxl2_io/voxl2_io_params.c +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -32,34 +32,73 @@ ****************************************************************************/ /** - * UART M0065 baud rate + * VOXL2_IO UART baud rate * - * Default rate is 921600, which is used for communicating with M0065. + * Default rate is 921600, which is used for communicating with VOXL2_IO board * * @group VOXL2 IO * @unit bit/s */ PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600); + /** - * M0065 PWM Min + * VOXL2_IO Disabled PWM * - * Minimum duration (microseconds) for M0065 PWM + * Pulse duration in disabled state (microseconds) for VOXL2_IO board * * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ -PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); +PARAM_DEFINE_INT32(VOXL2_IO_DIS, 1000); /** - * M0065 PWM Max + * VOXL2_IO Min PWM * - * Maximum duration (microseconds) for M0065 PWM + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1100); + +/** + * VOXL2_IO Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); + + +/** + * VOXL2_IO Calibration Min PWM + * + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_CMIN, 1050); + +/** + * VOXL2_IO Calibration Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_CMAX, 2000); diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp deleted file mode 100644 index cee0923ece..0000000000 --- a/src/drivers/voxl2_io/voxl2_io_serial.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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 "string.h" -#include "voxl2_io_serial.hpp" - -Voxl2IoSerial::Voxl2IoSerial() -{ -} - -Voxl2IoSerial::~Voxl2IoSerial() -{ - if (_uart_fd >= 0) { - uart_close(); - } -} - -int Voxl2IoSerial::uart_open(const char *dev, speed_t speed) -{ - if (_uart_fd >= 0) { - PX4_ERR("Port in use: %s (%i)", dev, errno); - return -1; - } - - /* Open UART */ -#ifdef __PX4_QURT - _uart_fd = qurt_uart_open(dev, speed); -#else - _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); -#endif - - if (_uart_fd < 0) { - PX4_ERR("Error opening port: %s (%i)", dev, errno); - return -1; - } - -#ifndef __PX4_QURT - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { - PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); - uart_close(); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &_cfg); - - /* Disable output post-processing */ - _cfg.c_oflag &= ~OPOST; - - _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ - _cfg.c_cflag &= ~CSIZE; - _cfg.c_cflag |= CS8; /* 8-bit characters */ - _cfg.c_cflag &= ~PARENB; /* no parity bit */ - _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ - _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ - - /* setup for non-canonical mode */ - _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); - _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); - - if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { - PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); - uart_close(); - return -1; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { - PX4_ERR("Error configuring port: %s (tcsetattr)", dev); - uart_close(); - return -1; - } - -#endif - - _speed = speed; - - return 0; -} - -int Voxl2IoSerial::uart_set_baud(speed_t speed) -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - return -1; - } - - if (cfsetispeed(&_cfg, speed) < 0) { - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { - return -1; - } - - _speed = speed; - - return 0; -#endif - - return -1; -} - -int Voxl2IoSerial::uart_close() -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - PX4_ERR("invalid state for closing"); - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { - PX4_ERR("failed restoring uart to original state"); - } - - if (close(_uart_fd)) { - PX4_ERR("error closing uart"); - } - -#endif - - _uart_fd = -1; - - return 0; -} - -int Voxl2IoSerial::uart_write(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for writing or buffer"); - return -1; - } - -#ifdef __PX4_QURT - return qurt_uart_write(_uart_fd, (const char *) buf, len); -#else - return write(_uart_fd, buf, len); -#endif -} - -int Voxl2IoSerial::uart_read(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for reading or buffer"); - return -1; - } - -#ifdef __PX4_QURT -#define ASYNC_UART_READ_WAIT_US 2000 - // The UART read on SLPI is via an asynchronous service so specify a timeout - // for the return. The driver will poll periodically until the read comes in - // so this may block for a while. However, it will timeout if no read comes in. - return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); -#else - return read(_uart_fd, buf, len); -#endif -} diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 7a7b5973fd..a9396caa1f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -34,15 +34,14 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) -add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) -add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) add_subdirectory(cdev EXCLUDE_FROM_ALL) add_subdirectory(cdrstream EXCLUDE_FROM_ALL) add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL) add_subdirectory(collision_prevention EXCLUDE_FROM_ALL) add_subdirectory(component_information EXCLUDE_FROM_ALL) +add_subdirectory(control_allocation EXCLUDE_FROM_ALL) add_subdirectory(controllib EXCLUDE_FROM_ALL) add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) @@ -54,6 +53,7 @@ add_subdirectory(geo EXCLUDE_FROM_ALL) add_subdirectory(heatshrink EXCLUDE_FROM_ALL) add_subdirectory(hysteresis EXCLUDE_FROM_ALL) add_subdirectory(l1 EXCLUDE_FROM_ALL) +add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL) add_subdirectory(led EXCLUDE_FROM_ALL) add_subdirectory(matrix EXCLUDE_FROM_ALL) add_subdirectory(mathlib EXCLUDE_FROM_ALL) @@ -68,6 +68,7 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) +add_subdirectory(rover_control EXCLUDE_FROM_ALL) add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index 8fa11a8f2a..22f393cd30 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -111,16 +111,15 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index) PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } -void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) +void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address, hrt_abstime now) { - _traffic_buffer.timestamp.push_back(hrt_absolute_time()); + _traffic_buffer.timestamp.push_back(now); _traffic_buffer.icao_address.push_back(icao_address); PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } -void AdsbConflict::get_traffic_state() +void AdsbConflict::get_traffic_state(hrt_abstime now) { - const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address); const bool old_conflict = (traffic_index >= 0); @@ -130,11 +129,11 @@ void AdsbConflict::get_traffic_state() bool old_conflict_warning_expired = false; if (old_conflict && _conflict_detected) { - old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT); + old_conflict_warning_expired = now > _traffic_buffer.timestamp[traffic_index] + CONFLICT_WARNING_TIMEOUT; } if (new_traffic && _conflict_detected && !_traffic_buffer_full) { - add_icao_address_from_conflict_list(_transponder_report.icao_address); + add_icao_address_from_conflict_list(_transponder_report.icao_address, now); _traffic_state = TRAFFIC_STATE::ADD_CONFLICT; } else if (new_traffic && _conflict_detected && _traffic_buffer_full) { @@ -142,7 +141,7 @@ void AdsbConflict::get_traffic_state() } else if (old_conflict && _conflict_detected && old_conflict_warning_expired) { - _traffic_buffer.timestamp[traffic_index] = hrt_absolute_time(); + _traffic_buffer.timestamp[traffic_index] = now; _traffic_state = TRAFFIC_STATE::REMIND_CONFLICT; } else if (old_conflict && !_conflict_detected) { @@ -152,7 +151,6 @@ void AdsbConflict::get_traffic_state() } else { _traffic_state = TRAFFIC_STATE::NO_CONFLICT; } - } void AdsbConflict::remove_expired_conflicts() @@ -172,8 +170,9 @@ void AdsbConflict::remove_expired_conflicts() bool AdsbConflict::handle_traffic_conflict() { + const hrt_abstime now = hrt_absolute_time(); - get_traffic_state(); + get_traffic_state(now); bool take_action = false; @@ -184,7 +183,8 @@ bool AdsbConflict::handle_traffic_conflict() take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f), (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, _transponder_report.callsign, - _transponder_report.icao_address); + _transponder_report.icao_address, + now); } break; @@ -192,7 +192,7 @@ bool AdsbConflict::handle_traffic_conflict() events::send(events::ID("navigator_traffic_resolved"), events::Log::Notice, "Traffic Conflict Resolved {1}!", _transponder_report.icao_address); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; } break; @@ -201,7 +201,7 @@ bool AdsbConflict::handle_traffic_conflict() if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) && (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) { events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on"); - _last_buffer_full_warning_time = hrt_absolute_time(); + _last_buffer_full_warning_time = now; } //disable conflict warnings when buffer is full @@ -234,7 +234,7 @@ void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, fl bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address) + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address, hrt_abstime now) { switch (_conflict_detection_params.traffic_avoidance_mode) { @@ -251,7 +251,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper } - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; break; } @@ -278,7 +278,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; break; } @@ -294,7 +294,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; return true; @@ -312,7 +312,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; return true; @@ -331,7 +331,7 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position", icao_address, traffic_seperation, traffic_direction); - _last_traffic_warning_time = hrt_absolute_time(); + _last_traffic_warning_time = now; return true; diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index 9942a6b921..5b6dc4edd9 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -109,16 +109,16 @@ public: void remove_icao_address_from_conflict_list(int traffic_index); - void add_icao_address_from_conflict_list(uint32_t icao_address); + void add_icao_address_from_conflict_list(uint32_t icao_address, hrt_abstime now); - void get_traffic_state(); + void get_traffic_state(hrt_abstime now); void set_conflict_detection_params(float crosstrack_separation, float vertical_separation, int collision_time_threshold, uint8_t traffic_avoidance_mode); bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address); + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address, hrt_abstime now); transponder_report_s _transponder_report{}; @@ -129,8 +129,7 @@ public: float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav, double lon_uav, float &alt_uav); - void run_fake_traffic(double &lat_uav, double &lon_uav, - float &alt_uav); + void run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav); void remove_expired_conflicts(); diff --git a/src/lib/adsb/AdsbConflictTest.cpp b/src/lib/adsb/AdsbConflictTest.cpp index 5d6b6ccbe6..bba64cd87b 100644 --- a/src/lib/adsb/AdsbConflictTest.cpp +++ b/src/lib/adsb/AdsbConflictTest.cpp @@ -4,7 +4,6 @@ #include "AdsbConflictTest.h" - class TestAdsbConflict : public AdsbConflict { public: @@ -25,8 +24,6 @@ public: TEST_F(AdsbConflictTest, detectTrafficConflict) { - - int collision_time_threshold = 60; float crosstrack_separation = 500.0f; @@ -42,19 +39,15 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) uint32_t traffic_dataset_size = sizeof(traffic_dataset) / sizeof(traffic_dataset[0]); - TestAdsbConflict adsb_conflict; adsb_conflict.set_conflict_detection_params(crosstrack_separation, vertical_separation, collision_time_threshold, 1); for (uint32_t i = 0; i < traffic_dataset_size; i++) { - - //printf("---------------%d--------------\n", i); - struct traffic_data_s traffic = traffic_dataset[i]; - + // GIVEN traffic dataset (which should result in conflict) adsb_conflict._transponder_report.lat = traffic.lat_traffic; adsb_conflict._transponder_report.lon = traffic.lon_traffic; adsb_conflict._transponder_report.altitude = traffic.alt_traffic; @@ -62,22 +55,17 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) adsb_conflict._transponder_report.hor_velocity = traffic.vxy_traffic; adsb_conflict._transponder_report.ver_velocity = traffic.vz_traffic; + // WHEN detect traffic conflict is called adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now); - //printf("conflict_detected %d \n", adsb_conflict._conflict_detected); - - //printf("------------------------------\n"); - //printf("------------------------------\n \n"); - + // THEN expect conflict to be detected EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict); } - } TEST_F(AdsbConflictTest, trafficAlerts) { - struct traffic_buffer_s used_buffer; used_buffer.icao_address.push_back(2345); used_buffer.icao_address.push_back(1234); @@ -116,77 +104,98 @@ TEST_F(AdsbConflictTest, trafficAlerts) full_buffer.timestamp.push_back(1000_s); full_buffer.timestamp.push_back(58943_s); - - struct traffic_buffer_s empty_buffer = {}; - TestAdsbConflict adsb_conflict; + // GIVEN used buffer adsb_conflict.set_traffic_buffer(used_buffer); + // WHEN no conflict detected bool conflict_detected = false; + hrt_abstime now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 00001; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect no conflict printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // GIVEN conflict detected conflict_detected = true; + now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be added to buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); + // GIVEN empty buffer adsb_conflict.set_traffic_buffer(empty_buffer); + // WHEN conflict detected conflict_detected = true; + now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 9876; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be added to buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); + // GIVEN full buffer adsb_conflict.set_traffic_buffer(full_buffer); + // WHEN conflict detected conflict_detected = true; + now = 1_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect buffer full printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL); + // WHEN conflict set to false again conflict_detected = false; + now = 2_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect no conflict message printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // WHEN existing conflict is set to false conflict_detected = false; + now = 3_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be removed from buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); + // GIVEN used buffer adsb_conflict.set_traffic_buffer(used_buffer); + // WHEN conflict is set to false again conflict_detected = false; + now = 0_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be removed from buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); - } TEST_F(AdsbConflictTest, trafficReminder) @@ -229,59 +238,77 @@ TEST_F(AdsbConflictTest, trafficReminder) full_buffer.timestamp.push_back(100_s); full_buffer.timestamp.push_back(5843_s); - TestAdsbConflict adsb_conflict; + // GIVEN buffer with 8685 at t=100 adsb_conflict.set_traffic_buffer(used_buffer); + // WHEN conflict detected at t=200 bool conflict_detected = true; + hrt_abstime now = 200_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict reminder printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); + // WHEN INSTEAD conflict is detected only 1s later conflict_detected = true; + now = 201_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN do not sent conflict notification again printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // GIVEN full buffer with 8685 at t=100 adsb_conflict.set_traffic_buffer(full_buffer); + // WHEN conflict detected at t=400 conflict_detected = true; + now = 400_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict reminder printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT); + // WHEN INSTEAD conflict is detected only 1s later conflict_detected = true; + now = 401_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN do not sent conflict notification again printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT); + // WHEN conflict is set to false conflict_detected = false; + now = 600_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 8685; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect conflict to be removed from buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT); + // WHEN new conflict is detected conflict_detected = true; + now = 700_s; adsb_conflict.set_conflict(conflict_detected); adsb_conflict._transponder_report.icao_address = 7777; - adsb_conflict.get_traffic_state(); + adsb_conflict.get_traffic_state(now); + // THEN expect new conflict to be added to buffer printf("adsb_conflict._traffic_state %d \n", adsb_conflict._traffic_state); EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT); - } diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp deleted file mode 100644 index f818f068a0..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance.cpp - */ - -#include "ObstacleAvoidance.hpp" -#include "bezier/BezierN.hpp" - -using namespace matrix; -using namespace time_literals; - -/** Timeout in us for trajectory data to get considered invalid */ -static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; -/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */ -static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; -static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; - -ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : - ModuleParams(parent) -{ - _desired_waypoint = empty_trajectory_waypoint; - _failsafe_position.setNaN(); - _avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE); - _no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US); - -} - -void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) -{ - _sub_vehicle_status.update(); - _sub_vehicle_trajectory_waypoint.update(); - _sub_vehicle_trajectory_bezier.update(); - - const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get(); - const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get(); - - const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US; - const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime( - bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f); - const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout; - - const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; - const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0; - - _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid - && !avoidance_bezier_valid, hrt_absolute_time()); - - const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()); - - if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) { - // if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system - return; - } - - if (avoidance_invalid) { - if (_avoidance_activated) { - // Invalid point received: deactivate - PX4_WARN("Obstacle Avoidance system failed, loitering"); - _publishVehicleCmdDoLoiter(); - _avoidance_activated = false; - } - - if (!_failsafe_position.isAllFinite()) { - // save vehicle position when entering failsafe - _failsafe_position = _position; - } - - pos_sp = _failsafe_position; - vel_sp.setNaN(); - yaw_sp = NAN; - yaw_speed_sp = NAN; - - // Do nothing further - wait until activation - return; - - } else if (!_avoidance_activated) { - // First setpoint has been received: activate - PX4_INFO("Obstacle Avoidance system activated"); - _failsafe_position.setNaN(); - _avoidance_activated = true; - } - - if (avoidance_point_valid && !wp_msg_timeout) { - const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - pos_sp = Vector3f(point0.position); - vel_sp = Vector3f(point0.velocity); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = point0.yaw; - yaw_speed_sp = point0.yaw_speed; - } - - } else if (avoidance_bezier_valid && !bezier_msg_timeout) { - float yaw = NAN, yaw_speed = NAN; - _generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = yaw; - yaw_speed_sp = yaw_speed; - } - } -} - -void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, - float &yaw, float &yaw_velocity) -{ - const auto &msg = _sub_vehicle_trajectory_bezier.get(); - int bezier_order = msg.bezier_order; - matrix::Vector3f bezier_points[bezier_order]; - float bezier_yaws[bezier_order]; - - for (int i = 0; i < bezier_order; i++) { - bezier_points[i] = Vector3f(msg.control_points[i].position); - bezier_yaws[i] = msg.control_points[i].yaw; - } - - const float duration_s = msg.control_points[bezier_order - 1].delta; - const hrt_abstime now = hrt_absolute_time(); - const hrt_abstime start = msg.timestamp; - const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f); - - float T = NAN; - - if (bezier::calculateT(start, end, now, T) && - bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) && - bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity) - ) { - // translate bezier velocities T [0;1] into real velocities m/s - yaw_velocity /= duration_s; - velocity /= duration_s; - - } else { - PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); - } -} - - -void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, - const bool ext_yaw_active, const int wp_type) -{ - _desired_waypoint.timestamp = hrt_absolute_time(); - _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - _curr_wp = curr_wp; - _ext_yaw_active = ext_yaw_active; - - curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type; - - next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true; -} - -void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type) -{ - pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); - vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); - - _desired_waypoint = empty_trajectory_waypoint; -} - -void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp, - float target_acceptance_radius, const Vector2f &closest_pt) -{ - _position = pos; - position_controller_status_s pos_control_status = {}; - pos_control_status.timestamp = hrt_absolute_time(); - - // vector from previous triplet to current target - Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp); - // vector from previous triplet to the vehicle projected position on the line previous-target triplet - Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp); - // fraction of the previous-tagerget line that has been flown - const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length(); - - Vector2f pos_to_target = Vector2f(_curr_wp - _position); - - if (prev_curr_travelled > 1.0f) { - // if the vehicle projected position on the line previous-target is past the target waypoint, - // increase the target acceptance radius such that navigator will update the triplets - pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f; - } - - const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2)); - - bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z); - _no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time()); - - if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get() - && _no_progress_z_hysteresis.get_state()) { - // vehicle above or below the target waypoint - pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; - } - - _prev_pos_to_target_z = pos_to_target_z; - - // do not check for waypoints yaw acceptance in navigator - pos_control_status.yaw_acceptance = NAN; - - _pub_pos_control_status.publish(pos_control_status); -} - -void ObstacleAvoidance::_publishVehicleCmdDoLoiter() -{ - vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - 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.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command - _pub_vehicle_command.publish(command); -} diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp deleted file mode 100644 index 8a27ffbbf1..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.hpp +++ /dev/null @@ -1,152 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance.hpp - * This class is used to inject the setpoints of an obstacle avoidance system - * into the FlightTasks - * - * @author Martina Rivizzigno - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, - { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} - } -}; - -class ObstacleAvoidance : public ModuleParams -{ -public: - ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance() = default; - - /** - * Inject setpoints from obstacle avoidance system into FlightTasks. - * @param pos_sp, position setpoint - * @param vel_sp, velocity setpoint - * @param yaw_sp, yaw setpoint - * @param yaw_speed_sp, yaw speed setpoint - */ - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp); - - /** - * Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight. - * @param curr_wp, current position triplet - * @param curr_yaw, current yaw triplet - * @param curr_yawspeed, current yaw speed triplet - * @param next_wp, next position triplet - * @param next_yaw, next yaw triplet - * @param next_yawspeed, next yaw speed triplet - * @param wp_type, current triplet type - */ - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type); - /** - * Updates the desired setpoints to send to the obstacle avoidance system. - * @param pos_sp, desired position setpoint computed by the active FlightTask - * @param vel_sp, desired velocity setpoint computed by the active FlightTask - * @param type, current triplet type - */ - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type); - - /** - * Checks the vehicle progress between previous and current position waypoint of the triplet. - * @param pos, vehicle position - * @param prev_wp, previous position triplet - * @param target_acceptance_radius, current position triplet xy acceptance radius - * @param closest_pt, closest point to the vehicle on the line previous-current position triplet - */ - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt); - -protected: - - uORB::SubscriptionData _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - - vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ - - uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ - uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ - uORB::Publication _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ - - matrix::Vector3f _curr_wp = {}; /**< current position triplet */ - matrix::Vector3f _position = {}; /**< current vehicle position */ - matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ - - bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */ - - systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */ - systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */ - - float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */ - - bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ - - /** - * Publishes vehicle command. - */ - void _publishVehicleCmdDoLoiter(); - void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity); - - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); - -}; diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp deleted file mode 100644 index 008a8758a9..0000000000 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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 -#include -#include - - -using namespace matrix; -// to run: make tests TESTFILTER=ObstacleAvoidance - -class ObstacleAvoidanceTest : public ::testing::Test -{ -public: - Vector3f pos_sp; - Vector3f vel_sp; - float yaw_sp = 0.123f; - float yaw_speed_sp = NAN; - void SetUp() override - - { - param_control_autosave(false); - param_reset_all(); - pos_sp = Vector3f(1.f, 1.2f, 0.1f); - vel_sp = Vector3f(0.3f, 0.4f, 0.1f); - } -}; - -class TestObstacleAvoidance : public ::ObstacleAvoidance -{ -public: - TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {} - void paramsChanged() {ObstacleAvoidance::updateParamsImpl();} - void test_setPosition(Vector3f &pos) {_position = pos;} -}; - -TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); } - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - message.timestamp = hrt_absolute_time(); - message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_bezier_s message {}; - message.timestamp = hrt_absolute_time(); - message.bezier_order = 2; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - vehicle_trajectory_bezier_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(2.6f, pos_sp(0)); - EXPECT_FLOAT_EQ(2.4f, pos_sp(1)); - EXPECT_LT(2.7f, pos_sp(2)); - EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests - EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0); - EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f); - EXPECT_FLOAT_EQ(0.23, yaw_sp); - EXPECT_FLOAT_EQ(yaw_speed_sp, 0); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - Vector3f pos(3.1f, 4.7f, 5.2f); - oa.test_setPosition(pos); - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints shouldn't be injected - EXPECT_FLOAT_EQ(pos(0), pos_sp(0)); - EXPECT_FLOAT_EQ(pos(1), pos_sp(1)); - EXPECT_FLOAT_EQ(pos(2), pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FALSE(PX4_ISFINITE(yaw_sp)); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_desired) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and the waypoints from FLightTaskAuto - TestObstacleAvoidance oa; - - pos_sp = Vector3f(1.f, 1.2f, NAN); - vel_sp = Vector3f(NAN, NAN, 0.7f); - int type = 4; - Vector3f curr_wp(1.f, 1.2f, 5.0f); - float curr_yaw = 1.02f; - float curr_yawspeed = NAN; - Vector3f next_wp(11.f, 1.2f, 5.0f); - float next_yaw = 0.82f; - float next_yawspeed = NAN; - bool ext_yaw_active = false; - - // WHEN: we inject the setpoints and waypoints in the interface - oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active, - type); - oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type); - - // WHEN: we subscribe to the uORB message out of the interface - uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 - EXPECT_FLOAT_EQ(pos_sp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]); - EXPECT_FLOAT_EQ(pos_sp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1])); - EXPECT_FLOAT_EQ(vel_sp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid); - - EXPECT_FLOAT_EQ(curr_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]); - EXPECT_FLOAT_EQ(curr_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]); - EXPECT_FLOAT_EQ(curr_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]); - EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed)); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid); - - EXPECT_FLOAT_EQ(next_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]); - EXPECT_FLOAT_EQ(next_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]); - EXPECT_FLOAT_EQ(next_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]); - EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed)); - EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid); - -} diff --git a/src/lib/bezier/BezierN.cpp b/src/lib/bezier/BezierN.cpp deleted file mode 100644 index 0d30aebd88..0000000000 --- a/src/lib/bezier/BezierN.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ -/** - * @file BezierN.cpp - * Bezier function - * - * @author Julian Kent - */ - -#include -#include - -namespace -{ - -/* - * Generic in-place bezier implementation. Leaves result in first element. - * - */ -template -void calculateBezier(matrix::Vector *positions, int N, Scalar t, Scalar one_minus_t) -{ - for (int bezier_order = 1; bezier_order < N; bezier_order++) { - for (int i = 0; i < N - bezier_order; i++) { - positions[i] = positions[i] * one_minus_t + positions[i + 1] * t; - } - } -} -} - -namespace bezier -{ - -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using Vector3Df = matrix::Vector3; - - Vector3Df intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = positions[i](j); - } - } - - Df dual_t(t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(intermediates[0]); - velocity = matrix::collectDerivatives(intermediates[0]); - - return true; -} - -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using DDf = matrix::Dual; - using Vector3DDf = matrix::Vector3; - - Vector3DDf intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = Df(positions[i](j)); - } - } - - DDf dual_t(Df(t, 0), 0); // 1st and 2nd derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(matrix::collectReals(intermediates[0])); - velocity = matrix::collectReals(matrix::collectDerivatives(intermediates[0])); - acceleration = matrix::collectDerivatives(matrix::collectDerivatives(intermediates[0])); - - return true; -} - -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint) -{ - if (setpoints == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - - using Df = matrix::Dual; - using Vector1Df = matrix::Vector; - - Vector1Df intermediates[N]; - - // all yaw setpoints are wrapped relative to the starting yaw - const float offset = setpoints[0]; - - for (int i = 0; i < N; i++) { - intermediates[i](0) = matrix::wrap_pi(setpoints[i] - offset); - } - - Df dual_t (t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - Df result = intermediates[0](0); - yaw_setpoint = matrix::wrap_pi(result.value + offset); - yaw_vel_setpoint = result.derivative(0); - - return true; -} - -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T) -{ - if (now < start_time || end_time < now) { - return false; - } - - int64_t total_duration = end_time - start_time; - int64_t elapsed_duration = now - start_time; - - T = (float) elapsed_duration / (float) total_duration; - - return true; -} - -} diff --git a/src/lib/bezier/BezierNTest.cpp b/src/lib/bezier/BezierNTest.cpp deleted file mode 100644 index b08a40fd62..0000000000 --- a/src/lib/bezier/BezierNTest.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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. - * - ****************************************************************************/ - -/** - * Test code for the Velocity Smoothing library - * Run this test only using make tests TESTFILTER=BezierN - * - * @author Julian Kent - */ - -#include -#include - -#include "BezierN.hpp" - -TEST(BezierN_calculateBezier, checks_validity) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b; - EXPECT_FALSE(bezier::calculateBezierPosVel(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezier, checks_validity_accel) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b, c; - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(nullptr, 10, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 0, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, -0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, 1.5f, a, b, c)); -} - -TEST(BezierN_calculateBezier, work_1_point) -{ - // GIVEN: a single point bezier curve - matrix::Vector3f points[2] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 1, 0.5f, pos, vel)); - - // THEN: it should be the same as the point, and the velocity should be 0 - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_EQ(vel.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_2_points) -{ - // GIVEN: a 2-point bezier curve - matrix::Vector3f points[3] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.5f, pos, vel)); - - // THEN: the position should be the mid-point between the start and end, and velocity should be the length - EXPECT_EQ((pos - matrix::Vector3f(3, 1, 2)).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the beginning point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the end point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 1.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_zero_accel) -{ - // GIVEN: 3 points bezier, evenly spaced in a straight line - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(9, -2, -1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - - // THEN: it should be the middle point, with velocity of 1st to last - EXPECT_FLOAT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - - matrix::Vector3f pos2, vel2, accel2; - - // WHEN: we use the accel interface - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel2, accel2)); - - // THEN: it should give same position, velocity as the non-accel interface, and zero accel (since this curve is 0 accel) - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel2, accel2)); - - // THEN: it should be the starting point and same velocity - EXPECT_FLOAT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel2, accel2)); - - // THEN: it should be the ending point and same velocity - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_accel) -{ - // GIVEN: 3 points bezier, in a curve - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(19, -8, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - matrix::Vector3f pos2; - pos2 *= NAN; - - matrix::Vector3f accel_start, accel_mid, accel_end; - matrix::Vector3f vel_start, vel_mid, vel_end; - - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel_start, accel_start)); - - // THEN: it should give same position, velocity as the non-accel interface, and non-zero accel - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_start - vel).norm(), 0.f); - EXPECT_GT(accel_start.norm(), 0.f); - - // WHEN: we use the accel interface to get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel_mid, accel_mid)); - - // THEN: the values should matche between accel and non-accel version - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_mid - vel).norm(), 0.f); - - // AND: the accel should be the same as the start - EXPECT_FLOAT_EQ((accel_mid - accel_start).norm(), 0.f); - - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel_end, accel_end)); - - // THEN: it should be the ending point, and accel should match - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_end - vel).norm(), 0.f); - EXPECT_FLOAT_EQ((accel_end - accel_start).norm(), 0.f); - - // FINALLY: mid point velocity should be average of start and end velocity - EXPECT_FLOAT_EQ((vel_mid - 0.5f * (vel_start + vel_end)).norm(), 0.f); -} - -TEST(BezierN_calculateBezierYaw, checks_validity) -{ - float points[10]; - float a, b; - EXPECT_FALSE(bezier::calculateBezierYaw(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezierYaw, work_1_point) -{ - // GIVEN: a single yaw point - float points[2] = {M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we use it as a 1-point bezier curve - EXPECT_TRUE(bezier::calculateBezierYaw(points, 1, 0.5f, yaw, yaw_speed)); - - // THEN: it should have that same value, and the velocity should be 0 - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, 0); -} - -TEST(BezierN_calculateBezierYaw, work_2_points) -{ - // GIVEN: a single yaw point - float points[3] = {0, M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, 0); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 4); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); -} - -TEST(BezierN_calculateBezierYaw, work_2_points_wrap) -{ - // GIVEN: 2 yaw points on either side of the +- PI wrap line - float points[3] = {-M_PI + 0.1, M_PI - 0.1, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, -M_PI + 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the wrapped middle value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(matrix::wrap_pi(yaw - float(M_PI)), 0); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the end value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, M_PI - 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); -} - - -TEST(BezierN_calculateT, rejects_bad_timestamps) -{ - float f = NAN; - EXPECT_FALSE(bezier::calculateT(100, 1000, 99, f)); - EXPECT_FALSE(bezier::calculateT(100, 1000, 1001, f)); - EXPECT_FALSE(bezier::calculateT(1001, 1000, 1001, f)); -} - - -TEST(BezierN_calculateT, begin_middle_end) -{ - float f = NAN; - EXPECT_TRUE(bezier::calculateT(100, 1000, 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} - -TEST(BezierN_calculateT, giant_offset) -{ - int64_t offset = 0xFFFFFFFFFFFF; // 48 bit max - float f = NAN; - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp deleted file mode 100644 index 61309cc496..0000000000 --- a/src/lib/bezier/BezierQuad.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/**************************************************************************** - * - * 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 - * 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 BezierQuad.cpp - * Bezier function - * - * @author Dennis Mannhart - */ - -// The header includes this implementation, so in order to have this library shared with other -// .cpp files and avoid duplication of constants, we need to prevent including it twice -#ifndef BEZIER_QUAD_CPP -#define BEZIER_QUAD_CPP - - -#include "BezierQuad.hpp" - -namespace bezier -{ -static constexpr double GOLDEN_RATIO = 1.6180339887; //(sqrt(5)+1)/2 -static constexpr double RESOLUTION = 0.0001; //End criterion for golden section search - -template -void BezierQuad::setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration) -{ - _pt0 = pt0; - _ctrl = ctrl; - _pt1 = pt1; - _duration = duration; - _cached_resolution = (Tp)(-1); -} - -template -void BezierQuad::getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1) -{ - pt0 = _pt0; - ctrl = _ctrl; - pt1 = _pt1; -} - -template -matrix::Vector BezierQuad::getPoint(const Tp t) -{ - return (_pt0 * ((Tp)1 - t / _duration) * ((Tp)1 - t / _duration) + _ctrl * (Tp)2 * (( - Tp)1 - t / _duration) * t / _duration + _pt1 * - t / _duration * t / _duration); -} - -template -matrix::Vector BezierQuad::getVelocity(const Tp t) -{ - return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration)); -} - -template -matrix::Vector BezierQuad::getAcceleration() -{ - return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration)); -} - -template -void BezierQuad::getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp time) -{ - point = getPoint(time); - vel = getVelocity(time); - acc = getAcceleration(); -} - -template -void BezierQuad::getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get states corresponding to t - getStates(point, vel, acc, t); - -} - -template -void BezierQuad::setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration) -{ - // update bezier points - _ctrl = ctrl; - _duration = duration; - _pt0 = _ctrl - vel0 * _duration / (Tp)2; - _pt1 = _ctrl + vel1 * _duration / (Tp)2; - _cached_resolution = (Tp)(-1); -} - -template -Tp BezierQuad::getArcLength(const Tp resolution) -{ - // we don't need to recompute arc length if: - // 1. _cached_resolution is up to date; 2. _cached_resolution is smaller than desired resolution (= more accurate) - if ((_cached_resolution > (Tp)0) && (_cached_resolution <= resolution)) { - return _cached_arc_length; - } - - // get number of elements - int n = (int)(roundf(_duration / resolution)); - - // check if n is even - if (n % 2 == 1) { - n += 1; - } - - // step size - Tp h = (_duration) / n; - - // get integration - _cached_arc_length = (Tp)0; - Vector3_t y; - - for (int i = 1; i < n; i++) { - - y = getVelocity(h * i); - - if (i % 2 == 1) { - _cached_arc_length += (Tp)4 * y.length(); - - } else { - _cached_arc_length += (Tp)2 * y.length(); - } - } - - // velocity length at start and end points - Tp y0 = getVelocity((Tp)0).length(); - Tp yn = getVelocity(_duration).length(); - - // 1/3 simpsons rule - _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length); - - // update cache - _cached_resolution = resolution; - - return _cached_arc_length; -} - -template -Tp BezierQuad::getDistToClosestPoint(const Vector3_t &pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get closest point - Vector3_t point = getPoint(t); - return (pose - point).length(); -} - -template -Tp BezierQuad::_goldenSectionSearch(const Vector3_t &pose) -{ - Tp a, b, c, d; - a = (Tp)0; // represents most left point - b = _duration * (Tp)1; // represents most right point - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - - while (fabsf(c - d) > RESOLUTION) { - if (_getDistanceSquared(c, pose) < _getDistanceSquared(d, pose)) { - b = d; - - } else { - a = c; - } - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - } - - return (b + a) / (Tp)2; -} - -template -Tp BezierQuad::_getDistanceSquared(const Tp t, const Vector3_t &pose) -{ - // get point on bezier - Vector3_t vec = getPoint(t); - - // get vector from point to pose - vec = vec - pose; - - // norm squared - return (vec * vec); -} -} - -#endif diff --git a/src/lib/bezier/BezierQuad.hpp b/src/lib/bezier/BezierQuad.hpp deleted file mode 100644 index 5aebe94b7b..0000000000 --- a/src/lib/bezier/BezierQuad.hpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * 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 - * 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 BezierQuad.hpp - * - * Quadratic bezier lib - * - * A quadratic bezier function/spline is completely defined by three 3D points in space and a time scaling factor. - * pt0 and pt1 define the start and end points of the spline. ctrl point is a point in space that effects the curvature - * of the spline. The time scaling factor (= duration) defines the time it takes to travel along the spline from pt0 to - * pt1. - * A bezier spline is a continuous function from which position, velocity and acceleration can be extracted. For a given spline, - * acceleration stays constant. - */ - - -#pragma once - -#include - -namespace bezier -{ -template -class BezierQuad -{ -public: - - using Vector3_t = matrix::Vector; - - /** - * Empty constructor - */ - BezierQuad() : - _pt0(Vector3_t()), _ctrl(Vector3_t()), _pt1(Vector3_t()), _duration(1.0f) {} - - /** - * Constructor from array - */ - BezierQuad(const Tp pt0[3], const Tp ctrl[3], const Tp pt1[3], Tp duration = 1.0f) : - _pt0(Vector3_t(pt0)), _ctrl(Vector3_t(ctrl)), _pt1(Vector3_t(pt1)), _duration(duration) {} - - /** - * Constructor from vector - */ - BezierQuad(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = 1.0f): - _pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {} - - - /* - * Get bezier points - */ - void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1); - - /* - * Return pt0 - */ - Vector3_t getPt0() {return _pt0;} - - /* - * Return ctrl - */ - Vector3_t getCtrl() {return _ctrl;} - - /* - * Return pt1 - */ - Vector3_t getPt1() {return _pt1;} - - /** - * Set new bezier points and duration - */ - void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = (Tp)1); - - /* - * Set duration - * - * @param time is the total time it takes to travel along the bezier spline. - */ - void setDuration(const Tp time) {_duration = time;} - - /** - * Return point on bezier point corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return a point on bezier - */ - Vector3_t getPoint(const Tp t); - - /* - * Distance to closest point given a position - * - * @param pose is a position in 3D space from which distance to bezier is computed. - * @return distance to closest point on bezier - */ - Tp getDistToClosestPoint(const Vector3_t &pose); - - /* - * Return velocity on bezier corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return velocity vector at time t - */ - Vector3_t getVelocity(const Tp t); - - /* - * Return acceleration on bezier corresponding to time t - * - * @return constant acceleration of bezier - */ - Vector3_t getAcceleration(); - - /* - * Get all states on bezier corresponding to time t - */ - void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t); - - /* - * Get states on bezier which are closest to pose in space - * - * @param point is a posiiton on the spline that is closest to a given pose - * @param vel is the velocity at that given point - * @param acc is the acceleration for that spline - * @param pose represent a position in space from which closest point is computed - */ - void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose); - - /* - * Compute bezier from velocity at bezier end points and ctrl point - * - * The bezier end points are fully defined by a given control point ctrl, the duration and - * the desired velocity vectors at the end points. - */ - void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration = (Tp)1); - - /* - * Return the arc length of a bezier spline - * - * The arc length is computed with simpsons integration. - * @param resolution in meters. - */ - Tp getArcLength(const Tp resolution); - -private: - - Vector3_t _pt0; /**< Bezier starting point */ - Vector3_t _ctrl; /**< Bezier control point */ - Vector3_t _pt1; /**< bezier end point */ - Tp _duration = (Tp)1; /**< Total time to travle along spline */ - - Tp _cached_arc_length = (Tp)0; /**< The saved arc length of the spline */ - Tp _cached_resolution = (Tp)(-1); /**< The resolution used to compute the arc length. - Negative number means that cache is not up to date. */ - - /* - * Golden section search - */ - Tp _goldenSectionSearch(const Vector3_t &pose); - - /* - * Get squared distance from 3D pose in space and a point on bezier. - * - * @param t is the time in between [0, duration] that defines a point on the bezier. - * @param pose is a 3D pose in space. - */ - Tp _getDistanceSquared(const Tp t, const Vector3_t &pose); - - -}; - -using BezierQuad_f = BezierQuad; -using BezierQuad_d = BezierQuad; -} - -// include implementation -#include "BezierQuad.cpp" diff --git a/src/lib/collision_prevention/CMakeLists.txt b/src/lib/collision_prevention/CMakeLists.txt index 663c9e9fee..ae42b65454 100644 --- a/src/lib/collision_prevention/CMakeLists.txt +++ b/src/lib/collision_prevention/CMakeLists.txt @@ -31,7 +31,13 @@ # ############################################################################ -px4_add_library(CollisionPrevention CollisionPrevention.cpp) +px4_add_library(CollisionPrevention + CollisionPrevention.cpp + ObstacleMath.cpp +) target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable +target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(CollisionPrevention PRIVATE mathlib) px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention) +px4_add_unit_gtest(SRC ObstacleMathTest.cpp LINKLIBS CollisionPrevention) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b69..7e04629915 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -38,54 +38,23 @@ */ #include "CollisionPrevention.hpp" +#include "ObstacleMath.hpp" #include using namespace matrix; -using namespace time_literals; - -namespace -{ -static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly -static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG; - -static float wrap_360(float f) -{ - return wrap(f, 0.f, 360.f); -} - -static int wrap_bin(int i) -{ - i = i % INTERNAL_MAP_USED_BINS; - - while (i < 0) { - i += INTERNAL_MAP_USED_BINS; - } - - return i; -} - -} // namespace CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { - static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5"); - static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly"); + static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5"); + static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly"); // initialize internal obstacle map - _obstacle_map_body_frame.timestamp = getTime(); _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - _obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG; + _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; - _obstacle_map_body_frame.max_distance = 0; - _obstacle_map_body_frame.angle_offset = 0.f; - uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - uint64_t current_time = getTime(); - for (uint32_t i = 0 ; i < internal_bins; i++) { - _data_timestamps[i] = current_time; - _data_maxranges[i] = 0; - _data_fov[i] = 0; + for (uint32_t i = 0 ; i < BIN_COUNT; i++) { _obstacle_map_body_frame.distances[i] = UINT16_MAX; } } @@ -112,48 +81,234 @@ bool CollisionPrevention::is_active() return activated; } -void -CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude) +void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) { - int msg_index = 0; - float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); - float increment_factor = 1.f / obstacle.increment; + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = Quatf(vehicle_attitude.q); + _vehicle_yaw = Eulerf(_vehicle_attitude).psi(); + } + } + + //calculate movement constraints based on range data + const Vector2f original_setpoint = setpoint_accel; + _updateObstacleMap(); + _updateObstacleData(); + _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); + + // publish constraints + collision_constraints_s constraints{}; + original_setpoint.copyTo(constraints.original_setpoint); + setpoint_accel.copyTo(constraints.adapted_setpoint); + constraints.timestamp = getTime(); + _constraints_pub.publish(constraints); +} + +void CollisionPrevention::_updateObstacleMap() +{ + // add distance sensor data + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; + + if (dist_sens_sub.update(&distance_sensor)) { + // consider only instances with valid data and orientations useful for collision prevention + if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { + + // update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); + _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, + (uint16_t)(distance_sensor.max_distance * 100.0f)); + _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, + (uint16_t)(distance_sensor.min_distance * 100.0f)); + + _addDistanceSensorData(distance_sensor, _vehicle_attitude); + } + } + } + + // add obstacle distance data + if (_sub_obstacle_distance.update()) { + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); + + // Update map with obstacle data if the data is not stale + if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { + //update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); + _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, + obstacle_distance.max_distance); + _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, + obstacle_distance.min_distance); + _addObstacleSensorData(obstacle_distance, _vehicle_yaw); + } + } + + // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor + _obstacle_distance_fused_pub.publish(_obstacle_map_body_frame); +} + +void CollisionPrevention::_updateObstacleData() +{ + _obstacle_data_present = false; + _closest_dist = UINT16_MAX; + _closest_dist_dir.setZero(); + + for (int i = 0; i < BIN_COUNT; i++) { + // if the data is stale, reset the bin + if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE + + _obstacle_map_body_frame.angle_offset)); + const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + const uint16_t bin_distance = _obstacle_map_body_frame.distances[i]; + + // check if there is avaliable data and the data of the map is not stale + if (bin_distance < UINT16_MAX + && (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { + _obstacle_data_present = true; + } + + if (bin_distance * 0.01f < _closest_dist) { + _closest_dist = bin_distance * 0.01f; + _closest_dist_dir = bin_direction; + } + } +} + +void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) +{ + const float setpoint_length = setpoint_accel.norm(); + _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); + + const hrt_abstime now = getTime(); + + const bool is_stick_deflected = setpoint_length > 0.001f; + + if (_obstacle_data_present && is_stick_deflected) { + + _transformSetpoint(setpoint_accel); + + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, + vel_comp_accel, vel_comp_accel_dir); + + Vector2f constr_accel_setpoint{}; + + if (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); + } + + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; + + } else if (!_obstacle_data_present) { + // allow no movement + setpoint_accel.setZero(); + + // if distance data is stale, switch to Loiter + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US && + getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + } + + PX4_WARN("No obstacle data, not moving..."); + _last_timeout_warning = now; + } + } +} + +// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed... +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) +{ + + float vehicle_orientation_deg = math::degrees(vehicle_yaw); + if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); + for (int i = 0; i < BIN_COUNT; i++) { + for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) { + float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, + obstacle.angle_offset - vehicle_orientation_deg); + float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, + obstacle.angle_offset - vehicle_orientation_deg); - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; + } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } } else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) { // Obstacle message arrives in body frame (front aligned) // corresponding data index (shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + - _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); + for (int i = 0; i < BIN_COUNT; i++) { + for (int j = 0; j < 360 / obstacle.increment; j++) { + float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset); + float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset); - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; + } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } @@ -197,89 +352,64 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_ return false; } -void -CollisionPrevention::_updateObstacleMap() +bool +CollisionPrevention::_checkSetpointDirectionFeasability() { - _sub_vehicle_attitude.update(); + bool setpoint_feasible = true; - // add distance sensor data - for (auto &dist_sens_sub : _distance_sensor_subs) { - distance_sensor_s distance_sensor; + for (int i = 0; i < BIN_COUNT; i++) { + // check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data + if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get() + || (_param_cp_go_no_data.get() && _data_fov[i]))) { + setpoint_feasible = false; - if (dist_sens_sub.update(&distance_sensor)) { - // consider only instances with valid data and orientations useful for collision prevention - if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { - - // update message description - _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); - _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, - (uint16_t)(distance_sensor.max_distance * 100.0f)); - _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, - (uint16_t)(distance_sensor.min_distance * 100.0f)); - - _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); - } } } - // add obstacle distance data - if (_sub_obstacle_distance.update()) { - const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); - - // Update map with obstacle data if the data is not stale - if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { - //update message description - _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); - _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, - obstacle_distance.max_distance); - _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, - obstacle_distance.min_distance); - _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); - } - } - - // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor - _obstacle_distance_pub.publish(_obstacle_map_body_frame); + return setpoint_feasible; } void -CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude) +CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) +{ + const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw; + const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - + _obstacle_map_body_frame.angle_offset); + _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); + // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps + _setpoint_dir = setpoint.unit_or_zero(); + _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); +} + +void +CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude) { // clamp at maximum sensor range float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance); - // discard values below min range - if ((distance_reading > distance_sensor.min_distance)) { + // negative values indicate out of range but valid measurements. + if (fabsf(distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) { + distance_reading = distance_sensor.max_distance; + } - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); + // discard values below min range + if (distance_reading > distance_sensor.min_distance) { + float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast + (distance_sensor.orientation), distance_sensor.q); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); - int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); - - // floor values above zero, ceil values below zero - if (lower_bound < 0) { lower_bound++; } - - if (upper_bound < 0) { upper_bound++; } - - // rotate vehicle attitude into the sensor body frame - matrix::Quatf attitude_sensor_frame = vehicle_attitude; - attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); - float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); + 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); if (distance_reading < distance_sensor.max_distance) { - distance_reading = distance_reading * sensor_dist_scale; + ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude); } uint16_t sensor_range = static_cast(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 = ObstacleMath::wrap_bin(bin, BIN_COUNT); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); @@ -294,8 +424,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { - const float col_prev_d = _param_cp_dist.get(); - const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); + const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE); const int sp_index_original = setpoint_index; float best_cost = 9999.f; int new_sp_index = setpoint_index; @@ -307,19 +436,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 = ObstacleMath::wrap_bin(j, BIN_COUNT); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { - mean_dist += col_prev_d * 100.f; + mean_dist += _param_cp_dist.get() * 100.f; } else { mean_dist += _obstacle_map_body_frame.distances[bin]; } } - const int bin = wrap_bin(i); + const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT); mean_dist = mean_dist / (2.f * filter_size + 1.f); - const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original); + const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original); const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin]; if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) { @@ -330,226 +459,131 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi //only change setpoint direction if it was moved to a different bin if (new_sp_index != setpoint_index) { - float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + float angle = math::radians((float)new_sp_index * BIN_SIZE + _obstacle_map_body_frame.angle_offset); angle = wrap_2pi(vehicle_yaw_angle_rad + angle); setpoint_dir = {cosf(angle), sinf(angle)}; setpoint_index = new_sp_index; } } -float -CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const +float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { - float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f; + float obstacle_distance = 0.f; + const float direction_norm = direction.norm(); - switch (distance_sensor.orientation) { - case distance_sensor_s::ROTATION_YAW_0: - offset = 0.0f; - break; + 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 = + ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); - case distance_sensor_s::ROTATION_YAW_45: - offset = M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_90: - offset = M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_YAW_135: - offset = 3.0f * M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_180: - offset = M_PI_F; - break; - - case distance_sensor_s::ROTATION_YAW_225: - offset = -3.0f * M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_270: - offset = -M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_YAW_315: - offset = -M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_CUSTOM: - offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi(); - break; + const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg); + obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; } - return offset; + return obstacle_distance; } -void -CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, - const Vector2f &curr_vel) +Vector2f +CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length) { - _updateObstacleMap(); + Vector2f new_setpoint{}; + const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir)); + const Vector2f tangential_component = _setpoint_dir - normal_component; - // read parameters - const float col_prev_d = _param_cp_dist.get(); - const float col_prev_dly = _param_cp_delay.get(); - const bool move_no_data = _param_cp_go_nodata.get(); - const float xy_p = _param_mpc_xy_p.get(); - const float max_jerk = _param_mpc_jerk_max.get(); - const float max_accel = _param_mpc_acc_hor.get(); - const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); + const float normal_scale = _getScale(_closest_dist); - const float setpoint_length = setpoint.norm(); - const hrt_abstime constrain_time = getTime(); - int num_fov_bins = 0; + const float closest_dist_tangential = _getObstacleDistance(tangential_component); + const float tangential_scale = _getScale(closest_dist_tangential); - if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { - if (setpoint_length > 0.001f) { - Vector2f setpoint_dir = setpoint / setpoint_length; - float vel_max = setpoint_length; - const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d); - - const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - - _obstacle_map_body_frame.angle_offset); - int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); - - // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps - _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); - - // limit speed for safe flight - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message - - // delete stale values - const hrt_abstime data_age = constrain_time - _data_timestamps[i]; - - if (data_age > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } - - const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters - const float max_range = _data_maxranges[i] * 0.01f; // convert to meters - float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); - - // convert from body to local frame in the range [0, 2*pi] - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); - - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; - - //count number of bins in the field of valid_new - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } - - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { - - if (setpoint_dir.dot(bin_direction) > 0) { - // calculate max allowed velocity with a P-controller (same gain as in the position controller) - const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - float delay_distance = curr_vel_parallel * col_prev_dly; - - if (distance < max_range) { - delay_distance += curr_vel_parallel * (data_age * 1e-6f); - } - - const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); - const float vel_max_posctrl = xy_p * stop_distance; - - const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); - const float projection = bin_direction.dot(setpoint_dir); - float vel_max_bin = vel_max; - - if (projection > 0.01f) { - vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection; - } - - // constrain the velocity - if (vel_max_bin >= 0) { - vel_max = math::min(vel_max, vel_max_bin); - } - } - - } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { - if (!move_no_data || (move_no_data && _data_fov[i])) { - vel_max = 0.f; - } - } - } - - //if the sensor field of view is zero, never allow to move (even if move_no_data=1) - if (num_fov_bins == 0) { - vel_max = 0.f; - } - - setpoint = setpoint_dir * vel_max; - } + // only scale accelerations towards the obstacle + if (_closest_dist_dir.dot(_setpoint_dir) > 0) { + new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length; } else { - //allow no movement - float vel_max = 0.f; - setpoint = setpoint * vel_max; - - // if distance data is stale, switch to Loiter - if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { - - if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US - && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { - _publishVehicleCmdDoLoiter(); - } - - _last_timeout_warning = getTime(); - } - - + new_setpoint = _setpoint_dir * setpoint_length; } + + return new_setpoint; } -void -CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos, - const Vector2f &curr_vel) +float +CollisionPrevention::_getScale(const float &reference_distance) { - //calculate movement constraints based on range data - Vector2f new_setpoint = original_setpoint; - _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); + 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()); - //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); + // 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; +} - _interfering = currently_interfering; +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; - // 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); + // 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)); - original_setpoint = new_setpoint; + const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) }; + float bin_distance = _obstacle_map_body_frame.distances[i]; + + // only consider bins which are between min and max values + if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) { + const float distance = bin_distance * 0.01f; + + // Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high + // acceleration maneuvers + const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction)); + float delay_distance = curr_vel_parallel * _param_cp_delay.get(); + + const hrt_abstime data_age = now - _data_timestamps[i]; + + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); + } + + const float stop_distance = distance - _min_dist_to_keep - delay_distance; + + float curr_acc_vel_constraint; + + if (stop_distance >= 0.f) { + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f); + + } else { + curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel; + } + + if (curr_acc_vel_constraint < vel_comp_accel) { + vel_comp_accel = curr_acc_vel_constraint; + vel_comp_accel_dir = bin_direction; + } + } + } } void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; - command.timestamp = getTime(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode + command.param1 = 1.f; // base mode VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; command.target_system = 1; command.target_component = 1; command.source_system = 1; command.source_component = 1; command.confirmation = false; command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command + command.timestamp = getTime(); _vehicle_command_pub.publish(command); } diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index aaff04e2cb..76bfcb5ea7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -34,6 +34,7 @@ /** * @file CollisionPrevention.hpp * @author Tanja Baumann + * @author Claudio Chies * * CollisionPrevention controller. * @@ -74,21 +75,29 @@ public: /** * Computes collision free setpoints - * @param original_setpoint, setpoint before collision prevention intervention - * @param max_speed, maximum xy speed - * @param curr_pos, current vehicle position - * @param curr_vel, current vehicle velocity + * @param setpoint_accel setpoint purely based on sticks, to be modified + * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed */ - void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, - const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); + void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + + static constexpr int BIN_COUNT = + sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); // 72 + static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly protected: + /** Aggregates the sensor data into an internal obstacle map in body frame */ + void _updateObstacleMap(); - obstacle_distance_s _obstacle_map_body_frame {}; - bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( - _obstacle_map_body_frame.distances[0])]; /**< in cm */ + /** Updates the obstacle data based on stale data and calculates values from the map */ + void _updateObstacleData(); + + /** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */ + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + + obstacle_distance_s _obstacle_map_body_frame{}; + bool _data_fov[BIN_COUNT] {}; + uint64_t _data_timestamps[BIN_COUNT] {}; + uint16_t _data_maxranges[BIN_COUNT] {}; /**< in cm */ void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); @@ -96,7 +105,7 @@ protected: * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -106,6 +115,34 @@ protected: */ void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + /** + * Constrain the acceleration setpoint based on the distance to the obstacle + * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above + * +1 ________ _ _ + * ┌─┐ │ // + * │X│ │ // + * │X│ │ // + * │X│ │ /// + * │X│ │ // + * │X│ │///// + * │X│──────┼─────────────┬───────────── + * │X│ /│ scale_distance + * │X│ / │ + * │X│ / │ + * │X│ / │ + * │X│ / │ + * └─┘/ │ + * -1 + */ + matrix::Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); + + void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, matrix::Vector2f &vel_comp_accel_dir); + + float _getObstacleDistance(const matrix::Vector2f &direction); + + float _getScale(const float &reference_distance); + /** * Determines whether a new sensor measurement is used * @param map_index, index of the bin in the internal map the measurement belongs in @@ -114,25 +151,39 @@ protected: */ bool _enterData(int map_index, float sensor_range, float sensor_reading); + bool _checkSetpointDirectionFeasability(); + + void _transformSetpoint(const matrix::Vector2f &setpoint); + //Timing functions. Necessary to mock time in the tests virtual hrt_abstime getTime(); virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr); - private: - - bool _interfering{false}; /**< states if the collision prevention interferes with the user input */ + bool _data_stale{true}; /**< states if the data is stale */ bool _was_active{false}; /**< states if the collision prevention interferes with the user input */ + bool _obstacle_data_present{false}; /**< states if obstacle data is present */ + + int _setpoint_index{}; /**< index of the setpoint*/ + matrix::Vector2f _setpoint_dir{}; /**< direction of the setpoint*/ + + float _closest_dist{}; /**< closest distance to an obstacle */ + matrix::Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */ + + float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + matrix::Quatf _vehicle_attitude{}; + float _vehicle_yaw{0.f}; + uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ + uORB::Publication _obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ - uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; @@ -142,22 +193,17 @@ private: hrt_abstime _time_activated{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ - (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ - (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ - (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ - (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ + (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ + (ParamBool) _param_cp_go_no_data, /**< movement allowed where no data*/ + (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ + (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ + (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_mpc_xy_vel_p_acc, /**< p gain from velocity controller*/ + (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) - /** - * Transforms the sensor orientation into a yaw in the local frame - * @param distance_sensor, distance sensor message - * @param angle_offset, sensor body frame offset - */ - float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; - /** * Computes collision free setpoints * @param setpoint, setpoint before collision prevention intervention @@ -180,11 +226,6 @@ private: */ void _publishObstacleDistance(obstacle_distance_s &obstacle); - /** - * Aggregates the sensor data into a internal obstacle map in body frame - */ - void _updateObstacleMap(); - /** * Publishes vehicle command. */ diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa..aa98ef821d 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -34,8 +34,12 @@ #include #include "CollisionPrevention.hpp" +using namespace matrix; + // to run: make tests TESTFILTER=CollisionPrevention hrt_abstime mocked_time = 0; +const uint bin_size = CollisionPrevention::BIN_SIZE; +const uint bin_count = CollisionPrevention::BIN_COUNT; class CollisionPreventionTest : public ::testing::Test { @@ -53,15 +57,15 @@ public: TestCollisionPrevention() : CollisionPrevention(nullptr) {} void paramsChanged() {CollisionPrevention::updateParamsImpl();} obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;} - void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude) + void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &attitude) { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - _addObstacleSensorData(obstacle, attitude); + _addObstacleSensorData(obstacle, vehicle_yaw); } - void test_adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, + void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { _adaptSetpointDirection(setpoint_dir, setpoint_index, vehicle_yaw_angle_rad); @@ -103,10 +107,8 @@ TEST_F(CollisionPreventionTest, noSensorData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3.f; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -116,8 +118,8 @@ TEST_F(CollisionPreventionTest, noSensorData) param_set(param, &value); cp.paramsChanged(); - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: collision prevention should be enabled and limit the speed to zero EXPECT_TRUE(cp.is_active()); @@ -128,11 +130,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -141,9 +141,12 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) attitude.q[3] = 0.0f; // AND: a parameter handle - param_t param = param_handle(px4::params::CP_DIST); - float value = 10; // try to keep 10m distance - param_set(param, &value); + param_t param1 = param_handle(px4::params::CP_DIST); + float value1 = 10; // try to keep 10m distance + param_set(param1, &value1); + param_t param2 = param_handle(px4::params::CP_GUIDE_ANG); + float value2 = 0; // dont guide sideways + param_set(param2, &value2); cp.paramsChanged(); // AND: an obstacle message @@ -164,7 +167,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) } else { message.distances[i] = 10001; } - } // WHEN: we publish the message and set the parameter and then run the setpoint modification @@ -172,32 +174,31 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; - cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); - cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; + cp.modifySetpoint(modified_setpoint1, curr_vel); + cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(obstacle_distance_pub); orb_unadvertise(vehicle_attitude_pub); // THEN: the internal map should know the obstacle - // case 1: the velocity setpoint should be cut down to zero - // case 2: the velocity setpoint should stay the same as the input + // case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low + // case 2: the acceleration setpoint should be lower EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); - - EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); - EXPECT_FLOAT_EQ(original_setpoint2.norm(), modified_setpoint2.norm()); + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); + EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1); } TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -232,21 +233,26 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); //WHEN: We run the setpoint modification - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; - cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); - cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; + cp.modifySetpoint(modified_setpoint1, curr_vel); + cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(distance_sensor_pub); orb_unadvertise(vehicle_attitude_pub); // THEN: the internal map should know the obstacle - // case 1: the velocity setpoint should be cut down to zero because there is an obstacle - // case 2: the velocity setpoint should be cut down to zero because there is no data + // case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low + // case 2: the acceleration setpoint should be lower EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); - EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); - EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1); + EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); + EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); + EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1); } TEST_F(CollisionPreventionTest, testPurgeOldData) @@ -255,10 +261,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -268,7 +272,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 10; // try to keep 10m distance + float value = 1; // try to keep 10m distance param_set(param, &value); cp.paramsChanged(); @@ -306,9 +310,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); mocked_time = mocked_time + 100000; //advance time by 0.1 seconds message_lost_data.timestamp = mocked_time; @@ -344,10 +347,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -384,9 +385,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); //advance time by 0.1 seconds but no new message comes in mocked_time = mocked_time + 100000; @@ -410,14 +410,12 @@ TEST_F(CollisionPreventionTest, noBias) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 5; // try to keep 5m distance + float value = 2; // try to keep 2m distance param_set(param, &value); cp.paramsChanged(); @@ -438,8 +436,8 @@ TEST_F(CollisionPreventionTest, noBias) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); orb_unadvertise(obstacle_distance_pub); // THEN: setpoint should go into the same direction as the stick input @@ -451,9 +449,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -486,22 +482,21 @@ TEST_F(CollisionPreventionTest, outsideFOV) orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); for (int i = 0; i < distances_array_size; i++) { - float angle_deg = (float)i * message.increment; float angle_rad = math::radians(angle_deg); - matrix::Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; + Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); //THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV float setpoint_length = modified_setpoint.norm(); if (setpoint_length > 0.f) { - matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length; + Vector2f setpoint_dir = modified_setpoint / setpoint_length; float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)); - float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame)); + float sp_angle_deg = math::degrees(wrap_2pi(sp_angle_body_frame)); EXPECT_GE(sp_angle_deg, 45.f); EXPECT_LE(sp_angle_deg, 225.f); } @@ -514,9 +509,7 @@ TEST_F(CollisionPreventionTest, goNoData) { // GIVEN: a simple setup condition with the initial state (no distance data) TestCollisionPrevention cp; - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: an obstacle message obstacle_distance_s message; @@ -532,7 +525,7 @@ TEST_F(CollisionPreventionTest, goNoData) float angle = i * message.increment; if (angle > 0.f && angle < 40.f) { - message.distances[i] = 700; + message.distances[i] = 1000; } else { message.distances[i] = UINT16_MAX; @@ -541,16 +534,16 @@ TEST_F(CollisionPreventionTest, goNoData) // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 5; // try to keep 5m distance + float value = 2; // try to keep 5m distance param_set(param, &value); cp.paramsChanged(); // AND: a setpoint outside the field of view - matrix::Vector2f original_setpoint = {-5, 0}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {-5, 0}; + Vector2f modified_setpoint = original_setpoint; - //THEN: the modified setpoint should be zero velocity - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + //THEN: the modified setpoint should be zero acceleration + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); //WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV @@ -561,7 +554,7 @@ TEST_F(CollisionPreventionTest, goNoData) //THEN: When all bins contain UINT_16MAX the setpoint should be zero even if CP_GO_NO_DATA=1 modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); //THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed @@ -570,7 +563,7 @@ TEST_F(CollisionPreventionTest, goNoData) orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm()); orb_unadvertise(obstacle_distance_pub); } @@ -579,10 +572,8 @@ TEST_F(CollisionPreventionTest, jerkLimit) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: distance set to 5m param_t param = param_handle(px4::params::CP_DIST); @@ -607,8 +598,8 @@ TEST_F(CollisionPreventionTest, jerkLimit) // AND: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; - cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint_default_jerk = original_setpoint; + cp.modifySetpoint(modified_setpoint_default_jerk, curr_vel); orb_unadvertise(obstacle_distance_pub); // AND: we now set max jerk to 0.1 @@ -618,19 +609,101 @@ TEST_F(CollisionPreventionTest, jerkLimit) cp.paramsChanged(); // WHEN: we run the setpoint modification again - matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint; - cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint_limited_jerk = original_setpoint; + cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel); - // THEN: the new setpoint should be much slower than the one with default jerk - EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm()); + // THEN: the new setpoint should be much higher than the one with default jerk, as the rate of change in acceleration is more limmited + EXPECT_GT(modified_setpoint_limited_jerk.norm(), modified_setpoint_default_jerk.norm()); + +} +TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + // Distance is out of Range + distance_sensor.current_distance = -1.f; + distance_sensor.signal_quality = 0; + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], distance_sensor.max_distance * 100.f); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + } +} + +TEST_F(CollisionPreventionTest, addDistanceSensorDataNarrow) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(0.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } +} +TEST_F(CollisionPreventionTest, addDistanceSensorDataSlightlyLarger) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(1.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the the bins corresponding to -5°, 0° and 5° should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 71 || i <= 1) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } } TEST_F(CollisionPreventionTest, addDistanceSensorData) { // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; distance_sensor.max_distance = 20.f; @@ -640,21 +713,24 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (uint32_t i = 0; i < distances_array_size; i++) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //WHEN: we add distance sensor data to the right distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING; distance_sensor.h_fov = math::radians(19.99f); cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + uint fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start = (90 - fov) / bin_size; + uint end = (90 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -663,17 +739,20 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(50.f); distance_sensor.current_distance = 8.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start2 = (270 - fov) / bin_size; + uint end2 = (270 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -682,20 +761,23 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(10.1f); distance_sensor.current_distance = 3.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start3 = (360 - fov) / bin_size; + uint end3 = (fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; - } else if (i == 0) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300); + } else if (i >= start3 || i <= end3) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -706,7 +788,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 5.f; @@ -714,22 +795,18 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 2; i < 6 ; i++) { + int start = 2; + int end = 6; + + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -738,11 +815,66 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + + //WHEN: we add obstacle distance sensor data while vehicle yaw 90deg to the right + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); + + //THEN: the correct bins in the map should be filled + int offset = bin_count - 90 / bin_size; + + for (int i = 0; i < distances_array_size; i++) { + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add obstacle distance sensor data while vehicle yaw 45deg to the left + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); + + //THEN: the correct bins in the map should be filled + offset = 45 / bin_size; + + for (int i = 0; i < distances_array_size; i++) { + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add obstacle distance sensor data while vehicle yaw 180deg + cp.test_addObstacleSensorData(obstacle_msg, M_PI); + + //THEN: the correct bins in the map should be filled + offset = 180 / bin_size; + + for (int i = 0; i < distances_array_size; i++) { + if (i >= offset + start && i <= offset + end) { EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); } else { @@ -752,50 +884,75 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } +} +TEST_F(CollisionPreventionTest, addObstacleSensorData_offset_bodyframe) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; // Body Frame + obstacle_msg.increment = 6.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; - //WHEN: we add distance sensor data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + //obstacle at 363°-39° deg world frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment + obstacle_msg.distances[i] = 500; + } + + //WHEN: we add distance sensor data + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); - //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 28 || i == 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° for (int i = 0; i < distances_array_size; i++) { - if (i == 6 || i == 7) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° - //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 19 || i == 20) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -807,7 +964,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned obstacle_msg.increment = 5.f; @@ -815,18 +971,12 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + int start = 2; + int end = 6; - for (int i = 2; i < 6 ; i++) { + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } @@ -839,15 +989,16 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -855,15 +1006,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -871,15 +1022,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -887,20 +1038,21 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } + } @@ -908,7 +1060,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 6.f; @@ -916,44 +1067,62 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - - //obstacle at 0-30 deg world frame, distance 5 meters + //obstacle at 363°-39° deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 5 ; i++) { + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment obstacle_msg.distances[i] = 500; } //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (int i = 0; i < distances_array_size; i++) { - if (i >= 0 && i <= 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data with an angle offset - obstacle_msg.angle_offset = 30.f; - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° for (int i = 0; i < distances_array_size; i++) { - if (i >= 3 && i <= 5) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -965,31 +1134,29 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 7 ; i++) { + for (int i = 0; i <= 6 ; i++) { obstacle_msg.distances[i] = 500; } obstacle_msg.distances[2] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + Vector2f setpoint_dir(1, 0); + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -999,29 +1166,27 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1035,10 +1200,10 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.distances[3] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + Vector2f setpoint_dir(1, 0); + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -1048,23 +1213,21 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262f); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012f); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, overlappingSensors) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -1114,8 +1277,8 @@ TEST_F(CollisionPreventionTest, overlappingSensors) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the long range measurement EXPECT_EQ(500, cp.getObstacleMap().distances[2]); @@ -1124,10 +1287,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // WHEN: we publish the short range message followed by a long range message short_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the short range measurement EXPECT_EQ(150, cp.getObstacleMap().distances[2]); @@ -1136,10 +1299,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // WHEN: we publish the short range message with values out of range followed by a long range message short_range_msg_no_obstacle.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the short range measurement EXPECT_EQ(500, cp.getObstacleMap().distances[2]); @@ -1152,16 +1315,15 @@ TEST_F(CollisionPreventionTest, enterData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with a valid reading distance_sensor_s distance_sensor {}; @@ -1173,36 +1335,38 @@ TEST_F(CollisionPreventionTest, enterData) cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(500, cp.getObstacleMap().distances[8]); - EXPECT_EQ(500, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(500, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain valid readings // a valid reading should only be accepted from sensors with shorter or equal range // a out of range reading should only be accepted from sensors with the same range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with an out of range reading distance_sensor.current_distance = 21.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(2000, cp.getObstacleMap().distances[8]); - EXPECT_EQ(2000, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(2000, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain readings out of range // a reading in range will be accepted in any case // out of range readings will only be accepted from sensors with bigger or equal range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range } diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp new file mode 100644 index 0000000000..e75f19d7b7 --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "ObstacleMath.hpp" +#include + +using namespace matrix; + +namespace ObstacleMath +{ + +void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle) +{ + const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f))); + const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor; + const Vector3f forward(1.f, 0.f, 0.f); + const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward); + + float horizontal_projection_scale = sensor_direction_in_world.xy().norm(); + horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f); + distance *= horizontal_projection_scale; +} + +int get_bin_at_angle(float bin_width, float angle) +{ + int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width); + return wrap_bin(bin_at_angle, 360 / bin_width); +} + +float get_lower_bound_angle(int bin, float bin_width, float angle_offset) +{ + bin = wrap_bin(bin, 360 / bin_width); + return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f); +} + +int get_offset_bin_index(int bin, float bin_width, float angle_offset) +{ + int offset = get_bin_at_angle(bin_width, angle_offset); + return wrap_bin(bin - offset, 360 / bin_width); +} + +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4]) +{ + float offset = 0.0f; + + switch (orientation) { + case SensorOrientation::ROTATION_YAW_0: + offset = 0.0f; + break; + + case SensorOrientation::ROTATION_YAW_45: + offset = M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_YAW_90: + offset = M_PI_F / 2.0f; + break; + + case SensorOrientation::ROTATION_YAW_135: + offset = 3.0f * M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_YAW_180: + offset = M_PI_F; + break; + + case SensorOrientation::ROTATION_YAW_225: + offset = -3.0f * M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_YAW_270: + offset = -M_PI_F / 2.0f; + break; + + case SensorOrientation::ROTATION_YAW_315: + offset = -M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_CUSTOM: + if (q != nullptr) { + offset = Eulerf(Quatf(q)).psi(); + } + + break; + } + + return offset; +} + +int wrap_bin(int bin, int bin_count) +{ + return (bin + bin_count) % bin_count; +} + +float wrap_360(const float angle) +{ + return matrix::wrap(angle, 0.0f, 360.0f); +} + +} // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp new file mode 100644 index 0000000000..8a1db0217a --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +namespace ObstacleMath +{ + +enum SensorOrientation { + ROTATION_YAW_0 = 0, // MAV_SENSOR_ROTATION_NONE + ROTATION_YAW_45 = 1, // MAV_SENSOR_ROTATION_YAW_45 + ROTATION_YAW_90 = 2, // MAV_SENSOR_ROTATION_YAW_90 + ROTATION_YAW_135 = 3, // MAV_SENSOR_ROTATION_YAW_135 + ROTATION_YAW_180 = 4, // MAV_SENSOR_ROTATION_YAW_180 + ROTATION_YAW_225 = 5, // MAV_SENSOR_ROTATION_YAW_225 + ROTATION_YAW_270 = 6, // MAV_SENSOR_ROTATION_YAW_270 + ROTATION_YAW_315 = 7, // MAV_SENSOR_ROTATION_YAW_315 + ROTATION_CUSTOM = 100, // MAV_SENSOR_ROTATION_CUSTOM + + ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE + ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 + ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180 + ROTATION_LEFT_FACING = 6 // MAV_SENSOR_ROTATION_YAW_270 +}; + +/** + * Converts a sensor orientation to a yaw offset + * @param orientation sensor orientation + */ +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr); + +/** + * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane + * @param distance measurement which is scaled down + * @param yaw orientation of the measurement on the body horizontal plane + * @param q_world_vehicle vehicle attitude quaternion + */ +void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle); + +/** + * Returns bin index at a given angle from the 0th bin + * @param bin_width width of a bin in degrees + * @param angle clockwise angle from start bin in degrees + */ +int get_bin_at_angle(float bin_width, float angle); + +/** + * Returns lower bound angle of a bin + * @param bin bin index + * @param bin_width width of a bin in degrees + * @param angle_offset clockwise angle offset in degrees + */ +float get_lower_bound_angle(int bin, float bin_width, float angle_offset); + +/** + * Returns bin index for the current bin after an angle offset + * @param bin current bin index + * @param bin_width width of a bin in degrees + * @param angle_offset clockwise angle offset in degrees + */ +int get_offset_bin_index(int bin, float bin_width, float angle_offset); + +/** + * Wraps a bin index to the range [0, bin_count) + * @param bin bin index + * @param bin_count number of bins + */ +int wrap_bin(int bin, int bin_count); + +/** + * Wraps an angle to the range [0, 360) + * @param angle angle in degrees + */ +float wrap_360(const float angle); + + +} // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp new file mode 100644 index 0000000000..9f6b00ae21 --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (C) 2025 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 +#include +#include +#include "ObstacleMath.hpp" + +using namespace matrix; + +TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane) +{ + // standard vehicle orientation inputs + Quatf vehicle_pitch_up_45(Eulerf(0.0f, M_PI_4_F, 0.0f)); + Quatf vehicle_roll_right_45(Eulerf(M_PI_4_F, 0.0f, 0.0f)); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + float distance = 1.0f; + float sensor_orientation = 0; // radians (forward facing) + + // WHEN: we project the distance onto the horizontal plane + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45); + + // THEN: the distance should be scaled correctly + float expected_scale = sqrtf(2) / 2; + float expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + distance = 1.0f; + + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45); + + // THEN: the distance should be scaled correctly + expected_scale = 1.f; + expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + distance = 1.0f; + sensor_orientation = M_PI_2_F; // radians (right facing) + + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45); + + // THEN: the distance should be scaled correctly + expected_scale = sqrtf(2) / 2; + expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + distance = 1.0f; + + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45); + + // THEN: the distance should be scaled correctly + expected_scale = 1.f; + expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); +} + +TEST(ObstacleMathTest, GetBinAtAngle) +{ + float bin_width = 5.0f; + + // GIVEN: a start bin, bin width, and angle + float angle = 0.0f; + + // WHEN: we calculate the bin index at the angle + uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 0); + + // GIVEN: a start bin, bin width, and angle + angle = 90.0f; + + // WHEN: we calculate the bin index at the angle + bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 18); + + // GIVEN: a start bin, bin width, and angle + angle = -90.0f; + + // WHEN: we calculate the bin index at the angle + bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 54); + + // GIVEN: a start bin, bin width, and angle + angle = 450.0f; + + // WHEN: we calculate the bin index at the angle + bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 18); +} + +TEST(ObstacleMathTest, GetLowerBound) +{ + // GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset + int bin = -1; + float bin_width = 7.5f; + float angle_offset = -4.3f; + + // WHEN: we calculate the lower bound angle of the bin + float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset); + + // THEN: the lower bound angle should be correct. The bin index is wrapped to the end and + // the angle offset is applied in the counter-clockwise direction. + EXPECT_FLOAT_EQ(lower_bound, 344.45); + +} + + +TEST(ObstacleMathTest, OffsetBinIndex) +{ + // In this test, we want to offset the bin index by a negative and positive angle. + // We take the output of the first offset and offset it by the same angle in the + // opposite direction to return back to the original bin index. + + // GIVEN: a bin index, bin width, and a negative angle offset + uint16_t bin = 0; + float bin_width = 5.0f; + float angle_offset = -120.0f; + + // WHEN: we offset the bin index by the negative angle + uint16_t new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset); + + // THEN: the new bin index should be correctly offset by the wrapped angle + EXPECT_EQ(new_bin_index, 24); + + // GIVEN: the output bin index of the previous offset, bin width, and the same angle + // offset in positive direction + bin = 24; + bin_width = 5.0f; + angle_offset = 120.0f; + + // WHEN: we offset the bin index by the positive angle + new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset); + + // THEN: the new bin index should return back to the original bin index + EXPECT_EQ(new_bin_index, 0); +} + + +TEST(ObstacleMathTest, WrapBin) +{ + // GIVEN: a bin index within bounds and the number of bins + int bin = 0; + int bin_count = 72; + + // WHEN: we wrap a bin index within the bounds + int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count); + + // THEN: the wrapped bin index should stay 0 + EXPECT_EQ(wrapped_bin, 0); + + // GIVEN: a bin index that is out of bounds, and the number of bins + bin = 73; + bin_count = 72; + + // WHEN: we wrap a bin index that is larger than the number of bins + wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count); + + // THEN: the wrapped bin index should be wrapped back to the beginning + EXPECT_EQ(wrapped_bin, 1); + + // GIVEN: a negative bin index and the number of bins + bin = -1; + bin_count = 72; + + // WHEN: we wrap a bin index that is negative + wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count); + + // THEN: the wrapped bin index should be wrapped back to the end + EXPECT_EQ(wrapped_bin, 71); +} + +TEST(ObstacleMathTest, HandleMissedBins) +{ + // In this test, the current and previous bin are adjacent to the bins that are outside + // the sensor field of view. The missed bins (0,1,6 & 7) should be populated, and no + // data should be filled in the bins outside the FOV. + + // GIVEN: measurements, current bin, previous bin, bin width, and field of view offset + float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0}; + int current_bin = 2; + int previous_bin = 5; + int bin_width = 45.0f; + float fov = 270.0f; + float fov_offset = 360.0f - fov / 2; + + float measurement = measurements[current_bin]; + + // WHEN: we handle missed bins + int current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, bin_width, fov_offset); + int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset); + + int start = math::min(current_bin_offset, previous_bin_offset) + 1; + int end = math::max(current_bin_offset, previous_bin_offset); + + EXPECT_EQ(start, 1); + EXPECT_EQ(end, 5); + + for (uint16_t i = start; i < end; i++) { + uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset); + measurements[bin_index] = measurement; + } + + // THEN: the correct missed bins should be populated with the measurement + EXPECT_EQ(measurements[0], 1); + EXPECT_EQ(measurements[1], 1); + EXPECT_EQ(measurements[2], 1); + EXPECT_EQ(measurements[3], 0); + EXPECT_EQ(measurements[4], 0); + EXPECT_EQ(measurements[5], 2); + EXPECT_EQ(measurements[6], 1); + EXPECT_EQ(measurements[7], 1); +} diff --git a/src/lib/component_information/generate_component_general.py b/src/lib/component_information/generate_component_general.py index 35445f1b88..3561cae46d 100755 --- a/src/lib/component_information/generate_component_general.py +++ b/src/lib/component_information/generate_component_general.py @@ -20,7 +20,7 @@ version_file = args.version_file version_dir = '' if version_file is not None: for line in open(version_file, "r"): - version_search = re.search('PX4_GIT_TAG_OR_BRANCH_NAME\s+"(.+)"', line) + version_search = re.search(r'PX4_GIT_TAG_OR_BRANCH_NAME\s+"(.+)"', line) if version_search: version_dir = version_search.group(1) break diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/lib/control_allocation/CMakeLists.txt similarity index 87% rename from src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt rename to src/lib/control_allocation/CMakeLists.txt index 81f350d484..f7c710b7f2 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/lib/control_allocation/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,8 +31,5 @@ # ############################################################################ -px4_add_library(RoverDifferentialGuidance - RoverDifferentialGuidance.cpp -) - -target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(control_allocation) +add_subdirectory(actuator_effectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp similarity index 83% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 901c469746..b36da8230f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectiveness.hpp" -#include "../ControlAllocation/ControlAllocation.hpp" #include @@ -51,12 +50,12 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m return -1; } - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2); matrix_selection_indexes[totalNumActuators()] = selected_matrix; ++num_actuators[(int)type]; return num_actuators_matrix[selected_matrix]++; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt similarity index 80% rename from src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt rename to src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt index 8d4489fa3e..b8a3d0076d 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt +++ b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,9 +31,15 @@ # ############################################################################ -px4_add_library(FlightTaskManualPositionSmoothVel - FlightTaskManualPositionSmoothVel.cpp +px4_add_library(ActuatorEffectiveness + ActuatorEffectiveness.cpp + ActuatorEffectiveness.hpp ) -target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility) -target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ActuatorEffectiveness + PRIVATE + mathlib + PID +) diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt similarity index 100% rename from src/modules/control_allocator/ControlAllocation/CMakeLists.txt rename to src/lib/control_allocation/control_allocation/CMakeLists.txt diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.hpp index c60784a03c..a6e2e2b254 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -71,7 +71,7 @@ #include -#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ControlAllocation { diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp similarity index 92% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp index 5d86814d7e..0dc512b4ed 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp @@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix( update_normalization_scale); _mix_update_needed = true; _normalization_needs_update = update_normalization_scale; + + if (_metric_allocation && update_normalization_scale) { + // adding #include + PX4_WARN leads to failed linking on test + _normalization_needs_update = false; + } } void @@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse() if (_mix_update_needed) { matrix::geninv(_effectiveness, _mix); - if (_normalization_needs_update && !_had_actuator_failure) { - updateControlAllocationMatrixScale(); - _normalization_needs_update = false; + if (!_metric_allocation) { + if (_normalization_needs_update && !_had_actuator_failure) { + updateControlAllocationMatrixScale(); + _normalization_needs_update = false; + } + + normalizeControlAllocationMatrix(); } - normalizeControlAllocationMatrix(); _mix_update_needed = false; } } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp similarity index 95% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp index 27c367376b..d4527cc1f4 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp @@ -57,11 +57,13 @@ public: void setEffectivenessMatrix(const matrix::Matrix &effectiveness, const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, bool update_normalization_scale) override; + void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; } protected: matrix::Matrix _mix; bool _mix_update_needed{false}; + bool _metric_allocation{false}; /** * Recalculate pseudo inverse if required. diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp similarity index 76% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp index 89faab8c92..0eafbc6c26 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp @@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase) EXPECT_EQ(actuator_sp, actuator_sp_expected); EXPECT_EQ(control_allocated, control_allocated_expected); } + +TEST(ControlAllocationMetricTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + matrix::Vector actuator_sp_expected; + + method.setMetricAllocation(true); + method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false); + method.setControlSetpoint(control_sp); + method.allocate(); + actuator_sp = method.getActuatorSetpoint(); + control_allocated_expected = method.getAllocatedControl(); + + EXPECT_EQ(actuator_sp, actuator_sp_expected); + EXPECT_EQ(control_allocated, control_allocated_expected); +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp index 83be2a3b33..4b28f44dac 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp @@ -219,7 +219,7 @@ ControlAllocationSequentialDesaturation::mixYaw() // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), // and allow some yaw response at maximum thrust ActuatorVector max_prev = _actuator_max; - _actuator_max += (_actuator_max - _actuator_min) * 0.15f; + _actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN; desaturateActuators(_actuator_sp, yaw); _actuator_max = max_prev; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp similarity index 95% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp index 53c422cd39..dfa69d9777 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp @@ -57,6 +57,10 @@ public: void allocate() override; void updateParameters() override; + + // This is the minimum actuator yaw granted when the controller is saturated. + // In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount. + static constexpr float MINIMUM_YAW_MARGIN{0.15f}; private: /** diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp similarity index 92% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 60392330c9..cd478a1b00 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,17 +40,27 @@ #include #include -#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp> using namespace matrix; namespace { +struct RotorGeometryTest { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + float moment_ratio; +}; + +struct GeometryTest { + RotorGeometryTest rotors[ActuatorEffectiveness::NUM_ACTUATORS]; + int num_rotors{0}; +}; // Makes and returns a Geometry object for a "standard" quad-x quadcopter. -ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() +GeometryTest make_quad_x_geometry() { - ActuatorEffectivenessRotors::Geometry geometry = {}; + GeometryTest geometry = {}; geometry.rotors[0].position(0) = 1.0f; geometry.rotors[0].position(1) = 1.0f; geometry.rotors[0].position(2) = 0.0f; @@ -88,7 +98,6 @@ ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() geometry.rotors[3].moment_ratio = -0.05f; geometry.num_rotors = 4; - return geometry; } @@ -98,7 +107,48 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() ActuatorEffectiveness::EffectivenessMatrix effectiveness; effectiveness.setZero(); const auto geometry = make_quad_x_geometry(); - ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + // Minimalistically copied from ActuatorEffectivenessRotors::computeEffectivenessMatrix + for (int i = 0; i < geometry.num_rotors; i++) { + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Fill corresponding items in effectiveness matrix + for (int j = 0; j < 3; j++) { + effectiveness(j, i) = moment(j); + effectiveness(j + 3, i) = thrust(j); + } + } + return effectiveness; } diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 19f9967880..3d00bf7db2 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -56,3 +56,31 @@ get_rot_quaternion(enum Rotation rot) math::radians((float)rot_lookup[rot].pitch), math::radians((float)rot_lookup[rot].yaw)}}; } + +__EXPORT void +rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; + x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); + y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); + z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); + } + } +} + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; + x = r(0); + y = r(1); + z = r(2); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index b33e4a8383..350404723e 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -157,8 +157,18 @@ __EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot); */ __EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot); +/** + * rotate a 3 element int16_t vector in-place + */ +__EXPORT void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z); + template -static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) +static bool rotate_3(enum Rotation rot, T &x, T &y, T &z) { switch (rot) { case ROTATION_NONE: @@ -371,35 +381,3 @@ static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) return false; } - -/** - * rotate a 3 element int16_t vector in-place - */ -__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) -{ - if (!rotate_3(rot, x, y, z)) { - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; - x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); - y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); - z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); - } - } -} - -/** - * rotate a 3 element float vector in-place - */ -__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z) -{ - if (!rotate_3(rot, x, y, z)) { - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; - x = r(0); - y = r(1); - z = r(2); - } - } -} diff --git a/src/lib/events/libevents b/src/lib/events/libevents index 7c1720749d..9ef591c447 160000 --- a/src/lib/events/libevents +++ b/src/lib/events/libevents @@ -1 +1 @@ -Subproject commit 7c1720749dfe555ec2e71d5f9f753e6ac1244e1c +Subproject commit 9ef591c447fe0386d698bf6fb9a6d27e43988ee4 diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 30cc10b6fe..c6821cd352 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,8 +48,6 @@ #include #include -#include - #include #include @@ -171,15 +169,7 @@ public: /** * @brief Construct and initialize a new Map Projection object */ - MapProjection(double lat_0, double lon_0) - { - initReference(lat_0, lon_0); - } - - /** - * @brief Construct and initialize a new Map Projection object - */ - MapProjection(double lat_0, double lon_0, uint64_t timestamp) + MapProjection(double lat_0, double lon_0, uint64_t timestamp = 0) { initReference(lat_0, lon_0, timestamp); } @@ -192,20 +182,7 @@ public: * @param lat in degrees (47.1234567°, not 471234567°) * @param lon in degrees (8.1234567°, not 81234567°) */ - void initReference(double lat_0, double lon_0, uint64_t timestamp); - - /** - * Initialize the map transformation - * - * with reference coordinates on the geographic coordinate system - * where the azimuthal equidistant plane's origin is located - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ - inline void initReference(double lat_0, double lon_0) - { - initReference(lat_0, lon_0, hrt_absolute_time()); - } + void initReference(double lat_0, double lon_0, uint64_t timestamp = 0); /** * @return true, if the map reference has been initialized before diff --git a/src/lib/lat_lon_alt/CMakeLists.txt b/src/lib/lat_lon_alt/CMakeLists.txt new file mode 100644 index 0000000000..acd30f51f2 --- /dev/null +++ b/src/lib/lat_lon_alt/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(lat_lon_alt + lat_lon_alt.cpp + lat_lon_alt.hpp +) + +add_dependencies(lat_lon_alt prebuild_targets) +target_include_directories(lat_lon_alt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_compile_options(lat_lon_alt PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS lat_lon_alt geo) diff --git a/src/lib/lat_lon_alt/lat_lon_alt.cpp b/src/lib/lat_lon_alt/lat_lon_alt.cpp new file mode 100644 index 0000000000..40b86a635a --- /dev/null +++ b/src/lib/lat_lon_alt/lat_lon_alt.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * 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::Vector3d; +using matrix::Vector2d; + +LatLonAlt LatLonAlt::fromEcef(const Vector3d &p_ecef) +{ + // Convert position using Borkowski closed-form exact solution + // P. D. Groves, "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd edition (appendix C) + const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(p_ecef(2)); + const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius; + const double beta = sqrt(p_ecef(0) * p_ecef(0) + p_ecef(1) * p_ecef(1)); + const double E = (k1 - k2) / beta; + const double F = (k1 + k2) / beta; + + const double P = 4.0 / 3.0 * (E * F + 1); + const double Q = 2 * (E * E - F * F); + const double D = P * P * P + Q * Q; + const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0); + const double G = 0.5 * (sqrt(E * E + V) + E); + const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G; + + const double lon = atan2(p_ecef(1), p_ecef(0)); + const double lat = matrix::sign(p_ecef(2)) * atan((1.0 - T * T) / (2.0 * T * sqrt(1.0 - Wgs84::eccentricity2))); + const double alt = (beta - Wgs84::equatorial_radius * T) * cos(lat) + + (p_ecef(2) - matrix::sign(p_ecef(2)) * Wgs84::equatorial_radius * sqrt(1.0 - Wgs84::eccentricity2)) * sin(lat); + + LatLonAlt lla; + lla.setLatLonRad(lat, lon); + lla.setAltitude(static_cast(alt)); + return lla; +} + +Vector3d LatLonAlt::toEcef() const +{ + const double cos_lat = cos(_latitude_rad); + const double sin_lat = sin(_latitude_rad); + const double cos_lon = cos(_longitude_rad); + const double sin_lon = sin(_longitude_rad); + + const double r_e = Wgs84::equatorial_radius / sqrt(1.0 - std::pow(Wgs84::eccentricity * sin_lat, 2.0)); + const double r_total = r_e + static_cast(_altitude); + + return Vector3d(r_total * cos_lat * cos_lon, + r_total * cos_lat * sin_lon, + ((1.0 - Wgs84::eccentricity2) * r_e + static_cast(_altitude)) * sin_lat); +} + +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(r_e) + _altitude), + -v_ned(0) / (static_cast(r_n) + _altitude), + -v_ned(1) * tanf(_latitude_rad) / (static_cast(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(altitude); + const double de_dlon = (r_e + static_cast(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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(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(delta_lat * d_lat_lon_to_d_xy(0)), + static_cast(delta_lon * d_lat_lon_to_d_xy(1)), + -delta_alt); +} diff --git a/src/lib/lat_lon_alt/lat_lon_alt.hpp b/src/lib/lat_lon_alt/lat_lon_alt.hpp new file mode 100644 index 0000000000..02614c1862 --- /dev/null +++ b/src/lib/lat_lon_alt/lat_lon_alt.hpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * 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 + +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; + } + + + static LatLonAlt fromEcef(const matrix::Vector3d &p_ecef); + matrix::Vector3d toEcef() const; + + 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; + + struct Wgs84 { + static constexpr double equatorial_radius = 6378137.0; + static constexpr double eccentricity = 0.0818191908425; + static constexpr double eccentricity2 = eccentricity * eccentricity; + static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2); + static constexpr double gravity_equator = 9.7803253359; + }; + +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); + + double _latitude_rad{0.0}; + double _longitude_rad{0.0}; + float _altitude{0.0}; +}; diff --git a/src/lib/lat_lon_alt/test_lat_lon_alt.cpp b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp new file mode 100644 index 0000000000..f2419951d5 --- /dev/null +++ b/src/lib/lat_lon_alt/test_lat_lon_alt.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#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)); +} + +TEST(TestLatLonAlt, fromAndToECEF) +{ + for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) { + for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) { + for (float alt = -500.f; alt < 8000.f; alt += 500.f) { + LatLonAlt lla(lat, lon, alt); + + LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef()); + EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt; + } + } + } +} diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 4e651cfee9..b80e2a73f5 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -52,7 +52,8 @@ class AlphaFilter { public: AlphaFilter() = default; - explicit AlphaFilter(float alpha) : _alpha(alpha) {} + explicit AlphaFilter(float sample_interval, float time_constant) { setParameters(sample_interval, time_constant); } + explicit AlphaFilter(float time_constant) : _time_constant(time_constant) {}; ~AlphaFilter() = default; @@ -61,8 +62,8 @@ public: * * Both parameters have to be provided in the same units. * - * @param sample_interval interval between two samples - * @param time_constant filter time constant determining convergence + * @param sample_interval interval between two samples in seconds + * @param time_constant filter time constant determining convergence in seconds */ void setParameters(float sample_interval, float time_constant) { @@ -71,6 +72,8 @@ public: if (denominator > FLT_EPSILON) { setAlpha(sample_interval / denominator); } + + _time_constant = time_constant; } bool setCutoffFreq(float sample_freq, float cutoff_freq) @@ -82,11 +85,20 @@ public: return false; } - setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq)); - _cutoff_freq = cutoff_freq; + setParameters(1.f / sample_freq, 1.f / (M_TWOPI_F * cutoff_freq)); return true; } + void setCutoffFreq(float cutoff_freq) + { + if (cutoff_freq > FLT_EPSILON) { + _time_constant = 1.f / (M_TWOPI_F * cutoff_freq); + + } else { + _time_constant = 0.f; + } + } + /** * Set filter parameter alpha directly without time abstraction * @@ -112,14 +124,19 @@ public: return _filter_state; } + const T update(const T &sample, float dt) + { + setParameters(dt, _time_constant); + return update(sample); + } + const T &getState() const { return _filter_state; } - float getCutoffFreq() const { return _cutoff_freq; } + float getCutoffFreq() const { return 1.f / (M_TWOPI_F * _time_constant); } protected: T updateCalculation(const T &sample); - - float _cutoff_freq{0.f}; + float _time_constant{0.f}; float _alpha{0.f}; T _filter_state{}; }; diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index 9e60b9ebef..d17447b5b2 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -55,6 +55,16 @@ public: } } + template + Matrix(const Matrix &aa) + { + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + _data[i][j] = static_cast(aa(i, j)); + } + } + } + template Matrix(const Slice &in_slice) { diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3dde..3a1581118b 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(mixer_module functions/FunctionConstantMax.hpp functions/FunctionConstantMin.hpp functions/FunctionGimbal.hpp + functions/FunctionICEngineControl.hpp functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp diff --git a/src/lib/mixer_module/functions/FunctionICEngineControl.hpp b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp new file mode 100644 index 0000000000..0609f5cc94 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "FunctionProviderBase.hpp" + +#include + +/** + * Functions: ICE... + */ +class FunctionICEControl : public FunctionProviderBase +{ +public: + FunctionICEControl() + { + resetAllToDisarmedValue(); + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionICEControl(); } + + void update() override + { + internal_combustion_engine_control_s internal_combustion_engine_control; + + // map [0, 1] to [-1, 1] which is the interface for non-motor PWM channels + if (_internal_combustion_engine_control_sub.update(&internal_combustion_engine_control)) { + _data[0] = internal_combustion_engine_control.ignition_on * 2.f - 1.f; + _data[1] = internal_combustion_engine_control.throttle_control * 2.f - 1.f; + _data[2] = internal_combustion_engine_control.choke_control * 2.f - 1.f; + _data[3] = internal_combustion_engine_control.starter_engine_control * 2.f - 1.f; + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::IC_Engine_Ignition]; } + +private: + static constexpr int num_data_points = 4; + + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + + static_assert(num_data_points == (int)OutputFunction::IC_Engine_Starter - (int)OutputFunction::IC_Engine_Ignition + 1, + "number of functions mismatch"); + + uORB::Subscription _internal_combustion_engine_control_sub{ORB_ID(internal_combustion_engine_control)}; + float _data[num_data_points] {}; +}; diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index fa42c8b63d..efba281884 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -76,7 +76,7 @@ public: static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) { - if (thrust_factor > 0.f && thrust_factor <= 1.f) { + if (thrust_factor > FLT_EPSILON && thrust_factor <= 1.f) { // thrust factor // rel_thrust = factor * x^2 + (1-factor) * x, const float a = thrust_factor; @@ -87,16 +87,17 @@ public: const float tmp2 = b * b / (4.f * a * a); for (int i = 0; i < num_values; ++i) { - float control = values[i]; - if (control > 0.f) { - values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); + const float control = values[i]; - } else if (control < -0.f) { - values[i] = tmp1 - sqrtf(tmp2 - (control / a)); + if (PX4_ISFINITE(control)) { + if (control > FLT_EPSILON) { + values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); - } else { - values[i] = 0.f; + } else if (control < -FLT_EPSILON) { + values[i] = tmp1 - sqrtf(tmp2 - (control / a)); + + } } } } diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab540..56a2d61356 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -64,6 +64,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Gripper, &FunctionGripper::allocate}, {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, {OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate}, + {OutputFunction::IC_Engine_Ignition, OutputFunction::IC_Engine_Starter, &FunctionICEControl::allocate}, }; MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index fa6cd81fcb..c55b72da4e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,6 +40,7 @@ #include "functions/FunctionConstantMin.hpp" #include "functions/FunctionGimbal.hpp" #include "functions/FunctionGripper.hpp" +#include "functions/FunctionICEngineControl.hpp" #include "functions/FunctionLandingGear.hpp" #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" @@ -170,8 +171,6 @@ public: void setAllMinValues(uint16_t value); void setAllMaxValues(uint16_t value); - uint16_t &reverseOutputMask() { return _reverse_output_mask; } - uint16_t &failsafeValue(int index) { return _failsafe_value[index]; } /** Disarmed values: disarmedValue < minValue needs to hold */ uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } uint16_t &minValue(int index) { return _min_value[index]; } @@ -295,7 +294,6 @@ private: DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode, ///< multicopter air-mode - (ParamFloat) _param_mot_slew_max, (ParamFloat) _param_thr_mdl_fac ///< thrust to motor control signal modelling factor ) }; diff --git a/src/lib/mixer_module/motor_params.c b/src/lib/mixer_module/motor_params.c index 319f470842..51cb34f3ec 100644 --- a/src/lib/mixer_module/motor_params.c +++ b/src/lib/mixer_module/motor_params.c @@ -38,22 +38,6 @@ * */ - -/** - * Minimum motor rise time (slew rate limit). - * - * Minimum time allowed for the motor input signal to pass through - * a range of 1000 PWM units. A value x means that the motor signal - * can only go from 1000 to 2000 PWM in minimum x seconds. - * - * Zero means that slew rate limiting is disabled. - * - * @min 0.0 - * @unit s/(1000*PWM) - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); - /** * Thrust to motor control signal model parameter * diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 7543ed388b..52543add97 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -38,6 +38,11 @@ functions: Landing_Gear_Wheel: 440 + IC_Engine_Ignition: 450 + IC_Engine_Throttle: 451 + IC_Engine_Choke: 452 + IC_Engine_Starter: 453 + # Add your own here: #MyCustomFunction: 10000 @@ -60,3 +65,9 @@ functions: condition: "PPS_CAP_ENABLE==0" text: "PPS needs to be enabled via PPS_CAP_ENABLE parameter." exclude_from_actuator_testing: true + RPM_Input: + start: 2070 + note: + condition: "RPM_CAP_ENABLE==0" + text: "RPM needs to be enabled via RPM_CAP_ENABLE parameter." + exclude_from_actuator_testing: true diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp index df27f92735..b009d815d4 100644 --- a/src/lib/modes/standard_modes.hpp +++ b/src/lib/modes/standard_modes.hpp @@ -47,27 +47,52 @@ enum class StandardMode : uint8_t { ORBIT = 2, CRUISE = 3, ALTITUDE_HOLD = 4, - RETURN_HOME = 5, - SAFE_RECOVERY = 6, - MISSION = 7, - LAND = 8, - TAKEOFF = 9, + SAFE_RECOVERY = 5, + MISSION = 6, + LAND = 7, + TAKEOFF = 8, }; /** * @return Get MAVLink standard mode from nav_state */ -static inline StandardMode getStandardModeFromNavState(uint8_t nav_state) +static inline StandardMode getStandardModeFromNavState(uint8_t nav_state, uint8_t vehicle_type, bool is_vtol) { switch (nav_state) { - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME; + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::SAFE_RECOVERY; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF; - // Note: all other standard modes do not directly map, or are vehicle-type specific + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return StandardMode::ALTITUDE_HOLD; + } + + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return StandardMode::POSITION_HOLD; + } + + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return StandardMode::CRUISE; + } + + break; + + case vehicle_status_s::NAVIGATION_STATE_ORBIT: + if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return StandardMode::ORBIT; + } + + break; } return StandardMode::NON_STANDARD; @@ -76,10 +101,10 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state) /** * @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported */ -static inline uint8_t getNavStateFromStandardMode(StandardMode mode) +static inline uint8_t getNavStateFromStandardMode(StandardMode mode, uint8_t vehicle_type, bool is_vtol) { switch (mode) { - case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + case StandardMode::SAFE_RECOVERY: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; @@ -87,6 +112,39 @@ static inline uint8_t getNavStateFromStandardMode(StandardMode mode) case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + case StandardMode::ALTITUDE_HOLD: + if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + || vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return vehicle_status_s::NAVIGATION_STATE_ALTCTL; + } + + break; + + case StandardMode::POSITION_HOLD: + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + + break; + + case StandardMode::CRUISE: + if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return vehicle_status_s::NAVIGATION_STATE_POSCTL; + } + + break; + + case StandardMode::ORBIT: + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + return vehicle_status_s::NAVIGATION_STATE_ORBIT; + } + + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + } + + break; + default: break; } diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index 28c70c690d..85c274ab5d 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -106,16 +106,32 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const { + const Vector3f &start_position = {_trajectory[0].getCurrentPosition(), + _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition() + }; + const Vector3f &target = waypoints[1]; + const Vector3f &next_target = waypoints[2]; - const auto &target = waypoints[1]; + const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)}; + const Vector2f target_xy_z = {target.xy().norm(), target(2)}; + const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)}; - Vector3f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition(), - _trajectory[2].getCurrentPosition()); + float arrival_z_speed = 0.0f; + const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f; - const float distance_start_target = fabs(target(2) - pos_traj(2)); - const float arrival_z_speed = 0.f; + if (target_next_different) { + const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot( + Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero())); + const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON); + float accel_tmp = _trajectory[2].getMaxAccel(); + float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp, + _vertical_acceptance_radius); + arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel()); + } + + const float distance_start_target = fabs(target(2) - start_position(2)); float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance( _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(), distance_start_target, arrival_z_speed)); diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp index ee06f8c2b1..5cf45fd5fe 100644 --- a/src/lib/motion_planning/PositionSmoothingTest.cpp +++ b/src/lib/motion_planning/PositionSmoothingTest.cpp @@ -151,14 +151,17 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration) TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) { - const int N_ITER = 2000; + const int N_ITER = 20000; const float DELTA_T = 0.02f; const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f}; const Vector3f TARGET{12.f, 17.f, 8.f}; const Vector3f NEXT_TARGET{8.f, 12.f, 80.f}; + const float XY_ACC_RAD = 10.f; + const float Z_ACC_RAD = 0.8f; - Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET}; + + Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET}; Vector3f ff_velocity{1.f, 0.1f, 0.3f}; Vector3f position{0.f, 0.f, 0.f}; @@ -180,12 +183,13 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity) ff_velocity = {0.f, 0.f, 0.f}; expectDynamicsLimitsRespected(out); - if (position == TARGET) { + if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) { printf("Converged in %d iterations\n", iteration); break; } } - EXPECT_EQ(TARGET, position); + EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD); + EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD); EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n"; } diff --git a/src/lib/motion_planning/TrajectoryConstraints.hpp b/src/lib/motion_planning/TrajectoryConstraints.hpp index 6d52e1861e..cc52a984de 100644 --- a/src/lib/motion_planning/TrajectoryConstraints.hpp +++ b/src/lib/motion_planning/TrajectoryConstraints.hpp @@ -74,15 +74,11 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co const bool target_next_different = distance_target_next > 0.001f; const bool waypoint_overlap = distance_target_next < config.xy_accept_rad; - const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad; - const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad; float speed_at_target = 0.0f; if (target_next_different && - !waypoint_overlap && - has_reached_altitude && - altitude_stays_same + !waypoint_overlap ) { const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot( Vector2f((target - next_target).xy()).unit_or_zero())); @@ -108,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co * * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits */ -template +template float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config) { static_assert(N >= 2, "Need at least 2 points to compute speed"); float max_speed = 0.f; - for (size_t j = 0; j < N - 1; j++) { - size_t i = N - 2 - j; + // go backwards through the waypoints + for (int i = (N - 2); i >= 0; i--) { max_speed = computeStartXYSpeedFromWaypoints(waypoints[i], waypoints[i + 1], waypoints[min(i + 2, N - 1)], diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index b87e652cad..81b609080a 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -62,6 +62,10 @@ if(DISABLE_PARAMS_MODULE_SCOPING) # allow those timer configurations to be skipped. if ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES pwm_out)) message(STATUS "Skipping pwm file path ${file_path} for VOXL2") + # Spacecraft has duplicate parameter names which kills the VOXL 2 build. VOXL 2 does not + # support the spacecraft module so we skip adding the parameters. + elseif ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES spacecraft)) + message(STATUS "Skipping spacecraft file path ${file_path} for VOXL2") else() list(APPEND module_config_files "${file_path}") endif() diff --git a/src/lib/parameters/flashparams/flashfs32.c b/src/lib/parameters/flashparams/flashfs32.c index 5680beb5a9..848623b261 100644 --- a/src/lib/parameters/flashparams/flashfs32.c +++ b/src/lib/parameters/flashparams/flashfs32.c @@ -1126,19 +1126,8 @@ int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16 /* No paramaters */ if (pf == NULL) { - size_t total_size = size + sizeof(flash_entry_header_t); - size_t alignment = 31;//32-byte flash line - 1 - size_t size_adjust = ((total_size + alignment) & ~alignment) - total_size; - total_size += size_adjust; - - /* Do we have free space ?*/ - - if (find_free(total_size) == NULL) { - - /* No parameters and no free space => need erase */ - - rv = parameter_flashfs_erase(); - } + // Parameters can't be found, assume sector is corrupt or empty + rv = parameter_flashfs_erase(); } return rv; diff --git a/src/lib/pid/CMakeLists.txt b/src/lib/pid/CMakeLists.txt index b8ed30b7a2..8adba376b8 100644 --- a/src/lib/pid/CMakeLists.txt +++ b/src/lib/pid/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-2024 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,4 +31,10 @@ # ############################################################################ -px4_add_library(pid pid.cpp) +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) diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp new file mode 100644 index 0000000000..6fbb346351 --- /dev/null +++ b/src/lib/pid/PID.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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; +} diff --git a/src/modules/mavlink/streams/COLLISION.hpp b/src/lib/pid/PID.hpp similarity index 55% rename from src/modules/mavlink/streams/COLLISION.hpp rename to src/lib/pid/PID.hpp index a3469c62d9..615fd60b22 100644 --- a/src/modules/mavlink/streams/COLLISION.hpp +++ b/src/lib/pid/PID.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * 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 @@ -31,54 +31,35 @@ * ****************************************************************************/ -#ifndef COLLISION_HPP -#define COLLISION_HPP +#pragma once -#include +#include -class MavlinkStreamCollision : public MavlinkStream +class PID { public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCollision(mavlink); } - - static constexpr const char *get_name_static() { return "COLLISION"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COLLISION; } - - const char *get_name() const override { return get_name_static(); } - uint16_t get_id() override { return get_id_static(); } - - unsigned get_size() override - { - return _collision_sub.advertised() ? MAVLINK_MSG_ID_COLLISION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; - } - + 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: - explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} + void updateIntegral(float error, const float dt); + float updateDerivative(float feedback, const float dt); - uORB::Subscription _collision_sub{ORB_ID(collision_report)}; + float _setpoint{0.f}; ///< current setpoint to track + float _integral{0.f}; ///< integral state + float _last_feedback{NAN}; - bool send() override - { - collision_report_s report; - bool sent = false; - - while ((_mavlink->get_free_tx_buf() >= get_size()) && _collision_sub.update(&report)) { - mavlink_collision_t msg = {}; - - msg.src = report.src; - msg.id = report.id; - msg.action = report.action; - msg.threat_level = report.threat_level; - msg.time_to_minimum_delta = report.time_to_minimum_delta; - msg.altitude_minimum_delta = report.altitude_minimum_delta; - msg.horizontal_minimum_delta = report.horizontal_minimum_delta; - - mavlink_msg_collision_send_struct(_mavlink->get_channel(), &msg); - sent = true; - } - - return sent; - } + // 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}; }; - -#endif // COLLISION_HPP diff --git a/src/lib/pid/PIDTest.cpp b/src/lib/pid/PIDTest.cpp new file mode 100644 index 0000000000..ae7d847bde --- /dev/null +++ b/src/lib/pid/PIDTest.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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 +#include + +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); +} diff --git a/src/lib/pid/pid.cpp b/src/lib/pid/pid.cpp deleted file mode 100644 index 076c1afeed..0000000000 --- a/src/lib/pid/pid.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes - * - * 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 - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin - * @author Julian Oes - */ - -#include "pid.h" -#include -#include - -#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; -} diff --git a/src/lib/pid/pid.h b/src/lib/pid/pid.h deleted file mode 100644 index e8b1aac4fd..0000000000 --- a/src/lib/pid/pid.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes - * - * 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 - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Anton Babushkin - * @author Julian Oes - */ - -#ifndef PID_H_ -#define PID_H_ - -#include - -__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_ */ diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp index 1af68e8da0..a0bce68718 100644 --- a/src/lib/rc/ghst.cpp +++ b/src/lib/rc/ghst.cpp @@ -69,6 +69,7 @@ #define GHST_FRAME_CRC_SIZE (1U) #define GHST_FRAME_TYPE_SIZE (1U) #define GHST_TYPE_DATA_CRC_SIZE (12U) +#define GHST_ADDR_FC (130U) #define GHST_MAX_NUM_CHANNELS (16) enum class ghst_parser_state_t : uint8_t { @@ -251,7 +252,8 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu if ((ghst_frame.type >= static_cast(ghstFrameType::frameTypeFirst)) && (ghst_frame.type <= static_cast(ghstFrameType::frameTypeLast)) && - (ghst_frame.header.length == GHST_TYPE_DATA_CRC_SIZE)) { + (ghst_frame.header.length == GHST_TYPE_DATA_CRC_SIZE) && + (ghst_frame.header.device_address == GHST_ADDR_FC)) { const uint8_t crc = ghst_frame.payload[ghst_frame.header.length - 2U]; if (crc == ghst_frame_CRC(ghst_frame)) { diff --git a/src/lib/rover_control/CMakeLists.txt b/src/lib/rover_control/CMakeLists.txt new file mode 100644 index 0000000000..f3a9277d29 --- /dev/null +++ b/src/lib/rover_control/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(rover_control + RoverControl.cpp + RoverControl.hpp +) + +target_link_libraries(rover_control PUBLIC PID) +target_link_libraries(rover_control PUBLIC geo) +px4_add_unit_gtest(SRC RoverControlTest.cpp LINKLIBS rover_control) diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp new file mode 100644 index 0000000000..0eafcaa9f8 --- /dev/null +++ b/src/lib/rover_control/RoverControl.cpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "RoverControl.hpp" +using namespace matrix; +namespace RoverControl +{ +float throttleControl(SlewRate &motor_setpoint, const float throttle_setpoint, + const float current_motor_setpoint, const float max_accel, const float max_decel, const float max_thr_spd, + const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(throttle_setpoint) || !PX4_ISFINITE(current_motor_setpoint) || !PX4_ISFINITE(dt)) { + return NAN; + } + + const bool accelerating = fabsf(throttle_setpoint) > fabsf(current_motor_setpoint); + + if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate + motor_setpoint.setSlewRate(max_accel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - + current_motor_setpoint)) { + motor_setpoint.setForcedValue(throttle_setpoint); + } + + } else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate + motor_setpoint.setSlewRate(max_decel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - + current_motor_setpoint)) { + motor_setpoint.setForcedValue(throttle_setpoint); + } + + } else { // Fallthrough if slew rate parameters are not configured + motor_setpoint.setForcedValue(throttle_setpoint); + } + + return motor_setpoint.getState(); +} + +float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, + const float yaw_slew_rate, float vehicle_yaw, float yaw_setpoint, const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(yaw_setpoint) || !PX4_ISFINITE(vehicle_yaw) || !PX4_ISFINITE(dt)) { + return NAN; + } + + yaw_setpoint = wrap_pi(yaw_setpoint); + vehicle_yaw = wrap_pi(vehicle_yaw); + + if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured + adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + adjusted_yaw_setpoint.update(yaw_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) { + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); + } + + } else { + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); + } + + // Feedback control + pid_yaw.setSetpoint( + wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)); // Error as setpoint to take care of wrapping + return pid_yaw.update(0.f, dt); + +} + +float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const float speed_setpoint, + const float vehicle_speed, const float max_accel, const float max_decel, const float max_thr_speed, const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(speed_setpoint) || !PX4_ISFINITE(vehicle_speed) || !PX4_ISFINITE(dt)) { + return NAN; + } + + // Apply acceleration and deceleration limit + if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { + speed_with_rate_limit.setSlewRate(max_accel); + speed_with_rate_limit.update(speed_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( + speed_setpoint - vehicle_speed)) { + speed_with_rate_limit.setForcedValue(speed_setpoint); + } + + } else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) { + speed_with_rate_limit.setSlewRate(max_decel); + speed_with_rate_limit.update(speed_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( + speed_setpoint - vehicle_speed)) { + speed_with_rate_limit.setForcedValue(speed_setpoint); + } + + } else { // Fallthrough if slew rate is not configured + speed_with_rate_limit.setForcedValue(speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + // Feedforward + if (max_thr_speed > FLT_EPSILON) { + forward_speed_normalized = math::interpolate(speed_with_rate_limit.getState(), + -max_thr_speed, max_thr_speed, + -1.f, 1.f); + } + + // Feedback control + if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) { + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + } else { + pid_speed.resetIntegral(); + } + + + return math::constrain(forward_speed_normalized, -1.f, 1.f); +} + +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint, + const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel, + const float wheel_track, const float dt) +{ + // Apply acceleration and deceleration limit + if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else { // Fallthrough if slew rate is not configured + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + // Feedback control + if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) { + pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); + + } else { + pid_yaw_rate.resetIntegral(); + } + + + return math::constrain(speed_diff_normalized, -1.f, 1.f); +} + +void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + MapProjection &global_ned_proj_ref) +{ + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + curr_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + curr_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, NAN); // Fallback if current waypoint is invalid + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + prev_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.previous.lat, + position_setpoint_triplet.previous.lon); + + } else { + prev_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, + NAN); // Fallback if previous waypoint is invalid + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN, + NAN); // Enables corner slow down with RTL + } +} + +float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned) +{ + // Sanitize inputs + if (!prev_wp_ned.isAllFinite() || !curr_wp_ned.isAllFinite() || !next_wp_ned.isAllFinite()) { + return NAN; + } + + const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned; + const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned; + + // Waypoint overlap + if (curr_to_next_wp_ned.norm() < FLT_EPSILON || curr_to_prev_wp_ned.norm() < FLT_EPSILON) { + return NAN; + } + + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + return acosf(cosin); +} + +} // RoverControl diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp new file mode 100644 index 0000000000..1196e04650 --- /dev/null +++ b/src/lib/rover_control/RoverControl.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +/** + * @file RoverControl.hpp + * + * Functions that are shared among the different rover modules. + * Also includes the parameters that are shared among the rover modules. + */ + +#pragma once +#include +#include +#include +#include +#include +#include + +using namespace matrix; +namespace RoverControl +{ +/** + * Applies acceleration/deceleration slew rate to a throttle setpoint. + * @param motor_setpoint Throttle setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param throttle_setpoint Throttle setpoint pre slew rate [-1, 1] + * @param current_motor_setpoint Currently applied motor input [-1, 1] + * @param max_accel Maximum allowed acceleration [m/s^2] + * @param max_decel Maximum allowed deceleration [m/s^2] + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s] + * @param dt Time since last update [s] + * @return Motor Setpoint [-1, 1] + */ +float throttleControl(SlewRate &motor_setpoint, float throttle_setpoint, float current_motor_setpoint, + float max_accel, float max_decel, float max_thr_spd, float dt); + +/** + * Applies yaw rate slew rate to a yaw setpoint and calculates the necessary yaw rate setpoint + * using a PID controller. + * @param adjusted_yaw_setpoint Yaw setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param pid_yaw Yaw PID (Updated by this function) + * @param yaw_slew_rate Yaw slew rate [rad/s] + * @param vehicle_yaw Measured vehicle yaw [rad] + * @param yaw_setpoint Yaw setpoint [rad] + * @param dt Time since last update [s] + * @return Yaw rate setpoint [rad/s] + */ +float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, float yaw_slew_rate, + float vehicle_yaw, float yaw_setpoint, float dt); +/** + * Applies acceleration/deceleration slew rate to a speed setpoint and calculates the necessary throttle setpoint + * using a feed forward term and PID controller. + * @param speed_with_rate_limit Speed setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param pid_speed Speed PID (Updated by this function) + * @param speed_setpoint Speed setpoint [m/s] + * @param vehicle_speed Measured vehicle speed [m/s] + * @param max_accel Maximum allowed acceleration [m/s^2] + * @param max_decel Maximum allowed deceleration [m/s^2] + * @param max_thr_speed Speed at maximum throttle [m/s] + * @param dt Time since last update [s] + * @return Throttle setpoint [-1, 1] + */ +float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float speed_setpoint, + float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt); + +/** + * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint + * using a feedforward term and/or a PID controller. + * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels. + * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function). + * @param pid_yaw_rate Yaw rate PID (Updated by this function). + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. + * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2]. + * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2]. + * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m]. + * @param dt Time since last update [s]. + * @return Normalized speed difference setpoint [-1, 1]. + */ +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, + float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel, + float wheel_track, float dt); + +/** + * Projects positionSetpointTriplet waypoints from global to ned frame. + * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) + * @param prev_wp_ned Previous waypoint in NED frame (Updated by this function) + * @param next_wp_ned Next waypoint in NED frame (Updated by this function) + * @param position_setpoint_triplet Position Setpoint Triplet + * @param curr_pos Current position of the rover in global frame + * @param home_pos Home position in global frame + * @param global_ned_proj_ref Global to ned projection + */ +void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + MapProjection &global_ned_proj_ref); + +/** + * Calculate and return the angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_wp_ned Previous waypoint in NED frame + * @param curr_wp_ned Current waypoint in NED frame + * @param next_wp_ned Next waypoint in NED frame + * @return Waypoint transition angle [rad] + */ +float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned); + +} diff --git a/src/lib/rover_control/RoverControlTest.cpp b/src/lib/rover_control/RoverControlTest.cpp new file mode 100644 index 0000000000..508cdd4486 --- /dev/null +++ b/src/lib/rover_control/RoverControlTest.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2025 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. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the Pure Pursuit algorithm + * Run this test only using "make tests TESTFILTER=RoverControl" +******************************************************************/ + +#include +#include "RoverControl.hpp" + +TEST(calcWaypointTransitionAngle, invalidInputs) +{ + Vector2f prev_wp_ned(NAN, NAN); + Vector2f curr_wp_ned(10.f, 10.f); + Vector2f next_wp_ned(10.f, 10.f); + float prevInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + prev_wp_ned = Vector2f(10.f, 10.f); + curr_wp_ned = Vector2f(NAN, NAN); + float currInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + curr_wp_ned = Vector2f(10.f, 10.f); + next_wp_ned = Vector2f(NAN, NAN); + float nextInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(prevInvalid)); + EXPECT_FALSE(PX4_ISFINITE(currInvalid)); + EXPECT_FALSE(PX4_ISFINITE(nextInvalid)); + +} + +TEST(calcWaypointTransitionAngle, validInputs) +{ + // P -- C -- N + Vector2f prev_wp_ned(0.f, 0.f); + Vector2f curr_wp_ned(10.f, 0.f); + Vector2f next_wp_ned(20.f, 0.f); + const float angle1 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle1, M_PI_F); + + /** + * N + * / + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(20.f, 10.f); + const float angle2 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle2, M_PI_F - M_PI_4_F); + + /** + * N + * | + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(10.f, 10.f); + const float angle3 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle3, M_PI_2_F); + + /** + * N + * \ + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(0.f, 10.f); + const float angle4 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle4, M_PI_4_F); + + // P/C -- N + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(0.f, 0.f); + next_wp_ned = Vector2f(10.f, 0.f); + const float angle5 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(angle5)); + + // P -- C/N + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(10.f, 0.f); + const float angle6 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(angle6)); +} diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml new file mode 100644 index 0000000000..a786a32c4b --- /dev/null +++ b/src/lib/rover_control/module.yaml @@ -0,0 +1,207 @@ +module_name: Rover Control + +parameters: + - group: Rover Control + definitions: + RO_MAX_THR_SPEED: + description: + short: Speed the rover drives at maximum throttle + long: Used to linearly map speeds [m/s] to throttle values [-1. 1]. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RO_ACCEL_LIM: + description: + short: Acceleration limit + long: | + Set to -1 to disable. + For mecanum rovers this limit is used for longitudinal and lateral acceleration. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_DECEL_LIM: + description: + short: Deceleration limit + long: | + Set to -1 to disable. + Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + For mecanum rovers this limit is used for longitudinal and lateral deceleration. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_JERK_LIM: + description: + short: Jerk limit + long: | + Set to -1 to disable. + Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + For mecanum rovers this limit is used for longitudinal and lateral jerk. + type: float + unit: m/s^3 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_RATE_TH: + description: + short: Yaw rate measurement threshold + long: The minimum threshold for the yaw rate measurement not to be interpreted as zero. + type: float + unit: deg/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 3 + + RO_SPEED_TH: + description: + short: Speed measurement threshold + long: The minimum threshold for the speed measurement not to be interpreted as zero. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.1 + + RO_YAW_STICK_DZ: + description: + short: Yaw stick deadzone + long: Percentage of stick input range that will be interpreted as zero around the stick centered value. + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 0.1 + + - group: Rover Rate Control + definitions: + RO_YAW_RATE_P: + description: + short: Proportional gain for closed loop yaw rate controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + RO_YAW_RATE_I: + description: + short: Integral gain for closed loop yaw rate controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + RO_YAW_RATE_LIM: + description: + short: Yaw rate limit + long: | + Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints + in Acro, Stabilized and Position mode. + type: float + unit: deg/s + min: 0 + max: 10000 + increment: 0.01 + decimal: 2 + default: 0 + + RO_YAW_ACCEL_LIM: + description: + short: Yaw acceleration limit + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can increase. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_DECEL_LIM: + description: + short: Yaw deceleration limit + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can decrease. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + - group: Rover Attitude Control + definitions: + RO_YAW_P: + description: + short: Proportional gain for closed loop yaw controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + - group: Rover Velocity Control + definitions: + RO_SPEED_P: + description: + short: Proportional gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RO_SPEED_I: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 + + RO_SPEED_LIM: + description: + short: Speed limit + long: | + Used to cap speed setpoints and map controller inputs to speed setpoints + in Position mode. + type: float + unit: m/s + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp index 055b62ccab..101769844f 100644 --- a/src/lib/rtl/rtl_time_estimator.cpp +++ b/src/lib/rtl/rtl_time_estimator.cpp @@ -51,7 +51,6 @@ RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr) { _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); - _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -79,7 +78,6 @@ rtl_time_estimate_s RtlTimeEstimator::getEstimate() const void RtlTimeEstimator::update() { - _vehicle_status_sub.update(); _wind_sub.update(); if (_parameter_update_sub.updated()) { @@ -130,22 +128,11 @@ void RtlTimeEstimator::addWait(float time_s) } } -void RtlTimeEstimator::addDescendMCLand(float alt) -{ - if (PX4_ISFINITE(alt)) { - _is_valid = true; - - if (alt < -FLT_EPSILON && PX4_ISFINITE(alt)) { - _time_estimate += -alt / getHoverLandSpeed(); - } - } -} - float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_norm) { float cruise_speed = getCruiseSpeed(); - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { matrix::Vector2f wind = get_wind(); const float wind_along_dir = wind.dot(direction_norm); @@ -186,17 +173,17 @@ float RtlTimeEstimator::getCruiseSpeed() { float ret = 1e6f; - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { ret = 1e6f; } @@ -205,17 +192,6 @@ float RtlTimeEstimator::getCruiseSpeed() return ret; } -float RtlTimeEstimator::getHoverLandSpeed() -{ - float ret = 1e6f; - - if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - - return ret; -} - matrix::Vector2f RtlTimeEstimator::get_wind() { _wind_sub.update(); @@ -233,12 +209,12 @@ float RtlTimeEstimator::getClimbRate() { float ret = 1e6f; - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { ret = 1e6f; @@ -252,12 +228,12 @@ float RtlTimeEstimator::getDescendRate() { float ret = 1e6f; - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { ret = 1e6f; } - } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + } else if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { ret = 1e6f; } diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h index db0f8c234e..8c8519cd9b 100644 --- a/src/lib/rtl/rtl_time_estimator.h +++ b/src/lib/rtl/rtl_time_estimator.h @@ -63,12 +63,12 @@ public: ~RtlTimeEstimator() = default; void update(); - void reset() { _time_estimate = 0.f; _is_valid = false;}; + void reset() { _time_estimate = 0.f; _is_valid = false; }; rtl_time_estimate_s getEstimate() const; void addDistance(float hor_dist, const matrix::Vector2f &hor_direction, float vert_dist); void addVertDistance(float alt); void addWait(float time_s); - void addDescendMCLand(float alt); + void setVehicleType(uint8_t vehicle_type) { _vehicle_type = vehicle_type; }; private: /** @@ -106,13 +106,6 @@ private: */ float getCruiseSpeed(); - /** - * @brief Get the Hover Land Speed - * - * @return Hover land speed [m/s] - */ - float getHoverLandSpeed(); - /** * @brief Get the horizontal wind velocity * @@ -120,9 +113,11 @@ private: */ matrix::Vector2f get_wind(); - float _time_estimate; /**< Accumulated time estimate [s] */ + float _time_estimate{0.f}; /**< Accumulated time estimate [s] */ bool _is_valid{false}; /**< Checks if time estimate is valid */ + uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; /**< the defined vehicle type to use for the calculation*/ + DEFINE_PARAMETERS( (ParamFloat) _param_rtl_time_factor, /**< Safety factory for safe time estimate */ (ParamInt) _param_rtl_time_margin /**< Safety margin for safe time estimate */ @@ -130,7 +125,6 @@ private: param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; /**< MC climb velocity parameter */ param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; /**< MC descend velocity parameter */ - param_t _param_mpc_land_speed{PARAM_INVALID}; /**< MC land descend speed parameter */ param_t _param_fw_climb_rate{PARAM_INVALID}; /**< FW climb speed parameter */ param_t _param_fw_sink_rate{PARAM_INVALID}; /**< FW descend speed parameter */ @@ -139,7 +133,6 @@ private: param_t _param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< Parameter update topic */ - uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; /**< wind topic */ }; diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 8d328e7a7d..5f9dd72d43 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -85,6 +85,7 @@ public: int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } + void disable() { _priority = 0; } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index 1ca273be84..d396039f3f 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -189,11 +189,10 @@ PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10); PARAM_DEFINE_INT32(SYS_HAS_GPS, 1); /** - * Control if the vehicle has a magnetometer + * Control if and how many magnetometers are expected * - * Set this to 0 if the board has no magnetometer. - * If set to 0, the preflight checks will not check for the presence of a - * magnetometer, otherwise N sensors are required. + * 0: System has no magnetometer, preflight checks should pass without one. + * 1-N: Require the presence of N magnetometer sensors for check to pass. * * @reboot_required true * @group System diff --git a/src/lib/world_magnetic_model/geo_mag_declination.cpp b/src/lib/world_magnetic_model/geo_mag_declination.cpp index 64738426f5..46e4cb6ec0 100644 --- a/src/lib/world_magnetic_model/geo_mag_declination.cpp +++ b/src/lib/world_magnetic_model/geo_mag_declination.cpp @@ -53,7 +53,7 @@ using math::constrain; -static constexpr unsigned get_lookup_table_index(float *val, float min, float max) +static unsigned get_lookup_table_index(float *val, float min, float max) { /* for the rare case of hitting the bounds exactly * the rounding logic wouldn't fit, so enforce it. @@ -66,7 +66,7 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma return static_cast((-(min) + *val) / SAMPLING_RES); } -static constexpr float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM]) +static float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM]) { latitude_deg = math::constrain(latitude_deg, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 1880d5ab3c..03b9d48574 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -124,6 +125,7 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; uORB::Subscription _position_setpoint_sub{ORB_ID(position_setpoint)}; + uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)}; uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; @@ -386,18 +388,25 @@ AirspeedModule::Run() input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; input_data.airspeed_timestamp = airspeed_raw.timestamp; - input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + input_data.air_temperature_celsius = _vehicle_air_data.ambient_temperature; - // takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed - if (_in_takeoff_situation && - (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() || - (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()))) { - _in_takeoff_situation = false; - } + if (_in_takeoff_situation) { + // set flag to false if either speed is above stall speed, + // or launch detection + land detection indicate flying + const bool speed_above_stall = airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get(); + airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() + || (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()); - // reset takeoff_situation to true when not in air and not in fixed-wing mode - if (!in_air_fixed_wing) { - _in_takeoff_situation = true; + launch_detection_status_s launch_detection_status{}; + _launch_detection_status_sub.copy(&launch_detection_status); + const bool launch_detection_flying = launch_detection_status.launch_detection_state == + launch_detection_status_s::STATE_FLYING + && !_vehicle_land_detected.landed; + _in_takeoff_situation = !(speed_above_stall || launch_detection_flying); + + } else { + // reset takeoff_situation to true when not in air and not in fixed-wing mode + _in_takeoff_situation = !in_air_fixed_wing; } input_data.in_fixed_wing_flight = (in_air_fixed_wing && !_in_takeoff_situation); diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index e7ca828ce0..8353b5d5d3 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -68,16 +68,45 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i snprintf(param_name, sizeof(param_name), "BAT%d_I_CHANNEL", index); _analog_param_handles.i_channel = param_find(param_name); + + snprintf(param_name, sizeof(param_name), "BAT%d_I_OVERWRITE", index); + _analog_param_handles.i_overwrite = param_find(param_name); } void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw) { - const float voltage_v = voltage_raw * _analog_params.v_div; - const float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; - + float voltage_v = voltage_raw * _analog_params.v_div; const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); + float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; + +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + + if (_last_timestamp == 0) { + _last_timestamp = timestamp; + } + + const float dt = (timestamp - _last_timestamp) / 1e6f; + _last_timestamp = timestamp; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt); +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + current_a = _current_filter.update(fmaxf(current_a, 0.f), dt); +#endif + + // Overwrite the measured current if current overwrite is defined and vehicle is unarmed + if (_analog_params.i_overwrite > 0) { + updateTopics(); + + if (_arming_state == vehicle_status_s::ARMING_STATE_DISARMED) { + current_a = _analog_params.i_overwrite; + } + } Battery::setConnected(connected); Battery::updateVoltage(voltage_v); @@ -123,7 +152,17 @@ AnalogBattery::updateParams() param_get(_analog_param_handles.a_per_v, &_analog_params.a_per_v); param_get(_analog_param_handles.v_channel, &_analog_params.v_channel); param_get(_analog_param_handles.i_channel, &_analog_params.i_channel); + param_get(_analog_param_handles.i_overwrite, &_analog_params.i_overwrite); param_get(_analog_param_handles.v_offs_cur, &_analog_params.v_offs_cur); Battery::updateParams(); } + +void AnalogBattery::updateTopics() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _arming_state = vehicle_status.arming_state; + } +} diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index ce2e677d74..1678f0aa01 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -34,7 +34,9 @@ #pragma once #include +#include #include +#include class AnalogBattery : public Battery { @@ -77,6 +79,7 @@ protected: param_t a_per_v; param_t v_channel; param_t i_channel; + param_t i_overwrite; } _analog_param_handles; struct { @@ -85,7 +88,26 @@ protected: float a_per_v; int32_t v_channel; int32_t i_channel; + float i_overwrite; } _analog_params; virtual void updateParams() override; + +private: + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uint8_t _arming_state{0}; + +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + hrt_abstime _last_timestamp {0}; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + AlphaFilter _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S}; +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + AlphaFilter _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S}; +#endif + + void updateTopics(); }; diff --git a/src/modules/battery_status/module.yaml b/src/modules/battery_status/module.yaml index 85b7bb2ecc..70ce948f89 100644 --- a/src/modules/battery_status/module.yaml +++ b/src/modules/battery_status/module.yaml @@ -61,3 +61,22 @@ parameters: num_instances: *max_num_config_instances instance_start: 1 default: [-1, -1] + + BAT${i}_I_OVERWRITE: + description: + short: Battery ${i} idle current overwrite + long: | + This parameter allows to overwrite the current measured during + idle (unarmed) state with a user-defined constant value (expressed in amperes). + When the system is armed, the measured current is used. This is useful + because on certain ESCs current measurements are inaccurate in case of no load. + Negative values are ignored and will cause the measured current to be used. + The default value of 0 disables the overwrite, in which case the measured value + is always used. + + type: float + decimal: 8 + reboot_required: true + num_instances: *max_num_config_instances + instance_start: 1 + default: [0, 0] diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a151b15989..f0537c7c6e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -594,7 +594,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ return TRANSITION_DENIED; } - _health_and_arming_checks.update(); + _health_and_arming_checks.update(false, true); if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) { tune_negative(true); @@ -716,6 +716,8 @@ Commander::Commander() : } updateParameters(); + + _failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this); } Commander::~Commander() @@ -1448,7 +1450,8 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: { mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1); - uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode); + uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode, _vehicle_status.vehicle_type, + _vehicle_status.is_vtol); if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) { answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); @@ -1714,8 +1717,6 @@ void Commander::updateParameters() _vehicle_status.system_type = value_int32; } - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status) @@ -1796,9 +1797,6 @@ void Commander::run() _status_changed = true; } - /* Update OA parameter */ - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - handlePowerButtonState(); systemPowerUpdate(); @@ -1970,7 +1968,7 @@ void Commander::checkForMissionUpdate() if (_mission_result_sub.updated()) { const mission_result_s &mission_result = _mission_result_sub.get(); - const auto prev_mission_mission_id = mission_result.mission_id; + const uint32_t prev_mission_mission_id = mission_result.mission_id; _mission_result_sub.update(); // if mission_result is valid for the current mission @@ -2155,9 +2153,10 @@ void Commander::vtolStatusUpdate() if (_vtol_vehicle_status_sub.update(&_vtol_vehicle_status) && is_vtol(_vehicle_status)) { // Check if there has been any change while updating the flags (transition = rotary wing status) - const auto new_vehicle_type = _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW ? - vehicle_status_s::VEHICLE_TYPE_FIXED_WING : - vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + const uint8_t new_vehicle_type = + _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW ? + vehicle_status_s::VEHICLE_TYPE_FIXED_WING : + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; if (new_vehicle_type != _vehicle_status.vehicle_type) { _vehicle_status.vehicle_type = new_vehicle_type; @@ -2264,7 +2263,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, + (_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } @@ -2798,8 +2798,8 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = false; if (_datalink_last_heartbeat_open_drone_id_system != 0) { - mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t"); - events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained"); + mavlink_log_info(&_mavlink_log_pub, "Remote ID system regained\t"); + events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "Remote ID system regained"); } } @@ -2809,16 +2809,6 @@ void Commander::dataLinkCheck() _vehicle_status.open_drone_id_system_present = true; _vehicle_status.open_drone_id_system_healthy = healthy; } - - if (telemetry.heartbeat_component_obstacle_avoidance) { - if (_avoidance_system_lost) { - _avoidance_system_lost = false; - _status_changed = true; - } - - _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; - _vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy; - } } } @@ -2860,27 +2850,16 @@ void Commander::dataLinkCheck() _status_changed = true; } - // OpenDroneID system + // Remote ID system if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s) && !_open_drone_id_system_lost) { - mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost"); - events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost"); + mavlink_log_critical(&_mavlink_log_pub, "Remote ID system lost"); + events::send(events::ID("commander_remote_id_lost"), events::Log::Critical, "Remote ID system lost"); _vehicle_status.open_drone_id_system_present = false; _vehicle_status.open_drone_id_system_healthy = false; _open_drone_id_system_lost = true; _status_changed = true; } - - // AVOIDANCE SYSTEM state check (only if it is enabled) - if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { - // if heartbeats stop - if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) - && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { - - _avoidance_system_lost = true; - _vehicle_status.avoidance_system_valid = false; - } - } } void Commander::battery_status_check() @@ -2999,6 +2978,20 @@ void Commander::send_parachute_command() set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } +void Commander::onFailsafeNotifyUserTrampoline(void *arg) +{ + Commander *commander = static_cast(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) { @@ -3017,7 +3010,7 @@ The commander module contains the state machine for mode switching and failsafe #ifndef CONSTRAINED_FLASH PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration"); PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false); - PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false); + PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false); PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); PRINT_MODULE_USAGE_COMMAND("arm"); PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6dddf0777f..395e7954cb 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -44,7 +44,6 @@ #include "UserModeIntention.hpp" #include "worker_thread.hpp" -#include #include #include #include @@ -200,6 +199,9 @@ private: void modeManagementUpdate(); + static void onFailsafeNotifyUserTrampoline(void *arg); + void onFailsafeNotifyUser(); + enum class PrearmedMode { DISABLED = 0, SAFETY_BUTTON = 1, @@ -239,7 +241,6 @@ private: Hysteresis _auto_disarm_killed{false}; hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; @@ -266,7 +267,6 @@ private: bool _failsafe_user_override_request{false}; ///< override request due to stick movements bool _open_drone_id_system_lost{true}; - bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; bool _parachute_system_lost{true}; @@ -328,7 +328,6 @@ private: param_t _param_rc_map_fltmode{PARAM_INVALID}; DEFINE_PARAMETERS( - (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, @@ -341,10 +340,10 @@ private: (ParamBool) _param_com_force_safety, (ParamFloat) _param_com_kill_disarm, (ParamBool) _param_com_mot_test_en, - (ParamBool) _param_com_obs_avoid, (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, + (ParamFloat) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 796427180f..16e074ba18 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -73,6 +73,14 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index); } +void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message) +{ + armingCheckFailure(required_modes.fail_modes, component, log_levels.external); + addEvent(event_id, log_levels, message, + (uint32_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index); +} + Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, uint32_t modes, unsigned args_size) { @@ -219,7 +227,7 @@ bool Report::finalize() return _results_changed; } -bool Report::report(bool is_armed, bool force) +bool Report::report(bool force) { const hrt_abstime now = hrt_absolute_time(); const bool has_difference = _had_unreported_difference || _results_changed; @@ -317,3 +325,12 @@ bool Report::report(bool is_armed, bool force) current_results.health.error, current_results.health.warning); return true; } + +bool Report::reportIfUnreportedDifferences() +{ + if (_had_unreported_difference) { + return report(true); + } + + return false; +} diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index da7c5c2965..e069788ddf 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -74,6 +74,12 @@ static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatc static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t), "type too small, use next larger type"); +// Type to pass two mode groups in one struct to have the same number of function arguments to facilitate events parsing +struct NavModesMessageFail { + NavModes message_modes; ///< modes in which there's user messageing but arming is allowed + NavModes fail_modes; ///< modes in which checks fail which must be a subset of message_modes +}; + static inline NavModes operator|(NavModes a, NavModes b) { return static_cast(static_cast(a) | static_cast(b)); @@ -128,10 +134,15 @@ public: bool isArmed() const { return _status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; } + bool isArmingRequest() const { return _is_arming_request; } + + void setIsArmingRequest(bool is_arming_request) { _is_arming_request = is_arming_request; } + const vehicle_status_s &status() const { return _status; } private: const vehicle_status_s &_status; + bool _is_arming_request{false}; // true if we currently have an arming request }; @@ -246,6 +257,14 @@ public: void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id, const events::LogLevels &log_levels, const char *message); + /** + * Overloaded variant of armingCheckFailure() which allows to separately specify modes in which a message should be emitted and a subset in which arming is blocked + * @param required_modes .message_modes modes in which to put out the event and hence user message. + * .failing_modes modes in which to to fail arming. Has to be a subset of message_modes to never disallow arming without a reason. + */ + void armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message); + void clearArmingBits(NavModes modes); /** @@ -332,7 +351,12 @@ private: */ bool finalize(); - bool report(bool is_armed, bool force); + bool report(bool force); + + /** + * Send out any unreported changes if there are any + */ + bool reportIfUnreportedDifferences(); const hrt_abstime _min_reporting_interval; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp index a638148513..f4f91d0450 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -50,12 +50,14 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu _failsafe_flags.home_position_invalid = true; } -bool HealthAndArmingChecks::update(bool force_reporting) +bool HealthAndArmingChecks::update(bool force_reporting, bool is_arming_request) { _reporter.reset(); _reporter.prepare(_context.status().vehicle_type); + _context.setIsArmingRequest(is_arming_request); + for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) { if (!_checks[i]) { break; @@ -65,7 +67,7 @@ bool HealthAndArmingChecks::update(bool force_reporting) } const bool results_changed = _reporter.finalize(); - const bool reported = _reporter.report(_context.isArmed(), force_reporting); + const bool reported = _reporter.report(force_reporting); if (reported) { @@ -87,7 +89,7 @@ bool HealthAndArmingChecks::update(bool force_reporting) } _reporter.finalize(); - _reporter.report(_context.isArmed(), false); + _reporter.report(false); _reporter._mavlink_log_pub = nullptr; // LEGACY end @@ -118,3 +120,8 @@ void HealthAndArmingChecks::updateParams() _checks[i]->updateParams(); } } + +bool HealthAndArmingChecks::reportIfUnreportedDifferences() +{ + return _reporter.reportIfUnreportedDifferences(); +} diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index fdf38f5d5d..d04b8d088e 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -83,9 +83,12 @@ public: * Run arming checks and report if necessary. * This should be called regularly (e.g. 1Hz). * @param force_reporting if true, force reporting even if nothing changed + * @param is_arming_request if true, then we are running the checks based on an actual arming request * @return true if there was a report (also when force_reporting=true) */ - bool update(bool force_reporting = false); + bool update(bool force_reporting = false, bool is_arming_request = false); + + bool reportIfUnreportedDifferences(); /** * Whether arming is possible for a given navigation mode diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index c068863205..4bd7749ce4 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -69,7 +69,7 @@ TEST_F(ReporterTest, basic_no_checks) reporter.reset(); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, 0xff); @@ -92,7 +92,7 @@ TEST_F(ReporterTest, basic_fail_all_modes) reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, events::ID("arming_test_basic_fail_all_modes_fail1"), events::Log::Info, ""); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(nav_state)); ASSERT_TRUE(reporter.canRun(nav_state)); @@ -113,7 +113,7 @@ TEST_F(ReporterTest, arming_checks_mode_category) events::ID("arming_test_arming_checks_mode_category_fail2"), events::Log::Info, ""); reporter.setIsPresent(health_component_t::battery); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); ASSERT_TRUE(reporter.canRun(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); @@ -138,7 +138,7 @@ TEST_F(ReporterTest, arming_checks_mode_category2) reporter.healthFailure(NavModes::Mission, health_component_t::remote_control, events::ID("arming_test_arming_checks_mode_category2_fail1"), events::Log::Warning, ""); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); @@ -178,7 +178,7 @@ TEST_F(ReporterTest, reporting) } reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); if (i == 0) { @@ -219,7 +219,7 @@ TEST_F(ReporterTest, reporting) } reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); if (i == 0) { @@ -265,7 +265,7 @@ TEST_F(ReporterTest, reporting_multiple) reporter.armingCheckFailure(NavModes::All, health_component_t::remote_control, events::ID("arming_test_reporting_multiple_fail3"), events::Log::Warning, "", 55); reporter.finalize(); - reporter.report(false, false); + reporter.report(false); ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); if (i == 0) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 5e96c39633..85a19cf550 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -75,6 +75,10 @@ static constexpr const char *battery_fault_reason_str(battery_fault_reason_t bat void BatteryChecks::checkAndReport(const Context &context, Report &reporter) { if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { + // Reset related failsafe flags otherwise failures from before disabling the check cause failsafes without reported reason + reporter.failsafeFlags().battery_unhealthy = false; + reporter.failsafeFlags().battery_low_remaining_time = false; + reporter.failsafeFlags().battery_warning = battery_status_s::BATTERY_WARNING_NONE; return; } @@ -191,44 +195,75 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_warning = worst_warning; } - if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE - && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) { + const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE + && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED; + const bool configured_arm_threshold_in_use = !context.isArmed() && (_param_com_arm_bat_min.get() >= -FLT_EPSILON); + const bool below_configured_arm_threshold = (worst_battery_remaining < _param_com_arm_bat_min.get()); + + if (battery_warning || (configured_arm_threshold_in_use && below_configured_arm_threshold)) { const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; - NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None; - events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning; - /* EVENT - * @description - * The battery state of charge of the worst battery is below the warning threshold. - * - * - * This check can be configured via BAT_LOW_THR, BAT_CRIT_THR and BAT_EMERGEN_THR parameters. - * - */ - reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level, - "Low battery"); + NavModes affected_modes = (!configured_arm_threshold_in_use && critical_or_higher) + || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; + events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold + ? events::Log::Critical : events::Log::Warning; - if (reporter.mavlink_log_pub()) { - mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); + switch (reporter.failsafeFlags().battery_warning) { + default: + case battery_status_s::BATTERY_WARNING_LOW: + /* EVENT + * @description + * The lowest battery state of charge is below the low threshold. + * + * + * Can be configured with BAT_LOW_THR. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), + log_level, "Low battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery\t"); + } + + break; + + case battery_status_s::BATTERY_WARNING_CRITICAL: + /* EVENT + * @description + * The lowest battery state of charge is below the critical threshold. + * + * + * Can be configured with BAT_CRIT_THR and from when to disalow arming with COM_ARM_BAT_MIN. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_critical"), + log_level, "Critical battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Critical battery\t"); + } + + break; + + case battery_status_s::BATTERY_WARNING_EMERGENCY: + /* EVENT + * @description + * The lowest battery state of charge is below the emergency threshold. + * + * + * Can be configured with BAT_EMERGEN_THR. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_emergency"), + log_level, "Emergency battery level"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Emergency battery level\t"); + } + + break; } - } else if (!context.isArmed() && _param_arm_battery_level_min.get() > FLT_EPSILON - && worst_battery_remaining < _param_arm_battery_level_min.get()) { - // if not armed, additionally check if the battery is below the separately configurable preflight threshold - /* EVENT - * @description - * The battery state of charge of the worst battery is below the preflight threshold. - * - * - * This check can be configured via COM_ARM_BAT_MIN parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_preflight_low"), - events::Log::Critical, - "Low battery"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); - } } rtlEstimateCheck(context, reporter, worst_battery_time_s); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp index 64cf1ed5e6..3aae0d1de1 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp @@ -57,7 +57,7 @@ private: bool _battery_connected_at_arming[battery_status_s::MAX_INSTANCES] {}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamFloat) _param_arm_battery_level_min, + (ParamFloat) _param_com_arm_bat_min, (ParamInt) _param_cbrk_supply_chk ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 9554c57a4f..9b35c80992 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -77,7 +77,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter) esc_status_s esc_status; - if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) { + if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) { checkEscStatus(context, reporter, esc_status); reporter.setIsPresent(health_component_t::motors_escs); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index aa400e27c1..994bf9cbc2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -273,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } if (!context.isArmed() && ekf_gps_check_fail) { - NavModes required_groups_gps; + NavModesMessageFail required_modes; events::Log log_level; switch (static_cast(_param_com_arm_wo_gps.get())) { @@ -281,17 +281,21 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* FALLTHROUGH */ case GnssArmingCheck::DenyArming: - required_groups_gps = required_groups; + required_modes.message_modes = required_modes.fail_modes = NavModes::All; log_level = events::Log::Error; break; case GnssArmingCheck::WarningOnly: - required_groups_gps = NavModes::None; // optional + required_modes.message_modes = (NavModes)(reporter.failsafeFlags().mode_req_local_position + | reporter.failsafeFlags().mode_req_local_position_relaxed + | reporter.failsafeFlags().mode_req_global_position); + // Only warn and don't block arming because there could still be a valid position estimate from another source e.g. optical flow, VIO + required_modes.fail_modes = NavModes::None; log_level = events::Log::Warning; break; case GnssArmingCheck::Disabled: - required_groups_gps = NavModes::None; + required_modes.message_modes = required_modes.fail_modes = NavModes::None; log_level = events::Log::Disabled; break; } @@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_fix_too_low"), log_level, "GPS fix too low"); @@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_num_sats_too_low"), log_level, "Not enough GPS Satellites"); @@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_pdop_too_high"), log_level, "GPS PDOP too high"); @@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_err_too_high"), log_level, "GPS Horizontal Position Error too high"); @@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_err_too_high"), log_level, "GPS Vertical Position Error too high"); @@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_speed_acc_too_low"), log_level, "GPS Speed Accuracy too low"); @@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_drift_too_high"), log_level, "GPS Horizontal Position Drift too high"); @@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_drift_too_high"), log_level, "GPS Vertical Position Drift too high"); @@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_speed_drift_too_high"), log_level, "GPS Horizontal Speed Drift too high"); @@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_speed_drift_too_high"), log_level, "GPS Vertical Speed Drift too high"); @@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_spoofed"), log_level, "GPS signal spoofed"); @@ -437,7 +441,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Estimator not using GPS"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_not_fusing"), log_level, "Estimator not using GPS"); @@ -446,7 +450,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Poor GPS Quality"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_generic"), log_level, "Poor GPS Quality"); } @@ -586,66 +590,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & } } } - - const hrt_abstime now = hrt_absolute_time(); - - /* Check estimator status for signs of bad yaw induced post takeoff navigation failure - * for a short time interval after takeoff. - * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial - * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but - * if this does not fix the issue we need to stop using a position controlled - * mode to prevent flyaway crashes. - */ - - if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - if (!context.isArmed()) { - _nav_test_failed = false; - _nav_test_passed = false; - - } else { - if (!_nav_test_passed) { - // Both test ratios need to pass/fail together to change the nav test status - const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f) - && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); - - const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); - - if (innovation_pass) { - _time_last_innov_pass = now; - - // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s); - const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); - - // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds - if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) { - _nav_test_passed = true; - _nav_test_failed = false; - } - - } else if (innovation_fail) { - _time_last_innov_fail = now; - - if (now > _time_last_innov_pass + 2_s) { - // if the innovation test has failed continuously, declare the nav as failed - _nav_test_failed = true; - /* EVENT - * @description - * Land and recalibrate the sensors. - */ - reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate, - events::ID("check_estimator_nav_failure"), - events::Log::Emergency, "Navigation failure"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t"); - } - } - } - } - } - } } void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const @@ -692,7 +636,8 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid && (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get()); - if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy) { + if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy + && _param_com_pos_low_act.get()) { // only report if armed if (context.isArmed()) { @@ -700,14 +645,15 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report * @description Local position estimate valid but has low accuracy. Warn user. * * - * This check can be configured via COM_POS_LOW_EPH parameter. + * This check can be configured via COM_POS_LOW_EPH and COM_POS_LOW_ACT parameters. * */ - events::send(events::ID("check_estimator_low_position_accuracy"), {events::Log::Error, events::LogInternal::Info}, - "Local position estimate has low accuracy"); + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_low_position_accuracy"), + events::Log::Error, "Position estimate has low accuracy"); if (reporter.mavlink_log_pub()) { - mavlink_log_warning(reporter.mavlink_log_pub(), "Local position estimate has low accuracy\t"); + mavlink_log_warning(reporter.mavlink_log_pub(), "Position estimate has low accuracy\t"); } } } @@ -733,8 +679,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // Check if quality checking of position accuracy and consistency is to be performed const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get(); - bool xy_valid = lpos.xy_valid && !_nav_test_failed; - bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; + bool xy_valid = lpos.xy_valid; + bool v_xy_valid = lpos.v_xy_valid; if (!context.isArmed()) { if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) { @@ -746,8 +692,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } } + const bool global_pos_valid = gpos.lat_lon_valid && gpos.alt_valid; + failsafe_flags.global_position_invalid = - !checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, + !checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period @@ -809,7 +757,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // altitude - failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s)); + failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 1_s); // attitude @@ -860,7 +808,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat const bool was_valid) const { bool valid = was_valid; - const bool data_stale = (now > data_timestamp_us + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0); + const bool data_stale = (now > data_timestamp_us + 1_s) || (data_timestamp_us == 0); const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index d5cb363e11..0e10484023 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -100,12 +100,6 @@ private: hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec) hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec) - // variables used to check for navigation failure after takeoff - hrt_abstime _time_last_innov_pass{0}; ///< last time velocity and position innovations passed - hrt_abstime _time_last_innov_fail{0}; ///< last time velocity and position innovations failed - bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed - bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - bool _gps_was_fused{false}; bool _gnss_spoofed{false}; @@ -118,7 +112,7 @@ private: (ParamBool) _param_sys_has_gps, (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_vel_fs_evh, - (ParamInt) _param_com_pos_fs_delay, - (ParamFloat) _param_com_low_eph + (ParamFloat) _param_com_low_eph, + (ParamInt) _param_com_pos_low_act ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index 7129e46203..2efa6896c3 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -38,9 +38,7 @@ #include #include #include - -static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) - health_component_t::avoidance, "enum definition missmatch"); +#include class ExternalChecks : public HealthAndArmingCheckBase { @@ -66,7 +64,7 @@ public: void update(); bool isUnresponsive(int registration_id); - + bool allowUpdateWhileArmed() const { return _param_com_mode_arm_chk.get(); } private: static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms; static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; @@ -109,4 +107,7 @@ private: uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)}; uORB::Publication _arming_check_request_pub{ORB_ID(arming_check_request)}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_mode_arm_chk + ); }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 6716a8e06a..ac35a61803 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -47,9 +47,6 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter int num_enabled_and_valid_calibration = 0; for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) { - bool is_mag_fault = false; - const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault); - const bool exists = _sensor_mag_sub[instance].advertised(); bool is_valid = false; bool is_calibration_valid = false; @@ -80,7 +77,9 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter } // Do not raise errors if a mag is not required - if (!is_required) { + bool is_mag_fault = false; + + if (!isMagRequired(instance, is_mag_fault)) { continue; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index 8e3c0e3a5a..7352a6bbfe 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -77,29 +77,35 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) if (local_position_modes != NavModes::None) { /* EVENT + * @description + * The available positioning data is not sufficient to execute the selected mode. */ reporter.armingCheckFailure(local_position_modes, health_component_t::local_position_estimate, events::ID("check_modes_local_pos"), - events::Log::Error, "No valid local position estimate"); + events::Log::Error, "Navigation error: No valid position estimate"); reporter.clearCanRunBits(local_position_modes); } if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) { /* EVENT + * @description + * The available positioning data is not sufficient to execute the selected mode. */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::global_position_estimate, events::ID("check_modes_global_pos"), - events::Log::Error, "No valid global position estimate"); + events::Log::Error, "Navigation error: No valid global position estimate"); reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position); } if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) { /* EVENT + * @description + * The available positioning data is not sufficient to execute the selected mode. */ reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system, events::ID("check_modes_local_alt"), - events::Log::Critical, "No valid altitude estimate"); + events::Log::Critical, "Navigation error: No valid altitude estimate"); reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt); } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp index c2e2fb1598..331b12b272 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp @@ -56,7 +56,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) } else if (offboard_control_mode.velocity && reporter.failsafeFlags().local_velocity_invalid) { offboard_available = false; - } else if (offboard_control_mode.acceleration && reporter.failsafeFlags().local_velocity_invalid) { + } else if (offboard_control_mode.acceleration && reporter.failsafeFlags().attitude_invalid) { // OFFBOARD acceleration handled by position controller offboard_available = false; } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 5d3a26d95b..1f2592c9ed 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -80,12 +80,13 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) * * This check can be configured via COM_ARM_WO_GPS parameter. * + * The available positioning data is not sufficient to determine the vehicle's global position. */ reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_global_pos"), - events::Log::Error, "Global position required"); + events::Log::Error, "Global position estimate required"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position estimate required"); } } else if (reporter.failsafeFlags().home_position_invalid) { @@ -120,27 +121,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // avoidance system - if (context.status().avoidance_system_required) { - if (context.status().avoidance_system_valid) { - reporter.setIsPresent(health_component_t::avoidance); - - } else { - /* EVENT - * @description - * - * This check can be configured via COM_OBS_AVOID parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"), - events::Log::Error, "Avoidance system not ready"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready"); - } - } - } - // VTOL in transition if (context.status().is_vtol && !context.isArmed()) { if (context.status().in_transition_mode) { @@ -172,7 +152,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } // Arm Requirements: authorization - if (_param_com_arm_auth_req.get() != 0 && !context.isArmed()) { + if (_param_com_arm_auth_req.get() != 0 && !context.isArmed() && context.isArmingRequest()) { if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { /* EVENT */ diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 4aa341dbcf..5597b22391 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -170,7 +170,7 @@ void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double l void HomePosition::setInAirHomePosition() { - auto &home = _home_position_pub.get(); + home_position_s &home = _home_position_pub.get(); if (home.manual_home || (home.valid_lpos && home.valid_hpos && home.valid_alt)) { return; @@ -251,13 +251,13 @@ void HomePosition::setInAirHomePosition() bool HomePosition::setManually(double lat, double lon, float alt, float yaw) { - const auto &vehicle_local_position = _local_position_sub.get(); + const vehicle_local_position_s &vehicle_local_position = _local_position_sub.get(); if (!vehicle_local_position.xy_global || !vehicle_local_position.z_global) { return false; } - auto &home = _home_position_pub.get(); + home_position_s &home = _home_position_pub.get(); home.manual_home = true; home.lat = lat; @@ -328,7 +328,7 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) } const vehicle_local_position_s &lpos = _local_position_sub.get(); - const auto &home = _home_position_pub.get(); + const home_position_s &home = _home_position_pub.get(); if (lpos.heading_reset_counter != _heading_reset_counter) { if (_valid && set_automatically) { diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 14ba7ba28f..4430a0f563 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -370,11 +370,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa _failsafe_action_active = failsafe_action_active; _external_checks.update(); - bool allow_update_while_armed = false; -#if defined(CONFIG_ARCH_BOARD_PX4_SITL) - // For simulation, allow registering modes while armed for developer convenience - allow_update_while_armed = true; -#endif + bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed(); if (armed && !allow_update_while_armed) { // Reject registration requests @@ -408,7 +404,8 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa } } - // As we're disarmed we can use the user intended mode, as no failsafe will be active + // As we're disarmed we can use the user intended mode, as no failsafe will be active. + // Note that this might not be true if COM_MODE_ARM_CHK is set checkNewRegistrations(update_request); checkUnregistrations(user_intended_nav_state, update_request); } diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 32eb5b1e81..555b564d97 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -115,7 +115,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) // NAVIGATION_STATE_DESCEND setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_prevent_arming); // NAVIGATION_STATE_TERMINATION @@ -141,7 +140,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_attitude); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_alt); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_position); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_local_position_relaxed); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_prevent_arming); // NAVIGATION_STATE_AUTO_FOLLOW_TARGET diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 63d04957af..ba8bc59b3e 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -157,22 +157,18 @@ struct accel_worker_data_s { orb_advert_t *mavlink_log_pub{nullptr}; unsigned done_count{0}; float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {}; + + calibration::Accelerometer calibration[MAX_ACCEL_SENS] {}; }; // Read specified number of accelerometer samples, calculate average and dispersion. -static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3], - unsigned orient, unsigned samples_num) +static calibrate_return read_accelerometer_avg(accel_worker_data_s *worker_data, unsigned orient, unsigned samples_num) { Vector3f accel_sum[MAX_ACCEL_SENS] {}; unsigned counts[MAX_ACCEL_SENS] {}; unsigned errcount = 0; - // sensor thermal corrections - uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; - sensor_correction_s sensor_correction{}; - sensor_correction_sub.copy(&sensor_correction); - uORB::SubscriptionBlocking accel_sub[MAX_ACCEL_SENS] { {ORB_ID(sensor_accel), 0, 0}, {ORB_ID(sensor_accel), 0, 1}, @@ -187,32 +183,9 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS sensor_accel_s arp; while (accel_sub[accel_index].update(&arp)) { - // fetch optional thermal offset corrections in sensor/board frame - Vector3f offset{0, 0, 0}; - sensor_correction_sub.update(&sensor_correction); - - if (sensor_correction.timestamp > 0 && arp.device_id != 0) { - for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) { - if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) { - switch (correction_index) { - case 0: - offset = Vector3f{sensor_correction.accel_offset_0}; - break; - case 1: - offset = Vector3f{sensor_correction.accel_offset_1}; - break; - case 2: - offset = Vector3f{sensor_correction.accel_offset_2}; - break; - case 3: - offset = Vector3f{sensor_correction.accel_offset_3}; - break; - } - } - } - } - - accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset; + // fetch optional thermal offset corrections + worker_data->calibration[accel_index].SensorCorrectionsUpdate(); + accel_sum[accel_index] += worker_data->calibration[accel_index].Correct(Vector3f(arp.x, arp.y, arp.z)); counts[accel_index]++; } @@ -228,16 +201,9 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS } } - // rotate sensor measurements from sensor to body frame using board rotation matrix - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); - - for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { - accel_sum[s] = board_rotation * accel_sum[s]; - } - for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { const Vector3f avg{accel_sum[s] / counts[s]}; - avg.copyTo(accel_avg[s][orient]); + avg.copyTo(worker_data->accel_ref[s][orient]); } return calibrate_return_ok; @@ -251,7 +217,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); - read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num); + read_accelerometer_avg(worker_data, orientation, samples_num); // check accel for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { @@ -325,18 +291,27 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) { calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; + accel_worker_data_s worker_data{}; + worker_data.mavlink_log_pub = mavlink_log_pub; + unsigned active_sensors = 0; for (uint8_t cur_accel = 0; cur_accel < MAX_ACCEL_SENS; cur_accel++) { uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel), cur_accel}; if (accel_sub.advertised() && (accel_sub.get().device_id != 0) && (accel_sub.get().timestamp > 0)) { - calibrations[cur_accel].set_device_id(accel_sub.get().device_id); + worker_data.calibration[cur_accel].set_device_id(accel_sub.get().device_id); + + // clear existing calibration + worker_data.calibration[cur_accel].Reset(); + + // force fetch optional thermal offset corrections + worker_data.calibration[cur_accel].SensorCorrectionsUpdate(true); + active_sensors++; } else { - calibrations[cur_accel].Reset(); + worker_data.calibration[cur_accel].Reset(); } } @@ -353,16 +328,11 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } /* measure and calculate offsets & scales */ - accel_worker_data_s worker_data{}; - worker_data.mavlink_log_pub = mavlink_log_pub; bool data_collected[detect_orientation_side_count] {}; if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data, false) == calibrate_return_ok) { - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); - const Dcmf board_rotation_t = board_rotation.transpose(); - bool param_save = false; bool failed = true; @@ -396,29 +366,22 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G; // update calibration - const Vector3f accel_offs_rotated{board_rotation_t *offset}; - calibrations[i].set_offset(accel_offs_rotated); - - const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation}; - calibrations[i].set_scale(accel_T_rotated.diag()); + worker_data.calibration[i].set_offset(offset); + worker_data.calibration[i].set_scale(accel_T.diag()); #if defined(DEBUD_BUILD) PX4_INFO("accel %d: offset", i); offset.print(); - PX4_INFO("accel %d: bT * offset", i); - accel_offs_rotated.print(); PX4_INFO("accel %d: mat_A", i); mat_A.print(); PX4_INFO("accel %d: accel_T", i); accel_T.print(); - PX4_INFO("accel %d: bT * accel_T * b", i); - accel_T_rotated.print(); #endif // DEBUD_BUILD - calibrations[i].PrintStatus(); + worker_data.calibration[i].PrintStatus(); - if (calibrations[i].ParametersSave(i, true)) { + if (worker_data.calibration[i].ParametersSave(i, true)) { param_save = true; failed = false; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8c94771930..44c2f07987 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -512,19 +512,6 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0); */ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1); -/** - * Loss of position failsafe activation delay. - * - * This sets number of seconds that the position checks need to be failed before the failsafe will activate. - * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used. - * - * @unit s - * @group Commander - * @min 1 - * @max 100 - */ -PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1); - /** * Horizontal position error threshold. * @@ -653,14 +640,6 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); */ PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0); -/** - * Flag to enable obstacle avoidance. - * - * @boolean - * @group Commander - */ -PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); - /** * Expect and require a healthy MAVLink parachute system * @@ -928,13 +907,11 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f); PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0); /** - * EPH threshold for RTL + * Low position accuracy failsafe threshold * - * Specify the threshold for triggering a warning for low local position accuracy. Additionally triggers - * a RTL if currently in Mission or Loiter mode. - * Local position has to be still declared valid, which is most of all depending on COM_POS_FS_EPH. - * Use this feature on systems with dead-reckoning capabilites (e.g. fixed-wing vehicles with airspeed sensor) - * to improve the user notification and failure mitigation when flying in GNSS-denied areas. + * This triggers the action specified in COM_POS_LOW_ACT if the estimated position accuracy is below this threshold. + * Local position has to be still declared valid, which requires some kind of velocity aiding or large dead-reckoning time (EKF2_NOAID_TOUT), + * and a high failsafe threshold (COM_POS_FS_EPH). * * Set to -1 to disable. * @@ -945,6 +922,26 @@ PARAM_DEFINE_INT32(COM_WIND_MAX_ACT, 0); */ PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f); +/** + * Low position accuracy action + * + * Action the system takes when the estimated position has an accuracy below the specified threshold. + * See COM_POS_LOW_EPH to set the failsafe threshold. + * The failsafe action is only executed if the vehicle is in auto mission or auto loiter mode, + * otherwise it is only a warning. + * + * @group Commander + * + * @value 0 None + * @value 1 Warning + * @value 2 Hold + * @value 3 Return + * @value 4 Terminate + * @value 5 Land + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_POS_LOW_ACT, 3); + /** * Flag to allow arming * @@ -960,19 +957,18 @@ PARAM_DEFINE_INT32(COM_ARMABLE, 1); /** * Minimum battery level for arming * - * Additional battery level check that only allows arming if the state of charge of the emptiest - * connected battery is above this value. + * Threshold for battery percentage below arming is prohibited. * - * A value of 0 disables the check. + * A negative value means BAT_CRIT_THR is the threshold. * * @unit norm - * @min 0 + * @min -1 * @max 0.9 * @decimal 2 * @increment 0.01 * @group Commander */ -PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f); +PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, -1.f); /** * Enable throw-start @@ -987,9 +983,9 @@ PARAM_DEFINE_INT32(COM_THROW_EN, 0); /** * Minimum speed for the throw start * - * When the throw launch is enabled, the drone will only arm after this speed is exceeded before detecting - * the freefall. This is a safety feature to ensure the drone does not turn on after accidental drop or - * a rapid movement before the throw. + * When the throw launch is enabled, the drone will only allow motors to spin after this speed + * is exceeded before detecting the freefall. This is a safety feature to ensure the drone does + * not turn on after accidental drop or a rapid movement before the throw. * * Set to 0 to disable. * @@ -1014,3 +1010,14 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); * @increment 1 */ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3); + +/** + * Allow external mode registration while armed. + * + * By default disabled for safety reasons + * + * @group Commander + * @boolean + * + */ +PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0); diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 926bdfe44d..3ba1131d97 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -368,6 +368,49 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value) return options; } +FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value) +{ + ActionOptions options{}; + options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again + + switch (command_after_pos_low_failsafe(param_value)) { + case command_after_pos_low_failsafe::None: + options.action = Action::None; + break; + + case command_after_pos_low_failsafe::Warning: + options.action = Action::Warn; + break; + + case command_after_pos_low_failsafe::Hold_mode: + options.action = Action::Hold; + options.clear_condition = ClearCondition::WhenConditionClears; + break; + + case command_after_pos_low_failsafe::Return_mode: + options.action = Action::RTL; + options.clear_condition = ClearCondition::WhenConditionClears; + break; + + case command_after_pos_low_failsafe::Terminate: + options.allow_user_takeover = UserTakeoverAllowed::Never; + options.action = Action::Terminate; + options.clear_condition = ClearCondition::Never; + break; + + case command_after_pos_low_failsafe::Land_mode: + options.action = Action::Land; + options.clear_condition = ClearCondition::WhenConditionClears; + break; + + default: + options.action = Action::Warn; + break; + } + + return options; +} + FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value) { ActionOptions options{}; @@ -469,10 +512,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, ActionOptions(fromHighWindLimitActParam(_param_com_wind_max_act.get()).cannotBeDeferred())); CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred()); - // trigger RTL if low position accurancy is detected + // trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter) if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { - CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); + CHECK_FAILSAFE(status_flags, local_position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get())); } if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || @@ -501,22 +544,27 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } // Battery low failsafe + // If battery was low and arming was allowed through COM_ARM_BAT_MIN, don't failsafe immediately for the current low battery warning state + const bool warning_worse_than_at_arming = (status_flags.battery_warning > _battery_warning_at_arming); + const int32_t low_battery_action = warning_worse_than_at_arming ? + _param_com_low_bat_act.get() : (int32_t)LowBatteryAction::Warning; + switch (status_flags.battery_warning) { case battery_status_s::BATTERY_WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, - true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_LOW)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_LOW)); break; case battery_status_s::BATTERY_WARNING_CRITICAL: _last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical, _last_state_battery_warning_critical, - true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_CRITICAL)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_CRITICAL)); break; case battery_status_s::BATTERY_WARNING_EMERGENCY: _last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency, _last_state_battery_warning_emergency, - true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_EMERGENCY)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_EMERGENCY)); break; default: @@ -563,6 +611,7 @@ void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const f if (!_was_armed && armed) { _armed_time = time_us; _manual_control_lost_at_arming = status_flags.manual_control_signal_lost; + _battery_warning_at_arming = status_flags.battery_warning; } else if (!armed) { _manual_control_lost_at_arming = status_flags.manual_control_signal_lost; // ensure action isn't added while disarmed diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 742b84ce36..7865c207db 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -141,6 +141,15 @@ private: Land_mode = 5 }; + enum class command_after_pos_low_failsafe : int32_t { + None = 0, + Warning = 1, + Hold_mode = 2, + Return_mode = 3, + Terminate = 4, + Land_mode = 5 + }; + enum class command_after_remaining_flight_time_low : int32_t { None = 0, Warning = 1, @@ -156,6 +165,7 @@ private: static ActionOptions fromQuadchuteActParam(int param_value); static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode); static ActionOptions fromHighWindLimitActParam(int param_value); + static ActionOptions fromPosLowActParam(int param_value); static ActionOptions fromRemainingFlightTimeLowActParam(int param_value); const int _caller_id_mode_fallback{genCallerId()}; @@ -173,6 +183,7 @@ private: hrt_abstime _armed_time{0}; bool _was_armed{false}; bool _manual_control_lost_at_arming{false}; ///< true if manual control was lost at arming time + uint8_t _battery_warning_at_arming{0}; ///< low battery state at arming time DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase, (ParamInt) _param_nav_dll_act, @@ -190,7 +201,8 @@ private: (ParamInt) _param_com_obl_rc_act, (ParamInt) _param_com_qc_act, (ParamInt) _param_com_wind_max_act, - (ParamInt) _param_com_fltt_low_act + (ParamInt) _param_com_fltt_low_act, + (ParamInt) _param_com_pos_low_act ); }; diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 6c0edb58c4..8d633e9f21 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -170,6 +170,10 @@ void FailsafeBase::removeActions(ClearCondition condition) void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause) { + if (_on_notify_user_cb) { + _on_notify_user_cb(_on_notify_user_arg); + } + int delay_s = (_current_delay + 500_ms) / 1_s; PX4_DEBUG("User notification: failsafe triggered (action=%i, delayed_action=%i, cause=%i, delay=%is)", (int)action, (int)delayed_action, (int)cause, delay_s); diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 4cd3ac5202..a0ff26601b 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -165,6 +165,17 @@ public: bool getDeferFailsafes() const { return _defer_failsafes; } bool failsafeDeferred() const { return _failsafe_defer_started != 0; } + using UserCallback = void(*)(void *); + + /** + * Register a callback that is called before notifying the user. + */ + void setOnNotifyUserCallback(UserCallback callback, void *arg) + { + _on_notify_user_cb = callback; + _on_notify_user_arg = arg; + } + protected: enum class UserTakeoverAllowed { Always, ///< allow takeover (immediately) @@ -278,6 +289,9 @@ private: orb_advert_t _mavlink_log_pub{nullptr}; + UserCallback _on_notify_user_cb{nullptr}; + void *_on_notify_user_arg{nullptr}; + DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamFloat) _param_com_fail_act_t ); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 0ebcaebfc1..676b05297b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -253,7 +253,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) { - auto &calibration = worker_data.calibrations[uorb_index]; + calibration::Gyroscope &calibration = worker_data.calibrations[uorb_index]; if (calibration.device_id() != 0) { calibration.set_offset(worker_data.offset[uorb_index]); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4121549bd2..7aeadeab55 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -903,9 +903,18 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma bool param_save = false; bool failed = true; + bool external_mag_available = false; + + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].external() && worker_data.calibration[cur_mag].enabled()) { + external_mag_available = true; + break; + } + } + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { - auto ¤t_cal = worker_data.calibration[cur_mag]; + calibration::Magnetometer ¤t_cal = worker_data.calibration[cur_mag]; if (current_cal.device_id() != 0) { if (worker_data.append_to_existing_calibration) { @@ -921,6 +930,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma current_cal.set_offdiagonal(offdiag[cur_mag]); } + if (external_mag_available && !current_cal.external()) { + // automatically disable the internal mags as they should not be used for navigation + current_cal.disable(); + } + current_cal.PrintStatus(); if (current_cal.ParametersSave(cur_mag, true)) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt deleted file mode 100644 index 2406b81bf8..0000000000 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 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. -# -############################################################################ - -px4_add_library(ActuatorEffectiveness - ActuatorEffectiveness.cpp - ActuatorEffectiveness.hpp - ActuatorEffectivenessUUV.cpp - ActuatorEffectivenessUUV.hpp - ActuatorEffectivenessControlSurfaces.cpp - ActuatorEffectivenessControlSurfaces.hpp - ActuatorEffectivenessCustom.cpp - ActuatorEffectivenessCustom.hpp - ActuatorEffectivenessFixedWing.cpp - ActuatorEffectivenessFixedWing.hpp - ActuatorEffectivenessHelicopter.cpp - ActuatorEffectivenessHelicopter.hpp - ActuatorEffectivenessHelicopterCoaxial.cpp - ActuatorEffectivenessHelicopterCoaxial.hpp - ActuatorEffectivenessMCTilt.cpp - ActuatorEffectivenessMCTilt.hpp - ActuatorEffectivenessMultirotor.cpp - ActuatorEffectivenessMultirotor.hpp - ActuatorEffectivenessTilts.cpp - ActuatorEffectivenessTilts.hpp - ActuatorEffectivenessRotors.cpp - ActuatorEffectivenessRotors.hpp - ActuatorEffectivenessStandardVTOL.cpp - ActuatorEffectivenessStandardVTOL.hpp - ActuatorEffectivenessTiltrotorVTOL.cpp - ActuatorEffectivenessTiltrotorVTOL.hpp - ActuatorEffectivenessTailsitterVTOL.cpp - ActuatorEffectivenessTailsitterVTOL.hpp - ActuatorEffectivenessRoverAckermann.hpp - ActuatorEffectivenessRoverAckermann.cpp -) - -target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(ActuatorEffectiveness - PRIVATE - mathlib -) - -px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) -px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 65872dc432..36d5c7e171 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -32,8 +32,7 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(ActuatorEffectiveness) -add_subdirectory(ControlAllocation) +add_subdirectory(VehicleActuatorEffectiveness) px4_add_module( MODULE modules__control_allocator @@ -50,6 +49,7 @@ px4_add_module( DEPENDS mathlib ActuatorEffectiveness + VehicleActuatorEffectiveness ControlAllocation px4_work_queue SlewRate diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e..99e280dd71 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -270,6 +270,14 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopterCoaxial(this); break; + case EffectivenessSource::SPACECRAFT_2D: + // spacecraft_allocation does allocation and publishes directly to actuator_motors topic + break; + + case EffectivenessSource::SPACECRAFT_3D: + // spacecraft_allocation does allocation and publishes directly to actuator_motors topic + break; + default: PX4_ERR("Unknown airframe"); break; @@ -314,7 +322,7 @@ ControlAllocator::Run() #endif // Check if parameters have changed - if (_parameter_update_sub.updated() && !_armed) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 303316f1ef..1f91f7d17d 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -158,6 +158,8 @@ private: HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, HELICOPTER_COAXIAL = 12, + SPACECRAFT_2D = 13, + SPACECRAFT_3D = 14, }; enum class FailureMode { diff --git a/src/modules/control_allocator/Kconfig b/src/modules/control_allocator/Kconfig index 4352269d50..63c1c61500 100644 --- a/src/modules/control_allocator/Kconfig +++ b/src/modules/control_allocator/Kconfig @@ -10,3 +10,10 @@ menuconfig USER_CONTROL_ALLOCATOR depends on BOARD_PROTECTED && MODULES_CONTROL_ALLOCATOR ---help--- Put control_allocator in userspace memory + +menuconfig CONTROL_ALLOCATOR_RPM_CONTROL + bool "Include RPM control for Helicopter rotor" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Add support for controlling the helicopter main rotor rpm diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 23dee70563..46fea5f33e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -69,15 +69,11 @@ void ActuatorEffectivenessControlSurfaces::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_count_handle, &count) != 0) { + if (param_get(_count_handle, &_count) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _count = count; - for (int i = 0; i < _count; i++) { param_get(_param_handles[i].type, (int32_t *)&_params[i].type); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index a0b8b06c16..5e64e5a738 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include @@ -107,7 +107,7 @@ private: param_t _count_handle; Params _params[MAX_COUNT] {}; - int _count{0}; + int32_t _count{0}; SlewRate _flaps_setpoint_with_slewrate; SlewRate _spoilers_setpoint_with_slewrate; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index 0cc1390bd2..b7906669f2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index c7c2bba7d7..0a4516ba56 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessFixedWing.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 5b4b7785b2..f0b095709e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp similarity index 93% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b67..beacc7007b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -73,14 +73,13 @@ void ActuatorEffectivenessHelicopter::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)3, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; @@ -134,9 +133,16 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector @@ -41,6 +41,8 @@ #include #include +#include "RpmControl.hpp" + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -56,7 +58,7 @@ public: struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; - int num_swash_plate_servos{0}; + int32_t num_swash_plate_servos{0}; float throttle_curve[NUM_CURVE_POINTS]; float pitch_curve[NUM_CURVE_POINTS]; float yaw_collective_pitch_scale; @@ -131,4 +133,8 @@ private: bool _main_motor_engaged{true}; const ActuatorType _tail_actuator_type; + +#if CONTROL_ALLOCATOR_RPM_CONTROL + RpmControl _rpm_control {this}; +#endif // CONTROL_ALLOCATOR_RPM_CONTROL }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 0c06f5963f..40e956df99 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -60,14 +60,13 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)2, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index d5316bf498..e97c31c0da 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include @@ -55,7 +55,7 @@ public: struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; - int num_swash_plate_servos{0}; + int32_t num_swash_plate_servos{0}; float spoolup_time; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 848c8b8853..0b12482781 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index e0a355edd2..e5b7f3dcf8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 3844df4c84..c6f0425569 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 94f5db16f3..e9eda4c538 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessRoverAckermann.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 294e453ec6..f6108b4baf 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 79853c7518..f15624dd67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessStandardVTOL.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 0e5d1bfa44..9f945a6cd8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 604f05f9e0..708f104faa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -39,7 +39,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 310d937064..512bd41f41 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index d885a09155..1255d1b282 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 57e86f9436..1b89da8d9b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 0000000000..349893a3ea --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,43 @@ +px4_add_library(VehicleActuatorEffectiveness + ActuatorEffectivenessUUV.cpp + ActuatorEffectivenessUUV.hpp + ActuatorEffectivenessControlSurfaces.cpp + ActuatorEffectivenessControlSurfaces.hpp + ActuatorEffectivenessCustom.cpp + ActuatorEffectivenessCustom.hpp + ActuatorEffectivenessFixedWing.cpp + ActuatorEffectivenessFixedWing.hpp + ActuatorEffectivenessHelicopter.cpp + ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp + ActuatorEffectivenessMCTilt.cpp + ActuatorEffectivenessMCTilt.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessTilts.cpp + ActuatorEffectivenessTilts.hpp + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectivenessTiltrotorVTOL.hpp + ActuatorEffectivenessTailsitterVTOL.cpp + ActuatorEffectivenessTailsitterVTOL.hpp + ActuatorEffectivenessRoverAckermann.hpp + ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp + RpmControl.cpp +) + +target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(VehicleActuatorEffectiveness + PRIVATE + mathlib + ActuatorEffectiveness +) + +px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS VehicleActuatorEffectiveness) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 0000000000..53d9766e2d --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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 "RpmControl.hpp" + +#include + +using namespace time_literals; + +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pid.setOutputLimit(PID_OUTPUT_LIMIT); + _pid.setIntegralLimit(PID_OUTPUT_LIMIT); +}; + +void RpmControl::setSpoolupProgress(float spoolup_progress) +{ + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) { + _pid.resetIntegral(); + } +} + +float RpmControl::getActuatorCorrection() +{ + hrt_abstime now = hrt_absolute_time(); + + // RPM measurement update + if (_rpm_sub.updated()) { + rpm_s rpm{}; + + if (_rpm_sub.copy(&rpm)) { + const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f); + _timestamp_last_measurement = rpm.timestamp; + + const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; + } + } + + // Timeout + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { + _pid.resetIntegral(); + _actuator_correction = 0.f; + } + + return _actuator_correction; +} diff --git a/src/lib/bezier/BezierN.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp similarity index 57% rename from src/lib/bezier/BezierN.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp index 7f635e94b5..5fd0c96d91 100644 --- a/src/lib/bezier/BezierN.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp @@ -1,8 +1,6 @@ - - /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,43 +32,46 @@ ****************************************************************************/ /** - * @file BezierN.hpp + * @file RpmControl.hpp * - * @author Julian Kent + * Control rpm of a helicopter rotor. + * Input: PWM input pulse period from an rpm sensor + * Output: Duty cycle command for the ESC * - * N-order Bezier library designed for time-aware trajectory tracking + * @author Matthias Grob */ #pragma once -#include -namespace bezier +#include +#include +#include +#include +#include + +class RpmControl : public ModuleParams { +public: + RpmControl(ModuleParams *parent); + ~RpmControl() = default; -/* - * Calculates the location and velocity with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity); + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); -/* - * Calculates the position, velocity and acceleration with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration); +private: + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] -/* - * Calculates the position and velocity of yaw with respect to t on a bezier curve. - * All yaw setpoints are wrapped relative to the starting yaw. - * - */ -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint); + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; + PID _pid; + float _spoolup_progress{0.f}; // [0,1] + hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout + float _actuator_correction{0.f}; -/* - * Calculates the fraction between the begin and end time which can be used for fast bezier curve lookups - */ -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T); - -} + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_heli_rpm_sp, + (ParamFloat) _param_ca_heli_rpm_p, + (ParamFloat) _param_ca_heli_rpm_i + ) +}; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df4..b7fe1eb019 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -31,6 +31,8 @@ parameters: 11: Helicopter (tail Servo) 12: Helicopter (Coaxial) 13: Rover (Mecanum) + 14: Spacecraft 2D + 15: Spacecraft 3D default: 0 CA_METHOD: @@ -71,15 +73,14 @@ parameters: description: short: Motor ${i} slew rate limit long: | - Minimum time allowed for the motor input signal to pass through - the full output range. A value x means that the motor signal - can only go from 0 to 1 in minimum x seconds (in case of - reversible motors, the range is -1 to 1). + Forces the motor output signal to take at least the configured time (in seconds) + to traverse its full range (normally [0, 1], or if reversible [-1, 1]). Zero means that slew rate limiting is disabled. type: float decimal: 2 increment: 0.01 + unit: s num_instances: *max_num_mc_motors min: 0 max: 10 @@ -90,14 +91,14 @@ parameters: description: short: Servo ${i} slew rate limit long: | - Minimum time allowed for the servo input signal to pass through - the full output range. A value x means that the servo signal - can only go from -1 to 1 in minimum x seconds. + Forces the servo output signal to take at least the configured time (in seconds) + to traverse its full range [-100%, 100%]. Zero means that slew rate limiting is disabled. type: float decimal: 2 increment: 0.05 + unit: s num_instances: *max_num_servos min: 0 max: 10 @@ -528,6 +529,41 @@ parameters: which is mostly the case when the main rotor turns counter-clockwise. type: boolean default: 0 + CA_HELI_RPM_SP: + description: + short: Setpoint for main rotor rpm + long: | + Requires rpm feedback for the controller. + type: float + decimal: 0 + increment: 1 + min: 100 + max: 10000 + default: 1500 + CA_HELI_RPM_P: + description: + short: Proportional gain for rpm control + long: | + Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it. + + motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000 + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 + CA_HELI_RPM_I: + description: + short: Integral gain for rpm control + long: | + Same definition as the proportional gain but for integral. + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 # Others CA_FAILURE_MODE: diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 62e6f29103..112b920ba2 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -277,6 +277,7 @@ px4_add_module( world_magnetic_model ${EKF_LIBS} + lat_lon_alt bias_estimator output_predictor UNITY_BUILD diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 954123b250..2c4c898d41 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -154,6 +154,7 @@ target_link_libraries(ecl_EKF PRIVATE bias_estimator geo + lat_lon_alt output_predictor world_magnetic_model ${EKF_LIBS} diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 51f6fc8f70..075401e7a8 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -44,7 +44,7 @@ #include "ekf.h" -#include +#include #include #include @@ -97,7 +97,6 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag const bool continuing_conditions_passing = _control_status.flags.in_air - && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos; const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr; @@ -204,10 +203,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour _fault_status.flags.bad_airspeed = false; - VectorState H; // Observation jacobian - VectorState K; // Kalman gain vector - - sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K); + const VectorState H = sym::ComputeAirspeedH(_state.vector(), FLT_EPSILON); + VectorState K = P * H / aid_src.innovation_variance; if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index dc8f318c97..675bffdd3c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -73,25 +73,22 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed return; } - estimator_aid_source2d_s aid_src{}; - Vector2f position; + estimator_aid_source2d_s &aid_src = _aid_src_aux_global_position; + const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl); + const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used - if (ekf.global_origin_valid()) { - position = ekf.global_origin().project(sample.latitude, sample.longitude); - //const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude; - // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate - float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); - const float pos_var = sq(pos_noise); - const Vector2f pos_obs_var(pos_var, pos_var); + // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate + float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); + const float pos_var = sq(pos_noise); + const Vector2f pos_obs_var(pos_var, pos_var); - ekf.updateAidSourceStatus(aid_src, - sample.time_us, // sample timestamp - position, // observation - pos_obs_var, // observation variance - Vector2f(ekf.state().pos) - position, // innovation - Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance - math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate - } + ekf.updateAidSourceStatus(aid_src, + sample.time_us, // sample timestamp + matrix::Vector2d(sample.latitude, sample.longitude), // observation + pos_obs_var, // observation variance + innovation, // innovation + Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance + math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) && ekf.control_status_flags().yaw_align; @@ -107,14 +104,26 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed _state = State::starting; if (ekf.global_origin_valid()) { - ekf.enableControlStatusAuxGpos(); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; - _state = State::active; + const bool fused = ekf.fuseHorizontalPosition(aid_src); + bool reset = false; + + if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) { + ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv)); + ekf.resetAidSourceStatusZeroInnovation(aid_src); + reset = true; + } + + if (fused || reset) { + ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _state = State::active; + } } else { // Try to initialize using measurement - if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, - sample.epv)) { + if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, + sq(sample.epv))) { + ekf.resetAidSourceStatusZeroInnovation(aid_src); ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; @@ -130,12 +139,18 @@ 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)) { + if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) { - ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); - ekf.resetAidSourceStatusZeroInnovation(aid_src); + ekf.resetAidSourceStatusZeroInnovation(aid_src); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + } } } else { diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index e5bc78026a..423462d6ee 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -92,6 +92,7 @@ private: uint8_t lat_lon_reset_counter{}; }; + estimator_aid_source2d_s _aid_src_aux_global_position{}; RingBuffer _aux_global_position_buffer{20}; // TODO: size with _obs_buffer_length and actual publication rate uint64_t _time_last_buffer_push{0}; diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index dc6d36bcb5..2d20c64c81 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -65,6 +65,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if ((_baro_counter == 0) || baro_sample.reset) { _baro_lpf.reset(measurement); _baro_counter = 1; + _control_status.flags.baro_fault = false; } else { _baro_lpf.update(measurement); @@ -73,7 +74,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (_baro_counter <= _obs_buffer_length) { // Initialize the pressure offset (included in the baro bias) - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); } } @@ -106,14 +107,14 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding const bool continuing_conditions_passing = (_params.baro_ctrl == 1) && measurement_valid && (_baro_counter > _obs_buffer_length) - && !_baro_hgt_faulty; + && !_control_status.flags.baro_fault; const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); @@ -131,8 +132,9 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); + resetAidSourceStatusZeroInnovation(aid_src); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -147,7 +149,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { // Some other height source is still working - _baro_hgt_faulty = true; + _control_status.flags.baro_fault = true; } } @@ -163,12 +165,13 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::BARO; _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + initialiseAltitudeTo(measurement, measurement_var); + bias_est.reset(); + resetAidSourceStatusZeroInnovation(aid_src); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); } aid_src.time_last_fuse = imu_sample.time_us; @@ -202,7 +205,7 @@ void Ekf::stopBaroHgtFusion() #if defined(CONFIG_EKF2_BARO_COMPENSATION) float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const { - if (_control_status.flags.wind && local_position_is_valid()) { + if (_control_status.flags.wind && isLocalHorizontalPositionValid()) { // calculate static pressure error = Pmeas - Ptruth // model position error sensitivity as a body fixed ellipse with a different scale in the positive and // negative X and Y directions. Used to correct baro data for positional errors diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 440c0fe7ac..232ecd3752 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (measurement_valid && quality_sufficient) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); - bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) @@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_height_sensor_ref == HeightSensor::EV) { _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); + resetAltitudeTo(-measurement, measurement_var); bias_est.reset(); } else { - bias_est.setBias(-_state.pos(2) + measurement); + bias_est.setBias(_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; @@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); - bias_est.setBias(-_state.pos(2) + measurement); + resetAltitudeTo(-measurement - bias_est.getBias(), measurement_var); + bias_est.setBias(_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; @@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_params.height_sensor_ref == static_cast(HeightSensor::EV)) { ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); + resetAltitudeTo(-measurement, measurement_var); _height_sensor_ref = HeightSensor::EV; bias_est.reset(); } else { ECL_INFO("starting %s fusion", AID_SRC_NAME); - bias_est.setBias(-_state.pos(2) + measurement); + bias_est.setBias(_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 590a289da3..266b4cf548 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample #endif // CONFIG_EKF2_GNSS + const Vector2f position_estimate = getLocalHorizontalPosition(); + const Vector2f measurement{pos(0), pos(1)}; const Vector2f measurement_var{ @@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) { if (quality_sufficient) { // reset the bias estimator - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-position_estimate + measurement); } else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) { // otherwise stop EV position, when quality is good again it will restart with reset bias @@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample ev_sample.time_us, // sample timestamp position, // observation pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation + position_estimate - position, // innovation Vector2f(getStateVariance()) + pos_obs_var, // innovation variance math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate @@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample if (measurement_valid && quality_sufficient) { _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO - _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), + _ev_pos_b_est.fuseBias(measurement - position_estimate, measurement_var + Vector2f(getStateVariance())); } @@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem // TODO: (_params.position_sensor_ref == PositionSensor::EV) if (_control_status.flags.gps) { ECL_INFO("starting %s fusion", EV_AID_SRC_NAME); - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); _ev_pos_b_est.setFusionActive(); } else { @@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure _ev_pos_b_est.reset(); } else { - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } aid_src.time_last_fuse = _time_delayed_us; @@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (_control_status.flags.gps && !pos_xy_fusion_failing) { // reset EV position bias - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement); } else { _information_events.flags.reset_pos_to_vision = true; if (_control_status.flags.gps) { resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar()); - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } else { resetHorizontalPositionTo(measurement, measurement_var); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index ac2ff78f24..c399a44adb 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -53,7 +53,8 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c // determine if we should use EV velocity aiding bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VEL)) && _control_status.flags.tilt_align - && ev._sample.vel.isAllFinite(); + && ev._sample.vel.isAllFinite() + && !ev._sample.vel.longerThan(_params.velocity_limit); continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite(); diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index d10fe57d44..8fa2dfb625 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion() const float obs_var = sq(_params.pos_noaid_noise); const float innov_gate = 3.f; - updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate); + updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate); const bool continuing_conditions_passing = !isVerticalAidingActive(); const bool starting_conditions_passing = continuing_conditions_passing @@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion() void Ekf::resetFakeHgtFusion() { ECL_INFO("reset fake height fusion"); - _last_known_pos(2) = _state.pos(2); + _last_known_gpos.setAltitude(_gpos.altitude()); resetVerticalVelocityToZero(); resetHeightToLastKnown(); @@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion() void Ekf::resetHeightToLastKnown() { _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2)); - resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise)); + ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude()); + resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise)); } void Ekf::stopFakeHgtFusion() diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index ecc6cdfacb..3e244412ab 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -63,17 +63,17 @@ void Ekf::controlFakePosFusion() obs_var(0) = obs_var(1) = sq(0.5f); } - const Vector2f position(_last_known_pos); + const Vector2f innovation = (_gpos - _last_known_gpos).xy(); const float innov_gate = 3.f; updateAidSourceStatus(aid_src, _time_delayed_us, - position, // observation - obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation - Vector2f(getStateVariance()) + obs_var, // innovation variance - innov_gate); // innovation gate + Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation + obs_var, // observation variance + innovation, // innovation + Vector2f(getStateVariance()) + obs_var, // innovation variance + innov_gate); // innovation gate const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest; const bool enable_fake_pos = !enable_valid_fake_pos @@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion() void Ekf::resetFakePosFusion() { ECL_INFO("reset fake position fusion"); - _last_known_pos.xy() = _state.pos.xy(); + _last_known_gpos.setLatLon(_gpos); resetHorizontalPositionToLastKnown(); resetHorizontalVelocityToZero(); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index cc165d4533..9426fa2664 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2); + const float gnss_alt = gps_sample.alt + pos_offset_earth(2); - const float measurement = gnss_alt - getEkfGlobalOriginAltitude(); + const float measurement = gnss_alt; const float measurement_var = sq(noise); const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); @@ -81,18 +81,25 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding + const bool common_conditions_passing = measurement_valid + && _local_origin_lat_lon.isInitialized() + && _gps_checks_passed; + const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VPOS)) - && measurement_valid - && _pos_ref.isInitialized() - && _gps_checks_passed; + && common_conditions_passing; const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); + const bool altitude_initialisation_conditions_passing = common_conditions_passing + && !PX4_ISFINITE(_local_origin_alt) + && _params.height_sensor_ref == static_cast(HeightSensor::GNSS) + && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); + if (_control_status.flags.gps_hgt) { if (continuing_conditions_passing) { @@ -105,8 +112,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(aid_src.observation, measurement_var); - bias_est.setBias(_state.pos(2) + measurement); + resetAltitudeTo(measurement, measurement_var); + bias_est.setBias(-_gpos.altitude() + measurement); + resetAidSourceStatusZeroInnovation(aid_src); aid_src.time_last_fuse = _time_delayed_us; @@ -128,18 +136,28 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) _height_sensor_ref = HeightSensor::GNSS; _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-measurement, measurement_var); - _gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty + + initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); + resetAidSourceStatusZeroInnovation(aid_src); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + measurement); + bias_est.setBias(-_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; bias_est.setFusionActive(); _control_status.flags.gps_hgt = true; + + } if (altitude_initialisation_conditions_passing) { + + // Do not start GNSS altitude aiding, but use measurement + // to initialize altitude and bias of other height sensors + _information_events.flags.reset_hgt_to_gps = true; + + initialiseAltitudeTo(measurement, measurement_var); + bias_est.reset(); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 8d3cbd9567..81b3fcc76b 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -55,32 +55,6 @@ #define MASK_GPS_VSPD (1<<8) #define MASK_GPS_SPOOFED (1<<9) -void Ekf::collect_gps(const gnssSample &gps) -{ - if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) { - // If we have good GPS data set the origin's WGS-84 position to the last gps fix - setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc); - - // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(gps.alt, gps.vacc); - } - - _information_events.flags.gps_checks_passed = true; - - ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f", - _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon()); - - } else { - // a rough 2D fix is sufficient to lookup earth spin rate - const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - - if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) { - _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); - } - } -} - bool Ekf::runGnssChecks(const gnssSample &gps) { _gps_check_fail_status.flags.spoofed = gps.spoofed; @@ -128,11 +102,13 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift); - Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); - pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit); + Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); // Apply a low pass filter - _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + _gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + + // Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals + _gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit); // hdrift: calculate the horizontal drift speed and fail if too high _gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm(); @@ -172,6 +148,16 @@ bool Ekf::runGnssChecks(const gnssSample &gps) resetGpsDriftCheckFilters(); } + // force horizontal speed failure if above the limit + if (gps.vel.xy().longerThan(_params.velocity_limit)) { + _gps_check_fail_status.flags.hspeed = true; + } + + // force vertical speed failure if above the limit + if (fabsf(gps.vel(2)) > _params.velocity_limit) { + _gps_check_fail_status.flags.vspeed = true; + } + // save GPS fix for next time _gps_pos_prev.initReference(lat, lon, gps.time_us); _gps_alt_prev = gps.alt; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index bf3d086c6a..6ce0cebc66 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -67,11 +67,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { // First time checks are passing, latching. + if (!_gps_checks_passed) { + _information_events.flags.gps_checks_passed = true; + } + _gps_checks_passed = true; } - collect_gps(gnss_sample); - } else { // Skip this sample _gps_data_ready = false; @@ -82,10 +84,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } - if (_pos_ref.isInitialized()) { - updateGnssPos(gnss_sample, _aid_src_gnss_pos); - } - + updateGnssPos(gnss_sample, _aid_src_gnss_pos); updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel); } else if (_control_status.flags.gps) { @@ -108,9 +107,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled) && _control_status.flags.tilt_align - && _control_status.flags.yaw_align - && _pos_ref.isInitialized(); + && _control_status.flags.yaw_align; const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; + const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed; if (_control_status.flags.gps) { if (continuing_conditions_passing) { @@ -174,6 +173,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } _control_status.flags.gps = true; + + } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { + resetHorizontalPositionToGnss(_aid_src_gnss_pos); } } } @@ -204,7 +206,13 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, // but limit innovation to prevent spikes that could destabilise the filter - if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) { + bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical + && (aid_src.test_ratio[2] > 1.f) // vz rejected + && (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted + + if (bad_acc_vz_rejected + && (gnss_sample.sacc < _params.req_sacc) + ) { const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]); aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); aid_src.innovation_rejected = false; @@ -215,8 +223,10 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s { // correct position and height for offset relative to IMU const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy(); + const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body); + const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt); + const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth); + const Vector2f innovation = (_gpos - measurement_corrected).xy(); // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise); @@ -231,12 +241,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s const float pos_var = math::max(sq(pos_noise), sq(0.01f)); const Vector2f pos_obs_var(pos_var, pos_var); + const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg()); updateAidSourceStatus(aid_src, gnss_sample.time_us, // sample timestamp - position, // observation + observation, // observation pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation + innovation, // innovation Vector2f(getStateVariance()) + pos_obs_var, // innovation variance math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate } @@ -316,8 +327,9 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src) void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) { _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); - _gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty + resetLatLonTo(aid_src.observation[0], aid_src.observation[1], + aid_src.observation_variance[0] + + aid_src.observation_variance[1]); resetAidSourceStatusZeroInnovation(aid_src); } @@ -381,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const return false; } - return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); + const float yaw_var = _yawEstimator.getYawVar(); + + return (yaw_var > 0.f) + && (yaw_var < sq(_params.EKFGSF_yaw_err_max)) + && PX4_ISFINITE(yaw_var); } bool Ekf::isYawFailure() const diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 13caff5ca2..ae1142c86b 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -88,14 +88,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse); if (global_origin_valid() - && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) + && (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) ) { - // position of local NED origin in GPS / WGS84 frame - double latitude_deg; - double longitude_deg; - global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg); - - if (updateWorldMagneticModel(latitude_deg, longitude_deg)) { + if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) { wmm_updated = true; } @@ -368,7 +363,7 @@ bool Ekf::checkHaglYawResetReq() const // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. static constexpr float mag_anomalies_max_hagl = 1.5f; - const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; + const bool above_mag_anomalies = (getTerrainVPos() + _gpos.altitude()) > mag_anomalies_max_hagl; return above_mag_anomalies; } @@ -487,7 +482,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) // Check if there has been enough change in horizontal velocity to make yaw observable const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; - if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) { + if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) { // yaw angle must be observable to consider consistency _control_status.flags.mag_heading_consistent = true; } diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index dbbc1a7296..906e755a2f 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -230,12 +230,6 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample) const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample); resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var); - // reset position, estimate is relative to initial position in this mode, so we start with zero error - if (!_control_status.flags.in_air) { - ECL_INFO("reset position to zero"); - resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); - } - resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); _innov_check_fail_status.flags.reject_optflow_X = false; @@ -247,7 +241,7 @@ void Ekf::resetTerrainToFlow() ECL_INFO("reset hagl to flow"); // TODO: use the flow data - const float new_terrain = fmaxf(0.0f, _state.pos(2)); + const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance; const float delta_terrain = new_terrain - _state.terrain; _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index ea722569d6..747fac96f1 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -67,7 +67,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) // recalculate the innovation using the updated state const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias; - _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1]; + _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast + (_aid_src_optical_flow.observation[1]); } if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) { diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 918ab523c0..e6b520b600 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -74,11 +74,6 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { - if (fabsf(vz) < _min_vz_for_valid_consistency) { - // We can only check consistency if there is vertical motion - return; - } - if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { _is_kinematically_consistent = false; @@ -86,7 +81,8 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) } } else { - if ((_test_ratio < 1.f) + if ((fabsf(vz) > _min_vz_for_valid_consistency) + && (_test_ratio < 1.f) && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us) ) { _is_kinematically_consistent = true; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 3a848f7d46..e6cd566a3f 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -78,7 +78,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements + // If we are supposed to be using range finder data but have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air && _range_sensor.isRegularlySendingData() @@ -139,6 +139,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } else if (do_range_aid) { @@ -148,8 +149,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::RANGE; _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance); + resetAltitudeTo(aid_src.observation, aid_src.observation_variance); _state.terrain = 0.f; + resetAidSourceStatusZeroInnovation(aid_src); _control_status.flags.rng_hgt = true; stopRngTerrFusion(); @@ -163,6 +165,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } } @@ -180,7 +183,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-(aid_src.observation - _state.terrain)); + resetAltitudeTo(aid_src.observation - _state.terrain); + resetAidSourceStatusZeroInnovation(aid_src); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -198,6 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } @@ -218,6 +223,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { if (aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } _control_status.flags.rng_terrain = true; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index c0ae2a71eb..ab1c747e01 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -76,25 +76,27 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) if (_is_sample_ready) { _is_sample_valid = false; - if (_sample.quality == 0) { - _time_bad_quality_us = current_time_us; + _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us; - } else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) { - // We did not receive bad quality data for some time + if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) { + return; + } - if (isTiltOk() && isDataInRange()) { - updateStuckCheck(); - updateFogCheck(getDistBottom(), _sample.time_us); + updateStuckCheck(); + updateFogCheck(getDistBottom(), _sample.time_us); - if (!_is_stuck && !_is_blocked) { - _is_sample_valid = true; - _time_last_valid_us = _sample.time_us; - } - } + if (!_is_stuck && !_is_blocked) { + _is_sample_valid = true; + _time_last_valid_us = _sample.time_us; } } } +bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const +{ + return current_time_us - _time_bad_quality_us > _quality_hyst_us; +} + void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us) { // Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant. @@ -149,7 +151,7 @@ void SensorRangeFinder::updateStuckCheck() void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us) { - if (_max_fog_dist > 0.f && time_us - _time_last_valid_us < 1e6) { + if (_max_fog_dist > 0.f) { const float median_dist = _median_dist.apply(dist_bottom); const float factor = 2.f; // magic hardcoded factor diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index fd53407fa8..e3e4f6e6cd 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -131,6 +131,7 @@ private: bool isDataContinuous() const { return _dt_data_lpf < 2e6f; } bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } bool isDataInRange() const; + bool isQualityOk(uint64_t current_time_us) const; void updateStuckCheck(); void updateFogCheck(const float dist_bottom, const uint64_t time_us); @@ -184,6 +185,7 @@ private: float _max_fog_dist{0.f}; //< maximum distance at which the range finder could detect fog (m) math::MedianFilter _median_dist{}; float _prev_median_dist{0.f}; + }; } // namespace sensor diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index a75c1bb54e..d529e5e994 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -43,14 +43,14 @@ #include "ekf.h" #include -#include +#include #include void Ekf::controlBetaFusion(const imuSample &imu_delayed) { _control_status.flags.fuse_beta = _params.beta_fusion_enabled - && _control_status.flags.fixed_wing + && (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd) && _control_status.flags.in_air && !_control_status.flags.fake_pos; @@ -127,10 +127,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; const float epsilon = 1e-3f; - VectorState H; // Observation jacobian - VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K); + const VectorState H = sym::ComputeSideslipH(_state.vector(), epsilon); + VectorState K = P * H / sideslip.innovation_variance; if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); @@ -143,7 +142,5 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) sideslip.fused = true; sideslip.time_last_fuse = _time_delayed_us; - _fault_status.flags.bad_sideslip = false; - return true; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index cc1d48cf16..95efc971bc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -79,7 +79,7 @@ static constexpr uint64_t MAG_MAX_INTERVAL = // bad accelerometer detection and mitigation static constexpr uint64_t BADACC_PROBATION = - 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) + 3e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) static constexpr float BADACC_BIAS_PNOISE = 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) @@ -270,6 +270,8 @@ struct parameters { int32_t imu_ctrl{static_cast(ImuCtrl::GyroBias) | static_cast(ImuCtrl::AccelBias)}; + float velocity_limit{100.f}; ///< velocity state limit (m/s) + // measurement source control int32_t height_sensor_ref{static_cast(HeightSensor::BARO)}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; @@ -571,10 +573,10 @@ union filter_control_status_u { uint64_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated - uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference + uint64_t baro_hgt : 1; ///< 9 - true when baro data is being fused uint64_t rng_hgt : - 1; ///< 10 - true when range finder height is being fused as a primary height reference - uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference + 1; ///< 10 - true when range finder data is being fused for height aiding + uint64_t gps_hgt : 1; ///< 11 - true when GPS altitude is being fused uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended uint64_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused @@ -618,6 +620,7 @@ uint64_t mag_heading_consistent : uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position + uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index ceefc01f6f..8cb9e4be67 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -146,9 +146,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // Additional horizontal velocity data from an auxiliary sensor can be fused controlAuxVelFusion(imu_delayed); #endif // CONFIG_EKF2_AUXVEL - // + #if defined(CONFIG_EKF2_TERRAIN) controlTerrainFakeFusion(); + updateTerrainValidity(); #endif // CONFIG_EKF2_TERRAIN controlZeroInnovationHeadingUpdate(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f148a99d91..e7b2eea396 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -202,8 +202,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_WIND) // wind vel: add process noise - float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt); + const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate)); + const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; for (unsigned index = 0; index < State::wind_vel.dof; index++) { const unsigned i = State::wind_vel.idx + index; @@ -279,7 +280,17 @@ void Ekf::constrainStateVariances() void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), min, max); + if (P(i, i) < min) { + P(i, i) = min; + + } else if (P(i, i) > max) { + // Constrain the variance growth by fusing zero innovation as clipping the variance + // would artifically increase the correlation between states and destabilize the filter. + const float innov = 0.f; + const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) + const float innov_var = P(i, i) + R; + fuseDirectStateMeasurement(innov, innov_var, R, i); + } } } @@ -297,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float limited_max = math::constrain(state_var_max, min, max); float limited_min = math::constrain(limited_max / max_ratio, min, max); - for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), limited_min, limited_max); - } + constrainStateVar(state, limited_min, limited_max); } void Ekf::resetQuatCov(const float yaw_noise) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 910eb54dd6..febe46f90e 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -74,7 +74,7 @@ void Ekf::reset() // #if defined(CONFIG_EKF2_TERRAIN) // assume a ground clearance - _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -97,7 +97,7 @@ void Ekf::reset() resetGpsDriftCheckFilters(); _gps_checks_passed = false; #endif // CONFIG_EKF2_GNSS - _gps_alt_ref = NAN; + _local_origin_alt = NAN; _output_predictor.reset(); @@ -113,7 +113,7 @@ void Ekf::reset() _time_last_heading_fuse = 0; _time_last_terrain_fuse = 0; - _last_known_pos.setZero(); + _last_known_gpos.setZero(); #if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; @@ -168,7 +168,7 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos, _state.gyro_bias, _state.accel_bias); return true; @@ -205,7 +205,7 @@ bool Ekf::initialiseFilter() initialiseCovariance(); // reset the output predictor state history to match the EKF initial values - _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); + _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos); return true; } @@ -230,6 +230,11 @@ bool Ekf::initialiseTilt() void Ekf::predictState(const imuSample &imu_delayed) { + if (std::fabs(_gpos.latitude_rad() - _earth_rate_lat_ref_rad) > math::radians(1.0)) { + _earth_rate_lat_ref_rad = _gpos.latitude_rad(); + _earth_rate_NED = calcEarthRateNED((float)_earth_rate_lat_ref_rad); + } + // apply imu bias corrections const Vector3f delta_ang_bias_scaled = getGyroBias() * imu_delayed.delta_ang_dt; Vector3f corrected_delta_ang = imu_delayed.delta_ang - delta_ang_bias_scaled; @@ -254,25 +259,21 @@ void Ekf::predictState(const imuSample &imu_delayed) // calculate the increment in velocity using the current orientation _state.vel += corrected_delta_vel_ef; - // compensate for acceleration due to gravity - _state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt; + // compensate for acceleration due to gravity, Coriolis and transport rate + const Vector3f gravity_acceleration(0.f, 0.f, CONSTANTS_ONE_G); // simplistic model + const Vector3f coriolis_acceleration = -2.f * _earth_rate_NED.cross(vel_last); + const Vector3f transport_rate = -_gpos.computeAngularRateNavFrame(vel_last).cross(vel_last); + _state.vel += (gravity_acceleration + coriolis_acceleration + transport_rate) * imu_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity - _state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; + _gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; + _state.pos(2) = -_gpos.altitude(); // constrain states - _state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f); - _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); + _state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit); - - // calculate a filtered horizontal acceleration with a 1 sec time constant - // this are used for manoeuvre detection elsewhere - const float alpha = 1.0f - imu_delayed.delta_vel_dt; - _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - - // Calculate low pass filtered height rate - float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant - _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; + // calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere + _accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt); } bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude, @@ -283,14 +284,12 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl return false; } - if (!_pos_ref.isInitialized()) { - if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + if (!_local_origin_lat_lon.isInitialized()) { + if (!resetLatLonTo(latitude, longitude, sq(eph))) { return false; } - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(altitude, epv); - } + initialiseAltitudeTo(altitude, sq(epv)); return true; } @@ -298,7 +297,7 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl Vector3f pos_correction; // apply a first order correction using velocity at the delayed time horizon and the delta time - if ((timestamp_observation > 0) && local_position_is_valid()) { + if ((timestamp_observation > 0) && isLocalHorizontalPositionValid()) { timestamp_observation = math::min(_time_latest_us, timestamp_observation); @@ -315,12 +314,20 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl pos_correction = _state.vel * dt_s; } - { - const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy(); + LatLonAlt gpos(latitude, longitude, altitude); + bool alt_valid = true; + if (!checkAltitudeValidity(gpos.altitude())) { + gpos.setAltitude(_gpos.altitude()); + alt_valid = false; + } + + const LatLonAlt gpos_corrected = gpos + pos_correction; + + { const float obs_var = math::max(sq(eph), sq(0.01f)); - const Vector2f innov = Vector2f(_state.pos.xy()) - hpos; + const Vector2f innov = (_gpos - gpos_corrected).xy(); const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; const float sq_gate = sq(5.f); // magic hardcoded gate @@ -334,8 +341,8 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl ECL_INFO("reset position to external observation"); _information_events.flags.reset_pos_to_ext_obs = true; - resetHorizontalPositionTo(hpos, obs_var); - _last_known_pos.xy() = _state.pos.xy(); + resetHorizontalPositionTo(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var); + _last_known_gpos.setLatLon(gpos_corrected); } else { ECL_INFO("fuse external observation as position measurement"); @@ -348,24 +355,16 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl _state_reset_status.posNE_change.zero(); _time_last_hor_pos_fuse = _time_delayed_us; - _last_known_pos.xy() = _state.pos.xy(); + _last_known_gpos.setLatLon(gpos_corrected); } } - if (checkAltitudeValidity(altitude)) { - const float altitude_corrected = altitude - pos_correction(2); + if (alt_valid) { + const float obs_var = math::max(sq(epv), sq(0.01f)); - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(altitude_corrected, epv); - - } else { - const float vpos = -(altitude_corrected - _gps_alt_ref); - const float obs_var = math::max(sq(epv), sq(0.01f)); - - ECL_INFO("reset height to external observation"); - resetVerticalPositionTo(vpos, obs_var); - _last_known_pos(2) = _state.pos(2); - } + ECL_INFO("reset height to external observation"); + initialiseAltitudeTo(gpos_corrected.altitude(), obs_var); + _last_known_gpos.setAltitude(gpos_corrected.altitude()); } return true; @@ -425,9 +424,10 @@ void Ekf::print_status() (double)getStateVariance()(2) ); + const Vector3f position = getPosition(); printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::pos.idx, State::pos.idx + State::pos.dof - 1, - (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2), + (double)position(0), (double)position(1), (double) position(2), (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) ); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 35c564157f..47320e8c12 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -99,11 +99,11 @@ public: #if defined(CONFIG_EKF2_TERRAIN) // terrain estimate - bool isTerrainEstimateValid() const; + bool isTerrainEstimateValid() const { return _terrain_valid; } // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const { return _state.terrain; }; - float getHagl() const { return _state.terrain - _state.pos(2); } + float getTerrainVertPos() const { return _state.terrain + getEkfGlobalOriginAltitude(); }; + float getHagl() const { return _state.terrain + _gpos.altitude(); } // get the terrain variance float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); } @@ -191,9 +191,9 @@ public: void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool checkLatLonValidity(double latitude, double longitude); bool checkAltitudeValidity(float altitude); - bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN); - bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN, - float epv = NAN); + bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float hpos_var = NAN, float vpos_var = NAN); + bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float hpos_var = NAN, + float vpos_var = 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; @@ -208,8 +208,9 @@ public: // vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. // vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. // hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. - // hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. - void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; + // hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed. + // hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed. + void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const; void resetGyroBias(); void resetGyroBiasCov(); @@ -217,16 +218,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 global_position_is_valid() const + bool isGlobalHorizontalPositionValid() const { - return (_pos_ref.isInitialized() && local_position_is_valid()); + return _local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid(); } - // return true if the local position estimate is valid - bool local_position_is_valid() const + bool isGlobalVerticalPositionValid() const + { + return PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid(); + } + + bool isLocalHorizontalPositionValid() const { return !_horizontal_deadreckon_time_exceeded; } @@ -371,8 +373,6 @@ public: #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - void collect_gps(const gnssSample &gps); - // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } @@ -469,6 +469,8 @@ private: StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + LatLonAlt _gpos{0.0, 0.0, 0.f}; + bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) @@ -482,14 +484,20 @@ private: uint64_t _time_last_heading_fuse{0}; uint64_t _time_last_terrain_fuse{0}; - Vector3f _last_known_pos{}; ///< last known local position vector (m) + LatLonAlt _last_known_gpos{}; - Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s + Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s + double _earth_rate_lat_ref_rad{0.0}; ///< latitude at which the earth rate was evaluated (radians) Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) - float _height_rate_lpf{0.0f}; + static constexpr float _kAccelHorizLpfTimeConstant = 1.f; + AlphaFilter _accel_horiz_lpf{_kAccelHorizLpfTimeConstant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) + +#if defined(CONFIG_EKF2_WIND) + static constexpr float _kHeightRateLpfTimeConstant = 10.f; + AlphaFilter _height_rate_lpf{_kHeightRateLpfTimeConstant}; +#endif // CONFIG_EKF2_WIND SquareMatrixState P{}; ///< state covariance matrix @@ -500,6 +508,8 @@ private: #if defined(CONFIG_EKF2_TERRAIN) // Terrain height state estimation float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) + + bool _terrain_valid{false}; #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -514,7 +524,7 @@ private: Vector3f _ref_body_rate{}; Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) - AlphaFilter _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) + AlphaFilter _flow_vel_body_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) uint32_t _flow_counter{0}; ///< number of flow samples read for initialization Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive @@ -579,29 +589,29 @@ private: // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; - AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) - AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) + static constexpr float _kSensorLpfTimeConstant = 0.09f; + AlphaFilter _accel_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilter _gyro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) #if defined(CONFIG_EKF2_BAROMETER) estimator_aid_source1d_s _aid_src_baro_hgt {}; // Variables used to perform in flight resets and switch between height sources - AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) + AlphaFilter _baro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered barometric height measurement (m) uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; - bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_MAGNETOMETER) // used by magnetometer fusion mode selection - AlphaFilter _mag_heading_innov_lpf{0.1f}; + AlphaFilter _mag_heading_innov_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time estimator_aid_source3d_s _aid_src_mag{}; - AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) + AlphaFilter _mag_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered magnetometer measurement for instant reset (Gauss) uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation // Variables used to control activation of post takeoff functionality @@ -639,11 +649,11 @@ private: P.slice(S.idx, S.idx) = cov; } - bool setLatLonOrigin(double latitude, double longitude, float eph = NAN); - bool setAltOrigin(float altitude, float epv = NAN); + bool setLatLonOrigin(double latitude, double longitude, float hpos_var = NAN); + bool setAltOrigin(float altitude, float vpos_var = NAN); - bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN); - bool setAltOriginFromCurrentPos(float altitude, float epv = NAN); + bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN); + bool initialiseAltitudeTo(float altitude, float vpos_var = NAN); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); @@ -705,14 +715,22 @@ private: void resetHorizontalPositionToLastKnown(); - void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); - void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } + void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, + const Vector2f &new_horz_pos_var); + void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, const float pos_var = NAN) { resetHorizontalPositionTo(new_latitude, new_longitude, Vector2f(pos_var, pos_var)); } + void resetHorizontalPositionTo(const Vector2f &new_pos, const Vector2f &new_horz_pos_var); + + Vector2f getLocalHorizontalPosition() const; + + Vector2f computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const; + void updateHorizontalPositionResetStatus(const Vector2f &delta); void resetWindTo(const Vector2f &wind, const Vector2f &wind_var); bool isHeightResetRequired() const; - void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); + void resetAltitudeTo(float new_altitude, float new_vert_pos_var = NAN); + void updateVerticalPositionResetStatus(const float delta_z); void resetVerticalVelocityToZero(); @@ -733,6 +751,9 @@ private: float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; } 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 bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain); @@ -824,6 +845,7 @@ private: void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); + void stopEvPosFusion(); void stopEvHgtFusion(); void stopEvVelFusion(); @@ -995,25 +1017,13 @@ private: #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref}; - AlphaFilter _ev_q_error_filt{0.001f}; + static constexpr float _kQuatErrorLpfTimeConstant = 10.f; + AlphaFilter _ev_q_error_filt{_dt_ekf_avg, _kQuatErrorLpfTimeConstant}; bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) - void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const - { - status.time_last_fuse = _time_delayed_us; - - status.innovation = 0.f; - status.innovation_filtered = 0.f; - status.innovation_variance = status.observation_variance; - - status.test_ratio = 0.f; - status.test_ratio_filtered = 0.f; - - status.innovation_rejected = false; - status.fused = true; - } + void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const; // helper used for populating and filtering estimator aid source struct for logging void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, @@ -1041,9 +1051,9 @@ private: } // helper used for populating and filtering estimator aid source struct for logging - template + template void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample, - const S &observation, const S &observation_variance, + const D &observation, const S &observation_variance, const S &innovation, const S &innovation_variance, float innovation_gate = 1.f) const { @@ -1098,7 +1108,7 @@ private: status.test_ratio[i] = test_ratio; - status.observation[i] = observation(i); + status.observation[i] = static_cast(observation(i)); status.observation_variance[i] = observation_variance(i); status.innovation[i] = innovation(i); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 26757aacbc..0d8fac2fb5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -65,9 +65,9 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { - origin_time = _pos_ref.getProjectionReferenceTimestamp(); - latitude = _pos_ref.getProjectionReferenceLat(); - longitude = _pos_ref.getProjectionReferenceLon(); + origin_time = _local_origin_lat_lon.getProjectionReferenceTimestamp(); + latitude = _local_origin_lat_lon.getProjectionReferenceLat(); + longitude = _local_origin_lat_lon.getProjectionReferenceLon(); origin_alt = getEkfGlobalOriginAltitude(); } @@ -85,156 +85,170 @@ bool Ekf::checkAltitudeValidity(const float altitude) return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f))); } -bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, - const float epv) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float hpos_var, + const float vpos_var) { - if (!setLatLonOrigin(latitude, longitude, eph)) { + if (!setLatLonOrigin(latitude, longitude, hpos_var)) { return false; } // altitude is optional - setAltOrigin(altitude, epv); + setAltOrigin(altitude, vpos_var); return true; } -bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph) +bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } - bool current_pos_available = false; - double current_lat = static_cast(NAN); - double current_lon = static_cast(NAN); + if (!_local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid()) { + // Already navigating in a local frame, use the origin to initialize global position + const Vector2f pos_prev = getLocalHorizontalPosition(); + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + double new_latitude; + double new_longitude; + _local_origin_lat_lon.reproject(pos_prev(0), pos_prev(1), new_latitude, new_longitude); + resetHorizontalPositionTo(new_latitude, new_longitude, hpos_var); - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && local_position_is_valid()) { - _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); - current_pos_available = true; - } - - // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _time_delayed_us); - - if (PX4_ISFINITE(eph) && (eph >= 0.f)) { - _gpos_origin_eph = eph; - } - - if (current_pos_available) { - // reset horizontal position if we already have a global origin - Vector2f position = _pos_ref.project(current_lat, current_lon); - resetHorizontalPositionTo(position); + } else { + // Simply move the origin and compute the change in local position + const Vector2f pos_prev = getLocalHorizontalPosition(); + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + const Vector2f pos_new = getLocalHorizontalPosition(); + const Vector2f delta_pos = pos_new - pos_prev; + updateHorizontalPositionResetStatus(delta_pos); } return true; } -bool Ekf::setAltOrigin(const float altitude, const float epv) +bool Ekf::setAltOrigin(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } - const float gps_alt_ref_prev = _gps_alt_ref; - _gps_alt_ref = altitude; + ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt, + (double)altitude); - if (PX4_ISFINITE(epv) && (epv >= 0.f)) { - _gpos_origin_epv = epv; - } + if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) { + const float local_alt_prev = _gpos.altitude(); + _local_origin_alt = altitude; + resetAltitudeTo(local_alt_prev + _local_origin_alt); - if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) { - // determine current z - const float z_prev = _state.pos(2); - const float current_alt = -z_prev + gps_alt_ref_prev; -#if defined(CONFIG_EKF2_GNSS) - const float gps_hgt_bias = _gps_hgt_b_est.getBias(); -#endif // CONFIG_EKF2_GNSS - resetVerticalPositionTo(_gps_alt_ref - current_alt); - ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, - (double)_state.pos(2)); -#if defined(CONFIG_EKF2_GNSS) - // adjust existing GPS height bias - _gps_hgt_b_est.setBias(gps_hgt_bias); -#endif // CONFIG_EKF2_GNSS + } else { + const float delta_origin_alt = altitude - _local_origin_alt; + _local_origin_alt = altitude; + updateVerticalPositionResetStatus(-delta_origin_alt); + +#if defined(CONFIG_EKF2_TERRAIN) + updateTerrainResetStatus(-delta_origin_alt); +#endif // CONFIG_EKF2_TERRAIN } return true; } -bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude, - const float eph, const float epv) +bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude, + const float hpos_var, const float vpos_var) { - if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + if (!resetLatLonTo(latitude, longitude, hpos_var)) { return false; } // altitude is optional - setAltOriginFromCurrentPos(altitude, epv); + initialiseAltitudeTo(altitude, vpos_var); return true; } -bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph) +bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } - _pos_ref.initReference(latitude, longitude, _time_delayed_us); + Vector2f pos_prev; - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (local_position_is_valid()) { - double est_lat; - double est_lon; - _pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon); - _pos_ref.initReference(est_lat, est_lon, _time_delayed_us); + if (!_local_origin_lat_lon.isInitialized()) { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + pos_prev = zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (isLocalHorizontalPositionValid()) { + double est_lat; + double est_lon; + _local_origin_lat_lon.reproject(-pos_prev(0), -pos_prev(1), est_lat, est_lon); + _local_origin_lat_lon.initReference(est_lat, est_lon, _time_delayed_us); + } + + ECL_INFO("Origin set to lat=%.6f, lon=%.6f", + _local_origin_lat_lon.getProjectionReferenceLat(), _local_origin_lat_lon.getProjectionReferenceLon()); + + } else { + pos_prev = _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); } - if (PX4_ISFINITE(eph) && (eph >= 0.f)) { - _gpos_origin_eph = eph; + _gpos.setLatLonDeg(latitude, longitude); + _output_predictor.resetLatLonTo(latitude, longitude); + + const Vector2f delta_horz_pos = getLocalHorizontalPosition() - pos_prev; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); +#endif // CONFIG_EKF2_EXTERNAL_VISION + + updateHorizontalPositionResetStatus(delta_horz_pos); + + if (PX4_ISFINITE(hpos_var)) { + P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, math::max(sq(0.01f), hpos_var)); } + // Reset the timout timer + _time_last_hor_pos_fuse = _time_delayed_us; + return true; } -bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv) +bool Ekf::initialiseAltitudeTo(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } - _gps_alt_ref = altitude + _state.pos(2); + if (!PX4_ISFINITE(_local_origin_alt)) { + const float local_alt_prev = _gpos.altitude(); - if (PX4_ISFINITE(epv) && (epv >= 0.f)) { - _gpos_origin_epv = epv; + if (isLocalVerticalPositionValid()) { + _local_origin_alt = altitude - local_alt_prev; + + } else { + _local_origin_alt = altitude; + } + + ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt); } + resetAltitudeTo(altitude, vpos_var); + return true; } void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { - float eph = INFINITY; - float epv = INFINITY; - if (global_origin_valid()) { - // report absolute accuracy taking into account the uncertainty in location of the origin - eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph)); - epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv)); + get_ekf_lpos_accuracy(ekf_eph, ekf_epv); - if (_horizontal_deadreckon_time_exceeded) { - float lpos_eph = 0.f; - float lpos_epv = 0.f; - get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv); - - eph = math::max(eph, lpos_eph); - epv = math::max(epv, lpos_epv); - } + } else { + *ekf_eph = INFINITY; + *ekf_epv = INFINITY; } - - *ekf_eph = eph; - *ekf_epv = epv; } void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const @@ -313,13 +327,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } -void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const +void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, + float *hagl_max_xy) const { // Do not require limiting by default *vxy_max = NAN; *vz_max = NAN; *hagl_min = NAN; - *hagl_max = NAN; + *hagl_max_z = NAN; + *hagl_max_xy = NAN; #if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits @@ -331,10 +347,9 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); - // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; - *hagl_max = rangefinder_hagl_max; + *hagl_max_z = rangefinder_hagl_max; } # if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -357,11 +372,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max); // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f); *vxy_max = flow_vxy_max; *hagl_min = flow_hagl_min; - *hagl_max = flow_hagl_max; + *hagl_max_xy = flow_hagl_max; } # endif // CONFIG_EKF2_OPTICAL_FLOW @@ -671,16 +687,16 @@ uint16_t Ekf::get_ekf_soln_status() const soln_status.flags.attitude = attitude_valid(); // 2 ESTIMATOR_VELOCITY_HORIZ True if the horizontal velocity estimate is good - soln_status.flags.velocity_horiz = local_position_is_valid(); + soln_status.flags.velocity_horiz = isLocalHorizontalPositionValid(); // 4 ESTIMATOR_VELOCITY_VERT True if the vertical velocity estimate is good soln_status.flags.velocity_vert = isLocalVerticalVelocityValid() || isLocalVerticalPositionValid(); // 8 ESTIMATOR_POS_HORIZ_REL True if the horizontal position (relative) estimate is good - soln_status.flags.pos_horiz_rel = local_position_is_valid(); + soln_status.flags.pos_horiz_rel = isLocalHorizontalPositionValid(); // 16 ESTIMATOR_POS_HORIZ_ABS True if the horizontal position (absolute) estimate is good - soln_status.flags.pos_horiz_abs = global_position_is_valid(); + soln_status.flags.pos_horiz_abs = isGlobalHorizontalPositionValid(); // 32 ESTIMATOR_POS_VERT_ABS True if the vertical position (absolute) estimate is good soln_status.flags.pos_vert_abs = isVerticalAidingActive(); @@ -726,7 +742,13 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.vel = matrix::constrain(_state.vel - K.slice(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f); // pos - _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); + const Vector3f pos_correction = K.slice(State::pos.idx, 0) * (-innovation); + + // Accumulate position in global coordinates + _gpos += pos_correction; + _state.pos.zero(); + // Also store altitude in the state vector as this is used for optical flow fusion + _state.pos(2) = -_gpos.altitude(); // gyro_bias _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, @@ -1111,6 +1133,21 @@ bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R, return true; } +void Ekf::resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const +{ + status.time_last_fuse = _time_delayed_us; + + status.innovation = 0.f; + status.innovation_filtered = 0.f; + status.innovation_variance = status.observation_variance; + + status.test_ratio = 0.f; + status.test_ratio_filtered = 0.f; + + status.innovation_rejected = false; + status.fused = true; +} + void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, const float &observation, const float &observation_variance, const float &innovation, const float &innovation_variance, diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index f7a6c70dec..22ccf958f0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -568,6 +568,26 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) return true; } +Vector3f EstimatorInterface::getPosition() const +{ + LatLonAlt lla = _output_predictor.getLatLonAlt(); + float x; + float y; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + } + + const float z = -(lla.altitude() - getEkfGlobalOriginAltitude()); + + return Vector3f(x, y, z); +} + bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const { return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag); @@ -580,12 +600,32 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f } int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +{ + return getNumberOfActiveHorizontalPositionAidingSources() + getNumberOfActiveHorizontalVelocityAidingSources(); +} + +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalPositionAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalPositionAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalPositionAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const { return int(_control_status.flags.gps) - + int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_pos) + + int(_control_status.flags.aux_gpos); +} + +int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const +{ + return int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_vel) - + int(_control_status.flags.aux_gpos) // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning // and so is treated as a single aiding source. + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index a188d2f88e..93632bc109 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -72,6 +72,7 @@ #endif // CONFIG_EKF2_RANGE_FINDER #include +#include #include #include #include @@ -206,8 +207,8 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) { _air_density = air_density; } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; + bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const; /* * Check if there are any other active source of horizontal aiding @@ -220,6 +221,7 @@ public: * @return true if an other source than aiding_flag is active */ bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const; // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos @@ -227,6 +229,8 @@ public: bool isVerticalAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; + int getNumberOfActiveHorizontalPositionAidingSources() const; + int getNumberOfActiveHorizontalVelocityAidingSources() const; bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; bool isVerticalPositionAidingActive() const; @@ -239,9 +243,13 @@ public: const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } Vector3f getVelocity() const { return _output_predictor.getVelocity(); } - const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } + + // get the mean velocity derivative in earth frame since last reset (see `resetVelocityDerivativeAccumulation()`) + Vector3f getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } + void resetVelocityDerivativeAccumulation() { return _output_predictor.resetVelocityDerivativeAccumulation(); } float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } - Vector3f getPosition() const { return _output_predictor.getPosition(); } + Vector3f getPosition() const; + LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -307,9 +315,9 @@ public: const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } - bool global_origin_valid() const { return _pos_ref.isInitialized(); } - const MapProjection &global_origin() const { return _pos_ref; } - float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } + bool global_origin_valid() const { return _local_origin_lat_lon.isInitialized(); } + const MapProjection &global_origin() const { return _local_origin_lat_lon; } + float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_local_origin_alt) ? _local_origin_alt : 0.f; } OutputPredictor &output_predictor() { return _output_predictor; }; @@ -379,10 +387,8 @@ protected: bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized // Variables used to publish the WGS-84 location of the EKF local NED origin - MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin - float _gps_alt_ref{NAN}; ///< WGS-84 height (m) - float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin - float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin + MapProjection _local_origin_lat_lon{}; + float _local_origin_alt{NAN}; #if defined(CONFIG_EKF2_GNSS) RingBuffer *_gps_buffer {nullptr}; diff --git a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt index 701a5cb13f..8d7b3e0607 100644 --- a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt +++ b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt @@ -36,4 +36,4 @@ add_library(output_predictor output_predictor.h ) -add_dependencies(output_predictor prebuild_targets) +add_dependencies(output_predictor prebuild_targets lat_lon_alt) diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index d16e20a790..05c452b05d 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -69,7 +69,7 @@ void OutputPredictor::print_status() _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); } -void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) +void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state) { const outputSample &output_delayed = _output_buffer.get_oldest(); @@ -77,9 +77,12 @@ void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()}; q_delta.normalize(); - // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon + // calculate the velocity delta between the output and EKF at the EKF fusion time horizon const Vector3f vel_delta = vel_state - output_delayed.vel; - const Vector3f pos_delta = pos_state - output_delayed.pos; + + // zero the position error at delayed time and reset the global reference + const Vector3f pos_delta = -output_delayed.pos; + _global_ref = gpos_state; // loop through the output filter state history and add the deltas for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { @@ -106,7 +109,8 @@ void OutputPredictor::reset() _R_to_earth_now.setIdentity(); _vel_imu_rel_body_ned.setZero(); - _vel_deriv.setZero(); + _delta_vel_sum.setZero(); + _delta_vel_dt = 0.f; _delta_angle_corr.setZero(); @@ -156,29 +160,15 @@ void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel) _output_vert_new.vert_vel += delta_vert_vel; } -void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos) +void OutputPredictor::resetLatLonTo(const double &new_latitude, const double &new_longitude) { - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].pos.xy() += delta_horz_pos; - } - - _output_new.pos.xy() += delta_horz_pos; + _global_ref.setLatitudeDeg(new_latitude); + _global_ref.setLongitudeDeg(new_longitude); } -void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change) +void OutputPredictor::resetAltitudeTo(const float new_altitude, const float vert_pos_change) { - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - _output_new.pos(2) += vert_pos_change; - - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].pos(2) += vert_pos_change; - _output_vert_buffer[i].vert_vel_integ += vert_pos_change; - } - - // add the reset amount to the output observer vertical position state - _output_vert_new.vert_vel_integ = new_vert_pos; + _global_ref.setAltitude(new_altitude); } void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle, @@ -221,9 +211,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector delta_vel_earth(2) += CONSTANTS_ONE_G * delta_velocity_dt; // calculate the earth frame velocity derivatives - if (delta_velocity_dt > 0.001f) { - _vel_deriv = delta_vel_earth / delta_velocity_dt; - } + _delta_vel_sum += delta_vel_earth; + _delta_vel_dt += delta_velocity_dt; // save the previous velocity so we can use trapezoidal integration const Vector3f vel_last(_output_new.vel); @@ -261,7 +250,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, + const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) { // calculate an average filter update time @@ -317,6 +306,8 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); + const Vector3f pos_state = gpos_state - _global_ref; + // calculate down velocity and position tracking errors const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel); const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ); @@ -325,7 +316,7 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, // using a PD feedback tuned to a 5% overshoot const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f; - applyCorrectionToVerticalOutputBuffer(vert_vel_correction); + applyCorrectionToVerticalOutputBuffer(vert_vel_correction, pos_state(2)); // calculate velocity and position tracking errors const Vector3f vel_err(vel_state - output_delayed.vel); @@ -342,10 +333,14 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, _pos_err_integ += pos_err; const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; - applyCorrectionToOutputBuffer(vel_correction, pos_correction); + // as the reference changes, adjust the position correction to keep a constant global position + const Vector3f pos_correction_with_ref_change = pos_correction - pos_state; + applyCorrectionToOutputBuffer(vel_correction, pos_correction_with_ref_change); + + _global_ref = gpos_state; } -void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction) +void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel_correction, const float pos_ref_change) { // loop through the vertical output filter state history starting at the oldest and apply the corrections to the // vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel @@ -367,7 +362,7 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre // position is propagated forward using the corrected velocity and a trapezoidal integrator next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * - next_state.dt; + next_state.dt - pos_ref_change; // advance the index index = (index + 1) % size; @@ -394,3 +389,19 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti // update output state to corrected values _output_new = _output_buffer.get_newest(); } + +matrix::Vector3f OutputPredictor::getVelocityDerivative() const +{ + if (_delta_vel_dt > FLT_EPSILON) { + return _delta_vel_sum / _delta_vel_dt; + + } else { + return matrix::Vector3f(0.f, 0.f, 0.f); + } +} + +void OutputPredictor::resetVelocityDerivativeAccumulation() +{ + _delta_vel_dt = 0.f; + _delta_vel_sum.setZero(); +} diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 07248f3dce..e90bdf9481 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -39,6 +39,7 @@ #include "../RingBuffer.h" #include +#include class OutputPredictor { @@ -52,7 +53,7 @@ public: // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, - const matrix::Vector3f &pos_state); + const LatLonAlt &gpos_state); /* * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. @@ -67,7 +68,7 @@ public: const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); void correctOutputStates(const uint64_t time_delayed_us, - const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, + const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); void resetQuaternion(const matrix::Quatf &quat_change); @@ -75,8 +76,8 @@ public: void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel); void resetVerticalVelocityTo(float delta_vert_vel); - void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos); - void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change); + void resetLatLonTo(const double &new_latitude, const double &new_longitude); + void resetAltitudeTo(float new_altitude, float vert_pos_change); void print_status(); @@ -100,19 +101,20 @@ public: // get the velocity of the body frame origin in local NED earth frame matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } - // get the velocity derivative in earth frame - const matrix::Vector3f &getVelocityDerivative() const { return _vel_deriv; } + // get the mean velocity derivative in earth frame since reset (see `resetVelocityDerivativeAccumulation()`) + matrix::Vector3f getVelocityDerivative() const; + + void resetVelocityDerivativeAccumulation(); // get the derivative of the vertical position of the body frame origin in local NED earth frame float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } - // get the position of the body frame origin in local earth frame - matrix::Vector3f getPosition() const + LatLonAlt getLatLonAlt() const { // rotate the position of the IMU relative to the boy origin into earth frame const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body}; // subtract from the EKF position (which is at the IMU) to get position at the body origin - return _output_new.pos - pos_offset_earth; + return _global_ref + (_output_new.pos - pos_offset_earth); } // return an array containing the output predictor angular, velocity and position tracking @@ -133,7 +135,7 @@ private: * This provides an alternative vertical velocity output that is closer to the first derivative * of the position but does degrade tracking relative to the EKF state. */ - void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); + void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction, const float pos_ref_change); /* * Calculate corrections to be applied to vel and pos output state history. @@ -159,6 +161,8 @@ private: float dt{0.f}; ///< delta time (sec) }; + LatLonAlt _global_ref{0.0, 0.0, 0.f}; + RingBuffer _output_buffer{12}; RingBuffer _output_vert_buffer{12}; @@ -176,7 +180,8 @@ private: outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon matrix::Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time matrix::Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame - matrix::Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s) + matrix::Vector3f _delta_vel_sum{}; // accumulation of delta velocity at the IMU in NED earth frame (m/s/s) + float _delta_vel_dt{}; // duration of delta velocity integration (s) // output predictor states matrix::Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 9008045842..7d874e6540 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -36,7 +36,7 @@ void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate) const { - float innovation = _state.pos(2) - observation; + float innovation = -_gpos.altitude() - observation; float innovation_variance = getStateVariance()(2) + observation_variance; updateAidSourceStatus(aid_src, time_us, @@ -93,10 +93,20 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) return aid_src.fused; } -void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) +void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, + const Vector2f &new_horz_pos_var) { - const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; - _state.pos.xy() = new_horz_pos; + const Vector2f delta_horz_pos = computeDeltaHorizontalPosition(new_latitude, new_longitude); + + updateHorizontalPositionResetStatus(delta_horz_pos); + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); +#endif // CONFIG_EKF2_EXTERNAL_VISION + //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); + + _gpos.setLatLonDeg(new_latitude, new_longitude); + _output_predictor.resetLatLonTo(new_latitude, new_longitude); if (PX4_ISFINITE(new_horz_pos_var(0))) { P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); @@ -106,54 +116,89 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } - _output_predictor.resetHorizontalPositionTo(delta_horz_pos); - - // record the state change - if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { - _state_reset_status.posNE_change = delta_horz_pos; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posNE_change += delta_horz_pos; - } - - _state_reset_status.reset_count.posNE++; - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); -#endif // CONFIG_EKF2_EXTERNAL_VISION - //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); - // Reset the timout timer _time_last_hor_pos_fuse = _time_delayed_us; } -void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) +Vector2f Ekf::computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const { - const float old_vert_pos = _state.pos(2); - _state.pos(2) = new_vert_pos; + Vector2f pos; + Vector2f pos_new; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1)); + _local_origin_lat_lon.project(new_latitude, new_longitude, pos_new(0), pos_new(1)); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1)); + zero_ref.project(new_latitude, new_longitude, pos_new(0), pos_new(1)); + } + + return pos_new - pos; +} + +Vector2f Ekf::getLocalHorizontalPosition() const +{ + if (_local_origin_lat_lon.isInitialized()) { + return _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + return zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + } +} + +void Ekf::updateHorizontalPositionResetStatus(const Vector2f &delta) +{ + if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { + _state_reset_status.posNE_change = delta; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posNE_change += delta; + } + + _state_reset_status.reset_count.posNE++; +} + +void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos, + const Vector2f &new_horz_pos_var) +{ + double new_latitude; + double new_longitude; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude); + } + + resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var); +} + +void Ekf::resetAltitudeTo(const float new_altitude, float new_vert_pos_var) +{ + const float old_altitude = _gpos.altitude(); + _gpos.setAltitude(new_altitude); if (PX4_ISFINITE(new_vert_pos_var)) { // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); } - const float delta_z = new_vert_pos - old_vert_pos; + const float delta_z = -(new_altitude - old_altitude); // apply the change in height / height rate to our newest height / height rate estimate // which have already been taken out from the output buffer - _output_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); + _output_predictor.resetAltitudeTo(new_altitude, delta_z); - // record the state change - if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { - _state_reset_status.posD_change = delta_z; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posD_change += delta_z; - } - - _state_reset_status.reset_count.posD++; + updateVerticalPositionResetStatus(delta_z); #if defined(CONFIG_EKF2_BAROMETER) _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); @@ -166,9 +211,29 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_TERRAIN) + updateTerrainResetStatus(delta_z); _state.terrain += delta_z; +#endif // CONFIG_EKF2_TERRAIN - // record the state change + // Reset the timout timer + _time_last_hgt_fuse = _time_delayed_us; +} + +void Ekf::updateVerticalPositionResetStatus(const float delta_z) +{ + if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { + _state_reset_status.posD_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posD_change += delta_z; + } + + _state_reset_status.reset_count.posD++; +} + +void Ekf::updateTerrainResetStatus(const float delta_z) +{ if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { _state_reset_status.hagl_change = delta_z; @@ -178,17 +243,15 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v } _state_reset_status.reset_count.hagl++; -#endif // CONFIG_EKF2_TERRAIN - - // Reset the timout timer - _time_last_hgt_fuse = _time_delayed_us; } void Ekf::resetHorizontalPositionToLastKnown() { - ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); + ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_gpos.latitude_deg(), + (double)_last_known_gpos.longitude_deg()); _information_events.flags.reset_pos_to_last_known = true; // Used when falling back to non-aiding mode of operation - resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); + resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(), + sq(_params.pos_noaid_noise)); } diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 36e2f34373..adbc761ae9 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -268,10 +268,8 @@ def compute_airspeed_innov_and_innov_var( return (innov, innov_var) -def compute_airspeed_h_and_k( +def compute_airspeed_h( state: VState, - P: MTangent, - innov_var: sf.Scalar, epsilon: sf.Scalar ) -> (VTangent, VTangent): @@ -281,9 +279,7 @@ def compute_airspeed_h_and_k( airspeed_pred = vel_rel.norm(epsilon=epsilon) H = jacobian_chain_rule(airspeed_pred, state) - K = P * H.T / sf.Max(innov_var, epsilon) - - return (H.T, K) + return H.T def compute_wind_init_and_cov_from_airspeed( v_local: sf.V3, @@ -337,9 +333,7 @@ def predict_sideslip( vel_rel = state["vel"] - wind relative_wind_body = state["quat_nominal"].inverse() * vel_rel - # 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) + sideslip_pred = sf.atan2(relative_wind_body[1], relative_wind_body[0], epsilon) return sideslip_pred @@ -360,10 +354,8 @@ def compute_sideslip_innov_and_innov_var( return (innov, innov_var) -def compute_sideslip_h_and_k( +def compute_sideslip_h( state: VState, - P: MTangent, - innov_var: sf.Scalar, epsilon: sf.Scalar ) -> (VTangent, VTangent): @@ -372,9 +364,7 @@ def compute_sideslip_h_and_k( H = jacobian_chain_rule(sideslip_pred, state) - K = P * H.T / sf.Max(innov_var, epsilon) - - return (H.T, K) + return (H.T) def predict_vel_body( state: VState @@ -741,11 +731,11 @@ if not args.disable_mag: generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) if not args.disable_wind: - generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_airspeed_h, output_names=None) generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"]) generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"]) - generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_sideslip_h, output_names=None) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h new file mode 100644 index 0000000000..999c01d1ee --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h.h @@ -0,0 +1,56 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_airspeed_h + * + * Args: + * state: Matrix25_1 + * epsilon: Scalar + * + * Outputs: + * res: Matrix24_1 + */ +template +matrix::Matrix ComputeAirspeedH(const matrix::Matrix& state, + const Scalar epsilon) { + // Total ops: 14 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = -state(22, 0) + state(4, 0); + const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + + epsilon + std::pow(state(6, 0), Scalar(2))), + Scalar(Scalar(-1) / Scalar(2))); + const Scalar _tmp3 = _tmp1 * _tmp2; + const Scalar _tmp4 = _tmp0 * _tmp2; + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(3, 0) = _tmp3; + _res(4, 0) = _tmp4; + _res(5, 0) = _tmp2 * state(6, 0); + _res(21, 0) = -_tmp3; + _res(22, 0) = -_tmp4; + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h deleted file mode 100644 index 52bb16353d..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h +++ /dev/null @@ -1,116 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_airspeed_h_and_k - * - * Args: - * state: Matrix25_1 - * P: Matrix24_24 - * innov_var: Scalar - * epsilon: Scalar - * - * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 - */ -template -void ComputeAirspeedHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 256 - - // Input arrays - - // Intermediate terms (7) - const Scalar _tmp0 = -state(23, 0) + state(5, 0); - const Scalar _tmp1 = -state(22, 0) + state(4, 0); - const Scalar _tmp2 = std::pow(Scalar(std::pow(_tmp0, Scalar(2)) + std::pow(_tmp1, Scalar(2)) + - epsilon + std::pow(state(6, 0), Scalar(2))), - Scalar(Scalar(-1) / Scalar(2))); - const Scalar _tmp3 = _tmp1 * _tmp2; - const Scalar _tmp4 = _tmp0 * _tmp2; - const Scalar _tmp5 = _tmp2 * state(6, 0); - const Scalar _tmp6 = Scalar(1.0) / (math::max(epsilon, innov_var)); - - // Output terms (2) - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(3, 0) = _tmp3; - _h(4, 0) = _tmp4; - _h(5, 0) = _tmp5; - _h(21, 0) = -_tmp3; - _h(22, 0) = -_tmp4; - } - - if (K != nullptr) { - matrix::Matrix& _k = (*K); - - _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 + - P(0, 5) * _tmp5); - _k(1, 0) = _tmp6 * (-P(1, 21) * _tmp3 - P(1, 22) * _tmp4 + P(1, 3) * _tmp3 + P(1, 4) * _tmp4 + - P(1, 5) * _tmp5); - _k(2, 0) = _tmp6 * (-P(2, 21) * _tmp3 - P(2, 22) * _tmp4 + P(2, 3) * _tmp3 + P(2, 4) * _tmp4 + - P(2, 5) * _tmp5); - _k(3, 0) = _tmp6 * (-P(3, 21) * _tmp3 - P(3, 22) * _tmp4 + P(3, 3) * _tmp3 + P(3, 4) * _tmp4 + - P(3, 5) * _tmp5); - _k(4, 0) = _tmp6 * (-P(4, 21) * _tmp3 - P(4, 22) * _tmp4 + P(4, 3) * _tmp3 + P(4, 4) * _tmp4 + - P(4, 5) * _tmp5); - _k(5, 0) = _tmp6 * (-P(5, 21) * _tmp3 - P(5, 22) * _tmp4 + P(5, 3) * _tmp3 + P(5, 4) * _tmp4 + - P(5, 5) * _tmp5); - _k(6, 0) = _tmp6 * (-P(6, 21) * _tmp3 - P(6, 22) * _tmp4 + P(6, 3) * _tmp3 + P(6, 4) * _tmp4 + - P(6, 5) * _tmp5); - _k(7, 0) = _tmp6 * (-P(7, 21) * _tmp3 - P(7, 22) * _tmp4 + P(7, 3) * _tmp3 + P(7, 4) * _tmp4 + - P(7, 5) * _tmp5); - _k(8, 0) = _tmp6 * (-P(8, 21) * _tmp3 - P(8, 22) * _tmp4 + P(8, 3) * _tmp3 + P(8, 4) * _tmp4 + - P(8, 5) * _tmp5); - _k(9, 0) = _tmp6 * (-P(9, 21) * _tmp3 - P(9, 22) * _tmp4 + P(9, 3) * _tmp3 + P(9, 4) * _tmp4 + - P(9, 5) * _tmp5); - _k(10, 0) = _tmp6 * (-P(10, 21) * _tmp3 - P(10, 22) * _tmp4 + P(10, 3) * _tmp3 + - P(10, 4) * _tmp4 + P(10, 5) * _tmp5); - _k(11, 0) = _tmp6 * (-P(11, 21) * _tmp3 - P(11, 22) * _tmp4 + P(11, 3) * _tmp3 + - P(11, 4) * _tmp4 + P(11, 5) * _tmp5); - _k(12, 0) = _tmp6 * (-P(12, 21) * _tmp3 - P(12, 22) * _tmp4 + P(12, 3) * _tmp3 + - P(12, 4) * _tmp4 + P(12, 5) * _tmp5); - _k(13, 0) = _tmp6 * (-P(13, 21) * _tmp3 - P(13, 22) * _tmp4 + P(13, 3) * _tmp3 + - P(13, 4) * _tmp4 + P(13, 5) * _tmp5); - _k(14, 0) = _tmp6 * (-P(14, 21) * _tmp3 - P(14, 22) * _tmp4 + P(14, 3) * _tmp3 + - P(14, 4) * _tmp4 + P(14, 5) * _tmp5); - _k(15, 0) = _tmp6 * (-P(15, 21) * _tmp3 - P(15, 22) * _tmp4 + P(15, 3) * _tmp3 + - P(15, 4) * _tmp4 + P(15, 5) * _tmp5); - _k(16, 0) = _tmp6 * (-P(16, 21) * _tmp3 - P(16, 22) * _tmp4 + P(16, 3) * _tmp3 + - P(16, 4) * _tmp4 + P(16, 5) * _tmp5); - _k(17, 0) = _tmp6 * (-P(17, 21) * _tmp3 - P(17, 22) * _tmp4 + P(17, 3) * _tmp3 + - P(17, 4) * _tmp4 + P(17, 5) * _tmp5); - _k(18, 0) = _tmp6 * (-P(18, 21) * _tmp3 - P(18, 22) * _tmp4 + P(18, 3) * _tmp3 + - P(18, 4) * _tmp4 + P(18, 5) * _tmp5); - _k(19, 0) = _tmp6 * (-P(19, 21) * _tmp3 - P(19, 22) * _tmp4 + P(19, 3) * _tmp3 + - P(19, 4) * _tmp4 + P(19, 5) * _tmp5); - _k(20, 0) = _tmp6 * (-P(20, 21) * _tmp3 - P(20, 22) * _tmp4 + P(20, 3) * _tmp3 + - P(20, 4) * _tmp4 + P(20, 5) * _tmp5); - _k(21, 0) = _tmp6 * (-P(21, 21) * _tmp3 - P(21, 22) * _tmp4 + P(21, 3) * _tmp3 + - P(21, 4) * _tmp4 + P(21, 5) * _tmp5); - _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 + - P(22, 4) * _tmp4 + P(22, 5) * _tmp5); - _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 + - P(23, 4) * _tmp4 + P(23, 5) * _tmp5); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h new file mode 100644 index 0000000000..436b0c07e7 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h @@ -0,0 +1,96 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_sideslip_h + * + * Args: + * state: Matrix25_1 + * epsilon: Scalar + * + * Outputs: + * res: Matrix24_1 + */ +template +matrix::Matrix ComputeSideslipH(const matrix::Matrix& state, + const Scalar epsilon) { + // Total ops: 131 + + // Input arrays + + // Intermediate terms (37) + const Scalar _tmp0 = -state(22, 0) + state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(3, 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 = 2 * state(0, 0); + const Scalar _tmp7 = _tmp6 * state(3, 0); + const Scalar _tmp8 = 2 * state(2, 0); + const Scalar _tmp9 = _tmp8 * state(1, 0); + const Scalar _tmp10 = _tmp7 + _tmp9; + const Scalar _tmp11 = -state(23, 0) + state(5, 0); + const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp8 * state(0, 0); + const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp11 + _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 = _tmp2 * state(0, 0); + const Scalar _tmp17 = std::pow(_tmp14, Scalar(2)); + const Scalar _tmp18 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp19 = -_tmp7 + _tmp9; + const Scalar _tmp20 = _tmp6 * state(1, 0) + _tmp8 * state(3, 0); + const Scalar _tmp21 = _tmp0 * _tmp19 + _tmp11 * _tmp18 + _tmp20 * state(6, 0); + const Scalar _tmp22 = _tmp21 / _tmp17; + const Scalar _tmp23 = _tmp17 / (_tmp17 + std::pow(_tmp21, Scalar(2))); + const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp25 = _tmp24 * (_tmp15 * (_tmp0 * _tmp1 + _tmp3) - + _tmp22 * (-4 * _tmp0 * state(2, 0) + _tmp1 * _tmp11 - _tmp16)); + const Scalar _tmp26 = 2 * state(3, 0); + const Scalar _tmp27 = _tmp2 * state(1, 0); + const Scalar _tmp28 = _tmp2 * state(2, 0); + const Scalar _tmp29 = + _tmp24 * (_tmp15 * (-_tmp0 * _tmp26 + _tmp27) - _tmp22 * (_tmp11 * _tmp26 - _tmp28)); + const Scalar _tmp30 = _tmp24 * (_tmp15 * (_tmp0 * _tmp8 - 4 * _tmp11 * state(1, 0) + _tmp16) - + _tmp22 * (_tmp11 * _tmp8 + _tmp3)); + const Scalar _tmp31 = 4 * state(3, 0); + const Scalar _tmp32 = _tmp24 * (_tmp15 * (-_tmp0 * _tmp6 - _tmp11 * _tmp31 + _tmp28) - + _tmp22 * (-_tmp0 * _tmp31 + _tmp11 * _tmp6 + _tmp27)); + const Scalar _tmp33 = _tmp22 * _tmp5; + const Scalar _tmp34 = _tmp15 * _tmp19; + const Scalar _tmp35 = _tmp15 * _tmp18; + const Scalar _tmp36 = _tmp10 * _tmp22; + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(0, 0) = + -_tmp25 * state(3, 0) - _tmp29 * state(1, 0) + _tmp30 * state(0, 0) + _tmp32 * state(2, 0); + _res(1, 0) = + _tmp25 * state(0, 0) - _tmp29 * state(2, 0) + _tmp30 * state(3, 0) - _tmp32 * state(1, 0); + _res(2, 0) = + _tmp25 * state(1, 0) - _tmp29 * state(3, 0) - _tmp30 * state(2, 0) + _tmp32 * state(0, 0); + _res(3, 0) = _tmp23 * (-_tmp33 + _tmp34); + _res(4, 0) = _tmp23 * (_tmp35 - _tmp36); + _res(5, 0) = _tmp23 * (-_tmp12 * _tmp22 + _tmp15 * _tmp20); + _res(21, 0) = _tmp23 * (_tmp33 - _tmp34); + _res(22, 0) = _tmp23 * (-_tmp35 + _tmp36); + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h deleted file mode 100644 index 002be9bbe8..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ /dev/null @@ -1,189 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_sideslip_h_and_k - * - * Args: - * state: Matrix25_1 - * P: Matrix24_24 - * innov_var: Scalar - * epsilon: Scalar - * - * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 - */ -template -void ComputeSideslipHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 513 - - // Input arrays - - // 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(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 = - -_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(epsilon, innov_var)); - - // Output terms (2) - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _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& _k = (*K); - - _k(0, 0) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) = - _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) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h index 6918bddfc3..65a2e4df95 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -30,70 +30,69 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { - // Total ops: 265 + // Total ops: 266 // Input arrays - // Intermediate terms (42) + // Intermediate terms (49) const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); - 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 _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 _tmp5 = 2 * state(1, 0); const Scalar _tmp6 = _tmp5 * state(2, 0); - const Scalar _tmp7 = _tmp4 + _tmp6; - const Scalar _tmp8 = -state(23, 0) + state(5, 0); + const Scalar _tmp7 = -_tmp4 + _tmp6; + const Scalar _tmp8 = -state(22, 0) + state(4, 0); const Scalar _tmp9 = 2 * state(2, 0); - const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0); + const Scalar _tmp10 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0); const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8; - const Scalar _tmp12 = - _tmp11 + epsilon * (2 * math::min(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; + 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); // Output terms (2) if (innov != nullptr) { Scalar& _innov = (*innov); - _innov = _tmp13 * _tmp17; + _innov = std::atan2(_tmp11, _tmp16); } if (innov_var != nullptr) { @@ -101,22 +100,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, _innov_var = R + - _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); + _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); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py index faf431da71..97a1152431 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py @@ -91,20 +91,21 @@ def generate_python_function(function_name, output_names): def build_state_struct(state, T="float"): out = "struct StateSample {\n" - def TypeFromLength(len): - if len == 1: + def get_px4_type(obj): + if isinstance(obj, sf.M11): return f"{T}" - elif len == 2: + elif isinstance(obj, sf.M21): return f"matrix::Vector2<{T}>" - elif len == 3: + elif isinstance(obj, sf.M31): return f"matrix::Vector3<{T}>" - elif len == 4: + elif isinstance(obj, sf.Rot3): return f"matrix::Quaternion<{T}>" else: + print(f"unknown type {type(obj)}") raise NotImplementedError for key, val in state.items(): - out += f"\t{TypeFromLength(val.storage_dim())} {key}{{}};\n" + out += f"\t{get_px4_type(val)} {key}{{}};\n" state_size = state.storage_dim() out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \ diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 61356b1a41..d005022289 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -43,7 +43,7 @@ void Ekf::initTerrain() { // assume a ground clearance - _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; // use the ground clearance value as our uncertainty P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); @@ -53,7 +53,7 @@ void Ekf::controlTerrainFakeFusion() { // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { - _last_on_ground_posD = _state.pos(2); + _last_on_ground_posD = -_gpos.altitude(); _control_status.flags.rng_fault = false; } else if (!_control_status_prev.flags.in_air) { @@ -63,40 +63,72 @@ void Ekf::controlTerrainFakeFusion() initTerrain(); } - if (!_control_status.flags.in_air - && !_control_status.flags.rng_terrain - && !_control_status.flags.opt_flow_terrain) { + if (!_control_status.flags.in_air) { + bool no_terrain_aiding = !_control_status.flags.rng_terrain + && !_control_status.flags.opt_flow_terrain + && isTimedOut(_time_last_terrain_fuse, (uint64_t)1e6); - bool recent_terrain_aiding = isRecent(_time_last_terrain_fuse, (uint64_t)1e6); - - if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { + if (no_terrain_aiding && (_height_sensor_ref != HeightSensor::RANGE)) { initTerrain(); } } } -bool Ekf::isTerrainEstimateValid() const +void Ekf::updateTerrainValidity() { - bool valid = false; + bool valid_opt_flow_terrain = false; + bool valid_rng_terrain = false; + bool positive_hagl_var = false; + bool small_relative_hagl_var = false; + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (_control_status.flags.opt_flow_terrain + && isRecent(_aid_src_optical_flow.time_last_fuse, _params.hgt_fusion_timeout_max) + ) { + valid_opt_flow_terrain = true; + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_control_status.flags.rng_terrain + && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max) + ) { + valid_rng_terrain = true; + } + +#endif // CONFIG_EKF2_RANGE_FINDER if (_time_last_terrain_fuse != 0) { // Assume being valid when the uncertainty is small compared to the height above ground float hagl_var = INFINITY; sym::ComputeHaglInnovVar(P, 0.f, &hagl_var); - if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) { - valid = true; + positive_hagl_var = hagl_var > 0.f; + + if (positive_hagl_var + && (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f))) + ) { + small_relative_hagl_var = true; } } -#if defined(CONFIG_EKF2_RANGE_FINDER) + const bool positive_hagl = getHagl() >= 0.f; - // Assume that the terrain estimate is always valid when direct observations are fused - if (_control_status.flags.rng_terrain && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)) { - valid = true; + if (!_terrain_valid) { + // require valid RNG or optical flow (+valid variance) to initially consider terrain valid + if (positive_hagl + && positive_hagl_var + && (valid_rng_terrain + || (valid_opt_flow_terrain && small_relative_hagl_var)) + ) { + _terrain_valid = true; + } + + } else { + // terrain was previously valid, continue considering valid if variance is good + _terrain_valid = positive_hagl && positive_hagl_var && small_relative_hagl_var; } - -#endif // CONFIG_EKF2_RANGE_FINDER - - return valid; } diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 7aad6620ce..89ce999146 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -132,13 +132,11 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, float total_weight = 0.0f; // calculate weighting for each model assuming a normal distribution const float min_weight = 1e-5f; - uint8_t n_weight_clips = 0; for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { _model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index); if (_model_weights(model_index) < min_weight) { - n_weight_clips++; _model_weights(model_index) = min_weight; } @@ -146,13 +144,7 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, } // normalise the weighting function - if (n_weight_clips < N_MODELS_EKFGSF) { - _model_weights /= total_weight; - - } else { - // all weights have collapsed due to excessive innovation variances so reset filters - reset(); - } + _model_weights /= total_weight; } // Calculate a composite yaw vector as a weighted average of the states for each model. @@ -175,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); } + + if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) { + reset(); + } } } @@ -183,8 +179,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an // generate attitude solution using simple complementary filter for the selected model const Vector3f ang_rate = delta_ang / fmaxf(delta_ang_dt, 0.001f) - _ahrs_ekf_gsf[model_index].gyro_bias; - const Dcmf R_to_body = _ahrs_ekf_gsf[model_index].R.transpose(); - const Vector3f gravity_direction_bf = R_to_body.col(2); + const Vector3f gravity_direction_bf = _ahrs_ekf_gsf[model_index].q.inversed().dcm_z(); const float ahrs_accel_norm = _ahrs_accel.norm(); @@ -225,8 +220,9 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an const Vector3f delta_angle_corrected = delta_ang + (tilt_correction - _ahrs_ekf_gsf[model_index].gyro_bias) * delta_ang_dt; - // Apply delta angle to rotation matrix - _ahrs_ekf_gsf[model_index].R = ahrsPredictRotMat(_ahrs_ekf_gsf[model_index].R, delta_angle_corrected); + // Apply delta angle to attitude + const Quatf dq(AxisAnglef{delta_angle_corrected}); + _ahrs_ekf_gsf[model_index].q = (_ahrs_ekf_gsf[model_index].q * dq).normalized(); } void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel) @@ -236,28 +232,11 @@ void EKFGSF_yaw::ahrsAlignTilt(const Vector3f &delta_vel) // 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences. // 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity. - // Calculate earth frame Down axis unit vector rotated into body frame - const Vector3f down_in_bf = -delta_vel.normalized(); - - // Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf' - const Vector3f i_vec_bf(1.f, 0.f, 0.f); - Vector3f north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf.dot(down_in_bf)); - north_in_bf.normalize(); - - // Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf' - const Vector3f east_in_bf = down_in_bf % north_in_bf; - - // Each column in a rotation matrix from earth frame to body frame represents the projection of the - // corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column. - // We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body - // frame are copied into corresponding rows instead. - Dcmf R; - R.setRow(0, north_in_bf); - R.setRow(1, east_in_bf); - R.setRow(2, down_in_bf); + // The tilt is simply the rotation between the measured gravity and the vertical axis + Quatf q(delta_vel, Vector3f(0.f, 0.f, -1.f)); for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { - _ahrs_ekf_gsf[model_index].R = R; + _ahrs_ekf_gsf[model_index].q = q; } } @@ -265,10 +244,9 @@ void EKFGSF_yaw::ahrsAlignYaw() { // Align yaw angle for each model for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) { - const float yaw = wrap_pi(_ekf_gsf[model_index].X(2)); - const Dcmf R = _ahrs_ekf_gsf[model_index].R; - _ahrs_ekf_gsf[model_index].R = updateYawInRotMat(yaw, R); + const Dcmf R(_ahrs_ekf_gsf[model_index].q); + _ahrs_ekf_gsf[model_index].q = Quatf(updateYawInRotMat(yaw, R)); } } @@ -283,16 +261,18 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang return; } + const Dcmf R(_ahrs_ekf_gsf[model_index].q); + // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock - _ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R); + _ekf_gsf[model_index].X(2) = getEulerYaw(R); // calculate delta velocity in a horizontal front-right frame - const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * delta_vel; + const Vector3f del_vel_NED = R * delta_vel; const float cos_yaw = cosf(_ekf_gsf[model_index].X(2)); const float sin_yaw = sinf(_ekf_gsf[model_index].X(2)); const float dvx = del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw; const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw; - const float daz = Vector3f(_ahrs_ekf_gsf[model_index].R * delta_ang)(2); + const float daz = Vector3f(R * delta_ang)(2); // delta velocity process noise double if we're not in air const float accel_noise = in_air ? _accel_noise : 2.f * _accel_noise; @@ -327,16 +307,16 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co const float vel_obs_var = sq(fmaxf(vel_accuracy, 0.01f)); // calculate velocity observation innovations - _ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - vel_NE(0); - _ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - vel_NE(1); + _ekf_gsf[model_index].innov = _ekf_gsf[model_index].X.xy() - vel_NE; matrix::Matrix K; matrix::SquareMatrix P_new; + matrix::SquareMatrix S_inverse; sym::YawEstComputeMeasurementUpdate(_ekf_gsf[model_index].P, vel_obs_var, FLT_EPSILON, - &_ekf_gsf[model_index].S_inverse, + &S_inverse, &_ekf_gsf[model_index].S_det_inverse, &K, &P_new); @@ -355,36 +335,29 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co _ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var); } - // test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] - const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); + // normalized innovation squared = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] + _ekf_gsf[model_index].nis = _ekf_gsf[model_index].innov * (S_inverse * _ekf_gsf[model_index].innov); // Perform a chi-square innovation consistency test and calculate a compression scale factor // that limits the magnitude of innovations to 5-sigma - // If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma + // If the normalized innovation squared is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma // This protects from large measurement spikes - const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f; + if (_ekf_gsf[model_index].nis > sq(5.f)) { + _ekf_gsf[model_index].innov *= sqrtf(sq(5.f) / _ekf_gsf[model_index].nis); + _ekf_gsf[model_index].nis = sq(5.f); + } - // Correct the state vector and capture the change in yaw angle - const float oldYaw = _ekf_gsf[model_index].X(2); + // Correct the state vector + const Vector3f delta_state = -K * _ekf_gsf[model_index].innov; + const float yawDelta = delta_state(2); - _ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor; + _ekf_gsf[model_index].X.xy() += delta_state.xy(); + _ekf_gsf[model_index].X(2) = wrap_pi(_ekf_gsf[model_index].X(2) + yawDelta); - const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw; - - // apply the change in yaw angle to the AHRS - // take advantage of sparseness in the yaw rotation matrix - const float cosYaw = cosf(yawDelta); - const float sinYaw = sinf(yawDelta); - const float R_prev00 = _ahrs_ekf_gsf[model_index].R(0, 0); - const float R_prev01 = _ahrs_ekf_gsf[model_index].R(0, 1); - const float R_prev02 = _ahrs_ekf_gsf[model_index].R(0, 2); - - _ahrs_ekf_gsf[model_index].R(0, 0) = R_prev00 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 0) * sinYaw; - _ahrs_ekf_gsf[model_index].R(0, 1) = R_prev01 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 1) * sinYaw; - _ahrs_ekf_gsf[model_index].R(0, 2) = R_prev02 * cosYaw - _ahrs_ekf_gsf[model_index].R(1, 2) * sinYaw; - _ahrs_ekf_gsf[model_index].R(1, 0) = R_prev00 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 0) * cosYaw; - _ahrs_ekf_gsf[model_index].R(1, 1) = R_prev01 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 1) * cosYaw; - _ahrs_ekf_gsf[model_index].R(1, 2) = R_prev02 * sinYaw + _ahrs_ekf_gsf[model_index].R(1, 2) * cosYaw; + // Apply the change in yaw angle to the AHRS using left multiplication to rotate + // the attitude around the earth Down axis + const Quatf dq(cosf(yawDelta / 2.f), 0.f, 0.f, sinf(yawDelta / 2.f)); + _ahrs_ekf_gsf[model_index].q = (dq * _ahrs_ekf_gsf[model_index].q).normalized(); return true; } @@ -417,10 +390,7 @@ void EKFGSF_yaw::initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accura float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const { - // calculate transpose(innovation) * inv(S) * innovation - const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); - - return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); + return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * _ekf_gsf[model_index].nis); } bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], @@ -463,30 +433,3 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const const float delta_accel_g = (ahrs_accel_norm - CONSTANTS_ONE_G) / CONSTANTS_ONE_G; return _tilt_gain * sq(1.f - math::min(attenuation * fabsf(delta_accel_g), 1.f)); } - -Matrix3f EKFGSF_yaw::ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g) -{ - Matrix3f ret = R; - ret(0, 0) += R(0, 1) * g(2) - R(0, 2) * g(1); - ret(0, 1) += R(0, 2) * g(0) - R(0, 0) * g(2); - ret(0, 2) += R(0, 0) * g(1) - R(0, 1) * g(0); - ret(1, 0) += R(1, 1) * g(2) - R(1, 2) * g(1); - ret(1, 1) += R(1, 2) * g(0) - R(1, 0) * g(2); - ret(1, 2) += R(1, 0) * g(1) - R(1, 1) * g(0); - ret(2, 0) += R(2, 1) * g(2) - R(2, 2) * g(1); - ret(2, 1) += R(2, 2) * g(0) - R(2, 0) * g(2); - ret(2, 2) += R(2, 0) * g(1) - R(2, 1) * g(0); - - // Renormalise rows - for (uint8_t r = 0; r < 3; r++) { - const float rowLengthSq = ret.row(r).norm_squared(); - - if (rowLengthSq > FLT_EPSILON) { - // Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0 - const float rowLengthInv = 1.5f - 0.5f * rowLengthSq; - ret.row(r) *= rowLengthInv; - } - } - - return ret; -} diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h index ec84ed06f9..db1e74d5fd 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h @@ -94,8 +94,8 @@ private: float _true_airspeed{NAN}; // true airspeed used for centripetal accel compensation (m/s) struct { - matrix::Dcmf R{matrix::eye()}; // matrix that rotates a vector from body to earth frame - matrix::Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation + matrix::Quatf q{}; // attitude: rotates a vector from body to earth frame + matrix::Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation } _ahrs_ekf_gsf[N_MODELS_EKFGSF] {}; bool _ahrs_ekf_gsf_tilt_aligned{false}; // true the initial tilt alignment has been calculated @@ -113,15 +113,12 @@ private: // align all AHRS yaw orientations to initial values void ahrsAlignYaw(); - // Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix - matrix::Matrix3f ahrsPredictRotMat(const matrix::Matrix3f &R, const matrix::Vector3f &g); - // Declarations used by a bank of N_MODELS_EKFGSF EKFs struct { matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s matrix::SquareMatrix P{}; // covariance matrix - matrix::SquareMatrix S_inverse{}; // inverse of the innovation covariance matrix + float nis{}; // normalized innovation squared float S_det_inverse{}; // inverse of the innovation covariance matrix determinant matrix::Vector2f innov{}; // Velocity N,E innovation (m/s) } _ekf_gsf[N_MODELS_EKFGSF] {}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8d66fcf52a..9e1f8f5378 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -66,6 +66,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_predict_us(_params->filter_update_interval_us), _param_ekf2_delay_max(_params->delay_max_ms), _param_ekf2_imu_ctrl(_params->imu_ctrl), + _param_ekf2_vel_lim(_params->velocity_limit), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), #endif // CONFIG_EKF2_AUXVEL @@ -416,7 +417,7 @@ int EKF2::print_status(bool verbose) { PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, attitude: %d, local position: %d, global position: %d\n", _instance, (double)_ekf.get_dt_ekf_avg(), _ekf.attitude_valid(), - _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); + _ekf.isLocalHorizontalPositionValid(), _ekf.isGlobalHorizontalPositionValid()); perf_print_counter(_ekf_update_perf); perf_print_counter(_msg_missed_imu_perf); @@ -744,6 +745,7 @@ void EKF2::Run() ekf2_timestamps_s ekf2_timestamps { .timestamp = now, .airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .airspeed_validated_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, @@ -1164,32 +1166,31 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) { - if (_ekf.global_position_is_valid()) { - const Vector3f position{_ekf.getPosition()}; - + if (_ekf.global_origin_valid() && _ekf.control_status().flags.yaw_align) { // generate and publish global position data vehicle_global_position_s global_pos{}; global_pos.timestamp_sample = timestamp; - // Position of local NED origin in GPS / WGS84 frame - _ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon); + // Position GPS / WGS84 frame + const LatLonAlt lla = _ekf.getLatLonAlt(); + global_pos.lat = lla.latitude_deg(); + global_pos.lon = lla.longitude_deg(); + global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid(); + + global_pos.alt = lla.altitude(); + global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid(); - global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters #if defined(CONFIG_EKF2_GNSS) global_pos.alt_ellipsoid = altAmslToEllipsoid(global_pos.alt); -#else - global_pos.alt_ellipsoid = global_pos.alt; #endif - // delta_alt, alt_reset_counter - // global altitude has opposite sign of local down position + // global altitude has opposite sign of local down position float delta_z = 0.f; uint8_t z_reset_counter = 0; _ekf.get_posD_reset(&delta_z, &z_reset_counter); global_pos.delta_alt = -delta_z; global_pos.alt_reset_counter = z_reset_counter; - // lat_lon_reset_counter float delta_xy[2] {}; uint8_t xy_reset_counter = 0; _ekf.get_posNE_reset(delta_xy, &xy_reset_counter); @@ -1197,16 +1198,11 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); - global_pos.terrain_alt = NAN; - global_pos.terrain_alt_valid = false; - #if defined(CONFIG_EKF2_TERRAIN) - if (_ekf.isTerrainEstimateValid()) { - // Terrain altitude in m, WGS84 - global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); - global_pos.terrain_alt_valid = true; - } + // Terrain altitude in m, WGS84 + global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos(); + global_pos.terrain_alt_valid = _ekf.isTerrainEstimateValid(); float delta_hagl = 0.f; _ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter); @@ -1561,12 +1557,13 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) // Acceleration of body origin in local frame const Vector3f vel_deriv{_ekf.getVelocityDerivative()}; + _ekf.resetVelocityDerivativeAccumulation(); lpos.ax = vel_deriv(0); lpos.ay = vel_deriv(1); lpos.az = vel_deriv(2); - lpos.xy_valid = _ekf.local_position_is_valid(); - lpos.v_xy_valid = _ekf.local_position_is_valid(); + lpos.xy_valid = _ekf.isLocalHorizontalPositionValid(); + lpos.v_xy_valid = _ekf.isLocalHorizontalPositionValid(); // TODO: some modules (e.g.: mc_pos_control) don't handle v_z_valid != z_valid properly lpos.z_valid = _ekf.isLocalVerticalPositionValid() || _ekf.isLocalVerticalVelocityValid(); @@ -1632,7 +1629,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) || _ekf.control_status_flags().wind_dead_reckoning; // get control limit information - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy); // convert NaN to INFINITY if (!PX4_ISFINITE(lpos.vxy_max)) { @@ -1647,8 +1644,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.hagl_min = INFINITY; } - if (!PX4_ISFINITE(lpos.hagl_max)) { - lpos.hagl_max = INFINITY; + if (!PX4_ISFINITE(lpos.hagl_max_z)) { + lpos.hagl_max_z = INFINITY; + } + + if (!PX4_ISFINITE(lpos.hagl_max_xy)) { + lpos.hagl_max_xy = INFINITY; } // publish vehicle local position data @@ -1932,6 +1933,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; + status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -2080,6 +2082,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } _airspeed_validated_timestamp_last = airspeed_validated.timestamp; + + ekf2_timestamps.airspeed_validated_timestamp_rel = (int16_t)((int64_t)airspeed_validated.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); } } else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { @@ -2410,6 +2415,15 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) return; //TODO: change and set to NAN } + if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) { + if (!PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) { + // Apply offset + float yaw_offset = matrix::wrap_pi(math::radians(_param_ekf2_gps_yaw_off.get())); + vehicle_gps_position.heading_offset = yaw_offset; + vehicle_gps_position.heading = matrix::wrap_pi(vehicle_gps_position.heading - yaw_offset); + } + } + const float altitude_amsl = static_cast(vehicle_gps_position.altitude_msl_m); const float altitude_ellipsoid = static_cast(vehicle_gps_position.altitude_ellipsoid_m); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 57b3cc749c..deb08c5a60 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -487,6 +487,7 @@ private: (ParamExtInt) _param_ekf2_predict_us, (ParamExtFloat) _param_ekf2_delay_max, (ParamExtInt) _param_ekf2_imu_ctrl, + (ParamExtFloat) _param_ekf2_vel_lim, #if defined(CONFIG_EKF2_AUXVEL) (ParamExtFloat) @@ -536,6 +537,7 @@ private: // Used by EKF-GSF experimental yaw estimator (ParamExtFloat) _param_ekf2_gsf_tas_default, + (ParamFloat) _param_ekf2_gps_yaw_off, #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) @@ -571,12 +573,9 @@ private: #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - (ParamExtFloat) - _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) - - (ParamExtInt) - _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables + (ParamExtFloat) _param_ekf2_beta_gate, + (ParamExtFloat) _param_ekf2_beta_noise, + (ParamExtInt) _param_ekf2_fuse_beta, #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 93f96a656c..04bcfc104c 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -34,7 +34,7 @@ depends on MODULES_EKF2 menuconfig EKF2_AUX_GLOBAL_POSITION depends on MODULES_EKF2 bool "aux global position fusion support" - default n + default y ---help--- EKF2 auxiliary global position fusion support. diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index da68460445..cd815ed56f 100644 --- a/src/modules/ekf2/module.yaml +++ b/src/modules/ekf2/module.yaml @@ -92,6 +92,8 @@ parameters: by this parameter. The range sensor and vision options should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level. + If GPS is set as reference but altitude fusion is disabled in EKF2_GPS_CTRL, + the GPS altitude is still used to initiaize the bias of the other height sensors. type: enum values: 0: Barometric pressure @@ -172,3 +174,12 @@ parameters: max: 1.0 unit: s decimal: 2 + + EKF2_VEL_LIM: + description: + short: Velocity limit + type: float + default: 100 + max: 299792458 + unit: m/s + decimal: 1 diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index 55c530a125..957d2d70a7 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -46,6 +46,15 @@ parameters: min: 1.0 unit: SD decimal: 1 + EKF2_GPS_YAW_OFF: + description: + short: Heading/Yaw offset for dual antenna GPS + type: float + default: 0.0 + min: 0.0 + max: 360.0 + unit: deg + decimal: 1 EKF2_GPS_V_GATE: description: short: Gate size for GNSS velocity fusion diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 78c30bbcca..97f60bdff4 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -158,7 +158,7 @@ parameters: If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the measurement may gets rejected. 0 - disabled type: float - default: 1.0 + default: 3.0 min: 0.0 max: 20.0 unit: m diff --git a/src/modules/ekf2/params_sideslip.yaml b/src/modules/ekf2/params_sideslip.yaml index 49d5467fcc..0dcec672ad 100644 --- a/src/modules/ekf2/params_sideslip.yaml +++ b/src/modules/ekf2/params_sideslip.yaml @@ -6,8 +6,8 @@ parameters: description: short: Enable synthetic sideslip fusion long: 'For reliable wind estimation both sideslip and airspeed fusion (see - EKF2_ARSP_THR) should be enabled. Only applies to fixed-wing vehicles (or - VTOLs in fixed-wing mode). Note: side slip fusion is currently not supported + EKF2_ARSP_THR) should be enabled. Only applies to vehicles in fixed-wing mode + or with airspeed fusion active. Note: side slip fusion is currently not supported for tailsitters.' type: boolean default: 0 diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 7e200c4c54..3761758282 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -62,3 +62,4 @@ px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_si px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_drag_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_grounded.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 02d18bc9ee..9799320ab2 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,391 +1,391 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 -90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 -190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 -290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,0.1,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 -390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,0.1,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 -490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0.00024,0.00049,-0.011,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,0.1,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 -590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.1,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 -690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0.0005,0.00055,-0.013,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,0.1,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 -790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0.0012,0.0012,-0.014,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.018,0.018,0.00077,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 -890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0.00096,0.00072,-0.021,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.019,0.019,0.00051,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 -990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0.0022,0.0014,-0.029,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.021,0.021,0.00062,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 -1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0.0018,0.00086,-0.039,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.023,0.023,0.00043,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 -1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0.0035,0.0014,-0.051,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.025,0.025,0.00051,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 -1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0.0027,0.00089,-0.064,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.026,0.026,0.00038,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 -1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0.0051,0.0013,-0.078,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.028,0.028,0.00043,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 -1490000,1,-0.01,-0.014,0.00016,0.024,0.0029,-0.16,0.0038,0.00083,-0.093,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 -1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0.0065,0.0012,-0.11,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.03,0.03,0.00037,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 -1690000,1,-0.011,-0.014,0.00013,0.028,-0.00014,-0.19,0.0045,0.00062,-0.13,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 -1790000,1,-0.011,-0.014,9.8e-05,0.035,-0.002,-0.2,0.0076,0.00053,-0.15,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 -1890000,1,-0.011,-0.015,7.8e-05,0.043,-0.0033,-0.22,0.011,0.00027,-0.17,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 -1990000,1,-0.011,-0.014,8.9e-05,0.036,-0.0048,-0.23,0.0082,-0.00029,-0.19,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 -2090000,1,-0.011,-0.014,5.1e-05,0.041,-0.0073,-0.24,0.012,-0.00089,-0.22,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 -2190000,1,-0.011,-0.014,6.4e-05,0.033,-0.007,-0.26,0.0079,-0.00099,-0.24,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 -2290000,1,-0.011,-0.014,5.1e-05,0.039,-0.0095,-0.27,0.011,-0.0018,-0.27,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 -2390000,1,-0.011,-0.013,6.9e-05,0.03,-0.0089,-0.29,0.0074,-0.0015,-0.3,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 -2490000,1,-0.011,-0.013,5.1e-05,0.035,-0.011,-0.3,0.011,-0.0025,-0.32,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 -2590000,1,-0.011,-0.013,6.6e-05,0.026,-0.0093,-0.31,0.0068,-0.0018,-0.36,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 -2690000,1,-0.011,-0.013,6.3e-05,0.03,-0.011,-0.33,0.0097,-0.0028,-0.39,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 -2790000,1,-0.011,-0.013,5.8e-05,0.023,-0.0096,-0.34,0.0062,-0.002,-0.42,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 -2890000,1,-0.011,-0.013,8.3e-06,0.027,-0.012,-0.35,0.0087,-0.003,-0.46,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 -2990000,1,-0.011,-0.013,5.6e-05,0.022,-0.0098,-0.36,0.0057,-0.0021,-0.49,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 -3090000,1,-0.011,-0.013,6e-05,0.025,-0.011,-0.38,0.008,-0.0032,-0.53,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 -3190000,1,-0.011,-0.013,2.9e-06,0.02,-0.009,-0.39,0.0053,-0.0022,-0.57,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 -3290000,1,-0.011,-0.013,4.4e-05,0.023,-0.011,-0.4,0.0074,-0.0032,-0.61,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 -3390000,1,-0.011,-0.012,1.5e-05,0.018,-0.0095,-0.42,0.0049,-0.0022,-0.65,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 -3490000,1,-0.011,-0.013,8.2e-06,0.022,-0.012,-0.43,0.0069,-0.0033,-0.69,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 -3590000,1,-0.011,-0.012,3.2e-05,0.017,-0.011,-0.44,0.0047,-0.0023,-0.73,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 -3690000,1,-0.011,-0.012,0.00015,0.019,-0.014,-0.46,0.0065,-0.0036,-0.78,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 -3790000,1,-0.011,-0.012,0.0002,0.016,-0.014,-0.47,0.0044,-0.0027,-0.82,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 -3890000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.48,0.006,-0.0041,-0.87,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -3990000,1,-0.011,-0.012,0.00017,0.02,-0.017,-0.5,0.0079,-0.0057,-0.92,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -4090000,1,-0.011,-0.012,0.00017,0.017,-0.015,-0.51,0.0056,-0.0042,-0.97,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4190000,1,-0.011,-0.012,0.00014,0.02,-0.017,-0.53,0.0075,-0.0058,-1,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4290000,1,-0.01,-0.012,9.8e-05,0.017,-0.012,-0.54,0.0054,-0.0042,-1.1,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4390000,1,-0.01,-0.012,0.00012,0.018,-0.013,-0.55,0.0071,-0.0055,-1.1,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4490000,1,-0.01,-0.012,0.00018,0.014,-0.01,-0.57,0.0051,-0.0038,-1.2,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4590000,1,-0.01,-0.012,0.00021,0.017,-0.012,-0.58,0.0067,-0.0049,-1.2,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4690000,1,-0.01,-0.012,0.00021,0.014,-0.01,-0.6,0.0048,-0.0035,-1.3,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4790000,1,-0.01,-0.012,0.0002,0.015,-0.012,-0.61,0.0062,-0.0045,-1.4,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4890000,1,-0.01,-0.011,0.00019,0.012,-0.01,-0.63,0.0044,-0.0033,-1.4,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -4990000,1,-0.01,-0.012,0.00017,0.015,-0.011,-0.64,0.0058,-0.0043,-1.5,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5090000,1,-0.01,-0.011,0.00022,0.011,-0.0085,-0.66,0.0041,-0.0031,-1.6,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5190000,1,-0.01,-0.011,0.00024,0.013,-0.0098,-0.67,0.0053,-0.004,-1.6,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5290000,1,-0.01,-0.011,0.00023,0.0086,-0.0073,-0.68,0.0037,-0.0029,-1.7,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5390000,1,-0.0099,-0.011,0.00029,0.0081,-0.0081,-0.7,0.0045,-0.0036,-1.8,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5490000,1,-0.0098,-0.011,0.0003,0.0055,-0.0062,-0.71,0.0031,-0.0026,-1.8,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5590000,1,-0.0097,-0.011,0.00028,0.0061,-0.0066,-0.73,0.0036,-0.0032,-1.9,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5690000,1,-0.0096,-0.011,0.00036,0.0041,-0.0038,-0.74,0.0025,-0.0022,-2,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5790000,1,-0.0095,-0.011,0.00035,0.0044,-0.0028,-0.75,0.0029,-0.0025,-2,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5890000,1,-0.0095,-0.011,0.00033,0.0038,-0.00092,0.0028,0.002,-0.0016,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-3.6e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5990000,1,-0.0094,-0.011,0.00036,0.0041,0.00056,0.015,0.0023,-0.0016,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-3.6e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -6090000,1,-0.0094,-0.011,0.00034,0.0051,0.0017,-0.011,0.0028,-0.0015,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-3.6e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6190000,1,-0.0094,-0.011,0.00027,0.0038,0.0042,-0.005,0.002,-0.00053,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-3.6e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6290000,1,-0.0094,-0.011,0.00024,0.005,0.0043,-0.012,0.0025,-0.00012,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-3.6e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6390000,1,-0.0093,-0.011,0.00026,0.0043,0.0053,-0.05,0.0019,0.00038,-3.7e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-3.6e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6490000,1,-0.0093,-0.011,0.00026,0.0049,0.0055,-0.052,0.0023,0.00093,-3.7e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-3.6e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6590000,1,-0.0093,-0.011,0.00019,0.0037,0.0055,-0.099,0.0018,0.001,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-3.6e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6690000,1,-0.0093,-0.011,0.00012,0.0046,0.0051,-0.076,0.0022,0.0015,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-3.6e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0051,-0.11,0.0028,0.0015,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-6.9e-05,0.21,8e-05,0.43,3.6e-05,0.00032,-0.0024,0,0,-3.6e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 -6890000,0.71,0.0013,-0.014,0.7,0.00053,0.0064,-0.12,0.0015,0.0026,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-9.7e-05,0.21,1.3e-05,0.43,1.3e-06,0.00089,-0.00086,0,0,-3.6e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 -6990000,0.71,0.0013,-0.014,0.71,-0.00023,0.0046,-0.12,0.0011,0.0017,-3.7e+02,-0.0015,-0.0057,-7.6e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-3.6e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 -7090000,0.71,0.0012,-0.014,0.71,-0.00079,0.0011,-0.13,0.0011,0.00014,-3.7e+02,-0.0014,-0.0057,-7.7e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00037,0.00025,-0.00041,0,0,-3.6e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 -7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0019,-0.15,0.00089,0.0011,-3.7e+02,-0.0015,-0.0057,-7.7e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-3.6e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 -7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.0099,-0.15,0.00034,0.0079,-3.7e+02,-0.0016,-0.0057,-7.3e-05,-0.00021,-0.0002,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00039,0,0,-3.6e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0.00062,0.012,-3.7e+02,-0.0016,-0.0057,-7.1e-05,-0.00028,-3.8e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00032,0,0,-3.6e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.011,-0.16,0.0014,0.0094,-3.7e+02,-0.0016,-0.0057,-7.2e-05,-0.00018,-7.3e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00078,-0.00043,0,0,-3.6e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7590000,0.71,0.0016,-0.014,0.71,-0.0034,0.02,-0.17,0.0033,0.018,-3.7e+02,-0.0017,-0.0057,-6.8e-05,-0.00024,0.00021,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-3.6e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 -7690000,0.71,0.0016,-0.014,0.71,-0.0046,0.017,-0.16,0.0032,0.016,-3.7e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00032,0.00084,-0.00033,0,0,-3.6e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 -7790000,0.71,0.0017,-0.014,0.71,-0.011,0.016,-0.16,-0.0031,0.016,-3.7e+02,-0.0016,-0.0057,-7.3e-05,-0.00036,6.2e-05,-0.0071,0.21,-1.6e-05,0.43,-0.00034,0.00078,-0.00038,0,0,-3.6e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 -7890000,0.71,0.0017,-0.014,0.71,-0.011,0.021,-0.16,-0.00066,0.02,-3.7e+02,-0.0016,-0.0057,-7.1e-05,-0.00038,0.0002,-0.0096,0.21,-1.5e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-3.6e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 -7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.022,-0.16,0.0022,0.022,-3.7e+02,-0.0016,-0.0056,-6.9e-05,-0.00037,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00085,-0.00038,0,0,-3.6e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 -8090000,0.71,0.0017,-0.014,0.71,-0.005,0.024,-0.17,0.0082,0.024,-3.7e+02,-0.0016,-0.0056,-6.7e-05,-0.0003,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00033,0.0009,-0.00035,0,0,-3.6e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8190000,0.71,0.0017,-0.014,0.71,-0.014,0.028,-0.18,-6.4e-05,0.029,-3.7e+02,-0.0016,-0.0056,-6.9e-05,-0.00043,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-3.6e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8290000,0.71,0.0017,-0.014,0.71,-0.017,0.021,-0.17,-0.0058,0.022,-3.7e+02,-0.0016,-0.0057,-7.2e-05,-0.00056,0.00031,-0.017,0.21,-1e-05,0.43,-0.00031,0.00079,-0.00035,0,0,-3.6e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00041,-0.17,0.0038,0.023,-3.7e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.021,0.21,-9.3e-06,0.43,-0.0003,0.00084,-0.00031,0,0,-3.6e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 -8490000,0.71,0.0017,-0.014,0.71,-0.003,0.0023,-0.17,0.0036,0.023,-3.7e+02,-0.0016,-0.0056,-7e-05,-0.00051,0.00039,-0.025,0.21,-9e-06,0.43,-0.00031,0.0008,-0.00028,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8590000,0.71,0.0021,-0.014,0.71,-0.00044,0.00093,-0.17,0.0032,0.023,-3.7e+02,-0.0018,-0.0057,-6.8e-05,-0.00051,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00042,0.00076,-0.00028,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8690000,0.71,0.002,-0.014,0.71,-0.0034,0.0028,-0.16,0.003,0.023,-3.7e+02,-0.0017,-0.0056,-6.6e-05,-0.00051,0.00039,-0.035,0.21,-1e-05,0.43,-0.00039,0.00084,-0.00029,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.088,5.8e-05,6.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8790000,0.71,0.0019,-0.014,0.71,-0.0057,0.0049,-0.15,0.0026,0.024,-3.7e+02,-0.0017,-0.0057,-6.9e-05,-0.0005,0.00043,-0.041,0.21,-9.1e-06,0.43,-0.00037,0.0008,-0.00029,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.087,5.6e-05,6.6e-05,2.4e-06,0.04,0.04,0.032,0.0013,6.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 -8890000,0.71,0.0019,-0.014,0.71,-0.0079,0.0055,-0.15,0.0019,0.024,-3.7e+02,-0.0016,-0.0057,-7.1e-05,-0.00063,0.00042,-0.045,0.21,-7.8e-06,0.43,-0.00034,0.00078,-0.00033,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.086,5.3e-05,6.4e-05,2.4e-06,0.04,0.04,0.03,0.0013,6.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -8990000,0.71,0.0018,-0.014,0.71,-0.01,0.0049,-0.14,0.00085,0.024,-3.7e+02,-0.0015,-0.0058,-7.5e-05,-0.00087,0.00044,-0.051,0.21,-6.6e-06,0.43,-0.0003,0.00071,-0.00034,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.087,5.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.029,0.0013,6.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9090000,0.71,0.002,-0.014,0.71,-0.014,0.006,-0.14,-0.00063,0.025,-3.7e+02,-0.0016,-0.0058,-7.6e-05,-0.00098,0.00054,-0.053,0.21,-6.9e-06,0.43,-0.00033,0.00068,-0.00034,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.095,1.1e+02,1.1e+02,0.086,4.9e-05,6.2e-05,2.4e-06,0.04,0.04,0.028,0.0013,5.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9190000,0.71,0.0019,-0.014,0.71,-0.012,0.0087,-0.14,-0.0008,0.026,-3.7e+02,-0.0015,-0.0056,-7e-05,-0.00086,0.00059,-0.057,0.21,-6.2e-06,0.43,-0.00031,0.00081,-0.00025,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.094,1.1e+02,1.1e+02,0.085,4.6e-05,6e-05,2.4e-06,0.04,0.04,0.027,0.0013,5.6e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 -9290000,0.71,0.0017,-0.014,0.71,-0.011,0.0086,-0.14,-0.0015,0.026,-3.7e+02,-0.0015,-0.0056,-7.1e-05,-0.00096,0.00059,-0.061,0.21,-5.1e-06,0.43,-0.00026,0.00082,-0.00024,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.093,1.1e+02,1.1e+02,0.085,4.4e-05,5.8e-05,2.4e-06,0.04,0.04,0.025,0.0013,5.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9390000,0.71,0.0015,-0.014,0.71,-0.011,0.0092,-0.14,-0.0023,0.027,-3.7e+02,-0.0014,-0.0056,-7.3e-05,-0.001,0.00058,-0.065,0.21,-4.3e-06,0.43,-0.00022,0.00082,-0.00022,0,0,-3.6e+02,0.0013,0.0014,0.039,25,25,0.093,1.2e+02,1.2e+02,0.086,4.2e-05,5.7e-05,2.4e-06,0.04,0.04,0.024,0.0013,5.2e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9490000,0.71,0.0014,-0.014,0.71,-0.0078,0.011,-0.13,-0.0012,0.028,-3.7e+02,-0.0014,-0.0055,-6.9e-05,-0.001,0.0006,-0.068,0.21,-3.7e-06,0.43,-0.00018,0.0009,-0.00013,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.091,1.2e+02,1.2e+02,0.085,4e-05,5.5e-05,2.4e-06,0.04,0.04,0.023,0.0013,5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9590000,0.71,0.0016,-0.014,0.71,-0.01,0.016,-0.13,-0.0026,0.031,-3.7e+02,-0.0015,-0.0054,-6.6e-05,-0.0011,0.00079,-0.072,0.21,-4.5e-06,0.43,-0.00024,0.00091,-0.00012,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.09,1.3e+02,1.3e+02,0.085,3.8e-05,5.4e-05,2.4e-06,0.04,0.04,0.021,0.0013,4.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 -9690000,0.71,0.0018,-0.014,0.71,-0.017,0.017,-0.12,-0.0071,0.032,-3.7e+02,-0.0015,-0.0056,-7e-05,-0.0013,0.00092,-0.077,0.21,-4.8e-06,0.43,-0.00027,0.0008,-0.00017,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.089,1.3e+02,1.3e+02,0.086,3.6e-05,5.2e-05,2.4e-06,0.04,0.04,0.02,0.0013,4.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9790000,0.71,0.0017,-0.014,0.71,-0.013,0.02,-0.11,-0.0063,0.034,-3.7e+02,-0.0015,-0.0055,-6.8e-05,-0.0014,0.001,-0.082,0.21,-4.4e-06,0.43,-0.00024,0.00085,-9.7e-05,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.087,1.4e+02,1.4e+02,0.085,3.4e-05,5e-05,2.4e-06,0.04,0.04,0.019,0.0013,4.5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9890000,0.71,0.0018,-0.014,0.71,-0.016,0.023,-0.11,-0.0088,0.037,-3.7e+02,-0.0015,-0.0055,-6.8e-05,-0.0015,0.0011,-0.085,0.21,-4.5e-06,0.43,-0.00026,0.00083,-0.0001,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.084,1.4e+02,1.4e+02,0.085,3.2e-05,4.9e-05,2.4e-06,0.04,0.04,0.018,0.0013,4.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -9990000,0.71,0.0019,-0.014,0.71,-0.02,0.021,-0.1,-0.012,0.037,-3.7e+02,-0.0015,-0.0056,-7.1e-05,-0.0016,0.0011,-0.089,0.21,-4.2e-06,0.43,-0.00025,0.00078,-0.00013,0,0,-3.6e+02,0.0013,0.0013,0.039,25,25,0.083,1.5e+02,1.5e+02,0.086,3e-05,4.7e-05,2.4e-06,0.04,0.04,0.017,0.0013,4.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 -10090000,0.71,0.002,-0.014,0.71,-0.022,0.025,-0.096,-0.015,0.042,-3.7e+02,-0.0015,-0.0056,-6.9e-05,-0.0017,0.0013,-0.091,0.21,-4.6e-06,0.43,-0.00029,0.00079,-0.00012,0,0,-3.6e+02,0.0013,0.0012,0.039,25,25,0.08,1.6e+02,1.6e+02,0.085,2.9e-05,4.6e-05,2.4e-06,0.04,0.04,0.016,0.0013,4.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10190000,0.71,0.002,-0.013,0.71,-0.033,0.022,-0.096,-0.025,0.04,-3.7e+02,-0.0015,-0.0057,-7.5e-05,-0.0019,0.0013,-0.093,0.21,-4.5e-06,0.43,-0.00027,0.00065,-0.00015,0,0,-3.6e+02,0.0012,0.0012,0.039,25,25,0.078,1.7e+02,1.7e+02,0.084,2.7e-05,4.4e-05,2.4e-06,0.04,0.04,0.014,0.0013,4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10290000,0.71,0.002,-0.013,0.71,-0.04,0.017,-0.084,-0.033,0.038,-3.7e+02,-0.0015,-0.0058,-7.8e-05,-0.0021,0.0014,-0.098,0.21,-4.2e-06,0.43,-0.00026,0.00058,-0.00017,0,0,-3.6e+02,0.0012,0.0012,0.039,25,25,0.076,1.8e+02,1.8e+02,0.085,2.6e-05,4.2e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10390000,0.71,0.0018,-0.013,0.71,0.0091,-0.02,0.0093,0.00085,-0.0018,-3.7e+02,-0.0015,-0.0058,-7.9e-05,-0.0021,0.0013,-0.098,0.21,-3.7e-06,0.43,-0.00023,0.0006,-0.00014,0,0,-3.6e+02,0.0012,0.0012,0.039,0.25,0.25,0.56,0.25,0.25,0.077,2.4e-05,4.1e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 -10490000,0.71,0.0018,-0.013,0.71,0.0071,-0.019,0.022,0.0016,-0.0038,-3.7e+02,-0.0014,-0.0059,-8.3e-05,-0.0022,0.0013,-0.098,0.21,-3.3e-06,0.43,-0.00021,0.00053,-0.00016,0,0,-3.6e+02,0.0012,0.0012,0.039,0.26,0.26,0.55,0.26,0.26,0.079,2.3e-05,3.9e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10590000,0.71,0.0022,-0.013,0.71,0.006,-0.0081,0.024,0.0018,-0.00082,-3.7e+02,-0.0016,-0.0059,-7.9e-05,-0.0024,0.0018,-0.098,0.21,-4.5e-06,0.43,-0.00029,0.00053,-0.00017,0,0,-3.6e+02,0.0012,0.0011,0.039,0.13,0.13,0.27,0.13,0.13,0.072,2.2e-05,3.8e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10690000,0.71,0.0021,-0.013,0.71,0.0032,-0.009,0.027,0.0023,-0.0017,-3.7e+02,-0.0015,-0.0059,-8.1e-05,-0.0024,0.0018,-0.098,0.21,-4.1e-06,0.43,-0.00027,0.00052,-0.00017,0,0,-3.6e+02,0.0012,0.0011,0.039,0.14,0.14,0.26,0.14,0.14,0.077,2.1e-05,3.6e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10790000,0.71,0.002,-0.013,0.71,0.0033,-0.0063,0.021,0.0026,-0.00085,-3.7e+02,-0.0015,-0.0059,-8e-05,-0.0025,0.0021,-0.098,0.21,-4e-06,0.43,-0.00026,0.00056,-0.00016,0,0,-3.6e+02,0.0012,0.0011,0.038,0.093,0.093,0.17,0.09,0.09,0.071,1.9e-05,3.4e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 -10890000,0.71,0.0019,-0.013,0.71,0.0021,-0.0065,0.016,0.0029,-0.0015,-3.7e+02,-0.0015,-0.0058,-8.1e-05,-0.0025,0.0021,-0.098,0.21,-3.7e-06,0.43,-0.00025,0.00057,-0.00015,0,0,-3.6e+02,0.0011,0.0011,0.038,0.1,0.1,0.16,0.096,0.096,0.074,1.8e-05,3.3e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -10990000,0.71,0.0017,-0.014,0.71,0.0067,-0.0015,0.011,0.0048,-0.0026,-3.7e+02,-0.0013,-0.0057,-7.9e-05,-0.0025,0.0031,-0.098,0.21,-3.2e-06,0.43,-0.00022,0.00071,-0.00011,0,0,-3.6e+02,0.0011,0.001,0.038,0.079,0.08,0.12,0.072,0.072,0.071,1.7e-05,3.1e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11090000,0.71,0.0017,-0.014,0.71,0.0067,0.0018,0.014,0.0057,-0.0023,-3.7e+02,-0.0014,-0.0056,-7.5e-05,-0.0023,0.0029,-0.098,0.21,-3.3e-06,0.43,-0.00023,0.00078,-6.8e-05,0,0,-3.6e+02,0.0011,0.00099,0.038,0.09,0.091,0.12,0.078,0.078,0.073,1.6e-05,2.9e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11190000,0.71,0.0017,-0.014,0.71,0.01,0.0033,0.0027,0.0067,-0.0032,-3.7e+02,-0.0013,-0.0056,-7.8e-05,-0.0022,0.004,-0.098,0.21,-3.4e-06,0.43,-0.00024,0.00079,-8.6e-05,0,0,-3.6e+02,0.00098,0.00091,0.038,0.073,0.074,0.091,0.062,0.062,0.068,1.5e-05,2.7e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 -11290000,0.71,0.0016,-0.014,0.71,0.0093,0.0017,0.0014,0.0076,-0.0034,-3.7e+02,-0.0013,-0.0057,-8.3e-05,-0.0026,0.0043,-0.098,0.21,-3e-06,0.43,-0.00022,0.00073,-0.00011,0,0,-3.6e+02,0.00097,0.0009,0.038,0.085,0.087,0.087,0.068,0.068,0.071,1.4e-05,2.6e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11390000,0.71,0.0016,-0.013,0.71,0.0047,0.0008,-0.0044,0.0055,-0.003,-3.7e+02,-0.0013,-0.0058,-8.6e-05,-0.0033,0.0041,-0.098,0.21,-3e-06,0.43,-0.00021,0.00066,-0.00016,0,0,-3.6e+02,0.00086,0.00082,0.038,0.071,0.073,0.072,0.056,0.056,0.067,1.3e-05,2.4e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11490000,0.71,0.0014,-0.013,0.71,0.00093,-0.0021,-0.0044,0.0055,-0.0039,-3.7e+02,-0.0012,-0.0059,-9.3e-05,-0.0039,0.0049,-0.098,0.21,-2.7e-06,0.43,-0.00018,0.00056,-0.00022,0,0,-3.6e+02,0.00086,0.00081,0.038,0.083,0.085,0.069,0.062,0.062,0.068,1.3e-05,2.3e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11590000,0.71,0.0013,-0.013,0.71,-0.0022,-0.0016,-0.01,0.0044,-0.0032,-3.7e+02,-0.0012,-0.0059,-9.4e-05,-0.0045,0.0051,-0.098,0.21,-2.5e-06,0.43,-0.00016,0.00054,-0.00023,0,0,-3.6e+02,0.00075,0.00072,0.038,0.07,0.072,0.059,0.052,0.052,0.066,1.2e-05,2.1e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 -11690000,0.71,0.0011,-0.013,0.71,-0.0037,-0.0027,-0.016,0.0044,-0.004,-3.7e+02,-0.0011,-0.006,-9.8e-05,-0.0053,0.0052,-0.098,0.21,-2e-06,0.43,-0.0001,0.00053,-0.00026,0,0,-3.6e+02,0.00075,0.00071,0.038,0.081,0.084,0.057,0.059,0.059,0.066,1.1e-05,2e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11790000,0.71,0.0011,-0.013,0.71,-0.0077,-0.00091,-0.018,0.0024,-0.002,-3.7e+02,-0.0011,-0.006,-9.9e-05,-0.0071,0.0054,-0.098,0.21,-2e-06,0.43,-8.7e-05,0.0005,-0.00027,0,0,-3.6e+02,0.00065,0.00063,0.038,0.068,0.07,0.051,0.05,0.05,0.063,1e-05,1.8e-05,2.3e-06,0.028,0.03,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11890000,0.71,0.00095,-0.013,0.71,-0.0087,-0.0038,-0.02,0.0016,-0.0027,-3.7e+02,-0.001,-0.006,-0.0001,-0.0077,0.0059,-0.098,0.21,-2e-06,0.43,-5.6e-05,0.00047,-0.00032,0,0,-3.6e+02,0.00065,0.00062,0.038,0.079,0.082,0.049,0.057,0.057,0.063,9.8e-06,1.8e-05,2.3e-06,0.028,0.029,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -11990000,0.71,0.0013,-0.013,0.71,-0.012,0.0011,-0.026,-0.0001,-7.8e-05,-3.7e+02,-0.0011,-0.006,-9.9e-05,-0.0073,0.0065,-0.097,0.21,-2.9e-06,0.43,-0.00013,0.00048,-0.00032,0,0,-3.6e+02,0.00056,0.00055,0.037,0.066,0.068,0.045,0.049,0.049,0.062,9.2e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 -12090000,0.71,0.0014,-0.013,0.71,-0.013,0.0043,-0.032,-0.0014,0.00091,-3.7e+02,-0.0012,-0.0059,-9.5e-05,-0.0063,0.0057,-0.097,0.21,-3e-06,0.43,-0.00016,0.00053,-0.00028,0,0,-3.6e+02,0.00056,0.00055,0.037,0.077,0.079,0.045,0.056,0.056,0.062,8.8e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12190000,0.71,0.0012,-0.013,0.71,-0.0069,0.0039,-0.026,0.0015,0.00037,-3.7e+02,-0.0011,-0.0059,-9.6e-05,-0.0061,0.006,-0.099,0.21,-2.4e-06,0.43,-0.00011,0.0006,-0.00026,0,0,-3.6e+02,0.00048,0.00048,0.037,0.064,0.066,0.041,0.048,0.048,0.059,8.2e-06,1.4e-05,2.3e-06,0.023,0.026,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12290000,0.71,0.001,-0.013,0.71,-0.0081,0.0044,-0.026,0.0011,0.00072,-3.7e+02,-0.0011,-0.0059,-9.6e-05,-0.0067,0.0056,-0.099,0.21,-2e-06,0.43,-8.3e-05,0.0006,-0.00026,0,0,-3.6e+02,0.00048,0.00048,0.037,0.073,0.076,0.042,0.055,0.056,0.06,7.9e-06,1.4e-05,2.3e-06,0.023,0.025,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12390000,0.71,0.001,-0.013,0.71,-0.0054,0.0032,-0.023,0.0025,0.00024,-3.7e+02,-0.0011,-0.0059,-9.9e-05,-0.0053,0.0073,-0.1,0.21,-2.5e-06,0.43,-0.00012,0.00061,-0.00025,0,0,-3.6e+02,0.00042,0.00043,0.037,0.061,0.063,0.039,0.048,0.048,0.058,7.4e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 -12490000,0.71,0.00094,-0.013,0.71,-0.0071,0.0027,-0.027,0.0017,3.4e-05,-3.7e+02,-0.0011,-0.0059,-0.0001,-0.0058,0.0085,-0.1,0.21,-2.7e-06,0.43,-0.00015,0.00055,-0.00025,0,0,-3.6e+02,0.00042,0.00042,0.037,0.069,0.072,0.04,0.055,0.055,0.058,7.1e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12590000,0.71,0.0011,-0.013,0.71,-0.014,0.0037,-0.033,-0.0033,0.00021,-3.7e+02,-0.0011,-0.006,-0.0001,-0.0051,0.0079,-0.1,0.21,-3e-06,0.43,-0.00019,0.00051,-0.00024,0,0,-3.6e+02,0.00037,0.00038,0.037,0.058,0.059,0.038,0.047,0.047,0.057,6.8e-06,1.2e-05,2.3e-06,0.019,0.022,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12690000,0.71,0.0014,-0.013,0.71,-0.017,0.0046,-0.037,-0.0058,0.00079,-3.7e+02,-0.0012,-0.006,-0.0001,-0.0033,0.0092,-0.1,0.21,-3.9e-06,0.43,-0.00026,0.0005,-0.00025,0,0,-3.6e+02,0.00037,0.00038,0.037,0.065,0.067,0.039,0.055,0.055,0.057,6.5e-06,1.1e-05,2.3e-06,0.019,0.022,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12790000,0.71,0.0012,-0.013,0.71,-0.02,0.0027,-0.041,-0.0081,0.00025,-3.7e+02,-0.0012,-0.006,-0.0001,-0.0049,0.008,-0.1,0.21,-3.3e-06,0.43,-0.0002,0.00048,-0.00027,0,0,-3.6e+02,0.00033,0.00034,0.037,0.054,0.056,0.037,0.047,0.047,0.056,6.2e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 -12890000,0.71,0.0011,-0.013,0.71,-0.02,0.0014,-0.039,-0.0096,0.00019,-3.7e+02,-0.0011,-0.006,-0.0001,-0.0062,0.0077,-0.1,0.21,-2.9e-06,0.43,-0.00017,0.00047,-0.00026,0,0,-3.6e+02,0.00033,0.00034,0.037,0.061,0.063,0.038,0.054,0.055,0.057,6e-06,1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -12990000,0.71,0.001,-0.013,0.71,-0.009,0.0019,-0.039,-0.0018,0.0005,-3.7e+02,-0.0011,-0.0059,-9.9e-05,-0.0038,0.0081,-0.1,0.21,-2.7e-06,0.43,-0.00018,0.00058,-0.0002,0,0,-3.6e+02,0.0003,0.00031,0.037,0.051,0.053,0.037,0.047,0.047,0.055,5.7e-06,9.9e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13090000,0.71,0.00095,-0.013,0.71,-0.0097,-0.00015,-0.039,-0.0029,-7.2e-05,-3.7e+02,-0.0011,-0.006,-0.0001,-0.005,0.0097,-0.1,0.21,-3.1e-06,0.43,-0.0002,0.00052,-0.00023,0,0,-3.6e+02,0.0003,0.00031,0.037,0.057,0.059,0.038,0.054,0.054,0.056,5.5e-06,9.6e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13190000,0.71,0.00094,-0.013,0.71,-0.0023,0.00081,-0.034,0.0028,0.00014,-3.7e+02,-0.0011,-0.006,-0.00011,-0.0032,0.011,-0.11,0.21,-3.4e-06,0.43,-0.00023,0.00056,-0.00021,0,0,-3.6e+02,0.00027,0.00028,0.037,0.048,0.05,0.037,0.047,0.047,0.054,5.2e-06,9.1e-06,2.3e-06,0.015,0.019,0.0098,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 -13290000,0.71,0.00079,-0.013,0.71,-0.00079,0.0015,-0.029,0.0034,0.0003,-3.7e+02,-0.001,-0.0059,-0.0001,-0.0047,0.0096,-0.11,0.21,-2.5e-06,0.43,-0.00018,0.00058,-0.0002,0,0,-3.6e+02,0.00027,0.00028,0.037,0.054,0.055,0.038,0.054,0.054,0.055,5.1e-06,8.9e-06,2.3e-06,0.015,0.018,0.0097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13390000,0.71,0.0007,-0.013,0.71,0.00023,0.0023,-0.024,0.0025,0.00042,-3.7e+02,-0.001,-0.0059,-0.0001,-0.0041,0.0089,-0.11,0.21,-2.3e-06,0.43,-0.00016,0.00063,-0.00019,0,0,-3.6e+02,0.00025,0.00026,0.037,0.045,0.047,0.037,0.047,0.047,0.054,4.8e-06,8.5e-06,2.3e-06,0.014,0.018,0.0091,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13490000,0.71,0.00071,-0.013,0.71,0.00021,0.0024,-0.022,0.0027,0.0008,-3.7e+02,-0.001,-0.0059,-0.0001,-0.0042,0.0081,-0.11,0.21,-2e-06,0.43,-0.00015,0.00063,-0.00017,0,0,-3.6e+02,0.00025,0.00026,0.037,0.05,0.052,0.038,0.054,0.054,0.055,4.7e-06,8.2e-06,2.3e-06,0.014,0.017,0.009,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13590000,0.71,0.00072,-0.013,0.71,-2.7e-05,0.0028,-0.024,0.0015,0.00038,-3.7e+02,-0.001,-0.0059,-0.0001,-0.004,0.0094,-0.11,0.21,-2.4e-06,0.43,-0.00017,0.00062,-0.00019,0,0,-3.6e+02,0.00023,0.00025,0.037,0.042,0.044,0.037,0.046,0.047,0.054,4.5e-06,7.9e-06,2.3e-06,0.013,0.017,0.0085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 -13690000,0.71,0.00071,-0.013,0.71,0.00091,0.0053,-0.029,0.0017,0.0011,-3.7e+02,-0.001,-0.0059,-9.9e-05,-0.0034,0.0082,-0.11,0.21,-2.1e-06,0.43,-0.00016,0.00064,-0.00016,0,0,-3.6e+02,0.00023,0.00024,0.037,0.047,0.049,0.038,0.053,0.054,0.055,4.3e-06,7.7e-06,2.3e-06,0.013,0.017,0.0083,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13790000,0.71,0.00075,-0.013,0.71,0.0006,0.0021,-0.03,0.0024,-0.00073,-3.7e+02,-0.0011,-0.0059,-9.9e-05,-0.002,0.0089,-0.11,0.21,-2.5e-06,0.43,-0.00019,0.00065,-0.00015,0,0,-3.6e+02,0.00022,0.00023,0.037,0.04,0.041,0.036,0.046,0.046,0.053,4.2e-06,7.3e-06,2.3e-06,0.012,0.016,0.0078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13890000,0.71,0.00059,-0.013,0.71,0.0024,0.0022,-0.035,0.003,-0.00056,-3.7e+02,-0.001,-0.0059,-9.8e-05,-0.0034,0.0078,-0.11,0.21,-1.8e-06,0.43,-0.00015,0.00065,-0.00015,0,0,-3.6e+02,0.00022,0.00023,0.037,0.044,0.045,0.037,0.053,0.053,0.055,4e-06,7.1e-06,2.3e-06,0.012,0.016,0.0076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -13990000,0.71,0.00066,-0.013,0.71,0.0019,0.00054,-0.034,0.0036,-0.0019,-3.7e+02,-0.001,-0.0059,-9.8e-05,-0.0021,0.0083,-0.11,0.21,-2e-06,0.43,-0.00018,0.00065,-0.00013,0,0,-3.6e+02,0.0002,0.00022,0.037,0.037,0.039,0.036,0.046,0.046,0.054,3.9e-06,6.8e-06,2.3e-06,0.012,0.015,0.0071,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 -14090000,0.71,0.00072,-0.013,0.71,0.0014,0.0019,-0.035,0.0036,-0.0013,-3.7e+02,-0.0011,-0.0059,-9.4e-05,-0.00063,0.0073,-0.11,0.21,-1.8e-06,0.43,-0.00018,0.00069,-9.7e-05,0,0,-3.6e+02,0.00021,0.00022,0.037,0.041,0.043,0.036,0.053,0.053,0.054,3.8e-06,6.7e-06,2.3e-06,0.012,0.015,0.007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14190000,0.71,0.00068,-0.013,0.71,0.0044,0.0019,-0.037,0.0057,-0.001,-3.7e+02,-0.0011,-0.0059,-9.3e-05,-0.00017,0.007,-0.11,0.21,-1.6e-06,0.43,-0.00018,0.00071,-8e-05,0,0,-3.6e+02,0.0002,0.00021,0.037,0.035,0.037,0.035,0.046,0.046,0.054,3.6e-06,6.4e-06,2.3e-06,0.011,0.015,0.0065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14290000,0.71,0.00075,-0.013,0.71,0.0046,0.0032,-0.035,0.0059,-0.00065,-3.7e+02,-0.0011,-0.0058,-9.2e-05,0.00068,0.007,-0.11,0.21,-1.8e-06,0.43,-0.00019,0.00071,-6.2e-05,0,0,-3.6e+02,0.0002,0.0002,0.037,0.038,0.04,0.036,0.052,0.052,0.055,3.5e-06,6.2e-06,2.3e-06,0.011,0.014,0.0063,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14390000,0.71,0.00064,-0.014,0.71,0.0071,0.005,-0.037,0.0077,-0.00036,-3.7e+02,-0.0011,-0.0058,-8.9e-05,0.00035,0.0055,-0.11,0.21,-9.2e-07,0.43,-0.00015,0.00074,-4.6e-05,0,0,-3.6e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 -14490000,0.71,0.00053,-0.013,0.71,0.008,0.0065,-0.041,0.0088,0.00011,-3.7e+02,-0.001,-0.0058,-8.9e-05,-0.00096,0.005,-0.11,0.21,-4.4e-07,0.43,-0.00013,0.0007,-4.6e-05,0,0,-3.6e+02,0.00019,0.00019,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14590000,0.71,0.00042,-0.013,0.71,0.0061,0.005,-0.041,0.006,-0.0015,-3.7e+02,-0.001,-0.0058,-8.9e-05,-0.0018,0.0047,-0.11,0.21,-3.4e-07,0.43,-0.00012,0.00067,-5.9e-05,0,0,-3.6e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14690000,0.71,0.00037,-0.013,0.71,0.0079,0.0031,-0.038,0.0068,-0.00088,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.0018,0.0038,-0.11,0.21,3.8e-08,0.43,-0.0001,0.00068,-4.5e-05,0,0,-3.6e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14790000,0.71,0.00035,-0.013,0.71,0.0055,0.0016,-0.033,0.0042,-0.0021,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.002,0.0037,-0.12,0.21,-2e-08,0.43,-0.00011,0.00066,-4.6e-05,0,0,-3.6e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0097,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 -14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0034,-0.037,0.005,-0.0018,-3.7e+02,-0.001,-0.0058,-8.6e-05,-0.0022,0.0032,-0.12,0.21,2.4e-07,0.43,-9.1e-05,0.00067,-4.3e-05,0,0,-3.6e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0096,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -14990000,0.71,0.00027,-0.013,0.71,0.0068,0.0024,-0.032,0.004,-0.0017,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.0028,0.0037,-0.12,0.21,4.6e-08,0.43,-0.0001,0.00064,-5.6e-05,0,0,-3.6e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.012,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15090000,0.71,0.0002,-0.013,0.71,0.0075,0.0025,-0.035,0.0047,-0.0016,-3.7e+02,-0.001,-0.0058,-8.8e-05,-0.0028,0.0041,-0.12,0.21,-5.6e-08,0.43,-0.00011,0.00063,-5.7e-05,0,0,-3.6e+02,0.00017,0.00017,0.037,0.03,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0092,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15190000,0.71,0.00017,-0.013,0.7,0.0073,0.003,-0.033,0.0039,-0.0015,-3.7e+02,-0.001,-0.0058,-9e-05,-0.0035,0.0044,-0.12,0.21,-5.2e-08,0.43,-0.00012,0.00059,-6.4e-05,0,0,-3.6e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.009,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 -15290000,0.71,0.00019,-0.013,0.7,0.0077,0.0041,-0.03,0.0044,-0.00097,-3.7e+02,-0.001,-0.0058,-8.8e-05,-0.0026,0.0042,-0.12,0.21,-8.2e-09,0.43,-0.00014,0.00059,-3.9e-05,0,0,-3.6e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15390000,0.71,0.0002,-0.013,0.7,0.007,0.0054,-0.028,0.0017,-0.00041,-3.7e+02,-0.001,-0.0058,-8.3e-05,-0.0018,0.0027,-0.12,0.21,3.4e-07,0.43,-0.00012,0.00062,-1.6e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15490000,0.71,0.00022,-0.013,0.7,0.0086,0.0045,-0.028,0.0024,-0.00036,-3.7e+02,-0.001,-0.0058,-8.7e-05,-0.0022,0.004,-0.12,0.21,-1.1e-07,0.43,-0.00014,0.00059,-3.5e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15590000,0.71,0.00018,-0.013,0.71,0.0072,0.0036,-0.027,8.4e-05,-0.00068,-3.7e+02,-0.001,-0.0059,-8.9e-05,-0.0028,0.0047,-0.12,0.21,-4.6e-07,0.43,-0.00014,0.00058,-6.1e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 -15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0037,-0.028,0.00059,-0.00037,-3.7e+02,-0.001,-0.0059,-8.9e-05,-0.0023,0.0052,-0.12,0.21,-7.6e-07,0.43,-0.00015,0.00059,-6.4e-05,0,0,-3.6e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15790000,0.71,0.00019,-0.013,0.7,0.0081,0.0021,-0.03,0.00049,-0.0016,-3.7e+02,-0.001,-0.0059,-8.9e-05,-0.0026,0.0054,-0.12,0.21,-8.8e-07,0.43,-0.00016,0.00057,-6.8e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15890000,0.71,0.0002,-0.013,0.71,0.0088,0.0021,-0.028,0.0011,-0.0014,-3.7e+02,-0.0011,-0.0059,-8.8e-05,-0.0018,0.0057,-0.12,0.21,-1.1e-06,0.43,-0.00018,0.00057,-6e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.1e-06,3.8e-06,2.3e-06,0.008,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -15990000,0.71,0.00018,-0.013,0.71,0.0086,0.0022,-0.024,0.00076,-0.0019,-3.7e+02,-0.0011,-0.0059,-8.4e-05,-0.0012,0.0049,-0.12,0.21,-9.8e-07,0.43,-0.00018,0.00058,-4.2e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 -16090000,0.71,0.00021,-0.013,0.71,0.011,0.0039,-0.02,0.0016,-0.0011,-3.7e+02,-0.0011,-0.0058,-8e-05,-0.00043,0.0037,-0.12,0.21,-7.1e-07,0.43,-0.00016,0.00063,-2.6e-05,0,0,-3.6e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.048,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16190000,0.71,0.00026,-0.013,0.71,0.01,0.0041,-0.019,0.001,-0.00095,-3.7e+02,-0.0011,-0.0058,-7.9e-05,0.00031,0.0039,-0.12,0.21,-1e-06,0.43,-0.00017,0.00063,-2.1e-05,0,0,-3.6e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16290000,0.71,0.00028,-0.014,0.71,0.012,0.0052,-0.02,0.0023,5.7e-05,-3.7e+02,-0.0011,-0.0058,-7.5e-05,0.00063,0.0026,-0.12,0.21,-5.6e-07,0.43,-0.00015,0.00065,-5.9e-06,0,0,-3.6e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.047,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16390000,0.71,0.00033,-0.013,0.71,0.01,0.0037,-0.019,0.0013,-0.00022,-3.7e+02,-0.0011,-0.0058,-7.6e-05,0.0018,0.0038,-0.12,0.21,-1.3e-06,0.43,-0.00018,0.00064,-2e-06,0,0,-3.6e+02,0.00014,0.00014,0.037,0.02,0.022,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 -16490000,0.71,0.00042,-0.013,0.71,0.0088,0.005,-0.022,0.0018,0.00032,-3.7e+02,-0.0011,-0.0058,-7.5e-05,0.0026,0.0039,-0.12,0.21,-1.6e-06,0.43,-0.00019,0.00064,1.2e-05,0,0,-3.6e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16590000,0.71,0.00053,-0.013,0.71,0.0068,0.0059,-0.023,-0.00051,0.0023,-3.7e+02,-0.0012,-0.0058,-7.6e-05,0.0028,0.0036,-0.12,0.21,-1.6e-06,0.43,-0.00017,0.00063,7.8e-06,0,0,-3.6e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0095,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16690000,0.71,0.00049,-0.013,0.71,0.0076,0.0062,-0.019,0.00033,0.0026,-3.7e+02,-0.0012,-0.0058,-7.9e-05,0.0022,0.0043,-0.12,0.21,-1.8e-06,0.43,-0.00018,0.00062,-3.3e-06,0,0,-3.6e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16790000,0.71,0.00049,-0.013,0.71,0.0058,0.0069,-0.018,-0.0016,0.0042,-3.7e+02,-0.0012,-0.0058,-8e-05,0.002,0.004,-0.12,0.21,-1.7e-06,0.43,-0.00016,0.00063,-1.6e-05,0,0,-3.6e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 -16890000,0.71,0.00054,-0.013,0.71,0.0055,0.0078,-0.016,-0.0013,0.0047,-3.7e+02,-0.0012,-0.0058,-8.2e-05,0.0023,0.0046,-0.13,0.21,-2e-06,0.43,-0.00017,0.00062,-1.2e-05,0,0,-3.6e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0091,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -16990000,0.71,0.00051,-0.013,0.71,0.0055,0.0055,-0.015,-0.0021,0.0025,-3.7e+02,-0.0012,-0.0059,-8.2e-05,0.0022,0.0056,-0.13,0.21,-2.5e-06,0.43,-0.00019,0.00061,-2.2e-05,0,0,-3.6e+02,0.00013,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17090000,0.71,0.00055,-0.013,0.71,0.0058,0.0071,-0.015,-0.002,0.0032,-3.7e+02,-0.0012,-0.0059,-8.1e-05,0.0032,0.0059,-0.13,0.21,-2.8e-06,0.43,-0.0002,0.00061,-1e-05,0,0,-3.6e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17190000,0.71,0.00061,-0.013,0.71,0.0059,0.008,-0.016,-0.0028,0.0021,-3.7e+02,-0.0012,-0.0059,-7.7e-05,0.004,0.0059,-0.13,0.21,-3.1e-06,0.43,-0.0002,0.00062,-7.9e-06,0,0,-3.6e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 -17290000,0.71,0.00063,-0.013,0.71,0.0077,0.0088,-0.011,-0.0026,0.0025,-3.7e+02,-0.0012,-0.0059,-7.9e-05,0.0043,0.0068,-0.13,0.21,-3.5e-06,0.43,-0.00022,0.00061,-5.3e-06,0,0,-3.6e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.5e-06,2.3e-06,0.0067,0.0086,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17390000,0.71,0.00068,-0.013,0.71,0.0075,0.0092,-0.0095,-0.0024,0.0016,-3.7e+02,-0.0012,-0.0059,-7.3e-05,0.0051,0.0066,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.4e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17490000,0.71,0.00063,-0.013,0.71,0.0091,0.0093,-0.0078,-0.0012,0.0025,-3.7e+02,-0.0012,-0.0059,-7.3e-05,0.0045,0.0064,-0.13,0.21,-3.4e-06,0.43,-0.00023,0.00061,7.6e-06,0,0,-3.6e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17590000,0.71,0.0006,-0.013,0.71,0.0099,0.0085,-0.0024,-0.001,0.0013,-3.7e+02,-0.0012,-0.0059,-7e-05,0.0048,0.0066,-0.13,0.21,-3.7e-06,0.43,-0.00024,0.00061,4.9e-06,0,0,-3.6e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 -17690000,0.71,0.00058,-0.013,0.71,0.011,0.01,-0.003,1.7e-05,0.0023,-3.7e+02,-0.0012,-0.0059,-6.9e-05,0.0049,0.0064,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00061,1.3e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.045,0.046,0.048,1.3e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17790000,0.71,0.00056,-0.013,0.71,0.012,0.011,-0.0042,1.3e-05,0.0029,-3.7e+02,-0.0012,-0.0058,-6e-05,0.0059,0.0053,-0.13,0.21,-3.6e-06,0.43,-0.00024,0.00064,2.2e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17890000,0.71,0.00055,-0.013,0.71,0.015,0.012,-0.0041,0.0016,0.0045,-3.7e+02,-0.0012,-0.0058,-5.7e-05,0.0057,0.0046,-0.13,0.21,-3.3e-06,0.43,-0.00024,0.00064,2.7e-05,0,0,-3.6e+02,0.00013,0.00012,0.037,0.018,0.02,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0062,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -17990000,0.71,0.00049,-0.013,0.71,0.016,0.0091,-0.0027,0.0018,0.0036,-3.7e+02,-0.0012,-0.0058,-5.6e-05,0.0054,0.0049,-0.13,0.21,-3.4e-06,0.43,-0.00024,0.00064,2.4e-05,0,0,-3.6e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0078,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 -18090000,0.71,0.00048,-0.013,0.71,0.017,0.0083,-0.00045,0.0035,0.0037,-3.7e+02,-0.0012,-0.0059,-6.2e-05,0.0049,0.0059,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00061,1.9e-05,0,0,-3.6e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18190000,0.71,0.00044,-0.013,0.71,0.018,0.0093,0.001,0.0041,0.0036,-3.7e+02,-0.0012,-0.0059,-5.7e-05,0.0051,0.0056,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00062,2e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.04,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0076,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18290000,0.71,0.00035,-0.013,0.71,0.018,0.0088,0.0022,0.0061,0.0041,-3.7e+02,-0.0012,-0.0059,-6e-05,0.0047,0.0059,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00061,1.4e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18390000,0.71,0.00032,-0.013,0.71,0.02,0.011,0.0035,0.0068,0.0042,-3.7e+02,-0.0012,-0.0059,-5.4e-05,0.0045,0.0051,-0.13,0.21,-3.4e-06,0.43,-0.00024,0.00062,1.6e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 -18490000,0.71,0.00038,-0.013,0.71,0.021,0.012,0.0031,0.0085,0.0055,-3.7e+02,-0.0012,-0.0059,-5.3e-05,0.005,0.0052,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00063,1.9e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18590000,0.71,0.00039,-0.013,0.71,0.02,0.013,0.0014,0.0074,0.0054,-3.7e+02,-0.0012,-0.0058,-4.5e-05,0.0058,0.0047,-0.13,0.21,-3.7e-06,0.43,-0.00025,0.00065,2.5e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18690000,0.71,0.00031,-0.013,0.71,0.022,0.013,-0.00039,0.01,0.0065,-3.7e+02,-0.0012,-0.0059,-4.7e-05,0.0051,0.0047,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00064,1.8e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18790000,0.71,0.00032,-0.013,0.71,0.021,0.012,-0.00061,0.0087,0.0054,-3.7e+02,-0.0012,-0.0059,-4.6e-05,0.0053,0.0053,-0.13,0.21,-3.8e-06,0.43,-0.00025,0.00063,1.5e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00067,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 -18890000,0.71,0.00041,-0.013,0.71,0.021,0.014,1.9e-05,0.01,0.0075,-3.7e+02,-0.0012,-0.0058,-4e-05,0.0062,0.0047,-0.13,0.21,-3.9e-06,0.43,-0.00026,0.00065,2.9e-05,0,0,-3.6e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.007,0.00065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -18990000,0.71,0.00046,-0.013,0.71,0.021,0.015,-0.0012,0.01,0.0068,-3.7e+02,-0.0013,-0.0058,-3.4e-05,0.0069,0.0049,-0.13,0.21,-4.2e-06,0.43,-0.00027,0.00066,3e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.015,0.017,0.012,0.039,0.04,0.044,9.7e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.00062,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19090000,0.71,0.00051,-0.013,0.71,0.021,0.016,0.0018,0.012,0.0084,-3.7e+02,-0.0013,-0.0058,-3.4e-05,0.0075,0.0052,-0.13,0.21,-4.4e-06,0.43,-0.00028,0.00066,3.4e-05,0,0,-3.6e+02,0.00011,0.0001,0.037,0.016,0.019,0.012,0.042,0.044,0.044,9.6e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.0006,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19190000,0.71,0.00056,-0.013,0.71,0.02,0.016,0.0019,0.012,0.0076,-3.7e+02,-0.0013,-0.0059,-2.7e-05,0.008,0.0054,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00067,3.7e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.014,0.017,0.012,0.038,0.04,0.043,9.3e-07,1.5e-06,2.1e-06,0.0055,0.0068,0.00058,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 -19290000,0.71,0.00058,-0.013,0.71,0.02,0.015,0.0046,0.013,0.0088,-3.7e+02,-0.0013,-0.0059,-3e-05,0.0078,0.0058,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00066,4e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.015,0.019,0.012,0.042,0.044,0.043,9.2e-07,1.5e-06,2.1e-06,0.0055,0.0067,0.00056,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19390000,0.71,0.00054,-0.013,0.71,0.019,0.014,0.0084,0.012,0.0079,-3.7e+02,-0.0013,-0.0059,-2.4e-05,0.0078,0.0056,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00065,3.5e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.014,0.017,0.011,0.038,0.04,0.043,8.9e-07,1.5e-06,2.1e-06,0.0054,0.0066,0.00054,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19490000,0.71,0.00056,-0.013,0.71,0.019,0.015,0.0049,0.014,0.01,-3.7e+02,-0.0013,-0.0058,-1.8e-05,0.0078,0.0049,-0.13,0.21,-4.6e-06,0.43,-0.00029,0.00066,3.8e-05,0,0,-3.6e+02,0.00011,0.0001,0.036,0.015,0.018,0.011,0.042,0.044,0.043,8.8e-07,1.4e-06,2.1e-06,0.0054,0.0066,0.00052,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19590000,0.71,0.00063,-0.013,0.71,0.017,0.015,0.0043,0.012,0.0095,-3.7e+02,-0.0013,-0.0058,-6.7e-06,0.0086,0.0047,-0.13,0.21,-4.7e-06,0.43,-0.0003,0.00068,4.7e-05,0,0,-3.6e+02,0.00011,9.8e-05,0.036,0.014,0.017,0.011,0.038,0.039,0.042,8.5e-07,1.4e-06,2.1e-06,0.0053,0.0065,0.0005,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 -19690000,0.71,0.00067,-0.013,0.71,0.017,0.013,0.0059,0.013,0.01,-3.7e+02,-0.0013,-0.0059,-1e-05,0.0089,0.0053,-0.13,0.21,-5e-06,0.43,-0.00031,0.00068,3.9e-05,0,0,-3.6e+02,0.00011,9.8e-05,0.036,0.015,0.018,0.011,0.042,0.043,0.042,8.4e-07,1.4e-06,2.1e-06,0.0053,0.0064,0.00049,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19790000,0.71,0.00075,-0.013,0.71,0.014,0.011,0.0064,0.011,0.009,-3.7e+02,-0.0013,-0.0059,-5.6e-06,0.0094,0.0057,-0.13,0.21,-5.3e-06,0.43,-0.00031,0.00068,3.7e-05,0,0,-3.6e+02,0.00011,9.6e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.042,8.2e-07,1.3e-06,2.1e-06,0.0053,0.0063,0.00047,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19890000,0.71,0.00066,-0.013,0.71,0.015,0.013,0.0076,0.014,0.011,-3.7e+02,-0.0013,-0.0058,2.5e-06,0.0092,0.0046,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.00069,4.5e-05,0,0,-3.6e+02,0.00011,9.6e-05,0.036,0.015,0.018,0.01,0.041,0.043,0.042,8.1e-07,1.3e-06,2.1e-06,0.0052,0.0063,0.00046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -19990000,0.71,0.00063,-0.013,0.7,0.013,0.013,0.01,0.012,0.011,-3.7e+02,-0.0013,-0.0058,1.8e-05,0.0095,0.0038,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.0007,5e-05,0,0,-3.6e+02,0.0001,9.4e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00044,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 -20090000,0.71,0.00066,-0.013,0.7,0.013,0.013,0.011,0.013,0.013,-3.7e+02,-0.0013,-0.0058,2.8e-05,0.01,0.0031,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00072,6.1e-05,0,0,-3.6e+02,0.00011,9.4e-05,0.036,0.014,0.018,0.01,0.041,0.043,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20190000,0.71,0.0007,-0.013,0.7,0.012,0.012,0.013,0.011,0.012,-3.7e+02,-0.0013,-0.0058,3.7e-05,0.0099,0.0028,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00072,6.1e-05,0,0,-3.6e+02,0.0001,9.2e-05,0.036,0.013,0.016,0.0098,0.037,0.039,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00042,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20290000,0.71,0.0007,-0.013,0.7,0.01,0.012,0.011,0.012,0.013,-3.7e+02,-0.0013,-0.0058,4.1e-05,0.01,0.0027,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00074,6.2e-05,0,0,-3.6e+02,0.0001,9.2e-05,0.036,0.014,0.018,0.0097,0.041,0.043,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.006,0.0004,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20390000,0.71,0.00066,-0.013,0.7,0.0086,0.0097,0.013,0.01,0.012,-3.7e+02,-0.0013,-0.0058,4.5e-05,0.01,0.0028,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00073,5.2e-05,0,0,-3.6e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.005,0.0059,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 -20490000,0.71,0.00071,-0.013,0.7,0.0087,0.0097,0.013,0.011,0.012,-3.7e+02,-0.0013,-0.0058,4.2e-05,0.01,0.003,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00072,5.1e-05,0,0,-3.6e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.0059,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20590000,0.71,0.00074,-0.013,0.7,0.0078,0.0076,0.0099,0.0091,0.01,-3.7e+02,-0.0013,-0.0058,4.3e-05,0.01,0.0036,-0.13,0.21,-5e-06,0.43,-0.00031,0.00072,5e-05,0,0,-3.6e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0058,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20690000,0.71,0.00077,-0.013,0.7,0.0084,0.0077,0.011,0.01,0.011,-3.7e+02,-0.0013,-0.0058,4.6e-05,0.01,0.0034,-0.13,0.21,-5e-06,0.43,-0.00031,0.00072,5.2e-05,0,0,-3.6e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.0049,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20790000,0.71,0.00081,-0.013,0.7,0.0063,0.0072,0.012,0.0084,0.01,-3.7e+02,-0.0013,-0.0058,5.1e-05,0.011,0.0035,-0.13,0.21,-5e-06,0.43,-0.00032,0.00073,4.6e-05,0,0,-3.6e+02,9.8e-05,8.7e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 -20890000,0.71,0.00082,-0.013,0.7,0.0062,0.0068,0.011,0.009,0.011,-3.7e+02,-0.0013,-0.0058,5.9e-05,0.011,0.0031,-0.13,0.21,-5e-06,0.43,-0.00033,0.00074,5.2e-05,0,0,-3.6e+02,9.8e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.6e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -20990000,0.71,0.00084,-0.013,0.7,0.0044,0.0045,0.011,0.009,0.01,-3.7e+02,-0.0013,-0.0058,6.3e-05,0.011,0.0033,-0.13,0.21,-5e-06,0.43,-0.00034,0.00075,4.9e-05,0,0,-3.6e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21090000,0.71,0.00081,-0.013,0.7,0.0054,0.0037,0.012,0.01,0.011,-3.7e+02,-0.0013,-0.0058,6.8e-05,0.011,0.0029,-0.13,0.21,-4.9e-06,0.43,-0.00033,0.00075,4.6e-05,0,0,-3.6e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.8e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21190000,0.71,0.00082,-0.013,0.7,0.0057,0.0028,0.011,0.011,0.0091,-3.7e+02,-0.0013,-0.0058,6.8e-05,0.011,0.0031,-0.13,0.21,-4.9e-06,0.43,-0.00033,0.00075,4.3e-05,0,0,-3.6e+02,9.4e-05,8.4e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.2e-07,9.4e-07,1.9e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 -21290000,0.71,0.0009,-0.013,0.7,0.005,0.0029,0.013,0.011,0.01,-3.7e+02,-0.0013,-0.0058,7.9e-05,0.011,0.0026,-0.13,0.21,-4.9e-06,0.43,-0.00034,0.00077,4.7e-05,0,0,-3.6e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.4e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21390000,0.71,0.00088,-0.013,0.7,0.004,0.00086,0.013,0.0095,0.0093,-3.7e+02,-0.0013,-0.0058,7.4e-05,0.011,0.0028,-0.13,0.21,-5e-06,0.43,-0.00033,0.00076,4.7e-05,0,0,-3.6e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21490000,0.71,0.00088,-0.013,0.7,0.0045,0.0013,0.013,0.01,0.0098,-3.7e+02,-0.0013,-0.0058,7.9e-05,0.011,0.0024,-0.13,0.21,-5e-06,0.43,-0.00032,0.00077,5.3e-05,0,0,-3.6e+02,9.4e-05,8.3e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21590000,0.71,0.00087,-0.013,0.7,0.0033,0.0017,0.012,0.009,0.0091,-3.7e+02,-0.0013,-0.0058,7.7e-05,0.011,0.0025,-0.13,0.21,-5.1e-06,0.43,-0.00032,0.00076,5e-05,0,0,-3.6e+02,9.1e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 -21690000,0.71,0.00084,-0.013,0.7,0.0049,0.0021,0.014,0.01,0.0097,-3.7e+02,-0.0013,-0.0058,8.1e-05,0.011,0.0021,-0.13,0.21,-5e-06,0.43,-0.00031,0.00077,5.1e-05,0,0,-3.6e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.6e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21790000,0.71,0.00084,-0.013,0.7,0.003,0.0042,0.013,0.0074,0.01,-3.7e+02,-0.0013,-0.0058,7.2e-05,0.011,0.0023,-0.13,0.21,-5.6e-06,0.43,-0.00033,0.00076,5.3e-05,0,0,-3.6e+02,9e-05,8e-05,0.036,0.012,0.015,0.0079,0.036,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0052,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0047,0.013,0.0079,0.011,-3.7e+02,-0.0013,-0.0058,7.3e-05,0.011,0.0022,-0.13,0.21,-5.5e-06,0.43,-0.00033,0.00076,5e-05,0,0,-3.6e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0052,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 -21990000,0.71,0.00085,-0.013,0.7,0.0026,0.0054,0.014,0.006,0.012,-3.7e+02,-0.0013,-0.0058,7e-05,0.012,0.002,-0.13,0.21,-5.9e-06,0.43,-0.00034,0.00076,5.2e-05,0,0,-3.7e+02,8.8e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22090000,0.71,0.00088,-0.013,0.7,0.0025,0.007,0.012,0.006,0.013,-3.7e+02,-0.0013,-0.0058,7e-05,0.012,0.0021,-0.13,0.21,-5.9e-06,0.43,-0.00034,0.00076,5.2e-05,0,0,-3.7e+02,8.9e-05,7.9e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,7.9e-07,1.8e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22190000,0.71,0.00085,-0.013,0.7,0.0019,0.007,0.013,0.0053,0.011,-3.7e+02,-0.0013,-0.0058,7.6e-05,0.012,0.0021,-0.13,0.21,-5.7e-06,0.43,-0.00035,0.00077,4.1e-05,0,0,-3.7e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.6e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22290000,0.71,0.00088,-0.013,0.7,0.0013,0.0067,0.013,0.0057,0.012,-3.7e+02,-0.0013,-0.0058,7.4e-05,0.012,0.0022,-0.13,0.21,-5.8e-06,0.43,-0.00035,0.00076,4.2e-05,0,0,-3.7e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.6e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22390000,0.71,0.00089,-0.013,0.7,-0.001,0.0064,0.015,0.0041,0.01,-3.7e+02,-0.0013,-0.0058,8.1e-05,0.012,0.0024,-0.13,0.21,-5.7e-06,0.43,-0.00035,0.00077,4.3e-05,0,0,-3.7e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.3e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22490000,0.71,0.00093,-0.013,0.7,-0.0021,0.0072,0.016,0.0033,0.011,-3.7e+02,-0.0013,-0.0058,8.2e-05,0.012,0.0026,-0.13,0.21,-5.7e-06,0.43,-0.00037,0.00078,4.1e-05,0,0,-3.7e+02,8.6e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.3e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22590000,0.71,0.00096,-0.013,0.7,-0.0038,0.0065,0.015,0.00096,0.01,-3.7e+02,-0.0014,-0.0058,8.5e-05,0.013,0.0027,-0.13,0.21,-5.6e-06,0.43,-0.00038,0.00079,3.8e-05,0,0,-3.7e+02,8.4e-05,7.5e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.0049,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22690000,0.71,0.001,-0.013,0.7,-0.0052,0.008,0.016,0.00022,0.012,-3.7e+02,-0.0014,-0.0058,9.1e-05,0.013,0.0026,-0.13,0.21,-5.5e-06,0.43,-0.00039,0.0008,3.7e-05,0,0,-3.7e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7e-07,1.8e-06,0.0044,0.0049,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22790000,0.71,0.001,-0.013,0.7,-0.0071,0.0069,0.017,-0.0024,0.0091,-3.7e+02,-0.0014,-0.0058,8.1e-05,0.013,0.0034,-0.13,0.21,-5.7e-06,0.43,-0.00039,0.00078,4.3e-05,0,0,-3.7e+02,8.3e-05,7.4e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22890000,0.71,0.00099,-0.013,0.7,-0.0076,0.0078,0.019,-0.0024,0.0098,-3.7e+02,-0.0014,-0.0058,8e-05,0.013,0.0033,-0.13,0.21,-5.6e-06,0.43,-0.00038,0.00077,3.9e-05,0,0,-3.7e+02,8.4e-05,7.5e-05,0.036,0.013,0.016,0.0071,0.039,0.042,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -22990000,0.71,0.00097,-0.013,0.7,-0.0077,0.0068,0.02,-0.0036,0.009,-3.7e+02,-0.0014,-0.0058,9e-05,0.013,0.0032,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.00078,3.6e-05,0,0,-3.7e+02,8.2e-05,7.3e-05,0.036,0.012,0.015,0.007,0.036,0.038,0.036,4.7e-07,6.5e-07,1.7e-06,0.0043,0.0048,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23090000,0.71,0.00092,-0.013,0.7,-0.0081,0.0067,0.02,-0.0043,0.009,-3.7e+02,-0.0014,-0.0058,8.1e-05,0.012,0.0034,-0.13,0.21,-5.5e-06,0.43,-0.00038,0.00077,3.3e-05,0,0,-3.7e+02,8.3e-05,7.4e-05,0.036,0.013,0.016,0.007,0.039,0.042,0.036,4.6e-07,6.5e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23190000,0.71,0.00097,-0.013,0.7,-0.0094,0.0047,0.022,-0.0076,0.0078,-3.7e+02,-0.0014,-0.0058,8.4e-05,0.013,0.0036,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00076,2.5e-05,0,0,-3.7e+02,8.1e-05,7.2e-05,0.036,0.012,0.014,0.0069,0.036,0.038,0.035,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23290000,0.71,0.00089,-0.013,0.7,-0.0093,0.0042,0.022,-0.0081,0.0084,-3.7e+02,-0.0014,-0.0058,8.6e-05,0.012,0.0034,-0.13,0.21,-5.5e-06,0.43,-0.00037,0.00076,2.4e-05,0,0,-3.7e+02,8.1e-05,7.3e-05,0.036,0.013,0.016,0.0069,0.039,0.042,0.036,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0047,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23390000,0.71,0.00095,-0.013,0.7,-0.0095,0.003,0.02,-0.01,0.0072,-3.7e+02,-0.0014,-0.0058,8.8e-05,0.012,0.0035,-0.13,0.21,-5.6e-06,0.43,-0.00035,0.00075,2.6e-05,0,0,-3.7e+02,8e-05,7.1e-05,0.036,0.012,0.014,0.0068,0.036,0.038,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 -23490000,0.71,0.0033,-0.01,0.7,-0.016,0.0032,-0.013,-0.011,0.008,-3.7e+02,-0.0014,-0.0058,9.5e-05,0.012,0.0032,-0.13,0.21,-5.5e-06,0.43,-0.00034,0.00079,5.1e-05,0,0,-3.7e+02,8e-05,7.2e-05,0.036,0.013,0.015,0.0068,0.039,0.042,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 -23590000,0.71,0.0086,-0.0026,0.7,-0.027,0.0032,-0.045,-0.011,0.0075,-3.7e+02,-0.0014,-0.0058,9.1e-05,0.012,0.0032,-0.13,0.21,-5.5e-06,0.43,-0.00034,0.00084,0.00012,0,0,-3.7e+02,7.8e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0046,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23690000,0.71,0.0082,0.0032,0.71,-0.058,-0.0046,-0.095,-0.015,0.0075,-3.7e+02,-0.0014,-0.0058,9.1e-05,0.012,0.0032,-0.13,0.21,-5.5e-06,0.43,-0.00034,0.00078,9e-05,0,0,-3.7e+02,7.9e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23790000,0.71,0.0053,-0.00011,0.71,-0.083,-0.016,-0.15,-0.016,0.0069,-3.7e+02,-0.0013,-0.0058,9.3e-05,0.011,0.0028,-0.13,0.21,-4.7e-06,0.43,-0.00039,0.00078,0.00044,0,0,-3.7e+02,7.7e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23890000,0.71,0.0026,-0.0062,0.71,-0.1,-0.025,-0.2,-0.026,0.0047,-3.7e+02,-0.0013,-0.0058,9.2e-05,0.012,0.0029,-0.13,0.21,-4.5e-06,0.43,-0.00042,0.00083,0.00035,0,0,-3.7e+02,7.8e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -23990000,0.71,0.0013,-0.011,0.71,-0.1,-0.029,-0.26,-0.031,0.00042,-3.7e+02,-0.0013,-0.0058,9.8e-05,0.012,0.0029,-0.13,0.21,-4.2e-06,0.43,-0.00039,0.00083,0.00032,0,0,-3.7e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24090000,0.71,0.0025,-0.0095,0.71,-0.1,-0.028,-0.3,-0.041,-0.002,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0026,-0.13,0.21,-3.8e-06,0.43,-0.00042,0.0008,0.00036,0,0,-3.7e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24190000,0.71,0.0036,-0.0072,0.71,-0.11,-0.03,-0.35,-0.044,-0.0052,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0025,-0.13,0.21,-3e-06,0.43,-0.00042,0.00083,0.00036,0,0,-3.7e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.3e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24290000,0.71,0.0041,-0.0064,0.71,-0.12,-0.033,-0.41,-0.055,-0.0085,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0025,-0.13,0.21,-2.7e-06,0.43,-0.00046,0.00088,0.00042,0,0,-3.7e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.3e-07,1.6e-06,0.0041,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24390000,0.71,0.0042,-0.0065,0.71,-0.13,-0.041,-0.46,-0.062,-0.021,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0031,-0.13,0.21,1.4e-07,0.43,-0.00034,0.00092,0.00042,0,0,-3.7e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24490000,0.71,0.005,-0.0024,0.71,-0.14,-0.046,-0.51,-0.076,-0.025,-3.7e+02,-0.0013,-0.0058,0.00011,0.011,0.0031,-0.13,0.21,1.4e-07,0.43,-0.00034,0.00093,0.00041,0,0,-3.7e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 -24590000,0.71,0.0055,0.0013,0.71,-0.16,-0.057,-0.56,-0.081,-0.035,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.0031,-0.13,0.21,1.2e-06,0.43,1.8e-05,0.00058,0.00036,0,0,-3.7e+02,7.4e-05,6.6e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24690000,0.71,0.0056,0.0022,0.71,-0.18,-0.07,-0.64,-0.097,-0.041,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.0029,-0.13,0.21,2.2e-06,0.43,-2.7e-05,0.00062,0.00054,0,0,-3.7e+02,7.4e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24790000,0.71,0.0053,0.00098,0.71,-0.2,-0.083,-0.73,-0.11,-0.054,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.003,-0.13,0.21,1.6e-06,0.43,5.7e-06,0.00059,0.0003,0,0,-3.7e+02,7.3e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24890000,0.71,0.0071,0.0027,0.71,-0.22,-0.094,-0.75,-0.13,-0.063,-3.7e+02,-0.0013,-0.0058,0.00013,0.01,0.0031,-0.13,0.21,2.4e-06,0.43,-0.0001,0.00075,0.00032,0,0,-3.7e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -24990000,0.71,0.0089,0.0044,0.71,-0.24,-0.1,-0.81,-0.13,-0.073,-3.7e+02,-0.0013,-0.0058,0.00011,0.0098,0.0027,-0.13,0.21,2e-06,0.43,-0.00018,0.00085,-2e-05,0,0,-3.7e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.7e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25090000,0.71,0.0092,0.0038,0.71,-0.27,-0.11,-0.86,-0.16,-0.084,-3.7e+02,-0.0013,-0.0058,0.00011,0.0098,0.0028,-0.13,0.21,1.5e-06,0.43,-0.0002,0.00086,-5.6e-05,0,0,-3.7e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.7e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25190000,0.71,0.0087,0.0024,0.71,-0.3,-0.13,-0.91,-0.18,-0.11,-3.7e+02,-0.0013,-0.0058,0.00013,0.0092,0.0031,-0.13,0.21,6.9e-06,0.43,5.9e-05,0.00083,8e-05,0,0,-3.7e+02,7.1e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25290000,0.71,0.011,0.0093,0.71,-0.33,-0.14,-0.96,-0.21,-0.12,-3.7e+02,-0.0013,-0.0058,0.00013,0.0092,0.0031,-0.13,0.21,6.8e-06,0.43,8.4e-05,0.00077,8.7e-05,0,0,-3.7e+02,7.2e-05,6.4e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,-0.22,-0.14,-3.7e+02,-0.0013,-0.0058,0.00014,0.0084,0.0029,-0.13,0.21,1.1e-05,0.43,0.00049,0.00046,0.00012,0,0,-3.7e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,-0.26,-0.16,-3.7e+02,-0.0013,-0.0058,0.00014,0.0084,0.0027,-0.13,0.21,9.5e-06,0.43,0.00066,0.00013,0.00031,0,0,-3.7e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,-0.29,-0.2,-3.7e+02,-0.0012,-0.0058,0.00016,0.0077,0.0031,-0.13,0.21,1.6e-05,0.43,0.00096,0.00014,0.00032,0,0,-3.7e+02,7e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0042,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 -25690000,0.71,0.015,0.022,0.71,-0.5,-0.23,-1.2,-0.34,-0.22,-3.7e+02,-0.0012,-0.0058,0.00016,0.0077,0.0031,-0.13,0.21,1.6e-05,0.43,0.00095,0.00016,0.0004,0,0,-3.7e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0042,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 -25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,-0.36,-0.25,-3.7e+02,-0.0012,-0.0058,0.00016,0.007,0.0024,-0.13,0.21,1.9e-05,0.43,0.0013,-8.9e-05,-4.9e-05,0,0,-3.7e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25890000,0.71,0.018,0.029,0.71,-0.62,-0.29,-1.3,-0.41,-0.28,-3.7e+02,-0.0012,-0.0058,0.00017,0.0072,0.0024,-0.13,0.21,2.1e-05,0.43,0.0014,-2.1e-05,-0.00011,0,0,-3.7e+02,7.1e-05,6.2e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 -25990000,0.7,0.017,0.026,0.71,-0.67,-0.32,-1.3,-0.45,-0.33,-3.7e+02,-0.0012,-0.0058,0.00019,0.0063,0.0027,-0.13,0.21,2.9e-05,0.43,0.0023,-0.00058,-0.00057,0,0,-3.7e+02,7e-05,6.1e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 -26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,-0.53,-0.37,-3.7e+02,-0.0012,-0.0059,0.00018,0.0064,0.0029,-0.13,0.21,2.5e-05,0.43,0.0024,-0.0005,-0.0012,0,0,-3.7e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 -26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,-0.55,-0.41,-3.7e+02,-0.0012,-0.0058,0.00018,0.0053,0.0017,-0.13,0.21,3.9e-05,0.43,0.0023,0.00039,-0.0014,0,0,-3.7e+02,7e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 -26290000,0.7,0.025,0.047,0.71,-0.89,-0.43,-1.3,-0.64,-0.45,-3.7e+02,-0.0012,-0.0058,0.00018,0.0052,0.0018,-0.13,0.21,3.8e-05,0.43,0.0023,0.00027,-0.0014,0,0,-3.7e+02,7.1e-05,6.1e-05,0.03,0.015,0.028,0.0059,0.039,0.043,0.033,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.00099,3.9e-05,0.00099,0.0013,0.001,0.00099,1,1,0.01 -26390000,0.7,0.024,0.044,0.71,-0.96,-0.49,-1.3,-0.7,-0.54,-3.7e+02,-0.0011,-0.0059,0.00021,0.0044,0.0025,-0.13,0.21,4.6e-05,0.44,0.0036,-0.00019,-0.0024,0,0,-3.7e+02,7.1e-05,6e-05,0.028,0.014,0.027,0.0058,0.036,0.039,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00014,0.00096,3.9e-05,0.00095,0.0012,0.00096,0.00095,1,1,0.01 -26490000,0.7,0.031,0.06,0.71,-1.1,-0.53,-1.3,-0.8,-0.59,-3.7e+02,-0.0011,-0.0059,0.00021,0.0044,0.0025,-0.13,0.21,3.9e-05,0.44,0.004,-0.001,-0.0026,0,0,-3.7e+02,7.1e-05,6.1e-05,0.028,0.016,0.031,0.0058,0.039,0.044,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00014,0.00092,3.9e-05,0.00092,0.0012,0.00092,0.00091,1,1,0.01 -26590000,0.7,0.037,0.076,0.71,-1.2,-0.59,-1.3,-0.85,-0.66,-3.7e+02,-0.0011,-0.0059,0.00019,0.0028,0.0015,-0.13,0.21,3.9e-05,0.44,0.0041,-0.00065,-0.0048,0,0,-3.7e+02,7.1e-05,6e-05,0.025,0.015,0.031,0.0058,0.036,0.04,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00087,3.9e-05,0.00086,0.001,0.00087,0.00086,1,1,0.01 -26690000,0.7,0.039,0.079,0.71,-1.3,-0.65,-1.3,-0.97,-0.72,-3.7e+02,-0.0011,-0.0059,0.00019,0.0028,0.0014,-0.13,0.21,4.5e-05,0.44,0.004,-0.00014,-0.004,0,0,-3.7e+02,7.2e-05,6.1e-05,0.025,0.017,0.038,0.0058,0.04,0.045,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00081,3.9e-05,0.0008,0.001,0.00081,0.00079,1,1,0.01 -26790000,0.7,0.036,0.074,0.71,-1.4,-0.74,-1.3,-1.1,-0.85,-3.7e+02,-0.0011,-0.0059,0.00022,0.0012,0.0023,-0.13,0.21,8.4e-05,0.44,0.0055,0.00061,-0.0037,0,0,-3.7e+02,7.2e-05,6e-05,0.022,0.016,0.036,0.0057,0.036,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00076,3.9e-05,0.00075,0.00092,0.00076,0.00074,1,1,0.01 -26890000,0.7,0.045,0.096,0.71,-1.6,-0.8,-1.3,-1.2,-0.93,-3.7e+02,-0.0011,-0.0059,0.00023,0.0013,0.0023,-0.13,0.21,8.9e-05,0.44,0.0054,0.0012,-0.0041,0,0,-3.7e+02,7.3e-05,6e-05,0.022,0.018,0.043,0.0058,0.04,0.046,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00072,3.9e-05,0.0007,0.00092,0.00072,0.0007,1,1,0.01 -26990000,0.7,0.051,0.12,0.71,-1.7,-0.89,-1.3,-1.3,-1,-3.7e+02,-0.00099,-0.0059,0.00022,-0.00068,0.00038,-0.13,0.21,0.00013,0.44,0.0061,0.0034,-0.0056,0,0,-3.7e+02,7.3e-05,5.9e-05,0.019,0.017,0.042,0.0057,0.037,0.041,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00065,3.9e-05,0.00063,0.00079,0.00065,0.00062,1,1,0.01 -27090000,0.7,0.052,0.12,0.71,-1.9,-0.98,-1.3,-1.5,-1.1,-3.7e+02,-0.00099,-0.0059,0.00022,-0.00072,0.00039,-0.13,0.21,0.00013,0.44,0.006,0.0035,-0.0052,0,0,-3.7e+02,7.4e-05,6e-05,0.019,0.02,0.052,0.0057,0.04,0.048,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00059,3.9e-05,0.00056,0.00078,0.0006,0.00056,1,1,0.01 -27190000,0.7,0.05,0.11,0.7,-2.1,-1,-1.2,-1.7,-1.2,-3.7e+02,-0.00097,-0.0059,0.00022,-0.0018,0.0002,-0.13,0.21,4.8e-05,0.44,0.0021,0.0027,-0.0049,0,0,-3.7e+02,7.5e-05,6e-05,0.016,0.02,0.051,0.0057,0.043,0.05,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00055,3.9e-05,0.00051,0.00066,0.00055,0.00051,1,1,0.01 -27290000,0.71,0.044,0.095,0.7,-2.3,-1.1,-1.2,-1.9,-1.3,-3.7e+02,-0.00097,-0.0059,0.00023,-0.0018,0.00019,-0.13,0.21,5.6e-05,0.44,0.0019,0.0034,-0.0049,0,0,-3.7e+02,7.5e-05,6e-05,0.016,0.022,0.059,0.0057,0.047,0.057,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00052,3.9e-05,0.00048,0.00066,0.00052,0.00048,1,1,0.01 -27390000,0.71,0.038,0.079,0.7,-2.4,-1.1,-1.2,-2.1,-1.4,-3.7e+02,-0.00092,-0.0058,0.00022,-0.003,-0.0016,-0.13,0.21,1.3e-05,0.44,-0.0005,0.0031,-0.0063,0,0,-3.7e+02,7.6e-05,6e-05,0.013,0.021,0.052,0.0057,0.049,0.059,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00049,3.9e-05,0.00046,0.00055,0.00049,0.00046,1,1,0.01 -27490000,0.71,0.032,0.064,0.7,-2.5,-1.1,-1.2,-2.3,-1.5,-3.7e+02,-0.00092,-0.0058,0.00022,-0.0029,-0.0015,-0.13,0.21,1.9e-05,0.44,-0.00058,0.0032,-0.0067,0,0,-3.7e+02,7.7e-05,6.1e-05,0.013,0.023,0.056,0.0057,0.054,0.067,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00048,3.9e-05,0.00045,0.00055,0.00048,0.00044,1,1,0.01 -27590000,0.72,0.028,0.051,0.69,-2.6,-1.1,-1.2,-2.6,-1.6,-3.7e+02,-0.00092,-0.0059,0.00023,-0.0036,-0.00088,-0.12,0.21,-3.7e-05,0.44,-0.0032,0.0027,-0.0065,0,0,-3.7e+02,7.7e-05,6e-05,0.011,0.021,0.047,0.0057,0.056,0.068,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00044,1,1,0.01 -27690000,0.72,0.027,0.05,0.69,-2.6,-1.2,-1.2,-2.9,-1.7,-3.7e+02,-0.00092,-0.0059,0.00023,-0.0036,-0.00075,-0.12,0.21,-3.2e-05,0.44,-0.0033,0.0026,-0.0067,0,0,-3.7e+02,7.8e-05,6.1e-05,0.011,0.022,0.049,0.0057,0.062,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00043,1,1,0.01 -27790000,0.72,0.027,0.051,0.69,-2.6,-1.2,-1.2,-3.1,-1.8,-3.7e+02,-0.00092,-0.0059,0.00022,-0.0042,-0.00056,-0.12,0.21,-6e-05,0.44,-0.005,0.0021,-0.0072,0,0,-3.7e+02,7.8e-05,6e-05,0.0097,0.02,0.042,0.0056,0.064,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00045,3.9e-05,0.00043,0.00041,0.00045,0.00043,1,1,0.01 -27890000,0.72,0.027,0.049,0.69,-2.7,-1.2,-1.2,-3.4,-1.9,-3.7e+02,-0.00091,-0.0059,0.00023,-0.0042,-0.00056,-0.12,0.21,-6.1e-05,0.44,-0.005,0.002,-0.0072,0,0,-3.7e+02,7.9e-05,6.1e-05,0.0097,0.021,0.043,0.0057,0.07,0.087,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00044,3.9e-05,0.00043,0.00041,0.00044,0.00042,1,1,0.01 -27990000,0.72,0.026,0.046,0.69,-2.7,-1.2,-1.2,-3.7,-2,-3.7e+02,-0.00093,-0.0059,0.00024,-0.004,0.00035,-0.12,0.21,-7.7e-05,0.44,-0.0063,0.0015,-0.0071,0,0,-3.7e+02,8e-05,6e-05,0.0086,0.02,0.038,0.0056,0.072,0.087,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00043,3.9e-05,0.00042,0.00037,0.00043,0.00042,1,1,0.01 -28090000,0.72,0.032,0.059,0.69,-2.8,-1.2,-1.2,-4,-2.1,-3.7e+02,-0.00093,-0.0059,0.00023,-0.0041,0.00056,-0.12,0.21,-7.6e-05,0.44,-0.0065,0.0013,-0.0072,0,0,-3.7e+02,8e-05,6.1e-05,0.0086,0.021,0.039,0.0057,0.078,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00042,3.9e-05,0.00042,0.00036,0.00042,0.00041,1,1,0.01 -28190000,0.72,0.037,0.072,0.69,-2.8,-1.2,-0.94,-4.3,-2.2,-3.7e+02,-0.00094,-0.0059,0.00024,-0.0042,0.001,-0.12,0.21,-9.4e-05,0.44,-0.0073,0.00091,-0.0071,0,0,-3.7e+02,8.1e-05,6.1e-05,0.0079,0.02,0.034,0.0056,0.08,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00041,3.9e-05,0.00041,0.00033,0.00041,0.00041,1,1,0.01 -28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,-4.5,-2.4,-3.7e+02,-0.00094,-0.0059,0.00024,-0.0043,0.0011,-0.12,0.21,-9.7e-05,0.44,-0.0073,0.0013,-0.0069,0,0,-3.7e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,-4.8,-2.5,-3.7e+02,-0.00094,-0.0059,0.00023,-0.0042,0.0014,-0.12,0.21,-8.5e-05,0.44,-0.0075,0.0014,-0.007,0,0,-3.7e+02,8.3e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28490000,0.73,0.0028,0.0061,0.69,-2.8,-1.2,1.1,-5.1,-2.6,-3.7e+02,-0.00095,-0.0059,0.00023,-0.0038,0.0017,-0.12,0.21,-6.6e-05,0.44,-0.0076,0.0014,-0.0069,0,0,-3.7e+02,8.3e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28590000,0.73,0.00089,0.0025,0.69,-2.7,-1.2,0.97,-5.4,-2.7,-3.7e+02,-0.00095,-0.0059,0.00024,-0.0039,0.0017,-0.12,0.21,-6.9e-05,0.44,-0.0075,0.0015,-0.0069,0,0,-3.7e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 -28690000,0.73,0.00019,0.0016,0.69,-2.6,-1.2,0.98,-5.7,-2.8,-3.7e+02,-0.00095,-0.0059,0.00024,-0.0036,0.0021,-0.12,0.21,-5.2e-05,0.44,-0.0076,0.0014,-0.0068,0,0,-3.7e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 -28790000,0.73,-8.8e-06,0.0015,0.69,-2.6,-1.2,0.98,-6,-2.9,-3.7e+02,-0.00099,-0.0059,0.00024,-0.0031,0.0023,-0.12,0.21,-9.2e-05,0.44,-0.009,0.00063,-0.006,0,0,-3.7e+02,8.5e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.004,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 -28890000,0.73,1.1e-06,0.0018,0.69,-2.5,-1.2,0.97,-6.2,-3,-3.7e+02,-0.00099,-0.0059,0.00024,-0.0029,0.0026,-0.12,0.21,-7.6e-05,0.44,-0.0091,0.00059,-0.0059,0,0,-3.7e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 -28990000,0.73,0.00035,0.0024,0.68,-2.5,-1.1,0.97,-6.6,-3.1,-3.7e+02,-0.001,-0.0059,0.00025,-0.0016,0.003,-0.12,0.21,-0.00011,0.44,-0.01,-0.00033,-0.0047,0,0,-3.7e+02,8.7e-05,6.1e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00039,0.00039,1,1,0.01 -29090000,0.73,0.00052,0.0028,0.68,-2.4,-1.1,0.96,-6.8,-3.3,-3.7e+02,-0.001,-0.0059,0.00025,-0.0014,0.0034,-0.12,0.21,-9.3e-05,0.44,-0.011,-0.00039,-0.0046,0,0,-3.7e+02,8.7e-05,6.2e-05,0.0073,0.021,0.025,0.0057,0.14,0.17,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29190000,0.73,0.00077,0.0032,0.68,-2.4,-1.1,0.95,-7.1,-3.4,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00099,0.0034,-0.12,0.21,-0.00013,0.44,-0.011,-0.00062,-0.0041,0,0,-3.7e+02,8.8e-05,6.1e-05,0.0072,0.02,0.023,0.0056,0.14,0.17,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29290000,0.73,0.0011,0.004,0.68,-2.3,-1.1,0.98,-7.3,-3.5,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00096,0.0039,-0.12,0.21,-0.00012,0.44,-0.011,-0.00069,-0.0041,0,0,-3.7e+02,8.8e-05,6.1e-05,0.0072,0.021,0.024,0.0056,0.14,0.18,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 -29390000,0.73,0.0017,0.0055,0.68,-2.3,-1.1,0.99,-7.6,-3.6,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00023,0.0044,-0.12,0.21,-0.00015,0.44,-0.012,-0.0014,-0.0033,0,0,-3.7e+02,8.8e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.18,0.031,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 -29490000,0.73,0.0023,0.0066,0.68,-2.3,-1.1,0.99,-7.9,-3.7,-3.7e+02,-0.0011,-0.0059,0.00026,-0.00022,0.0046,-0.12,0.21,-0.00015,0.44,-0.012,-0.0013,-0.0033,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.15,0.19,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 -29590000,0.73,0.0027,0.0076,0.68,-2.2,-1.1,0.98,-8.1,-3.8,-3.7e+02,-0.0011,-0.0059,0.00026,0.0004,0.0046,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.19,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00038,1,1,0.01 -29690000,0.73,0.003,0.0082,0.68,-2.2,-1.1,0.98,-8.3,-3.9,-3.7e+02,-0.0011,-0.0059,0.00026,0.00033,0.005,-0.12,0.21,-0.00015,0.44,-0.012,-0.0016,-0.0029,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29790000,0.73,0.0034,0.0087,0.68,-2.2,-1.1,0.96,-8.6,-4,-3.7e+02,-0.0012,-0.0059,0.00025,0.0014,0.0048,-0.12,0.21,-0.00017,0.44,-0.012,-0.0019,-0.0023,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29890000,0.73,0.0034,0.0088,0.68,-2.1,-1.1,0.95,-8.8,-4.1,-3.7e+02,-0.0012,-0.0059,0.00025,0.0011,0.0053,-0.12,0.21,-0.00018,0.44,-0.013,-0.0019,-0.0023,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.17,0.21,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -29990000,0.73,0.0036,0.0089,0.68,-2.1,-1.1,0.94,-9.1,-4.2,-3.7e+02,-0.0012,-0.0059,0.00022,0.0016,0.005,-0.12,0.21,-0.00019,0.44,-0.013,-0.0021,-0.002,0,0,-3.7e+02,8.8e-05,6e-05,0.0071,0.02,0.024,0.0056,0.17,0.21,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -30090000,0.73,0.0035,0.0088,0.68,-2.1,-1.1,0.92,-9.3,-4.3,-3.7e+02,-0.0012,-0.0059,0.00022,0.0012,0.0053,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-3.7e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.18,0.22,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 -30190000,0.73,0.0036,0.0085,0.68,-2.1,-1.1,0.91,-9.5,-4.4,-3.7e+02,-0.0012,-0.0059,0.00021,0.0021,0.0047,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-3.7e+02,8.8e-05,6e-05,0.007,0.02,0.025,0.0055,0.18,0.22,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00038,0.00028,0.00037,0.00038,1,1,0.01 -30290000,0.73,0.0035,0.0083,0.68,-2,-1.1,0.9,-9.7,-4.5,-3.7e+02,-0.0012,-0.0059,0.00021,0.0019,0.005,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-3.7e+02,8.8e-05,6e-05,0.007,0.021,0.026,0.0056,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30390000,0.73,0.0035,0.008,0.68,-2,-1.1,0.89,-10,-4.6,-3.7e+02,-0.0012,-0.0059,0.00019,0.0026,0.0048,-0.12,0.21,-0.00022,0.43,-0.012,-0.0025,-0.0014,0,0,-3.7e+02,8.7e-05,6e-05,0.007,0.021,0.025,0.0055,0.19,0.23,0.031,2.8e-07,3.5e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30490000,0.73,0.0034,0.0077,0.68,-2,-1.1,0.87,-10,-4.7,-3.7e+02,-0.0012,-0.0059,0.00019,0.0026,0.005,-0.12,0.21,-0.00022,0.43,-0.012,-0.0024,-0.0014,0,0,-3.7e+02,8.7e-05,6e-05,0.007,0.022,0.027,0.0056,0.2,0.24,0.031,2.8e-07,3.5e-07,1.2e-06,0.0038,0.0039,9.8e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30590000,0.73,0.0034,0.0073,0.68,-1.9,-1,0.83,-10,-4.8,-3.7e+02,-0.0012,-0.0059,0.00018,0.0034,0.0046,-0.12,0.21,-0.00024,0.43,-0.012,-0.0025,-0.001,0,0,-3.7e+02,8.6e-05,6e-05,0.0069,0.021,0.026,0.0055,0.2,0.24,0.031,2.8e-07,3.5e-07,1.1e-06,0.0037,0.0038,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30690000,0.73,0.0031,0.0069,0.68,-1.9,-1,0.83,-11,-4.9,-3.7e+02,-0.0012,-0.0059,0.00018,0.0032,0.0049,-0.12,0.21,-0.00024,0.43,-0.012,-0.0024,-0.001,0,0,-3.7e+02,8.6e-05,6e-05,0.0069,0.022,0.028,0.0055,0.21,0.25,0.031,2.8e-07,3.5e-07,1.1e-06,0.0037,0.0038,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30790000,0.73,0.0031,0.0065,0.68,-1.9,-1,0.82,-11,-5,-3.7e+02,-0.0012,-0.0059,0.00015,0.0039,0.0043,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00078,0,0,-3.7e+02,8.4e-05,6e-05,0.0068,0.021,0.027,0.0055,0.21,0.25,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30890000,0.73,0.0029,0.006,0.68,-1.9,-1,0.81,-11,-5.1,-3.7e+02,-0.0013,-0.0059,0.00015,0.0039,0.0047,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00076,0,0,-3.7e+02,8.5e-05,6e-05,0.0068,0.022,0.029,0.0055,0.22,0.26,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 -30990000,0.73,0.003,0.0054,0.68,-1.8,-1,0.8,-11,-5.2,-3.7e+02,-0.0013,-0.0058,0.00012,0.0048,0.0042,-0.12,0.21,-0.00025,0.43,-0.011,-0.0027,-0.00042,0,0,-3.7e+02,8.3e-05,5.9e-05,0.0067,0.021,0.028,0.0055,0.22,0.26,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.5e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31090000,0.73,0.0027,0.0049,0.68,-1.8,-1,0.79,-11,-5.3,-3.7e+02,-0.0013,-0.0058,0.00012,0.0046,0.0046,-0.12,0.21,-0.00026,0.43,-0.011,-0.0027,-0.00042,0,0,-3.7e+02,8.3e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31190000,0.73,0.0026,0.0044,0.68,-1.8,-1,0.78,-12,-5.3,-3.7e+02,-0.0013,-0.0058,9.7e-05,0.0049,0.0045,-0.12,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00025,0,0,-3.7e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31290000,0.73,0.0023,0.0039,0.68,-1.8,-1,0.78,-12,-5.4,-3.7e+02,-0.0013,-0.0058,0.0001,0.0046,0.0049,-0.11,0.21,-0.00027,0.43,-0.011,-0.0027,-0.00028,0,0,-3.7e+02,8.2e-05,5.9e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 -31390000,0.73,0.0022,0.0032,0.68,-1.7,-0.99,0.78,-12,-5.5,-3.7e+02,-0.0013,-0.0058,7.8e-05,0.0051,0.0047,-0.11,0.21,-0.0003,0.43,-0.011,-0.0028,-2.8e-05,0,0,-3.7e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 -31490000,0.73,0.0019,0.0026,0.68,-1.7,-0.99,0.78,-12,-5.6,-3.7e+02,-0.0013,-0.0058,7.4e-05,0.005,0.0052,-0.11,0.21,-0.0003,0.43,-0.011,-0.0029,9.1e-06,0,0,-3.7e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 -31590000,0.73,0.002,0.0021,0.68,-1.7,-0.97,0.77,-12,-5.7,-3.7e+02,-0.0013,-0.0058,4.7e-05,0.0059,0.0048,-0.11,0.21,-0.00029,0.43,-0.01,-0.0029,0.00025,0,0,-3.7e+02,7.7e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31690000,0.73,0.0016,0.0014,0.68,-1.6,-0.97,0.78,-12,-5.8,-3.7e+02,-0.0013,-0.0058,4.9e-05,0.0056,0.0052,-0.11,0.21,-0.00029,0.43,-0.01,-0.0029,0.00021,0,0,-3.7e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31790000,0.73,0.0015,0.00057,0.69,-1.6,-0.95,0.78,-13,-5.9,-3.7e+02,-0.0013,-0.0058,2.4e-05,0.0066,0.0051,-0.11,0.21,-0.0003,0.43,-0.0097,-0.0029,0.00055,0,0,-3.7e+02,7.6e-05,5.9e-05,0.0062,0.021,0.029,0.0054,0.26,0.3,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31890000,0.73,0.0013,-0.00016,0.69,-1.6,-0.95,0.77,-13,-6,-3.7e+02,-0.0013,-0.0058,2.5e-05,0.0065,0.0056,-0.11,0.21,-0.00029,0.43,-0.0097,-0.0029,0.00058,0,0,-3.7e+02,7.6e-05,5.9e-05,0.0062,0.022,0.031,0.0054,0.27,0.31,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00026,0.00035,0.00037,1,1,0.01 -31990000,0.73,0.0012,-0.00077,0.69,-1.6,-0.93,0.77,-13,-6.1,-3.7e+02,-0.0013,-0.0058,-6.6e-06,0.007,0.0054,-0.11,0.21,-0.00029,0.43,-0.0092,-0.003,0.00075,0,0,-3.7e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0036,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32090000,0.73,0.00083,-0.0015,0.69,-1.5,-0.93,0.78,-13,-6.2,-3.7e+02,-0.0013,-0.0058,-7.5e-06,0.0068,0.0059,-0.11,0.21,-0.00029,0.43,-0.0093,-0.003,0.00076,0,0,-3.7e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0036,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32190000,0.73,0.00062,-0.0025,0.69,-1.5,-0.91,0.78,-13,-6.2,-3.7e+02,-0.0014,-0.0058,-4.2e-05,0.0073,0.006,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-3.7e+02,7.2e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32290000,0.73,0.00035,-0.0032,0.69,-1.5,-0.91,0.77,-14,-6.3,-3.7e+02,-0.0014,-0.0058,-4.1e-05,0.0071,0.0065,-0.11,0.2,-0.0003,0.43,-0.0088,-0.0031,0.001,0,0,-3.7e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 -32390000,0.73,0.00025,-0.004,0.69,-1.5,-0.89,0.77,-14,-6.4,-3.7e+02,-0.0014,-0.0058,-5.9e-05,0.0076,0.0064,-0.11,0.2,-0.00029,0.43,-0.0084,-0.0031,0.0011,0,0,-3.7e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00036,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32490000,0.72,0.0001,-0.0042,0.69,-1.4,-0.88,0.78,-14,-6.5,-3.7e+02,-0.0014,-0.0058,-5.7e-05,0.0074,0.0069,-0.11,0.21,-0.00029,0.43,-0.0084,-0.0031,0.0011,0,0,-3.7e+02,7.1e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32590000,0.72,0.00015,-0.0046,0.69,-1.4,-0.87,0.78,-14,-6.6,-3.7e+02,-0.0014,-0.0057,-7.8e-05,0.0078,0.0068,-0.11,0.21,-0.0003,0.43,-0.008,-0.0031,0.0013,0,0,-3.7e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 -32690000,0.72,0.00011,-0.0046,0.69,-1.4,-0.86,0.77,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-7.9e-05,0.0078,0.0072,-0.11,0.2,-0.0003,0.43,-0.008,-0.0031,0.0014,0,0,-3.7e+02,7e-05,5.8e-05,0.0056,0.022,0.032,0.0053,0.31,0.35,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00036,1,1,0.01 -32790000,0.72,0.00023,-0.0047,0.69,-1.3,-0.84,0.77,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-9.9e-05,0.0081,0.0072,-0.11,0.21,-0.0003,0.43,-0.0076,-0.0031,0.0015,0,0,-3.7e+02,6.8e-05,5.7e-05,0.0054,0.021,0.03,0.0053,0.3,0.35,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -32890000,0.72,0.00031,-0.0046,0.69,-1.3,-0.84,0.77,-14,-6.8,-3.7e+02,-0.0014,-0.0057,-0.00011,0.008,0.0078,-0.11,0.2,-0.0003,0.43,-0.0076,-0.0032,0.0016,0,0,-3.7e+02,6.9e-05,5.8e-05,0.0054,0.022,0.031,0.0053,0.32,0.36,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -32990000,0.72,0.00052,-0.0047,0.69,-1.3,-0.82,0.77,-15,-6.9,-3.7e+02,-0.0014,-0.0057,-0.00011,0.0085,0.008,-0.11,0.2,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-3.7e+02,6.7e-05,5.7e-05,0.0052,0.021,0.029,0.0053,0.31,0.36,0.03,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -33090000,0.72,0.00049,-0.0047,0.69,-1.3,-0.82,0.76,-15,-7,-3.7e+02,-0.0014,-0.0057,-0.00011,0.0084,0.0083,-0.11,0.21,-0.0003,0.43,-0.0072,-0.0032,0.0017,0,0,-3.7e+02,6.7e-05,5.7e-05,0.0053,0.022,0.031,0.0053,0.33,0.37,0.031,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 -33190000,0.72,0.0039,-0.0039,0.7,-1.2,-0.8,0.7,-15,-7,-3.7e+02,-0.0014,-0.0057,-0.00012,0.0085,0.0082,-0.11,0.21,-0.0003,0.43,-0.0068,-0.0031,0.0017,0,0,-3.7e+02,6.6e-05,5.7e-05,0.0051,0.022,0.029,0.0053,0.32,0.37,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0035,8.4e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 -33290000,0.67,0.016,-0.0033,0.74,-1.2,-0.78,0.68,-15,-7.1,-3.7e+02,-0.0014,-0.0057,-0.00011,0.0083,0.0084,-0.11,0.2,-0.00028,0.43,-0.007,-0.0032,0.0016,0,0,-3.7e+02,6.6e-05,5.7e-05,0.0051,0.022,0.031,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0035,8.3e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 -33390000,0.56,0.014,-0.0037,0.83,-1.2,-0.77,0.88,-15,-7.2,-3.7e+02,-0.0014,-0.0057,-0.00013,0.0085,0.0087,-0.11,0.21,-0.00034,0.43,-0.0063,-0.0032,0.0018,0,0,-3.7e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 -33490000,0.43,0.007,-0.0011,0.9,-1.2,-0.76,0.89,-15,-7.3,-3.7e+02,-0.0014,-0.0057,-0.00014,0.0085,0.0088,-0.11,0.21,-0.00042,0.43,-0.0058,-0.002,0.0018,0,0,-3.7e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 -33590000,0.27,0.00094,-0.0036,0.96,-1.2,-0.75,0.86,-15,-7.4,-3.7e+02,-0.0014,-0.0057,-0.00018,0.0085,0.0088,-0.11,0.21,-0.00067,0.43,-0.0038,-0.0013,0.002,0,0,-3.7e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 -33690000,0.098,-0.0027,-0.0066,1,-1.1,-0.74,0.87,-15,-7.4,-3.7e+02,-0.0014,-0.0057,-0.00019,0.0085,0.0088,-0.11,0.21,-0.00072,0.43,-0.0035,-0.001,0.002,0,0,-3.7e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 -33790000,-0.074,-0.0045,-0.0084,1,-1.1,-0.72,0.85,-16,-7.5,-3.7e+02,-0.0014,-0.0057,-0.00021,0.0085,0.0088,-0.11,0.21,-0.00088,0.43,-0.0021,-0.001,0.0023,0,0,-3.7e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 -33890000,-0.24,-0.0059,-0.009,0.97,-1,-0.69,0.83,-16,-7.6,-3.7e+02,-0.0014,-0.0057,-0.00021,0.0085,0.0088,-0.11,0.21,-0.001,0.43,-0.0012,-0.0011,0.0024,0,0,-3.7e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 -33990000,-0.39,-0.0046,-0.012,0.92,-0.94,-0.64,0.8,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00022,0.0085,0.0089,-0.11,0.21,-0.00097,0.43,-0.0011,-0.00064,0.0025,0,0,-3.7e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 -34090000,-0.5,-0.0037,-0.014,0.87,-0.88,-0.59,0.81,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00022,0.0085,0.0092,-0.11,0.21,-0.00095,0.43,-0.0012,-0.00052,0.0025,0,0,-3.7e+02,6e-05,5.3e-05,0.0014,0.023,0.03,0.0052,0.37,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,3e-05,3.4e-05,0.00036,2.2e-05,2.3e-05,0.00036,1,1,0.01 -34190000,-0.57,-0.0036,-0.012,0.82,-0.86,-0.54,0.81,-16,-7.8,-3.7e+02,-0.0015,-0.0057,-0.00021,0.0063,0.012,-0.11,0.21,-0.00094,0.43,-0.001,-0.00031,0.0027,0,0,-3.7e+02,5.7e-05,5.1e-05,0.0013,0.023,0.029,0.0052,0.37,0.38,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.5e-05,3.4e-05,0.00036,1.8e-05,1.8e-05,0.00036,1,1,0.01 -34290000,-0.61,-0.0046,-0.0091,0.79,-0.8,-0.48,0.81,-16,-7.8,-3.7e+02,-0.0015,-0.0057,-0.0002,0.0061,0.012,-0.11,0.21,-0.00096,0.43,-0.00092,-0.00018,0.0027,0,0,-3.7e+02,5.7e-05,5.1e-05,0.0012,0.025,0.032,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.2e-05,3.4e-05,0.00036,1.5e-05,1.5e-05,0.00036,1,1,0.01 -34390000,-0.63,-0.0052,-0.0062,0.77,-0.77,-0.44,0.81,-16,-7.9,-3.7e+02,-0.0015,-0.0057,-0.00018,0.0033,0.016,-0.11,0.21,-0.00092,0.43,-0.00091,1.6e-05,0.0029,0,0,-3.7e+02,5.3e-05,4.9e-05,0.0012,0.025,0.031,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.2e-05,2e-05,3.3e-05,0.00036,1.3e-05,1.3e-05,0.00036,1,1,0.01 -34490000,-0.65,-0.0062,-0.004,0.76,-0.72,-0.39,0.81,-16,-7.9,-3.7e+02,-0.0015,-0.0057,-0.00018,0.0031,0.017,-0.11,0.21,-0.00093,0.43,-0.00085,-2.5e-05,0.0029,0,0,-3.7e+02,5.3e-05,4.9e-05,0.0011,0.027,0.035,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.1e-05,1.8e-05,3.3e-05,0.00036,1.2e-05,1.2e-05,0.00036,1,1,0.01 -34590000,-0.66,-0.0062,-0.0026,0.75,-0.7,-0.37,0.8,-16,-8,-3.7e+02,-0.0015,-0.0058,-0.00015,-0.0018,0.023,-0.11,0.21,-0.00088,0.43,-0.00092,3e-05,0.0032,0,0,-3.7e+02,5e-05,4.7e-05,0.0011,0.027,0.034,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,1.1e-05,1e-05,0.00036,1,1,0.01 -34690000,-0.67,-0.0066,-0.0018,0.75,-0.64,-0.32,0.8,-17,-8.1,-3.7e+02,-0.0015,-0.0058,-0.00015,-0.0021,0.023,-0.11,0.21,-0.0009,0.43,-0.00078,0.00023,0.0031,0,0,-3.7e+02,5e-05,4.7e-05,0.0011,0.03,0.037,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,9.9e-06,9.4e-06,0.00036,1,1,0.01 -34790000,-0.67,-0.006,-0.0013,0.74,-0.63,-0.3,0.79,-17,-8.1,-3.7e+02,-0.0015,-0.0058,-0.00011,-0.008,0.03,-0.11,0.21,-0.00089,0.43,-0.00062,0.00032,0.0032,0,0,-3.7e+02,4.6e-05,4.4e-05,0.001,0.029,0.036,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,9.1e-06,8.6e-06,0.00036,1,1,0.01 -34890000,-0.67,-0.006,-0.0012,0.74,-0.57,-0.26,0.79,-17,-8.2,-3.7e+02,-0.0015,-0.0058,-0.00011,-0.0081,0.03,-0.11,0.21,-0.00089,0.43,-0.00063,0.00028,0.0033,0,0,-3.7e+02,4.6e-05,4.4e-05,0.001,0.032,0.04,0.0052,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,8.4e-06,7.9e-06,0.00036,1,1,0.01 -34990000,-0.67,-0.013,-0.0037,0.74,0.47,0.35,-0.043,-17,-8.2,-3.7e+02,-0.0016,-0.0058,-6.7e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00051,0.00034,0.0035,0,0,-3.7e+02,4.2e-05,4.1e-05,0.001,0.034,0.046,0.0054,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.3e-05,3.3e-05,0.00036,8e-06,7.4e-06,0.00036,1,1,0.01 -35090000,-0.67,-0.013,-0.0037,0.74,0.6,0.39,-0.1,-17,-8.2,-3.7e+02,-0.0016,-0.0058,-6.9e-05,-0.016,0.039,-0.11,0.21,-0.00087,0.43,-0.00047,0.00028,0.0035,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00099,0.037,0.051,0.0055,0.42,0.43,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.2e-05,3.3e-05,0.00036,7.5e-06,6.9e-06,0.00036,1,1,0.01 -35190000,-0.67,-0.013,-0.0038,0.74,0.63,0.43,-0.1,-17,-8.1,-3.7e+02,-0.0016,-0.0058,-6.9e-05,-0.016,0.039,-0.11,0.21,-0.00088,0.43,-0.00041,0.00032,0.0035,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00099,0.041,0.055,0.0055,0.43,0.44,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.2e-05,3.3e-05,0.00036,7.1e-06,6.4e-06,0.00036,1,1,0.01 -35290000,-0.67,-0.013,-0.0038,0.74,0.66,0.47,-0.099,-17,-8.1,-3.7e+02,-0.0016,-0.0058,-7.1e-05,-0.016,0.039,-0.11,0.21,-0.00089,0.43,-0.00033,0.00032,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00098,0.044,0.06,0.0055,0.44,0.45,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6.8e-06,6.1e-06,0.00036,1,1,0.01 -35390000,-0.67,-0.013,-0.0038,0.74,0.69,0.51,-0.096,-17,-8,-3.7e+02,-0.0016,-0.0058,-7.3e-05,-0.016,0.039,-0.11,0.21,-0.00091,0.43,-0.00023,0.00029,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00098,0.048,0.064,0.0055,0.46,0.47,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6.5e-06,5.8e-06,0.00036,1,1,0.01 -35490000,-0.67,-0.013,-0.0038,0.74,0.73,0.56,-0.095,-17,-8,-3.7e+02,-0.0016,-0.0058,-7.6e-05,-0.016,0.039,-0.11,0.21,-0.00092,0.43,-0.00014,0.00024,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00097,0.053,0.069,0.0055,0.47,0.48,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6.2e-06,5.5e-06,0.00036,1,1,0.01 -35590000,-0.67,-0.013,-0.0038,0.74,0.76,0.6,-0.094,-16,-7.9,-3.7e+02,-0.0016,-0.0058,-7.5e-05,-0.016,0.039,-0.11,0.21,-0.00091,0.43,-0.00017,0.00025,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00097,0.057,0.075,0.0055,0.49,0.5,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1.1e-05,3.3e-05,0.00036,6e-06,5.2e-06,0.00036,1,1,0.01 -35690000,-0.67,-0.013,-0.0038,0.74,0.79,0.64,-0.092,-16,-7.9,-3.7e+02,-0.0016,-0.0058,-7.6e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-9.2e-05,0.00024,0.0036,0,0,-3.7e+02,4.2e-05,4.1e-05,0.00097,0.062,0.08,0.0056,0.51,0.52,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.8e-06,5e-06,0.00036,1,1,0.01 -35790000,-0.67,-0.013,-0.0038,0.74,0.82,0.68,-0.089,-16,-7.8,-3.7e+02,-0.0016,-0.0058,-7.7e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-6.7e-05,0.00023,0.0036,0,0,-3.7e+02,4.2e-05,4.2e-05,0.00097,0.067,0.086,0.0056,0.53,0.54,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.6e-06,4.8e-06,0.00036,1,1,0.023 -35890000,-0.67,-0.013,-0.0038,0.74,0.85,0.72,-0.086,-16,-7.7,-3.7e+02,-0.0016,-0.0058,-7.8e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-5.2e-05,0.00022,0.0037,0,0,-3.7e+02,4.2e-05,4.2e-05,0.00097,0.072,0.092,0.0056,0.55,0.56,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.4e-06,4.6e-06,0.00036,1,1,0.048 -35990000,-0.67,-0.013,-0.0038,0.74,0.88,0.76,-0.083,-16,-7.7,-3.7e+02,-0.0016,-0.0058,-7.7e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-7.7e-05,0.00021,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.078,0.098,0.0056,0.57,0.59,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,9.8e-06,3.3e-05,0.00036,5.3e-06,4.5e-06,0.00036,1,1,0.073 -36090000,-0.68,-0.013,-0.0038,0.74,0.91,0.81,-0.079,-16,-7.6,-3.7e+02,-0.0016,-0.0058,-7.9e-05,-0.016,0.039,-0.11,0.21,-0.00093,0.43,-4.2e-05,0.00019,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.083,0.11,0.0056,0.6,0.62,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.7e-06,3.3e-05,0.00036,5.2e-06,4.3e-06,0.00036,1,1,0.099 -36190000,-0.68,-0.013,-0.0038,0.74,0.94,0.85,-0.075,-16,-7.5,-3.7e+02,-0.0016,-0.0058,-8.3e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,5.5e-05,0.00018,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.089,0.11,0.0056,0.63,0.65,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.5e-06,3.3e-05,0.00036,5e-06,4.2e-06,0.00036,1,1,0.12 -36290000,-0.68,-0.013,-0.0038,0.74,0.97,0.89,-0.07,-16,-7.4,-3.7e+02,-0.0016,-0.0058,-8.4e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,7.5e-05,0.00018,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.095,0.12,0.0056,0.66,0.68,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.4e-06,3.3e-05,0.00036,4.9e-06,4.1e-06,0.00036,1,1,0.15 -36390000,-0.68,-0.013,-0.0038,0.74,1,0.93,-0.067,-16,-7.3,-3.7e+02,-0.0016,-0.0058,-8.4e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,7.3e-05,0.00022,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.1,0.13,0.0056,0.69,0.72,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.3e-06,3.3e-05,0.00036,4.8e-06,3.9e-06,0.00036,1,1,0.17 -36490000,-0.68,-0.013,-0.0038,0.74,1,0.97,-0.063,-16,-7.2,-3.7e+02,-0.0016,-0.0058,-8.3e-05,-0.016,0.039,-0.11,0.21,-0.00095,0.43,4.3e-05,0.00023,0.0037,0,0,-3.7e+02,4.3e-05,4.2e-05,0.00096,0.11,0.13,0.0056,0.72,0.76,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.2e-06,3.3e-05,0.00036,4.7e-06,3.8e-06,0.00036,1,1,0.2 -36590000,-0.68,-0.013,-0.0038,0.74,1.1,1,-0.057,-16,-7.1,-3.7e+02,-0.0016,-0.0058,-8.5e-05,-0.015,0.039,-0.11,0.21,-0.00096,0.43,9.1e-05,0.00025,0.0037,0,0,-3.7e+02,4.3e-05,4.3e-05,0.00096,0.12,0.14,0.0056,0.76,0.8,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.1e-06,3.3e-05,0.00036,4.6e-06,3.7e-06,0.00036,1,1,0.23 -36690000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.053,-15,-7,-3.7e+02,-0.0016,-0.0058,-8.7e-05,-0.015,0.039,-0.11,0.21,-0.00096,0.43,0.00012,0.00026,0.0037,0,0,-3.7e+02,4.3e-05,4.3e-05,0.00096,0.12,0.15,0.0057,0.8,0.84,0.032,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9e-06,3.3e-05,0.00036,4.6e-06,3.6e-06,0.00036,1,1,0.25 -36790000,-0.68,-0.013,-0.0037,0.74,1.1,1.1,-0.047,-15,-6.9,-3.7e+02,-0.0016,-0.0058,-9.1e-05,-0.015,0.038,-0.11,0.21,-0.00097,0.43,0.00017,0.00022,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.13,0.16,0.0057,0.84,0.9,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.9e-06,3.3e-05,0.00036,4.5e-06,3.5e-06,0.00036,1,1,0.28 -36890000,-0.68,-0.013,-0.0037,0.74,1.2,1.1,-0.042,-15,-6.8,-3.7e+02,-0.0016,-0.0058,-9.4e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00021,0.00022,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0056,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00035,1,1,0.3 -36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,-15,-6.7,-3.7e+02,-0.0016,-0.0058,-9.5e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00023,0.00023,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00035,1,1,0.33 -37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,-15,-6.6,-3.7e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00023,0.00026,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00035,1,1,0.35 -37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,-15,-6.4,-3.7e+02,-0.0016,-0.0058,-9.6e-05,-0.015,0.038,-0.11,0.21,-0.00098,0.43,0.00023,0.00026,0.0037,0,0,-3.7e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00035,1,1,0.38 -37290000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.02,-15,-6.3,-3.7e+02,-0.0016,-0.0058,-9.7e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00024,0.00026,0.0037,0,0,-3.7e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00035,1,1,0.41 -37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,-15,-6.2,-3.7e+02,-0.0016,-0.0058,-9.9e-05,-0.015,0.038,-0.11,0.21,-0.00099,0.43,0.00027,0.00027,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00035,1,1,0.43 -37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0093,-14,-6,-3.7e+02,-0.0016,-0.0058,-0.0001,-0.015,0.038,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.3e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00035,1,1,0.46 -37590000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0027,-14,-5.9,-3.7e+02,-0.0016,-0.0058,-0.0001,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00032,0.0003,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.2,0.23,0.0057,1.3,1.5,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.3e-06,3.3e-05,0.00036,4e-06,3e-06,0.00035,1,1,0.48 -37690000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,0.0046,-14,-5.7,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00033,0.00029,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.21,0.24,0.0057,1.4,1.6,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.51 -37790000,-0.68,-0.013,-0.0036,0.74,1.5,1.5,0.012,-14,-5.6,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00034,0.0003,0.0037,0,0,-3.7e+02,4.5e-05,4.4e-05,0.00096,0.22,0.25,0.0056,1.5,1.7,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00035,1,1,0.54 -37890000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.017,-14,-5.4,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0038,0,0,-3.7e+02,4.5e-05,4.5e-05,0.00096,0.23,0.27,0.0056,1.6,1.8,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.56 -37990000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.025,-14,-5.3,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.015,0.037,-0.11,0.21,-0.001,0.43,0.00035,0.00028,0.0037,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.24,0.28,0.0056,1.7,1.9,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.59 -38090000,-0.68,-0.013,-0.0036,0.74,1.5,1.7,0.034,-14,-5.1,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00037,0.00029,0.0037,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.25,0.29,0.0056,1.8,2,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00035,1,1,0.61 -38190000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.04,-13,-4.9,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00029,0.0038,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.26,0.3,0.0056,1.9,2.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.64 -38290000,-0.68,-0.013,-0.0035,0.74,1.6,1.7,0.046,-13,-4.7,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00027,0.0038,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.27,0.31,0.0056,2.1,2.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00035,1,1,0.67 -38390000,-0.68,-0.013,-0.0035,0.74,1.6,1.8,0.052,-13,-4.6,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00029,0.0037,0,0,-3.7e+02,4.6e-05,4.5e-05,0.00096,0.28,0.32,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.69 -38490000,-0.68,-0.013,-0.0035,0.74,1.7,1.8,0.058,-13,-4.4,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00031,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00096,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00035,1,1,0.72 -38590000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.063,-13,-4.2,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00035,1,1,0.75 -38690000,-0.68,-0.013,-0.0034,0.74,1.7,1.9,0.069,-13,-4,-3.7e+02,-0.0016,-0.0058,-0.00011,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.77 -38790000,-0.68,-0.013,-0.0034,0.74,1.8,1.9,0.075,-12,-3.8,-3.7e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0037,0,0,-3.7e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.1,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 -38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,-12,-3.6,-3.7e+02,-0.0016,-0.0058,-0.00012,-0.014,0.037,-0.11,0.21,-0.001,0.43,0.0004,0.0003,0.0037,0,0,-3.7e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,0,0,-1.2e+02,-3e-11,-2.6e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 +290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,0,0,-1.2e+02,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 +390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0,0,-1.2e+02,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 +490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0,0,-1.2e+02,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 +590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0,0,-1.2e+02,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 +690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0,0,-1.2e+02,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,-1.2e+02,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 +790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0,0,-1.2e+02,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.018,0.018,0.00077,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 +890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0,0,-1.2e+02,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.019,0.019,0.00051,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 +990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0,0,-1.2e+02,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.021,0.021,0.00062,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 +1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0,0,-1.2e+02,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.023,0.023,0.00043,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 +1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0,0,-1.2e+02,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00051,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 +1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0,0,-1.2e+02,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.00038,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 +1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0,0,-1.2e+02,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00043,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 +1490000,1,-0.01,-0.014,0.00015,0.024,0.0029,-0.16,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0,0,-1.2e+02,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.03,0.03,0.00037,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.011,-0.014,0.00012,0.028,-9.5e-05,-0.19,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.011,-0.014,9.5e-05,0.035,-0.0019,-0.2,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.011,-0.015,7.5e-05,0.043,-0.0032,-0.22,0,0,-1.2e+02,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,8.5e-05,0.036,-0.0046,-0.23,0,0,-1.2e+02,0.00022,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,4.7e-05,0.041,-0.0071,-0.24,0,0,-1.2e+02,0.00022,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,5.9e-05,0.033,-0.0068,-0.26,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,4.5e-05,0.039,-0.0093,-0.27,0,0,-1.2e+02,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,6.2e-05,0.03,-0.0087,-0.29,0,0,-1.2e+02,9e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.013,4.4e-05,0.035,-0.011,-0.3,0,0,-1.2e+02,9e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.011,-0.013,5.9e-05,0.026,-0.009,-0.31,0,0,-1.2e+02,-1.4e-05,-0.0029,-5.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.011,-0.013,5.5e-05,0.03,-0.01,-0.33,0,0,-1.2e+02,-1.4e-05,-0.0029,-5.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.011,-0.013,4.9e-05,0.023,-0.0093,-0.34,0,0,-1.2e+02,-0.00012,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.011,-0.013,-1.1e-06,0.027,-0.011,-0.35,0,0,-1.2e+02,-0.00012,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.011,-0.013,4.6e-05,0.022,-0.0095,-0.36,0,0,-1.2e+02,-0.00023,-0.0036,-6.5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.011,-0.013,4.9e-05,0.025,-0.011,-0.38,0,0,-1.2e+02,-0.00023,-0.0036,-6.5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.011,-0.013,-8e-06,0.02,-0.0086,-0.39,0,0,-1.2e+02,-0.00034,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.011,-0.013,3.2e-05,0.023,-0.01,-0.4,0,0,-1.2e+02,-0.00034,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.011,-0.012,3.2e-06,0.018,-0.0091,-0.42,0,0,-1.2e+02,-0.00044,-0.0041,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.011,-0.013,-4.5e-06,0.022,-0.012,-0.43,0,0,-1.2e+02,-0.00044,-0.0041,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.011,-0.012,1.9e-05,0.017,-0.011,-0.44,0,0,-1.2e+02,-0.00055,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.011,-0.012,0.00014,0.019,-0.014,-0.46,0,0,-1.2e+02,-0.00055,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,-1.2e+02,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.011,-0.012,0.00019,0.016,-0.013,-0.47,0,0,-1.2e+02,-0.00067,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.48,0,0,-1.2e+02,-0.00067,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.011,-0.012,0.00016,0.02,-0.016,-0.5,0,0,-1.2e+02,-0.00067,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.51,0,0,-1.2e+02,-0.00079,-0.0047,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.011,-0.012,0.00013,0.02,-0.016,-0.53,0,0,-1.2e+02,-0.00079,-0.0047,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.01,-0.012,8.1e-05,0.017,-0.012,-0.54,0,0,-1.2e+02,-0.00091,-0.0049,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.01,-0.012,0.0001,0.018,-0.013,-0.55,0,0,-1.2e+02,-0.00091,-0.0049,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-1.2e+02,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.01,-0.012,0.00016,0.014,-0.0097,-0.57,0,0,-1.2e+02,-0.001,-0.005,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.01,-0.012,0.00019,0.017,-0.011,-0.58,0,0,-1.2e+02,-0.001,-0.005,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.01,-0.012,0.00019,0.014,-0.0096,-0.6,0,0,-1.2e+02,-0.0011,-0.0052,-7.4e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.01,-0.012,0.00018,0.015,-0.011,-0.61,0,0,-1.2e+02,-0.0011,-0.0052,-7.4e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-1.2e+02,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.01,-0.011,0.00017,0.012,-0.0097,-0.63,0,0,-1.2e+02,-0.0012,-0.0053,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.01,-0.012,0.00015,0.015,-0.01,-0.64,0,0,-1.2e+02,-0.0012,-0.0053,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.01,-0.011,0.0002,0.011,-0.0081,-0.66,0,0,-1.2e+02,-0.0013,-0.0054,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.01,-0.011,0.00022,0.013,-0.0095,-0.67,0,0,-1.2e+02,-0.0013,-0.0054,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.0099,-0.011,0.00021,0.0086,-0.007,-0.68,0,0,-1.2e+02,-0.0013,-0.0055,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0099,-0.011,0.00027,0.0081,-0.0078,-0.7,0,0,-1.2e+02,-0.0013,-0.0055,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-1.2e+02,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0098,-0.011,0.00028,0.0055,-0.0059,-0.71,0,0,-1.2e+02,-0.0014,-0.0055,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0097,-0.011,0.00026,0.0061,-0.0063,-0.73,0,0,-1.2e+02,-0.0014,-0.0055,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0096,-0.011,0.00033,0.0041,-0.0036,-0.74,0,0,-1.2e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0095,-0.011,0.00033,0.0044,-0.0025,-0.75,0,0,-1.2e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-1.2e+02,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0094,-0.011,0.00031,0.0038,-0.0007,0.0028,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0094,-0.011,0.00033,0.0041,0.00081,0.015,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0094,-0.011,0.00032,0.0051,0.002,-0.011,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0094,-0.011,0.00024,0.0038,0.0044,-0.005,0,0,-4.9e+02,-0.0015,-0.0056,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0094,-0.011,0.00022,0.005,0.0045,-0.012,0,0,-4.9e+02,-0.0015,-0.0056,-7.5e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0093,-0.011,0.00023,0.0043,0.0055,-0.05,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0093,-0.011,0.00023,0.0049,0.0057,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0093,-0.011,0.00016,0.0037,0.0057,-0.099,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0093,-0.011,9.4e-05,0.0046,0.0053,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0052,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 +6890000,0.71,0.0013,-0.014,0.7,0.00052,0.0065,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 +6990000,0.71,0.0013,-0.014,0.71,-0.00025,0.0048,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 +7090000,0.71,0.0012,-0.014,0.71,-0.00081,0.0013,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 +7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0021,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 +7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.01,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.012,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7590000,0.71,0.0017,-0.014,0.71,-0.0034,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 +7690000,0.71,0.0017,-0.014,0.71,-0.0046,0.018,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 +7790000,0.71,0.0017,-0.014,0.71,-0.011,0.017,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 +7890000,0.71,0.0017,-0.014,0.71,-0.011,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 +7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.023,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 +8090000,0.71,0.0017,-0.014,0.71,-0.0051,0.025,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8190000,0.71,0.0018,-0.014,0.71,-0.014,0.029,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8290000,0.71,0.0017,-0.014,0.71,-0.018,0.023,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00048,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00052,0.00039,-0.021,0.21,-9.6e-06,0.43,-0.00031,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 +8490000,0.71,0.0017,-0.014,0.71,-0.0029,0.0026,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00052,0.00039,-0.025,0.21,-9.3e-06,0.43,-0.00032,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8590000,0.71,0.0022,-0.014,0.71,-0.00042,0.00097,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-7e-05,-0.00052,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00043,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8690000,0.71,0.0021,-0.014,0.71,-0.0034,0.003,-0.16,0,0,-4.9e+02,-0.0017,-0.0056,-6.7e-05,-0.00052,0.00039,-0.035,0.21,-1.1e-05,0.43,-0.00041,0.00085,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.088,5.8e-05,6.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8790000,0.71,0.002,-0.014,0.71,-0.0056,0.0053,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00052,0.00043,-0.041,0.21,-9.5e-06,0.43,-0.00038,0.0008,-0.00029,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.087,5.6e-05,6.6e-05,2.4e-06,0.04,0.04,0.032,0.0013,6.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2 +8890000,0.71,0.0019,-0.014,0.71,-0.0077,0.0061,-0.15,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00065,0.00043,-0.045,0.21,-8.2e-06,0.43,-0.00036,0.00078,-0.00033,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.086,5.3e-05,6.4e-05,2.4e-06,0.04,0.04,0.03,0.0013,6.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +8990000,0.71,0.0019,-0.014,0.71,-0.01,0.0057,-0.14,0,0,-4.9e+02,-0.0015,-0.0058,-7.7e-05,-0.00088,0.00044,-0.051,0.21,-7e-06,0.43,-0.00032,0.00071,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.087,5.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.029,0.0013,6.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9090000,0.71,0.002,-0.014,0.71,-0.014,0.0071,-0.14,0,0,-4.9e+02,-0.0015,-0.0058,-7.7e-05,-0.001,0.00055,-0.053,0.21,-7.3e-06,0.43,-0.00034,0.00067,-0.00034,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1.1e+02,1.1e+02,0.086,4.9e-05,6.2e-05,2.4e-06,0.04,0.04,0.028,0.0013,5.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9190000,0.71,0.0019,-0.014,0.71,-0.012,0.01,-0.14,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00088,0.0006,-0.057,0.21,-6.7e-06,0.43,-0.00033,0.00081,-0.00025,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.094,1.1e+02,1.1e+02,0.085,4.6e-05,6e-05,2.4e-06,0.04,0.04,0.027,0.0013,5.6e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.3 +9290000,0.71,0.0018,-0.014,0.71,-0.011,0.01,-0.14,0,0,-4.9e+02,-0.0014,-0.0056,-7.3e-05,-0.00098,0.0006,-0.061,0.21,-5.5e-06,0.43,-0.00028,0.00082,-0.00024,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.1e+02,1.1e+02,0.085,4.4e-05,5.8e-05,2.4e-06,0.04,0.04,0.025,0.0013,5.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9390000,0.71,0.0016,-0.014,0.71,-0.011,0.011,-0.14,0,0,-4.9e+02,-0.0014,-0.0056,-7.4e-05,-0.0011,0.0006,-0.065,0.21,-4.7e-06,0.43,-0.00024,0.00081,-0.00022,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.093,1.2e+02,1.2e+02,0.086,4.2e-05,5.7e-05,2.4e-06,0.04,0.04,0.024,0.0013,5.2e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9490000,0.71,0.0015,-0.014,0.71,-0.0079,0.013,-0.13,0,0,-4.9e+02,-0.0013,-0.0055,-7.1e-05,-0.001,0.00062,-0.068,0.21,-4.1e-06,0.43,-0.00021,0.00089,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.091,1.2e+02,1.2e+02,0.085,4e-05,5.5e-05,2.4e-06,0.04,0.04,0.023,0.0013,5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9590000,0.71,0.0017,-0.014,0.71,-0.01,0.019,-0.13,0,0,-4.9e+02,-0.0014,-0.0055,-6.8e-05,-0.0011,0.00082,-0.072,0.21,-4.9e-06,0.43,-0.00027,0.0009,-0.00013,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.09,1.3e+02,1.3e+02,0.085,3.8e-05,5.4e-05,2.4e-06,0.04,0.04,0.021,0.0013,4.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.4 +9690000,0.71,0.0019,-0.014,0.71,-0.018,0.019,-0.12,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.0014,0.00095,-0.077,0.21,-5.2e-06,0.43,-0.00029,0.00079,-0.00017,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.089,1.3e+02,1.3e+02,0.086,3.6e-05,5.2e-05,2.4e-06,0.04,0.04,0.02,0.0013,4.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9790000,0.71,0.0018,-0.014,0.71,-0.013,0.023,-0.11,0,0,-4.9e+02,-0.0014,-0.0055,-7e-05,-0.0014,0.001,-0.082,0.21,-4.9e-06,0.43,-0.00027,0.00084,-0.0001,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.087,1.4e+02,1.4e+02,0.085,3.4e-05,5e-05,2.4e-06,0.04,0.04,0.019,0.0013,4.5e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9890000,0.71,0.0019,-0.014,0.71,-0.017,0.026,-0.11,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.0015,0.0011,-0.085,0.21,-5e-06,0.43,-0.00028,0.00081,-0.00011,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.084,1.4e+02,1.4e+02,0.085,3.2e-05,4.9e-05,2.4e-06,0.04,0.04,0.018,0.0013,4.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +9990000,0.71,0.002,-0.014,0.71,-0.021,0.024,-0.1,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.0017,0.0012,-0.089,0.21,-4.7e-06,0.43,-0.00028,0.00077,-0.00014,0,0,-4.9e+02,0.0013,0.0013,0.039,25,25,0.083,1.5e+02,1.5e+02,0.086,3.1e-05,4.7e-05,2.4e-06,0.04,0.04,0.017,0.0013,4.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.5 +10090000,0.71,0.0021,-0.014,0.71,-0.023,0.029,-0.096,0,0,-4.9e+02,-0.0015,-0.0056,-7e-05,-0.0017,0.0013,-0.091,0.21,-5.1e-06,0.43,-0.00031,0.00077,-0.00013,0,0,-4.9e+02,0.0013,0.0012,0.039,25,25,0.08,1.6e+02,1.6e+02,0.085,2.9e-05,4.6e-05,2.4e-06,0.04,0.04,0.016,0.0013,4.1e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10190000,0.71,0.0022,-0.014,0.71,-0.034,0.026,-0.096,0,0,-4.9e+02,-0.0015,-0.0058,-7.6e-05,-0.0019,0.0013,-0.093,0.21,-4.9e-06,0.43,-0.0003,0.00064,-0.00016,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.078,1.7e+02,1.7e+02,0.084,2.7e-05,4.4e-05,2.4e-06,0.04,0.04,0.014,0.0013,4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10290000,0.71,0.0021,-0.013,0.71,-0.041,0.022,-0.084,0,0,-4.9e+02,-0.0015,-0.0059,-8e-05,-0.0021,0.0015,-0.098,0.21,-4.6e-06,0.43,-0.00029,0.00056,-0.00018,0,0,-4.9e+02,0.0012,0.0012,0.039,25,25,0.076,1.8e+02,1.8e+02,0.085,2.6e-05,4.2e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10390000,0.71,0.002,-0.013,0.71,0.009,-0.02,0.0093,0,0,-4.9e+02,-0.0015,-0.0059,-8.1e-05,-0.0021,0.0014,-0.098,0.21,-4.1e-06,0.43,-0.00026,0.00058,-0.00014,0,0,-4.9e+02,0.0012,0.0012,0.039,0.25,0.25,0.56,0.5,0.5,0.077,2.4e-05,4.1e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.6 +10490000,0.71,0.0019,-0.013,0.71,0.007,-0.019,0.022,0,0,-4.9e+02,-0.0014,-0.0059,-8.5e-05,-0.0022,0.0014,-0.098,0.21,-3.7e-06,0.43,-0.00024,0.00052,-0.00017,0,0,-4.9e+02,0.0012,0.0012,0.039,0.26,0.26,0.55,0.51,0.51,0.079,2.3e-05,3.9e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10590000,0.71,0.0023,-0.013,0.71,0.0059,-0.0077,0.024,0,0,-4.9e+02,-0.0015,-0.0059,-8.1e-05,-0.0024,0.0018,-0.098,0.21,-5e-06,0.43,-0.00032,0.00051,-0.00018,0,0,-4.9e+02,0.0012,0.0011,0.039,0.13,0.13,0.27,0.17,0.17,0.072,2.2e-05,3.8e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10690000,0.71,0.0022,-0.013,0.71,0.0031,-0.0083,0.027,0,0,-4.9e+02,-0.0015,-0.0059,-8.3e-05,-0.0024,0.0018,-0.098,0.21,-4.6e-06,0.43,-0.0003,0.0005,-0.00018,0,0,-4.9e+02,0.0012,0.0011,0.039,0.14,0.14,0.26,0.18,0.18,0.077,2.1e-05,3.6e-05,2.3e-06,0.04,0.04,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10790000,0.71,0.0021,-0.013,0.71,0.0032,-0.0057,0.021,0,0,-4.9e+02,-0.0015,-0.0059,-8.2e-05,-0.0025,0.0022,-0.098,0.21,-4.5e-06,0.43,-0.00029,0.00054,-0.00017,0,0,-4.9e+02,0.0012,0.0011,0.038,0.093,0.094,0.17,0.11,0.11,0.071,1.9e-05,3.4e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.7 +10890000,0.71,0.002,-0.013,0.71,0.0018,-0.0056,0.016,0,0,-4.9e+02,-0.0014,-0.0059,-8.3e-05,-0.0025,0.0021,-0.098,0.21,-4.2e-06,0.43,-0.00027,0.00055,-0.00016,0,0,-4.9e+02,0.0011,0.0011,0.038,0.1,0.1,0.16,0.11,0.11,0.074,1.8e-05,3.3e-05,2.3e-06,0.039,0.039,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +10990000,0.71,0.0018,-0.014,0.71,0.0065,-0.00059,0.011,0,0,-4.9e+02,-0.0013,-0.0057,-8.1e-05,-0.0025,0.0031,-0.098,0.21,-3.7e-06,0.43,-0.00025,0.00069,-0.00012,0,0,-4.9e+02,0.0011,0.001,0.038,0.079,0.08,0.12,0.079,0.079,0.071,1.7e-05,3.1e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11090000,0.71,0.0019,-0.014,0.71,0.0064,0.003,0.014,0,0,-4.9e+02,-0.0014,-0.0056,-7.7e-05,-0.0023,0.0029,-0.098,0.21,-3.8e-06,0.43,-0.00026,0.00076,-7.8e-05,0,0,-4.9e+02,0.0011,0.00099,0.038,0.09,0.091,0.12,0.085,0.085,0.073,1.6e-05,2.9e-05,2.3e-06,0.038,0.038,0.014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11190000,0.71,0.0018,-0.014,0.71,0.0098,0.0044,0.0027,0,0,-4.9e+02,-0.0013,-0.0057,-8e-05,-0.0021,0.004,-0.098,0.21,-3.9e-06,0.43,-0.00027,0.00077,-9.5e-05,0,0,-4.9e+02,0.00098,0.00092,0.038,0.074,0.075,0.091,0.066,0.066,0.068,1.5e-05,2.7e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.8 +11290000,0.71,0.0017,-0.014,0.71,0.0089,0.003,0.0014,0,0,-4.9e+02,-0.0013,-0.0057,-8.5e-05,-0.0025,0.0043,-0.098,0.21,-3.5e-06,0.43,-0.00024,0.00071,-0.00012,0,0,-4.9e+02,0.00098,0.00091,0.038,0.086,0.087,0.087,0.072,0.072,0.071,1.4e-05,2.6e-05,2.3e-06,0.036,0.036,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11390000,0.71,0.0017,-0.013,0.71,0.0044,0.002,-0.0044,0,0,-4.9e+02,-0.0013,-0.0058,-8.7e-05,-0.003,0.004,-0.098,0.21,-3.4e-06,0.43,-0.00024,0.00064,-0.00017,0,0,-4.9e+02,0.00087,0.00082,0.038,0.072,0.073,0.072,0.058,0.058,0.067,1.3e-05,2.4e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11490000,0.71,0.0015,-0.013,0.71,0.00059,-0.00071,-0.0044,0,0,-4.9e+02,-0.0012,-0.006,-9.4e-05,-0.0036,0.0048,-0.098,0.21,-3.2e-06,0.43,-0.00021,0.00054,-0.00023,0,0,-4.9e+02,0.00086,0.00081,0.038,0.083,0.086,0.069,0.064,0.064,0.068,1.3e-05,2.3e-05,2.3e-06,0.034,0.034,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11590000,0.71,0.0014,-0.013,0.71,-0.0025,-0.00042,-0.01,0,0,-4.9e+02,-0.0012,-0.006,-9.6e-05,-0.0041,0.0049,-0.098,0.21,-2.9e-06,0.43,-0.00018,0.00052,-0.00024,0,0,-4.9e+02,0.00075,0.00072,0.038,0.07,0.072,0.059,0.054,0.054,0.066,1.2e-05,2.1e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.9 +11690000,0.71,0.0012,-0.013,0.71,-0.0041,-0.0014,-0.016,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.0049,0.005,-0.098,0.21,-2.5e-06,0.43,-0.00013,0.00051,-0.00027,0,0,-4.9e+02,0.00075,0.00071,0.038,0.082,0.084,0.057,0.06,0.06,0.066,1.1e-05,2e-05,2.3e-06,0.031,0.032,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11790000,0.71,0.0011,-0.013,0.71,-0.008,0.00022,-0.018,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0066,0.0052,-0.098,0.21,-2.4e-06,0.43,-0.00011,0.00049,-0.00028,0,0,-4.9e+02,0.00065,0.00063,0.038,0.068,0.07,0.051,0.051,0.051,0.063,1e-05,1.8e-05,2.3e-06,0.028,0.03,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11890000,0.71,0.001,-0.013,0.71,-0.009,-0.0025,-0.02,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0072,0.0057,-0.098,0.21,-2.4e-06,0.43,-7.7e-05,0.00045,-0.00033,0,0,-4.9e+02,0.00065,0.00062,0.038,0.08,0.082,0.049,0.058,0.058,0.063,9.8e-06,1.8e-05,2.3e-06,0.028,0.029,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +11990000,0.71,0.0014,-0.013,0.71,-0.012,0.0022,-0.026,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0067,0.0062,-0.097,0.21,-3.3e-06,0.43,-0.00015,0.00047,-0.00032,0,0,-4.9e+02,0.00056,0.00055,0.037,0.066,0.068,0.045,0.049,0.049,0.062,9.2e-06,1.6e-05,2.3e-06,0.026,0.028,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3 +12090000,0.71,0.0015,-0.013,0.71,-0.014,0.0055,-0.032,0,0,-4.9e+02,-0.0012,-0.006,-9.6e-05,-0.0056,0.0054,-0.097,0.21,-3.4e-06,0.43,-0.00018,0.00052,-0.00028,0,0,-4.9e+02,0.00056,0.00055,0.037,0.077,0.079,0.045,0.056,0.057,0.062,8.8e-06,1.6e-05,2.3e-06,0.026,0.027,0.013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12190000,0.71,0.0012,-0.013,0.71,-0.0072,0.0049,-0.026,0,0,-4.9e+02,-0.0011,-0.0059,-9.8e-05,-0.0054,0.0057,-0.099,0.21,-2.7e-06,0.43,-0.00013,0.00059,-0.00027,0,0,-4.9e+02,0.00048,0.00048,0.037,0.064,0.066,0.041,0.048,0.048,0.059,8.2e-06,1.4e-05,2.3e-06,0.023,0.026,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12290000,0.71,0.0011,-0.013,0.71,-0.0085,0.0054,-0.026,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.0059,0.0053,-0.099,0.21,-2.4e-06,0.43,-0.0001,0.0006,-0.00026,0,0,-4.9e+02,0.00048,0.00048,0.037,0.073,0.076,0.042,0.056,0.056,0.06,7.9e-06,1.4e-05,2.3e-06,0.023,0.025,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12390000,0.71,0.001,-0.013,0.71,-0.0057,0.004,-0.023,0,0,-4.9e+02,-0.0011,-0.0059,-0.0001,-0.0045,0.007,-0.1,0.21,-2.8e-06,0.43,-0.00014,0.0006,-0.00025,0,0,-4.9e+02,0.00042,0.00043,0.037,0.061,0.063,0.039,0.048,0.048,0.058,7.4e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.1 +12490000,0.71,0.00098,-0.013,0.71,-0.0074,0.0036,-0.027,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0049,0.0082,-0.1,0.21,-3.1e-06,0.43,-0.00017,0.00054,-0.00025,0,0,-4.9e+02,0.00042,0.00042,0.037,0.069,0.072,0.04,0.055,0.055,0.058,7.1e-06,1.3e-05,2.3e-06,0.021,0.024,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12590000,0.71,0.0012,-0.013,0.71,-0.015,0.0044,-0.033,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0042,0.0074,-0.1,0.21,-3.3e-06,0.43,-0.00021,0.00051,-0.00024,0,0,-4.9e+02,0.00037,0.00038,0.037,0.058,0.059,0.038,0.047,0.048,0.057,6.8e-06,1.2e-05,2.3e-06,0.019,0.022,0.012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12690000,0.71,0.0014,-0.013,0.71,-0.018,0.0053,-0.037,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0024,0.0088,-0.1,0.21,-4.3e-06,0.43,-0.00028,0.00049,-0.00025,0,0,-4.9e+02,0.00037,0.00038,0.037,0.065,0.067,0.039,0.055,0.055,0.057,6.5e-06,1.1e-05,2.3e-06,0.019,0.022,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12790000,0.71,0.0013,-0.013,0.71,-0.02,0.0033,-0.041,0,0,-4.9e+02,-0.0012,-0.006,-0.0001,-0.0039,0.0076,-0.1,0.21,-3.6e-06,0.43,-0.00023,0.00048,-0.00027,0,0,-4.9e+02,0.00033,0.00034,0.037,0.054,0.056,0.037,0.047,0.047,0.056,6.2e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.2 +12890000,0.71,0.0011,-0.013,0.71,-0.02,0.002,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0052,0.0072,-0.1,0.21,-3.2e-06,0.43,-0.00019,0.00047,-0.00026,0,0,-4.9e+02,0.00033,0.00034,0.037,0.061,0.063,0.038,0.054,0.055,0.057,6e-06,1.1e-05,2.3e-06,0.018,0.021,0.011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +12990000,0.71,0.001,-0.013,0.71,-0.0092,0.0023,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.0001,-0.0027,0.0076,-0.1,0.21,-3e-06,0.43,-0.0002,0.00058,-0.0002,0,0,-4.9e+02,0.0003,0.00031,0.037,0.051,0.053,0.037,0.047,0.047,0.055,5.7e-06,9.9e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13090000,0.71,0.00097,-0.013,0.71,-0.0099,0.00034,-0.039,0,0,-4.9e+02,-0.0011,-0.006,-0.00011,-0.0038,0.0092,-0.1,0.21,-3.4e-06,0.43,-0.00022,0.00052,-0.00023,0,0,-4.9e+02,0.0003,0.00031,0.037,0.057,0.059,0.038,0.054,0.054,0.056,5.5e-06,9.6e-06,2.3e-06,0.016,0.02,0.01,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13190000,0.71,0.00097,-0.013,0.71,-0.0025,0.0012,-0.034,0,0,-4.9e+02,-0.0011,-0.006,-0.00011,-0.002,0.011,-0.11,0.21,-3.8e-06,0.43,-0.00025,0.00057,-0.00021,0,0,-4.9e+02,0.00027,0.00029,0.037,0.048,0.05,0.037,0.047,0.047,0.054,5.2e-06,9.1e-06,2.3e-06,0.015,0.019,0.0098,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.3 +13290000,0.71,0.00081,-0.013,0.71,-0.00098,0.0019,-0.029,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0035,0.009,-0.11,0.21,-2.8e-06,0.43,-0.00019,0.00058,-0.00019,0,0,-4.9e+02,0.00027,0.00028,0.037,0.054,0.055,0.038,0.054,0.054,0.055,5.1e-06,8.9e-06,2.3e-06,0.015,0.019,0.0097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13390000,0.71,0.00072,-0.013,0.71,8e-05,0.0026,-0.024,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0029,0.0083,-0.11,0.21,-2.6e-06,0.43,-0.00017,0.00063,-0.00019,0,0,-4.9e+02,0.00025,0.00026,0.037,0.045,0.047,0.037,0.047,0.047,0.054,4.8e-06,8.5e-06,2.3e-06,0.014,0.018,0.0091,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13490000,0.71,0.00072,-0.013,0.71,5e-05,0.0027,-0.022,0,0,-4.9e+02,-0.001,-0.0059,-0.0001,-0.0029,0.0075,-0.11,0.21,-2.2e-06,0.43,-0.00016,0.00064,-0.00017,0,0,-4.9e+02,0.00025,0.00026,0.037,0.05,0.052,0.038,0.054,0.054,0.055,4.7e-06,8.2e-06,2.3e-06,0.014,0.018,0.009,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13590000,0.71,0.00074,-0.013,0.71,-0.00015,0.003,-0.024,0,0,-4.9e+02,-0.001,-0.006,-0.0001,-0.0027,0.0087,-0.11,0.21,-2.7e-06,0.43,-0.00019,0.00063,-0.00019,0,0,-4.9e+02,0.00023,0.00025,0.037,0.042,0.044,0.037,0.046,0.047,0.054,4.5e-06,7.9e-06,2.3e-06,0.013,0.017,0.0085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.4 +13690000,0.71,0.00072,-0.013,0.71,0.00078,0.0055,-0.029,0,0,-4.9e+02,-0.001,-0.0059,-9.9e-05,-0.0021,0.0075,-0.11,0.21,-2.3e-06,0.43,-0.00017,0.00065,-0.00016,0,0,-4.9e+02,0.00023,0.00024,0.037,0.047,0.049,0.038,0.053,0.054,0.055,4.3e-06,7.7e-06,2.3e-06,0.013,0.017,0.0083,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13790000,0.71,0.00076,-0.013,0.71,0.0005,0.0022,-0.03,0,0,-4.9e+02,-0.0011,-0.006,-9.9e-05,-0.00065,0.0082,-0.11,0.21,-2.7e-06,0.43,-0.00021,0.00066,-0.00014,0,0,-4.9e+02,0.00022,0.00023,0.037,0.04,0.041,0.036,0.046,0.046,0.053,4.2e-06,7.3e-06,2.3e-06,0.013,0.016,0.0078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13890000,0.71,0.0006,-0.013,0.71,0.0023,0.0023,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-9.9e-05,-0.002,0.0071,-0.11,0.21,-2e-06,0.43,-0.00016,0.00066,-0.00015,0,0,-4.9e+02,0.00022,0.00023,0.037,0.044,0.046,0.037,0.053,0.053,0.055,4e-06,7.1e-06,2.3e-06,0.012,0.016,0.0076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +13990000,0.71,0.00067,-0.013,0.71,0.0018,0.00057,-0.034,0,0,-4.9e+02,-0.001,-0.0059,-9.8e-05,-0.00073,0.0076,-0.11,0.21,-2.3e-06,0.43,-0.0002,0.00066,-0.00013,0,0,-4.9e+02,0.00021,0.00022,0.037,0.037,0.039,0.036,0.046,0.046,0.054,3.9e-06,6.8e-06,2.3e-06,0.012,0.015,0.0071,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.5 +14090000,0.71,0.00073,-0.013,0.71,0.0013,0.0019,-0.035,0,0,-4.9e+02,-0.0011,-0.0059,-9.4e-05,0.00078,0.0065,-0.11,0.21,-2e-06,0.43,-0.0002,0.0007,-8.9e-05,0,0,-4.9e+02,0.00021,0.00022,0.037,0.041,0.043,0.036,0.053,0.053,0.054,3.8e-06,6.7e-06,2.3e-06,0.012,0.015,0.007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14190000,0.71,0.00069,-0.014,0.71,0.0044,0.0018,-0.037,0,0,-4.9e+02,-0.0011,-0.0059,-9.3e-05,0.0013,0.0062,-0.11,0.21,-1.8e-06,0.43,-0.00019,0.00072,-7.1e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.035,0.037,0.035,0.046,0.046,0.054,3.6e-06,6.4e-06,2.3e-06,0.011,0.015,0.0065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14290000,0.71,0.00076,-0.014,0.71,0.0046,0.0031,-0.035,0,0,-4.9e+02,-0.0011,-0.0059,-9.2e-05,0.0021,0.0062,-0.11,0.21,-1.9e-06,0.43,-0.0002,0.00072,-5.3e-05,0,0,-4.9e+02,0.0002,0.00021,0.037,0.039,0.04,0.036,0.052,0.052,0.055,3.5e-06,6.2e-06,2.3e-06,0.011,0.014,0.0063,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14390000,0.71,0.00064,-0.014,0.71,0.007,0.0049,-0.037,0,0,-4.9e+02,-0.0011,-0.0058,-8.9e-05,0.0018,0.0047,-0.11,0.21,-1.1e-06,0.43,-0.00016,0.00075,-3.7e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.033,0.035,0.034,0.046,0.046,0.053,3.4e-06,6e-06,2.3e-06,0.011,0.014,0.0059,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.6 +14490000,0.71,0.00053,-0.014,0.71,0.008,0.0064,-0.041,0,0,-4.9e+02,-0.001,-0.0058,-8.9e-05,0.00054,0.0041,-0.11,0.21,-5.6e-07,0.43,-0.00014,0.00072,-3.7e-05,0,0,-4.9e+02,0.00019,0.0002,0.037,0.036,0.038,0.035,0.052,0.052,0.054,3.3e-06,5.8e-06,2.3e-06,0.011,0.014,0.0057,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14590000,0.71,0.00043,-0.013,0.71,0.0061,0.0048,-0.041,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.00033,0.0039,-0.11,0.21,-4.7e-07,0.43,-0.00013,0.00069,-5.1e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.031,0.033,0.033,0.045,0.045,0.054,3.2e-06,5.6e-06,2.3e-06,0.01,0.014,0.0053,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14690000,0.71,0.00038,-0.013,0.71,0.0078,0.0029,-0.038,0,0,-4.9e+02,-0.001,-0.0058,-8.7e-05,-0.00028,0.003,-0.11,0.21,-7.4e-08,0.43,-0.00012,0.0007,-3.6e-05,0,0,-4.9e+02,0.00018,0.00019,0.037,0.034,0.036,0.034,0.051,0.052,0.054,3.1e-06,5.5e-06,2.3e-06,0.01,0.013,0.0051,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14790000,0.71,0.00036,-0.013,0.71,0.0055,0.0014,-0.033,0,0,-4.9e+02,-0.001,-0.0058,-8.6e-05,-0.00044,0.0028,-0.12,0.21,-1.3e-07,0.43,-0.00012,0.00068,-3.7e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.03,0.031,0.032,0.045,0.045,0.053,3e-06,5.2e-06,2.3e-06,0.0098,0.013,0.0048,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.7 +14890000,0.71,0.00032,-0.013,0.71,0.0077,0.0031,-0.037,0,0,-4.9e+02,-0.001,-0.0058,-8.5e-05,-0.00068,0.0022,-0.12,0.21,1.4e-07,0.43,-0.00011,0.00068,-3.4e-05,0,0,-4.9e+02,0.00017,0.00018,0.037,0.032,0.034,0.032,0.051,0.051,0.055,2.9e-06,5.1e-06,2.3e-06,0.0097,0.013,0.0046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +14990000,0.71,0.00028,-0.013,0.71,0.0068,0.0021,-0.032,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.0012,0.0027,-0.12,0.21,-6.1e-08,0.43,-0.00012,0.00066,-4.6e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.028,0.03,0.031,0.045,0.045,0.053,2.8e-06,4.9e-06,2.3e-06,0.0094,0.013,0.0043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15090000,0.71,0.00021,-0.013,0.71,0.0075,0.0021,-0.035,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.0013,0.0031,-0.12,0.21,-1.6e-07,0.43,-0.00013,0.00064,-4.7e-05,0,0,-4.9e+02,0.00017,0.00017,0.037,0.031,0.032,0.031,0.05,0.051,0.054,2.7e-06,4.8e-06,2.3e-06,0.0093,0.012,0.0041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15190000,0.71,0.00017,-0.013,0.71,0.0073,0.0027,-0.033,0,0,-4.9e+02,-0.00099,-0.0059,-8.9e-05,-0.0019,0.0034,-0.12,0.21,-1.5e-07,0.43,-0.00014,0.00061,-5.4e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.027,0.028,0.03,0.044,0.045,0.053,2.6e-06,4.6e-06,2.3e-06,0.0091,0.012,0.0038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.8 +15290000,0.71,0.00019,-0.013,0.71,0.0077,0.0037,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.7e-05,-0.00097,0.0032,-0.12,0.21,-8.9e-08,0.43,-0.00015,0.00061,-2.9e-05,0,0,-4.9e+02,0.00016,0.00017,0.037,0.029,0.031,0.03,0.05,0.05,0.054,2.6e-06,4.5e-06,2.3e-06,0.0089,0.012,0.0037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15390000,0.71,0.0002,-0.013,0.71,0.007,0.005,-0.028,0,0,-4.9e+02,-0.001,-0.0058,-8.3e-05,-0.00013,0.0017,-0.12,0.21,2.8e-07,0.43,-0.00014,0.00064,-5.8e-06,0,0,-4.9e+02,0.00016,0.00016,0.037,0.025,0.027,0.028,0.044,0.044,0.053,2.5e-06,4.4e-06,2.3e-06,0.0087,0.012,0.0034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15490000,0.71,0.00022,-0.013,0.71,0.0086,0.0041,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.6e-05,-0.00052,0.003,-0.12,0.21,-1.8e-07,0.43,-0.00016,0.00061,-2.4e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.027,0.029,0.029,0.049,0.05,0.054,2.4e-06,4.3e-06,2.3e-06,0.0086,0.011,0.0033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15590000,0.71,0.00018,-0.013,0.71,0.0072,0.0031,-0.027,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.0012,0.0037,-0.12,0.21,-5.4e-07,0.43,-0.00016,0.0006,-4.9e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.024,0.026,0.027,0.044,0.044,0.053,2.3e-06,4.1e-06,2.3e-06,0.0084,0.011,0.0031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,3.9 +15690000,0.71,0.00022,-0.013,0.71,0.0074,0.0032,-0.028,0,0,-4.9e+02,-0.001,-0.0059,-8.9e-05,-0.00068,0.0041,-0.12,0.21,-8.5e-07,0.43,-0.00017,0.0006,-5.2e-05,0,0,-4.9e+02,0.00016,0.00016,0.037,0.026,0.028,0.027,0.049,0.049,0.053,2.3e-06,4e-06,2.3e-06,0.0083,0.011,0.003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15790000,0.71,0.00019,-0.013,0.71,0.0081,0.0017,-0.03,0,0,-4.9e+02,-0.001,-0.0059,-8.8e-05,-0.00088,0.0043,-0.12,0.21,-9.6e-07,0.43,-0.00018,0.00059,-5.6e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.023,0.025,0.026,0.043,0.044,0.052,2.2e-06,3.9e-06,2.3e-06,0.0082,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15890000,0.71,0.0002,-0.013,0.71,0.0088,0.0016,-0.028,0,0,-4.9e+02,-0.0011,-0.0059,-8.8e-05,-0.00015,0.0046,-0.12,0.21,-1.2e-06,0.43,-0.00019,0.00059,-4.7e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.025,0.027,0.026,0.048,0.049,0.053,2.2e-06,3.8e-06,2.3e-06,0.0081,0.011,0.0027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +15990000,0.71,0.00018,-0.013,0.71,0.0085,0.0017,-0.024,0,0,-4.9e+02,-0.0011,-0.0059,-8.3e-05,0.00049,0.0038,-0.12,0.21,-1e-06,0.43,-0.0002,0.0006,-2.9e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.022,0.024,0.025,0.043,0.043,0.052,2.1e-06,3.6e-06,2.3e-06,0.0079,0.01,0.0025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4 +16090000,0.71,0.00021,-0.013,0.71,0.011,0.0034,-0.02,0,0,-4.9e+02,-0.0011,-0.0059,-7.9e-05,0.0013,0.0025,-0.12,0.21,-7.5e-07,0.43,-0.00017,0.00065,-1.3e-05,0,0,-4.9e+02,0.00015,0.00015,0.037,0.024,0.026,0.024,0.048,0.049,0.052,2e-06,3.6e-06,2.3e-06,0.0078,0.01,0.0024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16190000,0.71,0.00026,-0.013,0.71,0.01,0.0036,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.002,0.0027,-0.12,0.21,-1.1e-06,0.43,-0.00018,0.00065,-6.4e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.021,0.023,0.023,0.043,0.043,0.052,2e-06,3.4e-06,2.3e-06,0.0077,0.01,0.0022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16290000,0.71,0.00028,-0.014,0.71,0.012,0.0046,-0.02,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0024,0.0014,-0.12,0.21,-5.8e-07,0.43,-0.00017,0.00067,8.4e-06,0,0,-4.9e+02,0.00015,0.00014,0.037,0.023,0.025,0.023,0.048,0.048,0.052,1.9e-06,3.4e-06,2.3e-06,0.0076,0.01,0.0021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16390000,0.71,0.00033,-0.014,0.71,0.01,0.0031,-0.019,0,0,-4.9e+02,-0.0011,-0.0058,-7.5e-05,0.0035,0.0025,-0.12,0.21,-1.4e-06,0.43,-0.00019,0.00066,1.3e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.023,0.022,0.042,0.043,0.051,1.9e-06,3.2e-06,2.3e-06,0.0075,0.0098,0.002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.1 +16490000,0.71,0.00042,-0.014,0.71,0.0088,0.0044,-0.022,0,0,-4.9e+02,-0.0011,-0.0058,-7.4e-05,0.0044,0.0027,-0.12,0.21,-1.6e-06,0.43,-0.0002,0.00066,2.7e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.022,0.024,0.022,0.047,0.048,0.052,1.8e-06,3.2e-06,2.3e-06,0.0074,0.0097,0.0019,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16590000,0.71,0.00052,-0.013,0.71,0.0067,0.0053,-0.023,0,0,-4.9e+02,-0.0012,-0.0058,-7.5e-05,0.0045,0.0024,-0.12,0.21,-1.6e-06,0.43,-0.00019,0.00065,2.3e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.02,0.022,0.021,0.042,0.042,0.05,1.8e-06,3.1e-06,2.3e-06,0.0073,0.0095,0.0018,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16690000,0.71,0.00049,-0.013,0.71,0.0076,0.0056,-0.019,0,0,-4.9e+02,-0.0011,-0.0059,-7.8e-05,0.0039,0.0031,-0.12,0.21,-1.8e-06,0.43,-0.00019,0.00065,1.2e-05,0,0,-4.9e+02,0.00014,0.00014,0.037,0.021,0.024,0.021,0.047,0.047,0.051,1.7e-06,3e-06,2.3e-06,0.0072,0.0094,0.0017,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16790000,0.71,0.00048,-0.013,0.71,0.0057,0.0063,-0.018,0,0,-4.9e+02,-0.0011,-0.0058,-7.9e-05,0.0037,0.0027,-0.12,0.21,-1.7e-06,0.43,-0.00017,0.00065,-5.6e-07,0,0,-4.9e+02,0.00014,0.00013,0.037,0.019,0.021,0.02,0.042,0.042,0.05,1.7e-06,2.9e-06,2.3e-06,0.0071,0.0093,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.2 +16890000,0.71,0.00054,-0.013,0.71,0.0055,0.0072,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.0041,0.0033,-0.13,0.21,-2e-06,0.43,-0.00018,0.00064,4.1e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.023,0.02,0.046,0.047,0.05,1.6e-06,2.8e-06,2.3e-06,0.007,0.0092,0.0016,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +16990000,0.71,0.00051,-0.013,0.71,0.0055,0.0049,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8.1e-05,0.004,0.0043,-0.13,0.21,-2.5e-06,0.43,-0.0002,0.00063,-6e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.021,0.019,0.041,0.042,0.049,1.6e-06,2.7e-06,2.3e-06,0.0069,0.009,0.0015,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17090000,0.71,0.00055,-0.013,0.71,0.0058,0.0064,-0.015,0,0,-4.9e+02,-0.0012,-0.0059,-8e-05,0.005,0.0046,-0.13,0.21,-2.8e-06,0.43,-0.00022,0.00063,6.2e-06,0,0,-4.9e+02,0.00014,0.00013,0.037,0.02,0.022,0.019,0.046,0.047,0.049,1.5e-06,2.7e-06,2.3e-06,0.0068,0.0089,0.0014,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17190000,0.71,0.0006,-0.013,0.71,0.0059,0.0074,-0.016,0,0,-4.9e+02,-0.0012,-0.0059,-7.6e-05,0.0057,0.0045,-0.13,0.21,-3.1e-06,0.43,-0.00022,0.00064,8.8e-06,0,0,-4.9e+02,0.00013,0.00013,0.037,0.018,0.02,0.018,0.041,0.042,0.049,1.5e-06,2.6e-06,2.3e-06,0.0068,0.0087,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.3 +17290000,0.71,0.00063,-0.013,0.71,0.0077,0.0081,-0.011,0,0,-4.9e+02,-0.0012,-0.0059,-7.8e-05,0.0061,0.0055,-0.13,0.21,-3.4e-06,0.43,-0.00023,0.00063,1.2e-05,0,0,-4.9e+02,0.00013,0.00013,0.037,0.019,0.022,0.018,0.045,0.046,0.049,1.5e-06,2.6e-06,2.3e-06,0.0067,0.0086,0.0013,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17390000,0.71,0.00068,-0.013,0.71,0.0075,0.0085,-0.0095,0,0,-4.9e+02,-0.0012,-0.0059,-7.2e-05,0.0069,0.0052,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00063,2.9e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.02,0.017,0.041,0.041,0.048,1.4e-06,2.5e-06,2.2e-06,0.0066,0.0085,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17490000,0.71,0.00063,-0.013,0.71,0.0091,0.0086,-0.0078,0,0,-4.9e+02,-0.0012,-0.0059,-7.2e-05,0.0063,0.005,-0.13,0.21,-3.4e-06,0.43,-0.00025,0.00063,2.4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.019,0.021,0.017,0.045,0.046,0.049,1.4e-06,2.4e-06,2.2e-06,0.0065,0.0084,0.0012,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17590000,0.71,0.0006,-0.013,0.71,0.0099,0.0079,-0.0024,0,0,-4.9e+02,-0.0012,-0.0059,-6.9e-05,0.0067,0.0052,-0.13,0.21,-3.6e-06,0.43,-0.00025,0.00064,2.2e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.017,0.019,0.017,0.04,0.041,0.048,1.4e-06,2.3e-06,2.2e-06,0.0065,0.0083,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.4 +17690000,0.71,0.00057,-0.013,0.71,0.011,0.0094,-0.003,0,0,-4.9e+02,-0.0012,-0.0059,-6.8e-05,0.0068,0.005,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00064,3e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.021,0.016,0.045,0.046,0.048,1.3e-06,2.3e-06,2.2e-06,0.0064,0.0082,0.0011,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17790000,0.71,0.00056,-0.013,0.71,0.012,0.01,-0.0042,0,0,-4.9e+02,-0.0012,-0.0059,-5.9e-05,0.0077,0.0039,-0.13,0.21,-3.5e-06,0.43,-0.00025,0.00066,4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.016,0.019,0.016,0.04,0.041,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.0081,0.001,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17890000,0.71,0.00055,-0.013,0.71,0.015,0.011,-0.004,0,0,-4.9e+02,-0.0012,-0.0059,-5.7e-05,0.0076,0.0032,-0.13,0.21,-3.2e-06,0.43,-0.00025,0.00067,4.4e-05,0,0,-4.9e+02,0.00013,0.00012,0.037,0.018,0.02,0.016,0.044,0.045,0.047,1.3e-06,2.2e-06,2.2e-06,0.0063,0.008,0.00097,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +17990000,0.71,0.00048,-0.013,0.71,0.016,0.0084,-0.0027,0,0,-4.9e+02,-0.0012,-0.0059,-5.6e-05,0.0073,0.0035,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00066,4.1e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.016,0.019,0.015,0.04,0.041,0.046,1.2e-06,2.1e-06,2.2e-06,0.0062,0.0078,0.00092,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.5 +18090000,0.71,0.00047,-0.013,0.71,0.017,0.0076,-0.00044,0,0,-4.9e+02,-0.0012,-0.0059,-6.1e-05,0.0068,0.0044,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.6e-05,0,0,-4.9e+02,0.00012,0.00012,0.037,0.017,0.02,0.015,0.044,0.045,0.047,1.2e-06,2.1e-06,2.2e-06,0.0061,0.0078,0.00089,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18190000,0.71,0.00043,-0.013,0.71,0.018,0.0086,0.001,0,0,-4.9e+02,-0.0012,-0.0059,-5.7e-05,0.007,0.0041,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00065,3.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.018,0.014,0.04,0.041,0.046,1.2e-06,2e-06,2.2e-06,0.0061,0.0077,0.00085,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18290000,0.71,0.00035,-0.013,0.71,0.018,0.0081,0.0022,0,0,-4.9e+02,-0.0012,-0.0059,-5.9e-05,0.0066,0.0044,-0.13,0.21,-3.5e-06,0.43,-0.00027,0.00064,3.2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.017,0.02,0.014,0.044,0.045,0.046,1.2e-06,2e-06,2.2e-06,0.006,0.0076,0.00082,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18390000,0.71,0.00031,-0.013,0.71,0.02,0.01,0.0035,0,0,-4.9e+02,-0.0012,-0.0059,-5.3e-05,0.0064,0.0036,-0.13,0.21,-3.3e-06,0.43,-0.00026,0.00065,3.3e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.014,0.039,0.04,0.045,1.1e-06,1.9e-06,2.2e-06,0.006,0.0075,0.00078,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.6 +18490000,0.71,0.00037,-0.013,0.71,0.021,0.011,0.0031,0,0,-4.9e+02,-0.0012,-0.0059,-5.2e-05,0.0069,0.0037,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00066,3.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.02,0.014,0.043,0.045,0.045,1.1e-06,1.9e-06,2.2e-06,0.0059,0.0074,0.00076,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18590000,0.71,0.00039,-0.013,0.71,0.02,0.012,0.0014,0,0,-4.9e+02,-0.0012,-0.0059,-4.4e-05,0.0077,0.0031,-0.13,0.21,-3.6e-06,0.43,-0.00026,0.00067,4.2e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0073,0.00072,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18690000,0.71,0.0003,-0.013,0.71,0.022,0.012,-0.00039,0,0,-4.9e+02,-0.0012,-0.0059,-4.6e-05,0.007,0.0031,-0.13,0.21,-3.5e-06,0.43,-0.00026,0.00066,3.6e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.045,1.1e-06,1.8e-06,2.2e-06,0.0058,0.0072,0.0007,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18790000,0.71,0.00032,-0.013,0.71,0.021,0.011,-0.00061,0,0,-4.9e+02,-0.0012,-0.0059,-4.5e-05,0.0072,0.0037,-0.13,0.21,-3.7e-06,0.43,-0.00026,0.00065,3.3e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.015,0.018,0.013,0.039,0.04,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00067,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.7 +18890000,0.71,0.00041,-0.013,0.71,0.021,0.013,1.6e-05,0,0,-4.9e+02,-0.0012,-0.0059,-3.9e-05,0.0082,0.0032,-0.13,0.21,-3.8e-06,0.43,-0.00027,0.00067,4.7e-05,0,0,-4.9e+02,0.00012,0.00011,0.037,0.016,0.019,0.013,0.043,0.044,0.044,1e-06,1.7e-06,2.2e-06,0.0057,0.0071,0.00065,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +18990000,0.71,0.00046,-0.013,0.71,0.021,0.014,-0.0012,0,0,-4.9e+02,-0.0013,-0.0059,-3.3e-05,0.0088,0.0033,-0.13,0.21,-4.1e-06,0.43,-0.00028,0.00068,4.8e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.015,0.017,0.012,0.039,0.04,0.044,9.7e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.00062,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19090000,0.71,0.0005,-0.013,0.71,0.021,0.015,0.0018,0,0,-4.9e+02,-0.0013,-0.0059,-3.3e-05,0.0094,0.0036,-0.13,0.21,-4.3e-06,0.43,-0.00029,0.00069,5.2e-05,0,0,-4.9e+02,0.00011,0.0001,0.037,0.016,0.019,0.012,0.042,0.044,0.044,9.6e-07,1.6e-06,2.2e-06,0.0056,0.0069,0.0006,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19190000,0.71,0.00055,-0.013,0.71,0.02,0.015,0.0019,0,0,-4.9e+02,-0.0013,-0.0059,-2.7e-05,0.0099,0.0037,-0.13,0.21,-4.5e-06,0.43,-0.00031,0.00069,5.6e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.012,0.038,0.04,0.043,9.3e-07,1.5e-06,2.1e-06,0.0055,0.0068,0.00058,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.8 +19290000,0.71,0.00058,-0.013,0.71,0.02,0.015,0.0046,0,0,-4.9e+02,-0.0013,-0.0059,-3e-05,0.0098,0.0041,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00068,5.8e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.019,0.012,0.042,0.044,0.043,9.2e-07,1.5e-06,2.1e-06,0.0055,0.0067,0.00056,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19390000,0.71,0.00054,-0.013,0.71,0.019,0.014,0.0084,0,0,-4.9e+02,-0.0013,-0.0059,-2.3e-05,0.0097,0.004,-0.13,0.21,-4.6e-06,0.43,-0.0003,0.00068,5.3e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.014,0.017,0.011,0.038,0.04,0.043,8.9e-07,1.5e-06,2.1e-06,0.0055,0.0066,0.00054,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19490000,0.71,0.00055,-0.013,0.71,0.019,0.015,0.0049,0,0,-4.9e+02,-0.0013,-0.0059,-1.8e-05,0.0098,0.0033,-0.13,0.21,-4.4e-06,0.43,-0.0003,0.00069,5.6e-05,0,0,-4.9e+02,0.00011,0.0001,0.036,0.015,0.018,0.011,0.042,0.044,0.043,8.8e-07,1.4e-06,2.1e-06,0.0054,0.0066,0.00052,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19590000,0.71,0.00063,-0.013,0.71,0.017,0.014,0.0043,0,0,-4.9e+02,-0.0013,-0.0059,-6.7e-06,0.011,0.003,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.0007,6.4e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.014,0.017,0.011,0.038,0.039,0.042,8.6e-07,1.4e-06,2.1e-06,0.0054,0.0065,0.0005,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,4.9 +19690000,0.71,0.00067,-0.013,0.71,0.017,0.012,0.0058,0,0,-4.9e+02,-0.0013,-0.0059,-1e-05,0.011,0.0036,-0.13,0.21,-4.9e-06,0.43,-0.00031,0.0007,5.7e-05,0,0,-4.9e+02,0.00011,9.8e-05,0.036,0.015,0.018,0.011,0.042,0.043,0.042,8.4e-07,1.4e-06,2.1e-06,0.0053,0.0065,0.00049,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19790000,0.71,0.00074,-0.013,0.71,0.015,0.01,0.0064,0,0,-4.9e+02,-0.0013,-0.0059,-5.7e-06,0.011,0.004,-0.13,0.21,-5.1e-06,0.43,-0.00032,0.0007,5.5e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.042,8.2e-07,1.3e-06,2.1e-06,0.0053,0.0064,0.00047,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19890000,0.71,0.00066,-0.013,0.71,0.015,0.012,0.0075,0,0,-4.9e+02,-0.0013,-0.0058,2.4e-06,0.011,0.0029,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00071,6.2e-05,0,0,-4.9e+02,0.00011,9.6e-05,0.036,0.015,0.018,0.01,0.041,0.043,0.042,8.1e-07,1.3e-06,2.1e-06,0.0053,0.0063,0.00046,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +19990000,0.71,0.00063,-0.013,0.71,0.013,0.012,0.01,0,0,-4.9e+02,-0.0013,-0.0058,1.8e-05,0.012,0.0021,-0.13,0.21,-4.7e-06,0.43,-0.00031,0.00073,6.7e-05,0,0,-4.9e+02,0.0001,9.4e-05,0.036,0.014,0.017,0.01,0.038,0.039,0.041,7.9e-07,1.3e-06,2.1e-06,0.0052,0.0062,0.00044,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5 +20090000,0.71,0.00066,-0.013,0.71,0.013,0.013,0.011,0,0,-4.9e+02,-0.0013,-0.0058,2.8e-05,0.012,0.0013,-0.13,0.21,-4.5e-06,0.43,-0.00032,0.00075,7.7e-05,0,0,-4.9e+02,0.00011,9.5e-05,0.036,0.014,0.018,0.01,0.041,0.043,0.041,7.8e-07,1.2e-06,2.1e-06,0.0052,0.0062,0.00043,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20190000,0.71,0.00069,-0.013,0.71,0.012,0.011,0.013,0,0,-4.9e+02,-0.0013,-0.0058,3.7e-05,0.012,0.001,-0.13,0.21,-4.5e-06,0.43,-0.00031,0.00075,7.6e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.013,0.016,0.0098,0.038,0.039,0.041,7.6e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00042,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20290000,0.71,0.0007,-0.013,0.71,0.01,0.011,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4e-05,0.012,0.00092,-0.13,0.21,-4.5e-06,0.43,-0.00032,0.00076,7.8e-05,0,0,-4.9e+02,0.0001,9.3e-05,0.036,0.014,0.018,0.0097,0.041,0.043,0.041,7.5e-07,1.2e-06,2.1e-06,0.0051,0.0061,0.00041,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20390000,0.71,0.00066,-0.013,0.7,0.0086,0.0091,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.4e-05,0.012,0.001,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00076,6.8e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.013,0.016,0.0095,0.037,0.039,0.04,7.3e-07,1.1e-06,2e-06,0.0051,0.006,0.00039,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.1 +20490000,0.71,0.00071,-0.013,0.7,0.0087,0.009,0.013,0,0,-4.9e+02,-0.0013,-0.0058,4.1e-05,0.012,0.0013,-0.13,0.21,-4.6e-06,0.43,-0.00031,0.00075,6.6e-05,0,0,-4.9e+02,0.0001,9.1e-05,0.036,0.014,0.017,0.0094,0.041,0.043,0.04,7.2e-07,1.1e-06,2e-06,0.005,0.006,0.00038,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20590000,0.71,0.00074,-0.013,0.7,0.0078,0.007,0.0099,0,0,-4.9e+02,-0.0013,-0.0058,4.2e-05,0.012,0.0018,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00074,6.6e-05,0,0,-4.9e+02,9.9e-05,8.9e-05,0.036,0.013,0.016,0.0092,0.037,0.039,0.04,7e-07,1.1e-06,2e-06,0.005,0.0059,0.00037,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20690000,0.71,0.00077,-0.013,0.7,0.0085,0.007,0.011,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.012,0.0016,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00075,6.8e-05,0,0,-4.9e+02,0.0001,8.9e-05,0.036,0.014,0.017,0.0092,0.041,0.043,0.04,6.9e-07,1.1e-06,2e-06,0.005,0.0058,0.00036,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20790000,0.71,0.00081,-0.013,0.7,0.0063,0.0066,0.012,0,0,-4.9e+02,-0.0013,-0.0058,5e-05,0.013,0.0017,-0.13,0.21,-4.8e-06,0.43,-0.00032,0.00075,6.2e-05,0,0,-4.9e+02,9.8e-05,8.8e-05,0.036,0.013,0.016,0.009,0.037,0.039,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0058,0.00035,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.2 +20890000,0.71,0.00081,-0.013,0.7,0.0062,0.0062,0.011,0,0,-4.9e+02,-0.0013,-0.0058,5.8e-05,0.013,0.0013,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00077,6.7e-05,0,0,-4.9e+02,9.9e-05,8.8e-05,0.036,0.014,0.017,0.0089,0.04,0.043,0.039,6.7e-07,1e-06,2e-06,0.0049,0.0057,0.00034,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +20990000,0.71,0.00083,-0.013,0.7,0.0045,0.0039,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.2e-05,0.013,0.0015,-0.13,0.21,-4.8e-06,0.43,-0.00034,0.00077,6.4e-05,0,0,-4.9e+02,9.6e-05,8.6e-05,0.036,0.013,0.016,0.0087,0.037,0.039,0.039,6.5e-07,9.9e-07,2e-06,0.0049,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21090000,0.71,0.00081,-0.013,0.7,0.0054,0.0032,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.013,0.0011,-0.13,0.21,-4.6e-06,0.43,-0.00033,0.00078,6.1e-05,0,0,-4.9e+02,9.7e-05,8.6e-05,0.036,0.014,0.017,0.0087,0.04,0.043,0.039,6.4e-07,9.9e-07,2e-06,0.0048,0.0056,0.00033,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21190000,0.71,0.00082,-0.013,0.7,0.0057,0.0023,0.011,0,0,-4.9e+02,-0.0013,-0.0058,6.7e-05,0.013,0.0012,-0.13,0.21,-4.6e-06,0.43,-0.00033,0.00077,5.8e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.013,0.016,0.0085,0.037,0.039,0.039,6.3e-07,9.5e-07,2e-06,0.0048,0.0055,0.00032,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.3 +21290000,0.71,0.0009,-0.013,0.7,0.005,0.0023,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.8e-05,0.013,0.00077,-0.13,0.21,-4.6e-06,0.43,-0.00034,0.0008,6.2e-05,0,0,-4.9e+02,9.5e-05,8.5e-05,0.036,0.014,0.017,0.0084,0.04,0.043,0.038,6.2e-07,9.5e-07,1.9e-06,0.0048,0.0055,0.00031,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21390000,0.71,0.00088,-0.013,0.7,0.004,0.00035,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.3e-05,0.013,0.00093,-0.13,0.21,-4.8e-06,0.43,-0.00033,0.00079,6.2e-05,0,0,-4.9e+02,9.3e-05,8.3e-05,0.036,0.013,0.016,0.0083,0.037,0.039,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0055,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21490000,0.71,0.00088,-0.013,0.7,0.0045,0.00078,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.7e-05,0.013,0.00058,-0.13,0.21,-4.7e-06,0.43,-0.00032,0.00079,6.7e-05,0,0,-4.9e+02,9.4e-05,8.3e-05,0.036,0.014,0.017,0.0082,0.04,0.043,0.038,6e-07,9.1e-07,1.9e-06,0.0047,0.0054,0.0003,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21590000,0.71,0.00087,-0.013,0.7,0.0034,0.0013,0.012,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.013,0.00061,-0.13,0.21,-4.9e-06,0.43,-0.00032,0.00079,6.4e-05,0,0,-4.9e+02,9.1e-05,8.2e-05,0.036,0.013,0.015,0.0081,0.037,0.039,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0054,0.00029,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.4 +21690000,0.71,0.00084,-0.013,0.7,0.005,0.0016,0.014,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.013,0.0002,-0.13,0.21,-4.8e-06,0.43,-0.00031,0.00079,6.4e-05,0,0,-4.9e+02,9.2e-05,8.2e-05,0.036,0.013,0.017,0.0081,0.04,0.042,0.038,5.8e-07,8.7e-07,1.9e-06,0.0047,0.0053,0.00028,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21790000,0.71,0.00084,-0.013,0.7,0.0031,0.0037,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7e-05,0.013,0.00038,-0.13,0.21,-5.4e-06,0.43,-0.00033,0.00079,6.6e-05,0,0,-4.9e+02,9e-05,8.1e-05,0.036,0.012,0.015,0.0079,0.037,0.039,0.038,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21890000,0.71,0.00083,-0.013,0.7,0.0038,0.0043,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.1e-05,0.013,0.00029,-0.13,0.21,-5.3e-06,0.43,-0.00033,0.00079,6.4e-05,0,0,-4.9e+02,9.1e-05,8.1e-05,0.036,0.013,0.016,0.0079,0.04,0.042,0.037,5.6e-07,8.3e-07,1.9e-06,0.0046,0.0053,0.00027,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,5.5 +21990000,0.71,0.00085,-0.013,0.7,0.0026,0.005,0.014,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00012,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.6e-05,0,0,-4.9e+02,8.9e-05,7.9e-05,0.036,0.012,0.015,0.0077,0.036,0.038,0.037,5.5e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22090000,0.71,0.00088,-0.013,0.7,0.0025,0.0065,0.012,0,0,-4.9e+02,-0.0013,-0.0058,6.8e-05,0.014,0.00017,-0.13,0.21,-5.7e-06,0.43,-0.00034,0.00079,6.6e-05,0,0,-4.9e+02,8.9e-05,8e-05,0.036,0.013,0.016,0.0077,0.04,0.042,0.037,5.4e-07,8e-07,1.9e-06,0.0046,0.0052,0.00026,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22190000,0.71,0.00085,-0.013,0.7,0.002,0.0065,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.5e-05,0.014,0.00023,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.8e-05,0.036,0.012,0.015,0.0076,0.036,0.038,0.037,5.3e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22290000,0.71,0.00088,-0.013,0.7,0.0013,0.0062,0.013,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.014,0.00028,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.00079,5.6e-05,0,0,-4.9e+02,8.8e-05,7.8e-05,0.036,0.013,0.016,0.0075,0.04,0.042,0.037,5.2e-07,7.7e-07,1.8e-06,0.0045,0.0051,0.00025,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22390000,0.71,0.0009,-0.013,0.7,-0.00098,0.006,0.015,0,0,-4.9e+02,-0.0013,-0.0058,7.9e-05,0.014,0.00049,-0.13,0.21,-5.5e-06,0.43,-0.00035,0.0008,5.7e-05,0,0,-4.9e+02,8.6e-05,7.7e-05,0.036,0.012,0.015,0.0074,0.036,0.038,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22490000,0.71,0.00094,-0.013,0.7,-0.0021,0.0068,0.016,0,0,-4.9e+02,-0.0013,-0.0058,8e-05,0.014,0.00063,-0.13,0.21,-5.4e-06,0.43,-0.00037,0.0008,5.5e-05,0,0,-4.9e+02,8.7e-05,7.7e-05,0.036,0.013,0.016,0.0074,0.039,0.042,0.037,5.1e-07,7.4e-07,1.8e-06,0.0045,0.005,0.00024,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22590000,0.71,0.00096,-0.013,0.7,-0.0038,0.0062,0.015,0,0,-4.9e+02,-0.0014,-0.0058,8.3e-05,0.015,0.0008,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.00081,5.2e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.012,0.015,0.0073,0.036,0.038,0.036,5e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22690000,0.71,0.001,-0.013,0.7,-0.0051,0.0076,0.016,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.015,0.00069,-0.13,0.21,-5.3e-06,0.43,-0.00039,0.00082,5.1e-05,0,0,-4.9e+02,8.5e-05,7.6e-05,0.036,0.013,0.016,0.0073,0.039,0.042,0.036,4.9e-07,7.1e-07,1.8e-06,0.0044,0.005,0.00023,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22790000,0.71,0.001,-0.013,0.7,-0.0071,0.0065,0.017,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.015,0.0015,-0.13,0.21,-5.5e-06,0.43,-0.00038,0.00081,5.7e-05,0,0,-4.9e+02,8.3e-05,7.5e-05,0.036,0.012,0.015,0.0071,0.036,0.038,0.036,4.8e-07,6.8e-07,1.8e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22890000,0.71,0.001,-0.013,0.7,-0.0075,0.0074,0.019,0,0,-4.9e+02,-0.0014,-0.0058,7.8e-05,0.015,0.0013,-0.13,0.21,-5.4e-06,0.43,-0.00038,0.0008,5.3e-05,0,0,-4.9e+02,8.4e-05,7.5e-05,0.036,0.013,0.016,0.0071,0.039,0.042,0.036,4.8e-07,6.8e-07,1.7e-06,0.0044,0.0049,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +22990000,0.71,0.00097,-0.013,0.7,-0.0076,0.0065,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.8e-05,0.015,0.0012,-0.13,0.21,-5.1e-06,0.43,-0.00038,0.00081,5.1e-05,0,0,-4.9e+02,8.2e-05,7.4e-05,0.036,0.012,0.015,0.007,0.036,0.038,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00022,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23090000,0.71,0.00092,-0.013,0.7,-0.008,0.0063,0.02,0,0,-4.9e+02,-0.0014,-0.0058,7.9e-05,0.015,0.0015,-0.13,0.21,-5.3e-06,0.43,-0.00038,0.00079,4.8e-05,0,0,-4.9e+02,8.3e-05,7.4e-05,0.036,0.013,0.016,0.007,0.039,0.042,0.036,4.7e-07,6.6e-07,1.7e-06,0.0044,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0013,1,1,0.01 +23190000,0.71,0.00098,-0.013,0.7,-0.0093,0.0044,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.2e-05,0.015,0.0017,-0.13,0.21,-5.3e-06,0.43,-0.00037,0.00079,4e-05,0,0,-4.9e+02,8.1e-05,7.3e-05,0.036,0.012,0.014,0.0069,0.036,0.038,0.035,4.6e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23290000,0.71,0.0009,-0.013,0.7,-0.0092,0.0038,0.022,0,0,-4.9e+02,-0.0014,-0.0058,8.4e-05,0.015,0.0015,-0.13,0.21,-5.2e-06,0.43,-0.00037,0.00079,3.9e-05,0,0,-4.9e+02,8.2e-05,7.3e-05,0.036,0.013,0.016,0.0069,0.039,0.042,0.036,4.5e-07,6.3e-07,1.7e-06,0.0043,0.0048,0.00021,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23390000,0.71,0.00095,-0.013,0.7,-0.0095,0.0026,0.02,0,0,-4.9e+02,-0.0014,-0.0058,8.6e-05,0.014,0.0015,-0.13,0.21,-5.3e-06,0.43,-0.00035,0.00078,4e-05,0,0,-4.9e+02,8e-05,7.2e-05,0.036,0.012,0.014,0.0068,0.036,0.038,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23490000,0.71,0.0033,-0.011,0.7,-0.016,0.0029,-0.013,0,0,-4.9e+02,-0.0014,-0.0058,9.2e-05,0.014,0.0013,-0.13,0.21,-5.3e-06,0.43,-0.00034,0.00081,6.5e-05,0,0,-4.9e+02,8.1e-05,7.2e-05,0.036,0.013,0.015,0.0068,0.039,0.042,0.035,4.4e-07,6.1e-07,1.7e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0013,0.0016,0.0013,0.0012,1,1,0.01 +23590000,0.71,0.0086,-0.0027,0.7,-0.027,0.0029,-0.045,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.014,0.0013,-0.13,0.21,-5.2e-06,0.43,-0.00034,0.00087,0.00013,0,0,-4.9e+02,7.9e-05,7.1e-05,0.036,0.012,0.014,0.0067,0.036,0.038,0.035,4.3e-07,5.9e-07,1.6e-06,0.0043,0.0047,0.0002,0.0013,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23690000,0.71,0.0082,0.003,0.71,-0.058,-0.0049,-0.095,0,0,-4.9e+02,-0.0014,-0.0058,8.9e-05,0.014,0.0013,-0.13,0.21,-5.2e-06,0.43,-0.00034,0.00081,0.0001,0,0,-4.9e+02,8e-05,7.1e-05,0.036,0.013,0.015,0.0067,0.039,0.042,0.035,4.3e-07,5.9e-07,1.6e-06,0.0042,0.0047,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23790000,0.71,0.0053,-0.00025,0.71,-0.083,-0.017,-0.15,0,0,-4.9e+02,-0.0013,-0.0058,9.1e-05,0.014,0.00079,-0.13,0.21,-4.5e-06,0.43,-0.0004,0.0008,0.00045,0,0,-4.9e+02,7.8e-05,7e-05,0.036,0.012,0.014,0.0066,0.036,0.038,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23890000,0.71,0.0026,-0.0063,0.71,-0.1,-0.025,-0.2,0,0,-4.9e+02,-0.0013,-0.0058,9e-05,0.014,0.00092,-0.13,0.21,-4.4e-06,0.43,-0.00042,0.00086,0.00036,0,0,-4.9e+02,7.9e-05,7e-05,0.036,0.013,0.016,0.0066,0.039,0.042,0.035,4.2e-07,5.7e-07,1.6e-06,0.0042,0.0046,0.00019,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +23990000,0.71,0.0013,-0.011,0.71,-0.1,-0.029,-0.26,0,0,-4.9e+02,-0.0013,-0.0058,9.6e-05,0.014,0.00093,-0.13,0.21,-4e-06,0.43,-0.0004,0.00086,0.00034,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.012,0.015,0.0065,0.036,0.038,0.035,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24090000,0.71,0.0025,-0.0096,0.71,-0.1,-0.028,-0.3,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.014,0.00063,-0.13,0.21,-3.6e-06,0.43,-0.00042,0.00083,0.00037,0,0,-4.9e+02,7.8e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4.1e-07,5.5e-07,1.6e-06,0.0042,0.0046,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24190000,0.71,0.0036,-0.0073,0.71,-0.11,-0.03,-0.35,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.014,0.00052,-0.13,0.21,-2.9e-06,0.43,-0.00043,0.00086,0.00037,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24290000,0.71,0.0041,-0.0065,0.71,-0.12,-0.033,-0.41,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.013,0.00049,-0.13,0.21,-2.6e-06,0.43,-0.00046,0.0009,0.00044,0,0,-4.9e+02,7.7e-05,6.9e-05,0.036,0.013,0.016,0.0065,0.039,0.042,0.034,4e-07,5.4e-07,1.6e-06,0.0042,0.0045,0.00018,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24390000,0.71,0.0042,-0.0067,0.71,-0.13,-0.041,-0.46,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.013,0.0011,-0.13,0.21,2.4e-07,0.43,-0.00034,0.00095,0.00043,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.012,0.015,0.0064,0.036,0.038,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24490000,0.71,0.0051,-0.0025,0.71,-0.14,-0.046,-0.51,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.013,0.0011,-0.13,0.21,2.4e-07,0.43,-0.00034,0.00096,0.00042,0,0,-4.9e+02,7.6e-05,6.8e-05,0.036,0.013,0.016,0.0064,0.039,0.042,0.034,3.9e-07,5.2e-07,1.5e-06,0.0041,0.0045,0.00017,0.0012,3.9e-05,0.0012,0.0016,0.0012,0.0012,1,1,0.01 +24590000,0.71,0.0055,0.0012,0.71,-0.16,-0.057,-0.56,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.013,0.0011,-0.13,0.21,1.3e-06,0.43,1.3e-05,0.00061,0.00037,0,0,-4.9e+02,7.4e-05,6.7e-05,0.036,0.012,0.015,0.0063,0.036,0.038,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24690000,0.71,0.0056,0.0021,0.71,-0.18,-0.07,-0.64,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.013,0.00096,-0.13,0.21,2.3e-06,0.43,-3.2e-05,0.00065,0.00056,0,0,-4.9e+02,7.5e-05,6.7e-05,0.036,0.013,0.016,0.0063,0.039,0.042,0.034,3.8e-07,5e-07,1.5e-06,0.0041,0.0044,0.00017,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24790000,0.71,0.0053,0.00083,0.71,-0.2,-0.084,-0.73,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.012,0.001,-0.13,0.21,1.6e-06,0.43,1.6e-07,0.00062,0.00032,0,0,-4.9e+02,7.3e-05,6.6e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24890000,0.71,0.0071,0.0025,0.71,-0.22,-0.095,-0.75,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.012,0.0011,-0.13,0.21,2.4e-06,0.43,-0.00011,0.00077,0.00034,0,0,-4.9e+02,7.4e-05,6.6e-05,0.036,0.013,0.016,0.0062,0.039,0.042,0.034,3.7e-07,4.9e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +24990000,0.71,0.0089,0.0043,0.71,-0.24,-0.1,-0.81,0,0,-4.9e+02,-0.0013,-0.0058,0.00011,0.012,0.0007,-0.13,0.21,1.8e-06,0.43,-0.00019,0.00087,-3.7e-06,0,0,-4.9e+02,7.2e-05,6.5e-05,0.036,0.012,0.015,0.0062,0.036,0.038,0.034,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0044,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25090000,0.71,0.0092,0.0037,0.71,-0.27,-0.11,-0.86,0,0,-4.9e+02,-0.0013,-0.0058,0.0001,0.012,0.0008,-0.13,0.21,1.4e-06,0.43,-0.0002,0.00088,-3.9e-05,0,0,-4.9e+02,7.3e-05,6.5e-05,0.036,0.013,0.017,0.0062,0.039,0.042,0.033,3.7e-07,4.8e-07,1.5e-06,0.0041,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25190000,0.71,0.0087,0.0023,0.71,-0.3,-0.13,-0.91,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.011,0.0011,-0.13,0.21,6.8e-06,0.43,5e-05,0.00085,9.6e-05,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.012,0.016,0.0061,0.036,0.038,0.033,3.6e-07,4.6e-07,1.5e-06,0.004,0.0043,0.00016,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25290000,0.71,0.011,0.0091,0.71,-0.33,-0.14,-0.96,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.011,0.0011,-0.13,0.21,6.6e-06,0.43,7.6e-05,0.0008,0.0001,0,0,-4.9e+02,7.2e-05,6.5e-05,0.035,0.013,0.017,0.0061,0.039,0.042,0.033,3.6e-07,4.6e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25390000,0.71,0.012,0.016,0.71,-0.36,-0.16,-1,0,0,-4.9e+02,-0.0013,-0.0058,0.00013,0.011,0.00086,-0.13,0.21,1e-05,0.43,0.00048,0.00048,0.00014,0,0,-4.9e+02,7.1e-05,6.3e-05,0.035,0.012,0.016,0.006,0.036,0.038,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25490000,0.71,0.012,0.017,0.71,-0.41,-0.18,-1.1,0,0,-4.9e+02,-0.0013,-0.0058,0.00014,0.011,0.00074,-0.13,0.21,9.2e-06,0.43,0.00065,0.00016,0.00033,0,0,-4.9e+02,7.2e-05,6.4e-05,0.035,0.013,0.018,0.006,0.039,0.042,0.033,3.5e-07,4.5e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25590000,0.71,0.012,0.015,0.71,-0.45,-0.21,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,0.00015,0.0099,0.0011,-0.13,0.21,1.5e-05,0.43,0.00095,0.00016,0.00034,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.012,0.018,0.006,0.036,0.038,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0015,0.0012,0.0012,1,1,0.01 +25690000,0.71,0.015,0.022,0.71,-0.49,-0.23,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0099,0.0011,-0.13,0.21,1.6e-05,0.43,0.00094,0.00018,0.00042,0,0,-4.9e+02,7.1e-05,6.3e-05,0.034,0.013,0.02,0.006,0.039,0.042,0.033,3.5e-07,4.4e-07,1.4e-06,0.004,0.0043,0.00015,0.0012,3.9e-05,0.0012,0.0014,0.0012,0.0011,1,1,0.01 +25790000,0.71,0.018,0.028,0.71,-0.54,-0.26,-1.2,0,0,-4.9e+02,-0.0012,-0.0058,0.00016,0.0092,0.00041,-0.13,0.21,1.9e-05,0.43,0.0013,-6.7e-05,-3e-05,0,0,-4.9e+02,7e-05,6.2e-05,0.033,0.013,0.019,0.0059,0.036,0.038,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00015,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25890000,0.71,0.018,0.028,0.71,-0.62,-0.29,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00017,0.0094,0.00034,-0.13,0.21,2.1e-05,0.43,0.0014,7.5e-07,-9.6e-05,0,0,-4.9e+02,7.1e-05,6.3e-05,0.033,0.014,0.022,0.006,0.039,0.042,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0014,0.0011,0.0011,1,1,0.01 +25990000,0.7,0.017,0.025,0.71,-0.67,-0.32,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0085,0.00068,-0.13,0.21,2.8e-05,0.43,0.0023,-0.00056,-0.00055,0,0,-4.9e+02,7e-05,6.2e-05,0.032,0.013,0.021,0.0059,0.036,0.039,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 +26090000,0.7,0.022,0.035,0.71,-0.74,-0.35,-1.3,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0086,0.00085,-0.13,0.21,2.4e-05,0.43,0.0024,-0.00048,-0.0012,0,0,-4.9e+02,7.1e-05,6.2e-05,0.032,0.014,0.024,0.0059,0.039,0.043,0.033,3.4e-07,4.3e-07,1.4e-06,0.004,0.0042,0.00014,0.0011,3.9e-05,0.0011,0.0013,0.0011,0.0011,1,1,0.01 +26190000,0.7,0.024,0.045,0.71,-0.79,-0.39,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0075,-0.00028,-0.13,0.21,3.8e-05,0.43,0.0023,0.00041,-0.0014,0,0,-4.9e+02,7.1e-05,6.1e-05,0.03,0.014,0.024,0.0058,0.036,0.039,0.032,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.001,0.0013,0.001,0.001,1,1,0.01 +26290000,0.7,0.025,0.047,0.71,-0.89,-0.43,-1.3,0,0,-4.9e+02,-0.0012,-0.0058,0.00018,0.0075,-0.00026,-0.13,0.21,3.7e-05,0.43,0.0023,0.00028,-0.0014,0,0,-4.9e+02,7.1e-05,6.2e-05,0.03,0.015,0.028,0.0059,0.039,0.043,0.033,3.3e-07,4.2e-07,1.4e-06,0.004,0.0042,0.00014,0.001,3.9e-05,0.00099,0.0013,0.001,0.00099,1,1,0.01 +26390000,0.7,0.024,0.044,0.71,-0.96,-0.49,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00021,0.0066,0.00051,-0.13,0.21,4.4e-05,0.44,0.0036,-0.00018,-0.0024,0,0,-4.9e+02,7.1e-05,6.1e-05,0.028,0.014,0.027,0.0058,0.036,0.039,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00096,3.9e-05,0.00096,0.0012,0.00096,0.00095,1,1,0.01 +26490000,0.7,0.031,0.06,0.71,-1.1,-0.53,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.0002,0.0066,0.00051,-0.13,0.21,3.8e-05,0.44,0.0039,-0.00099,-0.0026,0,0,-4.9e+02,7.2e-05,6.1e-05,0.028,0.016,0.031,0.0058,0.039,0.044,0.032,3.3e-07,4.1e-07,1.3e-06,0.004,0.0042,0.00014,0.00092,3.9e-05,0.00092,0.0012,0.00092,0.00091,1,1,0.01 +26590000,0.7,0.037,0.076,0.71,-1.2,-0.59,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.005,-0.00049,-0.13,0.21,3.7e-05,0.44,0.0041,-0.00065,-0.0048,0,0,-4.9e+02,7.2e-05,6e-05,0.025,0.015,0.031,0.0058,0.036,0.04,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00087,3.9e-05,0.00086,0.001,0.00087,0.00086,1,1,0.01 +26690000,0.7,0.039,0.079,0.71,-1.3,-0.65,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00019,0.0051,-0.00058,-0.13,0.21,4.3e-05,0.44,0.0039,-0.00013,-0.004,0,0,-4.9e+02,7.2e-05,6.1e-05,0.025,0.017,0.038,0.0058,0.04,0.045,0.032,3.3e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00081,3.9e-05,0.0008,0.001,0.00081,0.00079,1,1,0.01 +26790000,0.7,0.036,0.073,0.71,-1.4,-0.74,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00022,0.0034,0.00028,-0.13,0.21,8.2e-05,0.44,0.0054,0.00061,-0.0037,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.016,0.036,0.0057,0.036,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00076,3.9e-05,0.00075,0.00092,0.00076,0.00074,1,1,0.01 +26890000,0.7,0.045,0.095,0.71,-1.6,-0.8,-1.3,0,0,-4.9e+02,-0.0011,-0.0059,0.00022,0.0035,0.00026,-0.13,0.21,8.7e-05,0.44,0.0053,0.0012,-0.0041,0,0,-4.9e+02,7.3e-05,6e-05,0.022,0.018,0.043,0.0058,0.04,0.046,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00072,3.9e-05,0.0007,0.00091,0.00072,0.0007,1,1,0.01 +26990000,0.7,0.051,0.12,0.71,-1.7,-0.89,-1.3,0,0,-4.9e+02,-0.00099,-0.0059,0.00022,0.0015,-0.0017,-0.13,0.21,0.00012,0.44,0.006,0.0034,-0.0056,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.017,0.042,0.0057,0.037,0.041,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00065,3.9e-05,0.00063,0.00079,0.00065,0.00062,1,1,0.01 +27090000,0.7,0.052,0.12,0.71,-1.9,-0.98,-1.3,0,0,-4.9e+02,-0.00098,-0.0059,0.00022,0.0015,-0.0016,-0.13,0.21,0.00012,0.44,0.006,0.0035,-0.0051,0,0,-4.9e+02,7.4e-05,6e-05,0.019,0.02,0.052,0.0057,0.04,0.048,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00059,3.9e-05,0.00056,0.00078,0.0006,0.00056,1,1,0.01 +27190000,0.7,0.05,0.11,0.7,-2.1,-1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00022,0.00037,-0.0018,-0.13,0.21,4.5e-05,0.44,0.002,0.0027,-0.0049,0,0,-4.9e+02,7.5e-05,6e-05,0.016,0.02,0.051,0.0057,0.043,0.05,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00013,0.00055,3.9e-05,0.00051,0.00066,0.00055,0.00051,1,1,0.01 +27290000,0.71,0.044,0.095,0.7,-2.3,-1.1,-1.2,0,0,-4.9e+02,-0.00097,-0.0059,0.00023,0.00042,-0.0018,-0.13,0.21,5.2e-05,0.44,0.0019,0.0034,-0.0049,0,0,-4.9e+02,7.6e-05,6.1e-05,0.016,0.022,0.059,0.0057,0.047,0.057,0.032,3.2e-07,4.1e-07,1.3e-06,0.0039,0.0041,0.00013,0.00052,3.9e-05,0.00048,0.00066,0.00052,0.00048,1,1,0.01 +27390000,0.71,0.038,0.079,0.7,-2.4,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0058,0.00021,-0.00081,-0.0036,-0.13,0.21,9.9e-06,0.44,-0.0006,0.0031,-0.0063,0,0,-4.9e+02,7.6e-05,6e-05,0.013,0.021,0.052,0.0057,0.049,0.059,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00049,3.9e-05,0.00046,0.00055,0.00049,0.00046,1,1,0.01 +27490000,0.71,0.032,0.064,0.7,-2.5,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00022,-0.00074,-0.0035,-0.13,0.21,1.5e-05,0.44,-0.00068,0.0032,-0.0067,0,0,-4.9e+02,7.7e-05,6.1e-05,0.013,0.023,0.056,0.0057,0.054,0.067,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00048,3.9e-05,0.00045,0.00055,0.00048,0.00044,1,1,0.01 +27590000,0.72,0.028,0.051,0.69,-2.6,-1.1,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00023,-0.0014,-0.0029,-0.12,0.21,-4.1e-05,0.44,-0.0033,0.0027,-0.0065,0,0,-4.9e+02,7.7e-05,6.1e-05,0.011,0.021,0.047,0.0057,0.056,0.068,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00044,1,1,0.01 +27690000,0.72,0.027,0.05,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00092,-0.0059,0.00022,-0.0014,-0.0028,-0.12,0.21,-3.6e-05,0.44,-0.0034,0.0026,-0.0067,0,0,-4.9e+02,7.8e-05,6.1e-05,0.011,0.022,0.049,0.0057,0.062,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00046,3.9e-05,0.00044,0.00047,0.00046,0.00043,1,1,0.01 +27790000,0.72,0.027,0.051,0.69,-2.6,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00022,-0.0019,-0.0026,-0.12,0.21,-6.4e-05,0.44,-0.0052,0.002,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.02,0.042,0.0056,0.064,0.077,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00045,3.9e-05,0.00043,0.00041,0.00045,0.00043,1,1,0.01 +27890000,0.72,0.027,0.049,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00091,-0.0059,0.00022,-0.002,-0.0026,-0.12,0.21,-6.5e-05,0.44,-0.0051,0.002,-0.0072,0,0,-4.9e+02,7.9e-05,6.1e-05,0.0097,0.021,0.043,0.0057,0.07,0.087,0.032,3.2e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00044,3.9e-05,0.00043,0.00041,0.00044,0.00042,1,1,0.01 +27990000,0.72,0.026,0.046,0.69,-2.7,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00024,-0.0018,-0.0017,-0.12,0.21,-8.1e-05,0.44,-0.0065,0.0014,-0.0071,0,0,-4.9e+02,8e-05,6.1e-05,0.0086,0.02,0.038,0.0056,0.072,0.087,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00043,3.9e-05,0.00042,0.00037,0.00043,0.00042,1,1,0.01 +28090000,0.72,0.032,0.059,0.69,-2.8,-1.2,-1.2,0,0,-4.9e+02,-0.00093,-0.0059,0.00023,-0.0019,-0.0015,-0.12,0.21,-8e-05,0.44,-0.0066,0.0013,-0.0072,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0086,0.021,0.039,0.0057,0.078,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00042,3.9e-05,0.00042,0.00036,0.00042,0.00041,1,1,0.01 +28190000,0.72,0.037,0.072,0.69,-2.8,-1.2,-0.94,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.002,-0.001,-0.12,0.21,-9.9e-05,0.44,-0.0074,0.00088,-0.0071,0,0,-4.9e+02,8.1e-05,6.1e-05,0.0079,0.02,0.034,0.0056,0.08,0.097,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.00041,3.9e-05,0.00041,0.00033,0.00041,0.00041,1,1,0.01 +28290000,0.72,0.029,0.055,0.69,-2.8,-1.2,-0.078,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0021,-0.00091,-0.12,0.21,-0.0001,0.44,-0.0075,0.0013,-0.0069,0,0,-4.9e+02,8.2e-05,6.1e-05,0.0079,0.02,0.035,0.0057,0.087,0.11,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00012,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28390000,0.73,0.012,0.024,0.69,-2.8,-1.2,0.78,0,0,-4.9e+02,-0.00094,-0.0059,0.00023,-0.002,-0.00063,-0.12,0.21,-8.9e-05,0.44,-0.0076,0.0013,-0.007,0,0,-4.9e+02,8.3e-05,6.2e-05,0.0079,0.02,0.035,0.0057,0.094,0.12,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28490000,0.73,0.0028,0.0059,0.69,-2.8,-1.2,1.1,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0016,-0.00036,-0.12,0.21,-7e-05,0.44,-0.0077,0.0014,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.035,0.0057,0.1,0.13,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28590000,0.73,0.00089,0.0024,0.69,-2.7,-1.2,0.97,0,0,-4.9e+02,-0.00094,-0.0059,0.00024,-0.0017,-0.0003,-0.12,0.21,-7.3e-05,0.44,-0.0076,0.0015,-0.0069,0,0,-4.9e+02,8.4e-05,6.2e-05,0.0079,0.021,0.033,0.0057,0.11,0.14,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.0004,0.0004,1,1,0.01 +28690000,0.73,0.00019,0.0015,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00095,-0.0059,0.00023,-0.0014,4.4e-05,-0.12,0.21,-5.6e-05,0.44,-0.0077,0.0014,-0.0068,0,0,-4.9e+02,8.5e-05,6.2e-05,0.0079,0.022,0.033,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.0004,3.9e-05,0.0004,0.00033,0.00039,0.0004,1,1,0.01 +28790000,0.73,-1.2e-05,0.0014,0.69,-2.6,-1.2,0.98,0,0,-4.9e+02,-0.00098,-0.0059,0.00024,-0.00092,0.00026,-0.12,0.21,-9.6e-05,0.44,-0.0092,0.0006,-0.006,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.12,0.15,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 +28890000,0.73,-2.3e-06,0.0016,0.69,-2.5,-1.2,0.97,0,0,-4.9e+02,-0.00099,-0.0059,0.00024,-0.00064,0.00058,-0.12,0.21,-8e-05,0.44,-0.0093,0.00056,-0.0059,0,0,-4.9e+02,8.6e-05,6.2e-05,0.0075,0.021,0.028,0.0057,0.13,0.16,0.032,3.1e-07,4e-07,1.3e-06,0.0039,0.0041,0.00011,0.00039,3.9e-05,0.0004,0.00031,0.00039,0.00039,1,1,0.01 +28990000,0.73,0.00035,0.0022,0.68,-2.5,-1.1,0.97,0,0,-4.9e+02,-0.001,-0.0059,0.00025,0.00066,0.001,-0.12,0.21,-0.00011,0.44,-0.011,-0.00036,-0.0047,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.02,0.025,0.0056,0.13,0.16,0.032,3e-07,4e-07,1.3e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.0004,0.0003,0.00039,0.00039,1,1,0.01 +29090000,0.73,0.00052,0.0026,0.68,-2.4,-1.1,0.96,0,0,-4.9e+02,-0.001,-0.0059,0.00025,0.00085,0.0014,-0.12,0.21,-9.7e-05,0.44,-0.011,-0.00042,-0.0046,0,0,-4.9e+02,8.7e-05,6.2e-05,0.0073,0.021,0.025,0.0057,0.14,0.17,0.032,3e-07,4e-07,1.2e-06,0.0038,0.004,0.00011,0.00039,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29190000,0.73,0.00076,0.003,0.68,-2.4,-1.1,0.96,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.0012,0.0014,-0.12,0.21,-0.00013,0.44,-0.011,-0.00065,-0.0041,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0072,0.02,0.023,0.0056,0.14,0.17,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29290000,0.73,0.0011,0.0038,0.68,-2.3,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.0013,0.0019,-0.12,0.21,-0.00012,0.44,-0.011,-0.00072,-0.0041,0,0,-4.9e+02,8.8e-05,6.2e-05,0.0072,0.021,0.024,0.0056,0.14,0.18,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.0003,0.00038,0.00039,1,1,0.01 +29390000,0.73,0.0017,0.0054,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.002,0.0024,-0.12,0.21,-0.00016,0.44,-0.012,-0.0014,-0.0033,0,0,-4.9e+02,8.8e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.18,0.031,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 +29490000,0.73,0.0022,0.0064,0.68,-2.3,-1.1,0.99,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.002,0.0026,-0.12,0.21,-0.00015,0.44,-0.012,-0.0014,-0.0033,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.15,0.19,0.032,3e-07,3.9e-07,1.2e-06,0.0038,0.004,0.00011,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00039,1,1,0.01 +29590000,0.73,0.0027,0.0075,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00026,0.0026,0.0026,-0.12,0.21,-0.00016,0.44,-0.012,-0.0016,-0.0029,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.15,0.19,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00038,0.00038,1,1,0.01 +29690000,0.73,0.003,0.0081,0.68,-2.2,-1.1,0.98,0,0,-4.9e+02,-0.0011,-0.0059,0.00025,0.0026,0.003,-0.12,0.21,-0.00016,0.44,-0.012,-0.0017,-0.0029,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.024,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00038,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29790000,0.73,0.0034,0.0085,0.68,-2.2,-1.1,0.96,0,0,-4.9e+02,-0.0012,-0.0059,0.00025,0.0036,0.0028,-0.12,0.21,-0.00018,0.44,-0.013,-0.0019,-0.0022,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.02,0.023,0.0056,0.16,0.2,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29890000,0.73,0.0034,0.0087,0.68,-2.1,-1.1,0.95,0,0,-4.9e+02,-0.0012,-0.0059,0.00024,0.0033,0.0033,-0.12,0.21,-0.00018,0.44,-0.013,-0.002,-0.0023,0,0,-4.9e+02,9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.17,0.21,0.031,2.9e-07,3.8e-07,1.2e-06,0.0038,0.004,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +29990000,0.73,0.0036,0.0087,0.68,-2.1,-1.1,0.94,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0038,0.0029,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-4.9e+02,8.9e-05,6e-05,0.0071,0.02,0.024,0.0056,0.17,0.21,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00029,0.00037,0.00038,1,1,0.01 +30090000,0.73,0.0035,0.0086,0.68,-2.1,-1.1,0.93,0,0,-4.9e+02,-0.0012,-0.0059,0.00022,0.0035,0.0033,-0.12,0.21,-0.0002,0.44,-0.013,-0.0022,-0.002,0,0,-4.9e+02,8.9e-05,6.1e-05,0.0071,0.021,0.025,0.0056,0.18,0.22,0.031,2.9e-07,3.7e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00039,0.00028,0.00037,0.00038,1,1,0.01 +30190000,0.73,0.0036,0.0084,0.68,-2.1,-1.1,0.91,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0043,0.0027,-0.12,0.21,-0.00022,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.02,0.025,0.0055,0.18,0.22,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,0.0001,0.00037,3.9e-05,0.00038,0.00028,0.00037,0.00038,1,1,0.01 +30290000,0.73,0.0035,0.0081,0.68,-2,-1.1,0.9,0,0,-4.9e+02,-0.0012,-0.0059,0.00021,0.0041,0.0029,-0.12,0.21,-0.00023,0.44,-0.013,-0.0024,-0.0016,0,0,-4.9e+02,8.9e-05,6.1e-05,0.007,0.021,0.026,0.0056,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30390000,0.73,0.0035,0.0079,0.68,-2,-1.1,0.89,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0049,0.0028,-0.12,0.21,-0.00022,0.43,-0.013,-0.0025,-0.0014,0,0,-4.9e+02,8.7e-05,6e-05,0.007,0.021,0.025,0.0055,0.19,0.23,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.9e-05,0.00037,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30490000,0.73,0.0034,0.0076,0.68,-2,-1.1,0.87,0,0,-4.9e+02,-0.0012,-0.0059,0.00019,0.0049,0.003,-0.12,0.21,-0.00022,0.43,-0.013,-0.0025,-0.0014,0,0,-4.9e+02,8.8e-05,6e-05,0.007,0.022,0.027,0.0056,0.2,0.24,0.031,2.8e-07,3.6e-07,1.2e-06,0.0038,0.0039,9.8e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30590000,0.73,0.0034,0.0071,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0057,0.0026,-0.12,0.21,-0.00024,0.43,-0.012,-0.0025,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.021,0.026,0.0055,0.2,0.24,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30690000,0.73,0.0031,0.0068,0.68,-1.9,-1,0.83,0,0,-4.9e+02,-0.0012,-0.0059,0.00018,0.0054,0.0029,-0.12,0.21,-0.00025,0.43,-0.012,-0.0025,-0.001,0,0,-4.9e+02,8.6e-05,6e-05,0.0069,0.022,0.028,0.0055,0.21,0.25,0.031,2.8e-07,3.5e-07,1.1e-06,0.0038,0.0039,9.7e-05,0.00036,3.9e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30790000,0.73,0.0031,0.0063,0.68,-1.9,-1,0.82,0,0,-4.9e+02,-0.0012,-0.0059,0.00014,0.0062,0.0023,-0.12,0.21,-0.00025,0.43,-0.012,-0.0026,-0.00077,0,0,-4.9e+02,8.4e-05,6e-05,0.0068,0.021,0.027,0.0055,0.21,0.25,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00028,0.00036,0.00038,1,1,0.01 +30890000,0.73,0.0029,0.0059,0.68,-1.9,-1,0.81,0,0,-4.9e+02,-0.0013,-0.0059,0.00015,0.0062,0.0026,-0.12,0.21,-0.00024,0.43,-0.012,-0.0026,-0.00075,0,0,-4.9e+02,8.5e-05,6e-05,0.0068,0.022,0.029,0.0055,0.22,0.26,0.031,2.8e-07,3.4e-07,1.1e-06,0.0037,0.0038,9.6e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00038,1,1,0.01 +30990000,0.73,0.003,0.0052,0.68,-1.8,-1,0.8,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0071,0.0021,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00041,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.021,0.028,0.0055,0.22,0.26,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.5e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00038,1,1,0.01 +31090000,0.73,0.0027,0.0047,0.68,-1.8,-1,0.79,0,0,-4.9e+02,-0.0013,-0.0058,0.00012,0.0069,0.0025,-0.12,0.21,-0.00026,0.43,-0.012,-0.0027,-0.00041,0,0,-4.9e+02,8.3e-05,6e-05,0.0067,0.022,0.03,0.0055,0.23,0.27,0.031,2.7e-07,3.3e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31190000,0.73,0.0026,0.0043,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.5e-05,0.0072,0.0024,-0.12,0.21,-0.00027,0.43,-0.011,-0.0028,-0.00024,0,0,-4.9e+02,8.1e-05,5.9e-05,0.0066,0.021,0.028,0.0055,0.23,0.27,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.4e-05,0.00036,3.8e-05,0.00038,0.00027,0.00036,0.00037,1,1,0.01 +31290000,0.73,0.0023,0.0037,0.68,-1.8,-1,0.78,0,0,-4.9e+02,-0.0013,-0.0058,9.8e-05,0.0069,0.0028,-0.11,0.21,-0.00028,0.43,-0.011,-0.0027,-0.00027,0,0,-4.9e+02,8.2e-05,6e-05,0.0066,0.022,0.03,0.0055,0.24,0.28,0.031,2.7e-07,3.2e-07,1.1e-06,0.0037,0.0038,9.3e-05,0.00036,3.8e-05,0.00038,0.00027,0.00035,0.00037,1,1,0.01 +31390000,0.73,0.0022,0.003,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.6e-05,0.0074,0.0026,-0.11,0.21,-0.00031,0.43,-0.011,-0.0028,-1.9e-05,0,0,-4.9e+02,7.9e-05,5.9e-05,0.0065,0.021,0.029,0.0054,0.24,0.28,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31490000,0.73,0.0019,0.0024,0.68,-1.7,-0.99,0.78,0,0,-4.9e+02,-0.0013,-0.0058,7.2e-05,0.0073,0.0032,-0.11,0.2,-0.0003,0.43,-0.011,-0.0029,1.8e-05,0,0,-4.9e+02,8e-05,5.9e-05,0.0065,0.022,0.031,0.0055,0.25,0.29,0.031,2.7e-07,3.1e-07,1e-06,0.0037,0.0037,9.2e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31590000,0.73,0.002,0.0019,0.68,-1.7,-0.97,0.77,0,0,-4.9e+02,-0.0013,-0.0058,4.5e-05,0.0082,0.0028,-0.11,0.2,-0.00029,0.43,-0.01,-0.0029,0.00026,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.021,0.029,0.0054,0.25,0.29,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31690000,0.73,0.0017,0.0012,0.68,-1.6,-0.97,0.78,0,0,-4.9e+02,-0.0013,-0.0058,4.7e-05,0.0079,0.0031,-0.11,0.2,-0.0003,0.43,-0.01,-0.0029,0.00022,0,0,-4.9e+02,7.8e-05,5.9e-05,0.0063,0.022,0.031,0.0054,0.26,0.3,0.031,2.6e-07,3.1e-07,1e-06,0.0037,0.0037,9.1e-05,0.00036,3.8e-05,0.00038,0.00026,0.00035,0.00037,1,1,0.01 +31790000,0.73,0.0015,0.0004,0.69,-1.6,-0.95,0.78,0,0,-4.9e+02,-0.0013,-0.0058,2.2e-05,0.0089,0.003,-0.11,0.2,-0.0003,0.43,-0.0098,-0.0029,0.00056,0,0,-4.9e+02,7.6e-05,5.9e-05,0.0062,0.021,0.029,0.0054,0.26,0.3,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +31890000,0.73,0.0013,-0.00032,0.69,-1.6,-0.95,0.78,0,0,-4.9e+02,-0.0013,-0.0058,2.3e-05,0.0088,0.0035,-0.11,0.2,-0.0003,0.43,-0.0099,-0.0029,0.00059,0,0,-4.9e+02,7.6e-05,5.9e-05,0.0062,0.022,0.031,0.0054,0.27,0.31,0.031,2.6e-07,3e-07,1e-06,0.0037,0.0037,9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +31990000,0.73,0.0012,-0.00093,0.69,-1.6,-0.93,0.77,0,0,-4.9e+02,-0.0013,-0.0058,-8.8e-06,0.0093,0.0034,-0.11,0.2,-0.0003,0.43,-0.0094,-0.003,0.00076,0,0,-4.9e+02,7.4e-05,5.8e-05,0.006,0.021,0.03,0.0054,0.27,0.31,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32090000,0.73,0.00083,-0.0017,0.69,-1.5,-0.93,0.78,0,0,-4.9e+02,-0.0013,-0.0058,-9.7e-06,0.0091,0.0039,-0.11,0.2,-0.00029,0.43,-0.0094,-0.003,0.00077,0,0,-4.9e+02,7.5e-05,5.9e-05,0.006,0.022,0.032,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.8e-07,0.0037,0.0037,8.9e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32190000,0.73,0.00063,-0.0026,0.69,-1.5,-0.91,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-4.4e-05,0.0096,0.0039,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.021,0.03,0.0054,0.28,0.32,0.031,2.6e-07,2.9e-07,9.6e-07,0.0036,0.0036,8.8e-05,0.00036,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32290000,0.73,0.00035,-0.0034,0.69,-1.5,-0.91,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-4.4e-05,0.0094,0.0045,-0.11,0.2,-0.00031,0.43,-0.0089,-0.0031,0.001,0,0,-4.9e+02,7.3e-05,5.8e-05,0.0059,0.022,0.032,0.0054,0.29,0.33,0.031,2.6e-07,2.9e-07,9.5e-07,0.0036,0.0036,8.8e-05,0.00035,3.8e-05,0.00037,0.00025,0.00035,0.00037,1,1,0.01 +32390000,0.73,0.00026,-0.0041,0.69,-1.5,-0.89,0.77,0,0,-4.9e+02,-0.0014,-0.0058,-6.2e-05,0.0099,0.0044,-0.11,0.2,-0.0003,0.43,-0.0085,-0.0031,0.0012,0,0,-4.9e+02,7.1e-05,5.8e-05,0.0057,0.021,0.03,0.0054,0.29,0.33,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32490000,0.72,0.00011,-0.0044,0.69,-1.4,-0.88,0.78,0,0,-4.9e+02,-0.0014,-0.0058,-6e-05,0.0097,0.0048,-0.11,0.2,-0.0003,0.43,-0.0085,-0.0031,0.0011,0,0,-4.9e+02,7.2e-05,5.8e-05,0.0057,0.022,0.032,0.0054,0.3,0.34,0.031,2.5e-07,2.8e-07,9.3e-07,0.0036,0.0036,8.7e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32590000,0.72,0.00016,-0.0047,0.69,-1.4,-0.87,0.78,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.01,0.0048,-0.11,0.2,-0.00031,0.43,-0.0081,-0.0031,0.0013,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.021,0.03,0.0053,0.3,0.34,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32690000,0.72,0.00012,-0.0048,0.69,-1.4,-0.86,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-8.1e-05,0.01,0.0052,-0.11,0.2,-0.00031,0.43,-0.0081,-0.0031,0.0014,0,0,-4.9e+02,7e-05,5.8e-05,0.0056,0.022,0.032,0.0053,0.31,0.35,0.031,2.5e-07,2.8e-07,9.1e-07,0.0036,0.0036,8.6e-05,0.00035,3.8e-05,0.00037,0.00024,0.00035,0.00037,1,1,0.01 +32790000,0.72,0.00024,-0.0048,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.0001,0.01,0.0052,-0.11,0.2,-0.0003,0.43,-0.0077,-0.0031,0.0015,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0054,0.022,0.03,0.0053,0.3,0.35,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +32890000,0.72,0.00031,-0.0048,0.69,-1.3,-0.84,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.01,0.0058,-0.11,0.2,-0.0003,0.43,-0.0078,-0.0032,0.0016,0,0,-4.9e+02,6.9e-05,5.8e-05,0.0054,0.022,0.031,0.0053,0.32,0.36,0.031,2.5e-07,2.7e-07,8.9e-07,0.0036,0.0036,8.5e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +32990000,0.72,0.00053,-0.0049,0.69,-1.3,-0.82,0.77,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.011,0.0059,-0.11,0.2,-0.00031,0.43,-0.0073,-0.0032,0.0017,0,0,-4.9e+02,6.7e-05,5.7e-05,0.0052,0.021,0.029,0.0053,0.31,0.36,0.03,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +33090000,0.72,0.0005,-0.0049,0.69,-1.3,-0.82,0.76,0,0,-4.9e+02,-0.0014,-0.0057,-0.00011,0.011,0.0062,-0.11,0.2,-0.00031,0.43,-0.0073,-0.0032,0.0017,0,0,-4.9e+02,6.8e-05,5.7e-05,0.0053,0.022,0.031,0.0053,0.33,0.37,0.031,2.5e-07,2.7e-07,8.7e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00023,0.00035,0.00036,1,1,0.01 +33190000,0.72,0.004,-0.0041,0.7,-1.2,-0.8,0.7,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.011,0.0061,-0.11,0.2,-0.00031,0.43,-0.0069,-0.0032,0.0017,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.029,0.0053,0.32,0.37,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.4e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 +33290000,0.67,0.016,-0.0035,0.74,-1.2,-0.78,0.68,0,0,-4.9e+02,-0.0014,-0.0057,-0.00012,0.011,0.0064,-0.11,0.2,-0.00029,0.43,-0.0071,-0.0033,0.0016,0,0,-4.9e+02,6.6e-05,5.7e-05,0.0051,0.022,0.031,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.5e-07,0.0036,0.0036,8.3e-05,0.00035,3.8e-05,0.00037,0.00022,0.00035,0.00036,1,1,0.01 +33390000,0.56,0.014,-0.0038,0.83,-1.2,-0.77,0.88,0,0,-4.9e+02,-0.0014,-0.0057,-0.00013,0.011,0.0067,-0.11,0.2,-0.00035,0.43,-0.0064,-0.0032,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0047,0.021,0.028,0.0053,0.33,0.38,0.031,2.4e-07,2.6e-07,8.3e-07,0.0036,0.0035,8.3e-05,0.00032,3.8e-05,0.00036,0.00021,0.00032,0.00036,1,1,0.01 +33490000,0.43,0.007,-0.0013,0.9,-1.2,-0.76,0.89,0,0,-4.9e+02,-0.0014,-0.0057,-0.00015,0.011,0.0068,-0.11,0.21,-0.00044,0.43,-0.0059,-0.002,0.0018,0,0,-4.9e+02,6.5e-05,5.6e-05,0.0041,0.022,0.029,0.0053,0.34,0.38,0.031,2.4e-07,2.6e-07,8.1e-07,0.0036,0.0035,8.3e-05,0.00025,3.7e-05,0.00036,0.00017,0.00024,0.00036,1,1,0.01 +33590000,0.27,0.00087,-0.0037,0.96,-1.2,-0.75,0.86,0,0,-4.9e+02,-0.0014,-0.0057,-0.00018,0.011,0.0068,-0.11,0.21,-0.00068,0.43,-0.0039,-0.0014,0.002,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0031,0.02,0.027,0.0052,0.34,0.37,0.03,2.4e-07,2.6e-07,7.9e-07,0.0036,0.0035,8.3e-05,0.00016,3.6e-05,0.00036,0.00012,0.00015,0.00036,1,1,0.01 +33690000,0.098,-0.0027,-0.0067,1,-1.1,-0.74,0.87,0,0,-4.9e+02,-0.0014,-0.0057,-0.00019,0.011,0.0068,-0.11,0.21,-0.00074,0.43,-0.0036,-0.001,0.0021,0,0,-4.9e+02,6.4e-05,5.5e-05,0.0024,0.021,0.028,0.0053,0.35,0.37,0.031,2.4e-07,2.6e-07,7.8e-07,0.0036,0.0035,8.3e-05,0.0001,3.5e-05,0.00036,8.3e-05,9.8e-05,0.00036,1,1,0.01 +33790000,-0.074,-0.0046,-0.0085,1,-1.1,-0.72,0.85,0,0,-4.9e+02,-0.0014,-0.0057,-0.00021,0.011,0.0068,-0.11,0.21,-0.0009,0.43,-0.0021,-0.001,0.0023,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0019,0.02,0.026,0.0052,0.35,0.37,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,6.8e-05,3.5e-05,0.00036,5.5e-05,6.1e-05,0.00036,1,1,0.01 +33890000,-0.24,-0.006,-0.0091,0.97,-0.99,-0.69,0.83,0,0,-4.9e+02,-0.0014,-0.0057,-0.00022,0.011,0.0068,-0.11,0.21,-0.001,0.43,-0.0013,-0.0011,0.0024,0,0,-4.9e+02,6.2e-05,5.4e-05,0.0016,0.022,0.028,0.0052,0.36,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,4.8e-05,3.4e-05,0.00036,3.8e-05,4.1e-05,0.00036,1,1,0.01 +33990000,-0.39,-0.0048,-0.012,0.92,-0.94,-0.64,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.011,0.0069,-0.11,0.21,-0.001,0.43,-0.0011,-0.00064,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0015,0.021,0.027,0.0052,0.36,0.37,0.03,2.4e-07,2.5e-07,7.7e-07,0.0036,0.0035,8.3e-05,3.6e-05,3.4e-05,0.00036,2.8e-05,2.9e-05,0.00036,1,1,0.01 +34090000,-0.5,-0.0039,-0.014,0.87,-0.88,-0.59,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00022,0.011,0.0072,-0.11,0.21,-0.00097,0.43,-0.0013,-0.00052,0.0025,0,0,-4.9e+02,6e-05,5.3e-05,0.0014,0.023,0.03,0.0052,0.37,0.38,0.03,2.4e-07,2.6e-07,7.7e-07,0.0036,0.0035,8.3e-05,3e-05,3.4e-05,0.00036,2.2e-05,2.3e-05,0.00036,1,1,0.01 +34190000,-0.57,-0.0038,-0.012,0.82,-0.85,-0.54,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0086,0.0099,-0.11,0.21,-0.00096,0.43,-0.001,-0.00031,0.0028,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0013,0.023,0.029,0.0052,0.37,0.38,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.5e-05,3.4e-05,0.00036,1.8e-05,1.8e-05,0.00036,1,1,0.01 +34290000,-0.61,-0.0048,-0.0091,0.79,-0.8,-0.48,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00021,0.0084,0.01,-0.11,0.21,-0.00098,0.43,-0.00093,-0.00018,0.0027,0,0,-4.9e+02,5.7e-05,5.1e-05,0.0012,0.025,0.032,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.6e-07,0.0035,0.0035,8.2e-05,2.2e-05,3.4e-05,0.00036,1.5e-05,1.5e-05,0.00036,1,1,0.01 +34390000,-0.63,-0.0054,-0.0062,0.77,-0.77,-0.44,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0056,0.014,-0.11,0.21,-0.00094,0.43,-0.00092,1.8e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0012,0.025,0.031,0.0052,0.38,0.39,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.2e-05,2e-05,3.3e-05,0.00036,1.3e-05,1.3e-05,0.00036,1,1,0.01 +34490000,-0.65,-0.0063,-0.004,0.76,-0.72,-0.39,0.81,0,0,-4.9e+02,-0.0015,-0.0057,-0.00019,0.0054,0.015,-0.11,0.21,-0.00095,0.43,-0.00086,-2.3e-05,0.0029,0,0,-4.9e+02,5.4e-05,4.9e-05,0.0011,0.027,0.035,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0034,0.0035,8.1e-05,1.8e-05,3.3e-05,0.00036,1.2e-05,1.2e-05,0.00036,1,1,0.01 +34590000,-0.66,-0.0063,-0.0026,0.75,-0.7,-0.36,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,0.00036,0.021,-0.11,0.21,-0.00091,0.43,-0.00093,3.3e-05,0.0032,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.027,0.034,0.0052,0.39,0.4,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,1.1e-05,1e-05,0.00036,1,1,0.01 +34690000,-0.67,-0.0067,-0.0018,0.75,-0.64,-0.32,0.8,0,0,-4.9e+02,-0.0015,-0.0058,-0.00015,0.00011,0.021,-0.11,0.21,-0.00093,0.43,-0.00079,0.00023,0.0031,0,0,-4.9e+02,5e-05,4.7e-05,0.0011,0.03,0.037,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.5e-07,0.0033,0.0034,8.1e-05,1.6e-05,3.3e-05,0.00036,9.9e-06,9.4e-06,0.00036,1,1,0.01 +34790000,-0.67,-0.0061,-0.0013,0.74,-0.63,-0.3,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.0059,0.028,-0.11,0.21,-0.00092,0.43,-0.00063,0.00033,0.0033,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.029,0.036,0.0052,0.4,0.41,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,9.1e-06,8.6e-06,0.00036,1,1,0.01 +34890000,-0.67,-0.0061,-0.0012,0.74,-0.57,-0.25,0.79,0,0,-4.9e+02,-0.0015,-0.0058,-0.00011,-0.0061,0.028,-0.11,0.21,-0.00092,0.43,-0.00064,0.00028,0.0033,0,0,-4.9e+02,4.6e-05,4.4e-05,0.001,0.032,0.04,0.0052,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.0032,0.0033,8e-05,1.4e-05,3.3e-05,0.00036,8.4e-06,7.9e-06,0.00036,1,1,0.01 +34990000,-0.67,-0.013,-0.0037,0.74,0.47,0.36,-0.043,0,0,-4.9e+02,-0.0016,-0.0058,-6.8e-05,-0.014,0.038,-0.11,0.21,-0.00089,0.43,-0.00052,0.00034,0.0035,0,0,-4.9e+02,4.2e-05,4.1e-05,0.001,0.034,0.046,0.0054,0.41,0.42,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.3e-05,3.3e-05,0.00036,8e-06,7.4e-06,0.00036,1,1,0.01 +35090000,-0.67,-0.013,-0.0037,0.74,0.61,0.39,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-6.9e-05,-0.014,0.038,-0.11,0.21,-0.00089,0.43,-0.00048,0.00029,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.037,0.051,0.0055,0.42,0.43,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.5e-06,6.9e-06,0.00036,1,1,0.01 +35190000,-0.67,-0.013,-0.0038,0.74,0.64,0.43,-0.1,0,0,-4.9e+02,-0.0016,-0.0058,-7e-05,-0.014,0.038,-0.11,0.21,-0.0009,0.43,-0.00041,0.00033,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00099,0.041,0.055,0.0055,0.43,0.44,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.2e-05,3.3e-05,0.00036,7.1e-06,6.4e-06,0.00036,1,1,0.01 +35290000,-0.67,-0.013,-0.0039,0.74,0.67,0.48,-0.098,0,0,-4.9e+02,-0.0016,-0.0058,-7.1e-05,-0.014,0.038,-0.11,0.21,-0.00092,0.43,-0.00034,0.00033,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.044,0.06,0.0055,0.44,0.45,0.03,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.8e-06,6.1e-06,0.00036,1,1,0.01 +35390000,-0.67,-0.013,-0.0038,0.74,0.7,0.52,-0.096,0,0,-4.9e+02,-0.0016,-0.0058,-7.4e-05,-0.014,0.038,-0.11,0.21,-0.00093,0.43,-0.00024,0.0003,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00098,0.048,0.064,0.0055,0.46,0.47,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.5e-06,5.8e-06,0.00036,1,1,0.01 +35490000,-0.67,-0.013,-0.0039,0.74,0.73,0.56,-0.094,0,0,-4.9e+02,-0.0016,-0.0058,-7.7e-05,-0.014,0.038,-0.11,0.21,-0.00094,0.43,-0.00015,0.00025,0.0037,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.053,0.069,0.0055,0.47,0.48,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6.2e-06,5.5e-06,0.00036,1,1,0.01 +35590000,-0.67,-0.013,-0.0039,0.74,0.76,0.6,-0.093,0,0,-4.9e+02,-0.0016,-0.0058,-7.6e-05,-0.014,0.038,-0.11,0.21,-0.00094,0.43,-0.00017,0.00025,0.0036,0,0,-4.9e+02,4.2e-05,4.1e-05,0.00097,0.057,0.075,0.0055,0.49,0.5,0.031,2.4e-07,2.5e-07,7.4e-07,0.003,0.0032,8e-05,1.1e-05,3.3e-05,0.00036,6e-06,5.2e-06,0.00036,1,1,0.01 +35690000,-0.68,-0.013,-0.0038,0.74,0.79,0.65,-0.091,0,0,-4.9e+02,-0.0016,-0.0058,-7.7e-05,-0.014,0.038,-0.11,0.21,-0.00095,0.43,-9.9e-05,0.00024,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.062,0.08,0.0056,0.51,0.52,0.031,2.4e-07,2.6e-07,7.4e-07,0.003,0.0032,8e-05,1e-05,3.3e-05,0.00036,5.8e-06,5e-06,0.00036,1,1,0.01 +35790000,-0.68,-0.013,-0.0038,0.74,0.82,0.69,-0.088,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.014,0.038,-0.11,0.21,-0.00095,0.43,-7.4e-05,0.00024,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.067,0.086,0.0056,0.53,0.54,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.6e-06,4.8e-06,0.00036,1,1,0.023 +35890000,-0.68,-0.013,-0.0039,0.74,0.86,0.73,-0.085,0,0,-4.9e+02,-0.0016,-0.0058,-7.9e-05,-0.014,0.038,-0.11,0.21,-0.00096,0.43,-5.9e-05,0.00023,0.0037,0,0,-4.9e+02,4.2e-05,4.2e-05,0.00097,0.072,0.092,0.0056,0.55,0.56,0.031,2.4e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,1e-05,3.3e-05,0.00036,5.4e-06,4.6e-06,0.00036,1,1,0.048 +35990000,-0.68,-0.013,-0.0039,0.74,0.89,0.77,-0.082,0,0,-4.9e+02,-0.0016,-0.0058,-7.8e-05,-0.014,0.038,-0.11,0.21,-0.00095,0.43,-8.4e-05,0.00022,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00097,0.078,0.099,0.0056,0.57,0.59,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,8e-05,9.9e-06,3.3e-05,0.00036,5.3e-06,4.5e-06,0.00036,1,1,0.073 +36090000,-0.68,-0.013,-0.0039,0.74,0.92,0.81,-0.078,0,0,-4.9e+02,-0.0016,-0.0058,-8e-05,-0.014,0.038,-0.11,0.21,-0.00096,0.43,-4.9e-05,0.0002,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.083,0.11,0.0056,0.6,0.62,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.7e-06,3.3e-05,0.00036,5.2e-06,4.3e-06,0.00036,1,1,0.099 +36190000,-0.68,-0.013,-0.0039,0.74,0.95,0.86,-0.074,0,0,-4.9e+02,-0.0016,-0.0058,-8.4e-05,-0.014,0.038,-0.11,0.21,-0.00097,0.43,4.9e-05,0.00019,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.089,0.11,0.0056,0.63,0.65,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.5e-06,3.3e-05,0.00036,5e-06,4.2e-06,0.00036,1,1,0.12 +36290000,-0.68,-0.013,-0.0038,0.74,0.98,0.9,-0.069,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.014,0.037,-0.11,0.21,-0.00098,0.43,6.9e-05,0.00019,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.096,0.12,0.0056,0.66,0.68,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.9e-05,9.4e-06,3.3e-05,0.00036,4.9e-06,4.1e-06,0.00036,1,1,0.15 +36390000,-0.68,-0.013,-0.0038,0.74,1,0.94,-0.066,0,0,-4.9e+02,-0.0016,-0.0058,-8.5e-05,-0.014,0.037,-0.11,0.21,-0.00098,0.43,6.6e-05,0.00022,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.1,0.13,0.0056,0.69,0.72,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.3e-06,3.3e-05,0.00036,4.8e-06,3.9e-06,0.00036,1,1,0.17 +36490000,-0.68,-0.013,-0.0039,0.74,1,0.98,-0.063,0,0,-4.9e+02,-0.0016,-0.0058,-8.3e-05,-0.014,0.037,-0.11,0.21,-0.00097,0.43,3.7e-05,0.00023,0.0037,0,0,-4.9e+02,4.3e-05,4.2e-05,0.00096,0.11,0.13,0.0056,0.72,0.76,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.2e-06,3.3e-05,0.00036,4.7e-06,3.8e-06,0.00036,1,1,0.2 +36590000,-0.68,-0.013,-0.0038,0.74,1.1,1,-0.057,0,0,-4.9e+02,-0.0016,-0.0058,-8.6e-05,-0.013,0.037,-0.11,0.21,-0.00098,0.43,8.5e-05,0.00026,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.14,0.0056,0.76,0.8,0.031,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9.1e-06,3.3e-05,0.00036,4.6e-06,3.7e-06,0.00036,1,1,0.23 +36690000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.052,0,0,-4.9e+02,-0.0016,-0.0058,-8.8e-05,-0.013,0.037,-0.11,0.21,-0.00099,0.43,0.00012,0.00027,0.0037,0,0,-4.9e+02,4.3e-05,4.3e-05,0.00096,0.12,0.15,0.0057,0.8,0.85,0.032,2.5e-07,2.6e-07,7.5e-07,0.003,0.0031,7.8e-05,9e-06,3.3e-05,0.00036,4.6e-06,3.6e-06,0.00036,1,1,0.25 +36790000,-0.68,-0.013,-0.0038,0.74,1.1,1.1,-0.046,0,0,-4.9e+02,-0.0016,-0.0058,-9.2e-05,-0.013,0.037,-0.11,0.21,-0.001,0.43,0.00016,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.13,0.16,0.0057,0.84,0.9,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.9e-06,3.3e-05,0.00036,4.5e-06,3.5e-06,0.00036,1,1,0.28 +36890000,-0.68,-0.013,-0.0038,0.74,1.2,1.2,-0.041,0,0,-4.9e+02,-0.0016,-0.0058,-9.5e-05,-0.013,0.037,-0.11,0.21,-0.001,0.43,0.0002,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.14,0.17,0.0057,0.89,0.95,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.8e-06,3.3e-05,0.00036,4.4e-06,3.4e-06,0.00036,1,1,0.3 +36990000,-0.68,-0.013,-0.0037,0.74,1.2,1.2,-0.037,0,0,-4.9e+02,-0.0016,-0.0058,-9.6e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00023,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,0.94,1,0.032,2.5e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.7e-06,3.3e-05,0.00036,4.3e-06,3.4e-06,0.00036,1,1,0.33 +37090000,-0.68,-0.013,-0.0036,0.74,1.2,1.2,-0.031,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00023,0.00026,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.15,0.18,0.0057,1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.7e-05,8.6e-06,3.3e-05,0.00036,4.3e-06,3.3e-06,0.00036,1,1,0.35 +37190000,-0.68,-0.013,-0.0036,0.74,1.3,1.3,-0.025,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00022,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.3e-05,0.00096,0.16,0.19,0.0057,1.1,1.1,0.032,2.6e-07,2.7e-07,7.5e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.38 +37290000,-0.68,-0.013,-0.0037,0.74,1.3,1.3,-0.019,0,0,-4.9e+02,-0.0016,-0.0058,-9.8e-05,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00024,0.00027,0.0037,0,0,-4.9e+02,4.4e-05,4.4e-05,0.00096,0.17,0.2,0.0057,1.1,1.2,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.5e-06,3.3e-05,0.00036,4.2e-06,3.2e-06,0.00036,1,1,0.41 +37390000,-0.68,-0.013,-0.0036,0.74,1.3,1.4,-0.015,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00026,0.00027,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.18,0.21,0.0057,1.2,1.3,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3.1e-06,0.00036,1,1,0.43 +37490000,-0.68,-0.013,-0.0035,0.74,1.4,1.4,-0.0088,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.0003,0.0003,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.19,0.22,0.0057,1.3,1.4,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.6e-05,8.4e-06,3.3e-05,0.00036,4.1e-06,3e-06,0.00036,1,1,0.46 +37590000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,-0.0022,0,0,-4.9e+02,-0.0016,-0.0058,-0.0001,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00031,0.00031,0.0037,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.2,0.23,0.0057,1.3,1.5,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.3e-06,3.3e-05,0.00036,4e-06,3e-06,0.00036,1,1,0.48 +37690000,-0.68,-0.013,-0.0035,0.74,1.4,1.5,0.0051,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.036,-0.11,0.21,-0.001,0.43,0.00033,0.00029,0.0038,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.21,0.24,0.0057,1.4,1.6,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00036,1,1,0.51 +37790000,-0.68,-0.013,-0.0036,0.74,1.5,1.5,0.012,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.035,-0.11,0.21,-0.001,0.43,0.00034,0.0003,0.0038,0,0,-4.9e+02,4.5e-05,4.4e-05,0.00096,0.22,0.26,0.0057,1.5,1.7,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.2e-06,3.3e-05,0.00036,4e-06,2.9e-06,0.00036,1,1,0.54 +37890000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.018,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.035,-0.11,0.21,-0.001,0.43,0.00034,0.00029,0.0038,0,0,-4.9e+02,4.5e-05,4.5e-05,0.00096,0.23,0.27,0.0056,1.6,1.8,0.032,2.6e-07,2.7e-07,7.6e-07,0.003,0.0031,7.5e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00036,1,1,0.56 +37990000,-0.68,-0.013,-0.0036,0.74,1.5,1.6,0.026,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.013,0.035,-0.11,0.21,-0.001,0.43,0.00035,0.00029,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.24,0.28,0.0057,1.7,1.9,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8.1e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00036,1,1,0.59 +38090000,-0.68,-0.014,-0.0036,0.74,1.6,1.7,0.034,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00036,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.25,0.29,0.0056,1.8,2,0.032,2.6e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.9e-06,2.8e-06,0.00036,1,1,0.61 +38190000,-0.68,-0.013,-0.0036,0.74,1.6,1.7,0.04,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00029,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.26,0.3,0.0056,1.9,2.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00036,1,1,0.64 +38290000,-0.68,-0.014,-0.0036,0.74,1.6,1.8,0.047,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00028,0.0038,0,0,-4.9e+02,4.6e-05,4.5e-05,0.00096,0.27,0.31,0.0056,2.1,2.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.4e-05,8e-06,3.3e-05,0.00036,3.8e-06,2.7e-06,0.00036,1,1,0.67 +38390000,-0.68,-0.014,-0.0035,0.74,1.7,1.8,0.052,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.0003,0.0038,0,0,-4.9e+02,4.6e-05,4.6e-05,0.00096,0.28,0.33,0.0056,2.2,2.5,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00036,1,1,0.69 +38490000,-0.68,-0.014,-0.0035,0.74,1.7,1.8,0.058,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00038,0.00032,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.29,0.34,0.0056,2.3,2.6,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.9e-06,3.3e-05,0.00036,3.8e-06,2.6e-06,0.00036,1,1,0.72 +38590000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.064,0,0,-4.9e+02,-0.0016,-0.0058,-0.00011,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00037,0.00032,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.3,0.35,0.0056,2.5,2.8,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.6e-06,0.00036,1,1,0.75 +38690000,-0.68,-0.014,-0.0034,0.74,1.7,1.9,0.069,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.00039,0.00034,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.31,0.36,0.0056,2.6,3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.3e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.77 +38790000,-0.68,-0.014,-0.0034,0.74,1.8,2,0.075,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.0004,0.00033,0.0038,0,0,-4.9e+02,4.7e-05,4.6e-05,0.00097,0.33,0.38,0.0056,2.8,3.2,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.8e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.8 +38890000,-0.68,-0.014,-0.0035,0.74,1.8,2,0.57,0,0,-4.9e+02,-0.0016,-0.0058,-0.00012,-0.012,0.035,-0.11,0.21,-0.001,0.43,0.0004,0.00031,0.0038,0,0,-4.9e+02,4.8e-05,4.7e-05,0.00097,0.33,0.39,0.0056,3,3.3,0.032,2.7e-07,2.8e-07,7.6e-07,0.003,0.0031,7.2e-05,7.7e-06,3.3e-05,0.00036,3.7e-06,2.5e-06,0.00035,1,1,0.83 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index a63e88ee9d..211fc9fcd0 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,351 +1,351 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 -90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 -190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-2.1e-11,0,0,-1.1e-06,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 -290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-2.1e-10,0,0,-1e-05,0,0,0,0,0,0,0,0,0.095,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 -390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.063,-0.00056,-0.00071,-0.013,-7.1e-09,-5.8e-09,1.5e-11,0,0,2.2e-06,0,0,0,0,0,0,0,0,0.095,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 -490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.069,-0.00015,-0.00046,-0.011,-1.2e-06,7.4e-07,4.1e-08,0,0,-1e-06,0,0,0,0,0,0,0,0,0.095,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 -590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.12,-0.00028,-0.0012,-0.029,-1.3e-06,7.7e-07,4.5e-08,0,0,7.8e-05,0,0,0,0,0,0,0,0,0.095,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 -690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.05,-8e-05,-0.00078,-0.0088,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.095,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 -790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,0.095,0.018,0.018,0.00077,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 -890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.093,0.00015,-0.0011,-0.031,-2.1e-05,1e-06,4.9e-07,0,0,-8.1e-05,0,0,0,0,0,0,0,0,0.095,0.019,0.019,0.00051,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 -990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1e-06,4.9e-07,0,0,-2.6e-05,0,0,0,0,0,0,0,0,0.095,0.021,0.021,0.00062,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 -1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0.00077,-0.0014,-0.062,-6e-05,-1.5e-05,9.8e-07,0,0,1.1e-05,0,0,0,0,0,0,0,0,0.095,0.023,0.023,0.00043,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 -1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,9.6e-07,0,0,-0.00056,0,0,0,0,0,0,0,0,0.095,0.025,0.025,0.00051,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 -1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.11,0.0019,-0.0024,-0.048,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00083,0,0,0,0,0,0,0,0,0.095,0.026,0.026,0.00038,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 -1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.097,0.0043,-0.0044,-0.038,-0.00016,-9.2e-05,1.5e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.095,0.028,0.028,0.00043,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 -1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.12,0.0034,-0.0032,-0.053,-0.00039,-0.00033,1.2e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,0.095,0.027,0.027,0.00033,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 -1590000,1,-0.012,-0.014,0.0004,0.031,-0.024,-0.13,0.0061,-0.0055,-0.063,-0.00039,-0.00033,1.2e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.095,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 -1690000,1,-0.012,-0.014,0.00045,0.028,-0.019,-0.13,0.0043,-0.0037,-0.068,-0.00073,-0.00074,-3.4e-07,0,0,-0.0019,0,0,0,0,0,0,0,0,0.095,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 -1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0.0076,-0.0059,-0.067,-0.00073,-0.00073,-2.9e-07,0,0,-0.0029,0,0,0,0,0,0,0,0,0.095,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 -1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0.011,-0.0083,-0.075,-0.00072,-0.00072,-2.7e-07,0,0,-0.0033,0,0,0,0,0,0,0,0,0.095,0.031,0.031,0.00037,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 -1990000,1,-0.011,-0.014,0.00041,0.035,-0.018,-0.14,0.0082,-0.0054,-0.074,-0.0011,-0.0013,-3.6e-06,0,0,-0.0047,0,0,0,0,0,0,0,0,0.095,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 -2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0.012,-0.0073,-0.071,-0.0011,-0.0012,-3.5e-06,0,0,-0.0066,0,0,0,0,0,0,0,0,0.095,0.027,0.027,0.00032,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 -2190000,1,-0.011,-0.014,0.00039,0.033,-0.014,-0.14,0.0081,-0.0043,-0.077,-0.0014,-0.0018,-8.7e-06,0,0,-0.0076,0,0,0,0,0,0,0,0,0.095,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 -2290000,1,-0.011,-0.014,0.00039,0.038,-0.014,-0.14,0.012,-0.0057,-0.075,-0.0014,-0.0018,-8.5e-06,0,0,-0.0099,0,0,0,0,0,0,0,0,0.095,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 -2390000,1,-0.011,-0.013,0.0004,0.029,-0.01,-0.14,0.0075,-0.0033,-0.072,-0.0017,-0.0023,-1.4e-05,0,0,-0.013,0,0,0,0,0,0,0,0,0.095,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 -2490000,1,-0.011,-0.014,0.00048,0.033,-0.0091,-0.14,0.011,-0.0043,-0.079,-0.0017,-0.0023,-1.4e-05,0,0,-0.014,0,0,0,0,0,0,0,0,0.095,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 -2590000,1,-0.01,-0.013,0.0004,0.023,-0.0062,-0.15,0.0066,-0.0024,-0.084,-0.0018,-0.0027,-2e-05,0,0,-0.015,0,0,0,0,0,0,0,0,0.095,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 -2690000,1,-0.01,-0.013,0.00044,0.027,-0.0055,-0.15,0.0091,-0.003,-0.084,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,0.095,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 -2790000,1,-0.01,-0.013,0.00038,0.022,-0.0032,-0.14,0.0059,-0.0017,-0.081,-0.0019,-0.003,-2.5e-05,0,0,-0.022,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 -2890000,1,-0.01,-0.013,0.00031,0.026,-0.005,-0.14,0.0082,-0.0021,-0.081,-0.0019,-0.003,-2.5e-05,0,0,-0.026,0,0,0,0,0,0,0,0,0.095,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 -2990000,1,-0.01,-0.013,0.00033,0.02,-0.0039,-0.15,0.0054,-0.0013,-0.086,-0.002,-0.0033,-3e-05,0,0,-0.028,0,0,0,0,0,0,0,0,0.095,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 -3090000,1,-0.01,-0.013,0.00053,0.025,-0.0068,-0.15,0.0077,-0.0019,-0.087,-0.002,-0.0033,-3e-05,0,0,-0.031,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 -3190000,1,-0.01,-0.013,0.00058,0.02,-0.0065,-0.15,0.0051,-0.0014,-0.097,-0.0021,-0.0036,-3.5e-05,0,0,-0.033,0,0,0,0,0,0,0,0,0.095,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 -3290000,1,-0.01,-0.013,0.0006,0.023,-0.0067,-0.15,0.0073,-0.0021,-0.11,-0.0021,-0.0036,-3.4e-05,0,0,-0.035,0,0,0,0,0,0,0,0,0.095,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 -3390000,1,-0.0098,-0.013,0.00061,0.019,-0.0036,-0.15,0.0049,-0.0014,-0.1,-0.0021,-0.0038,-3.8e-05,0,0,-0.04,0,0,0,0,0,0,0,0,0.095,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 -3490000,1,-0.0097,-0.013,0.0006,0.025,-0.0023,-0.15,0.0072,-0.0017,-0.1,-0.0021,-0.0038,-3.8e-05,0,0,-0.044,0,0,0,0,0,0,0,0,0.095,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 -3590000,1,-0.0095,-0.012,0.00056,0.021,-0.0018,-0.15,0.0051,-0.001,-0.11,-0.0022,-0.004,-4.2e-05,0,0,-0.047,0,0,0,0,0,0,0,0,0.095,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 -3690000,1,-0.0095,-0.013,0.00054,0.024,-0.0011,-0.15,0.0074,-0.0012,-0.11,-0.0022,-0.004,-4.2e-05,0,0,-0.052,0,0,0,0,0,0,0,0,0.095,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 -3790000,1,-0.0094,-0.012,0.00057,0.019,0.0033,-0.15,0.0051,-0.00056,-0.11,-0.0022,-0.0043,-4.7e-05,0,0,-0.055,0,0,0,0,0,0,0,0,0.095,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 -3890000,1,-0.0094,-0.013,0.00065,0.021,0.0046,-0.14,0.0072,-0.00017,-0.11,-0.0022,-0.0042,-4.7e-05,0,0,-0.059,0,0,0,0,0,0,0,0,0.095,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -3990000,1,-0.0094,-0.013,0.00072,0.026,0.0042,-0.14,0.0096,0.00021,-0.11,-0.0022,-0.0042,-4.7e-05,0,0,-0.064,0,0,0,0,0,0,0,0,0.095,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 -4090000,1,-0.0093,-0.012,0.00078,0.022,0.0037,-0.12,0.0071,0.00045,-0.098,-0.0022,-0.0044,-5.1e-05,0,0,-0.072,0,0,0,0,0,0,0,0,0.095,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4190000,1,-0.0094,-0.012,0.00075,0.024,0.0034,-0.12,0.0094,0.0008,-0.1,-0.0022,-0.0044,-5.1e-05,0,0,-0.074,0,0,0,0,0,0,0,0,0.095,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4290000,1,-0.0095,-0.012,0.00076,0.021,0.0033,-0.12,0.0068,0.00069,-0.11,-0.0022,-0.0046,-5.6e-05,0,0,-0.077,0,0,0,0,0,0,0,0,0.095,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4390000,1,-0.0094,-0.012,0.00072,0.025,0.0018,-0.11,0.0091,0.00086,-0.094,-0.0022,-0.0046,-5.6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,0.095,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 -4490000,1,-0.0094,-0.012,0.00078,0.021,0.0036,-0.11,0.0067,0.00072,-0.095,-0.0022,-0.0048,-6.1e-05,0,0,-0.086,0,0,0,0,0,0,0,0,0.095,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4590000,1,-0.0094,-0.012,0.00085,0.023,0.0024,-0.11,0.0089,0.001,-0.098,-0.0022,-0.0048,-6.1e-05,0,0,-0.088,0,0,0,0,0,0,0,0,0.095,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4690000,1,-0.0094,-0.012,0.00079,0.017,0.0027,-0.1,0.0064,0.00075,-0.09,-0.0021,-0.005,-6.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,0.095,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4790000,1,-0.0093,-0.012,0.00089,0.015,0.0048,-0.099,0.008,0.0012,-0.092,-0.0021,-0.005,-6.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,0.095,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 -4890000,1,-0.0093,-0.012,0.00093,0.01,0.0024,-0.093,0.0053,0.0009,-0.088,-0.0021,-0.0051,-6.9e-05,0,0,-0.099,0,0,0,0,0,0,0,0,0.095,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -4990000,1,-0.0092,-0.012,0.00091,0.013,0.0031,-0.085,0.0065,0.0012,-0.083,-0.0021,-0.0051,-6.9e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.095,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5090000,1,-0.0091,-0.011,0.00099,0.01,0.0034,-0.082,0.0045,0.00088,-0.082,-0.0021,-0.0052,-7.2e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.095,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5190000,1,-0.0089,-0.012,0.001,0.0099,0.007,-0.08,0.0055,0.0014,-0.079,-0.0021,-0.0052,-7.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 -5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0071,-0.068,0.0038,0.0013,-0.072,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0.0046,0.0021,-0.067,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.06,0.0031,0.002,-0.065,-0.002,-0.0054,-7.7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5590000,1,-0.0088,-0.012,0.001,0.0083,0.016,-0.053,0.004,0.0033,-0.058,-0.002,-0.0054,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 -5690000,1,-0.0089,-0.011,0.00093,0.0077,0.016,-0.052,0.0028,0.0029,-0.055,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5790000,1,-0.0088,-0.011,0.00088,0.0089,0.018,-0.049,0.0036,0.0046,-0.053,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5890000,1,-0.0088,-0.011,0.00092,0.0095,0.015,-0.048,0.0027,0.0037,-0.056,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -5990000,1,-0.0088,-0.012,0.00089,0.011,0.017,-0.041,0.0038,0.0053,-0.05,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 -6090000,1,-0.0088,-0.011,0.00071,0.011,0.018,-0.039,0.0049,0.0071,-0.047,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6190000,1,-0.0089,-0.011,0.00072,0.0087,0.017,-0.038,0.0038,0.0057,-0.047,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6290000,1,-0.0089,-0.011,0.00075,0.008,0.019,-0.041,0.0046,0.0075,-0.053,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6390000,1,-0.0089,-0.011,0.00076,0.0082,0.016,-0.042,0.0034,0.006,-0.056,-0.0018,-0.0056,-8.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 -6490000,1,-0.0089,-0.011,0.00066,0.0057,0.016,-0.039,0.0041,0.0076,-0.053,-0.0018,-0.0056,-8.7e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6590000,1,-0.0089,-0.011,0.00059,0.0039,0.015,-0.042,0.0029,0.0058,-0.056,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6690000,1,-0.0088,-0.011,0.00055,0.0022,0.018,-0.044,0.0032,0.0075,-0.057,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6790000,1,-0.0089,-0.011,0.00052,0.003,0.015,-0.042,0.0021,0.006,-0.058,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 -6890000,1,-0.0087,-0.011,0.00043,0.0023,0.015,-0.039,0.0024,0.0075,-0.055,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 -6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0.0023,0.0048,-0.055,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,0.095,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 -7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0.0014,0.0074,-0.056,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,0.095,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 -7190000,0.98,-0.0065,-0.012,0.18,-5.8e-05,0.019,-0.037,0.0016,0.0091,-0.058,-0.0016,-0.0056,-9.2e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.7e-06,0,0,0.095,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 -7290000,0.98,-0.0064,-0.012,0.18,-0.00067,0.024,-0.034,0.00055,0.012,-0.054,-0.0016,-0.0057,-9.2e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,0.095,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00094,-0.032,0.00016,0.014,-0.052,-0.0017,-0.0057,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.8e-05,0,0,0.095,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7490000,0.98,-0.0063,-0.012,0.18,0.00097,0.0035,-0.026,0.00016,0.014,-0.046,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.9e-05,0,0,0.095,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.006,-0.023,0.00032,0.014,-0.041,-0.0017,-0.0056,-9e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00034,-0.00039,-9.5e-06,0,0,0.095,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9 -7690000,0.98,-0.0064,-0.013,0.18,0.0021,0.0093,-0.022,0.00055,0.015,-0.036,-0.0017,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-3.4e-05,0.43,-0.00031,-0.0004,2.5e-06,0,0,0.095,0.0016,0.0015,0.042,25,25,0.025,52,52,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00017,0.0013,0.0014,0.0016,0.0013,1,1,2 -7790000,0.98,-0.0064,-0.013,0.18,0.0056,0.01,-0.025,0.00093,0.015,-0.041,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.9e-05,0.43,-0.00021,-0.00039,-1.5e-06,0,0,0.095,0.0016,0.0016,0.042,24,24,0.024,35,35,0.061,6.3e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00015,0.0013,0.0014,0.0016,0.0013,1,1,2 -7890000,0.98,-0.0065,-0.013,0.18,0.0046,0.014,-0.025,0.0013,0.017,-0.045,-0.0016,-0.0055,-9e-05,-0.00036,0.00036,-0.13,0.2,-2.6e-05,0.43,-0.00019,-0.0004,4.4e-05,0,0,0.095,0.0016,0.0016,0.042,24,24,0.023,36,36,0.06,6.3e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00013,0.0013,0.0014,0.0016,0.0013,1,1,2 -7990000,0.98,-0.0063,-0.013,0.18,0.0032,0.017,-0.022,0.00098,0.017,-0.042,-0.0016,-0.0056,-9.1e-05,-0.00036,0.00036,-0.13,0.2,-2.5e-05,0.43,-0.0002,-0.00042,7.4e-05,0,0,0.095,0.0017,0.0016,0.042,24,24,0.022,28,28,0.059,6.2e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00012,0.0013,0.0014,0.0016,0.0013,1,1,2 -8090000,0.98,-0.0063,-0.013,0.18,0.0043,0.019,-0.022,0.0013,0.019,-0.044,-0.0016,-0.0056,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-2.2e-05,0.43,-0.00017,-0.00042,9.9e-05,0,0,0.095,0.0017,0.0017,0.042,24,24,0.022,30,30,0.059,6.2e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,0.00011,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8190000,0.98,-0.0064,-0.012,0.18,0.0052,0.022,-0.018,0.0015,0.019,-0.038,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.44,-0.00014,-0.00041,0.00014,0,0,0.095,0.0018,0.0017,0.041,23,23,0.021,24,24,0.058,6.1e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0013,0.0001,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8290000,0.98,-0.0062,-0.012,0.18,0.002,0.028,-0.017,0.0003,0.022,-0.038,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.43,-0.00014,-0.00044,6.7e-05,0,0,0.095,0.0018,0.0017,0.041,23,23,0.02,27,27,0.057,6.1e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0013,9.6e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8390000,0.98,-0.0062,-0.012,0.18,-0.0023,0.028,-0.016,-0.00016,0.022,-0.036,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00044,2.1e-05,0,0,0.095,0.0019,0.0018,0.041,21,21,0.02,23,23,0.057,6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0013,9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 -8490000,0.98,-0.0059,-0.012,0.18,-0.0061,0.035,-0.017,-0.0015,0.026,-0.041,-0.0017,-0.0059,-9.6e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00048,-7.4e-06,0,0,0.095,0.0019,0.0018,0.041,21,21,0.019,25,25,0.056,5.9e-05,5.6e-05,2.2e-06,0.04,0.04,0.0011,0.0013,8.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8590000,0.98,-0.006,-0.012,0.18,-0.00034,0.034,-0.012,0.00033,0.026,-0.036,-0.0016,-0.0057,-9.6e-05,-0.00036,0.00036,-0.14,0.2,-1.7e-05,0.43,-0.00018,-0.00043,3.3e-06,0,0,0.095,0.0019,0.0018,0.041,19,19,0.018,22,22,0.055,5.8e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0013,7.9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8690000,0.98,-0.006,-0.012,0.18,-0.0022,0.037,-0.014,-0.00034,0.03,-0.038,-0.0016,-0.0058,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-1.6e-05,0.43,-0.00022,-0.00045,-8.7e-05,0,0,0.095,0.002,0.0018,0.041,19,19,0.018,24,24,0.055,5.8e-05,5.3e-05,2.2e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8790000,0.98,-0.006,-0.012,0.18,-0.005,0.039,-0.014,-0.0023,0.033,-0.035,-0.0016,-0.0058,-9.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00019,-0.00045,-0.00013,0,0,0.095,0.002,0.0018,0.041,19,19,0.018,27,27,0.055,5.7e-05,5.2e-05,2.2e-06,0.04,0.04,0.00099,0.0013,7.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 -8890000,0.98,-0.006,-0.012,0.18,0.00054,0.041,-0.0093,0.00054,0.033,-0.029,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00026,-0.00043,-0.00011,0,0,0.095,0.002,0.0018,0.041,17,17,0.017,24,24,0.054,5.6e-05,5e-05,2.2e-06,0.04,0.04,0.00094,0.0013,6.7e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -8990000,0.98,-0.0059,-0.013,0.18,0.01,0.047,-0.0085,0.0062,0.038,-0.032,-0.0017,-0.0056,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00037,-0.00039,-6.6e-05,0,0,0.095,0.0021,0.0018,0.041,17,17,0.017,27,27,0.054,5.5e-05,4.9e-05,2.2e-06,0.04,0.04,0.00091,0.0013,6.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -9090000,0.98,-0.0055,-0.012,0.18,-0.00032,0.056,-0.0095,0.00069,0.041,-0.032,-0.0018,-0.0057,-8.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00028,-0.00049,-8.8e-05,0,0,0.095,0.0021,0.0018,0.041,15,15,0.016,24,24,0.053,5.3e-05,4.7e-05,2.2e-06,0.04,0.04,0.00087,0.0013,6.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 -9190000,0.98,-0.0053,-0.013,0.18,0.0029,0.064,-0.0089,0.00089,0.049,-0.032,-0.0018,-0.0057,-8.6e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00031,-0.00053,-0.00011,0,0,0.095,0.0021,0.0018,0.041,15,15,0.016,27,27,0.052,5.2e-05,4.5e-05,2.2e-06,0.04,0.04,0.00083,0.0013,5.9e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.3 -9290000,0.98,-0.0053,-0.013,0.18,0.013,0.062,-0.0074,0.0055,0.046,-0.03,-0.0018,-0.0056,-8.4e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.0004,-0.00049,-8.5e-05,0,0,0.095,0.0021,0.0018,0.041,13,13,0.015,24,24,0.052,5.1e-05,4.4e-05,2.2e-06,0.04,0.04,0.00079,0.0013,5.6e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 -9390000,0.98,-0.0054,-0.013,0.18,0.014,0.06,-0.0062,0.0071,0.049,-0.03,-0.0017,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00035,-0.00043,-3.8e-05,0,0,0.095,0.0021,0.0018,0.041,13,13,0.015,26,26,0.052,5e-05,4.2e-05,2.2e-06,0.04,0.04,0.00077,0.0013,5.4e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 -9490000,0.98,-0.0053,-0.013,0.18,0.0087,0.061,-0.0045,0.0044,0.048,-0.027,-0.0018,-0.0056,-8.7e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00033,-0.00048,-7.6e-05,0,0,0.095,0.0021,0.0018,0.041,12,12,0.015,23,23,0.051,4.8e-05,4e-05,2.2e-06,0.04,0.04,0.00074,0.0013,5.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 -9590000,0.98,-0.0057,-0.013,0.18,0.0078,0.052,-0.0045,0.0046,0.048,-0.029,-0.0016,-0.0057,-9.3e-05,-0.00036,0.00036,-0.14,0.2,-1e-05,0.43,-0.00031,-0.00041,-0.00012,0,0,0.095,0.0022,0.0018,0.041,12,12,0.014,26,26,0.05,4.7e-05,3.9e-05,2.2e-06,0.04,0.04,0.00071,0.0013,5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 -9690000,0.98,-0.0059,-0.013,0.18,0.0096,0.047,-0.0016,0.0057,0.043,-0.027,-0.0016,-0.0056,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-9.2e-06,0.43,-0.00031,-0.00037,-9.9e-05,0,0,0.095,0.0022,0.0018,0.041,10,10,0.014,23,23,0.05,4.6e-05,3.7e-05,2.2e-06,0.04,0.04,0.00068,0.0013,4.8e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9790000,0.98,-0.0061,-0.012,0.18,0.001,0.041,-0.003,0.00039,0.042,-0.028,-0.0015,-0.0058,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.7e-06,0.43,-0.00018,-0.00036,-0.00015,0,0,0.095,0.0022,0.0017,0.041,10,10,0.014,25,25,0.05,4.4e-05,3.5e-05,2.2e-06,0.04,0.04,0.00066,0.0013,4.7e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9890000,0.98,-0.0061,-0.012,0.18,0.0086,0.04,-0.0016,0.0039,0.04,-0.029,-0.0015,-0.0057,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.6e-06,0.43,-0.00023,-0.00034,-9.4e-05,0,0,0.095,0.0022,0.0017,0.041,8.6,8.7,0.013,22,22,0.049,4.3e-05,3.4e-05,2.2e-06,0.04,0.04,0.00063,0.0013,4.5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -9990000,0.98,-0.0063,-0.012,0.18,0.0025,0.034,-0.00096,-0.0002,0.039,-0.031,-0.0014,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-6.4e-06,0.43,-0.00014,-0.00033,-0.00013,0,0,0.095,0.0022,0.0017,0.041,8.6,8.7,0.013,24,24,0.049,4.2e-05,3.2e-05,2.2e-06,0.04,0.04,0.00061,0.0013,4.4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 -10090000,0.98,-0.0067,-0.012,0.18,0.00073,0.019,0.00023,0.00018,0.03,-0.029,-0.0013,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-4.9e-06,0.43,-9.7e-05,-0.00025,-0.00015,0,0,0.095,0.0022,0.0017,0.041,7.4,7.5,0.013,21,21,0.048,4e-05,3.1e-05,2.2e-06,0.04,0.04,0.00059,0.0013,4.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10190000,0.98,-0.0072,-0.012,0.18,0.0053,0.0047,0.0011,0.0043,0.022,-0.03,-0.0011,-0.0058,-0.00012,-0.00036,0.00036,-0.14,0.2,-3.3e-06,0.43,-9.1e-05,-0.00012,-0.00015,0,0,0.095,0.0022,0.0016,0.041,7.4,7.6,0.012,23,23,0.048,3.9e-05,2.9e-05,2.2e-06,0.04,0.04,0.00057,0.0013,4.1e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10290000,0.98,-0.0071,-0.012,0.18,0.012,0.0085,3e-05,0.008,0.023,-0.029,-0.0012,-0.0057,-0.00011,-0.00036,0.00036,-0.14,0.2,-3.7e-06,0.43,-0.00016,-0.00011,-0.00012,0,0,0.095,0.0022,0.0016,0.041,6.4,6.5,0.012,20,20,0.048,3.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10390000,0.98,-0.0071,-0.012,0.18,0.0076,0.0026,-0.0025,0.00078,3e-05,-0.028,-0.0012,-0.0056,-0.00011,-0.0003,0.00041,-0.14,0.2,-3.9e-06,0.43,-0.0002,-0.00011,-6.5e-05,0,0,0.095,0.0022,0.0016,0.041,0.25,0.25,0.56,0.25,0.25,0.048,3.6e-05,2.7e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 -10490000,0.98,-0.0073,-0.012,0.18,0.0096,0.00082,0.007,0.0016,0.00011,-0.023,-0.0011,-0.0057,-0.00011,-0.00033,0.00027,-0.14,0.2,-3e-06,0.43,-0.00016,-5.9e-05,-8.7e-05,0,0,0.095,0.0021,0.0016,0.041,0.25,0.26,0.55,0.26,0.26,0.057,3.5e-05,2.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10590000,0.98,-0.007,-0.012,0.18,0.00028,-0.00036,0.013,-0.0011,-0.0055,-0.021,-0.0012,-0.0056,-0.00011,-0.0003,0.00025,-0.14,0.2,-3.6e-06,0.43,-0.00018,-9.8e-05,-7.3e-05,0,0,0.095,0.0021,0.0015,0.041,0.13,0.13,0.27,0.13,0.13,0.055,3.3e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10690000,0.98,-0.0071,-0.013,0.18,0.0031,-0.0029,0.016,-0.0009,-0.0057,-0.018,-0.0011,-0.0056,-0.00011,-0.00027,0.00018,-0.14,0.2,-3.3e-06,0.43,-0.00021,-4.9e-05,-7e-05,0,0,0.095,0.0021,0.0015,0.041,0.13,0.14,0.26,0.14,0.14,0.065,3.2e-05,2.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10790000,0.98,-0.0073,-0.013,0.18,0.0057,-0.0061,0.014,-0.00055,-0.005,-0.015,-0.0011,-0.0056,-0.00011,-0.00016,7.3e-05,-0.14,0.2,-3.1e-06,0.43,-0.00026,1.3e-05,-9e-05,0,0,0.095,0.002,0.0014,0.041,0.09,0.094,0.17,0.09,0.09,0.061,3e-05,2.1e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 -10890000,0.98,-0.0069,-0.013,0.18,0.008,-0.003,0.0099,0.00021,-0.0052,-0.018,-0.0012,-0.0055,-0.00011,-5.5e-05,0.0002,-0.14,0.2,-4.1e-06,0.43,-0.00033,-3e-05,-7.2e-05,0,0,0.095,0.002,0.0014,0.041,0.096,0.1,0.16,0.096,0.096,0.068,2.9e-05,2e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -10990000,0.98,-0.0069,-0.013,0.18,0.0054,0.0031,0.016,-0.00022,-0.0036,-0.012,-0.0012,-0.0056,-0.00011,-0.00021,0.00015,-0.14,0.2,-4.1e-06,0.43,-0.00026,-5.9e-05,-0.00012,0,0,0.095,0.0019,0.0014,0.041,0.074,0.081,0.12,0.072,0.072,0.065,2.6e-05,1.9e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11090000,0.98,-0.0075,-0.013,0.18,0.0093,0.0016,0.019,0.00071,-0.0039,-0.0075,-0.0011,-0.0056,-0.00011,-0.0003,-5.7e-05,-0.14,0.2,-2.9e-06,0.43,-0.00024,2.4e-05,-0.0001,0,0,0.095,0.0019,0.0013,0.041,0.081,0.092,0.11,0.078,0.078,0.069,2.6e-05,1.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11190000,0.98,-0.0077,-0.013,0.18,0.0082,0.0022,0.026,0.0013,-0.0032,-0.00047,-0.001,-0.0056,-0.00012,-0.00042,-1.2e-05,-0.14,0.2,-2.4e-06,0.43,-0.00017,-1.3e-06,-0.00012,0,0,0.095,0.0017,0.0013,0.04,0.066,0.075,0.084,0.062,0.062,0.066,2.2e-05,1.6e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 -11290000,0.98,-0.0077,-0.012,0.18,0.0065,-0.00037,0.025,0.0016,-0.0032,-0.00025,-0.001,-0.0057,-0.00012,-0.00052,0.00011,-0.14,0.2,-2.2e-06,0.43,-0.0001,-4e-05,-0.00015,0,0,0.095,0.0017,0.0012,0.04,0.074,0.088,0.078,0.067,0.068,0.069,2.2e-05,1.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11390000,0.98,-0.0076,-0.012,0.18,0.0045,0.00076,0.016,0.0009,-0.0025,-0.0086,-0.001,-0.0057,-0.00012,-0.00055,1.9e-05,-0.14,0.2,-2.3e-06,0.43,-7.8e-05,-5.1e-05,-0.00018,0,0,0.095,0.0015,0.0012,0.04,0.062,0.073,0.064,0.056,0.056,0.066,1.9e-05,1.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11490000,0.98,-0.0075,-0.012,0.18,0.0052,-0.00076,0.02,0.0016,-0.0025,-0.0026,-0.001,-0.0057,-0.00012,-0.00053,-6.4e-05,-0.14,0.2,-2.3e-06,0.43,-0.0001,-3e-05,-0.00017,0,0,0.095,0.0015,0.0011,0.04,0.07,0.086,0.058,0.061,0.062,0.067,1.8e-05,1.3e-05,2.2e-06,0.04,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11590000,0.98,-0.0076,-0.012,0.18,0.0047,-0.00067,0.018,0.00077,-0.002,-0.0037,-0.001,-0.0058,-0.00012,-0.00061,2.7e-05,-0.14,0.2,-2.3e-06,0.43,-5e-05,-7e-05,-0.00019,0,0,0.095,0.0014,0.0011,0.04,0.06,0.072,0.049,0.052,0.052,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 -11690000,0.98,-0.0075,-0.012,0.18,0.0041,0.0017,0.018,0.00093,-0.0018,-0.0051,-0.0011,-0.0058,-0.00012,-0.00057,0.00015,-0.14,0.2,-2.5e-06,0.43,-4.4e-05,-8.6e-05,-0.00022,0,0,0.095,0.0014,0.001,0.04,0.068,0.084,0.046,0.058,0.059,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11790000,0.98,-0.0075,-0.012,0.18,0.0028,0.0026,0.019,0.00044,-0.0024,-0.0021,-0.0011,-0.0058,-0.00012,-4.6e-05,0.00011,-0.14,0.2,-2.6e-06,0.43,-5.7e-05,-9.1e-05,-0.00022,0,0,0.095,0.0012,0.00096,0.039,0.058,0.07,0.039,0.049,0.05,0.062,1.2e-05,9.4e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11890000,0.98,-0.0077,-0.012,0.18,0.0039,0.0012,0.017,0.0003,-0.0026,-0.0015,-0.0011,-0.0058,-0.00012,-0.00025,0.00027,-0.14,0.2,-2.4e-06,0.43,-2.6e-05,-9.1e-05,-0.00026,0,0,0.095,0.0012,0.00095,0.039,0.066,0.082,0.036,0.056,0.057,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -11990000,0.98,-0.0077,-0.012,0.18,0.0067,0.0034,0.014,0.0014,-0.0027,-0.0051,-0.0011,-0.0058,-0.00012,-0.00032,0.00043,-0.14,0.2,-2.7e-06,0.43,-4.4e-05,-9.8e-05,-0.00026,0,0,0.095,0.0011,0.00088,0.039,0.056,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 -12090000,0.98,-0.0077,-0.012,0.18,0.0098,0.00099,0.017,0.0028,-0.0029,0.00085,-0.001,-0.0058,-0.00012,-0.00051,0.00017,-0.14,0.2,-2.4e-06,0.43,-4.3e-05,-6.6e-05,-0.00025,0,0,0.095,0.0011,0.00088,0.039,0.064,0.079,0.029,0.054,0.056,0.06,9.9e-06,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12190000,0.98,-0.0077,-0.012,0.18,0.011,-0.00018,0.016,0.0025,-0.0017,0.0027,-0.001,-0.0058,-0.00012,-0.0012,-0.00048,-0.14,0.2,-2.3e-06,0.43,-7.1e-06,-3.1e-05,-0.00028,0,0,0.095,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12290000,0.98,-0.0078,-0.012,0.18,0.0081,-0.0037,0.015,0.0032,-0.0024,0.0036,-0.001,-0.0058,-0.00013,-0.0015,-0.0003,-0.14,0.2,-2.1e-06,0.43,2e-05,-3.9e-05,-0.00028,0,0,0.095,0.00094,0.00081,0.039,0.062,0.075,0.024,0.053,0.055,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12390000,0.98,-0.0079,-0.012,0.18,0.008,-0.0049,0.013,0.0027,-0.0021,-0.0024,-0.001,-0.0058,-0.00013,-0.0018,-0.00078,-0.14,0.2,-2e-06,0.43,4.7e-05,-1.8e-05,-0.0003,0,0,0.095,0.00084,0.00075,0.039,0.053,0.062,0.022,0.046,0.047,0.056,6.8e-06,5.5e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 -12490000,0.98,-0.008,-0.012,0.18,0.0096,-0.0064,0.017,0.0041,-0.0032,-0.00046,-0.00099,-0.0058,-0.00013,-0.0021,-0.001,-0.14,0.2,-1.9e-06,0.43,5.5e-05,1.2e-05,-0.00031,0,0,0.095,0.00085,0.00074,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.7e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12590000,0.98,-0.008,-0.012,0.18,0.012,-0.01,0.018,0.0043,-0.0042,0.0013,-0.00098,-0.0058,-0.00013,-0.0023,-0.0009,-0.14,0.2,-1.8e-06,0.43,6.9e-05,1.2e-05,-0.00032,0,0,0.095,0.00077,0.00069,0.039,0.051,0.059,0.019,0.046,0.047,0.054,5.7e-06,4.7e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12690000,0.98,-0.008,-0.012,0.18,0.012,-0.014,0.018,0.0054,-0.0056,0.0028,-0.00097,-0.0058,-0.00013,-0.0024,-0.00085,-0.14,0.2,-1.8e-06,0.43,7.9e-05,1.7e-05,-0.00033,0,0,0.095,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.054,0.054,5.6e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.019,0.0054,-0.0062,0.005,-0.00098,-0.0058,-0.00013,-0.0018,-0.0013,-0.14,0.2,-1.8e-06,0.43,4.4e-05,4e-05,-0.00031,0,0,0.095,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 -12890000,0.98,-0.008,-0.012,0.18,0.016,-0.013,0.021,0.0071,-0.007,0.0079,-0.00099,-0.0058,-0.00012,-0.0014,-0.0015,-0.14,0.2,-1.9e-06,0.43,2.4e-05,4.5e-05,-0.00029,0,0,0.095,0.0007,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0.005,-0.0047,0.0091,-0.001,-0.0058,-0.00012,-0.0013,-0.0016,-0.14,0.2,-2.4e-06,0.43,3.8e-05,2e-05,-0.00032,0,0,0.095,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0.0067,-0.0048,0.0078,-0.001,-0.0058,-0.00012,-0.00076,-0.0018,-0.14,0.2,-2.4e-06,0.43,1.5e-05,2.5e-05,-0.0003,0,0,0.095,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.4e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13190000,0.98,-0.0079,-0.012,0.18,0.0097,-0.0078,0.017,0.003,-0.0048,0.0084,-0.0011,-0.0058,-0.00012,-0.00054,-0.0023,-0.14,0.2,-2.6e-06,0.43,3.9e-05,2.3e-05,-0.00031,0,0,0.095,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 -13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0.0046,-0.0059,0.0076,-0.001,-0.0058,-0.00012,-0.00076,-0.0028,-0.14,0.2,-2.4e-06,0.43,4.8e-05,4.4e-05,-0.00031,0,0,0.095,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.5e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13390000,0.98,-0.0079,-0.012,0.18,0.0091,-0.0081,0.014,0.0032,-0.0049,0.0081,-0.0011,-0.0058,-0.00012,-0.0014,-0.0028,-0.14,0.2,-2.7e-06,0.43,8.6e-05,4.1e-05,-0.00035,0,0,0.095,0.00057,0.00055,0.038,0.042,0.047,0.012,0.045,0.046,0.047,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13490000,0.98,-0.0079,-0.012,0.18,0.011,-0.0068,0.014,0.0048,-0.0056,0.0041,-0.001,-0.0058,-0.00012,-0.0015,-0.0033,-0.14,0.2,-2.6e-06,0.43,8.9e-05,6.4e-05,-0.00035,0,0,0.095,0.00057,0.00055,0.038,0.047,0.052,0.011,0.052,0.053,0.046,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13590000,0.98,-0.0079,-0.012,0.18,0.014,-0.0071,0.015,0.0065,-0.0048,0.0025,-0.001,-0.0058,-0.00013,-0.002,-0.0034,-0.14,0.2,-2.6e-06,0.43,9.8e-05,5.7e-05,-0.00035,0,0,0.095,0.00054,0.00053,0.038,0.04,0.044,0.011,0.045,0.046,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 -13690000,0.98,-0.0078,-0.012,0.18,0.014,-0.0082,0.015,0.0077,-0.0052,0.0051,-0.0011,-0.0058,-0.00012,-0.0017,-0.0031,-0.14,0.2,-2.6e-06,0.43,8.5e-05,3.7e-05,-0.00033,0,0,0.095,0.00054,0.00053,0.038,0.044,0.049,0.011,0.052,0.053,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13790000,0.98,-0.0077,-0.012,0.18,0.019,-0.0044,0.015,0.0098,-0.0027,0.0044,-0.0011,-0.0058,-0.00013,-0.0022,-0.0026,-0.14,0.2,-2.8e-06,0.43,8.3e-05,1.5e-05,-0.00034,0,0,0.095,0.00052,0.00051,0.038,0.038,0.042,0.01,0.045,0.046,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13890000,0.98,-0.0076,-0.012,0.18,0.019,-0.0029,0.016,0.011,-0.0025,0.0066,-0.0011,-0.0058,-0.00012,-0.0015,-0.002,-0.14,0.2,-3e-06,0.43,6.2e-05,1.1e-07,-0.00034,0,0,0.095,0.00052,0.00051,0.038,0.042,0.046,0.01,0.051,0.053,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -13990000,0.98,-0.0076,-0.012,0.18,0.019,-0.001,0.014,0.0088,-0.003,0.0053,-0.0011,-0.0058,-0.00012,-0.001,-0.0025,-0.14,0.2,-2.9e-06,0.43,7e-05,-1.1e-06,-0.00031,0,0,0.095,0.0005,0.0005,0.038,0.036,0.039,0.0097,0.045,0.046,0.043,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 -14090000,0.98,-0.0077,-0.011,0.18,0.017,-0.0082,0.015,0.011,-0.0049,0.0015,-0.0011,-0.0058,-0.00013,-0.0028,-0.0023,-0.14,0.2,-2.8e-06,0.43,0.00012,6.9e-06,-0.00035,0,0,0.095,0.0005,0.0005,0.038,0.04,0.043,0.0096,0.051,0.053,0.042,2.2e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14190000,0.98,-0.0078,-0.011,0.18,0.016,-0.0075,0.015,0.01,-0.0045,0.0015,-0.001,-0.0058,-0.00014,-0.0039,-0.0034,-0.14,0.2,-2.6e-06,0.43,0.00018,3.2e-05,-0.00037,0,0,0.095,0.00048,0.00048,0.038,0.034,0.037,0.0094,0.045,0.046,0.042,2.1e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14290000,0.98,-0.0077,-0.011,0.18,0.018,-0.0081,0.013,0.012,-0.0054,0.0057,-0.001,-0.0058,-0.00014,-0.004,-0.0036,-0.14,0.2,-2.6e-06,0.43,0.00018,3.6e-05,-0.00036,0,0,0.095,0.00048,0.00048,0.038,0.038,0.041,0.0092,0.051,0.052,0.041,2e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14390000,0.98,-0.0078,-0.011,0.18,0.018,-0.0095,0.015,0.011,-0.0055,0.01,-0.001,-0.0058,-0.00014,-0.0038,-0.0044,-0.14,0.2,-2.4e-06,0.43,0.00019,4.1e-05,-0.00035,0,0,0.095,0.00047,0.00047,0.038,0.033,0.035,0.009,0.045,0.046,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 -14490000,0.98,-0.008,-0.011,0.18,0.018,-0.011,0.019,0.013,-0.0073,0.012,-0.001,-0.0058,-0.00014,-0.0049,-0.0045,-0.14,0.2,-2.3e-06,0.43,0.00022,5e-05,-0.00037,0,0,0.095,0.00047,0.00047,0.038,0.036,0.038,0.009,0.051,0.052,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14590000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0.0099,-0.0073,0.008,-0.001,-0.0058,-0.00015,-0.0054,-0.0058,-0.14,0.2,-2.1e-06,0.43,0.00027,6.9e-05,-0.00037,0,0,0.095,0.00045,0.00046,0.038,0.031,0.033,0.0088,0.045,0.045,0.04,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14690000,0.98,-0.0081,-0.011,0.18,0.017,-0.01,0.016,0.012,-0.0083,0.0081,-0.001,-0.0058,-0.00014,-0.0053,-0.0062,-0.14,0.2,-2e-06,0.43,0.00027,7.5e-05,-0.00037,0,0,0.095,0.00045,0.00046,0.038,0.034,0.036,0.0088,0.05,0.051,0.039,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14790000,0.98,-0.0082,-0.011,0.18,0.019,-0.0034,0.016,0.01,-0.0022,0.011,-0.0011,-0.0058,-0.00014,-0.0047,-0.0076,-0.14,0.2,-2.2e-06,0.43,0.00027,7.3e-05,-0.00035,0,0,0.095,0.00044,0.00045,0.038,0.03,0.031,0.0086,0.044,0.045,0.039,1.7e-06,1.5e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 -14890000,0.98,-0.0082,-0.011,0.18,0.021,-0.0055,0.02,0.013,-0.0023,0.012,-0.0011,-0.0058,-0.00013,-0.0042,-0.0093,-0.14,0.2,-1.8e-06,0.43,0.00027,9.5e-05,-0.00031,0,0,0.095,0.00044,0.00045,0.038,0.032,0.034,0.0087,0.05,0.051,0.039,1.6e-06,1.5e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -14990000,0.98,-0.0083,-0.011,0.18,0.02,-0.0057,0.023,0.012,-0.0029,0.014,-0.0011,-0.0058,-0.00013,-0.0041,-0.011,-0.14,0.2,-1.5e-06,0.43,0.0003,0.00012,-0.00029,0,0,0.095,0.00043,0.00045,0.038,0.028,0.03,0.0085,0.044,0.045,0.038,1.6e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15090000,0.98,-0.0083,-0.011,0.18,0.021,-0.0049,0.026,0.014,-0.0036,0.016,-0.0011,-0.0058,-0.00013,-0.0042,-0.011,-0.14,0.2,-1.5e-06,0.43,0.0003,0.00013,-0.00029,0,0,0.095,0.00043,0.00045,0.038,0.03,0.032,0.0085,0.05,0.051,0.038,1.5e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15190000,0.98,-0.0085,-0.011,0.18,0.019,-0.0059,0.027,0.012,-0.0032,0.017,-0.0011,-0.0058,-0.00014,-0.0048,-0.012,-0.14,0.2,-1.4e-06,0.43,0.00034,0.00014,-0.00029,0,0,0.095,0.00043,0.00044,0.038,0.027,0.028,0.0085,0.044,0.045,0.038,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 -15290000,0.98,-0.0086,-0.012,0.18,0.022,-0.0065,0.026,0.015,-0.0034,0.014,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.2e-06,0.43,0.00033,0.00017,-0.00027,0,0,0.095,0.00043,0.00044,0.038,0.029,0.031,0.0085,0.049,0.05,0.037,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15390000,0.98,-0.0088,-0.011,0.18,0.021,-0.0039,0.025,0.012,-0.0028,0.014,-0.0011,-0.0058,-0.00013,-0.0045,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,0.095,0.00042,0.00043,0.038,0.025,0.027,0.0084,0.044,0.044,0.037,1.4e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15490000,0.98,-0.0088,-0.011,0.18,0.022,-0.0067,0.025,0.014,-0.0034,0.015,-0.0011,-0.0058,-0.00013,-0.0046,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,0.095,0.00042,0.00043,0.038,0.028,0.029,0.0085,0.049,0.05,0.037,1.4e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15590000,0.98,-0.0088,-0.011,0.18,0.023,-0.0081,0.025,0.014,-0.0059,0.013,-0.0011,-0.0058,-0.00013,-0.0042,-0.014,-0.14,0.2,-1.3e-06,0.43,0.00036,0.00019,-0.00028,0,0,0.095,0.00041,0.00043,0.038,0.024,0.026,0.0084,0.043,0.044,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 -15690000,0.98,-0.0086,-0.011,0.18,0.024,-0.01,0.025,0.015,-0.0063,0.014,-0.0011,-0.0058,-0.00012,-0.0032,-0.012,-0.14,0.2,-1.8e-06,0.43,0.00031,0.00013,-0.00026,0,0,0.095,0.00041,0.00043,0.038,0.026,0.028,0.0085,0.049,0.049,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15790000,0.98,-0.0086,-0.011,0.18,0.019,-0.0094,0.025,0.012,-0.0047,0.015,-0.0011,-0.0058,-0.00012,-0.0021,-0.012,-0.14,0.2,-2.2e-06,0.43,0.0003,0.00012,-0.00026,0,0,0.095,0.00041,0.00042,0.038,0.023,0.025,0.0084,0.043,0.044,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15890000,0.98,-0.0087,-0.011,0.18,0.02,-0.0085,0.025,0.015,-0.0061,0.014,-0.0011,-0.0058,-0.00012,-0.0031,-0.012,-0.14,0.2,-1.9e-06,0.43,0.00034,0.00014,-0.00027,0,0,0.095,0.00041,0.00042,0.038,0.025,0.026,0.0085,0.048,0.049,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -15990000,0.98,-0.0086,-0.011,0.18,0.018,-0.0074,0.022,0.013,-0.0051,0.013,-0.0011,-0.0058,-0.00012,-0.0034,-0.014,-0.14,0.2,-2.1e-06,0.43,0.00038,0.00019,-0.00029,0,0,0.095,0.0004,0.00042,0.038,0.022,0.023,0.0084,0.043,0.043,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 -16090000,0.98,-0.0087,-0.011,0.18,0.019,-0.0092,0.02,0.015,-0.0063,0.013,-0.0011,-0.0058,-0.00013,-0.004,-0.014,-0.14,0.2,-2e-06,0.43,0.00041,0.00021,-0.00031,0,0,0.095,0.0004,0.00042,0.038,0.024,0.025,0.0085,0.048,0.049,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16190000,0.98,-0.0086,-0.011,0.18,0.015,-0.0076,0.018,0.011,-0.0054,0.0094,-0.0011,-0.0058,-0.00013,-0.0047,-0.015,-0.13,0.2,-2e-06,0.43,0.00044,0.00021,-0.00033,0,0,0.095,0.0004,0.00041,0.038,0.021,0.022,0.0084,0.043,0.043,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16290000,0.98,-0.0086,-0.011,0.18,0.017,-0.009,0.018,0.013,-0.0061,0.011,-0.0011,-0.0058,-0.00013,-0.0044,-0.014,-0.13,0.2,-2e-06,0.43,0.00043,0.0002,-0.00032,0,0,0.095,0.0004,0.00041,0.038,0.023,0.024,0.0085,0.047,0.048,0.036,1.2e-06,1e-06,2e-06,0.035,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16390000,0.98,-0.0087,-0.011,0.18,0.019,-0.0081,0.018,0.013,-0.0048,0.011,-0.0011,-0.0058,-0.00013,-0.004,-0.017,-0.13,0.2,-1.7e-06,0.43,0.00045,0.00024,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.021,0.022,0.0084,0.042,0.043,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 -16490000,0.98,-0.0089,-0.011,0.18,0.022,-0.01,0.021,0.015,-0.0059,0.015,-0.0011,-0.0058,-0.00013,-0.0044,-0.017,-0.13,0.2,-1.5e-06,0.43,0.00046,0.00025,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.022,0.023,0.0085,0.047,0.048,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16590000,0.98,-0.0089,-0.011,0.18,0.026,-0.01,0.024,0.013,-0.0049,0.015,-0.0011,-0.0058,-0.00013,-0.0043,-0.018,-0.13,0.2,-1.7e-06,0.43,0.00048,0.00025,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.02,0.021,0.0084,0.042,0.042,0.036,1.1e-06,9.7e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16690000,0.98,-0.0089,-0.011,0.18,0.028,-0.015,0.024,0.016,-0.0059,0.015,-0.0011,-0.0058,-0.00013,-0.0037,-0.017,-0.13,0.2,-1.9e-06,0.43,0.00046,0.00024,-0.0003,0,0,0.095,0.00039,0.00041,0.038,0.021,0.022,0.0085,0.047,0.047,0.036,1.1e-06,9.6e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16790000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.023,0.013,-0.0048,0.015,-0.0011,-0.0058,-0.00012,-0.0031,-0.017,-0.13,0.2,-2.2e-06,0.43,0.00046,0.00024,-0.00029,0,0,0.095,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.042,0.042,0.036,1e-06,9.3e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 -16890000,0.98,-0.0087,-0.011,0.18,0.028,-0.014,0.024,0.016,-0.0056,0.013,-0.0011,-0.0058,-0.00012,-0.0021,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00043,0.00023,-0.00028,0,0,0.095,0.00038,0.0004,0.038,0.02,0.022,0.0085,0.046,0.047,0.036,1e-06,9.2e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -16990000,0.98,-0.0087,-0.011,0.18,0.026,-0.014,0.024,0.014,-0.0046,0.012,-0.0011,-0.0058,-0.00011,-0.0017,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00044,0.00025,-0.00028,0,0,0.095,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,1e-06,9e-07,2e-06,0.035,0.032,0.00042,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17090000,0.98,-0.0089,-0.011,0.18,0.03,-0.017,0.023,0.018,-0.0063,0.011,-0.0011,-0.0058,-0.00012,-0.0022,-0.019,-0.13,0.2,-2.4e-06,0.43,0.00048,0.00032,-0.00029,0,0,0.095,0.00038,0.0004,0.038,0.02,0.021,0.0085,0.046,0.046,0.035,9.9e-07,8.9e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17190000,0.98,-0.009,-0.011,0.18,0.028,-0.021,0.025,0.014,-0.0096,0.014,-0.0011,-0.0058,-0.00012,-0.0031,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00052,0.00033,-0.00033,0,0,0.095,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,9.6e-07,8.7e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 -17290000,0.98,-0.009,-0.011,0.18,0.031,-0.023,0.025,0.017,-0.012,0.014,-0.0011,-0.0058,-0.00013,-0.0038,-0.019,-0.13,0.2,-2.9e-06,0.43,0.00055,0.00034,-0.00035,0,0,0.095,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.045,0.046,0.036,9.6e-07,8.6e-07,2e-06,0.035,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17390000,0.98,-0.0089,-0.011,0.18,0.025,-0.023,0.024,0.016,-0.009,0.013,-0.0011,-0.0058,-0.00012,-0.0025,-0.018,-0.13,0.2,-3.2e-06,0.43,0.00053,0.00034,-0.00034,0,0,0.095,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.041,0.041,0.035,9.3e-07,8.4e-07,2e-06,0.034,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17490000,0.98,-0.009,-0.011,0.18,0.024,-0.025,0.024,0.019,-0.012,0.015,-0.0011,-0.0058,-0.00013,-0.0028,-0.019,-0.13,0.2,-3.2e-06,0.43,0.00054,0.00034,-0.00035,0,0,0.095,0.00037,0.00039,0.038,0.018,0.019,0.0085,0.045,0.046,0.036,9.3e-07,8.3e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17590000,0.98,-0.0089,-0.011,0.18,0.021,-0.022,0.023,0.014,-0.0098,0.013,-0.0011,-0.0058,-0.00013,-0.0023,-0.019,-0.13,0.2,-3.8e-06,0.43,0.00055,0.00034,-0.00035,0,0,0.095,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.04,0.041,0.035,9e-07,8.1e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 -17690000,0.98,-0.009,-0.011,0.18,0.022,-0.023,0.024,0.016,-0.012,0.015,-0.0011,-0.0058,-0.00013,-0.002,-0.019,-0.13,0.2,-3.5e-06,0.43,0.00053,0.00032,-0.00033,0,0,0.095,0.00037,0.00039,0.038,0.018,0.019,0.0084,0.045,0.045,0.035,9e-07,8e-07,2e-06,0.034,0.031,0.00038,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17790000,0.98,-0.0091,-0.011,0.18,0.023,-0.022,0.025,0.015,-0.011,0.02,-0.0011,-0.0058,-0.00013,-0.0013,-0.019,-0.13,0.2,-3.5e-06,0.43,0.0005,0.00029,-0.00031,0,0,0.095,0.00037,0.00039,0.038,0.016,0.017,0.0084,0.04,0.041,0.036,8.8e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17890000,0.98,-0.0089,-0.011,0.18,0.026,-0.024,0.025,0.017,-0.013,0.025,-0.0011,-0.0058,-0.00012,-0.00072,-0.018,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.0003,0,0,0.095,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.044,0.045,0.036,8.7e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -17990000,0.98,-0.0089,-0.011,0.18,0.025,-0.02,0.024,0.016,-0.0083,0.026,-0.0012,-0.0058,-0.00012,9.1e-05,-0.02,-0.13,0.2,-3.5e-06,0.43,0.00048,0.00027,-0.00029,0,0,0.095,0.00036,0.00038,0.038,0.016,0.017,0.0083,0.04,0.04,0.035,8.5e-07,7.6e-07,2e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 -18090000,0.98,-0.009,-0.011,0.18,0.026,-0.021,0.024,0.018,-0.01,0.024,-0.0012,-0.0058,-0.00012,0.00014,-0.02,-0.13,0.2,-3.3e-06,0.43,0.00047,0.00027,-0.00028,0,0,0.095,0.00036,0.00038,0.038,0.017,0.018,0.0083,0.044,0.045,0.036,8.5e-07,7.5e-07,1.9e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18190000,0.98,-0.0091,-0.011,0.18,0.025,-0.019,0.024,0.017,-0.0083,0.022,-0.0012,-0.0058,-0.00011,0.0017,-0.021,-0.13,0.2,-3.6e-06,0.43,0.00045,0.00029,-0.00025,0,0,0.095,0.00036,0.00038,0.038,0.015,0.016,0.0082,0.04,0.04,0.035,8.2e-07,7.4e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18290000,0.98,-0.0092,-0.011,0.18,0.028,-0.02,0.023,0.02,-0.01,0.02,-0.0012,-0.0058,-0.00011,0.0019,-0.022,-0.13,0.2,-3.4e-06,0.43,0.00045,0.0003,-0.00024,0,0,0.095,0.00036,0.00038,0.038,0.016,0.018,0.0082,0.044,0.044,0.036,8.2e-07,7.3e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18390000,0.98,-0.0091,-0.011,0.18,0.027,-0.019,0.023,0.019,-0.0089,0.019,-0.0012,-0.0058,-0.00011,0.0021,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00046,0.00031,-0.00025,0,0,0.095,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 -18490000,0.98,-0.0091,-0.011,0.18,0.031,-0.019,0.022,0.022,-0.01,0.021,-0.0012,-0.0058,-0.00011,0.0027,-0.022,-0.13,0.2,-3.7e-06,0.43,0.00044,0.00031,-0.00024,0,0,0.095,0.00036,0.00038,0.038,0.016,0.017,0.0082,0.043,0.044,0.036,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18590000,0.98,-0.0089,-0.011,0.18,0.028,-0.019,0.022,0.019,-0.0093,0.024,-0.0012,-0.0058,-0.00011,0.003,-0.021,-0.13,0.2,-4.2e-06,0.43,0.00044,0.00028,-0.00024,0,0,0.095,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,7.8e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18690000,0.98,-0.0088,-0.011,0.18,0.028,-0.018,0.021,0.021,-0.011,0.022,-0.0012,-0.0058,-0.00011,0.0039,-0.02,-0.13,0.2,-4.2e-06,0.43,0.0004,0.00025,-0.00022,0,0,0.095,0.00036,0.00038,0.038,0.016,0.017,0.0081,0.043,0.044,0.035,7.7e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18790000,0.98,-0.0087,-0.011,0.18,0.026,-0.017,0.02,0.019,-0.0093,0.02,-0.0012,-0.0058,-0.00011,0.0044,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00041,0.00026,-0.00023,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.008,0.039,0.04,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00032,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 -18890000,0.98,-0.0087,-0.011,0.18,0.027,-0.018,0.018,0.022,-0.011,0.016,-0.0012,-0.0058,-0.00011,0.0042,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00042,0.00028,-0.00024,0,0,0.095,0.00035,0.00037,0.038,0.015,0.017,0.008,0.043,0.043,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -18990000,0.98,-0.0087,-0.011,0.18,0.024,-0.018,0.019,0.019,-0.0097,0.02,-0.0012,-0.0058,-0.00011,0.005,-0.019,-0.13,0.2,-5.3e-06,0.43,0.00042,0.00027,-0.00024,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19090000,0.98,-0.0087,-0.011,0.18,0.023,-0.018,0.02,0.022,-0.011,0.016,-0.0012,-0.0058,-0.0001,0.0055,-0.019,-0.13,0.2,-5.2e-06,0.43,0.00041,0.00027,-0.00023,0,0,0.095,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19190000,0.98,-0.0086,-0.011,0.18,0.02,-0.018,0.02,0.019,-0.01,0.016,-0.0012,-0.0058,-0.00011,0.0053,-0.019,-0.13,0.2,-6e-06,0.43,0.00043,0.00028,-0.00026,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 -19290000,0.98,-0.0086,-0.011,0.18,0.021,-0.019,0.02,0.021,-0.012,0.015,-0.0012,-0.0058,-0.00011,0.0053,-0.019,-0.13,0.2,-6e-06,0.43,0.00043,0.00029,-0.00026,0,0,0.095,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.016,0.022,0.018,-0.0097,0.014,-0.0012,-0.0058,-0.00011,0.0059,-0.019,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00028,-0.00026,0,0,0.095,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19490000,0.98,-0.0088,-0.011,0.18,0.021,-0.017,0.021,0.021,-0.012,0.014,-0.0012,-0.0058,-0.00012,0.0053,-0.02,-0.13,0.2,-6e-06,0.43,0.00043,0.00026,-0.00025,0,0,0.095,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19590000,0.98,-0.0087,-0.011,0.18,0.018,-0.019,0.023,0.019,-0.011,0.014,-0.0012,-0.0058,-0.00012,0.0055,-0.02,-0.13,0.2,-6.3e-06,0.43,0.00043,0.00026,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 -19690000,0.98,-0.0088,-0.011,0.18,0.018,-0.017,0.022,0.022,-0.013,0.014,-0.0012,-0.0058,-0.00011,0.006,-0.021,-0.13,0.2,-6e-06,0.43,0.00042,0.00026,-0.00024,0,0,0.095,0.00034,0.00036,0.038,0.014,0.016,0.0076,0.042,0.042,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19790000,0.98,-0.0089,-0.011,0.18,0.016,-0.016,0.02,0.021,-0.011,0.01,-0.0012,-0.0058,-0.00012,0.0061,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.013,0.014,0.0076,0.038,0.039,0.035,6.6e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19890000,0.98,-0.0089,-0.011,0.18,0.017,-0.016,0.021,0.023,-0.013,0.0089,-0.0012,-0.0058,-0.00012,0.006,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00027,-0.00026,0,0,0.095,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.5e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -19990000,0.98,-0.0089,-0.012,0.18,0.015,-0.016,0.018,0.02,-0.01,0.006,-0.0012,-0.0058,-0.00012,0.007,-0.021,-0.13,0.2,-7e-06,0.43,0.00042,0.00027,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.038,0.038,0.035,6.4e-07,5.7e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 -20090000,0.98,-0.0089,-0.012,0.18,0.017,-0.019,0.019,0.022,-0.012,0.0095,-0.0012,-0.0058,-0.00012,0.0071,-0.021,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00028,-0.00025,0,0,0.095,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.4e-07,5.6e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5.1 -20190000,0.98,-0.0089,-0.012,0.18,0.017,-0.016,0.02,0.022,-0.011,0.0094,-0.0012,-0.0058,-0.00013,0.0071,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00042,0.00029,-0.00027,0,0,0.11,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.037,0.038,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20290000,0.98,-0.009,-0.012,0.18,0.015,-0.018,0.02,0.023,-0.012,0.01,-0.0012,-0.0058,-0.00012,0.0071,-0.022,-0.13,0.2,-7.5e-06,0.43,0.00042,0.0003,-0.00027,0,0,0.11,0.00034,0.00036,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20390000,0.98,-0.0089,-0.012,0.18,0.013,-0.016,0.02,0.022,-0.011,0.012,-0.0012,-0.0058,-0.00012,0.0081,-0.022,-0.13,0.2,-7.6e-06,0.43,0.00041,0.00028,-0.00026,0,0,0.11,0.00033,0.00035,0.038,0.013,0.014,0.0073,0.037,0.038,0.035,6.1e-07,5.4e-07,1.8e-06,0.031,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20490000,0.98,-0.0089,-0.012,0.18,0.0096,-0.017,0.02,0.023,-0.012,0.0099,-0.0012,-0.0058,-0.00012,0.0082,-0.022,-0.13,0.2,-7.4e-06,0.43,0.00041,0.00027,-0.00026,0,0,0.11,0.00034,0.00035,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6e-07,5.3e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20590000,0.98,-0.0088,-0.012,0.18,0.0088,-0.017,0.02,0.022,-0.01,0.0085,-0.0013,-0.0058,-0.00011,0.0094,-0.022,-0.13,0.2,-7.3e-06,0.43,0.00039,0.00025,-0.00025,0,0,0.11,0.00033,0.00035,0.038,0.013,0.014,0.0072,0.037,0.038,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20690000,0.98,-0.0087,-0.012,0.18,0.0077,-0.017,0.021,0.022,-0.012,0.0092,-0.0013,-0.0058,-0.00012,0.0092,-0.022,-0.13,0.2,-7.5e-06,0.43,0.00039,0.00025,-0.00025,0,0,0.11,0.00033,0.00035,0.038,0.013,0.015,0.0072,0.04,0.042,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20790000,0.98,-0.0081,-0.012,0.18,0.0038,-0.013,0.0061,0.019,-0.01,0.008,-0.0013,-0.0058,-0.00011,0.01,-0.022,-0.13,0.2,-7.9e-06,0.43,0.00038,0.00025,-0.00025,0,0,0.11,0.00033,0.00035,0.038,0.013,0.014,0.0071,0.037,0.038,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20890000,0.98,0.00098,-0.0077,0.18,-4.9e-05,-0.002,-0.11,0.02,-0.011,0.002,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00037,-0.00019,0,0,0.1,0.00033,0.00035,0.038,0.013,0.015,0.0071,0.04,0.041,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 -20990000,0.98,0.0044,-0.0043,0.18,-0.012,0.017,-0.25,0.017,-0.0086,-0.013,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-8.7e-06,0.43,0.00037,0.00036,-0.00015,0,0,0.087,0.00033,0.00035,0.038,0.013,0.014,0.007,0.037,0.038,0.034,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21090000,0.98,0.0027,-0.0047,0.18,-0.024,0.032,-0.37,0.016,-0.006,-0.043,-0.0013,-0.0058,-0.00011,0.01,-0.023,-0.13,0.2,-7e-06,0.43,0.00042,0.00016,-0.00019,0,0,0.057,0.00033,0.00035,0.038,0.014,0.015,0.007,0.04,0.041,0.035,5.6e-07,4.9e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21190000,0.98,-6.1e-05,-0.0063,0.18,-0.029,0.039,-0.5,0.013,-0.0039,-0.08,-0.0013,-0.0058,-0.0001,0.01,-0.024,-0.13,0.2,-6.3e-06,0.43,0.00043,0.00019,-0.00018,0,0,0.02,0.00033,0.00034,0.038,0.013,0.014,0.0069,0.037,0.038,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21290000,0.98,-0.0023,-0.0076,0.18,-0.029,0.041,-0.63,0.0099,-0.0003,-0.14,-0.0013,-0.0058,-0.00011,0.0096,-0.024,-0.13,0.2,-6.9e-06,0.43,0.00042,0.00023,-0.00011,0,0,-0.038,0.00033,0.00034,0.037,0.014,0.015,0.0069,0.04,0.041,0.034,5.4e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21390000,0.98,-0.0037,-0.0082,0.18,-0.026,0.038,-0.75,0.0082,0.0037,-0.2,-0.0013,-0.0058,-0.00011,0.0097,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00044,0.00015,-0.00012,0,0,-0.1,0.00032,0.00034,0.037,0.013,0.014,0.0068,0.037,0.038,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21490000,0.98,-0.0045,-0.0086,0.18,-0.021,0.035,-0.89,0.0054,0.0077,-0.29,-0.0013,-0.0058,-0.0001,0.01,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00043,0.00018,-0.00017,0,0,-0.19,0.00032,0.00034,0.037,0.014,0.016,0.0068,0.04,0.041,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21590000,0.98,-0.0049,-0.0086,0.18,-0.015,0.032,-1,0.0044,0.0099,-0.38,-0.0013,-0.0058,-9.5e-05,0.011,-0.024,-0.13,0.2,-6.5e-06,0.43,0.00042,0.00023,-0.00018,0,0,-0.28,0.00032,0.00034,0.037,0.013,0.015,0.0067,0.037,0.038,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21690000,0.98,-0.0052,-0.0085,0.18,-0.012,0.027,-1.1,0.0032,0.013,-0.49,-0.0013,-0.0058,-8.8e-05,0.011,-0.024,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00027,-0.00016,0,0,-0.39,0.00032,0.00034,0.037,0.014,0.016,0.0067,0.04,0.041,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21790000,0.98,-0.0055,-0.0086,0.18,-0.0065,0.022,-1.3,0.007,0.012,-0.61,-0.0013,-0.0058,-7.8e-05,0.012,-0.023,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00027,-0.00022,0,0,-0.51,0.00032,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21890000,0.98,-0.0058,-0.0088,0.18,-0.0029,0.017,-1.4,0.0062,0.014,-0.75,-0.0013,-0.0058,-8.1e-05,0.012,-0.023,-0.13,0.2,-6.8e-06,0.43,0.00036,0.00029,-0.00021,0,0,-0.65,0.00032,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -21990000,0.98,-0.0064,-0.009,0.18,-0.00083,0.013,-1.4,0.01,0.011,-0.89,-0.0013,-0.0058,-7.5e-05,0.012,-0.022,-0.13,0.2,-6.5e-06,0.43,0.00039,0.0003,-0.00023,0,0,-0.79,0.00031,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22090000,0.98,-0.0071,-0.0099,0.18,0.0012,0.0095,-1.4,0.0095,0.013,-1,-0.0013,-0.0058,-6.6e-05,0.012,-0.022,-0.13,0.2,-6.4e-06,0.43,0.00039,0.00031,-0.00024,0,0,-0.93,0.00031,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22190000,0.98,-0.0075,-0.01,0.18,0.007,0.0051,-1.4,0.015,0.008,-1.2,-0.0013,-0.0058,-5.8e-05,0.013,-0.02,-0.13,0.2,-6.6e-06,0.43,0.00039,0.00033,-0.00024,0,0,-1.1,0.00031,0.00032,0.037,0.013,0.014,0.0065,0.037,0.038,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22290000,0.98,-0.0082,-0.01,0.18,0.013,-0.00054,-1.4,0.016,0.0081,-1.3,-0.0013,-0.0058,-6e-05,0.013,-0.021,-0.13,0.2,-6.5e-06,0.43,0.00039,0.00033,-0.00023,0,0,-1.2,0.00031,0.00032,0.037,0.014,0.015,0.0065,0.04,0.041,0.034,4.8e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22390000,0.98,-0.0085,-0.01,0.18,0.017,-0.0091,-1.4,0.022,-0.00072,-1.5,-0.0013,-0.0058,-6.3e-05,0.012,-0.02,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00033,-0.00026,0,0,-1.4,0.0003,0.00032,0.037,0.013,0.014,0.0064,0.037,0.038,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22490000,0.98,-0.0087,-0.011,0.18,0.021,-0.016,-1.4,0.024,-0.0023,-1.6,-0.0013,-0.0058,-6.7e-05,0.011,-0.02,-0.13,0.2,-6.9e-06,0.43,0.00041,0.00035,-0.00028,0,0,-1.5,0.00031,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.7e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22590000,0.98,-0.0086,-0.012,0.18,0.029,-0.024,-1.4,0.034,-0.01,-1.7,-0.0013,-0.0058,-5.8e-05,0.012,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00042,0.00037,-0.00027,0,0,-1.6,0.0003,0.00032,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22690000,0.98,-0.0085,-0.012,0.18,0.032,-0.028,-1.4,0.038,-0.013,-1.9,-0.0013,-0.0058,-6.1e-05,0.012,-0.021,-0.13,0.2,-6.4e-06,0.43,0.00041,0.00037,-0.00028,0,0,-1.8,0.0003,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22790000,0.98,-0.0085,-0.012,0.18,0.038,-0.036,-1.4,0.048,-0.022,-2,-0.0013,-0.0058,-6.4e-05,0.011,-0.021,-0.13,0.2,-6.6e-06,0.43,0.0004,0.00034,-0.00031,0,0,-1.9,0.0003,0.00031,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22890000,0.98,-0.0087,-0.012,0.18,0.042,-0.041,-1.4,0.052,-0.026,-2.2,-0.0013,-0.0058,-5.6e-05,0.011,-0.021,-0.13,0.2,-6.3e-06,0.43,0.00041,0.00035,-0.00029,0,0,-2.1,0.0003,0.00031,0.037,0.013,0.015,0.0063,0.04,0.041,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -22990000,0.98,-0.0086,-0.013,0.18,0.046,-0.045,-1.4,0.061,-0.035,-2.3,-0.0013,-0.0058,-4.7e-05,0.012,-0.02,-0.13,0.2,-6.7e-06,0.43,0.0004,0.00032,-0.00026,0,0,-2.2,0.0003,0.00031,0.037,0.012,0.014,0.0062,0.036,0.038,0.033,4.3e-07,3.9e-07,1.6e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23090000,0.98,-0.0086,-0.013,0.18,0.051,-0.05,-1.4,0.065,-0.04,-2.5,-0.0013,-0.0058,-4.6e-05,0.012,-0.02,-0.13,0.2,-6.7e-06,0.43,0.00039,0.00033,-0.00025,0,0,-2.4,0.0003,0.00031,0.037,0.013,0.014,0.0062,0.04,0.041,0.033,4.3e-07,3.9e-07,1.5e-06,0.028,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23190000,0.98,-0.0086,-0.013,0.18,0.057,-0.052,-1.4,0.076,-0.05,-2.6,-0.0013,-0.0058,-4.9e-05,0.012,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00037,0.0003,-0.00025,0,0,-2.5,0.00029,0.00031,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23290000,0.98,-0.0091,-0.014,0.18,0.062,-0.057,-1.4,0.082,-0.055,-2.8,-0.0013,-0.0058,-4.6e-05,0.012,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00034,0.00034,-0.00025,0,0,-2.7,0.00029,0.00031,0.037,0.013,0.014,0.0062,0.039,0.041,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23390000,0.98,-0.009,-0.014,0.18,0.067,-0.06,-1.4,0.092,-0.061,-2.9,-0.0013,-0.0058,-5.9e-05,0.012,-0.019,-0.13,0.2,-7.7e-06,0.43,0.00035,0.00031,-0.00023,0,0,-2.8,0.00029,0.0003,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23490000,0.98,-0.0091,-0.014,0.18,0.072,-0.062,-1.4,0.1,-0.067,-3,-0.0013,-0.0058,-5.1e-05,0.012,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00033,0.00037,-0.00028,0,0,-2.9,0.00029,0.0003,0.037,0.013,0.014,0.0061,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23590000,0.98,-0.0093,-0.014,0.18,0.075,-0.064,-1.4,0.11,-0.076,-3.2,-0.0013,-0.0058,-4.8e-05,0.013,-0.019,-0.13,0.2,-8.4e-06,0.43,0.00029,0.00033,-0.00024,0,0,-3.1,0.00029,0.0003,0.037,0.012,0.013,0.006,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23690000,0.98,-0.01,-0.015,0.18,0.074,-0.066,-1.3,0.11,-0.082,-3.3,-0.0013,-0.0058,-4e-05,0.013,-0.02,-0.13,0.2,-8.3e-06,0.43,0.00027,0.00036,-0.00024,0,0,-3.2,0.00029,0.0003,0.037,0.012,0.014,0.006,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23790000,0.98,-0.012,-0.017,0.18,0.068,-0.062,-0.95,0.12,-0.086,-3.4,-0.0013,-0.0058,-3.8e-05,0.014,-0.02,-0.13,0.2,-8e-06,0.43,0.00025,0.00039,-0.00022,0,0,-3.3,0.00029,0.0003,0.037,0.011,0.013,0.006,0.036,0.037,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23890000,0.98,-0.015,-0.021,0.18,0.064,-0.062,-0.52,0.13,-0.092,-3.5,-0.0013,-0.0058,-3.6e-05,0.014,-0.02,-0.13,0.2,-8e-06,0.43,0.00024,0.00041,-0.00024,0,0,-3.4,0.00029,0.0003,0.037,0.012,0.013,0.006,0.039,0.041,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -23990000,0.98,-0.017,-0.024,0.18,0.064,-0.061,-0.13,0.14,-0.095,-3.6,-0.0013,-0.0058,-4.2e-05,0.015,-0.019,-0.13,0.2,-8.1e-06,0.43,0.0002,0.00043,3.8e-06,0,0,-3.5,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.033,3.9e-07,3.5e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24090000,0.98,-0.017,-0.023,0.18,0.07,-0.069,0.099,0.14,-0.1,-3.6,-0.0013,-0.0058,-4.5e-05,0.015,-0.019,-0.13,0.2,-8.1e-06,0.43,0.00022,0.0004,3.7e-06,0,0,-3.5,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.9e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24190000,0.98,-0.014,-0.019,0.18,0.08,-0.074,0.089,0.15,-0.1,-3.6,-0.0013,-0.0058,-4.5e-05,0.017,-0.019,-0.13,0.2,-7.7e-06,0.43,0.00022,0.00037,0.0001,0,0,-3.5,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.032,3.8e-07,3.5e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24290000,0.98,-0.012,-0.016,0.18,0.084,-0.078,0.067,0.16,-0.11,-3.6,-0.0013,-0.0058,-4.3e-05,0.017,-0.019,-0.13,0.2,-7.3e-06,0.43,0.00025,0.00032,9.8e-05,0,0,-3.5,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.8e-07,3.5e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24390000,0.98,-0.011,-0.015,0.18,0.077,-0.073,0.083,0.16,-0.11,-3.6,-0.0013,-0.0058,-4.2e-05,0.018,-0.02,-0.13,0.2,-6e-06,0.43,0.00024,0.00036,0.00016,0,0,-3.5,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24490000,0.98,-0.011,-0.015,0.18,0.073,-0.069,0.081,0.17,-0.12,-3.6,-0.0013,-0.0058,-2.8e-05,0.019,-0.021,-0.13,0.2,-6e-06,0.43,0.00017,0.00047,0.00021,0,0,-3.5,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24590000,0.98,-0.012,-0.015,0.18,0.069,-0.066,0.077,0.17,-0.12,-3.6,-0.0013,-0.0058,-3.9e-05,0.02,-0.021,-0.13,0.2,-4.7e-06,0.43,0.0002,0.00044,0.00023,0,0,-3.5,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24690000,0.98,-0.012,-0.015,0.18,0.068,-0.065,0.076,0.18,-0.12,-3.5,-0.0013,-0.0058,-3.8e-05,0.02,-0.021,-0.13,0.2,-4.9e-06,0.43,0.00019,0.00047,0.00022,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24790000,0.98,-0.012,-0.014,0.18,0.065,-0.063,0.068,0.18,-0.12,-3.5,-0.0014,-0.0058,-4.6e-05,0.022,-0.022,-0.13,0.2,-4.3e-06,0.43,0.00019,0.00047,0.00021,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24890000,0.98,-0.012,-0.014,0.18,0.063,-0.067,0.057,0.19,-0.13,-3.5,-0.0014,-0.0058,-4e-05,0.022,-0.022,-0.13,0.2,-4.1e-06,0.43,0.00018,0.00048,0.00024,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -24990000,0.98,-0.012,-0.014,0.18,0.055,-0.063,0.05,0.19,-0.12,-3.5,-0.0014,-0.0058,-5.4e-05,0.024,-0.023,-0.13,0.2,-4.3e-06,0.43,0.00015,0.00059,0.00025,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25090000,0.98,-0.012,-0.014,0.18,0.051,-0.063,0.048,0.19,-0.12,-3.5,-0.0014,-0.0058,-5.4e-05,0.024,-0.023,-0.13,0.2,-4.9e-06,0.43,0.00013,0.00066,0.00029,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25190000,0.98,-0.012,-0.014,0.18,0.046,-0.057,0.048,0.19,-0.12,-3.5,-0.0014,-0.0058,-7.2e-05,0.025,-0.024,-0.13,0.2,-4.9e-06,0.43,0.00011,0.00069,0.00028,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25290000,0.98,-0.012,-0.013,0.18,0.041,-0.059,0.043,0.2,-0.12,-3.5,-0.0014,-0.0058,-7.9e-05,0.025,-0.023,-0.13,0.2,-5.5e-06,0.43,0.0001,0.00072,0.00025,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25390000,0.98,-0.012,-0.013,0.18,0.033,-0.052,0.041,0.19,-0.11,-3.5,-0.0014,-0.0058,-9.4e-05,0.027,-0.024,-0.13,0.2,-6.3e-06,0.43,6.6e-05,0.00077,0.00026,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25490000,0.98,-0.013,-0.013,0.18,0.028,-0.052,0.041,0.19,-0.12,-3.5,-0.0014,-0.0058,-9.6e-05,0.027,-0.025,-0.13,0.2,-6.1e-06,0.43,6e-05,0.00073,0.00024,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25590000,0.98,-0.013,-0.012,0.18,0.023,-0.049,0.042,0.19,-0.11,-3.5,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-6.9e-06,0.43,2.9e-05,0.00076,0.00021,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25690000,0.98,-0.012,-0.012,0.18,0.023,-0.048,0.031,0.19,-0.11,-3.5,-0.0014,-0.0058,-0.00011,0.028,-0.026,-0.13,0.2,-7e-06,0.43,3.4e-05,0.00078,0.00022,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25790000,0.98,-0.012,-0.012,0.18,0.012,-0.04,0.031,0.18,-0.1,-3.5,-0.0014,-0.0058,-0.00012,0.029,-0.026,-0.13,0.2,-7.5e-06,0.43,-2.6e-05,0.00071,0.00021,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25890000,0.98,-0.012,-0.012,0.18,0.006,-0.039,0.033,0.18,-0.11,-3.5,-0.0014,-0.0058,-0.00013,0.029,-0.026,-0.13,0.2,-7.6e-06,0.43,-4.6e-05,0.00068,0.00016,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -25990000,0.98,-0.012,-0.012,0.18,-0.003,-0.032,0.027,0.17,-0.097,-3.5,-0.0014,-0.0058,-0.00015,0.031,-0.026,-0.13,0.2,-9e-06,0.43,-0.0001,0.00066,0.00014,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26090000,0.98,-0.012,-0.012,0.18,-0.0076,-0.032,0.026,0.17,-0.1,-3.5,-0.0014,-0.0058,-0.00014,0.031,-0.026,-0.13,0.2,-8.5e-06,0.43,-9.8e-05,0.00065,0.00017,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.025,0.021,0.16,-0.092,-3.5,-0.0015,-0.0058,-0.00014,0.032,-0.026,-0.13,0.2,-8.9e-06,0.43,-0.00012,0.00065,0.00019,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.024,0.015,0.16,-0.095,-3.5,-0.0015,-0.0058,-0.00015,0.032,-0.027,-0.13,0.2,-9.5e-06,0.43,-0.00013,0.00066,0.00016,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26390000,0.98,-0.011,-0.013,0.18,-0.021,-0.017,0.019,0.15,-0.086,-3.5,-0.0015,-0.0058,-0.00016,0.033,-0.027,-0.13,0.2,-1.1e-05,0.43,-0.00017,0.00064,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26490000,0.98,-0.011,-0.013,0.18,-0.023,-0.015,0.028,0.14,-0.087,-3.5,-0.0015,-0.0058,-0.00016,0.033,-0.027,-0.13,0.2,-1.1e-05,0.43,-0.00017,0.00067,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26590000,0.98,-0.01,-0.013,0.18,-0.026,-0.0064,0.029,0.13,-0.079,-3.5,-0.0015,-0.0058,-0.00017,0.034,-0.028,-0.13,0.2,-1.2e-05,0.43,-0.0002,0.00069,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26690000,0.98,-0.01,-0.012,0.18,-0.027,-0.0024,0.027,0.13,-0.08,-3.5,-0.0015,-0.0058,-0.00018,0.033,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00021,0.0007,0.00012,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26790000,0.98,-0.0099,-0.012,0.18,-0.035,0.0019,0.027,0.12,-0.073,-3.5,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00024,0.00068,0.00011,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26890000,0.98,-0.0092,-0.012,0.18,-0.04,0.0048,0.022,0.11,-0.073,-3.5,-0.0015,-0.0058,-0.00019,0.034,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00024,0.00066,0.00011,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -26990000,0.98,-0.0087,-0.013,0.18,-0.048,0.012,0.022,0.098,-0.065,-3.5,-0.0015,-0.0058,-0.00019,0.035,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00026,0.00065,0.00012,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27090000,0.98,-0.0085,-0.013,0.18,-0.05,0.018,0.025,0.093,-0.064,-3.5,-0.0015,-0.0058,-0.00019,0.035,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00025,0.00064,0.00012,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27190000,0.98,-0.0086,-0.013,0.18,-0.056,0.025,0.027,0.081,-0.056,-3.5,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00027,0.00063,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27290000,0.98,-0.0088,-0.014,0.18,-0.063,0.03,0.14,0.076,-0.054,-3.5,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00027,0.00063,0.00013,0,0,-3.4,0.00028,0.00029,0.037,0.012,0.013,0.0054,0.038,0.039,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27390000,0.98,-0.01,-0.016,0.18,-0.071,0.037,0.46,0.064,-0.045,-3.5,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00033,0.00056,0.00018,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27490000,0.98,-0.012,-0.018,0.18,-0.074,0.042,0.78,0.057,-0.041,-3.5,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.13,0.2,-1.6e-05,0.43,-0.00031,0.00052,0.00018,0,0,-3.4,0.00028,0.00029,0.037,0.011,0.013,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27590000,0.98,-0.011,-0.017,0.18,-0.07,0.046,0.87,0.046,-0.036,-3.4,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00029,0.00047,0.00021,0,0,-3.3,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27690000,0.98,-0.01,-0.014,0.18,-0.065,0.042,0.78,0.04,-0.031,-3.3,-0.0015,-0.0058,-0.00021,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00046,0.00019,0,0,-3.2,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27790000,0.98,-0.0088,-0.012,0.18,-0.065,0.04,0.77,0.032,-0.027,-3.2,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00042,0.0002,0,0,-3.1,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27890000,0.98,-0.0085,-0.013,0.18,-0.071,0.047,0.81,0.026,-0.023,-3.2,-0.0015,-0.0058,-0.0002,0.035,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00042,0.0002,0,0,-3.1,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -27990000,0.98,-0.0089,-0.013,0.18,-0.072,0.049,0.8,0.019,-0.019,-3.1,-0.0015,-0.0058,-0.0002,0.034,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00027,0.00036,0.00022,0,0,-3,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28090000,0.98,-0.0092,-0.013,0.18,-0.076,0.05,0.8,0.011,-0.014,-3,-0.0015,-0.0058,-0.00019,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00028,0.00038,0.00021,0,0,-2.9,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28190000,0.98,-0.0086,-0.013,0.18,-0.077,0.047,0.81,0.004,-0.012,-2.9,-0.0015,-0.0058,-0.00019,0.034,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00026,0.00037,0.00022,0,0,-2.8,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28290000,0.98,-0.0081,-0.013,0.18,-0.081,0.051,0.81,-0.0032,-0.0067,-2.9,-0.0015,-0.0058,-0.00018,0.034,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00027,0.00039,0.00019,0,0,-2.8,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28390000,0.98,-0.0081,-0.014,0.18,-0.082,0.055,0.81,-0.0097,-0.0023,-2.8,-0.0014,-0.0058,-0.00017,0.033,-0.029,-0.12,0.2,-1.6e-05,0.43,-0.00032,0.0003,0.0002,0,0,-2.7,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28490000,0.98,-0.0084,-0.015,0.18,-0.085,0.058,0.81,-0.019,0.0032,-2.7,-0.0014,-0.0058,-0.00017,0.033,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00029,0.00029,0.00023,0,0,-2.6,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 -28590000,0.98,-0.0085,-0.014,0.18,-0.079,0.054,0.81,-0.023,0.0026,-2.6,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.6e-05,0.43,-0.00026,0.00027,0.00022,0,0,-2.5,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28690000,0.98,-0.0082,-0.014,0.18,-0.078,0.055,0.81,-0.031,0.008,-2.6,-0.0014,-0.0058,-0.00016,0.032,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00023,0.00027,0.00022,0,0,-2.5,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.056,0.81,-0.035,0.011,-2.5,-0.0014,-0.0058,-0.00015,0.032,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00025,0.00017,0.00022,0,0,-2.4,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28890000,0.98,-0.0074,-0.013,0.18,-0.08,0.058,0.81,-0.043,0.017,-2.4,-0.0014,-0.0058,-0.00014,0.032,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00025,0.0002,0.00019,0,0,-2.3,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -28990000,0.98,-0.0072,-0.013,0.18,-0.077,0.055,0.81,-0.044,0.017,-2.3,-0.0014,-0.0058,-0.00014,0.031,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00029,5.9e-05,0.00023,0,0,-2.2,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29090000,0.98,-0.007,-0.013,0.18,-0.08,0.057,0.81,-0.052,0.023,-2.3,-0.0014,-0.0058,-0.00014,0.031,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00031,4.1e-05,0.00023,0,0,-2.2,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.81,-0.052,0.023,-2.2,-0.0014,-0.0058,-0.00012,0.03,-0.028,-0.12,0.2,-2e-05,0.43,-0.00033,-8.9e-05,0.00024,0,0,-2.1,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,-0.061,0.029,-2.1,-0.0014,-0.0058,-0.00012,0.03,-0.028,-0.12,0.2,-2e-05,0.43,-0.00033,-0.00011,0.00024,0,0,-2,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29390000,0.98,-0.0076,-0.013,0.18,-0.078,0.061,0.81,-0.06,0.031,-2,-0.0014,-0.0058,-0.00011,0.029,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00036,-0.00024,0.00025,0,0,-1.9,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29490000,0.98,-0.0076,-0.013,0.18,-0.081,0.062,0.81,-0.069,0.038,-2,-0.0014,-0.0058,-0.0001,0.029,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00039,-0.00021,0.00023,0,0,-1.9,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,-0.067,0.038,-1.9,-0.0014,-0.0058,-8.7e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00043,-0.00031,0.00023,0,0,-1.8,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,-0.076,0.044,-1.8,-0.0014,-0.0058,-8e-05,0.028,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00045,-0.00028,0.00021,0,0,-1.7,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,-0.072,0.043,-1.7,-0.0014,-0.0058,-6.4e-05,0.027,-0.028,-0.12,0.2,-2.5e-05,0.43,-0.00051,-0.0004,0.00021,0,0,-1.6,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29890000,0.98,-0.0068,-0.013,0.18,-0.08,0.056,0.8,-0.08,0.048,-1.7,-0.0014,-0.0058,-5.7e-05,0.028,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00053,-0.00037,0.00019,0,0,-1.6,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -29990000,0.98,-0.007,-0.013,0.18,-0.075,0.051,0.8,-0.076,0.044,-1.6,-0.0014,-0.0058,-4.6e-05,0.027,-0.029,-0.12,0.2,-2.6e-05,0.43,-0.00051,-0.00042,0.00021,0,0,-1.5,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30090000,0.98,-0.0071,-0.013,0.18,-0.077,0.051,0.8,-0.084,0.049,-1.5,-0.0014,-0.0058,-5.6e-05,0.026,-0.028,-0.12,0.2,-2.7e-05,0.43,-0.00046,-0.00048,0.00026,0,0,-1.4,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30190000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,-0.078,0.047,-1.5,-0.0014,-0.0058,-5.9e-05,0.026,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00047,-0.0007,0.00032,0,0,-1.4,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30290000,0.98,-0.0071,-0.013,0.18,-0.071,0.048,0.8,-0.085,0.052,-1.4,-0.0014,-0.0058,-5.7e-05,0.026,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00051,-0.00075,0.00032,0,0,-1.3,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30390000,0.98,-0.0071,-0.013,0.18,-0.065,0.043,0.8,-0.078,0.049,-1.3,-0.0014,-0.0058,-4e-05,0.025,-0.029,-0.12,0.2,-3e-05,0.43,-0.00065,-0.00096,0.00032,0,0,-1.2,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30490000,0.98,-0.0071,-0.013,0.18,-0.069,0.043,0.8,-0.085,0.054,-1.2,-0.0014,-0.0058,-3.4e-05,0.025,-0.029,-0.12,0.2,-2.9e-05,0.43,-0.00069,-0.00095,0.0003,0,0,-1.1,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30590000,0.98,-0.0074,-0.014,0.18,-0.064,0.041,0.8,-0.077,0.051,-1.2,-0.0014,-0.0058,-1.8e-05,0.025,-0.03,-0.12,0.2,-3e-05,0.43,-0.00079,-0.0011,0.0003,0,0,-1.1,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30690000,0.98,-0.0078,-0.014,0.18,-0.063,0.04,0.8,-0.084,0.055,-1.1,-0.0014,-0.0058,-1.8e-05,0.025,-0.03,-0.12,0.2,-3e-05,0.43,-0.00077,-0.001,0.00029,0,0,-0.99,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30790000,0.98,-0.0075,-0.013,0.17,-0.056,0.034,0.8,-0.076,0.053,-1,-0.0014,-0.0058,-1.4e-05,0.024,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00085,-0.0012,0.00033,0,0,-0.92,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30890000,0.98,-0.0069,-0.013,0.17,-0.056,0.031,0.79,-0.081,0.056,-0.95,-0.0014,-0.0058,-2.1e-05,0.024,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00075,-0.0011,0.00035,0,0,-0.85,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -30990000,0.98,-0.0071,-0.013,0.17,-0.049,0.025,0.8,-0.071,0.049,-0.88,-0.0014,-0.0057,-1.3e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00079,-0.0013,0.00038,0,0,-0.78,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31090000,0.98,-0.0073,-0.013,0.17,-0.048,0.024,0.79,-0.075,0.052,-0.81,-0.0014,-0.0057,-1.8e-05,0.024,-0.032,-0.12,0.2,-3.3e-05,0.43,-0.00074,-0.0012,0.00039,0,0,-0.71,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31190000,0.98,-0.0075,-0.013,0.17,-0.043,0.02,0.8,-0.066,0.047,-0.74,-0.0013,-0.0057,-1.7e-06,0.024,-0.033,-0.12,0.2,-3.4e-05,0.43,-0.00084,-0.0012,0.00038,0,0,-0.64,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.018,0.8,-0.07,0.049,-0.67,-0.0013,-0.0057,3.4e-06,0.024,-0.033,-0.12,0.2,-3.3e-05,0.43,-0.00087,-0.0012,0.00037,0,0,-0.57,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,-0.061,0.043,-0.59,-0.0013,-0.0057,2.7e-06,0.023,-0.033,-0.12,0.2,-3.3e-05,0.43,-0.00096,-0.0015,0.00042,0,0,-0.49,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0081,0.8,-0.065,0.044,-0.52,-0.0013,-0.0057,1e-06,0.023,-0.033,-0.12,0.2,-3.2e-05,0.43,-0.001,-0.0017,0.00043,0,0,-0.42,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.5e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31590000,0.98,-0.0071,-0.014,0.17,-0.033,0.0062,0.8,-0.054,0.04,-0.45,-0.0013,-0.0057,1.1e-05,0.023,-0.033,-0.12,0.2,-3.2e-05,0.43,-0.0011,-0.0018,0.00045,0,0,-0.35,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31690000,0.98,-0.0071,-0.014,0.17,-0.036,0.0056,0.8,-0.058,0.041,-0.38,-0.0013,-0.0057,1.9e-05,0.023,-0.033,-0.12,0.2,-3.1e-05,0.43,-0.0012,-0.0018,0.00043,0,0,-0.28,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31790000,0.98,-0.0074,-0.015,0.17,-0.027,0.003,0.8,-0.045,0.039,-0.3,-0.0013,-0.0057,2.4e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.0019,0.00045,0,0,-0.2,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31890000,0.99,-0.0071,-0.015,0.17,-0.025,0.00083,0.8,-0.049,0.039,-0.23,-0.0013,-0.0057,2.8e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.002,0.00047,0,0,-0.13,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -31990000,0.99,-0.0074,-0.014,0.17,-0.017,-0.00026,0.79,-0.036,0.036,-0.17,-0.0013,-0.0057,2.5e-05,0.024,-0.034,-0.12,0.2,-3e-05,0.43,-0.0014,-0.0022,0.00052,0,0,-0.066,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32090000,0.99,-0.0077,-0.014,0.17,-0.019,-0.0037,0.8,-0.038,0.036,-0.094,-0.0013,-0.0057,2.5e-05,0.024,-0.034,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0023,0.00054,0,0,0.0058,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32190000,0.99,-0.008,-0.014,0.17,-0.013,-0.0073,0.8,-0.026,0.034,-0.026,-0.0013,-0.0057,2.1e-05,0.024,-0.034,-0.12,0.2,-2.9e-05,0.43,-0.0014,-0.0024,0.00058,0,0,0.074,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32290000,0.99,-0.0079,-0.015,0.17,-0.013,-0.0098,0.8,-0.027,0.033,0.044,-0.0013,-0.0057,2.6e-05,0.025,-0.034,-0.12,0.2,-2.8e-05,0.43,-0.0015,-0.0025,0.00059,0,0,0.14,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32390000,0.99,-0.008,-0.015,0.17,-0.0063,-0.011,0.8,-0.015,0.03,0.12,-0.0013,-0.0057,2.3e-05,0.025,-0.034,-0.12,0.2,-2.6e-05,0.43,-0.0016,-0.0027,0.00065,0,0,0.22,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32490000,0.99,-0.011,-0.012,0.17,0.018,-0.077,-0.076,-0.014,0.024,0.12,-0.0013,-0.0057,1.9e-05,0.025,-0.034,-0.12,0.2,-2.7e-05,0.43,-0.0015,-0.0027,0.00064,0,0,0.22,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32590000,0.99,-0.011,-0.013,0.17,0.021,-0.079,-0.079,0.0015,0.02,0.1,-0.0013,-0.0057,1.8e-05,0.025,-0.034,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,0.2,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32690000,0.99,-0.011,-0.012,0.17,0.016,-0.085,-0.08,0.0033,0.012,0.09,-0.0013,-0.0057,1.7e-05,0.025,-0.034,-0.12,0.2,-2.7e-05,0.43,-0.0014,-0.0023,0.00071,0,0,0.19,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32790000,0.99,-0.011,-0.013,0.17,0.017,-0.084,-0.081,0.015,0.0093,0.074,-0.0013,-0.0057,1.6e-05,0.025,-0.034,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0022,0.00078,0,0,0.17,0.00028,0.00027,0.035,0.011,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0.017,0.00078,0.059,-0.0013,-0.0057,1.9e-05,0.025,-0.034,-0.12,0.2,-2.5e-05,0.43,-0.0015,-0.0021,0.00079,0,0,0.16,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0.028,-0.0031,0.046,-0.0014,-0.0057,2.1e-05,0.025,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0015,-0.002,0.00085,0,0,0.15,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -33090000,0.98,-0.011,-0.013,0.17,0.013,-0.093,-0.079,0.029,-0.012,0.038,-0.0014,-0.0057,2.1e-05,0.025,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0015,-0.002,0.00085,0,0,0.14,0.00027,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 -33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0.037,-0.015,0.031,-0.0014,-0.0057,1.2e-05,0.026,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0015,-0.002,0.00088,0,0,0.13,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33290000,0.98,-0.01,-0.013,0.17,0.0073,-0.096,-0.078,0.037,-0.024,0.023,-0.0014,-0.0057,2.3e-05,0.026,-0.034,-0.12,0.2,-2.1e-05,0.43,-0.0016,-0.0021,0.00092,0,0,0.12,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 -33390000,0.98,-0.01,-0.013,0.17,0.0052,-0.09,-0.076,0.043,-0.022,0.014,-0.0014,-0.0057,1.6e-05,0.028,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0016,-0.002,0.00096,0,0,0.12,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 -33490000,0.98,-0.01,-0.013,0.17,-0.00014,-0.093,-0.075,0.042,-0.031,0.0042,-0.0014,-0.0057,2.1e-05,0.028,-0.032,-0.12,0.2,-1.7e-05,0.43,-0.0017,-0.0021,0.00098,0,0,0.12,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 -33590000,0.98,-0.01,-0.013,0.17,-0.0029,-0.092,-0.072,0.046,-0.029,-0.0037,-0.0014,-0.0057,2e-05,0.029,-0.031,-0.12,0.2,-1.3e-05,0.43,-0.0018,-0.0022,0.001,0,0,0.12,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.083 -33690000,0.98,-0.01,-0.013,0.17,-0.0062,-0.095,-0.073,0.046,-0.038,-0.012,-0.0014,-0.0057,2.4e-05,0.029,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.0018,-0.002,0.001,0,0,0.12,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.11 -33790000,0.98,-0.01,-0.013,0.17,-0.0081,-0.092,-0.068,0.052,-0.036,-0.019,-0.0014,-0.0057,1.4e-05,0.031,-0.031,-0.12,0.2,-1.2e-05,0.43,-0.0017,-0.0019,0.0011,0,0,0.12,0.00027,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.13 -33890000,0.98,-0.01,-0.013,0.17,-0.012,-0.093,-0.067,0.051,-0.045,-0.025,-0.0014,-0.0057,2.4e-05,0.031,-0.031,-0.12,0.2,-1.2e-05,0.43,-0.0018,-0.0018,0.0011,0,0,0.12,0.00027,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.16 -33990000,0.98,-0.0099,-0.013,0.17,-0.013,-0.088,-0.064,0.055,-0.04,-0.029,-0.0014,-0.0057,1.1e-05,0.033,-0.031,-0.12,0.2,-8.6e-06,0.43,-0.0017,-0.0018,0.0011,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.18 -34090000,0.98,-0.0098,-0.013,0.17,-0.017,-0.093,-0.062,0.054,-0.049,-0.033,-0.0014,-0.0057,1.4e-05,0.033,-0.031,-0.12,0.2,-9e-06,0.43,-0.0017,-0.0017,0.0011,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.21 -34190000,0.98,-0.0098,-0.013,0.17,-0.018,-0.088,-0.06,0.058,-0.044,-0.037,-0.0014,-0.0057,1e-05,0.035,-0.031,-0.12,0.2,-6.6e-06,0.43,-0.0017,-0.0016,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.23 -34290000,0.98,-0.0096,-0.013,0.17,-0.019,-0.092,-0.058,0.056,-0.052,-0.043,-0.0014,-0.0057,1.7e-05,0.035,-0.031,-0.12,0.2,-5.9e-06,0.43,-0.0017,-0.0016,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.26 -34390000,0.98,-0.0095,-0.013,0.17,-0.021,-0.085,-0.054,0.058,-0.047,-0.047,-0.0014,-0.0056,9.7e-06,0.036,-0.03,-0.12,0.2,-3.8e-06,0.43,-0.0016,-0.0015,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.28 -34490000,0.98,-0.0095,-0.013,0.17,-0.024,-0.089,-0.052,0.057,-0.055,-0.05,-0.0014,-0.0056,1.8e-05,0.036,-0.03,-0.12,0.2,-3.5e-06,0.43,-0.0017,-0.0015,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.31 -34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.081,0.74,0.059,-0.05,-0.021,-0.0014,-0.0056,1.1e-05,0.038,-0.03,-0.12,0.2,-1e-06,0.43,-0.0017,-0.0014,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 -34690000,0.98,-0.0094,-0.013,0.17,-0.03,-0.079,1.7,0.056,-0.058,0.098,-0.0014,-0.0056,1.4e-05,0.038,-0.03,-0.12,0.2,-7.2e-07,0.43,-0.0017,-0.0014,0.0012,0,0,0.12,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 -34790000,0.98,-0.0093,-0.013,0.17,-0.033,-0.07,2.7,0.058,-0.052,0.28,-0.0015,-0.0056,7.7e-06,0.04,-0.031,-0.12,0.2,1.9e-06,0.43,-0.0017,-0.0013,0.0013,0,0,0.12,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 -34890000,0.98,-0.0092,-0.013,0.17,-0.038,-0.069,3.7,0.055,-0.058,0.57,-0.0015,-0.0056,1.1e-05,0.04,-0.031,-0.12,0.2,2e-06,0.43,-0.0017,-0.0013,0.0013,0,0,0.12,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.035,0,0,-4.9e+02,1.6e-09,-1.4e-09,-6.4e-11,0,0,-3.2e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 +290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.038,0,0,-4.9e+02,7.3e-09,-1.1e-08,-3.9e-10,0,0,-2e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 +390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.05,0,0,-4.9e+02,-2.1e-10,-1.4e-08,-3.2e-10,0,0,-1.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 +490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.055,0,0,-4.9e+02,-1.2e-06,7.3e-07,4.1e-08,0,0,-1.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 +590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.1,0,0,-4.9e+02,-1.3e-06,7.6e-07,4.5e-08,0,0,6.3e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 +690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.039,0,0,-4.9e+02,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00017,0,0,0,0,0,0,0,0,-4.9e+02,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 +790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.045,0,0,-4.9e+02,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,-4.9e+02,0.018,0.018,0.00077,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 +890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.086,0,0,-4.9e+02,-2.2e-05,1e-06,4.9e-07,0,0,-7.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.019,0.019,0.00051,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 +990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.11,0,0,-4.9e+02,-2.2e-05,9.9e-07,4.9e-07,0,0,-9.4e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.021,0.021,0.00062,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 +1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0,0,-4.9e+02,-6.1e-05,-1.5e-05,9.8e-07,0,0,3.8e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.023,0.023,0.00043,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 +1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.1,0,0,-4.9e+02,-5.8e-05,-1.4e-05,9.7e-07,0,0,-0.00052,0,0,0,0,0,0,0,0,-4.9e+02,0.025,0.025,0.00051,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 +1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.1,0,0,-4.9e+02,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00079,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.00038,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 +1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.094,0,0,-4.9e+02,-0.00016,-9.3e-05,1.5e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00043,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 +1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.11,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,-4.9e+02,0.027,0.027,0.00033,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1590000,1,-0.012,-0.014,0.00039,0.031,-0.024,-0.13,0,0,-4.9e+02,-0.00039,-0.00033,1.2e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,-4.9e+02,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.012,-0.014,0.00044,0.028,-0.019,-0.13,0,0,-4.9e+02,-0.00073,-0.00074,-3.5e-07,0,0,-0.0018,0,0,0,0,0,0,0,0,-4.9e+02,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0,0,-4.9e+02,-0.00073,-0.00073,-3e-07,0,0,-0.0028,0,0,0,0,0,0,0,0,-4.9e+02,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0,0,-4.9e+02,-0.00072,-0.00072,-2.7e-07,0,0,-0.0032,0,0,0,0,0,0,0,0,-4.9e+02,0.031,0.031,0.00037,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0,0,-4.9e+02,-0.0011,-0.0013,-3.7e-06,0,0,-0.0046,0,0,0,0,0,0,0,0,-4.9e+02,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0,0,-4.9e+02,-0.0011,-0.0012,-3.5e-06,0,0,-0.0064,0,0,0,0,0,0,0,0,-4.9e+02,0.027,0.027,0.00032,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,0.00039,0.033,-0.013,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.8e-06,0,0,-0.0075,0,0,0,0,0,0,0,0,-4.9e+02,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,0.00038,0.038,-0.014,-0.14,0,0,-4.9e+02,-0.0014,-0.0018,-8.6e-06,0,0,-0.0097,0,0,0,0,0,0,0,0,-4.9e+02,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,0.0004,0.029,-0.0098,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.5e-05,0,0,-0.012,0,0,0,0,0,0,0,0,-4.9e+02,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.014,0.00048,0.033,-0.0088,-0.14,0,0,-4.9e+02,-0.0017,-0.0023,-1.5e-05,0,0,-0.013,0,0,0,0,0,0,0,0,-4.9e+02,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.01,-0.013,0.00039,0.023,-0.0059,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2.1e-05,0,0,-0.015,0,0,0,0,0,0,0,0,-4.9e+02,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.01,-0.013,0.00043,0.027,-0.0051,-0.15,0,0,-4.9e+02,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,-4.9e+02,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.01,-0.013,0.00038,0.022,-0.0029,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.6e-05,0,0,-0.022,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.01,-0.013,0.0003,0.026,-0.0046,-0.14,0,0,-4.9e+02,-0.0019,-0.003,-2.6e-05,0,0,-0.025,0,0,0,0,0,0,0,0,-4.9e+02,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.01,-0.013,0.00032,0.02,-0.0035,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3.1e-05,0,0,-0.028,0,0,0,0,0,0,0,0,-4.9e+02,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.01,-0.013,0.00052,0.025,-0.0063,-0.15,0,0,-4.9e+02,-0.002,-0.0033,-3.1e-05,0,0,-0.031,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.01,-0.013,0.00057,0.02,-0.0061,-0.15,0,0,-4.9e+02,-0.002,-0.0036,-3.5e-05,0,0,-0.032,0,0,0,0,0,0,0,0,-4.9e+02,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.01,-0.013,0.00059,0.023,-0.0062,-0.15,0,0,-4.9e+02,-0.002,-0.0036,-3.5e-05,0,0,-0.034,0,0,0,0,0,0,0,0,-4.9e+02,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.0098,-0.013,0.0006,0.019,-0.0032,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.9e-05,0,0,-0.039,0,0,0,0,0,0,0,0,-4.9e+02,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.0097,-0.013,0.00059,0.025,-0.0018,-0.15,0,0,-4.9e+02,-0.0021,-0.0038,-3.9e-05,0,0,-0.044,0,0,0,0,0,0,0,0,-4.9e+02,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.0095,-0.012,0.00054,0.021,-0.0014,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.3e-05,0,0,-0.046,0,0,0,0,0,0,0,0,-4.9e+02,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.0095,-0.013,0.00053,0.024,-0.00063,-0.15,0,0,-4.9e+02,-0.0022,-0.004,-4.3e-05,0,0,-0.051,0,0,0,0,0,0,0,0,-4.9e+02,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.0094,-0.012,0.00055,0.019,0.0037,-0.15,0,0,-4.9e+02,-0.0022,-0.0043,-4.8e-05,0,0,-0.054,0,0,0,0,0,0,0,0,-4.9e+02,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.0094,-0.013,0.00064,0.021,0.005,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.058,0,0,0,0,0,0,0,0,-4.9e+02,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.0094,-0.013,0.0007,0.026,0.0048,-0.14,0,0,-4.9e+02,-0.0022,-0.0042,-4.7e-05,0,0,-0.063,0,0,0,0,0,0,0,0,-4.9e+02,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.0093,-0.012,0.00077,0.022,0.0042,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.2e-05,0,0,-0.071,0,0,0,0,0,0,0,0,-4.9e+02,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.0094,-0.012,0.00073,0.024,0.0039,-0.12,0,0,-4.9e+02,-0.0022,-0.0044,-5.2e-05,0,0,-0.074,0,0,0,0,0,0,0,0,-4.9e+02,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.0095,-0.012,0.00075,0.021,0.0037,-0.13,0,0,-4.9e+02,-0.0021,-0.0046,-5.7e-05,0,0,-0.076,0,0,0,0,0,0,0,0,-4.9e+02,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.0094,-0.012,0.0007,0.025,0.0023,-0.11,0,0,-4.9e+02,-0.0021,-0.0046,-5.7e-05,0,0,-0.083,0,0,0,0,0,0,0,0,-4.9e+02,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.0094,-0.012,0.00077,0.021,0.004,-0.11,0,0,-4.9e+02,-0.0021,-0.0048,-6.2e-05,0,0,-0.086,0,0,0,0,0,0,0,0,-4.9e+02,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.0094,-0.012,0.00083,0.023,0.0029,-0.11,0,0,-4.9e+02,-0.0021,-0.0048,-6.2e-05,0,0,-0.088,0,0,0,0,0,0,0,0,-4.9e+02,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.0094,-0.012,0.00077,0.017,0.0031,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.6e-05,0,0,-0.093,0,0,0,0,0,0,0,0,-4.9e+02,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.0093,-0.012,0.00087,0.015,0.0053,-0.1,0,0,-4.9e+02,-0.0021,-0.005,-6.6e-05,0,0,-0.095,0,0,0,0,0,0,0,0,-4.9e+02,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.0092,-0.012,0.00091,0.01,0.0028,-0.093,0,0,-4.9e+02,-0.0021,-0.0051,-7e-05,0,0,-0.099,0,0,0,0,0,0,0,0,-4.9e+02,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.0092,-0.012,0.00089,0.013,0.0035,-0.085,0,0,-4.9e+02,-0.0021,-0.0051,-7e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.0091,-0.011,0.00097,0.01,0.0038,-0.083,0,0,-4.9e+02,-0.002,-0.0052,-7.3e-05,0,0,-0.1,0,0,0,0,0,0,0,0,-4.9e+02,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.0089,-0.012,0.001,0.0099,0.0074,-0.08,0,0,-4.9e+02,-0.002,-0.0052,-7.3e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0074,-0.069,0,0,-4.9e+02,-0.002,-0.0053,-7.6e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0,0,-4.9e+02,-0.002,-0.0053,-7.6e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.061,0,0,-4.9e+02,-0.002,-0.0054,-7.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,-4.9e+02,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0088,-0.012,0.001,0.0083,0.016,-0.054,0,0,-4.9e+02,-0.002,-0.0054,-7.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0089,-0.011,0.0009,0.0077,0.016,-0.053,0,0,-4.9e+02,-0.0019,-0.0054,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0087,-0.011,0.00086,0.0089,0.018,-0.05,0,0,-4.9e+02,-0.0019,-0.0054,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0088,-0.011,0.0009,0.0095,0.016,-0.048,0,0,-4.9e+02,-0.0019,-0.0055,-8.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0088,-0.012,0.00087,0.011,0.017,-0.042,0,0,-4.9e+02,-0.0019,-0.0055,-8.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0087,-0.011,0.00068,0.011,0.018,-0.039,0,0,-4.9e+02,-0.0019,-0.0055,-8.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0089,-0.011,0.00069,0.0087,0.017,-0.038,0,0,-4.9e+02,-0.0018,-0.0055,-8.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0088,-0.011,0.00072,0.008,0.02,-0.041,0,0,-4.9e+02,-0.0018,-0.0055,-8.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0089,-0.011,0.00073,0.0082,0.017,-0.042,0,0,-4.9e+02,-0.0017,-0.0056,-8.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,-4.9e+02,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0088,-0.011,0.00063,0.0057,0.016,-0.04,0,0,-4.9e+02,-0.0017,-0.0056,-8.8e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0089,-0.011,0.00056,0.0039,0.015,-0.042,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0088,-0.011,0.00052,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,1,-0.0089,-0.011,0.00049,0.003,0.016,-0.043,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6890000,1,-0.0087,-0.011,0.0004,0.0023,0.016,-0.039,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 +6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 +7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 +7190000,0.98,-0.0065,-0.012,0.18,-6.6e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 +7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00095,-0.032,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.9e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7490000,0.98,-0.0063,-0.012,0.18,0.00098,0.0035,-0.026,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.8e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.0061,-0.023,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00035,-0.00039,-8.8e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9 +7690000,0.98,-0.0064,-0.013,0.18,0.0021,0.0093,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.4e-05,0.43,-0.00031,-0.0004,3.2e-06,0,0,-4.9e+02,0.0016,0.0015,0.042,25,25,0.025,52,52,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00017,0.0013,0.0014,0.0016,0.0013,1,1,2 +7790000,0.98,-0.0064,-0.013,0.18,0.0056,0.01,-0.025,0,0,-4.9e+02,-0.0015,-0.0055,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-2.9e-05,0.43,-0.00021,-0.00039,-8.1e-07,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.024,35,35,0.061,6.3e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00015,0.0013,0.0014,0.0016,0.0013,1,1,2 +7890000,0.98,-0.0064,-0.013,0.18,0.0047,0.014,-0.025,0,0,-4.9e+02,-0.0015,-0.0055,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-2.6e-05,0.43,-0.00019,-0.0004,4.5e-05,0,0,-4.9e+02,0.0016,0.0016,0.042,24,24,0.023,36,36,0.06,6.3e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00013,0.0013,0.0014,0.0016,0.0013,1,1,2 +7990000,0.98,-0.0063,-0.013,0.18,0.0032,0.017,-0.022,0,0,-4.9e+02,-0.0016,-0.0056,-9.3e-05,-0.00036,0.00036,-0.13,0.2,-2.5e-05,0.43,-0.0002,-0.00042,7.5e-05,0,0,-4.9e+02,0.0017,0.0016,0.042,24,24,0.022,28,28,0.059,6.2e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.00012,0.0013,0.0014,0.0016,0.0013,1,1,2 +8090000,0.98,-0.0062,-0.013,0.18,0.0043,0.019,-0.022,0,0,-4.9e+02,-0.0015,-0.0056,-9.5e-05,-0.00036,0.00036,-0.13,0.2,-2.2e-05,0.43,-0.00017,-0.00042,0.0001,0,0,-4.9e+02,0.0017,0.0017,0.042,24,24,0.022,30,30,0.059,6.2e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,0.00011,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8190000,0.98,-0.0064,-0.012,0.18,0.0053,0.022,-0.018,0,0,-4.9e+02,-0.0015,-0.0056,-9.7e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.44,-0.00014,-0.00041,0.00015,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.021,24,24,0.058,6.1e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0013,0.0001,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8290000,0.98,-0.0062,-0.012,0.18,0.002,0.028,-0.017,0,0,-4.9e+02,-0.0015,-0.0058,-9.8e-05,-0.00036,0.00036,-0.13,0.2,-2e-05,0.43,-0.00014,-0.00044,6.8e-05,0,0,-4.9e+02,0.0018,0.0017,0.041,23,23,0.02,27,27,0.057,6.1e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0013,9.6e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8390000,0.98,-0.0061,-0.012,0.18,-0.0023,0.028,-0.016,0,0,-4.9e+02,-0.0015,-0.0058,-9.9e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00044,2.1e-05,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.02,23,23,0.057,6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0013,9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.1 +8490000,0.98,-0.0058,-0.012,0.18,-0.0061,0.035,-0.017,0,0,-4.9e+02,-0.0016,-0.0059,-9.8e-05,-0.00036,0.00036,-0.13,0.2,-1.9e-05,0.43,-0.00014,-0.00048,-6.7e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,21,21,0.019,25,25,0.056,5.9e-05,5.6e-05,2.2e-06,0.04,0.04,0.0011,0.0013,8.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8590000,0.98,-0.006,-0.012,0.18,-0.0003,0.034,-0.012,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.14,0.2,-1.7e-05,0.43,-0.00018,-0.00043,3.8e-06,0,0,-4.9e+02,0.0019,0.0018,0.041,19,19,0.018,22,22,0.055,5.8e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0013,7.9e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8690000,0.98,-0.0059,-0.012,0.18,-0.0022,0.037,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.7e-05,-0.00036,0.00036,-0.14,0.2,-1.6e-05,0.43,-0.00022,-0.00045,-8.7e-05,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,24,24,0.055,5.8e-05,5.3e-05,2.2e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8790000,0.98,-0.0059,-0.012,0.18,-0.005,0.039,-0.014,0,0,-4.9e+02,-0.0016,-0.0058,-9.9e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00019,-0.00045,-0.00013,0,0,-4.9e+02,0.002,0.0018,0.041,19,19,0.018,27,27,0.055,5.7e-05,5.2e-05,2.2e-06,0.04,0.04,0.00099,0.0013,7.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.2 +8890000,0.98,-0.006,-0.012,0.18,0.00052,0.041,-0.0094,0,0,-4.9e+02,-0.0016,-0.0057,-9.6e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00026,-0.00043,-0.00011,0,0,-4.9e+02,0.002,0.0018,0.041,17,17,0.017,24,24,0.054,5.6e-05,5e-05,2.2e-06,0.04,0.04,0.00094,0.0013,6.7e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +8990000,0.98,-0.0059,-0.013,0.18,0.01,0.047,-0.0086,0,0,-4.9e+02,-0.0016,-0.0056,-9.1e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.00037,-0.00039,-6.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,17,17,0.017,27,27,0.054,5.5e-05,4.9e-05,2.2e-06,0.04,0.04,0.00091,0.0013,6.4e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +9090000,0.98,-0.0055,-0.012,0.18,-0.0004,0.057,-0.0096,0,0,-4.9e+02,-0.0017,-0.0057,-9e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00028,-0.0005,-8.8e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,24,24,0.053,5.3e-05,4.7e-05,2.2e-06,0.04,0.04,0.00087,0.0013,6.1e-05,0.0013,0.0014,0.0016,0.0013,1,1,2.3 +9190000,0.98,-0.0053,-0.013,0.18,0.0028,0.064,-0.009,0,0,-4.9e+02,-0.0018,-0.0057,-8.8e-05,-0.00036,0.00036,-0.14,0.2,-1.5e-05,0.43,-0.00031,-0.00053,-0.00011,0,0,-4.9e+02,0.0021,0.0018,0.041,15,15,0.016,27,27,0.052,5.2e-05,4.5e-05,2.2e-06,0.04,0.04,0.00083,0.0013,5.9e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.3 +9290000,0.98,-0.0052,-0.013,0.18,0.013,0.062,-0.0074,0,0,-4.9e+02,-0.0017,-0.0056,-8.6e-05,-0.00036,0.00036,-0.14,0.2,-1.4e-05,0.43,-0.0004,-0.00049,-8.6e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,24,24,0.052,5.1e-05,4.4e-05,2.2e-06,0.04,0.04,0.00079,0.0013,5.6e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 +9390000,0.98,-0.0054,-0.013,0.18,0.014,0.06,-0.0063,0,0,-4.9e+02,-0.0017,-0.0056,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00035,-0.00044,-3.9e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,13,13,0.015,26,26,0.052,5e-05,4.2e-05,2.2e-06,0.04,0.04,0.00077,0.0013,5.4e-05,0.0013,0.0013,0.0016,0.0013,1,1,2.4 +9490000,0.98,-0.0053,-0.013,0.18,0.0085,0.062,-0.0046,0,0,-4.9e+02,-0.0017,-0.0057,-8.9e-05,-0.00036,0.00036,-0.14,0.2,-1.2e-05,0.43,-0.00033,-0.00048,-7.7e-05,0,0,-4.9e+02,0.0021,0.0018,0.041,12,12,0.015,23,23,0.051,4.8e-05,4e-05,2.2e-06,0.04,0.04,0.00074,0.0013,5.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 +9590000,0.98,-0.0057,-0.013,0.18,0.0075,0.053,-0.0045,0,0,-4.9e+02,-0.0016,-0.0057,-9.5e-05,-0.00036,0.00036,-0.14,0.2,-1e-05,0.43,-0.0003,-0.00042,-0.00012,0,0,-4.9e+02,0.0022,0.0018,0.041,12,12,0.014,26,26,0.05,4.7e-05,3.9e-05,2.2e-06,0.04,0.04,0.00071,0.0013,5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.4 +9690000,0.98,-0.0059,-0.013,0.18,0.0093,0.048,-0.0016,0,0,-4.9e+02,-0.0015,-0.0057,-9.7e-05,-0.00036,0.00036,-0.14,0.2,-9.2e-06,0.43,-0.00031,-0.00038,-0.0001,0,0,-4.9e+02,0.0022,0.0018,0.041,10,10,0.014,23,23,0.05,4.6e-05,3.7e-05,2.2e-06,0.04,0.04,0.00068,0.0013,4.8e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9790000,0.98,-0.0061,-0.012,0.18,0.00062,0.042,-0.003,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.8e-06,0.43,-0.00018,-0.00037,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,10,10,0.014,25,25,0.05,4.4e-05,3.5e-05,2.2e-06,0.04,0.04,0.00066,0.0013,4.7e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9890000,0.98,-0.0061,-0.012,0.18,0.0082,0.041,-0.0017,0,0,-4.9e+02,-0.0014,-0.0057,-0.0001,-0.00036,0.00036,-0.14,0.2,-7.6e-06,0.43,-0.00023,-0.00035,-9.7e-05,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,22,22,0.049,4.3e-05,3.4e-05,2.2e-06,0.04,0.04,0.00063,0.0013,4.5e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +9990000,0.98,-0.0063,-0.012,0.18,0.002,0.034,-0.001,0,0,-4.9e+02,-0.0013,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-6.5e-06,0.43,-0.00013,-0.00033,-0.00013,0,0,-4.9e+02,0.0022,0.0017,0.041,8.6,8.7,0.013,24,24,0.049,4.2e-05,3.2e-05,2.2e-06,0.04,0.04,0.00061,0.0013,4.4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.5 +10090000,0.98,-0.0067,-0.012,0.18,0.00021,0.019,0.00017,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,-0.00036,0.00036,-0.14,0.2,-4.9e-06,0.43,-9.2e-05,-0.00026,-0.00015,0,0,-4.9e+02,0.0022,0.0017,0.041,7.4,7.5,0.013,21,21,0.048,4e-05,3.1e-05,2.2e-06,0.04,0.04,0.00059,0.0013,4.2e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10190000,0.98,-0.0072,-0.012,0.18,0.0047,0.0051,0.001,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00036,0.00036,-0.14,0.2,-3.3e-06,0.43,-8.4e-05,-0.00012,-0.00015,0,0,-4.9e+02,0.0022,0.0016,0.041,7.4,7.6,0.012,23,23,0.048,3.9e-05,2.9e-05,2.2e-06,0.04,0.04,0.00057,0.0013,4.1e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10290000,0.98,-0.0071,-0.012,0.18,0.011,0.0088,-3.6e-05,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.00036,0.00036,-0.14,0.2,-3.7e-06,0.43,-0.00015,-0.00011,-0.00012,0,0,-4.9e+02,0.0022,0.0016,0.041,6.4,6.5,0.012,20,20,0.048,3.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10390000,0.98,-0.007,-0.012,0.18,0.0076,0.0026,-0.0025,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.0003,0.00041,-0.14,0.2,-3.9e-06,0.43,-0.00019,-0.00012,-6.9e-05,0,0,-4.9e+02,0.0022,0.0016,0.041,0.25,0.25,0.56,0.5,0.5,0.048,3.6e-05,2.7e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.6 +10490000,0.98,-0.0073,-0.012,0.18,0.0096,0.00086,0.007,0,0,-4.9e+02,-0.001,-0.0057,-0.00012,-0.00034,0.00027,-0.14,0.2,-3e-06,0.43,-0.00016,-6.3e-05,-9.1e-05,0,0,-4.9e+02,0.0021,0.0016,0.041,0.25,0.26,0.55,0.51,0.51,0.057,3.5e-05,2.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10590000,0.98,-0.007,-0.012,0.18,0.00025,-0.00016,0.013,0,0,-4.9e+02,-0.0011,-0.0057,-0.00011,-0.0003,0.00025,-0.14,0.2,-3.6e-06,0.43,-0.00018,-0.0001,-7.7e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.13,0.27,0.17,0.17,0.055,3.3e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10690000,0.98,-0.0071,-0.013,0.18,0.003,-0.0027,0.016,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00027,0.00018,-0.14,0.2,-3.3e-06,0.43,-0.00021,-5.3e-05,-7.4e-05,0,0,-4.9e+02,0.0021,0.0015,0.041,0.13,0.14,0.26,0.17,0.18,0.065,3.2e-05,2.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10790000,0.98,-0.0073,-0.013,0.18,0.0057,-0.006,0.014,0,0,-4.9e+02,-0.001,-0.0056,-0.00011,-0.00016,7.7e-05,-0.14,0.2,-3.2e-06,0.43,-0.00025,8.2e-06,-9.4e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.09,0.095,0.17,0.11,0.11,0.061,3e-05,2.1e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.7 +10890000,0.98,-0.0069,-0.013,0.18,0.0078,-0.0029,0.01,0,0,-4.9e+02,-0.0011,-0.0055,-0.00011,-5.6e-05,0.0002,-0.14,0.2,-4.1e-06,0.43,-0.00032,-3.5e-05,-7.6e-05,0,0,-4.9e+02,0.002,0.0014,0.041,0.097,0.1,0.16,0.11,0.11,0.068,2.9e-05,2e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +10990000,0.98,-0.0069,-0.013,0.18,0.0053,0.0032,0.016,0,0,-4.9e+02,-0.0011,-0.0056,-0.00011,-0.00021,0.00016,-0.14,0.2,-4.1e-06,0.43,-0.00025,-6.5e-05,-0.00012,0,0,-4.9e+02,0.0019,0.0014,0.041,0.074,0.081,0.12,0.079,0.079,0.065,2.6e-05,1.9e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11090000,0.98,-0.0075,-0.013,0.18,0.0091,0.0017,0.02,0,0,-4.9e+02,-0.001,-0.0056,-0.00011,-0.00029,-4.9e-05,-0.14,0.2,-2.9e-06,0.43,-0.00023,1.9e-05,-0.00011,0,0,-4.9e+02,0.0019,0.0013,0.041,0.082,0.093,0.11,0.084,0.085,0.069,2.6e-05,1.8e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11190000,0.98,-0.0077,-0.013,0.18,0.0081,0.0023,0.026,0,0,-4.9e+02,-0.00097,-0.0057,-0.00012,-0.00041,8.1e-07,-0.14,0.2,-2.4e-06,0.43,-0.00016,-7.8e-06,-0.00012,0,0,-4.9e+02,0.0017,0.0013,0.04,0.066,0.076,0.084,0.065,0.066,0.066,2.2e-05,1.6e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.8 +11290000,0.98,-0.0077,-0.012,0.18,0.0063,-0.00021,0.025,0,0,-4.9e+02,-0.00097,-0.0057,-0.00012,-0.00051,0.00012,-0.14,0.2,-2.3e-06,0.43,-9.5e-05,-4.7e-05,-0.00016,0,0,-4.9e+02,0.0017,0.0012,0.04,0.074,0.088,0.078,0.071,0.072,0.069,2.2e-05,1.5e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11390000,0.98,-0.0076,-0.012,0.18,0.0043,0.00089,0.016,0,0,-4.9e+02,-0.00098,-0.0058,-0.00012,-0.00053,3.9e-05,-0.14,0.2,-2.3e-06,0.43,-7.1e-05,-5.8e-05,-0.00018,0,0,-4.9e+02,0.0015,0.0012,0.04,0.062,0.074,0.064,0.058,0.058,0.066,1.9e-05,1.3e-05,2.2e-06,0.04,0.04,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11490000,0.98,-0.0075,-0.012,0.18,0.005,-0.00059,0.02,0,0,-4.9e+02,-0.00097,-0.0057,-0.00012,-0.00051,-4.3e-05,-0.14,0.2,-2.3e-06,0.43,-9.3e-05,-3.8e-05,-0.00017,0,0,-4.9e+02,0.0015,0.0011,0.04,0.071,0.086,0.058,0.063,0.064,0.067,1.8e-05,1.3e-05,2.2e-06,0.04,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11590000,0.98,-0.0076,-0.012,0.18,0.0045,-0.00053,0.018,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00059,5.4e-05,-0.14,0.2,-2.4e-06,0.43,-4.5e-05,-7.8e-05,-0.0002,0,0,-4.9e+02,0.0014,0.0011,0.04,0.06,0.072,0.049,0.053,0.054,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,2.9 +11690000,0.98,-0.0075,-0.012,0.18,0.0039,0.0019,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00055,0.00017,-0.14,0.2,-2.6e-06,0.43,-3.9e-05,-9.5e-05,-0.00022,0,0,-4.9e+02,0.0014,0.001,0.04,0.068,0.084,0.046,0.059,0.06,0.065,1.5e-05,1.1e-05,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11790000,0.98,-0.0075,-0.012,0.18,0.0026,0.0027,0.019,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-1.2e-05,0.00014,-0.14,0.2,-2.7e-06,0.43,-5.3e-05,-0.0001,-0.00023,0,0,-4.9e+02,0.0012,0.00096,0.039,0.058,0.07,0.039,0.05,0.051,0.062,1.3e-05,9.4e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11890000,0.98,-0.0077,-0.012,0.18,0.0037,0.0014,0.017,0,0,-4.9e+02,-0.001,-0.0059,-0.00012,-0.00021,0.0003,-0.14,0.2,-2.5e-06,0.43,-2.3e-05,-0.0001,-0.00026,0,0,-4.9e+02,0.0012,0.00096,0.039,0.066,0.082,0.036,0.056,0.058,0.063,1.2e-05,9.1e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +11990000,0.98,-0.0077,-0.012,0.18,0.0065,0.0036,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00027,0.00047,-0.14,0.2,-2.8e-06,0.43,-4.1e-05,-0.00011,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.057,0.068,0.032,0.048,0.049,0.061,1e-05,7.9e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3 +12090000,0.98,-0.0077,-0.012,0.18,0.0096,0.0012,0.017,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00045,0.00022,-0.14,0.2,-2.5e-06,0.43,-4.1e-05,-7.5e-05,-0.00026,0,0,-4.9e+02,0.0011,0.00088,0.039,0.064,0.079,0.029,0.055,0.056,0.06,1e-05,7.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12190000,0.98,-0.0077,-0.012,0.18,0.011,-4.8e-06,0.016,0,0,-4.9e+02,-0.00097,-0.0058,-0.00012,-0.0011,-0.00042,-0.14,0.2,-2.4e-06,0.43,-6.2e-06,-4e-05,-0.00028,0,0,-4.9e+02,0.00094,0.00081,0.039,0.055,0.065,0.026,0.047,0.048,0.058,8.3e-06,6.6e-06,2.2e-06,0.039,0.039,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12290000,0.98,-0.0079,-0.012,0.18,0.0079,-0.0035,0.015,0,0,-4.9e+02,-0.00096,-0.0058,-0.00013,-0.0014,-0.00025,-0.14,0.2,-2.2e-06,0.43,2e-05,-4.9e-05,-0.00028,0,0,-4.9e+02,0.00095,0.00081,0.039,0.062,0.075,0.024,0.054,0.056,0.058,8.2e-06,6.4e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12390000,0.98,-0.0079,-0.012,0.18,0.0078,-0.0047,0.013,0,0,-4.9e+02,-0.00095,-0.0058,-0.00013,-0.0017,-0.00072,-0.14,0.2,-2.2e-06,0.43,4.6e-05,-2.8e-05,-0.0003,0,0,-4.9e+02,0.00085,0.00075,0.039,0.053,0.062,0.022,0.047,0.048,0.056,6.9e-06,5.6e-06,2.2e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.1 +12490000,0.98,-0.008,-0.012,0.18,0.0094,-0.0062,0.017,0,0,-4.9e+02,-0.00093,-0.0058,-0.00013,-0.002,-0.00096,-0.14,0.2,-2.1e-06,0.43,5.3e-05,1.9e-06,-0.00031,0,0,-4.9e+02,0.00085,0.00075,0.039,0.06,0.071,0.021,0.053,0.055,0.056,6.8e-06,5.4e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12590000,0.98,-0.0081,-0.012,0.18,0.011,-0.01,0.018,0,0,-4.9e+02,-0.00092,-0.0058,-0.00013,-0.0021,-0.00083,-0.14,0.2,-1.9e-06,0.43,6.6e-05,2e-06,-0.00032,0,0,-4.9e+02,0.00077,0.0007,0.039,0.051,0.059,0.019,0.046,0.047,0.054,5.7e-06,4.7e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12690000,0.98,-0.0081,-0.012,0.18,0.012,-0.014,0.018,0,0,-4.9e+02,-0.00091,-0.0058,-0.00013,-0.0023,-0.00078,-0.14,0.2,-1.9e-06,0.43,7.5e-05,5.9e-06,-0.00033,0,0,-4.9e+02,0.00077,0.00069,0.039,0.057,0.067,0.018,0.053,0.055,0.054,5.7e-06,4.6e-06,2.1e-06,0.039,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12790000,0.98,-0.0081,-0.012,0.18,0.014,-0.012,0.02,0,0,-4.9e+02,-0.00093,-0.0058,-0.00013,-0.0016,-0.0012,-0.14,0.2,-2e-06,0.43,3.9e-05,3e-05,-0.0003,0,0,-4.9e+02,0.0007,0.00065,0.039,0.049,0.056,0.016,0.046,0.047,0.052,4.8e-06,4.1e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.2 +12890000,0.98,-0.0081,-0.012,0.18,0.015,-0.013,0.021,0,0,-4.9e+02,-0.00094,-0.0058,-0.00013,-0.0012,-0.0014,-0.14,0.2,-2e-06,0.43,1.9e-05,3.4e-05,-0.00029,0,0,-4.9e+02,0.00071,0.00065,0.039,0.055,0.063,0.016,0.052,0.054,0.052,4.8e-06,4e-06,2.1e-06,0.038,0.038,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +12990000,0.98,-0.0079,-0.012,0.18,0.013,-0.0088,0.021,0,0,-4.9e+02,-0.00098,-0.0058,-0.00012,-0.0011,-0.0015,-0.14,0.2,-2.5e-06,0.43,3.2e-05,9.3e-06,-0.00031,0,0,-4.9e+02,0.00065,0.00061,0.038,0.047,0.053,0.014,0.046,0.047,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13090000,0.98,-0.0079,-0.012,0.18,0.015,-0.0077,0.018,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00055,-0.0017,-0.14,0.2,-2.6e-06,0.43,8.6e-06,1.4e-05,-0.0003,0,0,-4.9e+02,0.00065,0.00061,0.038,0.052,0.059,0.014,0.052,0.054,0.05,4.1e-06,3.5e-06,2.1e-06,0.038,0.037,0.00055,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13190000,0.98,-0.0079,-0.012,0.18,0.0096,-0.0078,0.017,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00031,-0.0022,-0.14,0.2,-2.7e-06,0.43,3.1e-05,1.2e-05,-0.00031,0,0,-4.9e+02,0.00061,0.00058,0.038,0.044,0.05,0.013,0.046,0.047,0.048,3.6e-06,3.1e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.3 +13290000,0.98,-0.008,-0.012,0.18,0.011,-0.0095,0.015,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.00051,-0.0027,-0.14,0.2,-2.6e-06,0.43,3.9e-05,3.2e-05,-0.0003,0,0,-4.9e+02,0.00061,0.00058,0.038,0.049,0.056,0.013,0.052,0.054,0.048,3.6e-06,3e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13390000,0.98,-0.0079,-0.012,0.18,0.009,-0.0082,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.0027,-0.14,0.2,-2.8e-06,0.43,7.7e-05,3e-05,-0.00034,0,0,-4.9e+02,0.00057,0.00056,0.038,0.042,0.047,0.012,0.045,0.046,0.047,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13490000,0.98,-0.0079,-0.012,0.18,0.011,-0.0069,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.0032,-0.14,0.2,-2.8e-06,0.43,7.9e-05,5.3e-05,-0.00035,0,0,-4.9e+02,0.00058,0.00056,0.038,0.047,0.052,0.011,0.052,0.053,0.046,3.1e-06,2.7e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13590000,0.98,-0.0079,-0.012,0.18,0.014,-0.0071,0.015,0,0,-4.9e+02,-0.00099,-0.0058,-0.00012,-0.0017,-0.0033,-0.14,0.2,-2.7e-06,0.43,8.6e-05,4.5e-05,-0.00034,0,0,-4.9e+02,0.00054,0.00053,0.038,0.04,0.044,0.011,0.045,0.046,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.037,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.4 +13690000,0.98,-0.0078,-0.012,0.18,0.014,-0.0083,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0013,-0.003,-0.14,0.2,-2.7e-06,0.43,7.3e-05,2.4e-05,-0.00032,0,0,-4.9e+02,0.00055,0.00053,0.038,0.044,0.049,0.011,0.052,0.053,0.045,2.8e-06,2.4e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13790000,0.98,-0.0078,-0.012,0.18,0.019,-0.0045,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0018,-0.0024,-0.14,0.2,-3e-06,0.43,7.1e-05,2.9e-06,-0.00033,0,0,-4.9e+02,0.00052,0.00051,0.038,0.038,0.042,0.01,0.045,0.046,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13890000,0.98,-0.0076,-0.012,0.18,0.019,-0.0031,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0012,-0.0019,-0.14,0.2,-3.2e-06,0.43,4.9e-05,-1.2e-05,-0.00033,0,0,-4.9e+02,0.00052,0.00052,0.038,0.042,0.046,0.01,0.051,0.053,0.044,2.5e-06,2.2e-06,2.1e-06,0.038,0.036,0.00054,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +13990000,0.98,-0.0076,-0.012,0.18,0.019,-0.0012,0.014,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.00062,-0.0024,-0.14,0.2,-3.1e-06,0.43,5.6e-05,-1.5e-05,-0.0003,0,0,-4.9e+02,0.0005,0.0005,0.038,0.036,0.039,0.0097,0.045,0.046,0.043,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.5 +14090000,0.98,-0.0077,-0.011,0.18,0.017,-0.0084,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0024,-0.0022,-0.14,0.2,-3e-06,0.43,0.00011,-6.9e-06,-0.00034,0,0,-4.9e+02,0.0005,0.0005,0.038,0.04,0.043,0.0096,0.051,0.053,0.042,2.3e-06,2e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14190000,0.98,-0.0078,-0.011,0.18,0.016,-0.0078,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0035,-0.0033,-0.14,0.2,-2.8e-06,0.43,0.00016,1.8e-05,-0.00036,0,0,-4.9e+02,0.00048,0.00048,0.038,0.034,0.037,0.0094,0.045,0.046,0.042,2.1e-06,1.9e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14290000,0.98,-0.0078,-0.011,0.18,0.018,-0.0084,0.013,0,0,-4.9e+02,-0.00099,-0.0058,-0.00014,-0.0036,-0.0035,-0.14,0.2,-2.7e-06,0.43,0.00017,2.1e-05,-0.00036,0,0,-4.9e+02,0.00048,0.00049,0.038,0.038,0.041,0.0092,0.051,0.052,0.041,2.1e-06,1.8e-06,2.1e-06,0.037,0.036,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14390000,0.98,-0.0078,-0.011,0.18,0.018,-0.0098,0.015,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0033,-0.0043,-0.14,0.2,-2.6e-06,0.43,0.00017,2.6e-05,-0.00034,0,0,-4.9e+02,0.00047,0.00047,0.038,0.033,0.035,0.009,0.045,0.046,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00053,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.6 +14490000,0.98,-0.008,-0.011,0.18,0.018,-0.011,0.018,0,0,-4.9e+02,-0.00098,-0.0058,-0.00014,-0.0044,-0.0044,-0.14,0.2,-2.5e-06,0.43,0.0002,3.4e-05,-0.00036,0,0,-4.9e+02,0.00047,0.00047,0.038,0.036,0.038,0.009,0.051,0.052,0.04,1.9e-06,1.7e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14590000,0.98,-0.0081,-0.011,0.18,0.017,-0.011,0.016,0,0,-4.9e+02,-0.00098,-0.0058,-0.00014,-0.0049,-0.0057,-0.14,0.2,-2.4e-06,0.43,0.00025,5.2e-05,-0.00037,0,0,-4.9e+02,0.00046,0.00046,0.038,0.031,0.033,0.0088,0.045,0.045,0.04,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14690000,0.98,-0.0082,-0.011,0.18,0.017,-0.011,0.016,0,0,-4.9e+02,-0.00098,-0.0058,-0.00014,-0.0048,-0.0061,-0.14,0.2,-2.3e-06,0.43,0.00025,5.9e-05,-0.00036,0,0,-4.9e+02,0.00046,0.00046,0.038,0.034,0.036,0.0088,0.05,0.051,0.039,1.8e-06,1.6e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14790000,0.98,-0.0082,-0.011,0.18,0.019,-0.0038,0.016,0,0,-4.9e+02,-0.001,-0.0058,-0.00014,-0.0041,-0.0075,-0.14,0.2,-2.4e-06,0.43,0.00025,5.7e-05,-0.00034,0,0,-4.9e+02,0.00044,0.00045,0.038,0.03,0.031,0.0086,0.044,0.045,0.039,1.7e-06,1.5e-06,2.1e-06,0.037,0.035,0.00052,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.7 +14890000,0.98,-0.0082,-0.011,0.18,0.021,-0.006,0.02,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0036,-0.0091,-0.14,0.2,-2e-06,0.43,0.00025,7.8e-05,-0.0003,0,0,-4.9e+02,0.00045,0.00045,0.038,0.032,0.034,0.0087,0.05,0.051,0.039,1.6e-06,1.5e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +14990000,0.98,-0.0084,-0.011,0.18,0.02,-0.0062,0.022,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0035,-0.011,-0.14,0.2,-1.8e-06,0.43,0.00027,0.00011,-0.00028,0,0,-4.9e+02,0.00044,0.00045,0.038,0.028,0.03,0.0085,0.044,0.045,0.038,1.6e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15090000,0.98,-0.0083,-0.011,0.18,0.021,-0.0055,0.026,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0036,-0.011,-0.14,0.2,-1.8e-06,0.43,0.00028,0.00011,-0.00028,0,0,-4.9e+02,0.00044,0.00045,0.038,0.031,0.032,0.0085,0.05,0.051,0.038,1.5e-06,1.4e-06,2.1e-06,0.037,0.035,0.00051,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15190000,0.98,-0.0085,-0.011,0.18,0.019,-0.0064,0.027,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0041,-0.012,-0.14,0.2,-1.6e-06,0.43,0.00031,0.00012,-0.00028,0,0,-4.9e+02,0.00043,0.00044,0.038,0.027,0.028,0.0085,0.044,0.045,0.038,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.8 +15290000,0.98,-0.0087,-0.012,0.18,0.022,-0.0071,0.026,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0035,-0.013,-0.14,0.2,-1.5e-06,0.43,0.00031,0.00015,-0.00026,0,0,-4.9e+02,0.00043,0.00044,0.038,0.029,0.031,0.0085,0.049,0.05,0.037,1.5e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15390000,0.98,-0.0088,-0.012,0.18,0.021,-0.0046,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0038,-0.014,-0.14,0.2,-1.6e-06,0.43,0.00034,0.00017,-0.00028,0,0,-4.9e+02,0.00042,0.00043,0.038,0.026,0.027,0.0084,0.044,0.044,0.037,1.4e-06,1.3e-06,2.1e-06,0.036,0.034,0.0005,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15490000,0.98,-0.0088,-0.012,0.18,0.022,-0.0074,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0038,-0.014,-0.14,0.2,-1.6e-06,0.43,0.00034,0.00017,-0.00028,0,0,-4.9e+02,0.00042,0.00043,0.038,0.028,0.029,0.0085,0.049,0.05,0.037,1.4e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15590000,0.98,-0.0089,-0.011,0.18,0.023,-0.0088,0.024,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0034,-0.014,-0.14,0.2,-1.6e-06,0.43,0.00034,0.00017,-0.00027,0,0,-4.9e+02,0.00041,0.00043,0.038,0.024,0.026,0.0084,0.043,0.044,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,3.9 +15690000,0.98,-0.0087,-0.011,0.18,0.024,-0.011,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0023,-0.012,-0.14,0.2,-2e-06,0.43,0.00029,0.00011,-0.00025,0,0,-4.9e+02,0.00041,0.00043,0.038,0.026,0.028,0.0085,0.049,0.049,0.037,1.3e-06,1.2e-06,2.1e-06,0.036,0.034,0.00049,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15790000,0.98,-0.0086,-0.011,0.18,0.019,-0.01,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00011,-0.0012,-0.011,-0.14,0.2,-2.4e-06,0.43,0.00027,0.0001,-0.00024,0,0,-4.9e+02,0.00041,0.00042,0.038,0.023,0.025,0.0084,0.043,0.044,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15890000,0.98,-0.0088,-0.011,0.18,0.02,-0.0093,0.025,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0022,-0.012,-0.14,0.2,-2.2e-06,0.43,0.00031,0.00013,-0.00026,0,0,-4.9e+02,0.00041,0.00042,0.038,0.025,0.027,0.0085,0.048,0.049,0.036,1.3e-06,1.1e-06,2.1e-06,0.036,0.034,0.00048,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +15990000,0.98,-0.0087,-0.011,0.18,0.018,-0.0083,0.022,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0025,-0.013,-0.14,0.2,-2.4e-06,0.43,0.00035,0.00017,-0.00028,0,0,-4.9e+02,0.0004,0.00042,0.038,0.022,0.024,0.0084,0.043,0.043,0.036,1.2e-06,1.1e-06,2.1e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4 +16090000,0.98,-0.0087,-0.011,0.18,0.019,-0.01,0.02,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0031,-0.014,-0.14,0.2,-2.3e-06,0.43,0.00037,0.00019,-0.00029,0,0,-4.9e+02,0.0004,0.00042,0.038,0.024,0.025,0.0085,0.048,0.049,0.036,1.2e-06,1.1e-06,2e-06,0.036,0.033,0.00047,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16190000,0.98,-0.0087,-0.011,0.18,0.015,-0.0085,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0037,-0.015,-0.13,0.2,-2.3e-06,0.43,0.00041,0.00019,-0.00031,0,0,-4.9e+02,0.0004,0.00041,0.038,0.021,0.023,0.0084,0.043,0.043,0.036,1.2e-06,1e-06,2e-06,0.036,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16290000,0.98,-0.0087,-0.011,0.18,0.017,-0.01,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00013,-0.0034,-0.014,-0.13,0.2,-2.3e-06,0.43,0.0004,0.00018,-0.0003,0,0,-4.9e+02,0.0004,0.00041,0.038,0.023,0.024,0.0085,0.047,0.048,0.036,1.2e-06,1e-06,2e-06,0.035,0.033,0.00046,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16390000,0.98,-0.0088,-0.011,0.18,0.019,-0.009,0.018,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0029,-0.017,-0.13,0.2,-2.1e-06,0.43,0.00041,0.00022,-0.00028,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.022,0.0084,0.042,0.043,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.1 +16490000,0.98,-0.0089,-0.011,0.18,0.022,-0.011,0.021,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0033,-0.017,-0.13,0.2,-1.9e-06,0.43,0.00042,0.00023,-0.00028,0,0,-4.9e+02,0.00039,0.00041,0.038,0.022,0.023,0.0085,0.047,0.048,0.036,1.1e-06,1e-06,2e-06,0.035,0.033,0.00045,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16590000,0.98,-0.0089,-0.011,0.18,0.026,-0.011,0.024,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0031,-0.017,-0.13,0.2,-2.1e-06,0.43,0.00044,0.00023,-0.00029,0,0,-4.9e+02,0.00039,0.00041,0.038,0.02,0.021,0.0084,0.042,0.043,0.036,1.1e-06,9.7e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16690000,0.98,-0.0089,-0.011,0.18,0.028,-0.016,0.024,0,0,-4.9e+02,-0.001,-0.0058,-0.00012,-0.0025,-0.017,-0.13,0.2,-2.3e-06,0.43,0.00042,0.00022,-0.00028,0,0,-4.9e+02,0.00039,0.00041,0.038,0.021,0.023,0.0085,0.047,0.047,0.036,1.1e-06,9.6e-07,2e-06,0.035,0.033,0.00044,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16790000,0.98,-0.0088,-0.011,0.18,0.028,-0.015,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.002,-0.017,-0.13,0.2,-2.6e-06,0.43,0.00042,0.00022,-0.00028,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.042,0.042,0.036,1e-06,9.3e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.2 +16890000,0.98,-0.0087,-0.011,0.18,0.028,-0.015,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.00087,-0.016,-0.13,0.2,-2.7e-06,0.43,0.00039,0.00021,-0.00026,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.022,0.0085,0.046,0.047,0.036,1e-06,9.2e-07,2e-06,0.035,0.032,0.00043,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +16990000,0.98,-0.0088,-0.011,0.18,0.026,-0.015,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.00051,-0.017,-0.13,0.2,-2.7e-06,0.43,0.0004,0.00023,-0.00026,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,1e-06,9e-07,2e-06,0.035,0.032,0.00042,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17090000,0.98,-0.009,-0.011,0.18,0.03,-0.018,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,-0.0009,-0.019,-0.13,0.2,-2.8e-06,0.43,0.00044,0.0003,-0.00027,0,0,-4.9e+02,0.00038,0.0004,0.038,0.02,0.021,0.0085,0.046,0.047,0.035,9.9e-07,8.9e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17190000,0.98,-0.0091,-0.011,0.18,0.028,-0.023,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0018,-0.019,-0.13,0.2,-3.3e-06,0.43,0.00048,0.00031,-0.00031,0,0,-4.9e+02,0.00038,0.0004,0.038,0.018,0.019,0.0084,0.041,0.042,0.035,9.6e-07,8.7e-07,2e-06,0.035,0.032,0.00041,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.3 +17290000,0.98,-0.0091,-0.011,0.18,0.031,-0.024,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0025,-0.019,-0.13,0.2,-3.3e-06,0.43,0.0005,0.00032,-0.00033,0,0,-4.9e+02,0.00038,0.0004,0.038,0.019,0.02,0.0085,0.045,0.046,0.036,9.6e-07,8.6e-07,2e-06,0.035,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17390000,0.98,-0.009,-0.011,0.18,0.025,-0.025,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0011,-0.018,-0.13,0.2,-3.6e-06,0.43,0.00048,0.00032,-0.00032,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.041,0.041,0.035,9.3e-07,8.4e-07,2e-06,0.034,0.032,0.0004,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17490000,0.98,-0.009,-0.011,0.18,0.024,-0.026,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.0014,-0.018,-0.13,0.2,-3.6e-06,0.43,0.00049,0.00032,-0.00032,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.02,0.0085,0.045,0.046,0.036,9.3e-07,8.3e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17590000,0.98,-0.009,-0.011,0.18,0.021,-0.023,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00091,-0.019,-0.13,0.2,-4.2e-06,0.43,0.0005,0.00032,-0.00033,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.018,0.0084,0.04,0.041,0.035,9e-07,8.1e-07,2e-06,0.034,0.032,0.00039,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.4 +17690000,0.98,-0.0091,-0.011,0.18,0.022,-0.024,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,-0.00059,-0.019,-0.13,0.2,-4e-06,0.43,0.00048,0.0003,-0.00031,0,0,-4.9e+02,0.00037,0.00039,0.038,0.018,0.019,0.0084,0.045,0.045,0.035,9e-07,8e-07,2e-06,0.034,0.031,0.00038,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17790000,0.98,-0.0091,-0.011,0.18,0.023,-0.023,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,0.00019,-0.019,-0.13,0.2,-3.9e-06,0.43,0.00046,0.00027,-0.00029,0,0,-4.9e+02,0.00037,0.00039,0.038,0.016,0.017,0.0084,0.04,0.041,0.036,8.8e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17890000,0.98,-0.009,-0.011,0.18,0.026,-0.025,0.025,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,0.0008,-0.018,-0.13,0.2,-3.9e-06,0.43,0.00043,0.00025,-0.00027,0,0,-4.9e+02,0.00037,0.00039,0.038,0.017,0.019,0.0084,0.044,0.045,0.036,8.7e-07,7.8e-07,2e-06,0.034,0.031,0.00037,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +17990000,0.98,-0.009,-0.011,0.18,0.025,-0.021,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,0.0016,-0.02,-0.13,0.2,-4e-06,0.43,0.00043,0.00025,-0.00026,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0083,0.04,0.041,0.035,8.5e-07,7.6e-07,2e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.5 +18090000,0.98,-0.0091,-0.011,0.18,0.026,-0.023,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00012,0.0017,-0.02,-0.13,0.2,-3.8e-06,0.43,0.00042,0.00024,-0.00025,0,0,-4.9e+02,0.00036,0.00038,0.038,0.017,0.018,0.0083,0.044,0.045,0.036,8.5e-07,7.5e-07,1.9e-06,0.034,0.031,0.00036,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18190000,0.98,-0.0091,-0.011,0.18,0.025,-0.021,0.024,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,0.0033,-0.021,-0.13,0.2,-4.1e-06,0.43,0.0004,0.00027,-0.00023,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0082,0.04,0.04,0.035,8.2e-07,7.4e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18290000,0.98,-0.0092,-0.011,0.18,0.028,-0.022,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.0001,0.0035,-0.022,-0.13,0.2,-3.8e-06,0.43,0.0004,0.00028,-0.00022,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.018,0.0082,0.044,0.044,0.036,8.2e-07,7.3e-07,1.9e-06,0.034,0.031,0.00035,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18390000,0.98,-0.0091,-0.011,0.18,0.027,-0.02,0.023,0,0,-4.9e+02,-0.0011,-0.0058,-0.00011,0.0038,-0.022,-0.13,0.2,-4.2e-06,0.43,0.00041,0.00029,-0.00023,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.6 +18490000,0.98,-0.0092,-0.011,0.18,0.031,-0.021,0.022,0,0,-4.9e+02,-0.0011,-0.0058,-0.0001,0.0044,-0.022,-0.13,0.2,-4.1e-06,0.43,0.00039,0.00029,-0.00021,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0082,0.043,0.044,0.036,8e-07,7.1e-07,1.9e-06,0.033,0.031,0.00034,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18590000,0.98,-0.0089,-0.011,0.18,0.028,-0.02,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0047,-0.021,-0.13,0.2,-4.6e-06,0.43,0.00039,0.00026,-0.00022,0,0,-4.9e+02,0.00036,0.00038,0.038,0.015,0.016,0.0081,0.039,0.04,0.035,7.8e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18690000,0.98,-0.0088,-0.011,0.18,0.028,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.7e-05,0.0057,-0.02,-0.13,0.2,-4.7e-06,0.43,0.00035,0.00023,-0.0002,0,0,-4.9e+02,0.00036,0.00038,0.038,0.016,0.017,0.0081,0.043,0.044,0.035,7.7e-07,6.9e-07,1.9e-06,0.033,0.03,0.00033,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18790000,0.98,-0.0088,-0.011,0.18,0.026,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.8e-05,0.0062,-0.02,-0.13,0.2,-5.1e-06,0.43,0.00035,0.00024,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.008,0.039,0.04,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00032,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.7 +18890000,0.98,-0.0088,-0.011,0.18,0.027,-0.019,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-9.9e-05,0.006,-0.02,-0.13,0.2,-5.2e-06,0.43,0.00037,0.00026,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.017,0.008,0.043,0.044,0.036,7.5e-07,6.7e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +18990000,0.98,-0.0087,-0.011,0.18,0.024,-0.019,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0068,-0.019,-0.13,0.2,-5.8e-06,0.43,0.00036,0.00025,-0.00022,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0079,0.039,0.039,0.035,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.00031,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19090000,0.98,-0.0088,-0.011,0.18,0.023,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-9.3e-05,0.0074,-0.019,-0.13,0.2,-5.7e-06,0.43,0.00035,0.00025,-0.00021,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.008,0.042,0.043,0.036,7.3e-07,6.5e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19190000,0.98,-0.0087,-0.011,0.18,0.02,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.0003,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.8 +19290000,0.98,-0.0087,-0.011,0.18,0.021,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0073,-0.019,-0.13,0.2,-6.4e-06,0.43,0.00037,0.00027,-0.00024,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,7.1e-07,6.3e-07,1.9e-06,0.033,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19390000,0.98,-0.0087,-0.011,0.18,0.019,-0.017,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0079,-0.019,-0.13,0.2,-6.9e-06,0.43,0.00036,0.00026,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.014,0.015,0.0078,0.038,0.039,0.035,6.9e-07,6.2e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19490000,0.98,-0.0089,-0.011,0.18,0.021,-0.019,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0073,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00037,0.00024,-0.00023,0,0,-4.9e+02,0.00035,0.00037,0.038,0.015,0.016,0.0078,0.042,0.043,0.035,6.9e-07,6.1e-07,1.9e-06,0.032,0.03,0.00029,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19590000,0.98,-0.0088,-0.011,0.18,0.018,-0.02,0.023,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0076,-0.02,-0.13,0.2,-6.8e-06,0.43,0.00037,0.00024,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.015,0.0076,0.038,0.039,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,4.9 +19690000,0.98,-0.0088,-0.011,0.18,0.018,-0.019,0.022,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.0081,-0.021,-0.13,0.2,-6.5e-06,0.43,0.00036,0.00024,-0.00022,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.016,0.0076,0.042,0.043,0.035,6.7e-07,6e-07,1.8e-06,0.032,0.029,0.00028,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19790000,0.98,-0.0089,-0.011,0.18,0.016,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0082,-0.021,-0.13,0.2,-7e-06,0.43,0.00036,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0076,0.038,0.039,0.035,6.6e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19890000,0.98,-0.0089,-0.011,0.18,0.017,-0.018,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0082,-0.021,-0.13,0.2,-7e-06,0.43,0.00036,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.5e-07,5.8e-07,1.8e-06,0.032,0.029,0.00027,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +19990000,0.98,-0.0089,-0.012,0.18,0.015,-0.018,0.018,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0092,-0.021,-0.13,0.2,-7.5e-06,0.43,0.00035,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.038,0.038,0.035,6.4e-07,5.7e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5 +20090000,0.98,-0.0089,-0.012,0.18,0.017,-0.021,0.019,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.0093,-0.021,-0.13,0.2,-7.5e-06,0.43,0.00035,0.00026,-0.00023,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0075,0.041,0.042,0.035,6.4e-07,5.6e-07,1.8e-06,0.032,0.029,0.00026,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,5.1 +20190000,0.98,-0.009,-0.012,0.18,0.017,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0094,-0.022,-0.13,0.2,-8e-06,0.43,0.00035,0.00027,-0.00025,0,0,-4.9e+02,0.00034,0.00036,0.038,0.013,0.014,0.0074,0.037,0.038,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20290000,0.98,-0.009,-0.012,0.18,0.015,-0.02,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00012,0.0094,-0.022,-0.13,0.2,-8e-06,0.43,0.00036,0.00028,-0.00025,0,0,-4.9e+02,0.00034,0.00036,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6.2e-07,5.5e-07,1.8e-06,0.032,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20390000,0.98,-0.0089,-0.012,0.18,0.013,-0.017,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.01,-0.022,-0.13,0.2,-8.2e-06,0.43,0.00034,0.00026,-0.00024,0,0,-4.9e+02,0.00034,0.00035,0.038,0.013,0.014,0.0073,0.037,0.038,0.035,6.1e-07,5.4e-07,1.8e-06,0.031,0.029,0.00025,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20490000,0.98,-0.0089,-0.012,0.18,0.0096,-0.019,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.011,-0.022,-0.13,0.2,-8e-06,0.43,0.00034,0.00025,-0.00023,0,0,-4.9e+02,0.00034,0.00035,0.038,0.014,0.015,0.0073,0.041,0.042,0.035,6e-07,5.3e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20590000,0.98,-0.0089,-0.012,0.18,0.0088,-0.018,0.02,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.012,-0.022,-0.13,0.2,-7.9e-06,0.43,0.00032,0.00023,-0.00022,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0072,0.037,0.038,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20690000,0.98,-0.0088,-0.012,0.18,0.0078,-0.018,0.021,0,0,-4.9e+02,-0.0012,-0.0058,-0.00011,0.012,-0.022,-0.13,0.2,-8.1e-06,0.43,0.00032,0.00023,-0.00022,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0072,0.041,0.042,0.035,5.9e-07,5.2e-07,1.8e-06,0.031,0.029,0.00024,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20790000,0.98,-0.0082,-0.012,0.18,0.0039,-0.015,0.006,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.012,-0.022,-0.13,0.2,-8.5e-06,0.43,0.0003,0.00023,-0.00022,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.0071,0.037,0.038,0.035,5.8e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20890000,0.98,0.00095,-0.0079,0.18,1.9e-05,-0.0038,-0.11,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.013,-0.023,-0.13,0.2,-9.4e-06,0.43,0.0003,0.00035,-0.00017,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.015,0.0071,0.04,0.041,0.035,5.7e-07,5.1e-07,1.8e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0013,0.0013,0.0015,0.0013,1,1,0.01 +20990000,0.98,0.0043,-0.0044,0.18,-0.012,0.015,-0.25,0,0,-4.9e+02,-0.0012,-0.0058,-0.0001,0.013,-0.023,-0.13,0.2,-9.4e-06,0.43,0.0003,0.00034,-0.00013,0,0,-4.9e+02,0.00033,0.00035,0.038,0.013,0.014,0.007,0.037,0.038,0.034,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00023,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21090000,0.98,0.0027,-0.0048,0.18,-0.023,0.031,-0.37,0,0,-4.9e+02,-0.0012,-0.0058,-9.9e-05,0.013,-0.023,-0.13,0.2,-7.6e-06,0.43,0.00035,0.00014,-0.00018,0,0,-4.9e+02,0.00033,0.00035,0.038,0.014,0.015,0.007,0.04,0.041,0.035,5.6e-07,5e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21190000,0.98,-9.1e-05,-0.0064,0.18,-0.029,0.037,-0.5,0,0,-4.9e+02,-0.0012,-0.0058,-9.1e-05,0.013,-0.024,-0.13,0.2,-7e-06,0.43,0.00036,0.00016,-0.00016,0,0,-4.9e+02,0.00033,0.00034,0.038,0.013,0.014,0.0069,0.037,0.038,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21290000,0.98,-0.0023,-0.0077,0.18,-0.028,0.039,-0.63,0,0,-4.9e+02,-0.0012,-0.0058,-9.9e-05,0.012,-0.024,-0.13,0.2,-7.7e-06,0.43,0.00034,0.0002,-9.5e-05,0,0,-4.9e+02,0.00033,0.00034,0.037,0.014,0.015,0.0069,0.04,0.041,0.034,5.5e-07,4.8e-07,1.7e-06,0.031,0.028,0.00022,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21390000,0.98,-0.0037,-0.0083,0.18,-0.026,0.036,-0.75,0,0,-4.9e+02,-0.0012,-0.0058,-9.7e-05,0.012,-0.024,-0.13,0.2,-7.2e-06,0.43,0.00036,0.00013,-0.00011,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.014,0.0068,0.037,0.038,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21490000,0.98,-0.0045,-0.0088,0.18,-0.021,0.033,-0.89,0,0,-4.9e+02,-0.0012,-0.0058,-9.1e-05,0.013,-0.024,-0.13,0.2,-7.2e-06,0.43,0.00035,0.00015,-0.00016,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0068,0.04,0.041,0.034,5.3e-07,4.7e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21590000,0.98,-0.0049,-0.0088,0.18,-0.014,0.03,-1,0,0,-4.9e+02,-0.0012,-0.0058,-8.5e-05,0.013,-0.024,-0.13,0.2,-7.3e-06,0.43,0.00034,0.00021,-0.00017,0,0,-4.9e+02,0.00032,0.00034,0.037,0.013,0.015,0.0067,0.037,0.038,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.00021,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21690000,0.98,-0.0052,-0.0086,0.18,-0.011,0.025,-1.1,0,0,-4.9e+02,-0.0012,-0.0058,-7.8e-05,0.014,-0.024,-0.13,0.2,-7.2e-06,0.43,0.00031,0.00025,-0.00016,0,0,-4.9e+02,0.00032,0.00034,0.037,0.014,0.016,0.0067,0.04,0.041,0.034,5.2e-07,4.6e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21790000,0.98,-0.0055,-0.0088,0.18,-0.0062,0.021,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-6.9e-05,0.015,-0.023,-0.13,0.2,-7.2e-06,0.43,0.00031,0.00024,-0.00021,0,0,-4.9e+02,0.00032,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21890000,0.98,-0.0058,-0.0089,0.18,-0.0025,0.016,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-7.1e-05,0.015,-0.023,-0.13,0.2,-7.6e-06,0.43,0.00028,0.00026,-0.0002,0,0,-4.9e+02,0.00032,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,5e-07,4.5e-07,1.7e-06,0.03,0.028,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +21990000,0.98,-0.0064,-0.0092,0.18,-0.00048,0.011,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-6.6e-05,0.015,-0.022,-0.13,0.2,-7.3e-06,0.43,0.00031,0.00027,-0.00022,0,0,-4.9e+02,0.00031,0.00033,0.037,0.013,0.015,0.0066,0.037,0.038,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.0002,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22090000,0.98,-0.0071,-0.01,0.18,0.0015,0.0077,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.7e-05,0.015,-0.022,-0.13,0.2,-7.2e-06,0.43,0.00031,0.00029,-0.00023,0,0,-4.9e+02,0.00031,0.00033,0.037,0.014,0.016,0.0066,0.04,0.041,0.034,4.9e-07,4.4e-07,1.6e-06,0.03,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22190000,0.98,-0.0075,-0.01,0.18,0.0073,0.0034,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.9e-05,0.016,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00031,0.00031,-0.00023,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0065,0.037,0.038,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22290000,0.98,-0.0082,-0.01,0.18,0.013,-0.0024,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.016,-0.021,-0.13,0.2,-7.3e-06,0.43,0.00031,0.0003,-0.00022,0,0,-4.9e+02,0.00031,0.00032,0.037,0.014,0.015,0.0065,0.04,0.041,0.034,4.8e-07,4.3e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22390000,0.98,-0.0085,-0.011,0.18,0.017,-0.011,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.3e-05,0.015,-0.02,-0.13,0.2,-7.4e-06,0.43,0.00032,0.00031,-0.00024,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.014,0.0064,0.037,0.038,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00019,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22490000,0.98,-0.0087,-0.011,0.18,0.022,-0.018,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.7e-05,0.014,-0.02,-0.13,0.2,-7.8e-06,0.43,0.00033,0.00033,-0.00026,0,0,-4.9e+02,0.00031,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.7e-07,4.2e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22590000,0.98,-0.0086,-0.012,0.18,0.029,-0.025,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.9e-05,0.015,-0.02,-0.13,0.2,-7.3e-06,0.43,0.00033,0.00034,-0.00025,0,0,-4.9e+02,0.0003,0.00032,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22690000,0.98,-0.0085,-0.012,0.18,0.032,-0.03,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.015,-0.021,-0.13,0.2,-7.3e-06,0.43,0.00033,0.00035,-0.00027,0,0,-4.9e+02,0.0003,0.00032,0.037,0.013,0.015,0.0064,0.04,0.041,0.034,4.5e-07,4.1e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22790000,0.98,-0.0085,-0.012,0.18,0.038,-0.038,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.5e-05,0.014,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00031,0.00031,-0.00029,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0063,0.036,0.038,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22890000,0.98,-0.0087,-0.013,0.18,0.042,-0.043,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.6e-05,0.015,-0.021,-0.13,0.2,-7.1e-06,0.43,0.00032,0.00032,-0.00028,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.015,0.0063,0.04,0.041,0.033,4.4e-07,4e-07,1.6e-06,0.029,0.027,0.00018,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +22990000,0.98,-0.0086,-0.013,0.18,0.046,-0.047,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.8e-05,0.015,-0.02,-0.13,0.2,-7.5e-06,0.43,0.00032,0.0003,-0.00025,0,0,-4.9e+02,0.0003,0.00031,0.037,0.012,0.014,0.0062,0.036,0.038,0.033,4.3e-07,3.9e-07,1.6e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23090000,0.98,-0.0086,-0.013,0.18,0.051,-0.052,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.6e-05,0.015,-0.02,-0.13,0.2,-7.5e-06,0.43,0.0003,0.0003,-0.00023,0,0,-4.9e+02,0.0003,0.00031,0.037,0.013,0.014,0.0062,0.04,0.041,0.033,4.3e-07,3.9e-07,1.5e-06,0.029,0.027,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23190000,0.98,-0.0086,-0.014,0.18,0.057,-0.053,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4e-05,0.015,-0.019,-0.13,0.2,-8.3e-06,0.43,0.00028,0.00028,-0.00023,0,0,-4.9e+02,0.00029,0.00031,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23290000,0.98,-0.0091,-0.014,0.18,0.062,-0.059,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.015,-0.02,-0.13,0.2,-8.3e-06,0.43,0.00025,0.00032,-0.00023,0,0,-4.9e+02,0.00029,0.00031,0.037,0.013,0.014,0.0062,0.039,0.041,0.033,4.2e-07,3.8e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23390000,0.98,-0.009,-0.014,0.18,0.067,-0.062,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-5.1e-05,0.016,-0.019,-0.13,0.2,-8.5e-06,0.43,0.00026,0.00029,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.0061,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00017,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23490000,0.98,-0.0091,-0.014,0.18,0.072,-0.064,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-4.3e-05,0.016,-0.02,-0.13,0.2,-8.3e-06,0.43,0.00024,0.00035,-0.00026,0,0,-4.9e+02,0.00029,0.0003,0.037,0.013,0.014,0.0061,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23590000,0.98,-0.0093,-0.014,0.18,0.075,-0.066,-1.4,0,0,-4.9e+02,-0.0013,-0.0058,-3.9e-05,0.016,-0.019,-0.13,0.2,-9.2e-06,0.43,0.0002,0.00032,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.036,0.037,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23690000,0.98,-0.01,-0.015,0.18,0.074,-0.068,-1.3,0,0,-4.9e+02,-0.0013,-0.0058,-3.1e-05,0.016,-0.019,-0.13,0.2,-9e-06,0.43,0.00018,0.00034,-0.00022,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.014,0.006,0.039,0.041,0.033,4.1e-07,3.7e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23790000,0.98,-0.012,-0.018,0.18,0.068,-0.064,-0.95,0,0,-4.9e+02,-0.0013,-0.0058,-2.9e-05,0.018,-0.019,-0.13,0.2,-8.6e-06,0.43,0.00016,0.00038,-0.0002,0,0,-4.9e+02,0.00029,0.0003,0.037,0.011,0.013,0.006,0.036,0.037,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23890000,0.98,-0.015,-0.022,0.18,0.064,-0.064,-0.52,0,0,-4.9e+02,-0.0013,-0.0058,-2.7e-05,0.018,-0.02,-0.13,0.2,-8.7e-06,0.43,0.00015,0.0004,-0.00023,0,0,-4.9e+02,0.00029,0.0003,0.037,0.012,0.013,0.006,0.039,0.041,0.033,4e-07,3.6e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +23990000,0.98,-0.017,-0.024,0.18,0.063,-0.063,-0.13,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.019,-0.019,-0.13,0.2,-8.7e-06,0.43,0.00011,0.00042,2.4e-05,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.033,3.9e-07,3.5e-07,1.5e-06,0.028,0.026,0.00016,0.0013,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24090000,0.98,-0.017,-0.023,0.18,0.07,-0.071,0.099,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.019,-0.019,-0.13,0.2,-8.7e-06,0.43,0.00013,0.00039,2.6e-05,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.9e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24190000,0.98,-0.014,-0.019,0.18,0.08,-0.076,0.089,0,0,-4.9e+02,-0.0013,-0.0058,-3.7e-05,0.02,-0.019,-0.13,0.2,-8.3e-06,0.43,0.00012,0.00036,0.00013,0,0,-4.9e+02,0.00028,0.0003,0.037,0.011,0.012,0.0059,0.036,0.037,0.032,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24290000,0.98,-0.012,-0.016,0.18,0.084,-0.08,0.067,0,0,-4.9e+02,-0.0013,-0.0058,-3.5e-05,0.02,-0.019,-0.13,0.2,-7.9e-06,0.43,0.00016,0.00031,0.00012,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0059,0.039,0.04,0.033,3.8e-07,3.5e-07,1.4e-06,0.028,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24390000,0.98,-0.011,-0.015,0.18,0.077,-0.074,0.083,0,0,-4.9e+02,-0.0013,-0.0058,-3.4e-05,0.022,-0.02,-0.13,0.2,-6.5e-06,0.43,0.00015,0.00036,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24490000,0.98,-0.011,-0.015,0.18,0.073,-0.071,0.081,0,0,-4.9e+02,-0.0013,-0.0058,-2e-05,0.023,-0.02,-0.13,0.2,-6.5e-06,0.43,7.6e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.0003,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.8e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24590000,0.98,-0.012,-0.015,0.18,0.069,-0.067,0.077,0,0,-4.9e+02,-0.0013,-0.0058,-3.2e-05,0.024,-0.021,-0.13,0.2,-5.2e-06,0.43,0.00012,0.00044,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.037,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24690000,0.98,-0.012,-0.015,0.18,0.067,-0.067,0.076,0,0,-4.9e+02,-0.0013,-0.0058,-3e-05,0.024,-0.021,-0.13,0.2,-5.4e-06,0.43,0.0001,0.00047,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.4e-07,1.4e-06,0.027,0.026,0.00015,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24790000,0.98,-0.012,-0.015,0.18,0.064,-0.065,0.068,0,0,-4.9e+02,-0.0013,-0.0058,-3.8e-05,0.026,-0.022,-0.13,0.2,-4.8e-06,0.43,9.8e-05,0.00047,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0058,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24890000,0.98,-0.012,-0.014,0.18,0.063,-0.068,0.057,0,0,-4.9e+02,-0.0013,-0.0058,-3.3e-05,0.026,-0.022,-0.13,0.2,-4.6e-06,0.43,9.6e-05,0.00048,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0058,0.038,0.04,0.032,3.7e-07,3.3e-07,1.4e-06,0.027,0.026,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +24990000,0.98,-0.012,-0.014,0.18,0.054,-0.065,0.05,0,0,-4.9e+02,-0.0014,-0.0058,-4.7e-05,0.027,-0.023,-0.13,0.2,-4.8e-06,0.43,6e-05,0.00059,0.00027,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25090000,0.98,-0.012,-0.014,0.18,0.051,-0.064,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-4.7e-05,0.028,-0.023,-0.13,0.2,-5.4e-06,0.43,4.1e-05,0.00066,0.00031,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.6e-07,3.3e-07,1.4e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25190000,0.98,-0.012,-0.014,0.18,0.045,-0.058,0.048,0,0,-4.9e+02,-0.0014,-0.0058,-6.5e-05,0.029,-0.024,-0.13,0.2,-5.4e-06,0.43,1.9e-05,0.00069,0.0003,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25290000,0.98,-0.012,-0.013,0.18,0.041,-0.06,0.043,0,0,-4.9e+02,-0.0014,-0.0058,-7.2e-05,0.029,-0.024,-0.13,0.2,-6e-06,0.43,1e-05,0.00072,0.00027,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25390000,0.98,-0.012,-0.013,0.18,0.032,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-8.8e-05,0.031,-0.025,-0.13,0.2,-6.8e-06,0.43,-3e-05,0.00077,0.00029,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0057,0.035,0.036,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25490000,0.98,-0.013,-0.013,0.18,0.028,-0.054,0.041,0,0,-4.9e+02,-0.0014,-0.0058,-9e-05,0.031,-0.025,-0.13,0.2,-6.6e-06,0.43,-3.6e-05,0.00073,0.00026,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0057,0.038,0.04,0.032,3.5e-07,3.2e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25590000,0.98,-0.013,-0.013,0.18,0.023,-0.05,0.042,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.032,-0.026,-0.13,0.2,-7.4e-06,0.43,-6.9e-05,0.00075,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00014,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25690000,0.98,-0.012,-0.012,0.18,0.023,-0.05,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.0001,0.032,-0.026,-0.13,0.2,-7.5e-06,0.43,-6.4e-05,0.00078,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25790000,0.98,-0.012,-0.012,0.18,0.011,-0.041,0.031,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.033,-0.026,-0.13,0.2,-8e-06,0.43,-0.00012,0.00071,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25890000,0.98,-0.012,-0.012,0.18,0.0057,-0.04,0.033,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.033,-0.026,-0.13,0.2,-8.2e-06,0.43,-0.00014,0.00068,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.4e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +25990000,0.98,-0.012,-0.012,0.18,-0.0033,-0.033,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.035,-0.027,-0.13,0.2,-9.6e-06,0.43,-0.0002,0.00066,0.00017,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0056,0.035,0.036,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26090000,0.98,-0.012,-0.012,0.18,-0.0079,-0.033,0.025,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.027,-0.13,0.2,-9.1e-06,0.43,-0.0002,0.00065,0.0002,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3.1e-07,1.3e-06,0.027,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26190000,0.98,-0.012,-0.012,0.18,-0.015,-0.026,0.021,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.027,-0.13,0.2,-9.5e-06,0.43,-0.00022,0.00064,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26290000,0.98,-0.012,-0.013,0.18,-0.015,-0.026,0.015,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.027,-0.13,0.2,-1e-05,0.43,-0.00023,0.00066,0.00019,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0056,0.038,0.039,0.032,3.3e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26390000,0.98,-0.011,-0.013,0.18,-0.021,-0.018,0.019,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.037,-0.028,-0.13,0.2,-1.1e-05,0.43,-0.00027,0.00063,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26490000,0.98,-0.011,-0.013,0.18,-0.023,-0.016,0.028,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.037,-0.028,-0.13,0.2,-1.2e-05,0.43,-0.00028,0.00066,0.00016,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26590000,0.98,-0.01,-0.013,0.18,-0.026,-0.0074,0.029,0,0,-4.9e+02,-0.0015,-0.0058,-0.00017,0.038,-0.028,-0.13,0.2,-1.3e-05,0.43,-0.00031,0.00068,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26690000,0.98,-0.01,-0.013,0.18,-0.027,-0.0035,0.027,0,0,-4.9e+02,-0.0014,-0.0058,-0.00018,0.037,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00033,0.00069,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,3e-07,1.2e-06,0.026,0.025,0.00013,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26790000,0.98,-0.0099,-0.012,0.18,-0.035,0.00085,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.038,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00035,0.00067,0.00014,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0055,0.035,0.036,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26890000,0.98,-0.0092,-0.012,0.18,-0.041,0.0037,0.022,0,0,-4.9e+02,-0.0015,-0.0058,-0.00018,0.038,-0.027,-0.13,0.2,-1.3e-05,0.43,-0.00035,0.00065,0.00014,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.2e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +26990000,0.98,-0.0087,-0.013,0.18,-0.048,0.011,0.021,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00037,0.00063,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27090000,0.98,-0.0085,-0.013,0.18,-0.05,0.017,0.025,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.4e-05,0.43,-0.00036,0.00063,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0055,0.038,0.039,0.031,3.1e-07,2.9e-07,1.2e-06,0.026,0.025,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27190000,0.98,-0.0086,-0.013,0.18,-0.056,0.024,0.027,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00037,0.00061,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27290000,0.98,-0.0088,-0.014,0.18,-0.063,0.029,0.14,0,0,-4.9e+02,-0.0015,-0.0058,-0.00019,0.039,-0.028,-0.13,0.2,-1.5e-05,0.43,-0.00038,0.00062,0.00015,0,0,-4.9e+02,0.00028,0.00029,0.037,0.012,0.013,0.0054,0.038,0.039,0.031,3.1e-07,2.8e-07,1.2e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27390000,0.98,-0.01,-0.016,0.18,-0.071,0.037,0.46,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.028,-0.13,0.2,-1.6e-05,0.43,-0.00045,0.00054,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27490000,0.98,-0.012,-0.018,0.18,-0.075,0.042,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.00021,0.039,-0.028,-0.13,0.2,-1.7e-05,0.43,-0.00042,0.0005,0.00021,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.013,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27590000,0.98,-0.011,-0.017,0.18,-0.07,0.045,0.87,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.028,-0.13,0.2,-1.7e-05,0.43,-0.0004,0.00044,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27690000,0.98,-0.01,-0.014,0.18,-0.066,0.041,0.78,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.00043,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27790000,0.98,-0.0088,-0.013,0.18,-0.066,0.04,0.77,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.0004,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.035,0.036,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27890000,0.98,-0.0085,-0.013,0.18,-0.071,0.046,0.81,0,0,-4.9e+02,-0.0015,-0.0058,-0.0002,0.039,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.00039,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.8e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +27990000,0.98,-0.0089,-0.013,0.18,-0.072,0.049,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00019,0.038,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00039,0.00033,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28090000,0.98,-0.0092,-0.013,0.18,-0.076,0.049,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00019,0.038,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.0004,0.00036,0.00024,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0054,0.038,0.039,0.031,3e-07,2.7e-07,1.1e-06,0.026,0.024,0.00012,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28190000,0.98,-0.0086,-0.013,0.18,-0.077,0.047,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00018,0.038,-0.028,-0.12,0.2,-1.7e-05,0.43,-0.00038,0.00034,0.00025,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28290000,0.98,-0.0081,-0.014,0.18,-0.081,0.05,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.038,-0.029,-0.12,0.2,-1.7e-05,0.43,-0.00039,0.00036,0.00022,0,0,-4.9e+02,0.00028,0.00029,0.037,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0015,0.0012,1,1,0.01 +28390000,0.98,-0.0081,-0.014,0.18,-0.082,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.038,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00044,0.00027,0.00023,0,0,-4.9e+02,0.00028,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28490000,0.98,-0.0084,-0.015,0.18,-0.085,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00017,0.038,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.0004,0.00025,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1.1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28590000,0.98,-0.0085,-0.015,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.037,-0.028,-0.12,0.2,-1.8e-05,0.43,-0.00037,0.00023,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28690000,0.98,-0.0082,-0.014,0.18,-0.078,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00016,0.037,-0.029,-0.12,0.2,-1.8e-05,0.43,-0.00035,0.00024,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.9e-07,2.7e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28790000,0.98,-0.0076,-0.014,0.18,-0.076,0.055,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00015,0.036,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00037,0.00013,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28890000,0.98,-0.0074,-0.014,0.18,-0.08,0.057,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00014,0.036,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00037,0.00017,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.026,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +28990000,0.98,-0.0072,-0.014,0.18,-0.077,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-2e-05,0.43,-0.00041,1.5e-05,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29090000,0.98,-0.007,-0.014,0.18,-0.08,0.056,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00013,0.035,-0.029,-0.12,0.2,-1.9e-05,0.43,-0.00043,-4.3e-06,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29190000,0.98,-0.0069,-0.014,0.18,-0.078,0.056,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.034,-0.029,-0.12,0.2,-2.1e-05,0.43,-0.00046,-0.00014,0.00028,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.035,0.036,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29290000,0.98,-0.0072,-0.014,0.18,-0.081,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00012,0.034,-0.028,-0.12,0.2,-2.2e-05,0.43,-0.00045,-0.00016,0.00028,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,1e-06,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29390000,0.98,-0.0076,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-0.00011,0.034,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00049,-0.00029,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.8e-07,2.6e-07,9.9e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29490000,0.98,-0.0076,-0.013,0.18,-0.08,0.062,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-9.9e-05,0.034,-0.028,-0.12,0.2,-2.3e-05,0.43,-0.00051,-0.00026,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0053,0.038,0.039,0.031,2.8e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29590000,0.98,-0.0075,-0.013,0.18,-0.077,0.061,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-8.2e-05,0.033,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00056,-0.00037,0.00027,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.6e-07,9.8e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29690000,0.98,-0.0075,-0.013,0.18,-0.082,0.06,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-7.5e-05,0.033,-0.028,-0.12,0.2,-2.4e-05,0.43,-0.00057,-0.00033,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.6e-07,9.7e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29790000,0.98,-0.0073,-0.013,0.18,-0.079,0.054,0.81,0,0,-4.9e+02,-0.0014,-0.0058,-5.9e-05,0.032,-0.028,-0.12,0.2,-2.6e-05,0.43,-0.00063,-0.00045,0.00026,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29890000,0.98,-0.0068,-0.014,0.18,-0.08,0.055,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.3e-05,0.032,-0.028,-0.12,0.2,-2.6e-05,0.43,-0.00065,-0.00043,0.00023,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.031,2.7e-07,2.5e-07,9.6e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +29990000,0.98,-0.007,-0.014,0.18,-0.075,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-4.1e-05,0.031,-0.029,-0.12,0.2,-2.7e-05,0.43,-0.00063,-0.00048,0.00025,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.5e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30090000,0.98,-0.0071,-0.014,0.18,-0.077,0.051,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.2e-05,0.031,-0.029,-0.12,0.2,-2.8e-05,0.43,-0.00058,-0.00055,0.0003,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.4e-07,0.025,0.024,0.00011,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30190000,0.98,-0.0071,-0.014,0.18,-0.071,0.047,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.4e-05,0.03,-0.029,-0.12,0.2,-3.1e-05,0.43,-0.00059,-0.00077,0.00036,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30290000,0.98,-0.0071,-0.014,0.18,-0.071,0.048,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-5.3e-05,0.03,-0.029,-0.12,0.2,-3e-05,0.43,-0.00063,-0.00082,0.00037,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.3e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30390000,0.98,-0.0071,-0.013,0.18,-0.065,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-3.5e-05,0.03,-0.03,-0.12,0.2,-3.1e-05,0.43,-0.00077,-0.001,0.00037,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30490000,0.98,-0.0071,-0.014,0.18,-0.068,0.043,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-2.9e-05,0.03,-0.03,-0.12,0.2,-3.1e-05,0.43,-0.00081,-0.001,0.00035,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.7e-07,2.5e-07,9.2e-07,0.025,0.024,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30590000,0.98,-0.0074,-0.014,0.17,-0.064,0.041,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.4e-05,0.029,-0.03,-0.12,0.2,-3.2e-05,0.43,-0.00092,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.5e-07,9.1e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30690000,0.98,-0.0078,-0.014,0.18,-0.062,0.039,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-1.4e-05,0.029,-0.031,-0.12,0.2,-3.2e-05,0.43,-0.00089,-0.0011,0.00035,0,0,-4.9e+02,0.00029,0.00029,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.5e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30790000,0.98,-0.0075,-0.014,0.17,-0.056,0.034,0.8,0,0,-4.9e+02,-0.0014,-0.0058,-9.5e-06,0.029,-0.031,-0.12,0.2,-3.3e-05,0.43,-0.00097,-0.0013,0.00039,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0052,0.035,0.036,0.03,2.6e-07,2.4e-07,9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30890000,0.98,-0.0069,-0.014,0.17,-0.056,0.03,0.79,0,0,-4.9e+02,-0.0014,-0.0058,-1.6e-05,0.029,-0.031,-0.12,0.2,-3.4e-05,0.43,-0.00087,-0.0012,0.00041,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.9e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +30990000,0.98,-0.0071,-0.013,0.17,-0.048,0.025,0.8,0,0,-4.9e+02,-0.0013,-0.0057,-8.8e-06,0.028,-0.032,-0.12,0.2,-3.5e-05,0.43,-0.00091,-0.0013,0.00044,0,0,-4.9e+02,0.00028,0.00028,0.036,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31090000,0.98,-0.0073,-0.014,0.17,-0.048,0.024,0.79,0,0,-4.9e+02,-0.0013,-0.0057,-1.3e-05,0.028,-0.033,-0.12,0.2,-3.5e-05,0.43,-0.00086,-0.0013,0.00045,0,0,-4.9e+02,0.00029,0.00028,0.036,0.011,0.012,0.0052,0.038,0.039,0.03,2.6e-07,2.4e-07,8.8e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31190000,0.98,-0.0075,-0.014,0.17,-0.043,0.02,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.7e-06,0.028,-0.033,-0.12,0.2,-3.6e-05,0.43,-0.00096,-0.0013,0.00045,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31290000,0.98,-0.0077,-0.014,0.17,-0.04,0.017,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.8e-06,0.029,-0.034,-0.12,0.2,-3.5e-05,0.43,-0.001,-0.0012,0.00043,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.7e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31390000,0.98,-0.0075,-0.013,0.17,-0.035,0.011,0.8,0,0,-4.9e+02,-0.0013,-0.0057,7.1e-06,0.028,-0.034,-0.12,0.2,-3.4e-05,0.43,-0.0011,-0.0016,0.00048,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31490000,0.98,-0.0073,-0.014,0.17,-0.036,0.0078,0.8,0,0,-4.9e+02,-0.0013,-0.0057,5.5e-06,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0011,-0.0018,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.6e-07,2.4e-07,8.6e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31590000,0.98,-0.0071,-0.014,0.17,-0.032,0.0059,0.8,0,0,-4.9e+02,-0.0013,-0.0057,1.5e-05,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0013,-0.0019,0.00052,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.5e-07,0.025,0.023,0.0001,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31690000,0.98,-0.0071,-0.015,0.17,-0.036,0.0053,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.028,-0.034,-0.12,0.2,-3.3e-05,0.43,-0.0014,-0.0019,0.0005,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31790000,0.99,-0.0074,-0.015,0.17,-0.026,0.0027,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.8e-05,0.028,-0.034,-0.12,0.2,-3.2e-05,0.43,-0.0015,-0.002,0.00053,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.4e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31890000,0.99,-0.0071,-0.015,0.17,-0.025,0.00049,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3.2e-05,0.028,-0.034,-0.12,0.2,-3.1e-05,0.43,-0.0016,-0.0021,0.00055,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.3e-07,0.025,0.023,9.9e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +31990000,0.99,-0.0074,-0.015,0.17,-0.017,-0.00059,0.79,0,0,-4.9e+02,-0.0013,-0.0057,2.9e-05,0.029,-0.035,-0.12,0.2,-3.2e-05,0.43,-0.0015,-0.0023,0.0006,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32090000,0.99,-0.0077,-0.014,0.17,-0.018,-0.004,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3e-05,0.029,-0.034,-0.12,0.2,-3.1e-05,0.43,-0.0016,-0.0024,0.00062,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.4e-07,8.2e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32190000,0.99,-0.008,-0.015,0.17,-0.013,-0.0076,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.029,-0.035,-0.12,0.2,-3e-05,0.43,-0.0016,-0.0025,0.00067,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.8e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32290000,0.99,-0.0079,-0.015,0.17,-0.012,-0.01,0.8,0,0,-4.9e+02,-0.0013,-0.0057,3e-05,0.029,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0017,-0.0026,0.00068,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8.1e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32390000,0.99,-0.008,-0.015,0.17,-0.0061,-0.012,0.8,0,0,-4.9e+02,-0.0013,-0.0057,2.7e-05,0.03,-0.035,-0.12,0.2,-2.8e-05,0.43,-0.0017,-0.0028,0.00074,0,0,-4.9e+02,0.00028,0.00028,0.035,0.011,0.012,0.0051,0.035,0.036,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32490000,0.99,-0.011,-0.013,0.17,0.019,-0.078,-0.076,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0028,0.00073,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.014,0.0051,0.038,0.039,0.03,2.5e-07,2.3e-07,8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32590000,0.99,-0.011,-0.013,0.17,0.022,-0.079,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,2.3e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0024,0.0008,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32690000,0.99,-0.011,-0.013,0.17,0.017,-0.085,-0.08,0,0,-4.9e+02,-0.0013,-0.0057,2.2e-05,0.03,-0.035,-0.12,0.2,-2.9e-05,0.43,-0.0016,-0.0025,0.00079,0,0,-4.9e+02,0.00028,0.00028,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.9e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32790000,0.99,-0.011,-0.013,0.17,0.018,-0.084,-0.081,0,0,-4.9e+02,-0.0013,-0.0057,2e-05,0.03,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0016,-0.0023,0.00086,0,0,-4.9e+02,0.00028,0.00027,0.035,0.011,0.013,0.0051,0.035,0.036,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0013,0.0014,0.0012,1,1,0.01 +32890000,0.99,-0.011,-0.013,0.17,0.017,-0.09,-0.083,0,0,-4.9e+02,-0.0013,-0.0057,2.4e-05,0.03,-0.035,-0.12,0.2,-2.7e-05,0.43,-0.0016,-0.0022,0.00088,0,0,-4.9e+02,0.00028,0.00027,0.035,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.8e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +32990000,0.98,-0.011,-0.013,0.17,0.017,-0.089,-0.082,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.03,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0017,-0.0021,0.00094,0,0,-4.9e+02,0.00027,0.00027,0.035,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33090000,0.98,-0.011,-0.013,0.17,0.013,-0.094,-0.079,0,0,-4.9e+02,-0.0013,-0.0057,2.5e-05,0.03,-0.035,-0.12,0.2,-2.5e-05,0.43,-0.0017,-0.0021,0.00094,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.7e-07,0.025,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33190000,0.98,-0.01,-0.013,0.17,0.011,-0.094,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,1.7e-05,0.031,-0.034,-0.12,0.2,-2.4e-05,0.43,-0.0016,-0.0021,0.00096,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33290000,0.99,-0.01,-0.013,0.17,0.0078,-0.096,-0.078,0,0,-4.9e+02,-0.0014,-0.0057,2.7e-05,0.031,-0.034,-0.12,0.2,-2.2e-05,0.43,-0.0018,-0.0022,0.001,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.6e-07,0.024,0.023,9.7e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.01 +33390000,0.98,-0.01,-0.013,0.17,0.0056,-0.091,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,2e-05,0.033,-0.033,-0.12,0.2,-2e-05,0.43,-0.0018,-0.0021,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.033 +33490000,0.99,-0.01,-0.013,0.17,0.0003,-0.094,-0.075,0,0,-4.9e+02,-0.0014,-0.0057,2.5e-05,0.033,-0.033,-0.12,0.2,-1.8e-05,0.43,-0.0019,-0.0023,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.3e-07,7.5e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.058 +33590000,0.99,-0.01,-0.013,0.17,-0.0025,-0.092,-0.072,0,0,-4.9e+02,-0.0014,-0.0057,2.4e-05,0.034,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.002,-0.0023,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.083 +33690000,0.98,-0.01,-0.013,0.17,-0.0058,-0.095,-0.073,0,0,-4.9e+02,-0.0014,-0.0057,2.8e-05,0.034,-0.032,-0.12,0.2,-1.6e-05,0.43,-0.0019,-0.0022,0.0011,0,0,-4.9e+02,0.00027,0.00027,0.034,0.012,0.013,0.0051,0.038,0.039,0.03,2.4e-07,2.2e-07,7.4e-07,0.024,0.023,9.6e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.11 +33790000,0.98,-0.01,-0.013,0.17,-0.0077,-0.093,-0.068,0,0,-4.9e+02,-0.0014,-0.0057,1.8e-05,0.036,-0.032,-0.12,0.2,-1.4e-05,0.43,-0.0019,-0.002,0.0012,0,0,-4.9e+02,0.00027,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.13 +33890000,0.98,-0.01,-0.013,0.17,-0.012,-0.093,-0.067,0,0,-4.9e+02,-0.0014,-0.0057,2.8e-05,0.036,-0.032,-0.12,0.2,-1.3e-05,0.43,-0.0019,-0.002,0.0012,0,0,-4.9e+02,0.00027,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.4e-07,2.2e-07,7.3e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.16 +33990000,0.98,-0.0099,-0.013,0.17,-0.013,-0.089,-0.064,0,0,-4.9e+02,-0.0014,-0.0057,1.6e-05,0.038,-0.031,-0.12,0.2,-1e-05,0.43,-0.0019,-0.0019,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0014,0.0012,1,1,0.18 +34090000,0.98,-0.0098,-0.013,0.17,-0.017,-0.093,-0.062,0,0,-4.9e+02,-0.0014,-0.0057,1.8e-05,0.038,-0.031,-0.12,0.2,-1.1e-05,0.43,-0.0019,-0.0018,0.0012,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.2e-07,0.024,0.023,9.5e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.21 +34190000,0.98,-0.0098,-0.013,0.17,-0.018,-0.088,-0.06,0,0,-4.9e+02,-0.0014,-0.0057,1.4e-05,0.04,-0.031,-0.12,0.2,-8.5e-06,0.43,-0.0018,-0.0017,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.23 +34290000,0.98,-0.0096,-0.014,0.17,-0.019,-0.092,-0.058,0,0,-4.9e+02,-0.0014,-0.0057,2.1e-05,0.04,-0.031,-0.12,0.2,-7.8e-06,0.43,-0.0019,-0.0017,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7.1e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.26 +34390000,0.98,-0.0095,-0.014,0.17,-0.021,-0.086,-0.054,0,0,-4.9e+02,-0.0014,-0.0057,1.4e-05,0.041,-0.031,-0.12,0.2,-5.7e-06,0.43,-0.0018,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.4e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.28 +34490000,0.98,-0.0095,-0.014,0.17,-0.024,-0.089,-0.052,0,0,-4.9e+02,-0.0014,-0.0056,2.2e-05,0.041,-0.031,-0.12,0.2,-5.4e-06,0.43,-0.0019,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,7e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.31 +34590000,0.98,-0.0094,-0.013,0.17,-0.026,-0.082,0.74,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.043,-0.03,-0.12,0.2,-2.9e-06,0.43,-0.0018,-0.0015,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.33 +34690000,0.98,-0.0094,-0.013,0.17,-0.031,-0.08,1.7,0,0,-4.9e+02,-0.0014,-0.0056,1.8e-05,0.043,-0.031,-0.12,0.2,-2.6e-06,0.43,-0.0019,-0.0016,0.0013,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.012,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.9e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.36 +34790000,0.98,-0.0093,-0.013,0.17,-0.034,-0.071,2.7,0,0,-4.9e+02,-0.0014,-0.0056,1.2e-05,0.045,-0.031,-0.12,0.2,7.2e-08,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.011,0.011,0.005,0.035,0.036,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.3e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.38 +34890000,0.98,-0.0092,-0.013,0.17,-0.04,-0.069,3.7,0,0,-4.9e+02,-0.0014,-0.0056,1.5e-05,0.045,-0.031,-0.12,0.2,2.4e-07,0.43,-0.0019,-0.0015,0.0014,0,0,-4.9e+02,0.00026,0.00026,0.034,0.012,0.013,0.005,0.038,0.039,0.03,2.3e-07,2.2e-07,6.8e-07,0.024,0.023,9.2e-05,0.0012,4e-05,0.0012,0.0012,0.0013,0.0012,1,1,0.41 diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index b9dc3e3672..d905fb8570 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -180,15 +180,31 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) _ekf->set_vehicle_at_rest(false); _ekf->set_is_fixed_wing(true); + const Vector3f pos_prev = _ekf->getPosition(); + const double latitude_new = -15.0000005; const double longitude_new = -115.0000005; const float altitude_new = 1500.0; const float eph = 50.f; const float epv = 10.f; - _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new); _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0); + const Vector3f pos = _ekf->getPosition(); + + // lat/lon is initialized so the local horizontal position remains constant + EXPECT_NEAR(pos(0), pos_prev(0), 1e-3f); + EXPECT_NEAR(pos(1), pos_prev(1), 1e-3f); + + // alt is updated as the local altitude origin was already set + EXPECT_NEAR(pos(2), pos_prev(2) - (altitude_new - _ekf->getEkfGlobalOriginAltitude()), 1e-3f); + + const LatLonAlt lla = _ekf->getLatLonAlt(); + EXPECT_NEAR(lla.latitude_deg(), latitude_new, 1e-6f); + EXPECT_NEAR(lla.longitude_deg(), longitude_new, 1e-6f); + EXPECT_NEAR(lla.altitude(), altitude_new, 1e-3f); + + // Simulate the fact that the sideslip can start immediately, without // waiting for a measurement sample. _ekf_wrapper.enableBetaFusion(); @@ -207,7 +223,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) @@ -227,7 +243,6 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) const float eph = 50.f; const float epv = 1.f; - _ekf->setEkfGlobalOrigin(latitude, longitude, altitude); _ekf->resetGlobalPosToExternalObservation(latitude, longitude, altitude, eph, epv, 0); _ekf_wrapper.enableBetaFusion(); @@ -239,7 +254,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) _sensor_simulator.runSeconds(10.f); EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: an external position reset is sent ResetLoggingChecker reset_logging_checker(_ekf); @@ -260,7 +275,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) EXPECT_NEAR(altitude_est, altitude_new, 0.01f); EXPECT_NEAR(latitude_est, latitude_new, 1e-3f); EXPECT_NEAR(longitude_est, longitude_new, 1e-3f); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); reset_logging_checker.capturePostResetState(); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index e954433442..d67cac0e52 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -219,19 +219,22 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) _altitude_new = 100.0; _sensor_simulator.startGps(); - _ekf->set_min_required_gps_health_time(1e6); - _sensor_simulator.runSeconds(1); + _ekf_wrapper.enableGpsHeightFusion(); _sensor_simulator.setGpsLatitude(_latitude_new); _sensor_simulator.setGpsLongitude(_longitude_new); _sensor_simulator.setGpsAltitude(_altitude_new); + _ekf->set_min_required_gps_health_time(1e6); + _sensor_simulator.runSeconds(1); _sensor_simulator.runSeconds(5); _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); EXPECT_DOUBLE_EQ(_latitude, _latitude_new); EXPECT_DOUBLE_EQ(_longitude, _longitude_new); - EXPECT_NEAR(_altitude, _altitude_new, 0.01f); + + // In baro height ref the origin is set using baro data and not GNSS altitude + EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f); // Note: we cannot reset too far since the local position is limited to 1e6m _latitude_new = 14.0000005; @@ -261,11 +264,13 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) { - _ekf->getEkfGlobalOrigin(_origin_time, _latitude_new, _longitude_new, _altitude_new); + _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); EXPECT_DOUBLE_EQ(_latitude, _latitude_new); EXPECT_DOUBLE_EQ(_longitude, _longitude_new); - EXPECT_FLOAT_EQ(_altitude, _altitude_new); + + // In baro height ref the origin is set using baro data and not GNSS altitude + EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f); EXPECT_FALSE(_ekf->global_origin_valid()); @@ -283,7 +288,7 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) // Global origin has been initialized but since there is no position aiding, the global // position is still not valid EXPECT_TRUE(_ekf->global_origin_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _sensor_simulator.runSeconds(1); @@ -307,9 +312,9 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev) _sensor_simulator.runSeconds(1); // THEN; since there is no origin, only the local position can be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_FALSE(_ekf->global_origin_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _latitude_new = 45.0000005; _longitude_new = 111.0000005; @@ -320,8 +325,8 @@ TEST_F(EkfBasicsTest, global_position_from_local_ev) // THEN: local and global positions are valid EXPECT_TRUE(_ekf->global_origin_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } TEST_F(EkfBasicsTest, global_position_from_opt_flow) @@ -338,9 +343,9 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow) _sensor_simulator.runSeconds(1); // THEN; since there is no origin, only the local position can be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_FALSE(_ekf->global_origin_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _latitude_new = 45.0000005; _longitude_new = 111.0000005; @@ -351,8 +356,8 @@ TEST_F(EkfBasicsTest, global_position_from_opt_flow) // THEN: local and global positions are valid EXPECT_TRUE(_ekf->global_origin_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } // TODO: Add sampling tests diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 5b1439edd9..f87151eeaf 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -85,8 +85,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.runSeconds(2); @@ -95,8 +95,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); _ekf_wrapper.enableExternalVisionHeadingFusion(); _sensor_simulator.runSeconds(2); @@ -105,8 +105,8 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfExternalVisionTest, visionVelocityReset) diff --git a/src/modules/ekf2/test/test_EKF_fake_pos.cpp b/src/modules/ekf2/test/test_EKF_fake_pos.cpp index 3cd755a3a5..fb3b5991e1 100644 --- a/src/modules/ekf2/test/test_EKF_fake_pos.cpp +++ b/src/modules/ekf2/test/test_EKF_fake_pos.cpp @@ -117,5 +117,5 @@ TEST_F(EkfFakePosTest, testValidFakePosValidLocalPos) _sensor_simulator.runSeconds(60); EXPECT_EQ(1, (int) _ekf->control_status_flags().valid_fake_pos); - EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 650d168a98..7e00703971 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -345,3 +345,49 @@ TEST_F(EkfFlowTest, yawMotionNoMagFusion) _ekf->state().vector().print(); _ekf->covariances().print(); } + +TEST_F(EkfFlowTest, deadReckoning) +{ + ResetLoggingChecker reset_logging_checker(_ekf); + + // WHEN: simulate being 5m above ground + const float simulated_distance_to_ground = 5.f; + _sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground); + startRangeFinderFusion(simulated_distance_to_ground); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + + // WHEN: moving a couple of meters while doing flow dead_reckoning + Vector3f simulated_velocity(0.5f, -0.2f, 0.f); + _sensor_simulator._trajectory[0].setCurrentVelocity(simulated_velocity(0)); + _sensor_simulator._trajectory[1].setCurrentVelocity(simulated_velocity(1)); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + + _sensor_simulator.runTrajectorySeconds(5.f); + + simulated_velocity = Vector3f(0.f, 0.f, 0.f); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + + _sensor_simulator.runTrajectorySeconds(_sensor_simulator._trajectory[0].getTotalTime()); + + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + const Vector3f lpos_before_reset = _ekf->getPosition(); + const float altitude_ref_prev = _ekf->getEkfGlobalOriginAltitude(); + + const double latitude_new = -15.0000005; + const double longitude_new = -115.0000005; + const float altitude_new = 1500.0; + const float eph = 50.f; + const float epv = 10.f; + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph * eph, epv * epv); + + const Vector3f lpos_after_reset = _ekf->getPosition(); + + EXPECT_NEAR(lpos_after_reset(0), lpos_before_reset(0), 1e-3); + EXPECT_NEAR(lpos_after_reset(1), lpos_before_reset(1), 1e-3); + EXPECT_NEAR(lpos_after_reset(2), lpos_before_reset(2) + (altitude_new - altitude_ref_prev), 1e-3); +} diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index 9367b86da3..2b89c8aa8f 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -84,13 +84,13 @@ TEST_F(EkfFusionLogicTest, doNoFusion) // GIVEN: a tilt and heading aligned filter // WHEN: having no aiding source // THEN: EKF should not have a valid position estimate - EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); _sensor_simulator.runSeconds(4); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doGpsFusion) @@ -104,8 +104,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // THEN: EKF should intend to fuse GPS EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); // THEN: Local and global position should be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: GPS data is not send for 11s _sensor_simulator.stopGps(); @@ -113,8 +113,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // THEN: EKF should stop to intend to fuse GPS EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: GPS data is send again for 11s _sensor_simulator.startGps(); @@ -122,8 +122,8 @@ TEST_F(EkfFusionLogicTest, doGpsFusion) // THEN: EKF should to intend to fuse GPS EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: clients decides to stop GPS fusion _ekf_wrapper.disableGpsFusion(); @@ -230,8 +230,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should not intend to fuse flow measurements EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: Flow data is not send and we enable flow fusion _sensor_simulator.stopFlow(); @@ -242,8 +242,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should not intend to fuse flow EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: Flow data is sent and we enable flow fusion _sensor_simulator.startFlow(); @@ -253,8 +253,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should intend to fuse flow EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should be valid - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: Stop sending flow data _sensor_simulator.stopFlow(); @@ -263,8 +263,8 @@ TEST_F(EkfFusionLogicTest, doFlowFusion) // THEN: EKF should not intend to fuse flow measurements EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); // THEN: Local and global position should not be valid - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doVisionPositionFusion) @@ -279,8 +279,8 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion) EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: stop sending vision data _sensor_simulator.stopExternalVision(); @@ -291,8 +291,8 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) @@ -307,8 +307,8 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // WHEN: stop sending vision data _sensor_simulator.stopExternalVision(); @@ -319,8 +319,8 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); } TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) @@ -336,8 +336,8 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // THEN: Yaw state should be reset to vision EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); @@ -350,8 +350,8 @@ TEST_F(EkfFusionLogicTest, doVisionHeadingFusion) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionPositionFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionVelocityFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeadingFusion()); - EXPECT_FALSE(_ekf->local_position_is_valid()); - EXPECT_FALSE(_ekf->global_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); // THEN: Yaw state shoud be reset to mag EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index b03a8701d5..f6aae937df 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -107,7 +107,7 @@ TEST_F(EkfGpsTest, gpsFixLoss) // THEN: after dead-reconing for a couple of seconds, the local position gets invalidated _sensor_simulator.runSeconds(6); EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning); - EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); // The control logic takes a bit more time to deactivate the GNSS fusion completely _sensor_simulator.runSeconds(5); @@ -164,7 +164,7 @@ TEST_F(EkfGpsTest, resetToGpsPosition) // AND: simulate jump in position _sensor_simulator.startGps(); - const Vector3f simulated_position_change(2.0f, -1.0f, 0.f); + const Vector3f simulated_position_change(20.0f, -1.0f, 0.f); _sensor_simulator._gps.stepHorizontalPositionByMeters( Vector2f(simulated_position_change)); _sensor_simulator.runSeconds(6); diff --git a/src/modules/ekf2/test/test_EKF_grounded.cpp b/src/modules/ekf2/test/test_EKF_grounded.cpp new file mode 100644 index 0000000000..14f3b6ad52 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_grounded.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +/** + * Test EKF altitude estimation while grounded and using Rangefinder as primary alt source + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" + +class EkfGroundedTest : public ::testing::Test +{ +public: + + EkfGroundedTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + void SetUp() override + { + // Init EKF + _ekf->init(0); + _sensor_simulator.runSeconds(0.1); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + + _sensor_simulator._rng.setData(0.1f, 100); + _sensor_simulator._rng.setLimits(0.1f, 25.f); + + _sensor_simulator.startGps(); + _sensor_simulator.startBaro(); + _sensor_simulator.startRangeFinder(); + + // Set Range as primary height source + _ekf_wrapper.setRangeHeightRef(); + + // Enable fusion for all height sources + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + _ekf_wrapper.enableRangeHeightFusion(); + + // Give EKF time for GPS + _sensor_simulator.runSeconds(20); + } +}; + +TEST_F(EkfGroundedTest, rangeFinderOnGround) +{ + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + + float distance = 0.17f; + _sensor_simulator._rng.setData(distance, 100); + + _sensor_simulator.runSeconds(60); + + EXPECT_NEAR(_ekf->getLatLonAlt().altitude(), 0, 0.5f); +} diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index b322a28182..3b9cd2363b 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -123,7 +123,7 @@ TEST_F(EkfHeightFusionTest, baroRef) /* EXPECT_EQ(status.bias, _sensor_simulator._baro.getData()); */ // This is the real bias, but the estimator isn't running so the status isn't updated EXPECT_EQ(baro_status.bias, 0.f); const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus(); - EXPECT_NEAR(gps_status.bias, -baro_increment, 0.2f); + EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f); const float terrain = _ekf->getTerrainVertPos(); EXPECT_NEAR(terrain, -baro_increment, 1.2f); @@ -170,8 +170,13 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion()); - // AND WHEN: the baro data increases const float baro_initial = _sensor_simulator._baro.getData(); + + const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus(); + const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt; + EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f); + + // AND WHEN: the baro data increases const float baro_increment = 5.f; _sensor_simulator._baro.setData(baro_initial + baro_increment); _sensor_simulator.runSeconds(100); @@ -180,7 +185,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) // the GPS height value and the baro gets its bias estimated EXPECT_NEAR(_ekf->getPosition()(2), 0.f, 1.f); const BiasEstimator::status &baro_status = _ekf->getBaroBiasEstimatorStatus(); - EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f); + EXPECT_NEAR(baro_status.bias, baro_rel_initial + baro_increment, 1.3f); const float terrain = _ekf->getTerrainVertPos(); EXPECT_NEAR(terrain, 0.f, 1.1f); // TODO: why? @@ -196,13 +201,39 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f); + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_rel_initial + baro_increment - gps_step, 0.2f); // and the innovations are close to zero EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f); EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f); } +TEST_F(EkfHeightFusionTest, gpsRefNoAltFusion) +{ + // GIVEN: GNSS alt reference but not selected as an aiding source + _ekf_wrapper.setGpsHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.disableGpsHeightFusion(); + _ekf_wrapper.enableRangeHeightFusion(); + _sensor_simulator.runSeconds(1); + + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); // Fallback to baro as GNSS alt is disabled + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion()); + + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + + // THEN: the altitude estimate is initialised using GNSS altitude + EXPECT_NEAR(_ekf->getLatLonAlt().altitude(), _sensor_simulator._gps.getData().alt, 1.f); + // We cannot check the value of the bias estimate as the status is only updatad when the bias estimator is + // active. Since the estimator had a baro fallback, the baro bias estimate is not actively updated. + // EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() - _sensor_simulator._gps.getData().alt, 0.2f); +} + TEST_F(EkfHeightFusionTest, baroRefFailOver) { // GIVEN: baro reference with GPS and range height fusion @@ -340,12 +371,10 @@ TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) // Also check the reset counters to make sure the reset logic triggered reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); + // The velocity does not reset as baro only provides height measurement EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); - - // The height resets twice in a row as the baro innovation is not corrected after a height - // reset and triggers a new reset at the next iteration - EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); } TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) @@ -367,6 +396,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) ResetLoggingChecker reset_logging_checker(_ekf); reset_logging_checker.capturePreResetState(); + const float baro_bias_prev = _ekf->getBaroBiasEstimatorStatus().bias; const float alt_increment = 4478.f; _ekf->setEkfGlobalOrigin(lat, lon, alt + alt_increment); @@ -376,9 +406,12 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) EXPECT_NEAR(_ekf->getPosition()(2), alt_increment, 1.f); reset_logging_checker.capturePostResetState(); - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() + alt_increment, 0.2f); + + // An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS) + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f); EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); } diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index a49cd5c66f..1d1fdfb6ce 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -40,7 +40,6 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" -#include "test_helper/reset_logging_checker.h" class EKFYawEstimatorTest : public ::testing::Test { @@ -91,8 +90,6 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) // WHEN: the vehicle starts accelerating in the horizontal plane _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f)); _ekf->set_in_air_status(true); - ResetLoggingChecker reset_logging_checker(_ekf); - reset_logging_checker.capturePreResetState(); _sensor_simulator.runTrajectorySeconds(3.f); @@ -106,11 +103,13 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_NEAR(yaw_est, yaw, tolerance_rad); EXPECT_LT(yaw_est_var, tolerance_rad); - // 1 reset when starting GNSS aiding - reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); - EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); + EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[0], 0.5f); + EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[1], 0.5f); - EXPECT_TRUE(_ekf->local_position_is_valid()); - EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[0], 0.1f); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[1], 0.1f); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[2], 0.1f); + + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid()); } diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index c2e9d1c667..ecf69d83b5 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -354,14 +354,13 @@ TEST_F(SensorRangeFinderTest, blockedByFog) const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; const uint64_t dt_update_us = 10e3; uint64_t dt_sensor_us = 3e5; - uint64_t duration_us = 5e5; + uint64_t duration_us = 2e6; updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us); // THEN: the data should be marked as healthy EXPECT_TRUE(_range_finder.isDataHealthy()); EXPECT_TRUE(_range_finder.isHealthy()); - // WHEN: sensor is then blocked by fog // range jumps to value below 2m uint64_t t_now_us = _range_finder.getSampleAddress()->time_us; @@ -374,6 +373,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog) // WHEN: the sensor is not blocked by fog anymore sample.rng = 5.f; + sample.time_us = _range_finder.getSampleAddress()->time_us; updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us); // THEN: the data should be marked as healthy again diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 6a6a21f02a..0fb4a3efad 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -91,17 +91,25 @@ EscBattery::Run() const uint8_t online_esc_count = math::countSetBits(esc_status.esc_online_flags); float average_voltage_v = 0.0f; float total_current_a = 0.0f; + float average_temperature_c = 0.0f; for (unsigned i = 0; i < esc_status.esc_count; ++i) { if ((1 << i) & esc_status.esc_online_flags) { average_voltage_v += esc_status.esc[i].esc_voltage; total_current_a += esc_status.esc[i].esc_current; + + if (PX4_ISFINITE(esc_status.esc[i].esc_temperature)) { + average_temperature_c += esc_status.esc[i].esc_temperature; + } } } average_voltage_v /= online_esc_count; + total_current_a /= online_esc_count; + average_temperature_c /= online_esc_count; _battery.setConnected(true); + _battery.updateTemperature(average_temperature_c); _battery.updateVoltage(average_voltage_v); _battery.updateCurrent(total_current_a); _battery.updateAndPublishBatteryStatus(esc_status.timestamp); diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 81ff6a7a33..84ccba6ac0 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -47,7 +47,6 @@ list(APPEND flight_tasks_all ManualAltitude ManualAltitudeSmoothVel ManualPosition - ManualPositionSmoothVel Transition ) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 3d70726073..f17e00aa96 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -213,10 +213,6 @@ void FlightModeManager::start_flight_task() error = switchTask(FlightTaskIndex::ManualPosition); break; - case 3: - error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); - break; - case 4: default: if (_param_mpc_pos_mode.get() != 4) { @@ -378,11 +374,9 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) // Save current setpoints for the next FlightTask trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint; - ekf_reset_counters_s last_reset_counters{}; if (isAnyTaskActive()) { last_setpoint = _current_task.task->getTrajectorySetpoint(); - last_reset_counters = _current_task.task->getResetCounters(); } if (_initTask(new_task_index)) { @@ -403,7 +397,6 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) return FlightTaskError::ActivationFailed; } - _current_task.task->setResetCounters(last_reset_counters); _command_failed = false; return FlightTaskError::NoError; diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 2fadf2754d..905e08e80b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane) +target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility WeatherVane) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index b2bb5632b7..419f25827a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -156,20 +156,9 @@ bool FlightTaskAuto::update() break; } - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; - if (isTargetModified()) { - // In case object avoidance has injected a new setpoint, we take this as the next waypoints - waypoints[2] = _position_setpoint; - } - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; @@ -282,12 +271,6 @@ void FlightTaskAuto::_prepareLandSetpoints() sticks_xy.setZero(); } - // If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed - if (PX4_ISFINITE(_dist_to_bottom)) { - // Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed - max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f)); - } - _stick_acceleration_xy.setVelocityConstraint(max_speed); _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); @@ -483,17 +466,6 @@ bool FlightTaskAuto::_evaluateTriplets() _updateInternalWaypoints(); } - if (_param_com_obs_avoid.get() - && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - (float)NAN, - _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); - _obstacle_avoidance.checkAvoidanceProgress( - _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); - } - // set heading _weathervane.update(); @@ -623,7 +595,7 @@ bool FlightTaskAuto::_evaluateGlobalReference() } // init projection - _reference_position.initReference(ref_lat, ref_lon); + _reference_position.initReference(ref_lat, ref_lon, _time_stamp_current); // check if everything is still finite return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon); @@ -787,15 +759,6 @@ bool FlightTaskAuto::_generateHeadingAlongTraj() return res; } -bool FlightTaskAuto::isTargetModified() const -{ - const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); - const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); - const bool z_modified = z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; - - return xy_modified || z_modified; -} - void FlightTaskAuto::_updateTrajConstraints() { // update params of the position smoothing diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23..5b8d3e9b5b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -53,13 +53,6 @@ #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" -// TODO: make this switchable in the board config, like a module -#if CONSTRAINED_FLASH -#include -#else -#include -#endif - /** * This enum has to agree with position_setpoint_s type definition * The only reason for not using the struct position_setpoint is because @@ -115,7 +108,6 @@ protected: void _checkEmergencyBraking(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ - bool isTargetModified() const; void _updateTrajConstraints(); void rcHelpModifyYaw(float &yaw_sp); @@ -147,8 +139,6 @@ protected: AlphaFilter _yawspeed_filter; bool _yaw_sp_aligned{false}; - ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */ - PositionSmoothing _position_smoothing; Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; @@ -165,7 +155,6 @@ protected: (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, - (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp index c667594b10..1c3e0f0011 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp @@ -231,7 +231,8 @@ void TargetEstimator::measurement_update(follow_target_s follow_target) // Decompose follow_target message into the individual measurements for position and velocity const Vector3f vel_measured{follow_target.vx, follow_target.vy, follow_target.vz}; Vector3f pos_measured{NAN, NAN, -(follow_target.alt - _vehicle_local_position.ref_alt)}; - _reference_position.initReference(_vehicle_local_position.ref_lat, _vehicle_local_position.ref_lon); + _reference_position.initReference(_vehicle_local_position.ref_lat, _vehicle_local_position.ref_lon, + hrt_absolute_time()); _reference_position.project(follow_target.lat, follow_target.lon, pos_measured(0), pos_measured(1)); // Initialize filter if necessary diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp index 85ba49ea0b..4d16f7d4c9 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -55,7 +55,7 @@ bool FlightTaskDescend::update() } else { // descend with constant acceleration (crash landing) _velocity_setpoint(2) = NAN; - _acceleration_setpoint(2) = .15f; + _acceleration_setpoint(2) = .3f; } // Nudging @@ -69,7 +69,7 @@ bool FlightTaskDescend::update() _acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f; } else { - _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, NAN); // stay level to minimize horizontal drift + _acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f); // stay level to minimize horizontal drift _yawspeed_setpoint = NAN; // keep heading diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 55b587d270..fa2943f147 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -11,6 +11,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint) { _resetSetpoints(); _setDefaultConstraints(); + initEkfResetCounters(); _time_stamp_activate = hrt_absolute_time(); _gear = empty_landing_gear_default_keep; return true; @@ -24,6 +25,16 @@ void FlightTask::reActivate() activate(setpoint_preserve_vertical); } +void FlightTask::initEkfResetCounters() +{ + _reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter; + _reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter; + _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; + _reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter; + _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; + _reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter; +} + bool FlightTask::updateInitialize() { _time_stamp_current = hrt_absolute_time(); @@ -66,6 +77,8 @@ void FlightTask::_checkEkfResetCounters() if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) { _ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom); _reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter; + + _dist_to_bottom = NAN; } if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) { diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 6ee861f3cd..06327d8ca8 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -55,15 +55,6 @@ #include #include -struct ekf_reset_counters_s { - uint8_t xy; - uint8_t vxy; - uint8_t z; - uint8_t vz; - uint8_t heading; - uint8_t hagl; -}; - class FlightTask : public ModuleParams { public: @@ -88,6 +79,8 @@ public: */ virtual void reActivate(); + virtual void initEkfResetCounters(); + /** * To be called to adopt parameters from an arrived vehicle command * @param command received command message containing the parameters @@ -115,9 +108,6 @@ public: */ const trajectory_setpoint_s getTrajectorySetpoint(); - const ekf_reset_counters_s getResetCounters() const { return _reset_counters; } - void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; } - /** * Get vehicle constraints. * The constraints can vary with task. @@ -236,7 +226,14 @@ protected: float _yaw_setpoint{}; float _yawspeed_setpoint{}; - ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets + struct ekf_reset_counters_s { + uint8_t xy; + uint8_t vxy; + uint8_t z; + uint8_t vz; + uint8_t heading; + uint8_t hagl; + } _reset_counters{}; ///< Counters for estimator local position resets /** * Vehicle constraints. diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index a1ea2b6b80..6b79aa1833 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -59,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se bool FlightTaskManualAcceleration::update() { + const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get(); + setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy); bool ret = FlightTaskManualAltitudeSmoothVel::update(); + float max_hagl_ratio = 0.0f; + + if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) { + max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy; + } + + // limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps + static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl + static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl + + if (max_hagl_ratio > factor_threshold) { + max_hagl_ratio = math::min(max_hagl_ratio, 1.f); + const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get()); + _stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel)); + + } else { + _stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max)); + } + _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 0ae3d35bb6..ac19500236 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -52,4 +52,9 @@ protected: StickAccelerationXY _stick_acceleration_xy{this}; WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_acc_hor + ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 7b1c24f1dd..dd2ca1da59 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -39,6 +39,7 @@ #include using namespace time_literals; +using namespace matrix; bool FlightTaskManualAccelerationSlow::update() { @@ -128,7 +129,29 @@ bool FlightTaskManualAccelerationSlow::update() FlightTaskManualAltitude::_velocity_constraint_down = velocity_down; FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate); - return FlightTaskManualAcceleration::update(); + bool ret = FlightTaskManualAcceleration::update(); + + // Optimize input-to-video latency gimbal control + if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) { + _gimbal.acquireGimbalControlIfNeeded(); + + // the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward + const float pitchrate_setpoint = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate; + _yawspeed_setpoint = _sticks.getYaw() * yaw_rate; + + _gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, pitchrate_setpoint, _yawspeed_setpoint)); + + if (_gimbal.allAxesLockedConfirmed()) { + // but the vehicle makes sure it stays alligned with the gimbal absolute yaw + _yaw_setpoint = _gimbal.getTelemetryYaw(); + } + + } else { + _gimbal.releaseGimbalControlIfNeeded(); + } + + return ret; } float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value) @@ -136,3 +159,11 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i const int sanitized_index = math::constrain(parameter_value - 1, 0, 5); return _sticks.getAux()(sanitized_index); } + +bool FlightTaskManualAccelerationSlow::haveTakenOff() +{ + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + + return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT; +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index b54d9b765d..66b2de4a8f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -42,6 +42,7 @@ #include "FlightTaskManualAcceleration.hpp" #include "StickAccelerationXY.hpp" +#include "Gimbal.hpp" #include #include @@ -67,9 +68,15 @@ private: bool _velocity_limits_received_before{false}; + // Gimbal control + Gimbal _gimbal{this}; + uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)}; velocity_limits_s _velocity_limits{}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + bool haveTakenOff(); + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, (ParamInt) _param_mc_slow_map_hvel, (ParamInt) _param_mc_slow_map_vvel, @@ -83,6 +90,7 @@ private: (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_man_y_max + (ParamFloat) _param_mpc_man_y_max, + (ParamInt) _param_mc_slow_map_pitch ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c index 4c643d87d8..32558d98ea 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -156,3 +156,17 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f); * @group Multicopter Position Slow Mode */ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f); + +/** + * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode + * + * @value 0 No pitch rate input + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 9c22ae66a3..ed7676296f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() _min_distance_to_ground = -INFINITY; } - if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { - _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; - - } else { - _max_distance_to_ground = INFINITY; + if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) { + _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z; } } @@ -132,6 +129,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } else { _position_setpoint(2) = _position(2); + _dist_to_ground_lock = NAN; } } @@ -154,8 +152,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { // terrain following _terrainFollowing(apply_brake, stopped); - // respect maximum altitude - _respectMaxAltitude(); } else { // normal mode where height is dependent on local frame @@ -185,9 +181,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // user demands velocity change _position_setpoint(2) = NAN; // ensure that maximum altitude is respected - _respectMaxAltitude(); } } + + _respectMaxAltitude(); } void FlightTaskManualAltitude::_respectMinAltitude() @@ -229,29 +226,20 @@ void FlightTaskManualAltitude::_respectMaxAltitude() { if (PX4_ISFINITE(_dist_to_bottom)) { - // if there is a valid maximum distance to ground, linearly increase speed limit with distance - // below the maximum, preserving control loop vertical position error gain. - // TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller + float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom); + if (PX4_ISFINITE(_max_distance_to_ground)) { - _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), - -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); + _constraints.speed_up = math::constrain(vel_constrained, -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); } else { _constraints.speed_up = _param_mpc_z_vel_max_up.get(); } - // if distance to bottom exceeded maximum distance, slowly approach maximum distance - if (_dist_to_bottom > _max_distance_to_ground) { - // difference between current distance to ground and maximum distance to ground - const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground; - // set position setpoint to maximum distance to ground - _position_setpoint(2) = _position(2) + delta_distance_to_max; - // limit speed downwards to 0.7m/s - _constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f); - - } else { - _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + if (_dist_to_bottom > _max_distance_to_ground && !(_sticks.getThrottleZeroCenteredExpo() < FLT_EPSILON)) { + _velocity_setpoint(2) = math::constrain(-vel_constrained, 0.f, _param_mpc_z_vel_max_dn.get()); } + + _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); } } @@ -279,6 +267,11 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) _stick_yaw.ekfResetHandler(delta_psi); } +void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl) +{ + _dist_to_ground_lock = NAN; +} + void FlightTaskManualAltitude::_updateSetpoints() { _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw); @@ -301,6 +294,7 @@ bool FlightTaskManualAltitude::update() _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); + _max_distance_to_ground = INFINITY; return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 4df9444e98..5370de28ff 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -53,9 +53,12 @@ public: bool activate(const trajectory_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; + void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; } protected: void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ + void _ekfResetHandlerHagl(float delta_hagl) override; + virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ bool _checkTakeoff() override; diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 71dedaf056..352875764e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -94,11 +94,6 @@ void FlightTaskManualPosition::_scaleSticks() // Rotate setpoint into local frame Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint); - // collision prevention - if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy()); - } - _velocity_setpoint.xy() = vel_sp_xy; } diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index fb7f5dd4e5..b1f05092d3 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -68,5 +68,5 @@ private: uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention{this}; /**< collision prevention setpoint amendment */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp deleted file mode 100644 index a89283e146..0000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 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 "FlightTaskManualPositionSmoothVel.hpp" - -#include -#include - -using namespace matrix; - -bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskManualPosition::activate(last_setpoint); - - // Check if the previous FlightTask provided setpoints - Vector3f accel_prev{last_setpoint.acceleration}; - Vector3f vel_prev{last_setpoint.velocity}; - Vector3f pos_prev{last_setpoint.position}; - - for (int i = 0; i < 3; i++) { - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } - } - - _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); - _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); - - return ret; -} - -void FlightTaskManualPositionSmoothVel::reActivate() -{ - FlightTaskManualPosition::reActivate(); - // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly - // using the generated jerk, reset the z derivatives to zero - _smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy()); - _smoothing_z.reset(0.f, 0.f, _position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) -{ - _smoothing_xy.setCurrentPosition(_position.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) -{ - _smoothing_xy.setCurrentVelocity(_velocity.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ(float delta_z) -{ - _smoothing_z.setCurrentPosition(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz) -{ - _smoothing_z.setCurrentVelocity(_velocity(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateSetpoints() -{ - // Set max accel/vel/jerk - // Has to be done before _updateTrajectories() - _updateTrajConstraints(); - _updateTrajVelFeedback(); - _updateTrajCurrentPositionEstimate(); - - // Get yaw setpoint, un-smoothed position setpoints - FlightTaskManualPosition::_updateSetpoints(); - - _updateTrajectories(_velocity_setpoint); - - // Fill the jerk, acceleration, velocity and position setpoint vectors - _setOutputState(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() -{ - _updateTrajConstraintsXY(); - _updateTrajConstraintsZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() -{ - _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy.setMaxVel(_param_mpc_vel_manual.get()); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() -{ - _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); - - _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); - _smoothing_z.setMaxVelUp(_constraints.speed_up); - - _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); - _smoothing_z.setMaxVelDown(_constraints.speed_down); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() -{ - _smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy()); - _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() -{ - _smoothing_xy.setCurrentPositionEstimate(_position.xy()); - _smoothing_z.setCurrentPositionEstimate(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) -{ - _smoothing_xy.update(_deltatime, vel_target.xy()); - _smoothing_z.update(_deltatime, vel_target(2)); -} - -void FlightTaskManualPositionSmoothVel::_setOutputState() -{ - _setOutputStateXY(); - _setOutputStateZ(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateXY() -{ - _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); - _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); - _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateZ() -{ - _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); - _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - - if (!_terrain_hold) { - if (_terrain_hold_previous) { - // Reset position setpoint to current position when switching from terrain hold to non-terrain hold - _smoothing_z.setCurrentPosition(_position(2)); - } - - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); - } - - _terrain_hold_previous = _terrain_hold; -} diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp deleted file mode 100644 index c8fbdf42c9..0000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 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. - * - ****************************************************************************/ - -/** - * @file FlightTaskManualPositionSmoothVel.hpp - * - * Flight task for smooth manual controlled position. - */ - -#pragma once - -#include "FlightTaskManualPosition.hpp" -#include -#include - -using matrix::Vector2f; -using matrix::Vector3f; - -class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition -{ -public: - FlightTaskManualPositionSmoothVel() = default; - virtual ~FlightTaskManualPositionSmoothVel() = default; - - bool activate(const trajectory_setpoint_s &last_setpoint) override; - void reActivate() override; - -protected: - - virtual void _updateSetpoints() override; - - /** Reset position or velocity setpoints in case of EKF reset event */ - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - void _ekfResetHandlerPositionZ(float delta_z) override; - void _ekfResetHandlerVelocityZ(float delta_vz) override; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ) - -private: - void _updateTrajConstraints(); - void _updateTrajConstraintsXY(); - void _updateTrajConstraintsZ(); - - void _updateTrajVelFeedback(); - void _updateTrajCurrentPositionEstimate(); - - void _updateTrajectories(Vector3f vel_target); - - void _setOutputState(); - void _setOutputStateXY(); - void _setOutputStateZ(); - - ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions - ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction - - bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ -}; diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp index bf5d2799d3..727d7cd529 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp @@ -41,41 +41,15 @@ using namespace matrix; FlightTaskTransition::FlightTaskTransition() { - _param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF"); - - if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { - param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); - } - -} - -bool FlightTaskTransition::updateInitialize() -{ - updateParameters(); - return FlightTask::updateInitialize(); -} - -void FlightTaskTransition::updateParameters() -{ - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) { - param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees); - } - } + param_get(param_find("FW_PSP_OFF"), &_param_fw_psp_off); + param_get(param_find("VT_B_DEC_I"), &_param_vt_b_dec_i); + param_get(param_find("VT_B_DEC_MSS"), &_param_vt_b_dec_mss); } bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTask::activate(last_setpoint); - _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); - if (PX4_ISFINITE(last_setpoint.velocity[2])) { _vel_z_filter.reset(last_setpoint.velocity[2]); @@ -83,14 +57,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) _vel_z_filter.reset(_velocity(2)); } - _velocity_setpoint(2) = _vel_z_filter.getState(); - - _sub_vehicle_status.update(); - - const bool is_vtol_front_transition = _sub_vehicle_status.get().in_transition_mode - && _sub_vehicle_status.get().in_transition_to_fw; - - if (is_vtol_front_transition) { + if (_sub_vehicle_status.get().in_transition_to_fw) { _gear.landing_gear = landing_gear_s::GEAR_UP; } else { @@ -100,23 +67,75 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint) return ret; } +bool FlightTaskTransition::updateInitialize() +{ + bool ret = FlightTask::updateInitialize(); + _sub_vehicle_status.update(); + _sub_position_sp_triplet.update(); + return ret; +} + bool FlightTaskTransition::update() { // tailsitters will override attitude and thrust setpoint // tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint bool ret = FlightTask::update(); - _position_setpoint.setAll(NAN); - - // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees - // and zero roll angle - const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); - _acceleration_setpoint.xy() = tmp * tanf(math::radians(_param_pitch_cruise_degrees)) * CONSTANTS_ONE_G; - // slowly move vertical velocity setpoint to zero - _vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const); - _velocity_setpoint(2) = _vel_z_filter.update(0.0f); + _velocity_setpoint(2) = _vel_z_filter.update(0.0f, _deltatime); + + // calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_fw_psp_off + // and zero roll angle + float pitch_setpoint = math::radians(_param_fw_psp_off); + + if (!_sub_vehicle_status.get().in_transition_to_fw) { + pitch_setpoint = computeBackTranstionPitchSetpoint(); + } + + // Calculate horizontal acceleration components to follow a pitch setpoint with the current vehicle heading + const Vector2f horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f); + _acceleration_setpoint.xy() = tanf(pitch_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction; _yaw_setpoint = NAN; return ret; } + +float FlightTaskTransition::computeBackTranstionPitchSetpoint() +{ + const Vector2f position_xy{_position}; + const Vector2f velocity_xy{_velocity}; + const Vector2f velocity_xy_direction = + velocity_xy.unit_or_zero(); // Zero when velocity invalid Vector2f(NAN, NAN).unit_or_zero() == Vector2f(0.f, 0.f) + + float deceleration_setpoint = _param_vt_b_dec_mss; + + if (_sub_position_sp_triplet.get().current.valid && _sub_vehicle_local_position.get().xy_global + && position_xy.isAllFinite() && velocity_xy.isAllFinite()) { + Vector2f position_setpoint_local; + _geo_projection.project(_sub_position_sp_triplet.get().current.lat, _sub_position_sp_triplet.get().current.lon, + position_setpoint_local(0), position_setpoint_local(1)); + + const Vector2f pos_to_target = position_setpoint_local - position_xy; // backtransition end-point w.r.t. vehicle + const float dist_to_target_in_moving_direction = pos_to_target.dot(velocity_xy_direction); + + if (dist_to_target_in_moving_direction > FLT_EPSILON) { + // Backtransition target point is ahead of the vehicle, compute the desired deceleration + deceleration_setpoint = velocity_xy.norm_squared() / (2.f * dist_to_target_in_moving_direction); + + } else { + deceleration_setpoint = 2.f * _param_vt_b_dec_mss; + } + + deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss); + } + + // Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow + const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay}; + const float deceleration = -acceleration_xy.dot(velocity_xy_direction); // Zero when velocity invalid + const float deceleration_error = deceleration_setpoint - deceleration; + + // Update back-transition deceleration error integrator + _decel_error_bt_int += (_param_vt_b_dec_i * deceleration_error) * _deltatime; + _decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, DECELERATION_INTEGRATOR_LIMIT); + return _decel_error_bt_int; +} diff --git a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp index 6e255dbd1c..12687223b5 100644 --- a/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp +++ b/src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -60,18 +61,18 @@ public: bool update() override; private: - - static constexpr float _vel_z_filter_time_const = 2.0f; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + static constexpr float VERTICAL_VELOCITY_TIME_CONSTANT = 2.0f; + static constexpr float DECELERATION_INTEGRATOR_LIMIT = .3f; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)}; - param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID}; - float _param_pitch_cruise_degrees{0.f}; + float _param_fw_psp_off{0.f}; + float _param_vt_b_dec_i{0.f}; + float _param_vt_b_dec_mss{0.f}; - AlphaFilter _vel_z_filter; - - void updateParameters(); + AlphaFilter _vel_z_filter{VERTICAL_VELOCITY_TIME_CONSTANT}; + float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value + float computeBackTranstionPitchSetpoint(); }; diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 911da11d26..44a5e1dba1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,9 +36,10 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp + Gimbal.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp new file mode 100644 index 0000000000..21e97f5fc3 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "Gimbal.hpp" +#include + +using namespace time_literals; +using namespace matrix; + +Gimbal::Gimbal(ModuleParams *parent) : + ModuleParams(parent) +{} + +Gimbal::~Gimbal() +{ + releaseGimbalControlIfNeeded(); +} + +bool Gimbal::checkForTelemetry(const hrt_abstime now) +{ + if (_gimbal_device_attitude_status_sub.updated()) { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status)) { + _telemtry_timestamp = gimbal_device_attitude_status.timestamp; + _telemetry_flags = gimbal_device_attitude_status.device_flags; + _telemetry_yaw = Eulerf(Quatf(gimbal_device_attitude_status.q)).psi(); + } + } + + return now < _telemtry_timestamp + 2_s; +} + +void Gimbal::acquireGimbalControlIfNeeded() +{ + if (!_have_gimbal_control) { + _have_gimbal_control = true; + + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _param_mav_sys_id.get(); + vehicle_command.param2 = _param_mav_comp_id.get(); + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_sys_id.get(); + vehicle_command.from_external = false; + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::releaseGimbalControlIfNeeded() +{ + if (_have_gimbal_control) { + _have_gimbal_control = false; + + // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior + publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, + Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, 0.f, 0.f)); + + // Release gimbal + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = -3.0f; // Remove control if it had it. + vehicle_command.param2 = -3.0f; // Remove control if it had it. + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.from_external = false; + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_comp_id.get(); + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates) +{ + gimbal_manager_set_attitude_s gimbal_setpoint{}; + gimbal_setpoint.origin_sysid = _param_mav_sys_id.get(); + gimbal_setpoint.origin_compid = _param_mav_comp_id.get(); + gimbal_setpoint.flags = gimbal_flags; + q_gimbal_setpoint.copyTo(gimbal_setpoint.q); + gimbal_setpoint.angular_velocity_x = gimbal_rates(0); + gimbal_setpoint.angular_velocity_y = gimbal_rates(1); + gimbal_setpoint.angular_velocity_z = gimbal_rates(2); + gimbal_setpoint.timestamp = hrt_absolute_time(); + _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp new file mode 100644 index 0000000000..938ef32aa2 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +/** + * @file Gimbal.hpp + * @brief Gimbal interface with flight tasks + * @author Pernilla Wikström + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Sticks.hpp" + + +class Gimbal : public ModuleParams +{ +public: + Gimbal(ModuleParams *parent); + ~Gimbal(); + + bool checkForTelemetry(const hrt_abstime now); + void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates); + void acquireGimbalControlIfNeeded(); + void releaseGimbalControlIfNeeded(); + float getTelemetryYaw() { return _telemetry_yaw; } + bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } + + static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK; + static constexpr uint16_t FLAGS_ROLL_PITCH_LOCKED = + gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + +private: + bool _have_gimbal_control{false}; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + hrt_abstime _telemtry_timestamp{}; + uint16_t _telemetry_flags{}; + float _telemetry_yaw{}; + + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id + ) +}; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c0fe9b315b..c36d024ead 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -73,8 +73,20 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) { + // gradually adjust velocity constraint because good tracking is required for the drag estimation + if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) { + if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) { + _current_velocity_constraint = _targeted_velocity_constraint; + + } else { + _current_velocity_constraint = math::constrain(_targeted_velocity_constraint, + _current_velocity_constraint - dt * _param_mpc_acc_hor.get(), + _current_velocity_constraint + dt * _param_mpc_acc_hor.get()); + } + } + // maximum commanded velocity can be constrained dynamically - const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint); + const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint); Vector2f velocity_scale(velocity_sc, velocity_sc); // maximum commanded acceleration is scaled down with velocity const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get()); @@ -96,6 +108,10 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp); _acceleration_setpoint = stick_xy.emult(acceleration_scale); + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(_acceleration_setpoint, _velocity_setpoint); + } + // Add drag to limit speed and brake again Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 8bc2bb7531..1b29580c4f 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -62,9 +63,12 @@ public: void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp); float getMaxAcceleration() { return _param_mpc_acc_hor.get(); }; float getMaxJerk() { return _param_mpc_jerk_max.get(); }; - void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; + void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); }; + float getVelocityConstraint() { return _current_velocity_constraint; }; private: + CollisionPrevention _collision_prevention{this}; + void applyJerkLimit(const float dt); matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, const matrix::Vector2f &vel_sp); @@ -82,7 +86,8 @@ private: matrix::Vector2f _acceleration_setpoint; matrix::Vector2f _acceleration_setpoint_prev; - float _velocity_constraint{INFINITY}; + float _targeted_velocity_constraint{INFINITY}; + float _current_velocity_constraint{INFINITY}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_vel_manual, diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 608e4443c3..0118e53cd4 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -57,9 +57,8 @@ void StickYaw::ekfResetHandler(const float delta_yaw) void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, const float yaw, const float deltatime, const float unaided_yaw) { - _yaw_error_lpf.setParameters(deltatime, _kYawErrorTimeConstant); const float yaw_correction_prev = _yaw_correction; - const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw); + const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw, deltatime); if (reset_setpoint) { yaw_setpoint = NAN; @@ -71,7 +70,7 @@ void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev); } -bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw) +bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw, const float deltatime) { if (!PX4_ISFINITE(unaided_yaw)) { _yaw_correction = 0.f; @@ -84,7 +83,7 @@ bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw) // Run it through a high-pass filter to detect transients const float yaw_error_hpf = wrap_pi(yaw_error - _yaw_error_lpf.getState()); - _yaw_error_lpf.update(yaw_error); + _yaw_error_lpf.update(yaw_error, deltatime); const bool was_converging = _yaw_estimate_converging; _yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index f15b4dd574..a1fdaae9f0 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -60,11 +60,11 @@ private: float _yaw_error_ref{0.f}; float _yaw_correction{0.f}; bool _yaw_estimate_converging{false}; - AlphaFilter _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter + AlphaFilter _yaw_error_lpf{_kYawErrorTimeConstant}; ///< used to create a high-pass filter static constexpr float _kYawErrorTimeConstant{1.f}; ///< time constant of the high-pass filter used to detect yaw convergence static constexpr float _kYawErrorChangeThreshold{radians(1.f)}; ///< we consider the yaw estimate as "converging" when above this threshold - bool updateYawCorrection(float yaw, float unaided_yaw); + bool updateYawCorrection(float yaw, float unaided_yaw, float deltatime); /** * Lock yaw when not currently turning diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 988853fdea..9057d792b9 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -214,9 +214,6 @@ void FixedwingAttitudeControl::Run() _last_run = time_now_us; } - vehicle_angular_velocity_s angular_velocity{}; - _vehicle_rates_sub.copy(&angular_velocity); - if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -324,22 +321,22 @@ void FixedwingAttitudeControl::Run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { - const Eulerf setpoint(Quatf(_att_sp.q_d)); - const float roll_body = setpoint.phi(); - const float pitch_body = setpoint.theta(); + const Quatf q_sp(_att_sp.q_d); - if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) { + if (q_sp.isAllFinite()) { + const Eulerf euler_sp(q_sp); + const float roll_sp = euler_sp.phi(); + const float pitch_sp = euler_sp.theta(); - _roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _roll_ctrl.control_roll(roll_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _pitch_ctrl.control_pitch(pitch_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { - Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); - _wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi()); + _wheel_ctrl.control_attitude(euler_sp.psi(), euler_angles.psi()); } else { _wheel_ctrl.reset_integrator(); @@ -394,10 +391,13 @@ void FixedwingAttitudeControl::Run() wheel_u = _manual_control_setpoint.yaw; } else { + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from // position controller during auto modes _manual_control_setpoint.r gets passed // whenever nudging is enabled, otherwise zero - const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, euler_angles.psi(), _groundspeed, + const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, angular_velocity.xyz[2], _groundspeed, groundspeed_scale); wheel_u = wheel_control ? wheel_controller_output + _att_sp.yaw_sp_move_rate : 0.f; } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4a20c0cf69..5a55be1cc8 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -487,8 +487,6 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - pos_ctrl_status.yaw_acceptance = NAN; - pos_ctrl_status.timestamp = hrt_absolute_time(); pos_ctrl_status.type = _position_sp_type; @@ -690,6 +688,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current; _skipping_takeoff_detection = false; + const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid && _control_mode.flag_control_position_enabled) { @@ -711,10 +710,9 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. - const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; if (doing_backtransition) { - _control_mode_current = FW_POSCTRL_MODE_TRANSITON; + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW; } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -764,8 +762,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _time_in_fixed_bank_loiter = now; } - if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) - && !_vehicle_status.in_transition_mode) { + if (doing_backtransition) { + // we handle loss of position control during backtransition as a special case + _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; + + } else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) + && !_vehicle_status.in_transition_mode) { if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) { // Need to init because last loop iteration was in a different mode events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical, @@ -961,7 +963,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle const float yaw_body = 0.f; - if (_landed) { + // Special case: if z or vz estimate is invalid we cannot control height anymore. To prevent a + // "climb-away" we set the thrust to MIN in that case. + if (_landed || !_local_pos.z_valid || !_local_pos.v_z_valid) { _att_sp.thrust_body[0] = _param_fw_thr_min.get(); } else { @@ -1000,7 +1004,11 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle const float yaw_body = 0.f; - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + // Special case: if vz estimate is invalid we cannot control height rate anymore. To prevent a + // "climb-away" we set the thrust to MIN in that case. + _att_sp.thrust_body[0] = (_landed + || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + const float pitch_body = get_tecs_pitch(); const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); attitude_setpoint.copyTo(_att_sp.q_d); @@ -1848,8 +1856,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, POST_TOUCHDOWN_CLAMP_TIME, 0.0f, 1.0f); - pitch_max_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_max_rad; - pitch_min_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_min_rad; + pitch_max_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_max_rad; + pitch_min_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_min_rad; } // idle throttle may be >0 for internal combustion engines @@ -2068,8 +2078,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float touchdown_interpolator = math::constrain((seconds_since_flare_start - touchdown_time) / POST_TOUCHDOWN_CLAMP_TIME, 0.0f, 1.0f); - pitch_max_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_max_rad; - pitch_min_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_min_rad; + pitch_max_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_max_rad; + pitch_min_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_min_rad; } // idle throttle may be >0 for internal combustion engines @@ -2346,18 +2358,49 @@ FixedwingPositionControl::control_manual_position(const float control_interval, attitude_setpoint.copyTo(_att_sp.q_d); } -void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, +void FixedwingPositionControl::control_backtransition_heading_hold() +{ + if (!PX4_ISFINITE(_backtrans_heading)) { + _backtrans_heading = _local_pos.heading; + } + + float true_airspeed = _airspeed_eas * _eas2tas; + + if (!_airspeed_valid) { + true_airspeed = _performance_model.getCalibratedTrimAirspeed() * _eas2tas; + } + + // we can achieve heading control by setting airspeed and groundspeed vector equal + const Vector2f airspeed_vector = Vector2f(cosf(_local_pos.heading), sinf(_local_pos.heading)) * true_airspeed; + const Vector2f &ground_speed = airspeed_vector; + + _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); + _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); + + Vector2f virtual_target_point = Vector2f(cosf(_backtrans_heading), sinf(_backtrans_heading)) * HDG_HOLD_DIST_NEXT; + + navigateLine(Vector2f(0.f, 0.f), virtual_target_point, Vector2f(0.f, 0.f), ground_speed, Vector2f(0.f, 0.f)); + + const float roll_body = getCorrectedNpfgRollSetpoint(); + + const float yaw_body = _backtrans_heading; + + // these values are overriden by transition logic + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + const float pitch_body = 0.0f; + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + +} + +void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - const bool is_low_height = checkLowHeightConditions(); - - float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, - _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed); - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); - _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedNom(_performance_model.getCalibratedTrimAirspeed() * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); // Set the position where the backtransition started the first ime we pass through here. @@ -2367,25 +2410,13 @@ void FixedwingPositionControl::control_backtransition(const float control_interv } navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + const float roll_body = getCorrectedNpfgRollSetpoint(); - float roll_body = getCorrectedNpfgRollSetpoint(); - target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + const float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw - - tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, - target_airspeed, - radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_sinkrate_target.get(), - _param_climbrate_target.get(), - is_low_height); - - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - const float pitch_body = get_tecs_pitch(); + // these values are overriden by transition logic + _att_sp.thrust_body[0] = _param_fw_thr_min.get(); + const float pitch_body = 0.0f; const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); attitude_setpoint.copyTo(_att_sp.q_d); @@ -2593,6 +2624,7 @@ FixedwingPositionControl::Run() if (!_vehicle_status.in_transition_mode) { // reset position of backtransition start if not in transition _lpos_where_backtrans_started = Vector2f(NAN, NAN); + _backtrans_heading = NAN; } } @@ -2689,8 +2721,13 @@ FixedwingPositionControl::Run() break; } - case FW_POSCTRL_MODE_TRANSITON: { - control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current); + case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW: { + control_backtransition_line_follow(ground_speed, _pos_sp_triplet.current); + break; + } + + case FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD: { + control_backtransition_heading_hold(); break; } } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5600e71fcd..e56263c5e3 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -260,7 +260,8 @@ private: FW_POSCTRL_MODE_AUTO_PATH, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, - FW_POSCTRL_MODE_TRANSITON, + FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW, + FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD, FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed @@ -409,6 +410,7 @@ private: // VTOL / TRANSITION bool _is_vtol_tailsitter{false}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; + float _backtrans_heading{NAN}; // used to lock the initial heading for backtransition with no position control // ESTIMATOR RESET COUNTERS uint8_t _xy_reset_counter{0}; @@ -707,15 +709,20 @@ private: */ void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed); + /** + * @brief Holds the initial heading during the course of a transition to hover. Used when there is no local + * position to do line following. + */ + void control_backtransition_heading_hold(); + /** * @brief Controls flying towards a transition waypoint and then transitioning to MC mode. * - * @param control_interval Time since last position control call [s] * @param ground_speed Local 2D ground speed of vehicle [m/s] * @param pos_sp_curr current position setpoint */ - void control_backtransition(const float control_interval, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_curr); + void control_backtransition_line_follow(const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr); float get_tecs_pitch(); float get_tecs_thrust(); diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index f0c8683386..2e2005997a 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -34,8 +34,8 @@ /** * Path navigation roll slew rate limit. * - * The maximum change in roll angle setpoint per second. - * This limit is applied in all Auto modes, plus manual Position and Altitude modes. + * Maximum change in roll angle setpoint per second. + * Applied in all Auto modes, plus manual Position & Altitude modes. * * @unit deg/s * @min 0 @@ -48,7 +48,7 @@ PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f); /** * NPFG period * - * Period of the NPFG control law. + * Period of NPFG control law. * * @unit s * @min 1.0 @@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(NPFG_PERIOD, 10.0f); /** * NPFG damping ratio * - * Damping ratio of the NPFG control law. + * Damping ratio of NPFG control law. * * @min 0.10 * @max 1.00 @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f); * Enable automatic lower bound on the NPFG period * * Avoids limit cycling from a too aggressively tuned period/damping combination. - * If set to false, also disables the upper bound NPFG_PERIOD_UB. + * If false, also disables upper bound NPFG_PERIOD_UB. * * @boolean * @group FW NPFG Control @@ -339,8 +339,6 @@ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f); * * This is critical for detecting when to flare, and should be enabled if possible. * - * NOTE: terrain estimate is currently solely derived from a distance sensor. - * * If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing * will be aborted, depending on the criteria set in FW_LND_ABORT. * @@ -428,12 +426,12 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); /** * Low-height threshold for tighter altitude tracking * - * Defines the height (distance to bottom) threshold below which tighter altitude + * Height above ground threshold below which tighter altitude * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC * to FW_LND_THRTC_SC*FW_T_ALT_TC. * - * If equal to -1, low-height traking is disabled. + * -1 to disable. * * @unit m * @min -1 @@ -451,8 +449,6 @@ PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * * @unit m/s * @min 1.0 * @max 15.0 @@ -579,15 +575,12 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f); PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); /** - * Speed <--> Altitude priority + * Speed <--> Altitude weight * * Adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Set to 2 for gliders. + * applies to speed vs height errors. + * 0 -> control height only + * 2 -> control speed only (gliders) * * @min 0.0 * @max 2.0 diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 1f2cff00d6..9841ea8ca6 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -84,10 +84,6 @@ FixedwingRateControl::parameters_update() _rate_control.setIntegratorLimit( Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get())); - _rate_control.setFeedForwardGain( - // set FF gains to 0 as we add the FF control outside of the rate controller - Vector3f(0.f, 0.f, 0.f)); - if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) { param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en); } @@ -354,14 +350,15 @@ void FixedwingRateControl::Run() body_rates_setpoint = Vector3f(-_rates_sp.yaw, _rates_sp.pitch, _rates_sp.roll); } + const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); + const Vector3f scaled_gain_ff = gain_ff / _airspeed_scaling; + _rate_control.setFeedForwardGain(scaled_gain_ff); + // Run attitude RATE controllers which need the desired attitudes from above, add trim. const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt, _landed); - const Vector3f gain_ff(_param_fw_rr_ff.get(), _param_fw_pr_ff.get(), _param_fw_yr_ff.get()); - const Vector3f feedforward = gain_ff.emult(body_rates_setpoint) * _airspeed_scaling; - - Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling + feedforward; + Vector3f control_u = angular_acceleration_setpoint * _airspeed_scaling * _airspeed_scaling; // Special case yaw in Acro: if the parameter FW_ACRO_YAW_CTL is not set then don't control yaw if (!_vcontrol_mode.flag_control_attitude_enabled && !_param_fw_acro_yaw_en.get()) { diff --git a/src/modules/gimbal/common.h b/src/modules/gimbal/common.h index e7f29b726b..2436bd68dd 100644 --- a/src/modules/gimbal/common.h +++ b/src/modules/gimbal/common.h @@ -83,6 +83,7 @@ struct ControlData { // uint8_t sysid_secondary_control = 0; // The MAVLink system ID selected for additional input, not implemented yet. // uint8_t compid_secondary_control = 0; // The MAVLink component ID selected for additional input, not implemented yet. uint8_t device_compid = 0; + uint64_t timestamp_last_update{0}; // Timestamp when there was the last setpoint set by the input used for timeout }; diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 804f792fc6..9179055b31 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -113,7 +113,7 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.input_objs[thread_data.input_objs_len++] = thread_data.test_input; switch (params.mnt_mode_in) { - case 0: + case MNT_MODE_IN_AUTO: // Automatic // MAVLINK_V2 as well as RC input are supported together. // Whichever signal is updated last, gets control, for RC there is a deadzone @@ -123,19 +123,19 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params); break; - case 1: // RC only + case MNT_MODE_IN_RC: // RC only thread_data.input_objs[thread_data.input_objs_len++] = new InputRC(params); break; - case 2: // MAVLINK_ROI commands only (to be deprecated) + case MNT_MODE_IN_MAVLINK_ROI: // MAVLINK_ROI commands only (to be deprecated) thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkROI(params); break; - case 3: // MAVLINK_DO_MOUNT commands only (to be deprecated) + case MNT_MODE_IN_MAVLINK_DO_MOUNT: // MAVLINK_DO_MOUNT commands only (to be deprecated) thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkCmdMount(params); break; - case 4: //MAVLINK_V2 + case MNT_MODE_IN_MAVLINK_V2: //MAVLINK_V2 thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params); break; @@ -165,21 +165,21 @@ static int gimbal_thread_main(int argc, char *argv[]) } switch (params.mnt_mode_out) { - case 0: //AUX + case MNT_MODE_OUT_AUX: //AUX thread_data.output_obj = new OutputRC(params); if (!thread_data.output_obj) { alloc_failed = true; } break; - case 1: //MAVLink gimbal v1 protocol + case MNT_MODE_OUT_MAVLINK_V1: //MAVLink gimbal v1 protocol thread_data.output_obj = new OutputMavlinkV1(params); if (!thread_data.output_obj) { alloc_failed = true; } break; - case 2: //MAVLink gimbal v2 protocol + case MNT_MODE_OUT_MAVLINK_V2: //MAVLink gimbal v2 protocol thread_data.output_obj = new OutputMavlinkV2(params); if (!thread_data.output_obj) { alloc_failed = true; } @@ -264,6 +264,11 @@ static int gimbal_thread_main(int argc, char *argv[]) thread_data.output_obj->set_stabilize(false, false, false); } + if (thread_data.output_obj->check_and_handle_setpoint_timeout(thread_data.control_data, hrt_absolute_time())) { + // Without flagging an update the changes are not processed in the output + update_result = InputBase::UpdateResult::UpdatedActive; + } + // Update output thread_data.output_obj->update( thread_data.control_data, @@ -271,7 +276,7 @@ static int gimbal_thread_main(int argc, char *argv[]) // Only publish the mount orientation if the mode is not mavlink v1 or v2 // If the gimbal speaks mavlink it publishes its own orientation. - if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2 + if (params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V1 && params.mnt_mode_out != MNT_MODE_OUT_MAVLINK_V2) { thread_data.output_obj->publish(); } @@ -364,27 +369,77 @@ int gimbal_main(int argc, char *argv[]) if (thread_running.load() && g_thread_data && g_thread_data->test_input) { if (argc >= 4) { - bool found_axis = false; - const char *axis_names[3] = {"roll", "pitch", "yaw"}; - int angles[3] = { 0, 0, 0 }; + + float roll_deg = 0.0f; + float pitch_deg = 0.0f; + float yaw_deg = 0.0f; + float rollrate_deg_s = 0.0f; + float pitchrate_deg_s = 0.0f; + float yawrate_deg_s = 0.0f; + + bool angles_set = false; + bool rates_set = false; for (int arg_i = 2 ; arg_i < (argc - 1); ++arg_i) { - for (int axis_i = 0; axis_i < 3; ++axis_i) { - if (!strcmp(argv[arg_i], axis_names[axis_i])) { - int angle_deg = (int)strtol(argv[arg_i + 1], nullptr, 0); - angles[axis_i] = angle_deg; - found_axis = true; - } + + if (!strcmp(argv[arg_i], "roll")) { + roll_deg = (int)strtof(argv[arg_i + 1], nullptr); + angles_set = true; + + } else if (!strcmp(argv[arg_i], "pitch")) { + pitch_deg = (int)strtof(argv[arg_i + 1], nullptr); + angles_set = true; + + } else if (!strcmp(argv[arg_i], "yaw")) { + yaw_deg = (int)strtof(argv[arg_i + 1], nullptr); + angles_set = true; + + } else if (!strcmp(argv[arg_i], "rollrate")) { + rollrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr); + rates_set = true; + + } else if (!strcmp(argv[arg_i], "pitchrate")) { + pitchrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr); + rates_set = true; + + } else if (!strcmp(argv[arg_i], "yawrate")) { + yawrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr); + rates_set = true; + + } else { + PX4_ERR("Unknown argument: %s", argv[arg_i]); + usage(); + return -1; } } - if (!found_axis) { + if (angles_set && rates_set) { + PX4_ERR("This driver doesn't support both, angles and rates, to be set"); + usage(); + return -1; + + } else if (angles_set) { + g_thread_data->test_input->set_test_input_angles( + roll_deg, + pitch_deg, + yaw_deg + ); + return 0; + + } else if (rates_set) { + + g_thread_data->test_input->set_test_input_angle_rates( + rollrate_deg_s, + pitchrate_deg_s, + yawrate_deg_s + ); + return 0; + + } else { + PX4_ERR("No angles or angle rates set"); usage(); return -1; } - - g_thread_data->test_input->set_test_input(angles[0], angles[1], angles[2]); - return 0; } } else { @@ -568,5 +623,6 @@ $ gimbal test pitch -45 yaw 30 PRINT_MODULE_USAGE_ARG(" ", "MAVLink system ID and MAVLink component ID", false); PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one or multiple axes (gimbal must be running)"); PRINT_MODULE_USAGE_ARG("roll|pitch|yaw ", "Specify an axis and an angle in degrees", false); + PRINT_MODULE_USAGE_ARG("rollrate|pitchrate|yawrate ", "Specify an axis and an angle rate in degrees / second", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index f1f3d2d514..5a265d0237 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -40,6 +40,21 @@ namespace gimbal { +enum MntModeIn { + MNT_MODE_IN_DISABLED = -1, + MNT_MODE_IN_AUTO, // RC and MAVLink gimbal protocol v2 + MNT_MODE_IN_RC, + MNT_MODE_IN_MAVLINK_ROI, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_IN_MAVLINK_DO_MOUNT, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_IN_MAVLINK_V2 // MAVLink gimbal protocol v2 +}; + +enum MntModeOut { + MNT_MODE_OUT_AUX = 0, + MNT_MODE_OUT_MAVLINK_V1, // MAVLink gimbal protocol v1 (to be deprecated) + MNT_MODE_OUT_MAVLINK_V2 // MAVLink gimbal protocol v2 +}; + struct Parameters { int32_t mnt_mode_in; int32_t mnt_mode_out; diff --git a/src/modules/gimbal/input.cpp b/src/modules/gimbal/input.cpp index 93ab31b4e6..a6c5c137a9 100644 --- a/src/modules/gimbal/input.cpp +++ b/src/modules/gimbal/input.cpp @@ -42,15 +42,15 @@ InputBase::InputBase(Parameters ¶meters) : {} void InputBase::control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, - float roll_angle, - float pitch_fixed_angle) + uint64_t timestamp) { + control_data.timestamp_last_update = timestamp; control_data.type = ControlData::Type::LonLat; control_data.type_data.lonlat.lon = lon; control_data.type_data.lonlat.lat = lat; control_data.type_data.lonlat.altitude = altitude; - control_data.type_data.lonlat.roll_offset = roll_angle; - control_data.type_data.lonlat.pitch_fixed_angle = pitch_fixed_angle; + control_data.type_data.lonlat.roll_offset = NAN; + control_data.type_data.lonlat.pitch_fixed_angle = NAN; control_data.type_data.lonlat.pitch_offset = 0.f; control_data.type_data.lonlat.yaw_offset = 0.f; } diff --git a/src/modules/gimbal/input.h b/src/modules/gimbal/input.h index d849d37137..49ae53f274 100644 --- a/src/modules/gimbal/input.h +++ b/src/modules/gimbal/input.h @@ -60,8 +60,7 @@ public: virtual UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) = 0; virtual void print_status() const = 0; protected: - void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, float roll_angle = NAN, - float pitch_fixed_angle = NAN); + void control_data_set_lon_lat(ControlData &control_data, double lon, double lat, float altitude, uint64_t timestamp); Parameters &_parameters; }; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index ebfde30d2a..aef348ca53 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -43,6 +43,8 @@ #include #include +using namespace matrix; + namespace gimbal { @@ -100,6 +102,7 @@ InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_roi.timestamp; _cur_roi_mode = vehicle_roi.mode; control_data.sysid_primary_control = _parameters.mav_sysid; control_data.compid_primary_control = _parameters.mav_compid; @@ -120,7 +123,7 @@ InputMavlinkROI::update(unsigned int timeout_ms, ControlData &control_data, bool return UpdateResult::UpdatedActive; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { - control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt, vehicle_roi.timestamp); _cur_roi_mode = vehicle_roi.mode; @@ -155,6 +158,7 @@ void InputMavlinkROI::_read_control_data_from_position_setpoint_sub(ControlData { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + control_data.timestamp_last_update = position_setpoint_triplet.timestamp; control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; @@ -261,6 +265,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ // fallthrough case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.sysid_primary_control = vehicle_command.source_system; control_data.compid_primary_control = vehicle_command.source_component; update_result = UpdateResult::UpdatedActiveOnce; @@ -268,6 +273,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; @@ -302,7 +308,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5, - vehicle_command.param4); + vehicle_command.param4, vehicle_command.timestamp); control_data.sysid_primary_control = vehicle_command.source_system; control_data.compid_primary_control = vehicle_command.source_component; update_result = UpdateResult::UpdatedActive; @@ -348,6 +354,7 @@ InputMavlinkCmdMount::_process_command(ControlData &control_data, const vehicle_ } } + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.sysid_primary_control = vehicle_command.source_system; control_data.compid_primary_control = vehicle_command.source_component; @@ -468,28 +475,49 @@ void InputMavlinkGimbalV2::_stream_gimbal_manager_status(const ControlData &cont void InputMavlinkGimbalV2::_stream_gimbal_manager_information(const ControlData &control_data) { - // TODO: Take gimbal_device_information into account. + gimbal_device_information_s gimbal_device_info; - gimbal_manager_information_s gimbal_manager_info; - gimbal_manager_info.timestamp = hrt_absolute_time(); + if (_gimbal_device_information_sub.update(&gimbal_device_info) && _parameters.mnt_mode_out == MNT_MODE_OUT_MAVLINK_V2) { + gimbal_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); - gimbal_manager_info.cap_flags = - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | - gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + gimbal_manager_info.cap_flags = gimbal_device_info.cap_flags; - gimbal_manager_info.pitch_max = M_PI_F / 2; - gimbal_manager_info.pitch_min = -M_PI_F / 2; - gimbal_manager_info.yaw_max = M_PI_F; - gimbal_manager_info.yaw_min = -M_PI_F; + gimbal_manager_info.roll_max = gimbal_device_info.roll_max; + gimbal_manager_info.roll_min = gimbal_device_info.roll_min; + gimbal_manager_info.pitch_max = gimbal_device_info.pitch_max; + gimbal_manager_info.pitch_min = gimbal_device_info.pitch_min; + gimbal_manager_info.yaw_max = gimbal_device_info.yaw_max; + gimbal_manager_info.yaw_min = gimbal_device_info.yaw_min; - gimbal_manager_info.gimbal_device_id = control_data.device_compid; + gimbal_manager_info.gimbal_device_id = control_data.device_compid; + + _gimbal_manager_info_pub.publish(gimbal_manager_info); + + } else if (_parameters.mnt_mode_out == MNT_MODE_OUT_AUX) { + // since we have a non-Mavlink gimbal device, the gimbal manager itself has to act as the gimbal device + gimbal_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); + + gimbal_manager_info.cap_flags = + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + + gimbal_manager_info.pitch_max = _parameters.mnt_range_pitch; + gimbal_manager_info.pitch_min = -_parameters.mnt_range_pitch; + gimbal_manager_info.yaw_max = _parameters.mnt_range_yaw; + gimbal_manager_info.yaw_min = -_parameters.mnt_range_yaw; + + gimbal_manager_info.gimbal_device_id = control_data.device_compid; + + _gimbal_manager_info_pub.publish(gimbal_manager_info); + } - _gimbal_manager_info_pub.publish(gimbal_manager_info); } InputMavlinkGimbalV2::UpdateResult @@ -603,7 +631,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_attitude(C set_attitude.angular_velocity_y, set_attitude.angular_velocity_z); - _set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity); + _set_control_data_from_set_attitude(control_data, set_attitude.flags, q, angular_velocity, set_attitude.timestamp); return UpdateResult::UpdatedActive; } else { @@ -619,11 +647,13 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(Co if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_roi.timestamp; _cur_roi_mode = vehicle_roi.mode; return UpdateResult::UpdatedActiveOnce; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { control_data.type = ControlData::Type::LonLat; + control_data.timestamp_last_update = vehicle_roi.timestamp; _read_control_data_from_position_setpoint_sub(control_data); control_data.type_data.lonlat.pitch_fixed_angle = -10.f; @@ -636,7 +666,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_vehicle_roi(Co return UpdateResult::UpdatedActive; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { - control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + control_data_set_lon_lat(control_data, vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt, vehicle_roi.timestamp); _cur_roi_mode = vehicle_roi.mode; @@ -656,6 +686,7 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_position_setpo const position_setpoint_triplet_s &position_setpoint_triplet) { if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + control_data.timestamp_last_update = position_setpoint_triplet.timestamp; control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; @@ -692,11 +723,13 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: control_data.type = ControlData::Type::Neutral; + control_data.timestamp_last_update = vehicle_command.timestamp; update_result = InputBase::UpdateResult::UpdatedActiveOnce; break; case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = vehicle_command.timestamp; control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; @@ -731,7 +764,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: control_data_set_lon_lat(control_data, (double)vehicle_command.param6, (double)vehicle_command.param5, - vehicle_command.param4); + vehicle_command.param4, vehicle_command.timestamp); update_result = UpdateResult::UpdatedActive; break; } @@ -872,13 +905,13 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); const matrix::Quatf q(euler); - const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, - vehicle_command.param4); + const matrix::Vector3f angular_velocity(NAN, math::radians(vehicle_command.param3), + math::radians(vehicle_command.param4)); const uint32_t flags = vehicle_command.param5; // TODO: support gimbal device id for multiple gimbals - _set_control_data_from_set_attitude(control_data, flags, q, angular_velocity); + _set_control_data_from_set_attitude(control_data, flags, q, angular_velocity, vehicle_command.timestamp); _ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -905,24 +938,22 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con if (set_manual_control.origin_sysid == control_data.sysid_primary_control && set_manual_control.origin_compid == control_data.compid_primary_control) { - const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) - ? - matrix::Quatf( - matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) - : - matrix::Quatf(NAN, NAN, NAN, NAN); + Quatf q(NAN, NAN, NAN, NAN); - const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && - PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); + if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) { + q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)); + } + + Vector3f angular_velocity(NAN, NAN, NAN); + + if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) { + angular_velocity = Vector3f(0.0f, + set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch), + set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw)); + } _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, - angular_velocity); + angular_velocity, set_manual_control.timestamp); return UpdateResult::UpdatedActive; @@ -934,9 +965,10 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con } void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, - const matrix::Quatf &q, - const matrix::Vector3f &angular_velocity) + const matrix::Quatf &q, const matrix::Vector3f &angular_velocity, const uint64_t timestamp) { + control_data.timestamp_last_update = timestamp; + if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) { // not implemented in ControlData } else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) { @@ -984,6 +1016,7 @@ void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub(Control { position_setpoint_triplet_s position_setpoint_triplet; orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + control_data.timestamp_last_update = position_setpoint_triplet.timestamp; control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index 04264410d7..a47af82fc4 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -112,7 +113,7 @@ private: UpdateResult _process_set_manual_control(ControlData &control_data, const gimbal_manager_set_manual_control_s &set_manual_control); void _set_control_data_from_set_attitude(ControlData &control_data, const uint32_t flags, const matrix::Quatf &q, - const matrix::Vector3f &angular_velocity); + const matrix::Vector3f &angular_velocity, const uint64_t timestamp); void _ack_vehicle_command(const vehicle_command_s &cmd, uint8_t result); void _stream_gimbal_manager_information(const ControlData &control_data); void _stream_gimbal_manager_status(const ControlData &control_data); @@ -125,6 +126,7 @@ private: int _vehicle_command_sub = -1; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index c15f916ab9..3c2926ffbc 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -99,6 +99,7 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData manual_control_setpoint_s manual_control_setpoint{}; orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint); control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = manual_control_setpoint.timestamp; float new_aux_values[3]; @@ -129,9 +130,11 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData if (_parameters.mnt_rc_in_mode == 0) { // We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees. - matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f), + // We use 179.99 instead of 180 so to avoid that the conversion between quaternions and Euler representation + // (when new_aux_value = 1) gives the equivalent angle (e.g., -180 instead of 180). + matrix::Eulerf euler(new_aux_values[0] * math::radians(179.99f), new_aux_values[1] * math::radians(90.f), - new_aux_values[2] * math::radians(180.f)); + new_aux_values[2] * math::radians(179.99f)); matrix::Quatf q(euler); q.copyTo(control_data.type_data.angle.q); diff --git a/src/modules/gimbal/input_test.cpp b/src/modules/gimbal/input_test.cpp index 146266d80e..67dc952512 100644 --- a/src/modules/gimbal/input_test.cpp +++ b/src/modules/gimbal/input_test.cpp @@ -53,22 +53,32 @@ InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData & } control_data.type = ControlData::Type::Angle; + control_data.timestamp_last_update = hrt_absolute_time(); - control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; - control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + if (PX4_ISFINITE(_roll_deg) && PX4_ISFINITE(_pitch_deg) && PX4_ISFINITE(_yaw_deg)) { + control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + matrix::Eulerf euler( + math::radians((float)_roll_deg), + math::radians((float)_pitch_deg), + math::radians((float)_yaw_deg)); + matrix::Quatf q(euler); + q.copyTo(control_data.type_data.angle.q); - matrix::Eulerf euler( - math::radians((float)_roll_deg), - math::radians((float)_pitch_deg), - math::radians((float)_yaw_deg)); - matrix::Quatf q(euler); + } else { + control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate; + control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate; + control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate; + control_data.type_data.angle.q[0] = NAN; + control_data.type_data.angle.q[1] = NAN; + control_data.type_data.angle.q[2] = NAN; + control_data.type_data.angle.q[3] = NAN; + control_data.type_data.angle.angular_velocity[0] = math::radians(_rollrate_deg_s); + control_data.type_data.angle.angular_velocity[1] = math::radians(_pitchrate_deg_s); + control_data.type_data.angle.angular_velocity[2] = math::radians(_yawrate_deg_s); + } - q.copyTo(control_data.type_data.angle.q); - - control_data.type_data.angle.angular_velocity[0] = NAN; - control_data.type_data.angle.angular_velocity[1] = NAN; - control_data.type_data.angle.angular_velocity[2] = NAN; // For testing we mark ourselves as in control. control_data.sysid_primary_control = _parameters.mav_sysid; @@ -86,17 +96,32 @@ int InputTest::initialize() void InputTest::print_status() const { PX4_INFO("Input: Test"); - PX4_INFO_RAW(" roll : % 3d deg\n", _roll_deg); - PX4_INFO_RAW(" pitch: % 3d deg\n", _pitch_deg); - PX4_INFO_RAW(" yaw : % 3d deg\n", _yaw_deg); + PX4_INFO_RAW(" roll : % .1f deg\n", (double)_roll_deg); + PX4_INFO_RAW(" pitch: % .1f deg\n", (double)_pitch_deg); + PX4_INFO_RAW(" yaw : % .1f deg\n", (double)_yaw_deg); + PX4_INFO_RAW(" rollrate : % .1f deg/s\n", (double)_rollrate_deg_s); + PX4_INFO_RAW(" pitchrate: % .1f deg/s\n", (double)_pitchrate_deg_s); + PX4_INFO_RAW(" yawrate : % .1f deg/s\n", (double)_yawrate_deg_s); } -void InputTest::set_test_input(int roll_deg, int pitch_deg, int yaw_deg) +void InputTest::set_test_input_angles(float roll_deg, float pitch_deg, float yaw_deg) { _roll_deg = roll_deg; _pitch_deg = pitch_deg; _yaw_deg = yaw_deg; - + _rollrate_deg_s = NAN; + _pitchrate_deg_s = NAN; + _yawrate_deg_s = NAN; + _has_been_set.store(true); +} +void InputTest::set_test_input_angle_rates(float rollrate_deg_s, float pitchrate_deg_s, float yawrate_deg_s) +{ + _roll_deg = NAN; + _pitch_deg = NAN; + _yaw_deg = NAN; + _rollrate_deg_s = rollrate_deg_s; + _pitchrate_deg_s = pitchrate_deg_s; + _yawrate_deg_s = yawrate_deg_s; _has_been_set.store(true); } diff --git a/src/modules/gimbal/input_test.h b/src/modules/gimbal/input_test.h index 4511838c5c..80f60746aa 100644 --- a/src/modules/gimbal/input_test.h +++ b/src/modules/gimbal/input_test.h @@ -50,12 +50,17 @@ public: int initialize() override; void print_status() const override; - void set_test_input(int roll_deg, int pitch_deg, int yaw_deg); + void set_test_input_angles( + float roll_deg, float pitch_deg, float yaw_deg); + void set_test_input_angle_rates(float rollrate_deg_s, float pitchrate_deg_s, float yawrate_deg_s); private: - int _roll_deg {0}; - int _pitch_deg {0}; - int _yaw_deg {0}; + float _roll_deg {NAN}; + float _pitch_deg {NAN}; + float _yaw_deg {NAN}; + float _rollrate_deg_s {NAN}; + float _pitchrate_deg_s {NAN}; + float _yawrate_deg_s {NAN}; px4::atomic _has_been_set {false}; }; diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index 61486f3f77..ea474bf33e 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -43,6 +43,8 @@ #include #include +using namespace time_literals; + namespace gimbal { @@ -68,7 +70,7 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position) { if (!_projection_reference.isInitialized()) { - _projection_reference.initReference(global_position.lat, global_position.lon); + _projection_reference.initReference(global_position.lat, global_position.lon, hrt_absolute_time()); } float x1, y1, x2, y2; @@ -81,6 +83,27 @@ float OutputBase::_calculate_pitch(double lon, double lat, float altitude, return atan2f(z, target_distance); } +bool OutputBase::check_and_handle_setpoint_timeout(ControlData &control_data, const hrt_abstime &now) +{ + bool ret = false; + const bool timeout = (control_data.timestamp_last_update + 2_s < now); + const bool type_angle = (control_data.type == ControlData::Type::Angle); + + if (timeout && type_angle) { + // Avoid gimbal keeps on spinning if the last setpoint was angular_velocity but it times out + for (int i = 0; i < 3; ++i) { + float &vel = control_data.type_data.angle.angular_velocity[i]; + + if (PX4_ISFINITE(vel) && (fabsf(vel) > FLT_EPSILON)) { + vel = 0.f; + ret = true; + } + } + } + + return ret; +} + void OutputBase::_set_angle_setpoints(const ControlData &control_data) { switch (control_data.type) { @@ -235,12 +258,19 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t) _angle_outputs[i] -= euler_vehicle(i); } - if (PX4_ISFINITE(_angle_outputs[i])) { - // bring angles into proper range [-pi, pi] + if (PX4_ISFINITE(_angle_outputs[i]) && _parameters.mnt_rc_in_mode == 0) { + // if we are in angle input mode, we bring angles into proper range [-pi, pi] _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); } } + // constrain angle outputs to [-range/2, range/2] + _angle_outputs[0] = math::constrain(_angle_outputs[0], math::radians(-_parameters.mnt_range_roll / 2), + math::radians(_parameters.mnt_range_roll / 2)); + _angle_outputs[1] = math::constrain(_angle_outputs[1], math::radians(-_parameters.mnt_range_pitch / 2), + math::radians(_parameters.mnt_range_pitch / 2)); + _angle_outputs[2] = math::constrain(_angle_outputs[2], math::radians(-_parameters.mnt_range_yaw / 2), + math::radians(_parameters.mnt_range_yaw / 2)); // constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed if (_landed) { diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index f0dd983240..0da012c35f 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -63,6 +63,14 @@ public: void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize); + /** + * Time out if setpoint that should be streamed stops + * @param control_data setpoint to check timestamp of and amend upon timeout + * @param now Current system timestamp + * @return true iff setpoint was amended because of timeout + */ + bool check_and_handle_setpoint_timeout(ControlData &control_data, const hrt_abstime &now); + protected: float _calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position); diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index cceb34525d..ae5222c674 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -48,6 +48,8 @@ OutputMavlinkV1::OutputMavlinkV1(const Parameters ¶meters) void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) { + hrt_abstime now = hrt_absolute_time(); + vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); vehicle_command.target_system = (uint8_t)_parameters.mnt_mav_sysid_v1; @@ -91,10 +93,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _handle_position_update(control_data); - hrt_abstime t = hrt_absolute_time(); - _calculate_angle_output(t); + _calculate_angle_output(now); - vehicle_command.timestamp = t; + vehicle_command.timestamp = now; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; // gimbal spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively @@ -108,7 +109,10 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _stream_device_attitude_status(); - _last_update = t; + // If the output is MAVLink v1, then we signal this by referring to compid 1. + gimbal_device_id = 1; + + _last_update = now; } void OutputMavlinkV1::_stream_device_attitude_status() @@ -144,14 +148,13 @@ OutputMavlinkV2::OutputMavlinkV2(const Parameters ¶meters) void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) { + hrt_abstime now = hrt_absolute_time(); + _check_for_gimbal_device_information(); - hrt_abstime t = hrt_absolute_time(); - - - if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) { + if (!_gimbal_device_found && now - _last_gimbal_device_checked > 1000000) { _request_gimbal_device_information(); - _last_gimbal_device_checked = t; + _last_gimbal_device_checked = now; } else { if (new_setpoints) { @@ -159,10 +162,10 @@ void OutputMavlinkV2::update(const ControlData &control_data, bool new_setpoints _set_angle_setpoints(control_data); _handle_position_update(control_data); - _last_update = t; + _last_update = now; } - gimbal_device_id = _gimbal_device_found ? _gimbal_device_compid : 0; + gimbal_device_id = _gimbal_device_found ? _gimbal_device_id : 0; _publish_gimbal_device_set_attitude(); } @@ -191,7 +194,7 @@ void OutputMavlinkV2::_check_for_gimbal_device_information() if (_gimbal_device_information_sub.update(&gimbal_device_information)) { _gimbal_device_found = true; - _gimbal_device_compid = gimbal_device_information.gimbal_device_compid; + _gimbal_device_id = gimbal_device_information.gimbal_device_id; } } @@ -210,7 +213,7 @@ void OutputMavlinkV2::print_status() const (double)_angle_velocity[2]); if (_gimbal_device_found) { - PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_compid); + PX4_INFO_RAW(" gimbal device compid found: %d\n", _gimbal_device_id); } else { PX4_INFO_RAW(" gimbal device compid not found\n"); @@ -222,7 +225,7 @@ void OutputMavlinkV2::_publish_gimbal_device_set_attitude() gimbal_device_set_attitude_s set_attitude{}; set_attitude.timestamp = hrt_absolute_time(); set_attitude.target_system = (uint8_t)_parameters.mav_sysid; - set_attitude.target_component = _gimbal_device_compid; + set_attitude.target_component = _gimbal_device_id; set_attitude.angular_velocity_x = _angle_velocity[0]; set_attitude.angular_velocity_y = _angle_velocity[1]; diff --git a/src/modules/gimbal/output_mavlink.h b/src/modules/gimbal/output_mavlink.h index 79ef0e46e7..0e9d9f217f 100644 --- a/src/modules/gimbal/output_mavlink.h +++ b/src/modules/gimbal/output_mavlink.h @@ -81,7 +81,7 @@ private: uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; - uint8_t _gimbal_device_compid{0}; + uint8_t _gimbal_device_id{0}; hrt_abstime _last_gimbal_device_checked{0}; bool _gimbal_device_found {false}; }; diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index 4b48349a4d..dba7b02b1a 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -50,14 +50,15 @@ OutputRC::OutputRC(const Parameters ¶meters) void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id) { + hrt_abstime now = hrt_absolute_time(); + if (new_setpoints) { _set_angle_setpoints(control_data); } _handle_position_update(control_data); - hrt_abstime t = hrt_absolute_time(); - _calculate_angle_output(t); + _calculate_angle_output(now); _stream_device_attitude_status(); @@ -81,7 +82,7 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 gimbal_controls.timestamp = hrt_absolute_time(); _gimbal_controls_pub.publish(gimbal_controls); - _last_update = t; + _last_update = now; } void OutputRC::print_status() const @@ -95,16 +96,29 @@ void OutputRC::_stream_device_attitude_status() attitude_status.timestamp = hrt_absolute_time(); attitude_status.target_system = 0; attitude_status.target_component = 0; - attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | - gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | - gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK | - gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + attitude_status.device_flags = 0; + + if (_absolute_angle[0]) { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK; + } + + if (_absolute_angle[1]) { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK; + } + + if (_absolute_angle[2]) { + attitude_status.device_flags |= gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + } matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); matrix::Quatf q(euler); q.copyTo(attitude_status.q); attitude_status.failure_flags = 0; + + // If the output is RC, then we signal this by referring to compid 1. + attitude_status.gimbal_device_id = 1; + _attitude_status_pub.publish(attitude_status); } diff --git a/src/modules/internal_combustion_engine_control/CMakeLists.txt b/src/modules/internal_combustion_engine_control/CMakeLists.txt new file mode 100644 index 0000000000..15df6ca368 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ +px4_add_module( + MODULE modules__internal_combustion_engine_control + MAIN internal_combustion_engine_control + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + InternalCombustionEngineControl.cpp + InternalCombustionEngineControl.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + SlewRate +) diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp new file mode 100644 index 0000000000..f4ab6a05aa --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -0,0 +1,397 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "InternalCombustionEngineControl.hpp" + +#include + +using namespace time_literals; + +namespace internal_combustion_engine_control +{ + +InternalCombustionEngineControl::InternalCombustionEngineControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + _internal_combustion_engine_control_pub.advertise(); + _internal_combustion_engine_status_pub.advertise(); +} + +InternalCombustionEngineControl::~InternalCombustionEngineControl() +{ + +} + +int InternalCombustionEngineControl::task_spawn(int argc, char *argv[]) +{ + InternalCombustionEngineControl *obj = new InternalCombustionEngineControl(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void InternalCombustionEngineControl::start() +{ + ScheduleOnInterval(20_ms); // 50 Hz +} + +void InternalCombustionEngineControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + _throttle_control_slew_rate.setSlewRate(_param_ice_thr_slew.get()); + } + + + manual_control_setpoint_s manual_control_setpoint; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + + actuator_motors_s actuator_motors; + _actuator_motors.copy(&actuator_motors); + + const float throttle_in = actuator_motors.control[0]; + + const hrt_abstime now = hrt_absolute_time(); + + UserOnOffRequest user_request = UserOnOffRequest::Off; + + switch (static_cast(_param_ice_on_source.get())) { + case ICESource::ArmingState: { + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux1: { + if (manual_control_setpoint.aux1 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux2: { + if (manual_control_setpoint.aux2 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + } + + switch (_state) { + case State::Stopped: { + controlEngineStop(); + + if (user_request == UserOnOffRequest::On && !maximumAttemptsReached()) { + + _state = State::Starting; + _state_start_time = now; + PX4_INFO("ICE: Starting"); + } + } + break; + + case State::Starting: { + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + + switch (_starting_sub_state) { + case SubState::Rest: { + if (isStartingPermitted(now)) { + _state_start_time = now; + _starting_sub_state = SubState::Run; + } + } + break; + + case SubState::Run: + default: { + controlEngineStartup(now); + + if (isEngineRunning(now)) { + _state = State::Running; + PX4_INFO("ICE: Starting finished"); + + } else { + + if (maximumAttemptsReached()) { + _state = State::Fault; + PX4_WARN("ICE: Fault"); + + } else if (!isStartingPermitted(now)) { + controlEngineStop(); + _starting_sub_state = SubState::Rest; + } + } + + break; + } + + } + } + + } + break; + + case State::Running: { + controlEngineRunning(throttle_in); + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else if (!isEngineRunning(now) && _param_ice_running_fault_detection.get()) { + // without RPM feedback we assume the engine is running after the + // starting procedure but only switch state if fault detection is enabled + _state = State::Starting; + _state_start_time = now; + _starting_retry_cycle = 0; + PX4_WARN("ICE: Running Fault detected"); + events::send(events::ID("internal_combustion_engine_control_fault"), events::Log::Critical, + "IC engine fault detected"); + } + } + + break; + + case State::Fault: { + + // do nothing + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + controlEngineFault(); + } + } + + + break; + } + + const float control_interval = math::constrain(static_cast((now - _last_time_run) * 1e-6f), 0.01f, 0.1f); + + _last_time_run = now; + + // slew rate limit throttle control if it's finite, otherwise just pass it through (0 throttle = NAN = disarmed) + if (PX4_ISFINITE(_throttle_control)) { + _throttle_control = _throttle_control_slew_rate.update(_throttle_control, control_interval); + + } else { + _throttle_control_slew_rate.setForcedValue(0.f); + } + + publishControl(now, user_request); +} + +void InternalCombustionEngineControl::publishControl(const hrt_abstime now, const UserOnOffRequest user_request) +{ + internal_combustion_engine_control_s ice_control{}; + ice_control.timestamp = now; + ice_control.choke_control = _choke_control; + ice_control.ignition_on = _ignition_on; + ice_control.starter_engine_control = _starter_engine_control; + ice_control.throttle_control = _throttle_control; + ice_control.user_request = static_cast(user_request); + _internal_combustion_engine_control_pub.publish(ice_control); + + internal_combustion_engine_status_s ice_status; + ice_status.state = static_cast(_state); + ice_status.timestamp = now; + _internal_combustion_engine_status_pub.publish(ice_status); +} + +bool InternalCombustionEngineControl::isEngineRunning(const hrt_abstime now) +{ + rpm_s rpm; + + if (_rpm_sub.copy(&rpm)) { + const hrt_abstime rpm_timestamp = rpm.timestamp; + + return (_param_ice_min_run_rpm.get() > FLT_EPSILON && (now < rpm_timestamp + 2_s) + && rpm.rpm_estimate > _param_ice_min_run_rpm.get()); + } + + return false; +} + +void InternalCombustionEngineControl::controlEngineRunning(float throttle_in) +{ + _ignition_on = true; + _choke_control = 0.f; + _starter_engine_control = 0.f; + _throttle_control = throttle_in; + +} + +void InternalCombustionEngineControl::controlEngineStop() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineFault() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineStartup(const hrt_abstime now) +{ + float ignition_delay = 0.f; + float choke_duration = 0.f; + const float starter_duration = _param_ice_strt_dur.get(); + + if (_starting_retry_cycle == 0) { + ignition_delay = math::max(_param_ice_ign_delay.get(), 0.f); + + if (_param_ice_choke_st_dur.get() > FLT_EPSILON) { + choke_duration = _param_ice_choke_st_dur.get(); + } + } + + _ignition_on = true; + _throttle_control = _param_ice_strt_thr.get(); + _choke_control = now < _state_start_time + (choke_duration + ignition_delay) * 1_s ? 1.f : 0.f; + _starter_engine_control = now > _state_start_time + (ignition_delay * 1_s) ? 1.f : 0.f; + const hrt_abstime cycle_timeout_duration = (ignition_delay + choke_duration + starter_duration) * 1_s; + + if (now > _state_start_time + cycle_timeout_duration) { + // start resting timer if engine is not running + _starting_rest_time = now; + _starting_retry_cycle++; + PX4_INFO("ICE: starting attempt %i finished", _starting_retry_cycle); + } +} + +bool InternalCombustionEngineControl::isStartingPermitted(const hrt_abstime now) +{ + return now > _starting_rest_time + DELAY_BEFORE_RESTARTING * 1_s; +} + +bool InternalCombustionEngineControl::maximumAttemptsReached() +{ + // First and only attempt + if (_param_ice_strt_attempts.get() == 0) { + return _starting_retry_cycle > 0; + } + + return _starting_retry_cycle >= _param_ice_strt_attempts.get(); +} + +int InternalCombustionEngineControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The module controls internal combustion engine (ICE) features including: +ignition (on/off),throttle and choke level, starter engine delay, and user request. +The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). +The architecture is as shown below.: +![Architecture](../../assets/diagrams/ice_control_diagram.png) + +### Enabling +This feature is not enabled by default needs to be configured in the +build target for your board together with the rpm capture driver: +CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y +CONFIG_DRIVERS_RPM_CAPTURE=y + +Additionally, to enable the module: +- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN) +to true and adjust the other module parameters ICE_ according to your needs. +- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true. + +### Implementation +The ICE is implemented with a (4) state machine: +![Architecture](../../assets/diagrams/ice_control_state_machine.png) +The state machine: +- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running +- allows for user inputs from +- AUX{N} +- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md) + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("internal_combustion_engine_control", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int internal_combustion_engine_control_main(int argc, char *argv[]) +{ + return InternalCombustionEngineControl::main(argc, argv); +} + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp new file mode 100644 index 0000000000..01884e4967 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace internal_combustion_engine_control +{ + +class InternalCombustionEngineControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + InternalCombustionEngineControl(); + ~InternalCombustionEngineControl() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void start(); + +private: + void Run() override; + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + uORB::Subscription _actuator_motors{ORB_ID(actuator_motors)}; + + uORB::Publication _internal_combustion_engine_control_pub{ORB_ID(internal_combustion_engine_control)}; + uORB::Publication _internal_combustion_engine_status_pub{ORB_ID(internal_combustion_engine_status)}; + + + // has to mirror internal_combustion_engine_status_s::State + enum class State { + Stopped, + Starting, + Running, + Fault + } _state{State::Stopped}; + + enum class SubState { + Run, + Rest + }; + + enum class UserOnOffRequest { + Off, + On + }; + + enum class ICESource { + ArmingState, + Aux1, + Aux2, + }; + + hrt_abstime _state_start_time{0}; + hrt_abstime _last_time_run{0}; + + bool _ignition_on{false}; + float _choke_control{1.f}; + float _starter_engine_control{0.f}; + float _throttle_control{0.f}; + + SlewRate _throttle_control_slew_rate; + + bool isEngineRunning(const hrt_abstime now); + void controlEngineRunning(float throttle_in); + void controlEngineStop(); + void controlEngineStartup(const hrt_abstime now); + void controlEngineFault(); + bool maximumAttemptsReached(); + void publishControl(const hrt_abstime now, const UserOnOffRequest user_request); + + // Starting state specifics + static constexpr float DELAY_BEFORE_RESTARTING{1.f}; + int _starting_retry_cycle{0}; + hrt_abstime _starting_rest_time{0}; + SubState _starting_sub_state{SubState::Run}; + + /** + * @brief Currently the ICE starting state is permitted after resting + * DELAY_BEFORE_RESTARTING s to reduce wear on the starter motor. + * @param now current time + * @return if true, otherwise false + */ + bool isStartingPermitted(const hrt_abstime now); + + DEFINE_PARAMETERS( + (ParamInt) _param_ice_on_source, + (ParamFloat) _param_ice_choke_st_dur, + (ParamFloat) _param_ice_strt_dur, + (ParamFloat) _param_ice_min_run_rpm, + (ParamInt) _param_ice_strt_attempts, + (ParamInt) _param_ice_running_fault_detection, + (ParamFloat) _param_ice_strt_thr, + (ParamInt) _param_ice_stop_choke, + (ParamFloat) _param_ice_thr_slew, + (ParamFloat) _param_ice_ign_delay + ) +}; + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/Kconfig b/src/modules/internal_combustion_engine_control/Kconfig new file mode 100644 index 0000000000..c0d0e9049f --- /dev/null +++ b/src/modules/internal_combustion_engine_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL + bool "internal_combustion_engine_control" + default n + ---help--- + Enable support for internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml new file mode 100644 index 0000000000..517fc89fee --- /dev/null +++ b/src/modules/internal_combustion_engine_control/module.yaml @@ -0,0 +1,120 @@ +module_name: Internal Combustion Engine Control + +parameters: + - group: ICE + definitions: + + ICE_EN: + description: + short: Enable internal combustion engine + type: boolean + default: true + + ICE_ON_SOURCE: + description: + short: Engine start/stop input source + type: enum + default: 0 + values: + 0: On arming - disarming + 1: Aux1 + 2: Aux2 + + ICE_CHOKE_ST_DUR: + description: + short: Duration of choking during startup + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_STRT_DUR: + description: + short: Duration of single starting attempt (excl. choking) + long: | + Maximum expected time for startup before declaring timeout. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_MIN_RUN_RPM: + description: + short: Minimum RPM for engine to be declared running + type: float + unit: rpm + min: 0 + max: 10000 + decimal: 0 + increment: 1 + default: 2000 + + ICE_STRT_ATTEMPT: + description: + short: Number attempts for starting the engine + long: | + Number of accepted attempts for starting the engine before declaring a fault. + type: int32 + min: 0 + max: 10 + default: 3 + + ICE_RUN_FAULT_D: + description: + short: Fault detection if it stops in running state + long: | + Enables restart if a fault is detected during the running state. Otherwise + commands continues in running state until given an user request off. + type: boolean + default: true + + ICE_STRT_THR: + description: + short: Throttle value for starting engine + long: | + During the choking and the starting phase, the throttle value is set to this value. + type: float + unit: norm + min: 0 + max: 1 + decimal: 0 + increment: 0.01 + default: 0.1 + + ICE_STOP_CHOKE: + description: + short: Apply choke when stopping engine + type: boolean + default: true + + ICE_THR_SLEW: + description: + short: Throttle slew rate + long: | + Maximum rate of change of throttle value per second. + type: float + unit: 1/s + min: 0 + max: 1 + decimal: 2 + increment: 0.01 + default: 0.5 + + ICE_IGN_DELAY: + description: + short: Cold-start delay after ignition before engaging starter + long: | + In case that the ignition takes a moment to be up and running, this parameter can be set to account for that. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 0 diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index ec8157034b..4ed4a353ab 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -71,9 +71,13 @@ bool FixedwingLandDetector::_get_landed_state() } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { - // Horizontal velocity complimentary filter. - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + - _vehicle_local_position.vy * _vehicle_local_position.vy); + float val = 0.0f; + + if (_vehicle_local_position.v_xy_valid) { + // Horizontal velocity complimentary filter. + val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + + _vehicle_local_position.vy * _vehicle_local_position.vy); + } if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; @@ -105,15 +109,26 @@ bool FixedwingLandDetector::_get_landed_state() const float acc_hor = matrix::Vector2f(_acceleration).norm(); _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; + // Check for angular velocity + const float rot_vel_hor = _angular_velocity.norm(); + val = _velocity_rot_filtered * 0.95f + rot_vel_hor * 0.05f; + + if (PX4_ISFINITE(val)) { + _velocity_rot_filtered = val; + } + // make groundspeed threshold tighter if airspeed is invalid - const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : - _param_lndfw_vel_xy_max.get(); + const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : + _param_lndfw_vel_xy_max.get(); + + const float max_rotation_threshold = math::radians(_param_lndfw_rot_max.get()) ; // Crude land detector for fixedwing. - landDetected = _airspeed_filtered < _param_lndfw_airspd.get() - && _velocity_xy_filtered < vel_xy_max_threshold - && _velocity_z_filtered < _param_lndfw_vel_z_max.get() - && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); + landDetected = _airspeed_filtered < _param_lndfw_airspd.get() + && _velocity_xy_filtered < vel_xy_max_threshold + && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _xy_accel_filtered < _param_lndfw_xyaccel_max.get() + && _velocity_rot_filtered < max_rotation_threshold; } else { // Control state topic has timed out and we need to assume we're landed. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 981fdc2fb5..3f3fa9ad35 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -75,6 +75,7 @@ private: float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; float _xy_accel_filtered{0.0f}; + float _velocity_rot_filtered{0.0f}; DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, @@ -82,6 +83,7 @@ private: (ParamFloat) _param_lndfw_airspd, (ParamFloat) _param_lndfw_vel_xy_max, (ParamFloat) _param_lndfw_vel_z_max, + (ParamFloat) _param_lndfw_rot_max, (ParamFloat) _param_lndfw_trig_time ); }; diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c index 02ab459032..6ab78fb867 100644 --- a/src/modules/land_detector/land_detector_params_fw.c +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -102,3 +102,14 @@ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f); * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f); + +/** + * Fixed-wing land detector: max rotational speed + * + * Maximum allowed norm of the angular velocity in the landed state. + * + * @unit deg/s + * @decimal 1 + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f); diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 121b365bb5..1d61a7e00b 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -76,9 +76,9 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); /** - * Multicopter max rotation + * Multicopter max rotational speed * - * Maximum allowed angular velocity around each axis allowed in the landed state. + * Maximum allowed norm of the angular velocity (roll, pitch) in the landed state. * * @unit deg/s * @decimal 1 diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 8517ff03b0..f28bb78c08 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -643,7 +643,8 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vxy_max = INFINITY; _pub_lpos.get().vz_max = INFINITY; _pub_lpos.get().hagl_min = INFINITY; - _pub_lpos.get().hagl_max = INFINITY; + _pub_lpos.get().hagl_max_z = INFINITY; + _pub_lpos.get().hagl_max_xy = INFINITY; _pub_lpos.get().timestamp = hrt_absolute_time();; _pub_lpos.update(); } diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index 2001baf2f6..530076f49b 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -88,7 +88,7 @@ LogWriterFile::~LogWriterFile() } #if defined(PX4_CRYPTO) -bool LogWriterFile::init_logfile_encryption(const char *filename) +bool LogWriterFile::init_logfile_encryption(const LogType type) { if (_algorithm == CRYPTO_NONE) { _min_blocksize = 1; @@ -151,39 +151,18 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) rsa_crypto.close(); - // Write the encrypted key to the disk - - // Allocate a buffer for filename - size_t fnlen = strlen(filename); - char *tmp_buf = (char *)malloc(fnlen + 1); - - if (!tmp_buf) { - PX4_ERR("out of memory"); - free(key); - return false; - } - - // Copy the original logfile name, and append 'k' to the filename - - memcpy(tmp_buf, filename, fnlen + 1); - tmp_buf[fnlen - 1] = 'k'; - tmp_buf[fnlen] = 0; - - int key_fd = ::open((const char *)tmp_buf, O_CREAT | O_WRONLY, PX4_O_MODE_666); - - // The file name is no longer needed, free it - free(tmp_buf); - tmp_buf = nullptr; + // Write the encrypted key to the beginning of the opened log file + int key_fd = _buffers[(int)type].fd(); if (key_fd < 0) { - PX4_ERR("Can't open key file, errno: %d", errno); + PX4_ERR("Log file not open for storing the key, errno: %d", errno); free(key); return false; } - // write the header to the key exchange file + // write header and key to the beginning of the log file struct ulog_key_header_s keyfile_header = { - .magic = {'U', 'L', 'o', 'g', 'K', 'e', 'y'}, + .magic = {'U', 'L', 'o', 'g', 'E', 'n', 'c'}, .hdr_ver = 1, .timestamp = hrt_absolute_time(), .exchange_algorithm = CRYPTO_RSA_OAEP, @@ -192,20 +171,14 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) .initdata_size = (uint16_t)nonce_size }; - size_t hdr_sz = ::write(key_fd, (uint8_t *)&keyfile_header, sizeof(keyfile_header)); - size_t written = 0; - - if (hdr_sz == sizeof(keyfile_header)) { - // Header write succeeded, write the key - written = ::write(key_fd, key, key_size + nonce_size); - } + size_t written = ::write(key_fd, (uint8_t *)&keyfile_header, sizeof(keyfile_header)); + written += ::write(key_fd, key, key_size + nonce_size); // Free temporary memory allocations free(key); - ::close(key_fd); // Check that writing to the disk succeeded - if (written != key_size + nonce_size) { + if (written != sizeof(keyfile_header) + key_size + nonce_size) { PX4_ERR("Writing the encryption key to disk fail"); return false; } @@ -241,18 +214,22 @@ bool LogWriterFile::start_log(LogType type, const char *filename) } } -#if PX4_CRYPTO - bool enc_init = init_logfile_encryption(filename); + if (_buffers[(int)type].start_log(filename)) { - if (!enc_init) { - PX4_ERR("Failed to start encrypted logging"); - _crypto.close(); - return false; - } +#if PX4_CRYPTO + bool enc_init = init_logfile_encryption(type); + + if (!enc_init) { + PX4_ERR("Failed to start encrypted logging"); + _crypto.close(); + _buffers[(int)type]._should_run = false; + _buffers[(int)type].close_file(); + _buffers[(int)type].reset(); + return false; + } #endif - if (_buffers[(int)type].start_log(filename)) { PX4_INFO("Opened %s log file: %s", log_type_str(type), filename); notify(); return true; diff --git a/src/modules/logger/log_writer_file.h b/src/modules/logger/log_writer_file.h index 7654878938..4739b5b799 100644 --- a/src/modules/logger/log_writer_file.h +++ b/src/modules/logger/log_writer_file.h @@ -224,7 +224,7 @@ private: pthread_cond_t _cv; pthread_t _thread = 0; #if defined(PX4_CRYPTO) - bool init_logfile_encryption(const char *filename); + bool init_logfile_encryption(const LogType type); PX4Crypto _crypto; int _min_blocksize; px4_crypto_algorithm_t _algorithm; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8bd55e62fe..b98d185aff 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); - add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); @@ -80,6 +79,7 @@ void LoggedTopics::add_default_topics() add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); + add_optional_topic("internal_combustion_engine_control", 10); add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("iridiumsbd_status", 1000); add_optional_topic("irlock_report", 1000); @@ -103,11 +103,13 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); - add_optional_topic("rover_ackermann_guidance_status", 100); - add_optional_topic("rover_ackermann_status", 100); - add_optional_topic("rover_differential_guidance_status", 100); - add_optional_topic("rover_differential_setpoint", 100); - add_optional_topic("rover_differential_status", 100); + add_optional_topic("rover_attitude_setpoint", 100); + add_optional_topic("rover_attitude_status", 100); + add_optional_topic("rover_velocity_status", 100); + add_optional_topic("rover_rate_setpoint", 100); + add_optional_topic("rover_rate_status", 100); + add_optional_topic("rover_steering_setpoint", 100); + add_optional_topic("rover_throttle_setpoint", 100); add_optional_topic("rover_mecanum_guidance_status", 100); add_optional_topic("rover_mecanum_setpoint", 100); add_optional_topic("rover_mecanum_status", 100); @@ -250,6 +252,10 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + +#ifdef CONFIG_BOARD_UAVCAN_INTERFACES + add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES); +#endif } void LoggedTopics::add_high_rate_topics() @@ -290,6 +296,7 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); + add_topic("airspeed_validated"); add_topic("vehicle_optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); @@ -322,10 +329,10 @@ void LoggedTopics::add_sensor_comparison_topics() void LoggedTopics::add_vision_and_avoidance_topics() { add_topic("collision_constraints"); + add_topic_multi("distance_sensor"); add_topic("obstacle_distance_fused"); + add_topic("obstacle_distance"); add_topic("vehicle_mocap_odometry", 30); - add_topic("vehicle_trajectory_waypoint", 200); - add_topic("vehicle_trajectory_waypoint_desired", 200); add_topic("vehicle_visual_odometry", 30); } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 8d897a103a..99ec75898c 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -1307,7 +1307,7 @@ int Logger::get_log_file_name(LogType type, char *file_name, size_t file_name_si #if defined(PX4_CRYPTO) if (_param_sdlog_crypto_algorithm.get() != 0) { - crypto_suffix = "c"; + crypto_suffix = "e"; } #endif @@ -1615,6 +1615,11 @@ void Logger::initialize_load_output(PrintLoadReason reason) { // If already in progress, don't try to start again if (_next_load_print != 0) { + // To never miss watchdog triggers due to load measuring in progress, overwrite the measurement reason + if (reason == PrintLoadReason::Watchdog) { + _print_load_reason = reason; + } + return; } @@ -1635,7 +1640,13 @@ void Logger::write_load_output() _writer.set_need_reliable_transfer(true, _print_load_reason != PrintLoadReason::Watchdog); if (_print_load_reason == PrintLoadReason::Watchdog) { - PX4_ERR("Writing watchdog data"); // this is just that we see it easily in the log + // This is just that we see it easily in the log + PX4_ERR("Writing watchdog data..."); +#ifdef __PX4_NUTTX + bool cycle_trigger = _timer_callback_data.watchdog_data.triggered_by_cycle_delay; + bool ready_trigger = _timer_callback_data.watchdog_data.triggered_by_ready_delay; + PX4_ERR("Watchdog triggers - cycle trigger: %d, ready trigger: %d", cycle_trigger, ready_trigger); +#endif write_perf_data(PrintLoadReason::Watchdog); } diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 727aad54d4..e1bbd5145f 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators) * 5 : Debugging topics (debug_*.msg topics, for custom code) * 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data) - * 7 : Topics for computer vision and collision avoidance + * 7 : Topics for computer vision and collision prevention * 8 : Raw FIFO high-rate IMU (Gyro) * 9 : Raw FIFO high-rate IMU (Accel) * 10: Logging of mavlink tunnel message (useful for payload communication debugging) @@ -184,7 +184,6 @@ PARAM_DEFINE_INT32(SDLOG_UUID, 1); * * @value 0 Disabled * @value 2 XChaCha20 - * @value 3 AES * * @group SD Logging */ diff --git a/src/modules/logger/watchdog.cpp b/src/modules/logger/watchdog.cpp index aa04bd0f6e..6816fa042c 100644 --- a/src/modules/logger/watchdog.cpp +++ b/src/modules/logger/watchdog.cpp @@ -129,10 +129,10 @@ bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_satura watchdog_data.sem_counter_saturated_start = now; } - if (watchdog_data.manual_watchdog_trigger - || now > watchdog_data.sem_counter_saturated_start + 3_s - || now > watchdog_data.ready_to_run_timestamp + 1_s) { + bool cycle_trigger = now > watchdog_data.sem_counter_saturated_start + 5_s; + bool ready_trigger = now > watchdog_data.ready_to_run_timestamp + 1_s; + if (watchdog_data.manual_watchdog_trigger || cycle_trigger || ready_trigger) { sched_param param{}; // Get the current priorities @@ -153,6 +153,8 @@ bool watchdog_update(watchdog_data_t &watchdog_data, bool semaphore_value_satura sched_setparam(log_writer_task.tcb->pid, ¶m); + watchdog_data.triggered_by_cycle_delay = cycle_trigger; + watchdog_data.triggered_by_ready_delay = ready_trigger; watchdog_data.trigger_time = now; return true; } diff --git a/src/modules/logger/watchdog.h b/src/modules/logger/watchdog.h index 0ccfe0e4a9..aebd23ff14 100644 --- a/src/modules/logger/watchdog.h +++ b/src/modules/logger/watchdog.h @@ -56,6 +56,8 @@ struct watchdog_data_t { int logger_main_priority = 0; hrt_abstime trigger_time = 0; ///< timestamp when it was triggered bool manual_watchdog_trigger = false; + bool triggered_by_cycle_delay = false; + bool triggered_by_ready_delay = false; #endif /* __PX4_NUTTX */ }; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 53383fbb4a..a927035f93 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -271,6 +271,19 @@ void ManualControl::processSwitches(hrt_abstime &now) } } +#if defined(PAYLOAD_POWER_EN) + + if (switches.payload_power_switch != _previous_switches.payload_power_switch) { + if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_ON) { + PAYLOAD_POWER_EN(true); + + } else if (switches.payload_power_switch == manual_control_switches_s::SWITCH_POS_OFF) { + PAYLOAD_POWER_EN(false); + } + } + +#endif // PAYLOAD_POWER_EN + } else if (!_armed) { // Directly initialize mode using RC switch but only before arming evaluateModeSlot(switches.mode_slot); @@ -453,7 +466,7 @@ void ManualControl::send_camera_mode_command(CameraMode camera_mode) command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE; command.param2 = static_cast(camera_mode); command.target_system = _system_id; - command.target_component = 100; // any camera + command.target_component = 100; // MAV_COMP_ID_CAMERA uORB::Publication command_pub{ORB_ID(vehicle_command)}; command.timestamp = hrt_absolute_time(); @@ -467,7 +480,7 @@ void ManualControl::send_photo_command() command.param3 = 1; // one picture command.param4 = _image_sequence++; command.target_system = _system_id; - command.target_component = 100; // any camera + command.target_component = 100; // MAV_COMP_ID_CAMERA uORB::Publication command_pub{ORB_ID(vehicle_command)}; command.timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index bb52e30d2b..619947d8bc 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit bb52e30d2b924d5a250f2400144d33012271a19d +Subproject commit 619947d8bc29e80eecff18b0f4fecc42d9e171dd diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 058e9d95f1..f811eb82f0 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -399,17 +399,11 @@ MavlinkFTP::_workList(PayloadHeader *payload) payload->data[offset++] = kDirentSkip; *((char *)&payload->data[offset]) = '\0'; offset++; - payload->size = offset; - closedir(dp); + errorCode = kErrFailErrno; - return errorCode; - } - - // FIXME: does this ever happen? I would assume readdir always sets errno. - // no more entries? - if (payload->offset != 0 && offset == 0) { + } else if (offset == 0) { // User is requesting subsequent dir entries but there were none. This means the user asked - // to seek past EOF. + // to seek past EOF. This can happen with `payload->offset == 0` if the directory is empty. errorCode = kErrEOF; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e0a4a9c2ba..724391cf86 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1074,6 +1074,21 @@ Mavlink::send_autopilot_capabilities() msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY; + + + { + param_t param_handle = param_find_no_notification("MNT_MODE_IN"); + int32_t mnt_mode_in = 0; + + if (mnt_mode_in != PARAM_INVALID) { + param_get(param_handle, &mnt_mode_in); + + if (mnt_mode_in == 4) { + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER; + } + } + } + msg.flight_sw_version = px4_firmware_version(); msg.middleware_sw_version = px4_firmware_version(); msg.os_sw_version = px4_os_version(); @@ -1405,7 +1420,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("DISTANCE_SENSOR", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); @@ -1481,7 +1495,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); @@ -1512,7 +1525,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1560,7 +1572,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); @@ -1580,7 +1591,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RC_CHANNELS", 5.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1646,7 +1656,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 10.0f); configure_stream_local("ESC_INFO", 10.0f); @@ -1733,6 +1742,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("CAMERA_TRIGGER", unlimited_rate); configure_stream_local("LOCAL_POSITION_NED", 30.0f); configure_stream_local("ATTITUDE", 20.0f); + configure_stream_local("ATTITUDE_QUATERNION", 20.0f); configure_stream_local("ALTITUDE", 10.0f); configure_stream_local("DISTANCE_SENSOR", 10.0f); configure_stream_local("MOUNT_ORIENTATION", 10.0f); @@ -1749,7 +1759,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); - configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); @@ -1767,12 +1776,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("PING", 0.1f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); + configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 20.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 60f021a5b1..7d5e23bb6e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -65,7 +65,6 @@ #include "streams/BATTERY_STATUS.hpp" #include "streams/CAMERA_IMAGE_CAPTURED.hpp" #include "streams/CAMERA_TRIGGER.hpp" -#include "streams/COLLISION.hpp" #include "streams/COMMAND_LONG.hpp" #include "streams/COMPONENT_INFORMATION.hpp" #include "streams/COMPONENT_METADATA.hpp" @@ -117,7 +116,6 @@ #include "streams/SYSTEM_TIME.hpp" #include "streams/TIME_ESTIMATE_TO_TARGET.hpp" #include "streams/TIMESYNC.hpp" -#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" #include "streams/VFR_HUD.hpp" #include "streams/VIBRATION.hpp" #include "streams/WIND_COV.hpp" @@ -383,9 +381,6 @@ static const StreamListItem streams_list[] = { #if defined(MANUAL_CONTROL_HPP) create_stream_list_item(), #endif // MANUAL_CONTROL_HPP -#if defined(TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP) - create_stream_list_item(), -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP #if defined(OPTICAL_FLOW_RAD_HPP) create_stream_list_item(), #endif // OPTICAL_FLOW_RAD_HPP diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 1c5437270a..36d4453344 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -387,7 +387,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t } } else { - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + send_mission_ack(sysid, compid, MAV_MISSION_ERROR); if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { mavlink_log_critical(_mavlink.get_mavlink_log_pub(), @@ -730,8 +730,9 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) switch_to_idle_state(); } - if (_state == MAVLINK_WPM_STATE_IDLE || (_state == MAVLINK_WPM_STATE_SENDLIST - && (uint8_t)_mission_type == wprl.mission_type)) { + if ((_state == MAVLINK_WPM_STATE_IDLE) || ((_state == MAVLINK_WPM_STATE_SENDLIST) + && ((uint8_t)_mission_type == wprl.mission_type) && (msg->sysid == _transfer_partner_sysid) + && (msg->compid == _transfer_partner_compid))) { _time_last_recv = hrt_absolute_time(); _state = MAVLINK_WPM_STATE_SENDLIST; @@ -900,12 +901,14 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); - if (_transfer_in_progress) { - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + if (_transfer_in_progress) { // Transfer over different mavlink instance. ignore + send_mission_ack(msg->sysid, msg->compid, MAV_MISSION_ERROR); return; } _transfer_in_progress = true; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; _mission_type = (MAV_MISSION_TYPE)wpc.mission_type; _transfer_current_crc32 = 0; @@ -983,8 +986,6 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _state = MAVLINK_WPM_STATE_GETLIST; _transfer_seq = 0; - _transfer_partner_sysid = msg->sysid; - _transfer_partner_compid = msg->compid; _transfer_count = wpc.count; _transfer_current_seq = -1; _transfer_land_start_marker = -1; @@ -993,6 +994,13 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) } else if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); + if (msg->sysid != _transfer_partner_sysid || msg->compid != _transfer_partner_compid) { + PX4_DEBUG("WPM: Request from another partner while receiveing. Ignore"); + + send_mission_ack(msg->sysid, msg->compid, MAV_MISSION_ERROR); + return; + } + if (_transfer_seq == 0) { /* looks like our MISSION_REQUEST was lost, try again */ PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); @@ -1014,7 +1022,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _mavlink.send_statustext_critical("WPM: IGN MISSION_COUNT: Busy\t"); events::send(events::ID("mavlink_mission_ignore_mis_count"), events::Log::Error, "Mission upload busy, ignoring MISSION_COUNT"); - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + send_mission_ack(msg->sysid, msg->compid, MAV_MISSION_ERROR); return; } @@ -1062,241 +1070,249 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mavlink_msg_mission_item_decode(msg, &wp); if (CHECK_SYSID_COMPID_MISSION(wp)) { - - if (wp.mission_type != _mission_type) { - PX4_WARN("WPM: Unexpected mission type (%d %d)", (int)wp.mission_type, (int)_mission_type); - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - return; - } - - if (_state == MAVLINK_WPM_STATE_GETLIST) { - _time_last_recv = hrt_absolute_time(); - - if (wp.seq != _transfer_seq) { - PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); - - /* Item sequence not expected, ignore item */ + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (wp.mission_type != _mission_type) { + PX4_WARN("WPM: Unexpected mission type (%d %d)", (int)wp.mission_type, (int)_mission_type); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); return; } - } else if (_state == MAVLINK_WPM_STATE_IDLE) { - if (_transfer_seq == wp.seq + 1) { - // Assume this is a duplicate, where we already successfully got all mission items, - // but the GCS did not receive the last ack and sent the same item again - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32); + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); + + /* Item sequence not expected, ignore item */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_transfer_seq == wp.seq + 1) { + // Assume this is a duplicate, where we already successfully got all mission items, + // but the GCS did not receive the last ack and sent the same item again + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32); + + } else { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer"); + + _mavlink.send_statustext_critical("IGN MISSION_ITEM: No transfer\t"); + events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error, + "Ignoring mission item, no transfer in progress"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + return; } else { - PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer"); + PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state); - _mavlink.send_statustext_critical("IGN MISSION_ITEM: No transfer\t"); - events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error, - "Ignoring mission item, no transfer in progress"); + _mavlink.send_statustext_critical("IGN MISSION_ITEM: Busy\t"); + events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error, + "Ignoring mission item, busy"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + return; } - return; + struct mission_item_s mission_item = {}; - } else { - PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state); + int ret = parse_mavlink_mission_item(&wp, &mission_item); - _mavlink.send_statustext_critical("IGN MISSION_ITEM: Busy\t"); - events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error, - "Ignoring mission item, busy"); - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - return; - } + if (ret != PX4_OK) { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); - struct mission_item_s mission_item = {}; + _mavlink.send_statustext_critical("IGN MISSION_ITEM: Invalid item\t"); + events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error, + "Ignoring mission item, invalid item"); - int ret = parse_mavlink_mission_item(&wp, &mission_item); - - if (ret != PX4_OK) { - PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); - - _mavlink.send_statustext_critical("IGN MISSION_ITEM: Invalid item\t"); - events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error, - "Ignoring mission item, invalid item"); - - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); - switch_to_idle_state(); - _transfer_in_progress = false; - return; - } - - _transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32); - - bool write_failed = false; - bool check_failed = false; - - switch (_mission_type) { - - case MAV_MISSION_TYPE_MISSION: { - // check that we don't get a wrong item (hardening against wrong client implementations, the list here - // does not need to be complete) - if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) { - check_failed = true; - - } else { - - write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, - reinterpret_cast(&mission_item), - sizeof(struct mission_item_s)); - - // Check for land start marker - if ((mission_item.nav_cmd == MAV_CMD_DO_LAND_START) && (_transfer_land_start_marker == -1)) { - _transfer_land_start_marker = wp.seq; - } - - // Check for land index - if (((mission_item.nav_cmd == MAV_CMD_NAV_VTOL_LAND) || (mission_item.nav_cmd == MAV_CMD_NAV_LAND)) - && (_transfer_land_marker == -1)) { - _transfer_land_marker = wp.seq; - - if (_transfer_land_start_marker == -1) { - _transfer_land_start_marker = _transfer_land_marker; - } - } - - if (!write_failed) { - /* waypoint marked as current */ - if (wp.current) { - _transfer_current_seq = wp.seq; - } - } - } - } - break; - - case MAV_MISSION_TYPE_FENCE: { // Write a geofence point - mission_fence_point_s mission_fence_point; - mission_fence_point.nav_cmd = mission_item.nav_cmd; - mission_fence_point.lat = mission_item.lat; - mission_fence_point.lon = mission_item.lon; - mission_fence_point.alt = mission_item.altitude; - - if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || - mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { - mission_fence_point.vertex_count = mission_item.vertex_count; - - if (mission_item.vertex_count < 3) { // feasibility check - PX4_ERR("Fence: too few vertices"); - check_failed = true; - } - - } else { - mission_fence_point.circle_radius = mission_item.circle_radius; - } - - mission_fence_point.frame = mission_item.frame; - - if (!check_failed) { - write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, - reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); - } - - } - break; - - case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point - write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, - reinterpret_cast(&mission_item), sizeof(mission_item_s), 2_s); - } - break; - - default: - _mavlink.send_statustext_critical("Received unknown mission type, abort.\t"); - events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error, - "Received unknown mission type, abort"); - break; - } - - if (write_failed || check_failed) { - PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); - - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - - if (write_failed) { - _mavlink.send_statustext_critical("Unable to write on micro SD\t"); - events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error, - "Mission: unable to write to storage"); + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + switch_to_idle_state(); + _transfer_in_progress = false; + return; } - switch_to_idle_state(); - _transfer_in_progress = false; - return; - } + _transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32); - /* waypoint marked as current */ - if (wp.current) { - _transfer_current_seq = wp.seq; - } - - PX4_DEBUG("WPM: MISSION_ITEM seq %u received", wp.seq); - - _transfer_seq = wp.seq + 1; - - if (_transfer_seq == _transfer_count) { - /* got all new mission items successfully */ - PX4_DEBUG("WPM: MISSION_ITEM got all %u items, current_seq=%ld, changing state to MAVLINK_WPM_STATE_IDLE", - _transfer_count, _transfer_current_seq); - - ret = 0; + bool write_failed = false; + bool check_failed = false; switch (_mission_type) { - case MAV_MISSION_TYPE_MISSION: - _land_start_marker = _transfer_land_start_marker; - _land_marker = _transfer_land_marker; - // Only need to update if the mission actually changed - if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) { - update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); + case MAV_MISSION_TYPE_MISSION: { + // check that we don't get a wrong item (hardening against wrong client implementations, the list here + // does not need to be complete) + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) { + check_failed = true; + + } else { + + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, + reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); + + // Check for land start marker + if ((mission_item.nav_cmd == MAV_CMD_DO_LAND_START) && (_transfer_land_start_marker == -1)) { + _transfer_land_start_marker = wp.seq; + } + + // Check for land index + if (((mission_item.nav_cmd == MAV_CMD_NAV_VTOL_LAND) || (mission_item.nav_cmd == MAV_CMD_NAV_LAND)) + && (_transfer_land_marker == -1)) { + _transfer_land_marker = wp.seq; + + if (_transfer_land_start_marker == -1) { + _transfer_land_start_marker = _transfer_land_marker; + } + } + + if (!write_failed) { + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + } + } } - break; - case MAV_MISSION_TYPE_FENCE: + case MAV_MISSION_TYPE_FENCE: { // Write a geofence point + mission_fence_point_s mission_fence_point; + mission_fence_point.nav_cmd = mission_item.nav_cmd; + mission_fence_point.lat = mission_item.lat; + mission_fence_point.lon = mission_item.lon; + mission_fence_point.alt = mission_item.altitude; + + if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION || + mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) { + mission_fence_point.vertex_count = mission_item.vertex_count; + + if (mission_item.vertex_count < 3) { // feasibility check + PX4_ERR("Fence: too few vertices"); + check_failed = true; + } + + } else { + mission_fence_point.circle_radius = mission_item.circle_radius; + } + + mission_fence_point.frame = mission_item.frame; + + if (!check_failed) { + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); + } - // Only need to update if the mission actually changed - if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) { - ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); } - break; - case MAV_MISSION_TYPE_RALLY: - - // Only need to update if the mission actually changed - if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) { - ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, + reinterpret_cast(&mission_item), sizeof(mission_item_s), 2_s); } - break; default: - PX4_ERR("mission type %u not handled", _mission_type); + _mavlink.send_statustext_critical("Received unknown mission type, abort.\t"); + events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error, + "Received unknown mission type, abort"); break; } - // Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order - switch_to_idle_state(); + if (write_failed || check_failed) { + PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); - - if (ret == PX4_OK) { - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32); - - } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + + if (write_failed) { + _mavlink.send_statustext_critical("Unable to write on micro SD\t"); + events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error, + "Mission: unable to write to storage"); + } + + switch_to_idle_state(); + _transfer_in_progress = false; + return; } - _transfer_in_progress = false; + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + PX4_DEBUG("WPM: MISSION_ITEM seq %u received", wp.seq); + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + PX4_DEBUG("WPM: MISSION_ITEM got all %u items, current_seq=%ld, changing state to MAVLINK_WPM_STATE_IDLE", + _transfer_count, _transfer_current_seq); + + ret = 0; + + switch (_mission_type) { + case MAV_MISSION_TYPE_MISSION: + _land_start_marker = _transfer_land_start_marker; + _land_marker = _transfer_land_marker; + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_MISSION]) { + update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32); + } + + break; + + case MAV_MISSION_TYPE_FENCE: + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_FENCE]) { + ret = update_geofence_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + } + + break; + + case MAV_MISSION_TYPE_RALLY: + + // Only need to update if the mission actually changed + if (_transfer_current_crc32 != _crc32[MAV_MISSION_TYPE_RALLY]) { + ret = update_safepoint_count(_transfer_dataman_id, _transfer_count, _transfer_current_crc32); + } + + break; + + default: + PX4_ERR("mission type %u not handled", _mission_type); + break; + } + + // Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order + switch_to_idle_state(); + + + if (ret == PX4_OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED, _transfer_current_crc32); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + _transfer_in_progress = false; + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } } else { - /* request next item */ - send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + _mavlink.send_statustext_critical("REJ. MIS ITEM: partner id mismatch\t"); + events::send(events::ID("mavlink_mission_item_partner_id_mismatch"), events::Log::Error, + "Rejecting mission item, component or system ID mismatch"); + + PX4_DEBUG("WPM: MISSION_ITEM ERR: ID mismatch"); } } } @@ -1354,10 +1370,10 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) if (ret == PX4_OK) { PX4_DEBUG("WPM: CLEAR_ALL OK (mission_type=%i)", _mission_type); - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + send_mission_ack(msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); } else { - send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + send_mission_ack(msg->sysid, msg->compid, MAV_MISSION_ERROR); } } else { @@ -1591,6 +1607,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_OBLIQUE_SURVEY: case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case MAV_CMD_SET_CAMERA_MODE: + case MAV_CMD_SET_CAMERA_SOURCE: case MAV_CMD_DO_VTOL_TRANSITION: case MAV_CMD_NAV_DELAY: case MAV_CMD_NAV_RETURN_TO_LAUNCH: @@ -1626,7 +1643,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * return MAV_MISSION_ACCEPTED; } - int MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) @@ -1688,6 +1704,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: + case NAV_CMD_SET_CAMERA_SOURCE: case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_VTOL_TRANSITION: @@ -1860,8 +1877,6 @@ void MavlinkMissionManager::check_active_mission() || (_my_mission_dataman_id != (dm_item_t)_mission_sub.get().mission_dataman_id)) { PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); init_offboard_mission(_mission_sub.get()); - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], - MAV_MISSION_TYPE_MISSION, _crc32[MAV_MISSION_TYPE_MISSION]); } } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9c53b16f36..0b06a24b76 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -224,10 +224,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_adsb_vehicle(msg); break; - case MAVLINK_MSG_ID_COLLISION: - handle_message_collision(msg); - break; - case MAVLINK_MSG_ID_GPS_RTCM_DATA: handle_message_gps_rtcm_data(msg); break; @@ -260,14 +256,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_tunnel(msg); break; - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER: - handle_message_trajectory_representation_bezier(msg); - break; - - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS: - handle_message_trajectory_representation_waypoints(msg); - break; - case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS: handle_message_onboard_computer_status(msg); break; @@ -566,10 +554,10 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility if (!target_ok) { - // Reject alien commands only if there is no forwarding or we've never seen target component before if (!_mavlink.get_forwarding_on() || !_mavlink.component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) { - acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); + PX4_INFO("Ignore command %d from %d/%d to %d/%d", + cmd_mavlink.command, msg->sysid, msg->compid, cmd_mavlink.target_system, cmd_mavlink.target_component); } return; @@ -593,12 +581,13 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const result = handle_request_message_command(MAVLINK_MSG_ID_STORAGE_INFORMATION); } else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) { - if (set_message_interval((int)roundf(cmd_mavlink.param1), cmd_mavlink.param2, cmd_mavlink.param3)) { + if (set_message_interval( + (int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3, cmd_mavlink.param4, vehicle_command.param7)) { result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; } } else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) { - get_message_interval((int)roundf(cmd_mavlink.param1)); + get_message_interval((int)(cmd_mavlink.param1 + 0.5f)); } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { @@ -802,6 +791,16 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); + // We should not clog the command_ack queue with acks that are not for us. + // Therefore, we drop them early and move on. + bool target_ok = evaluate_target_ok(0, ack.target_system, ack.target_component); + + if (!target_ok) { + PX4_DEBUG("Drop ack %d for %d from %d/%d to %d/%d\n", + ack.result, ack.command, msg->sysid, msg->compid, ack.target_system, ack.target_component); + return; + } + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink.get_channel()); vehicle_command_ack_s command_ack{}; @@ -1981,64 +1980,6 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg) } -void -MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_bezier_t trajectory; - mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory); - - vehicle_trajectory_bezier_s trajectory_bezier{}; - - trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); - - for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) { - trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i]; - trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i]; - trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i]; - - trajectory_bezier.control_points[i].delta = trajectory.delta[i]; - trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i]; - } - - trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS); - _trajectory_bezier_pub.publish(trajectory_bezier); -} - -void -MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_waypoints_t trajectory; - mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); - - vehicle_trajectory_waypoint_s trajectory_waypoint{}; - - const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS); - - for (int i = 0; i < number_valid_points; ++i) { - trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i]; - trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i]; - trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i]; - - trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i]; - trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i]; - trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i]; - - trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i]; - trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i]; - trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i]; - - trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; - trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; - - trajectory_waypoint.waypoints[i].point_valid = true; - - trajectory_waypoint.waypoints[i].type = UINT8_MAX; - } - - trajectory_waypoint.timestamp = hrt_absolute_time(); - _trajectory_waypoint_pub.publish(trajectory_waypoint); -} - void MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) { @@ -2138,25 +2079,36 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } manual_control_setpoint_s manual_control_setpoint{}; - manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; - manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; + + if (math::isInRange((int)mavlink_manual_control.x, -1000, 1000)) { manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; } + + if (math::isInRange((int)mavlink_manual_control.y, -1000, 1000)) { manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; } + // For backwards compatibility at the moment interpret throttle in range [0,1000] - manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; - manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; + if (math::isInRange((int)mavlink_manual_control.z, 0, 1000)) { manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; } + + if (math::isInRange((int)mavlink_manual_control.r, -1000, 1000)) { manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; } + // Pass along the button states manual_control_setpoint.buttons = mavlink_manual_control.buttons; - if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 2) + && math::isInRange((int)mavlink_manual_control.aux1, -1000, 1000)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 3) + && math::isInRange((int)mavlink_manual_control.aux2, -1000, 1000)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 4) + && math::isInRange((int)mavlink_manual_control.aux3, -1000, 1000)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 5) + && math::isInRange((int)mavlink_manual_control.aux4, -1000, 1000)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 6) + && math::isInRange((int)mavlink_manual_control.aux5, -1000, 1000)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } - if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } + if (mavlink_manual_control.enabled_extensions & (1u << 7) + && math::isInRange((int)mavlink_manual_control.aux6, -1000, 1000)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); @@ -2241,11 +2193,6 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) _heartbeat_component_osd = now; break; - case MAV_COMP_ID_OBSTACLE_AVOIDANCE: - _heartbeat_component_obstacle_avoidance = now; - _mavlink.telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); - break; - case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: _heartbeat_component_visual_inertial_odometry = now; break; @@ -2273,14 +2220,25 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) } int -MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) +MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, float param4, float param7) { if (msgId == MAVLINK_MSG_ID_HEARTBEAT) { return PX4_ERROR; } - if (data_rate > 0) { - _mavlink.set_data_rate(data_rate); + if (PX4_ISFINITE(param3) && (int)(param3 + 0.5f) != 0) { + PX4_ERR("SET_MESSAGE_INTERVAL requested param3 not supported."); + return PX4_ERROR; + } + + if (PX4_ISFINITE(param4) && (int)(param4 + 0.5f) != 0) { + PX4_ERR("SET_MESSAGE_INTERVAL requested param4 not supported."); + return PX4_ERROR; + } + + if (PX4_ISFINITE(param7) && (int)(param7 + 0.5f) != 0) { + PX4_ERR("SET_MESSAGE_INTERVAL response target not supported."); + return PX4_ERROR; } // configure_stream wants a rate (msgs/second), so convert here. @@ -2606,26 +2564,6 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) _transponder_report_pub.publish(t); } -void -MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) -{ - mavlink_collision_t collision; - mavlink_msg_collision_decode(msg, &collision); - - collision_report_s collision_report{}; - - collision_report.timestamp = hrt_absolute_time(); - collision_report.src = collision.src; - collision_report.id = collision.id; - collision_report.action = collision.action; - collision_report.threat_level = collision.threat_level; - collision_report.time_to_minimum_delta = collision.time_to_minimum_delta; - collision_report.altitude_minimum_delta = collision.altitude_minimum_delta; - collision_report.horizontal_minimum_delta = collision.horizontal_minimum_delta; - - _collision_report_pub.publish(collision_report); -} - void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) { @@ -2660,7 +2598,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - airspeed.air_temperature_celsius = 15.f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } @@ -2695,7 +2632,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) const double lon = hil_state.lon * 1e-7; if (!_global_local_proj_ref.isInitialized() || !PX4_ISFINITE(_global_local_alt0)) { - _global_local_proj_ref.initReference(lat, lon); + _global_local_proj_ref.initReference(lat, lon, hrt_absolute_time()); _global_local_alt0 = hil_state.alt / 1000.f; } @@ -2729,7 +2666,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.vxy_max = INFINITY; hil_local_pos.vz_max = INFINITY; hil_local_pos.hagl_min = INFINITY; - hil_local_pos.hagl_max = INFINITY; + hil_local_pos.hagl_max_z = INFINITY; + hil_local_pos.hagl_max_xy = INFINITY; hil_local_pos.timestamp = hrt_absolute_time(); _local_pos_pub.publish(hil_local_pos); } @@ -2966,7 +2904,6 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd); - tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance); tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry); tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager); tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); @@ -3065,7 +3002,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - gimbal_information.gimbal_device_compid = msg->compid; + gimbal_information.gimbal_device_id = msg->compid; _gimbal_device_information_pub.publish(gimbal_information); } @@ -3092,6 +3029,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t gimbal_attitude_status.failure_flags = gimbal_device_attitude_status_msg.failure_flags; gimbal_attitude_status.received_from_mavlink = true; + gimbal_attitude_status.gimbal_device_id = gimbal_device_attitude_status_msg.gimbal_device_id; _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 39fe7c33f9..3d617a7731 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -67,7 +67,6 @@ #include #include #include -#include #include #include #include @@ -111,8 +110,6 @@ #include #include #include -#include -#include #include #if !defined(CONSTRAINED_FLASH) @@ -202,8 +199,6 @@ private: void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); void handle_message_tunnel(mavlink_message_t *msg); - void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); - void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); #if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used void handle_message_set_velocity_limits(mavlink_message_t *msg); @@ -230,11 +225,10 @@ private: * * @param msgId The ID of the message interval to be set. * @param interval The interval in usec to send the message. - * @param data_rate The total link data rate in bytes per second. * * @return PX4_OK on success, PX4_ERROR on fail. */ - int set_message_interval(int msgId, float interval, int data_rate = -1); + int set_message_interval(int msgId, float interval, float param3, float param4, float param7); void get_message_interval(int msgId); bool evaluate_target_ok(int command, int target_system, int target_component); @@ -303,7 +297,6 @@ private: uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Publication _camera_status_pub{ORB_ID(camera_status)}; uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; - uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; @@ -332,8 +325,6 @@ private: uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; @@ -403,7 +394,6 @@ private: hrt_abstime _heartbeat_component_telemetry_radio{0}; hrt_abstime _heartbeat_component_log{0}; hrt_abstime _heartbeat_component_osd{0}; - hrt_abstime _heartbeat_component_obstacle_avoidance{0}; hrt_abstime _heartbeat_component_visual_inertial_odometry{0}; hrt_abstime _heartbeat_component_pairing_manager{0}; hrt_abstime _heartbeat_component_udp_bridge{0}; diff --git a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp index 5e4dea2a03..ae5f229cfb 100644 --- a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp +++ b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp @@ -70,6 +70,7 @@ private: uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; bool send() override { @@ -78,14 +79,22 @@ private: if (_att_sub.update(&att)) { mavlink_autopilot_state_for_gimbal_device_t msg{}; + bool hil_state = false; + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + hil_state = vehicle_status.hil_state; + } + //msg.target_system = 0; // TODO //msg.target_component = 0; // TODO msg.time_boot_us = att.timestamp; - msg.q[0] = att.q[0]; - msg.q[1] = att.q[1]; - msg.q[2] = att.q[2]; - msg.q[3] = att.q[3]; + // In HIL mode the gimbal is not moving. Send a static attitude to not disturb the gimbal + msg.q[0] = !hil_state ? att.q[0] : 1; + msg.q[1] = !hil_state ? att.q[1] : 0; + msg.q[2] = !hil_state ? att.q[2] : 0; + msg.q[3] = !hil_state ? att.q[3] : 0; msg.q_estimated_delay_us = 0; // I don't know. { @@ -102,7 +111,7 @@ private: { vehicle_attitude_setpoint_s att_sp; - if (_att_sp_sub.copy(&att_sp)) { + if (!hil_state && _att_sp_sub.copy(&att_sp)) { msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate; } } diff --git a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp index d19aeb4613..ae077c8239 100644 --- a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp +++ b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp @@ -86,7 +86,8 @@ private: const bool cannot_be_selected = (vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0; // Set the mode name if not a standard mode - available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state); + available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state, vehicle_status.vehicle_type, + vehicle_status.is_vtol); if (mode_util::isAdvanced(nav_state)) { available_modes.properties |= MAV_MODE_PROPERTY_ADVANCED; diff --git a/src/modules/mavlink/streams/CURRENT_MODE.hpp b/src/modules/mavlink/streams/CURRENT_MODE.hpp index 5527801670..2be1c47c4b 100644 --- a/src/modules/mavlink/streams/CURRENT_MODE.hpp +++ b/src/modules/mavlink/streams/CURRENT_MODE.hpp @@ -66,7 +66,8 @@ private: mavlink_current_mode_t current_mode{}; current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data; current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data; - current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state); + current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state, + vehicle_status.vehicle_type, vehicle_status.is_vtol); mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), ¤t_mode); return true; } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 615a797488..ae7f3cbad1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -93,6 +93,10 @@ private: msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; + + msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; + msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp index d0aa42f9f0..ff3def5f09 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp @@ -83,6 +83,7 @@ private: msg.pitch_max = gimbal_device_information.pitch_max; msg.yaw_min = gimbal_device_information.yaw_min; msg.yaw_max = gimbal_device_information.yaw_max; + msg.gimbal_device_id = gimbal_device_information.gimbal_device_id; mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp index 6d2997fe71..d5b015cc07 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -72,39 +72,21 @@ private: mavlink_global_position_int_t msg{}; - if (lpos.z_valid && lpos.z_global) { - msg.alt = (-lpos.z + lpos.ref_alt) * 1000.0f; - - } else { - // fall back to baro altitude - vehicle_air_data_s air_data{}; - _air_data_sub.copy(&air_data); - - if (air_data.timestamp > 0) { - msg.alt = air_data.baro_alt_meter * 1000.0f; - } - } - home_position_s home{}; _home_sub.copy(&home); if ((home.timestamp > 0) && home.valid_alt) { - if (lpos.z_valid) { - msg.relative_alt = -(lpos.z - home.z) * 1000.0f; - - } else { - msg.relative_alt = msg.alt - (home.alt * 1000.0f); - } + msg.relative_alt = (gpos.alt - home.alt) * 1000.0f; } else { - if (lpos.z_valid) { - msg.relative_alt = -lpos.z * 1000.0f; - } + msg.relative_alt = gpos.alt * 1000.0f; } msg.time_boot_ms = gpos.timestamp / 1000; + msg.lat = gpos.lat * 1e7; msg.lon = gpos.lon * 1e7; + msg.alt = gpos.alt * 1000.0f; msg.vx = lpos.vx * 100.0f; msg.vy = lpos.vy * 100.0f; diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf..7a3d96cb38 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -193,7 +193,7 @@ private: msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_pa; msg.pressure_alt = air_data.baro_alt_meter; - msg.temperature = air_data.baro_temp_celcius; + msg.temperature = air_data.ambient_temperature; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 6aca9f7d98..8623950e02 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -521,6 +522,7 @@ private: update_gps(); update_vehicle_status(); update_wind(); + update_vehicle_air_data(); } void update_airspeed() @@ -529,7 +531,6 @@ private: if (_airspeed_sub.update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); - _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); } } @@ -610,6 +611,15 @@ private: } } + void update_vehicle_air_data() + { + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _temperature.add_value(air_data.ambient_temperature, _update_rate_filtered); + } + } + void set_default_values(mavlink_high_latency2_t &msg) const { msg.airspeed = 0; @@ -659,6 +669,7 @@ private: uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _health_report_sub{ORB_ID(health_report)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/mavlink/streams/RAW_RPM.hpp b/src/modules/mavlink/streams/RAW_RPM.hpp index 9d2088123f..cfc4ceb54d 100644 --- a/src/modules/mavlink/streams/RAW_RPM.hpp +++ b/src/modules/mavlink/streams/RAW_RPM.hpp @@ -68,7 +68,7 @@ private: mavlink_raw_rpm_t msg{}; msg.index = i; - msg.frequency = rpm.indicated_frequency_rpm; + msg.frequency = rpm.rpm_estimate; mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg); updated = true; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index c875eaceaa..7a89501793 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -144,7 +144,6 @@ private: fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_BATTERY, health_component_t::battery, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, health_component_t::motors_escs, msg); fillOutComponent(health_report, MAV_SYS_STATUS_RECOVERY_SYSTEM, health_component_t::parachute, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, health_component_t::avoidance, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_ACCEL, health_component_t::accel, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_GYRO, health_component_t::gyro, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); diff --git a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp deleted file mode 100644 index 174c633fd2..0000000000 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ - -#ifndef TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP -#define TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP - -#include - -class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream -{ -public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTrajectoryRepresentationWaypoints(mavlink); } - - const char *get_name() const override { return get_name_static(); } - uint16_t get_id() override { return get_id_static(); } - - static constexpr const char *get_name_static() { return "TRAJECTORY_REPRESENTATION_WAYPOINTS"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; } - - unsigned get_size() override - { - if (_traj_wp_avoidance_sub.advertised()) { - return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - - return 0; - } - -private: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} - - uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - bool send() override - { - vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; - - if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) { - mavlink_trajectory_representation_waypoints_t msg{}; - - msg.time_usec = traj_wp_avoidance_desired.timestamp; - int number_valid_points = 0; - - for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { - msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0]; - msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1]; - msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2]; - - msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0]; - msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1]; - msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2]; - - msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0]; - msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1]; - msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2]; - - msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw; - msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed; - - switch (traj_wp_avoidance_desired.waypoints[i].type) { - case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; - break; - - case position_setpoint_s::SETPOINT_TYPE_LOITER: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; - break; - - case position_setpoint_s::SETPOINT_TYPE_LAND: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - break; - - default: - msg.command[i] = UINT16_MAX; - } - - if (traj_wp_avoidance_desired.waypoints[i].point_valid) { - number_valid_points++; - } - - } - - msg.valid_points = number_valid_points; - - mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg); - - return true; - } - - return false; - } -}; - -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 6f1585f801..02f3894f0e 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -68,17 +68,21 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const qd_red = qd; } else { - // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + // Transform rotation from current to desired thrust vector into a world frame reduced desired attitude. + // This is a right multiplication as the tilt error quaternion is obtained from two Z vectors expressed in the world frame. qd_red *= q; } - // mix full and reduced desired attitude - Quatf q_mix = qd_red.inversed() * qd; - q_mix.canonicalize(); + // With a full desired attitude given by: qd = qd_red * qd_dyaw, extract the delta yaw component. + // By definition, the delta yaw quaternion has the form (cos(angle/2), 0, 0, sin(angle/2)) + Quatf qd_dyaw = qd_red.inversed() * qd; + qd_dyaw.canonicalize(); // catch numerical problems with the domain of acosf and asinf - q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); - q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); - qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f); + qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f); + + // scale the delta yaw angle and re-combine the desired attitude + qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3)))); // quaternion attitude control law, qe is rotation from q to qd const Quatf qe = q.inversed() * qd; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c15..995eea4256 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -98,9 +99,11 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -118,8 +121,10 @@ private: matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + float _hover_thrust{NAN}; + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index f6cc5db43d..22b78d519c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; + { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } + } + } + + if (!PX4_ISFINITE(_hover_thrust)) { + _hover_thrust = _param_mpc_thr_hover.get(); + } + + // throttle_stick_input is in range [-1, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; - default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle - thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, - {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale to hover throttle at 0 stick input + thrust = math::interpolateNXY(throttle_stick_input, + {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()}); break; } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 07c23bbb5d..d8b97336b9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run() } } - // new local position setpoint needed every iteration - if (!_vehicle_local_position_setpoint_sub.updated()) { - return; - } - // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run() _hover_thrust_ekf.predict(dt); - vehicle_local_position_setpoint_s local_pos_sp; + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); - if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { - if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + control_allocator_status_s control_allocator_status; + + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint) + && _control_allocator_status_sub.update(&control_allocator_status) + && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms) + && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms) + ) { + matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust); + matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated; + + const matrix::Quatf q_att{vehicle_attitude.q}; + matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated); + + if (PX4_ISFINITE(thrust_allocated(2))) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects @@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run() 1.f); _hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z)); - _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); @@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run() _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state(); - publishStatus(local_pos.timestamp); + publishStatus(vehicle_thrust_setpoint.timestamp); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index b1d0324298..1738cfe785 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -54,10 +54,12 @@ #include #include #include +#include #include #include -#include #include +#include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +103,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; hrt_abstime _timestamp_last{0}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index f7cd9bbbe0..6d58b85cd7 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -167,7 +167,6 @@ private: (ParamFloat) _param_mpc_vel_man_side, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index 9174b1ef0d..4f617d0092 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -73,15 +73,13 @@ PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); /** * Altitude reference mode * - * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in - * flight due to sensor drift. - * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down - * with terrain height variation. Requires a distance to ground sensor. The height controller will - * revert to using height above origin if the distance to ground estimate becomes invalid as indicated - * by the local_position.distance_bottom_valid message being false. - * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative - * to earth frame origin when moving horizontally. - * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. + * Control height + * 0: relative earth frame origin which may drift due to sensors + * 1: relative to ground (requires distance sensor) which changes with terrain variation. + * It will revert to relative earth frame if the distance to ground estimate becomes invalid. + * 2: relative to ground (requires distance sensor) when stationary + * and relative to earth frame when moving horizontally. + * The speed threshold is MPC_HOLD_MAX_XY * * @min 0 * @max 2 diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index a65ef4f571..b70bb95a3e 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -35,18 +35,14 @@ * Position/Altitude mode variant * * The supported sub-modes are: - * - "Direct velocity": + * Direct velocity: * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * - "Smoothed velocity": - * Sticks map to velocity but with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). - * - "Acceleration based": - * Sticks map to acceleration and there's a virtual brake drag + * Acceleration based: + * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity - * @value 3 Smoothed velocity * @value 4 Acceleration based * @group Multicopter Position Control */ @@ -104,7 +100,6 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); * * MPC_POS_MODE * 1 just deceleration - * 3 acceleration and deceleration * 4 not used, use MPC_ACC_HOR instead * * @unit m/s^2 @@ -119,13 +114,12 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); /** * Maximum horizontal and vertical jerk in Position/Altitude mode * - * Limit the maximum jerk of the vehicle (how fast the acceleration can change). - * A lower value leads to smoother motions but limits agility - * (how fast it can change directions or break). + * Limit the maximum jerk (acceleration change) of the vehicle. + * A lower value leads to smoother motions but limits agility. * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. + * Only used with MPC_POS_MODE Acceleration based. * * @unit m/s^3 * @min 0.5 diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c index 481d9cce66..50cf5a6536 100644 --- a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c @@ -75,18 +75,16 @@ PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); /** * Thrust curve mapping in Stabilized Mode * - * This parameter defines how the throttle stick input is mapped to collective thrust - * in Stabilized mode. + * Defines how the throttle stick is mapped to collective thrust in Stabilized mode. * - * In case the default is used ('Rescale to hover thrust'), the stick input is linearly - * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * Rescale to hover thrust: + * Stick input is linearly rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). * - * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful - * in case the hover thrust is very low and the default would lead to too much distortion - * (e.g. if hover thrust is set to 20%, then 80% of the upper thrust range is squeezed into the - * upper half of the stick range). + * No Rescale: + * Directly map the stick 1:1 to the output. + * Can be useful with very low hover thrust which leads to much distortion and the upper half getting sensitive. * - * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * With MPC_THR_HOVER 0.5 both modes are the same. * * @value 0 Rescale to hover thrust * @value 1 No Rescale diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index e8a371d1e2..3931125289 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -95,6 +95,8 @@ MulticopterRateControl::parameters_updated() // manual rate control acro mode rate limits _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get())); + + _output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get()); } void @@ -214,7 +216,11 @@ MulticopterRateControl::Run() } // run rate controller - const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + Vector3f torque_setpoint = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + + // apply low-pass filtering on yaw axis to reduce high frequency torque caused by rotor acceleration + torque_setpoint(2) = _output_lpf_yaw.update(torque_setpoint(2), dt); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; @@ -227,9 +233,9 @@ MulticopterRateControl::Run() vehicle_torque_setpoint_s vehicle_torque_setpoint{}; _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); - vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f; - vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f; - vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f; + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_setpoint(0)) ? torque_setpoint(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_setpoint(1)) ? torque_setpoint(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_setpoint(2)) ? torque_setpoint(2) : 0.f; // scale setpoints by battery status if enabled if (_param_mc_bat_scale_en.get()) { diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 0f402c12c3..1a2427da18 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -129,6 +130,8 @@ private: float _energy_integration_time{0.0f}; float _control_energy[4] {}; + AlphaFilter _output_lpf_yaw; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, @@ -150,6 +153,7 @@ private: (ParamFloat) _param_mc_yawrate_d, (ParamFloat) _param_mc_yawrate_ff, (ParamFloat) _param_mc_yawrate_k, + (ParamFloat) _param_mc_yaw_tq_cutoff, (ParamFloat) _param_mc_acro_r_max, (ParamFloat) _param_mc_acro_p_max, diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 2571a70c1f..b37c52e746 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -292,3 +292,17 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); * @group Multicopter Rate Control */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * Low pass filter cutoff frequency for yaw torque setpoint + * + * Reduces vibrations by lowering high frequency torque caused by rotor acceleration. + * 0 disables the filter + * + * @min 0 + * @max 10 + * @unit Hz + * @decimal 3 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAW_TQ_CUTOFF, 2.f); diff --git a/src/modules/navigator/Kconfig b/src/modules/navigator/Kconfig index 83e42cc594..429acd60e7 100644 --- a/src/modules/navigator/Kconfig +++ b/src/modules/navigator/Kconfig @@ -19,3 +19,11 @@ menuconfig MODE_NAVIGATOR_VTOL_TAKEOFF Add VTOL takeoff mode to enable support for MAV_CMD_NAV_VTOL_TAKEOFF. The VTOL takes off in MC mode and transition to FW. The mode ends with an infinite loiter + +menuconfig NAVIGATOR_ADSB + bool "Include traffic reporting and avoidance" + default y + depends on MODULES_NAVIGATOR + ---help--- + Add support for acting on ADSB transponder_report or ADSB_VEHICLE MAVLink messages. + Actions are warnings, Loiter, Land and RTL without climb. diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index c2b3a81198..c35d2c1bde 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,12 +46,7 @@ FeasibilityChecker::FeasibilityChecker() : void FeasibilityChecker::reset() { - _mission_validity_failed = false; - _takeoff_failed = false; - _land_pattern_validity_failed = false; - _distance_between_waypoints_failed = false; - _fixed_wing_land_approach_failed = false; - _takeoff_land_available_failed = false; + _checks_failed.value = 0; _found_item_with_position = false; _has_vtol_takeoff = false; @@ -155,11 +150,11 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int updateData(); } - if (!_mission_validity_failed) { - _mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); + if (!_checks_failed.flags.mission_validity_failed) { + _checks_failed.flags.mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); } - if (_mission_validity_failed) { + if (_checks_failed.flags.mission_validity_failed) { // if a mission item is not valid then abort the other checks return false; } @@ -177,7 +172,7 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int } if (current_index == total_count - 1) { - _takeoff_land_available_failed = !checkTakeoffLandAvailable(); + _checks_failed.flags.takeoff_land_available_failed = !checkTakeoffLandAvailable(); } _mission_item_previous = mission_item; @@ -188,39 +183,39 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) { - if (!_distance_between_waypoints_failed) { - _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); + if (!_checks_failed.flags.distance_between_waypoints_failed) { + _checks_failed.flags.distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); } if (!_first_waypoint_found) { checkHorizontalDistanceToFirstWaypoint(mission_item); } - if (!_takeoff_failed) { - _takeoff_failed = !checkTakeoff(mission_item); + if (!_checks_failed.flags.takeoff_failed) { + _checks_failed.flags.takeoff_failed = !checkTakeoff(mission_item); } - if (!_items_fit_to_vehicle_type_failed) { - _items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); + if (!_checks_failed.flags.items_fit_to_vehicle_type_failed) { + _checks_failed.flags.items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); } } void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index) { - if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + if (!_checks_failed.flags.land_pattern_validity_failed) { + _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } } void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index) { - if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + if (!_checks_failed.flags.land_pattern_validity_failed) { + _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } - if (!_fixed_wing_land_approach_failed) { - _fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); + if (!_checks_failed.flags.fixed_wing_land_approach_failed) { + _checks_failed.flags.fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); } } @@ -286,6 +281,7 @@ bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_SOURCE && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { @@ -371,6 +367,7 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY && mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && + mission_item.nav_cmd != NAV_CMD_SET_CAMERA_SOURCE && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index d0905d3635..1c002baf9a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -77,12 +77,7 @@ public: */ bool someCheckFailed() { - return _takeoff_failed || - _distance_between_waypoints_failed || - _land_pattern_validity_failed || - _fixed_wing_land_approach_failed || - _mission_validity_failed || - _takeoff_land_available_failed; + return _checks_failed.value != 0; } /** @@ -110,14 +105,18 @@ private: matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; - // internal flags to keep track of which checks failed - bool _mission_validity_failed{false}; - bool _takeoff_failed{false}; - bool _land_pattern_validity_failed{false}; - bool _distance_between_waypoints_failed{false}; - bool _fixed_wing_land_approach_failed{false}; - bool _takeoff_land_available_failed{false}; - bool _items_fit_to_vehicle_type_failed{false}; + union checks_failed_u { + struct { + bool mission_validity_failed : 1; + bool takeoff_failed : 1; + bool land_pattern_validity_failed : 1; + bool distance_between_waypoints_failed : 1; + bool fixed_wing_land_approach_failed : 1; + bool takeoff_land_available_failed : 1; + bool items_fit_to_vehicle_type_failed : 1; + } flags; + uint16_t value {0}; + } _checks_failed{}; // internal checkTakeoff related variables bool _found_item_with_position{false}; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 00a1e3f940..09fa712b20 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -511,7 +511,7 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, } if (!_projection_reference.isInitialized()) { - _projection_reference.initReference(lat, lon); + _projection_reference.initReference(lat, lon, hrt_absolute_time()); } float x1, y1, x2, y2; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 0a2346154a..2c351ffa6e 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -58,8 +58,9 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon); + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && _navigator->get_local_position()->xy_global) { // only execute if global position is valid + _navigator->preproject_stop_point(_mission_item.lat, _mission_item.lon); } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); @@ -82,13 +83,13 @@ void Land::on_active() { /* for VTOL update landing location during back transition */ - if (_navigator->get_vstatus()->is_vtol && - _navigator->get_vstatus()->in_transition_mode) { + if (_navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->in_transition_mode + && _navigator->get_local_position()->xy_global) { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); // create a wp in front of the VTOL while in back-transition, based on MPC settings that will apply in MC phase afterwards - _navigator->calculate_breaking_stop(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); - + _navigator->preproject_stop_point(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon); _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index db61030528..86efa42ee9 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -109,7 +109,7 @@ Loiter::set_loiter_position() setLoiterItemFromCurrentPositionSetpoint(&_mission_item); } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - setLoiterItemFromCurrentPositionWithBreaking(&_mission_item); + setLoiterItemFromCurrentPositionWithBraking(&_mission_item); } else { setLoiterItemFromCurrentPosition(&_mission_item); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8c50780847..08a200eda0 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -211,8 +211,14 @@ void Mission::setActiveMissionItems() // prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() && - _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT; + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (fw_on_mission_landing || mc_landing_after_transition) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 37f104dea2..0d653a1882 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -299,15 +299,14 @@ MissionBase::on_active() replayCachedCameraModeItems(); } + // Replay cached gimbal commands immediately upon mission resume, but only after the vehicle has reached the final target altitude + if (haveCachedGimbalItems() && _work_item_type != WorkItemType::WORK_ITEM_TYPE_CLIMB) { + replayCachedGimbalItems(); + } // Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption. // Each replay function also clears the cached items afterwards if (_mission.current_seq > _mission_activation_index) { - // replay gimbal commands - if (haveCachedGimbalItems()) { - replayCachedGimbalItems(); - } - // replay trigger commands if (cameraWasTriggering()) { replayCachedTriggerItems(); diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index b8093ca01c..65a021d45d 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -483,8 +483,8 @@ private: mission_item_s _last_camera_trigger_item {}; mission_item_s _last_speed_change_item {}; - DEFINE_PARAMETERS( - (ParamFloat) _param_mis_dist_1wp, + DEFINE_PARAMETERS_CUSTOM_PARENT( + ModuleParams, (ParamInt) _param_mis_mnt_yaw_ctl ) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index c2b17bbedb..1b05628a6c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -81,7 +81,6 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: - case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: @@ -91,6 +90,7 @@ MissionBlock::is_mission_item_reached_or_completed() case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: + case NAV_CMD_SET_CAMERA_SOURCE: case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_CHANGE_SPEED: @@ -141,40 +141,14 @@ MissionBlock::is_mission_item_reached_or_completed() break; - case NAV_CMD_DO_WINCH: { - const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * - 1E-6f; // TODO: Add proper microseconds_to_seconds function - - if (_payload_deploy_ack_successful) { - PX4_DEBUG("Winch Deploy Ack received! Resuming mission"); - return true; - - } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { - PX4_DEBUG("Winch Deploy Timed out, resuming mission!"); - return true; - - } - - // We are still waiting for the acknowledgement / execution of deploy - return false; + case NAV_CMD_DO_WINCH: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GRIPPER: + if (now > _timestamp_command_timeout + (_command_timeout * 1_s)) { + return true; } - case NAV_CMD_DO_GRIPPER: { - const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f; - - if (_payload_deploy_ack_successful) { - PX4_DEBUG("Gripper Deploy Ack received! Resuming mission"); - return true; - - } else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) { - PX4_DEBUG("Gripper Deploy Timed out, resuming mission!"); - return true; - - } - - // We are still waiting for the acknowledgement / execution of deploy - return false; - } + return false; // Still waiting default: /* do nothing, this is a 3D waypoint */ @@ -444,7 +418,7 @@ MissionBlock::is_mission_item_reached_or_completed() /* check if the MAV was long enough inside the waypoint orbit */ if ((get_time_inside(_mission_item) < FLT_EPSILON) || - (now - _time_wp_reached >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + (now >= (hrt_abstime)(get_time_inside(_mission_item) * 1_s) + _time_wp_reached)) { time_inside_reached = true; } @@ -548,53 +522,37 @@ MissionBlock::issue_command(const mission_item_s &item) return; } - if (item.nav_cmd == NAV_CMD_DO_WINCH || - item.nav_cmd == NAV_CMD_DO_GRIPPER) { - // Initiate Payload Deployment - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - _navigator->publish_vehicle_cmd(&vcmd); + // This is to support legacy DO_MOUNT_CONTROL as part of a mission. + if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { + _navigator->acquire_gimbal_control(); + } - // Reset payload deploy flag & data to get ready to receive deployment ack result - _payload_deploy_ack_successful = false; - _payload_deployed_time = hrt_absolute_time(); + // Mission item's NAV_CMD enums directly map to the according vehicle command + // So set the raw value directly (MAV_FRAME_MISSION mission item) + vehicle_command_s vcmd = {}; + vcmd.command = item.nav_cmd; + vcmd.param1 = item.params[0]; + vcmd.param2 = item.params[1]; + vcmd.param3 = item.params[2]; + vcmd.param4 = item.params[3]; + vcmd.param5 = static_cast(item.params[4]); + vcmd.param6 = static_cast(item.params[5]); + vcmd.param7 = item.params[6]; - } else { + if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { + // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision + vcmd.param5 = item.lat; + vcmd.param6 = item.lon; - // This is to support legacy DO_MOUNT_CONTROL as part of a mission. - if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { - _navigator->acquire_gimbal_control(); + if (item.altitude_is_relative) { + vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; } + } - // Mission item's NAV_CMD enums directly map to the according vehicle command - // So set the raw value directly (MAV_FRAME_MISSION mission item) - vehicle_command_s vcmd = {}; - vcmd.command = item.nav_cmd; - vcmd.param1 = item.params[0]; - vcmd.param2 = item.params[1]; - vcmd.param3 = item.params[2]; - vcmd.param4 = item.params[3]; - vcmd.param5 = static_cast(item.params[4]); - vcmd.param6 = static_cast(item.params[5]); - vcmd.param7 = item.params[6]; + _navigator->publish_vehicle_cmd(&vcmd); - if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION) { - // We need to send out the ROI location that was parsed potentially with double precision to lat/lon because mission item parameters 5 and 6 only have float precision - vcmd.param5 = item.lat; - vcmd.param6 = item.lon; - - if (item.altitude_is_relative) { - vcmd.param7 = item.altitude + _navigator->get_home_position()->alt; - } - } - - _navigator->publish_vehicle_cmd(&vcmd); + if (item_has_timeout(item)) { + _timestamp_command_timeout = hrt_absolute_time(); } } @@ -618,10 +576,13 @@ MissionBlock::get_time_inside(const mission_item_s &item) const // and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER // or so, and have a function named 'item_is_conditional' // Reference: https://mavlink.io/en/services/mission.html#mavlink_commands +// A similar condition applies to DO_GIMBAL_MANAGER_PITCHYAW bool MissionBlock::item_has_timeout(const mission_item_s &item) { - return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER; + return item.nav_cmd == NAV_CMD_DO_WINCH || + item.nav_cmd == NAV_CMD_DO_GRIPPER || + item.nav_cmd == NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW; } bool @@ -661,11 +622,11 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->lon = item.lon; sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; - sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : + sp->loiter_radius = (fabsf(item.loiter_radius) > FLT_EPSILON) ? fabsf(item.loiter_radius) : _navigator->get_loiter_radius(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; - if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) { + if (item.acceptance_radius > FLT_EPSILON && PX4_ISFINITE(item.acceptance_radius)) { // if the mission item has a specified acceptance radius, overwrite the default one from parameters sp->acceptance_radius = item.acceptance_radius; @@ -771,11 +732,11 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item) } void -MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item) +MissionBlock::setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item) { setLoiterItemCommonFields(item); - _navigator->calculate_breaking_stop(item->lat, item->lon); + _navigator->preproject_stop_point(item->lat, item->lon); item->altitude = _navigator->get_global_position()->alt; item->loiter_radius = _navigator->get_loiter_radius(); @@ -831,8 +792,15 @@ MissionBlock::set_land_item(struct mission_item_s *item) item->nav_cmd = NAV_CMD_LAND; // set land item to current position - item->lat = _navigator->get_global_position()->lat; - item->lon = _navigator->get_global_position()->lon; + if (_navigator->get_local_position()->xy_global) { + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + + } else { + item->lat = (double)NAN; + item->lon = (double)NAN; + } + item->yaw = NAN; item->altitude = 0; @@ -1010,7 +978,7 @@ void MissionBlock::startPrecLand(uint16_t land_precision) _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); _navigator->get_precland()->on_activation(); - } else { //_mission_item.land_precision == 2 + } else if (_mission_item.land_precision == 2) { _navigator->get_precland()->set_mode(PrecLandMode::Required); _navigator->get_precland()->on_activation(); } @@ -1058,7 +1026,8 @@ void MissionBlock::updateMaxHaglFailsafe() const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; if (_navigator->get_global_position()->terrain_alt_valid - && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + && ((target_alt - _navigator->get_global_position()->terrain_alt) + > math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) { // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index c19fcfe118..52f626a2fe 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -108,30 +108,11 @@ public: static bool item_contains_marker(const mission_item_s &item); /** - * @brief Set the payload deployment successful flag object + * Set the item_has_timeout() command timeout * - * Function is accessed in Navigator (navigator_main.cpp) to flag when a successful - * payload deployment ack command has been received. Which in turn allows the mission item - * to continue to the next in the 'is_mission_item_reached_or_completed' function below + * @param timeout Timeout in seconds */ - void set_payload_depolyment_successful_flag(bool payload_deploy_result) - { - _payload_deploy_ack_successful = payload_deploy_result; - } - - /** - * @brief Set the payload deployment timeout - * - * Accessed in Navigator to set the appropriate timeout to wait for while waiting for a successful - * payload delivery acknowledgement. If the payload deployment takes longer than timeout, mission will - * continue into the next item automatically. - * - * @param timeout_s Timeout in seconds - */ - void set_payload_deployment_timeout(const float timeout_s) - { - _payload_deploy_timeout_s = timeout_s; - } + void set_command_timeout(const float timeout) { _command_timeout = timeout; } /** * Copies position from setpoint if valid, otherwise copies current position @@ -183,7 +164,7 @@ protected: void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item); void setLoiterItemFromCurrentPosition(struct mission_item_s *item); - void setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item); + void setLoiterItemFromCurrentPositionWithBraking(struct mission_item_s *item); void setLoiterItemCommonFields(struct mission_item_s *item); @@ -247,12 +228,10 @@ protected: hrt_abstime _time_wp_reached{0}; - /* Payload Deploy internal states are used by two NAV_CMDs: DO_WINCH and DO_GRIPPER */ - bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment - hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts - float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + // Mission items that have a timeout to allow the payload e.g. gripper, winch, gimbal executing the command see item_has_timeout() + hrt_abstime _timestamp_command_timeout{0}; ///< Timestamp when the current item_has_timeout() command was started + float _command_timeout{0.f}; ///< Time in seconds any item_has_timeout() command should be waited for before continuing the mission private: void updateMaxHaglFailsafe(); - }; diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 1c637b8a50..c0bbf5c254 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -74,15 +74,15 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); /** - * Maximal horizontal distance from current position to first waypoint + * Maximal horizontal distance from Home to first waypoint * - * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. - * Set a value of zero or less to disable. The mission will not be started if the current - * waypoint is more distant than MIS_DIST_1WP from the current position. + * There will be a warning message if the current waypoint is more distant than MIS_DIST_1WP from Home. + * Has no effect on mission validity. + * Set a value of zero or less to disable. * * @unit m * @min -1 - * @max 10000 + * @max 100000 * @decimal 1 * @increment 100 * @group Mission @@ -132,17 +132,6 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f); */ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f); -/** - * Timeout for a successful payload deployment acknowledgement - * - * @unit s - * @min 0 - * @decimal 1 - * @increment 1 - * @group Mission - */ -PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); - /** * Landing abort min altitude * @@ -155,3 +144,21 @@ PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f); * @group Mission */ PARAM_DEFINE_INT32(MIS_LND_ABRT_ALT, 30); + +/** + * Timeout to allow the payload to execute the mission command + * + * Ensure: + * gripper: NAV_CMD_DO_GRIPPER + * has released before continuing mission. + * winch: CMD_DO_WINCH + * has delivered before continuing mission. + * gimbal: CMD_DO_GIMBAL_MANAGER_PITCHYAW + * has reached the commanded orientation before beginning to take pictures. + * + * @unit s + * @min 0 + * @decimal 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_COMMAND_TOUT, 0.f); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 87dd05d166..520430c014 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -56,8 +56,6 @@ # define NUM_MISSIONS_SUPPORTED 500 #endif -#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ - /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_IDLE = 0, @@ -89,6 +87,7 @@ enum NAV_CMD { NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206, NAV_CMD_OBLIQUE_SURVEY = 260, NAV_CMD_SET_CAMERA_MODE = 530, + NAV_CMD_SET_CAMERA_SOURCE = 534, NAV_CMD_SET_CAMERA_ZOOM = 531, NAV_CMD_SET_CAMERA_FOCUS = 532, NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7fece40a4e..5e30d22ccc 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,8 +57,11 @@ #include "GeofenceBreachAvoidance/geofence_breach_avoidance.h" +#if CONFIG_NAVIGATOR_ADSB #include +#endif // CONFIG_NAVIGATOR_ADSB #include +#include #include #include #include @@ -136,14 +139,14 @@ public: */ void publish_vehicle_cmd(vehicle_command_s *vcmd); +#if CONFIG_NAVIGATOR_ADSB /** * Check nearby traffic for potential collisions */ void check_traffic(); - void take_traffic_conflict_action(); - void run_fake_traffic(); +#endif // CONFIG_NAVIGATOR_ADSB /** * Setters @@ -278,7 +281,7 @@ public: void release_gimbal_control(); void set_gimbal_neutral(); - void calculate_breaking_stop(double &lat, double &lon); + void preproject_stop_point(double &lat, double &lon); void stop_capturing_images(); void disable_camera_trigger(); @@ -365,7 +368,10 @@ private: Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ +#if CONFIG_NAVIGATOR_ADSB AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */ + traffic_buffer_s _traffic_buffer{}; +#endif // CONFIG_NAVIGATOR_ADSB NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */ @@ -381,8 +387,6 @@ private: float _cruising_speed_current_mode{-1.0f}; float _mission_throttle{NAN}; - traffic_buffer_s _traffic_buffer{}; - bool _is_capturing_images{false}; // keep track if we need to stop capturing images @@ -424,10 +428,10 @@ private: _param_nav_min_gnd_dist, /**< minimum distance to ground (Mission and RTL)*/ // non-navigator parameters: Mission (MIS_*) - (ParamFloat) _param_mis_takeoff_alt, - (ParamFloat) _param_mis_yaw_tmt, - (ParamFloat) _param_mis_yaw_err, - (ParamFloat) _param_mis_payload_delivery_timeout, - (ParamInt) _param_mis_lnd_abrt_alt + (ParamFloat) _param_mis_takeoff_alt, + (ParamFloat) _param_mis_yaw_tmt, + (ParamFloat) _param_mis_yaw_err, + (ParamInt) _param_mis_lnd_abrt_alt, + (ParamFloat) _param_mis_command_tout ) }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cd73e7e7d3..34b4d2c00e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -115,13 +114,6 @@ Navigator::Navigator() : _distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF; _distance_sensor_mode_change_request_pub.update(); - // Update the timeout used in mission_block (which can't hold it's own parameters) - _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); - - _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), - _param_nav_traff_a_ver.get(), - _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); - reset_triplets(); } @@ -149,7 +141,12 @@ void Navigator::params_update() param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor); } - _mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get()); + _mission.set_command_timeout(_param_mis_command_tout.get()); +#if CONFIG_NAVIGATOR_ADSB + _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), + _param_nav_traff_a_ver.get(), + _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); +#endif // CONFIG_NAVIGATOR_ADSB } void Navigator::run() @@ -356,7 +353,7 @@ void Navigator::run() if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { - calculate_breaking_stop(rep->current.lat, rep->current.lon); + preproject_stop_point(rep->current.lat, rep->current.lon); } else { // For fixedwings we can use the current vehicle's position to define the loiter point @@ -467,7 +464,7 @@ void Navigator::run() if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) { - calculate_breaking_stop(rep->current.lat, rep->current.lon); + preproject_stop_point(rep->current.lat, rep->current.lon); } if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) { @@ -756,8 +753,10 @@ void Navigator::run() } } +#if CONFIG_NAVIGATOR_ADSB /* Check for traffic */ check_traffic(); +#endif // CONFIG_NAVIGATOR_ADSB /* Check geofence violation */ geofence_breach_check(); @@ -1143,12 +1142,11 @@ float Navigator::get_altitude_acceptance_radius() } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); + const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); + if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { + alt_acceptance_radius = curr_sp.alt_acceptance_radius; - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { - alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } return alt_acceptance_radius; @@ -1210,14 +1208,6 @@ bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw) { float yaw = mission_item_yaw; - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that - // the waypoint can be reached from any direction - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { - yaw = pos_ctrl_status.yaw_acceptance; - } - return PX4_ISFINITE(yaw); } @@ -1226,6 +1216,7 @@ void Navigator::load_fence_from_file(const char *filename) _geofence.loadFromFile(filename); } +#if CONFIG_NAVIGATOR_ADSB void Navigator::take_traffic_conflict_action() { @@ -1255,21 +1246,17 @@ void Navigator::take_traffic_conflict_action() } } - } void Navigator::run_fake_traffic() { - _adsb_conflict.run_fake_traffic(get_global_position()->lat, get_global_position()->lon, get_global_position()->alt); } void Navigator::check_traffic() { - if (_traffic_sub.updated()) { - _traffic_sub.copy(&_adsb_conflict._transponder_report); uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | @@ -1288,8 +1275,8 @@ void Navigator::check_traffic() } _adsb_conflict.remove_expired_conflicts(); - } +#endif // CONFIG_NAVIGATOR_ADSB bool Navigator::abort_landing() { @@ -1332,11 +1319,14 @@ int Navigator::custom_command(int argc, char *argv[]) get_instance()->load_fence_from_file(GEOFENCE_FILENAME); return 0; +#if CONFIG_NAVIGATOR_ADSB + } else if (!strcmp(argv[0], "fake_traffic")) { get_instance()->run_fake_traffic(); return 0; +#endif // CONFIG_NAVIGATOR_ADSB } return print_usage("unknown command"); @@ -1411,9 +1401,13 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) vcmd->confirmation = false; vcmd->from_external = false; + int target_camera_component_id; + // The camera commands are not processed on the autopilot but will be // sent to the mavlink links to other components. switch (vcmd->command) { + + case NAV_CMD_IMAGE_START_CAPTURE: if (static_cast(vcmd->param3) == 1) { @@ -1433,12 +1427,52 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) _is_capturing_images = true; } - vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + target_camera_component_id = static_cast(vcmd->param1); // Target id from param 1 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + } + break; case NAV_CMD_IMAGE_STOP_CAPTURE: _is_capturing_images = false; - vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + target_camera_component_id = static_cast(vcmd->param1); // Target id from param 1 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + } + + break; + + case NAV_CMD_SET_CAMERA_MODE: + target_camera_component_id = static_cast(vcmd->param1); // Target id from param 1 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + } + + break; + + case NAV_CMD_SET_CAMERA_SOURCE: + target_camera_component_id = static_cast(vcmd->param1); // Target id from param 1 + + if (target_camera_component_id > 0 && target_camera_component_id < 256) { + vcmd->target_component = target_camera_component_id; + + } else { + vcmd->target_component = 100; // MAV_COMP_ID_CAMERA + } + break; case NAV_CMD_VIDEO_START_CAPTURE: @@ -1544,7 +1578,7 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos) return true; } -void Navigator::calculate_breaking_stop(double &lat, double &lon) +void Navigator::preproject_stop_point(double &lat, double &lon) { // For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back const float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx); diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 74c81569cf..e75db68629 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -81,7 +81,7 @@ PrecLand::on_activation() vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position(); if (!_map_ref.isInitialized()) { - _map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon); + _map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon, hrt_absolute_time()); } position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index cb33f350b7..f27c112aac 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -59,6 +59,7 @@ RTL::RTL(Navigator *navigator) : ModuleParams(navigator), _rtl_direct(navigator) { + _rtl_direct.initialize(); } void RTL::updateDatamanCache() @@ -157,24 +158,6 @@ void RTL::updateDatamanCache() _dataman_cache_landItem.update(); } -void RTL::on_inactivation() -{ - switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: // Fall through - case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_mission_type_handle->on_inactivation(); - break; - - case RtlType::RTL_DIRECT: - _rtl_direct.on_inactivation(); - break; - - default: - break; - } -} - void RTL::on_inactive() { _global_pos_sub.update(); @@ -187,21 +170,12 @@ void RTL::on_inactive() parameters_update(); - switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: - case RtlType::RTL_MISSION_FAST_REVERSE: - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_mission_type_handle->on_inactive(); - break; - - case RtlType::RTL_DIRECT: - _rtl_direct.on_inactive(); - break; - - default: - break; + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->run(false); } + _rtl_direct.run(false); + // Limit inactive calculation to 0.5Hz hrt_abstime now{hrt_absolute_time()}; @@ -230,7 +204,10 @@ void RTL::publishRemainingTimeEstimate() case RtlType::RTL_DIRECT_MISSION_LAND: case RtlType::RTL_MISSION_FAST: case RtlType::RTL_MISSION_FAST_REVERSE: - estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + if (_rtl_mission_type_handle) { + estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + } + break; default: @@ -250,12 +227,10 @@ void RTL::on_activation() case RtlType::RTL_MISSION_FAST: // Fall through case RtlType::RTL_MISSION_FAST_REVERSE: _rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt); - _rtl_mission_type_handle->on_activation(); break; case RtlType::RTL_DIRECT: _rtl_direct.setReturnAltMin(_enforce_rtl_alt); - _rtl_direct.on_activation(); break; default: @@ -279,16 +254,23 @@ void RTL::on_active() updateDatamanCache(); switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: - case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_mission_type_handle->on_active(); - _rtl_mission_type_handle->updateFailsafeChecks(); + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->run(true); + } + + _rtl_direct.run(false); break; case RtlType::RTL_DIRECT: - _rtl_direct.on_active(); - _rtl_direct.updateFailsafeChecks(); + _rtl_direct.run(true); + + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->run(false); + } + break; default: @@ -312,7 +294,10 @@ bool RTL::isLanding() case RtlType::RTL_MISSION_FAST: case RtlType::RTL_MISSION_FAST_REVERSE: case RtlType::RTL_DIRECT_MISSION_LAND: - is_landing = _rtl_mission_type_handle->isLanding(); + if (_rtl_mission_type_handle) { + is_landing = _rtl_mission_type_handle->isLanding(); + } + break; case RtlType::RTL_DIRECT: @@ -600,6 +585,8 @@ void RTL::init_rtl_mission_type() _set_rtl_mission_type = RtlType::NONE; } + mission_s new_mission = _mission_sub.get(); + switch (new_rtl_mission_type) { case RtlType::RTL_DIRECT_MISSION_LAND: _rtl_mission_type_handle = new RtlDirectMissionLand(_navigator); @@ -608,13 +595,13 @@ void RTL::init_rtl_mission_type() break; case RtlType::RTL_MISSION_FAST: - _rtl_mission_type_handle = new RtlMissionFast(_navigator); + _rtl_mission_type_handle = new RtlMissionFast(_navigator, new_mission); _set_rtl_mission_type = RtlType::RTL_MISSION_FAST; _rtl_type = RtlType::RTL_MISSION_FAST; break; case RtlType::RTL_MISSION_FAST_REVERSE: - _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator); + _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator, new_mission); _set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; break; @@ -622,6 +609,10 @@ void RTL::init_rtl_mission_type() default: break; } + + if (_rtl_mission_type_handle) { + _rtl_mission_type_handle->initialize(); + } } void RTL::parameters_update() diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 4ef13cb787..18a7ad3ea1 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -78,7 +78,6 @@ public: RTL_MISSION_FAST_REVERSE, }; - void on_inactivation() override; void on_inactive() override; void on_activation() override; void on_active() override; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index f255a9f11d..88f4da2249 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -73,7 +73,6 @@ void RtlDirect::on_inactivation() void RtlDirect::on_activation() { _global_pos_sub.update(); - _land_detected_sub.update(); _vehicle_status_sub.update(); parameters_update(); @@ -111,7 +110,7 @@ void RtlDirect::on_active() updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } - if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) { + if (_rtl_state == RTLState::LAND && _mission_item.land_precision > 0) { // Need to update the position and type on the current setpoint triplet. _navigator->get_precland()->on_active(); @@ -120,6 +119,12 @@ void RtlDirect::on_active() } } +void RtlDirect::on_inactive() +{ + _global_pos_sub.update(); + _vehicle_status_sub.update(); +} + void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s loiter_pos) { _home_pos_sub.update(); @@ -350,6 +355,7 @@ void RtlDirect::set_rtl_item() pos_yaw_sp.alt = loiter_altitude; pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled + altitude_acceptance_radius = FLT_MAX; setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); _navigator->reset_position_setpoint(pos_sp_triplet->previous); @@ -363,7 +369,9 @@ void RtlDirect::set_rtl_item() _mission_item.land_precision = _param_rtl_pld_md.get(); - startPrecLand(_mission_item.land_precision); + if (_mission_item.land_precision > 0) { + startPrecLand(_mission_item.land_precision); + } mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); @@ -421,7 +429,7 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() { _global_pos_sub.update(); _rtl_time_estimator.update(); - + _rtl_time_estimator.setVehicleType(_vehicle_status_sub.get().vehicle_type); _rtl_time_estimator.reset(); RTLState start_state_for_estimate; @@ -455,8 +463,13 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() matrix::Vector2f direction{}; get_vector_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon, &direction(0), &direction(1)); - _rtl_time_estimator.addDistance(get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - land_approach.lat, land_approach.lon), direction, 0.f); + float move_to_land_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_approach.lat, land_approach.lon)}; + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + move_to_land_dist = max(0.f, move_to_land_dist - land_approach.loiter_radius_m); + } + + _rtl_time_estimator.addDistance(move_to_land_dist, direction, 0.f); } // FALLTHROUGH @@ -527,7 +540,11 @@ rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() initial_altitude = loiter_altitude; } - _rtl_time_estimator.addDescendMCLand(_destination.alt - initial_altitude); + if (_vehicle_status_sub.get().is_vtol) { + _rtl_time_estimator.setVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + } + + _rtl_time_estimator.addVertDistance(_destination.alt - initial_altitude); } break; @@ -560,7 +577,6 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con if (!PX4_ISFINITE(land_approach.lat) || !PX4_ISFINITE(land_approach.lon)) { sanitized_land_approach.lat = _destination.lat; sanitized_land_approach.lon = _destination.lon; - } if (!PX4_ISFINITE(land_approach.height_m)) { diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index aed7cf3e4c..0a584234ec 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -91,6 +91,13 @@ public: */ void on_active() override; + /** + * @brief on inactive + * Poll required topics also when incative for rtl time estimate. + * + */ + void on_inactive() override; + /** * @brief Calculate the estimated time needed to return to launch. * diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index bc6df21a49..6b7f2f649a 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -215,8 +215,14 @@ void RtlDirectMissionLand::setActiveMissionItems() // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && MissionBase::isLanding() - && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT; + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (fw_on_mission_landing || mc_landing_after_transition) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; } } @@ -239,6 +245,7 @@ void RtlDirectMissionLand::setActiveMissionItems() rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() { _rtl_time_estimator.update(); + _rtl_time_estimator.setVehicleType(_vehicle_status_sub.get().vehicle_type); _rtl_time_estimator.reset(); if (_mission.count > 0 && hasMissionLandStart()) { @@ -247,7 +254,7 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() if (isActive()) { start_item_index = math::max(_mission.current_seq, _mission.land_start_index); - is_in_climbing_submode = _needs_climbing; + is_in_climbing_submode = _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB; } else { start_item_index = _mission.land_start_index; @@ -305,7 +312,12 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.lat, + next_position_mission_item.lon); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + hor_dist = math::max(0.f, hor_dist - next_position_mission_item.loiter_radius); + } _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); @@ -321,7 +333,12 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + hor_position_at_calculation_point(1), next_position_mission_item.lat, + next_position_mission_item.lon); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + hor_dist = math::max(0.f, hor_dist - next_position_mission_item.loiter_radius); + } _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); @@ -339,14 +356,13 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); - float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + const float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); // For fixed wing, add diagonal line if ((_vehicle_status_sub.get().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && (!_vehicle_status_sub.get().is_vtol)) { - _rtl_time_estimator.addDistance(hor_dist, direction, get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); @@ -354,8 +370,12 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() // For VTOL, Rotary, go there horizontally first, then land _rtl_time_estimator.addDistance(hor_dist, direction, 0.f); - _rtl_time_estimator.addDescendMCLand(get_absolute_altitude_for_item(next_position_mission_item) - - altitude_at_calculation_point); + if (_vehicle_status_sub.get().is_vtol) { + _rtl_time_estimator.setVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + } + + _rtl_time_estimator.addVertDistance(get_absolute_altitude_for_item(next_position_mission_item) - + altitude_at_calculation_point); } break; @@ -367,8 +387,8 @@ rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() get_vector_to_next_waypoint(hor_position_at_calculation_point(0), hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon, &direction(0), &direction(1)); - float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), - hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); + const float hor_dist = get_distance_to_next_waypoint(hor_position_at_calculation_point(0), + hor_position_at_calculation_point(1), next_position_mission_item.lat, next_position_mission_item.lon); _rtl_time_estimator.addDistance(hor_dist, direction, get_absolute_altitude_for_item(next_position_mission_item) - altitude_at_calculation_point); diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index d3e07a36dd..ccfc91cfde 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -46,10 +46,10 @@ static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5; -RtlMissionFast::RtlMissionFast(Navigator *navigator) : +RtlMissionFast::RtlMissionFast(Navigator *navigator, mission_s mission) : RtlBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) { - + _mission = mission; } void RtlMissionFast::on_inactive() diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index c782a471bd..d1231022b6 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -52,7 +52,7 @@ class Navigator; class RtlMissionFast : public RtlBase { public: - RtlMissionFast(Navigator *navigator); + RtlMissionFast(Navigator *navigator, mission_s mission); ~RtlMissionFast() = default; void on_activation() override; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index d78f6c7fa6..b50d49e50a 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -46,10 +46,10 @@ static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5; -RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : +RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator, mission_s mission) : RtlBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) { - + _mission = mission; } void RtlMissionFastReverse::on_inactive() @@ -155,6 +155,13 @@ void RtlMissionFastReverse::setActiveMissionItems() } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (mc_landing_after_transition) { + pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; + } } issue_command(_mission_item); @@ -262,6 +269,13 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _mission_item.altitude = _home_pos_sub.get().alt; _mission_item.altitude_is_relative = false; _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision > 0) { + startPrecLand(_mission_item.land_precision); + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + } } } } diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 168d20c2e3..0351f8dbf8 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -52,7 +52,7 @@ class Navigator; class RtlMissionFastReverse : public RtlBase { public: - RtlMissionFastReverse(Navigator *navigator); + RtlMissionFastReverse(Navigator *navigator, mission_s mission); ~RtlMissionFastReverse() = default; void on_activation() override; @@ -74,4 +74,8 @@ private: bool _in_landing_phase{false}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ + DEFINE_PARAMETERS_CUSTOM_PARENT( + RtlBase, + (ParamInt) _param_rtl_pld_md + ) }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 6fd734cc8e..951993a724 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -139,6 +139,7 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 45); * RTL precision land mode * * Use precision landing when doing an RTL landing phase. + * This setting does not apply for RTL destinations planned as part of a mission. * * @value 0 No precision landing * @value 1 Opportunistic precision landing diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index f9a552ef58..503a0279f9 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -118,7 +118,7 @@ VtolTakeoff::on_active() _mission_item.time_inside = 1.f; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get(); + _mission_item.altitude = _takeoff_alt_msl + _param_loiter_alt.get(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.lat = _loiter_location(0); @@ -170,6 +170,8 @@ VtolTakeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, _transition_alt_amsl); + _takeoff_alt_msl = _navigator->get_global_position()->alt; + _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/vtol_takeoff.h b/src/modules/navigator/vtol_takeoff.h index a353d89ce8..3ba32ce7df 100644 --- a/src/modules/navigator/vtol_takeoff.h +++ b/src/modules/navigator/vtol_takeoff.h @@ -70,6 +70,7 @@ private: } _takeoff_state; float _transition_alt_amsl{0.f}; // absolute altitude at which vehicle will transition to forward flight + float _takeoff_alt_msl{0.f}; matrix::Vector2d _loiter_location; float _loiter_height{0}; diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 91f884eb9a..8ada8e9561 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2025 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 @@ -1592,8 +1592,6 @@ PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0); /** * AUX1 Passthrough RC channel * - * Default function: Camera pitch - * * @min 0 * @max 18 * @group Radio Calibration @@ -1622,8 +1620,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /** * AUX2 Passthrough RC channel * - * Default function: Camera roll - * * @min 0 * @max 18 * @group Radio Calibration @@ -1652,8 +1648,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /** * AUX3 Passthrough RC channel * - * Default function: Camera azimuth / yaw - * * @min 0 * @max 18 * @group Radio Calibration @@ -1762,6 +1756,35 @@ PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); * @value 18 Channel 18 */ PARAM_DEFINE_INT32(RC_MAP_AUX6, 0); + +/** + * Payload Power Switch RC channel + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 Channel 1 + * @value 2 Channel 2 + * @value 3 Channel 3 + * @value 4 Channel 4 + * @value 5 Channel 5 + * @value 6 Channel 6 + * @value 7 Channel 7 + * @value 8 Channel 8 + * @value 9 Channel 9 + * @value 10 Channel 10 + * @value 11 Channel 11 + * @value 12 Channel 12 + * @value 13 Channel 13 + * @value 14 Channel 14 + * @value 15 Channel 15 + * @value 16 Channel 16 + * @value 17 Channel 17 + * @value 18 Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_PAY_SW, 0); + /** * PARAM1 tuning channel * @@ -2029,6 +2052,22 @@ PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.75f); */ PARAM_DEFINE_FLOAT(RC_ENG_MOT_TH, 0.75f); +/** + * Threshold for selecting payload power switch + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel) _param_rc_fails_thr, + (ParamInt) _param_rc_map_payload_sw, + (ParamFloat) _param_rc_loiter_th, (ParamFloat) _param_rc_offb_th, (ParamFloat) _param_rc_killswitch_th, @@ -238,6 +240,7 @@ protected: (ParamFloat) _param_rc_gear_th, (ParamFloat) _param_rc_return_th, (ParamFloat) _param_rc_eng_mot_th, + (ParamFloat) _param_rc_payload_th, (ParamInt) _param_rc_chan_cnt ) diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index b12e899cc0..a49c005bb5 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -418,7 +418,7 @@ string Replay::getOrbFields(const orb_metadata *meta) return format; } -bool +Replay::ReadAndAndAddSubResult Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) { _read_buffer.reserve(msg_size + 1); @@ -428,11 +428,11 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) message[msg_size] = 0; if (!file) { - return false; + return ReadAndAndAddSubResult::kFailure; } if (file.tellg() <= _subscription_file_pos) { //already read this subscription - return true; + return ReadAndAndAddSubResult::kIgnoringMsg; } _subscription_file_pos = file.tellg(); @@ -444,7 +444,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) if (!orb_meta) { PX4_WARN("Topic %s not found internally. Will ignore it", topic_name.c_str()); - return true; + return ReadAndAndAddSubResult::kIgnoringMsg; } CompatBase *compat = nullptr; @@ -520,7 +520,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) } } - return true; // not a fatal error + return ReadAndAndAddSubResult::kIgnoringMsg; // not a fatal error } } @@ -535,13 +535,13 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) if (!timestamp_found) { delete subscription; - return true; + return ReadAndAndAddSubResult::kIgnoringMsg; } if (field_size != 8) { PX4_ERR("Unsupported timestamp with size %i, ignoring the topic %s", field_size, orb_meta->o_name); delete subscription; - return true; + return ReadAndAndAddSubResult::kIgnoringMsg; } //find first data message (and the timestamp) @@ -550,7 +550,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) if (!nextDataMessage(file, *subscription, msg_id)) { delete subscription; - return false; + return ReadAndAndAddSubResult::kFailure; } file.seekg(cur_pos); @@ -558,7 +558,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) if (!subscription->orb_meta) { //no message found. This is not a fatal error delete subscription; - return true; + return ReadAndAndAddSubResult::kIgnoringMsg; } PX4_DEBUG("adding subscription for %s (msg_id %i)", subscription->orb_meta->o_name, msg_id); @@ -572,7 +572,7 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) onSubscriptionAdded(*_subscriptions[msg_id], msg_id); - return true; + return ReadAndAndAddSubResult::kSuccess; } bool @@ -908,12 +908,18 @@ Replay::run() replay_file.seekg(_data_section_start); //we know the next message must be an ADD_LOGGED_MSG - replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + ReadAndAndAddSubResult res; - if (!readAndAddSubscription(replay_file, message_header.msg_size)) { - PX4_ERR("Failed to read subscription"); - return; - } + do { + replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); + res = readAndAddSubscription(replay_file, message_header.msg_size); + + if (res == ReadAndAndAddSubResult::kFailure) { + PX4_ERR("Failed to read subscription"); + return; + } + + } while (res != ReadAndAndAddSubResult::kSuccess); const uint64_t timestamp_offset = getTimestampOffset(); uint32_t nr_published_messages = 0; diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 75ee3c1095..eebf82fb47 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -262,7 +262,9 @@ private: ///file parsing methods. They return false, when further parsing should be aborted. bool readFormat(std::ifstream &file, uint16_t msg_size); - bool readAndAddSubscription(std::ifstream &file, uint16_t msg_size); + + enum class ReadAndAndAddSubResult : uint8_t { kSuccess, kIgnoringMsg, kFailure }; + ReadAndAndAddSubResult readAndAddSubscription(std::ifstream &file, uint16_t msg_size); bool readFlagBits(std::ifstream &file, uint16_t msg_size); /** diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 1add36489d..e03072b59e 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,6 +39,7 @@ // for ekf2 replay #include +#include #include #include #include @@ -91,6 +92,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(airspeed)) { _airspeed_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(airspeed_validated)) { + _airspeed_validated_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; @@ -108,6 +112,15 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(aux_global_position)) { _aux_global_position_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_local_position_groundtruth)) { + _vehicle_local_position_groundtruth_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_attitude_groundtruth)) { + _vehicle_attitude_groundtruth_msg_id = msg_id; + + } else if (sub.orb_meta == ORB_ID(vehicle_global_position_groundtruth)) { + _vehicle_global_position_groundtruth_msg_id = msg_id; } // the main loop should only handle publication of the following topics, the sensor topics are @@ -129,12 +142,16 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs }; handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); + handle_sensor_publication(ekf2_timestamps.airspeed_validated_timestamp_rel, _airspeed_validated_msg_id); handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_magnetometer_timestamp_rel, _vehicle_magnetometer_msg_id); handle_sensor_publication(ekf2_timestamps.visual_odometry_timestamp_rel, _vehicle_visual_odometry_msg_id); handle_sensor_publication(0, _aux_global_position_msg_id); + handle_sensor_publication(0, _vehicle_local_position_groundtruth_msg_id); + handle_sensor_publication(0, _vehicle_global_position_groundtruth_msg_id); + handle_sensor_publication(0, _vehicle_attitude_groundtruth_msg_id); // sensor_combined: publish last because ekf2 is polling on this if (!findTimestampAndPublish(ekf2_timestamps.timestamp / 100, _sensor_combined_msg_id, replay_file)) { @@ -213,6 +230,7 @@ ReplayEkf2::onExitMainLoop() PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):"); print_sensor_statistics(_airspeed_msg_id, "airspeed"); + print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated"); print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index bb7f8d1359..519b4242d2 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -82,6 +82,7 @@ private: static constexpr uint16_t msg_id_invalid = 0xffff; uint16_t _airspeed_msg_id = msg_id_invalid; + uint16_t _airspeed_validated_msg_id = msg_id_invalid; uint16_t _distance_sensor_msg_id = msg_id_invalid; uint16_t _optical_flow_msg_id = msg_id_invalid; uint16_t _sensor_combined_msg_id = msg_id_invalid; @@ -89,6 +90,9 @@ private: uint16_t _vehicle_magnetometer_msg_id = msg_id_invalid; uint16_t _vehicle_visual_odometry_msg_id = msg_id_invalid; uint16_t _aux_global_position_msg_id = msg_id_invalid; + uint16_t _vehicle_local_position_groundtruth_msg_id = msg_id_invalid; + uint16_t _vehicle_global_position_groundtruth_msg_id = msg_id_invalid; + uint16_t _vehicle_attitude_groundtruth_msg_id = msg_id_invalid; }; } //namespace px4 diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp new file mode 100644 index 0000000000..1af95cbc1a --- /dev/null +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "AckermannAttControl.hpp" + +using namespace time_literals; + +AckermannAttControl::AckermannAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void AckermannAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(0.f); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void AckermannAttControl::updateAttControl() +{ + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate (actuator_motors.control[0], -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + + if (_vehicle_control_mode.flag_control_manual_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void AckermannAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } +} + +void AckermannAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + // Calculate yaw rate limit for slew rate + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) / + _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate); + + float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool AckermannAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp new file mode 100644 index 0000000000..2acf8de524 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// PX4 includes +#include + +// Library includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann attitude control. + */ +class AckermannAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannAttControl. + * @param parent The parent ModuleParams object. + */ + AckermannAttControl(ModuleParams *parent); + ~AckermannAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + float _vehicle_yaw{0.f}; + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_ro_max_thr_speed].*/ + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/lib/avoidance/CMakeLists.txt b/src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt similarity index 86% rename from src/lib/avoidance/CMakeLists.txt rename to src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt index c1da2a76dc..127927f09e 100644 --- a/src/lib/avoidance/CMakeLists.txt +++ b/src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(avoidance - ObstacleAvoidance.cpp +px4_add_library(AckermannAttControl + AckermannAttControl.cpp ) -target_link_libraries(avoidance PUBLIC hysteresis bezier) -px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance) +target_link_libraries(AckermannAttControl PUBLIC PID) +target_include_directories(AckermannAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp new file mode 100644 index 0000000000..e9938b5bc8 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp @@ -0,0 +1,418 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "AckermannPosVelControl.hpp" + +using namespace time_literals; + +AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void AckermannPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } + + if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); + } +} + +void AckermannPosVelControl::updatePosVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void AckermannPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void AckermannPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Position Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void AckermannPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_vehicle_speed_body_x) * yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void AckermannPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void AckermannPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void AckermannPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2)); + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + if (_mission_finished) { + _speed_body_x_setpoint = 0.f; + + } else { // Regular guidance algorithm + + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp, + _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state, + _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } +} + +void AckermannPosVelControl::updateAutoSubscriptions() +{ + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypointsAndAcceptanceRadius(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _prev_waypoint_transition_angle = _waypoint_transition_angle; + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Update acceptance radius + _prev_acceptance_radius = _acceptance_radius; + + if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { + _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), + _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get()); + + } else { + _acceptance_radius = _param_nav_acc_rad.get(); + } + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); +} + +float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transition_angle, + const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) +{ + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + float acceptance_radius = default_acceptance_radius; + const float theta = waypoint_transition_angle / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = _timestamp; + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min, + const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state, + const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) +{ + // Catch improper values + if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { + return cruising_speed; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); + const float cornering_speed = _max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + const float cornering_speed = _max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } + + // Straight line speed + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float straight_line_speed{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + + } else { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + float cornering_speed = _max_yaw_rate * turning_circle; + cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); + straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::min(straight_line_speed, cruising_speed); + + } else { + return cruising_speed; + } + +} + +bool AckermannPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp new file mode 100644 index 0000000000..a993be28a5 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// PX4 includes +#include +#include + +// Library includes +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for ackermann position control. + */ +class AckermannPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannPosVelControl. + * @param parent The parent ModuleParams object. + */ + AckermannPosVelControl(ModuleParams *parent); + ~AckermannPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Update global/NED waypoint coordinates and acceptance radius. + */ + void updateWaypointsAndAcceptanceRadius(); + + /** + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. + */ + float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius, + float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + + /** + * @brief Calculate the speed setpoint. During cornering the speed is restricted based on the radius of the corner. + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next corner with the + * desired cornering speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param miss_speed_min Minimum speed setpoint [m/s]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state, + float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + bool _prev_param_check_passed{true}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _acceptance_radius{0.5f}; + float _prev_acceptance_radius{0.5f}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_gain, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt new file mode 100644 index 0000000000..caa8fb9d5e --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(AckermannPosVelControl + AckermannPosVelControl.cpp +) + +target_link_libraries(AckermannPosVelControl PUBLIC PID) +target_include_directories(AckermannPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp new file mode 100644 index 0000000000..3ec37eb844 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "AckermannRateControl.hpp" + +using namespace time_literals; + +AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void AckermannRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); +} + +void AckermannRateControl::updateRateControl() +{ + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate(actuator_motors.control[0], -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + _estimated_speed_body_x = fabsf(_estimated_speed_body_x) > _param_ro_speed_th.get() ? _estimated_speed_body_x : 0.f; + } + + if (_vehicle_control_mode.flag_control_manual_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void AckermannRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * math::interpolate + (manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } +} + +void AckermannRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float steering_setpoint{0.f}; + + if (fabsf(_estimated_speed_body_x) > FLT_EPSILON) { + // Set up feasible yaw rate setpoint + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) / + _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate); + float constrained_yaw_rate = math::constrain(_rover_rate_setpoint.yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit); + + if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate - + _vehicle_yaw_rate)) { + _yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate); + } + + _yaw_rate_setpoint.update(constrained_yaw_rate, _dt); + + } else { + _yaw_rate_setpoint.setForcedValue(constrained_yaw_rate); + } + + // Feed forward + steering_setpoint = atanf(_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed_body_x); + + // Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability) + if (_estimated_speed_body_x > FLT_EPSILON) { + _pid_yaw_rate.setSetpoint(_yaw_rate_setpoint.getState()); + steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, _dt); + } + + } else { + _pid_yaw_rate.resetIntegral(); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_steering_angle = math::interpolate(steering_setpoint, + -_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool AckermannRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error, + "Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get()); + } + + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error, + "Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get()); + } + + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error, + "Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get()); + } + + } + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp new file mode 100644 index 0000000000..b7e194d70c --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann rate control. + */ +class AckermannRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannRateControl. + * @param parent The parent ModuleParams object. + */ + AckermannRateControl(ModuleParams *parent); + ~AckermannRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_ro_max_thr_speed].*/ + float _max_yaw_rate{0.f}; + float _vehicle_yaw_rate{0.f}; + hrt_abstime _timestamp{0}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_speed_th + ) +}; diff --git a/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt new file mode 100644 index 0000000000..aa80c0734b --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(AckermannRateControl + AckermannRateControl.cpp +) + +target_link_libraries(AckermannRateControl PUBLIC PID) +target_include_directories(AckermannRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index f02a91c0b6..93269c29b9 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,7 +31,9 @@ # ############################################################################ -add_subdirectory(RoverAckermannGuidance) +add_subdirectory(AckermannRateControl) +add_subdirectory(AckermannAttControl) +add_subdirectory(AckermannPosVelControl) px4_add_module( MODULE modules__rover_ackermann @@ -40,10 +42,13 @@ px4_add_module( RoverAckermann.cpp RoverAckermann.hpp DEPENDS - RoverAckermannGuidance + AckermannRateControl + AckermannAttControl + AckermannPosVelControl px4_work_queue - SlewRate - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) + +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/rover_control/module.yaml) diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index 3ba1350659..bfcff4a895 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of ackermann drive rovers + Enable support for ackermann rovers diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 5409e40d84..c76bbf9ebb 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -34,13 +34,13 @@ #include "RoverAckermann.hpp" using namespace time_literals; -using namespace matrix; RoverAckermann::RoverAckermann() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _rover_ackermann_status_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); } @@ -54,142 +54,111 @@ void RoverAckermann::updateParams() { ModuleParams::updateParams(); - // Update slew rates - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) { - _throttle_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get()); + if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get()); } - if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) / - _param_ra_max_steer_angle.get()); + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); } } void RoverAckermann::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Generate motor setpoints - if (_armed) { - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _motor_setpoint.steering = manual_control_setpoint.roll; - _motor_setpoint.throttle = manual_control_setpoint.throttle; - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state); - break; - - default: // Unimplemented nav states will stop the rover - _motor_setpoint.steering = 0.f; - _motor_setpoint.throttle = 0.f; - _throttle_with_accel_limit.setForcedValue(0.f); - _steering_with_rate_limit.setForcedValue(0.f); - break; - } - - } else { // Reset on disarm - _motor_setpoint.steering = 0.f; - _motor_setpoint.throttle = 0.f; - _throttle_with_accel_limit.setForcedValue(0.f); - _steering_with_rate_limit.setForcedValue(0.f); - } - - publishMotorSetpoints(applySlewRates(_motor_setpoint, dt)); -} - -void RoverAckermann::updateSubscriptions() { if (_parameter_update_sub.updated()) { updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == 2; + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + _ackermann_pos_vel_control.updatePosVelControl(); + _ackermann_att_control.updateAttControl(); + _ackermann_rate_control.updateRateControl(); + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - _actual_speed = rover_velocity.norm(); - } -} -motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt) -{ - // Sanitize actuator commands - if (!PX4_ISFINITE(motor_setpoint.steering)) { - motor_setpoint.steering = 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (!PX4_ISFINITE(motor_setpoint.throttle)) { - motor_setpoint.throttle = 0.f; - } + generateActuatorSetpoint(); - // Acceleration slew rate - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON - && fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) { - _throttle_with_accel_limit.update(motor_setpoint.throttle, dt); - - } else { - _throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle); - } - - // Steering slew rate - if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _steering_with_rate_limit.update(motor_setpoint.steering, dt); - - } else { - _steering_with_rate_limit.setForcedValue(motor_setpoint.steering); - } - - motor_setpoint_struct motor_setpoint_temp{}; - motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); - motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); - return motor_setpoint_temp; } -void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates) +void RoverAckermann::generateSteeringSetpoint() { - // Publish rover Ackermann status (logging) - rover_ackermann_status_s rover_ackermann_status{}; - rover_ackermann_status.timestamp = _timestamp; - rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; - rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; - rover_ackermann_status.actual_speed = _actual_speed; - _rover_ackermann_status_pub.publish(rover_ackermann_status); + manual_control_setpoint_s manual_control_setpoint{}; - // Publish to motor - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle; - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + } +} + +void RoverAckermann::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_motor_setpoint = actuator_motors.control[0]; + } + + if (_vehicle_control_mode.flag_armed) { + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (_actuator_servos_sub.updated()) { + actuator_servos_s actuator_servos{}; + _actuator_servos_sub.copy(&actuator_servos); + _current_servo_setpoint = actuator_servos.control[0]; + } + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf( + _rover_steering_setpoint.normalized_steering_angle - + _current_servo_setpoint)) { + _servo_setpoint.setForcedValue(_current_servo_setpoint); + } + + _servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt); + + } else { + _servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle); + } - // Publish to servo actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering; + actuator_servos.control[0] = _servo_setpoint.getState(); actuator_servos.timestamp = _timestamp; _actuator_servos_pub.publish(actuator_servos); } diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 3617b5b80b..635abff3f8 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -39,29 +39,28 @@ #include #include #include + +// Libraries +#include +#include #include // uORB includes +#include #include #include -#include -#include #include -#include #include #include -#include -#include - - -// Standard library includes -#include +#include +#include +#include +#include // Local includes -#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" -using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint; - -using namespace time_literals; +#include "AckermannRateControl/AckermannRateControl.hpp" +#include "AckermannAttControl/AckermannAttControl.hpp" +#include "AckermannPosVelControl/AckermannPosVelControl.hpp" class RoverAckermann : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -84,61 +83,65 @@ public: bool init(); - protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); /** - * @brief Apply slew rates to motor setpoints. - * @param motor_setpoint Normalized steering and throttle setpoints. - * @param dt Time since last update [s]. - * @return Motor setpoint with applied slew rates. + * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. */ - motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt); - - /** - * @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus. - * @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates. - */ - void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates); + void generateActuatorSetpoint(); // uORB subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; // uORB publications uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; - uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances - RoverAckermannGuidance _ackermann_guidance{this}; + AckermannRateControl _ackermann_rate_control{this}; + AckermannAttControl _ackermann_att_control{this}; + AckermannPosVelControl _ackermann_pos_vel_control{this}; // Variables - int _nav_state{0}; - motor_setpoint_struct _motor_setpoint; hrt_abstime _timestamp{0}; - float _actual_speed{0.f}; - SlewRate _steering_with_rate_limit{0.f}; - SlewRate _throttle_with_accel_limit{0.f}; - bool _armed{false}; + float _dt{0.f}; + float _current_servo_setpoint{0.f}; + float _current_motor_setpoint{0.f}; + + // Controllers + SlewRate _servo_setpoint{0.f}; + SlewRate _motor_setpoint{0.f}; // Parameters DEFINE_PARAMETERS( (ParamInt) _param_r_rev, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_accel, - (ParamFloat) _param_ra_max_steering_rate + (ParamFloat) _param_ra_str_rate_limit, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp deleted file mode 100644 index 48890323bb..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ /dev/null @@ -1,330 +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 "RoverAckermannGuidance.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - _rover_ackermann_guidance_status_pub.advertise(); - updateParams(); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void RoverAckermannGuidance::updateParams() -{ - ModuleParams::updateParams(); - pid_set_parameters(&_pid_throttle, - _param_ra_p_speed.get(), // Proportional gain - _param_ra_i_speed.get(), // Integral gain - 0, // Derivative gain - 1, // Integral limit - 1); // Output limit -} - -RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state) -{ - updateSubscriptions(); - - // Distances to waypoints - _distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _prev_wp(0), _prev_wp(1)); - _distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - _distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _next_wp(0), _next_wp(1)); - - // Catch return to launch - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = _distance_to_next_wp < _acceptance_radius; - } - - // Guidance logic - if (_mission_finished) { // Mission is finished - _desired_steering = 0.f; - _desired_speed = 0.f; - - } else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command - _desired_speed = 0.f; - - } else { // Regular guidance algorithm - - _desired_speed = calcDesiredSpeed(_wp_max_desired_vel, _param_ra_miss_vel_min.get(), - _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius, - _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state); - - _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - _param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get()); - - } - - // Calculate throttle setpoint - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - const float throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(), - dt); - - // Publish ackermann controller status (logging) - _rover_ackermann_guidance_status.timestamp = _timestamp; - _rover_ackermann_guidance_status.desired_speed = _desired_speed; - _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - - // Return motor setpoints - motor_setpoint motor_setpoint_temp; - motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint - motor_setpoint_temp.throttle = throttle; - return motor_setpoint_temp; -} - -void RoverAckermannGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - _actual_speed = rover_velocity.norm(); - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypointsAndAcceptanceRadius(); - } - - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - _vehicle_attitude_sub.copy(&vehicle_attitude); - matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } -} - -void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - _prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid - _curr_wp = Vector2d(0, 0); - _next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL - - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Update acceptance radius - _prev_acceptance_radius = _acceptance_radius; - - if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { - _acceptance_radius = updateAcceptanceRadius(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, _param_nav_acc_rad.get(), - _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get()); - - } else { - _acceptance_radius = _param_nav_acc_rad.get(); - } - - if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _wp_max_desired_vel = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get()); - - } else { - _wp_max_desired_vel = _param_ra_miss_vel_def.get(); - } -} - -float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &next_wp_ned, const float default_acceptance_radius, const float acceptance_radius_gain, - const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) -{ - // Setup variables - const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned; - const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned; - float acceptance_radius = default_acceptance_radius; - - // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment - if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) { - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - const float theta = acosf(cosin) / 2.f; - const float min_turning_radius = wheel_base / sinf(max_steer_angle); - const float acceptance_radius_temp = min_turning_radius / tanf(theta); - const float acceptance_radius_temp_scaled = acceptance_radius_gain * - acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects - acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, - acceptance_radius_max); - } - - // Publish updated acceptance radius - position_controller_status_s pos_ctrl_status{}; - pos_ctrl_status.acceptance_radius = acceptance_radius; - pos_ctrl_status.timestamp = hrt_absolute_time(); - _position_controller_status_pub.publish(pos_ctrl_status); - return acceptance_radius; -} - -float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min, - const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, - const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state) -{ - // Catch improper values - if (miss_vel_min < 0.f || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) { - return miss_vel_def; - } - - // Cornering slow down effect - if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { - const float cornering_speed = miss_vel_gain / prev_acc_rad; - return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); - - } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { - const float cornering_speed = miss_vel_gain / acc_rad; - return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); - - } - - // Straight line speed - if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { - float max_velocity{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_accel, distance_to_curr_wp, 0.f); - - } else { - const float cornering_speed = miss_vel_gain / acc_rad; - max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_accel, distance_to_curr_wp - acc_rad, cornering_speed); - } - - return math::constrain(max_velocity, miss_vel_min, miss_vel_def); - - } else { - return miss_vel_def; - } - -} - -float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, - const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, - const float vehicle_yaw, const float max_steering) -{ - const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, - desired_speed); - const float lookahead_distance = pure_pursuit.getLookaheadDistance(); - const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); - // For logging - _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; - _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); - - float desired_steering{0.f}; - - if (math::abs_t(heading_error) <= M_PI_2_F) { - desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); - - - } else { - desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - - sign(heading_error) * M_PI_2_F)) / lookahead_distance); - } - - return math::constrain(desired_steering, -max_steering, max_steering); - -} - -float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed, - const float actual_speed, const float max_speed, const float dt) -{ - float throttle = 0.f; - - if (desired_speed < FLT_EPSILON) { - pid_reset_integral(&pid_throttle); - - } else { - throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt); - } - - if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term - throttle += math::interpolate(desired_speed, 0.f, max_speed, 0.f, 1.f); - } - - return math::constrain(throttle, 0.f, 1.f); -} diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp deleted file mode 100644 index de5ef58780..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ /dev/null @@ -1,235 +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 - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard library includes -#include -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for ackermann drive guidance. - */ -class RoverAckermannGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverAckermannGuidance. - * @param parent The parent ModuleParams object. - */ - RoverAckermannGuidance(ModuleParams *parent); - ~RoverAckermannGuidance() = default; - - /** - * @brief Struct for steering and throttle setpoints. - * @param steering Steering setpoint. - * @param throttle Throttle setpoint. - */ - struct motor_setpoint { - float steering{0.f}; - float throttle{0.f}; - }; - - /** - * @brief Calculate motor setpoints based on the mission plan. - * @param nav_state Vehicle navigation state. - * @return Motor setpoints for throttle and steering. - */ - motor_setpoint computeGuidance(int nav_state); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions - */ - void updateSubscriptions(); - - /** - * @brief Update global/NED waypoint coordinates and acceptance radius. - */ - void updateWaypointsAndAcceptanceRadius(); - - /** - * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. - * @param curr_wp_ned Current waypoint in NED frame [m]. - * @param prev_wp_ned Previous waypoint in NED frame [m]. - * @param next_wp_ned Next waypoint in NED frame [m]. - * @param default_acceptance_radius Default acceptance radius for waypoints [m]. - * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. - * @param acceptance_radius_max Maximum value for the acceptance radius [m]. - * @param wheel_base Rover wheelbase [m]. - * @param max_steer_angle Rover maximum steer angle [rad]. - * @return Updated acceptance radius [m]. - */ - float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain, - float acceptance_radius_max, float wheel_base, float max_steer_angle); - - - /** - * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse - * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory - * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the - * maximum acceleration and jerk. - * @param miss_vel_def Default desired velocity for the rover during mission [m/s]. - * @param miss_vel_min Minimum desired velocity for the rover during mission [m/s]. - * @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-]. - * @param distance_to_prev_wp Distance to the previous waypoint [m]. - * @param distance_to_curr_wp Distance to the current waypoint [m]. - * @param acc_rad Acceptance radius of the current waypoint [m]. - * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. - * @param max_accel Maximum allowed acceleration for the rover [m/s^2]. - * @param max_jerk Maximum allowed jerk for the rover [m/s^3]. - * @param nav_state Current nav_state of the rover. - * @return Speed setpoint for the rover [m/s]. - */ - float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp, - float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state); - - /** - * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to - * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). - * @param pure_pursuit Pure pursuit class instance. - * @param curr_wp_ned Current waypoint in NED frame [m]. - * @param prev_wp_ned Previous waypoint in NED frame [m]. - * @param curr_pos_ned Current position of the vehicle in NED frame [m]. - * @param wheel_base Rover wheelbase [m]. - * @param desired_speed Desired speed for the rover [m/s]. - * @param vehicle_yaw Current yaw of the rover [rad]. - * @param max_steering Maximum steering angle of the rover [rad]. - * @return Steering setpoint for the rover [rad]. - */ - float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering); - - /** - * @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between - * the desired/actual speed and a feedforward term based on the full throttle speed. - * @param pid_throttle Reference to PID instance. - * @param desired_speed Reference speed for the rover [m/s]. - * @param actual_speed Actual speed of the rover [m/s]. - * @param max_speed Rover speed at full throttle [m/s]. - * @param dt Time interval since last update [s]. - * @return Normalized throttle setpoint [0, 1]. - */ - float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - - // uORB publications - uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; - uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; - - // Class instances - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates - PurePursuit _pure_pursuit{this}; // Pure pursuit library - - // Rover variables - float _desired_steering{0.f}; - float _vehicle_yaw{0.f}; - float _desired_speed{0.f}; - float _actual_speed{0.f}; - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - PID_t _pid_throttle; - hrt_abstime _timestamp{0}; - - // Waypoint variables - Vector2d _home_position{}; - Vector2f _curr_wp_ned{}; - Vector2f _prev_wp_ned{}; - Vector2f _next_wp_ned{}; - Vector2d _curr_wp{}; - Vector2d _prev_wp{}; - Vector2d _next_wp{}; - float _distance_to_prev_wp{0.f}; - float _distance_to_curr_wp{0.f}; - float _distance_to_next_wp{0.f}; - float _acceptance_radius{0.5f}; - float _prev_acceptance_radius{0.5f}; - float _wp_max_desired_vel{0.f}; - bool _mission_finished{false}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_acc_rad_max, - (ParamFloat) _param_ra_acc_rad_gain, - (ParamFloat) _param_ra_miss_vel_def, - (ParamFloat) _param_ra_miss_vel_min, - (ParamFloat) _param_ra_miss_vel_gain, - (ParamFloat) _param_ra_p_speed, - (ParamFloat) _param_ra_i_speed, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_jerk, - (ParamFloat) _param_ra_max_accel, - (ParamFloat) _param_nav_acc_rad - ) -}; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index 6571d0eeeb..383c9e73b2 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -6,26 +6,37 @@ parameters: RA_WHEEL_BASE: description: short: Wheel base - long: Distance from the front to the rear axle + long: Distance from the front to the rear axle. type: float unit: m - min: 0.001 + min: 0 max: 100 increment: 0.001 decimal: 3 - default: 0.5 + default: 0 RA_MAX_STR_ANG: description: short: Maximum steering angle - long: The maximum angle that the rover can steer type: float unit: rad - min: 0.1 + min: 0 max: 1.5708 increment: 0.01 decimal: 2 - default: 0.5236 + default: 0 + + RA_STR_RATE_LIM: + description: + short: Steering rate limit + long: Set to -1 to disable. + type: float + unit: deg/s + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 RA_ACC_RAD_MAX: description: @@ -41,7 +52,7 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 3 + default: -1 RA_ACC_RAD_GAIN: description: @@ -55,121 +66,4 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 - - RA_MISS_VEL_DEF: - description: - short: Default rover velocity during a mission - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 2 - - RA_MISS_VEL_MIN: - description: - short: Minimum rover velocity during a mission - long: | - The velocity off the rover is reduced based on the corner it has to take - to smooth the trajectory (Set to -1 to disable) - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 default: 1 - - RA_MISS_VEL_GAIN: - description: - short: Tuning parameter for the velocity reduction during cornering - long: | - The cornering speed is equal to the inverse of the acceptance radius - of the WP multiplied with this factor. - Lower value -> More velocity reduction during cornering. - type: float - min: 0.05 - max: 100 - increment: 0.01 - decimal: 2 - default: 5 - - RA_SPEED_P: - description: - short: Proportional gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RA_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RA_MAX_SPEED: - description: - short: Speed the rover drives at maximum throttle - long: | - This is used for the feed-forward term of the speed controller. - A value of -1 disables the feed-forward term in which case the - Integrator (RA_SPEED_I) becomes necessary to track speed setpoints. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_ACCEL: - description: - short: Maximum acceleration for the rover - long: | - This is used for the acceleration slew rate, the feed-forward term - for the speed controller during missions and the corner slow down effect. - Note: For the corner slow down effect RA_MAX_JERK, RA_MISS_VEL_GAIN and - RA_MISS_VEL_MIN also have to be set. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_JERK: - description: - short: Maximum jerk - long: | - Limit for forwards acc/deceleration change. - This is used for the corner slow down effect. - Note: RA_MAX_ACCEL, RA_MISS_VEL_GAIN and RA_MISS_VEL_MIN also have to be set - for this to be enabled. - type: float - unit: m/s^3 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_STR_RATE: - description: - short: Maximum steering rate for the rover - type: float - unit: deg/s - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 8805893160..8fafc30bab 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverDifferentialGuidance) -add_subdirectory(RoverDifferentialControl) +add_subdirectory(DifferentialRateControl) +add_subdirectory(DifferentialAttControl) +add_subdirectory(DifferentialPosVelControl) px4_add_module( MODULE modules__rover_differential @@ -41,10 +42,11 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS - RoverDifferentialGuidance - RoverDifferentialControl + DifferentialRateControl + DifferentialAttControl + DifferentialPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt new file mode 100644 index 0000000000..456b21489d --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(DifferentialAttControl + DifferentialAttControl.cpp +) + +target_link_libraries(DifferentialAttControl PUBLIC PID) +target_include_directories(DifferentialAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp new file mode 100644 index 0000000000..a36ed41ada --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "DifferentialAttControl.hpp" + +using namespace time_literals; + +DifferentialAttControl::DifferentialAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void DifferentialAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void DifferentialAttControl::updateAttControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void DifferentialAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude && !_offboard_control_mode.position + && !_offboard_control_mode.velocity; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool DifferentialAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp new file mode 100644 index 0000000000..e4198384c0 --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential attitude control. + */ +class DifferentialAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialAttControl. + * @param parent The parent ModuleParams object. + */ + DifferentialAttControl(ModuleParams *parent); + ~DifferentialAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * or trajectorySetpoint (Offboard attitude control). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt new file mode 100644 index 0000000000..a4a4795be7 --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(DifferentialPosVelControl + DifferentialPosVelControl.cpp +) + +target_link_libraries(DifferentialPosVelControl PUBLIC PID) +target_include_directories(DifferentialPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp new file mode 100644 index 0000000000..32532f4c56 --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "DifferentialPosVelControl.hpp" + +using namespace time_literals; + +DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void DifferentialPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void DifferentialPosVelControl::updatePosVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { + + const float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (fabsf(speed_body_x_setpoint_normalized) > 1.f - + fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + _speed_body_x_setpoint = sign(_speed_body_x_setpoint) * + math::interpolate(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void DifferentialPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void DifferentialPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void DifferentialPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void DifferentialPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void DifferentialPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + // State machine + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + fabsf(_vehicle_speed_body_x)); + const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); + + if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + // Calculate desired speed in body x direction + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get()); + + } break; + + case GuidanceState::SPOT_TURNING: + if (fabsf(_vehicle_speed_body_x) > 0.f) { + yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop + + } + + _speed_body_x_setpoint = 0.f; + break; + + case GuidanceState::STOPPED: + default: + yaw_setpoint = _vehicle_yaw; + _speed_body_x_setpoint = 0.f; + break; + + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); +} + +void DifferentialPosVelControl::updateAutoSubscriptions() +{ + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float trans_drv_trn, const float miss_spd_gain) +{ + float speed_body_x_setpoint = cruising_speed; + + if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) { + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f); + + } else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed); + +} + +bool DifferentialPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp new file mode 100644 index 0000000000..0ae8a24eae --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential position/velocity control. + */ +class DifferentialPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialPosVelControl. + * @param parent The parent ModuleParams object. + */ + DifferentialPosVelControl(ModuleParams *parent); + ~DifferentialPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain); + + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain, + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt new file mode 100644 index 0000000000..0182e89d30 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +px4_add_library(DifferentialRateControl + DifferentialRateControl.cpp +) + +target_link_libraries(DifferentialRateControl PUBLIC PID) +target_include_directories(DifferentialRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp new file mode 100644 index 0000000000..fe62328cd9 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "DifferentialRateControl.hpp" + +using namespace time_literals; + +DifferentialRateControl::DifferentialRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void DifferentialRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void DifferentialRateControl::updateRateControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void DifferentialRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.roll, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.position + && !_offboard_control_mode.velocity && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void DifferentialRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rd_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool DifferentialRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), + _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp new file mode 100644 index 0000000000..5b9bd6d632 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential rate control. + */ +class DifferentialRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialRateControl. + * @param parent The parent ModuleParams object. + */ + DifferentialRateControl(ModuleParams *parent); + ~DifferentialRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) + * or trajectorySetpoint (Offboard rate control). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_thr_yaw_r, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig index 840e2cdbf9..ed62f619b7 100644 --- a/src/modules/rover_differential/Kconfig +++ b/src/modules/rover_differential/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_DIFFERENTIAL bool "rover_differential" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of differential rovers + Enable support for differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index a7a9e82d02..5ad4439597 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -33,12 +33,15 @@ #include "RoverDifferential.hpp" +using namespace time_literals; + RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_differential_setpoint_pub.advertise(); } bool RoverDifferential::init() @@ -50,207 +53,103 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverDifferential::Run() { - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude and velocity setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_setpoint = NAN; - rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); - rover_differential_setpoint.yaw_rate_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) - if (!_yaw_ctl) { - _stab_desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Course control if the yaw rate input is zero (keep driving on a straight line) - if (!_yaw_ctl) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _yaw_ctl = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( - rover_differential_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, - _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); - rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? - yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); - -} - -void RoverDifferential::updateSubscriptions() -{ - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { // Reset on mode change - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } + _differential_pos_vel_control.updatePosVelControl(); + _differential_att_control.updateAttControl(); + _differential_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - _vehicle_attitude_sub.copy(&vehicle_attitude); - _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + if (_vehicle_control_mode.flag_armed) { + generateActuatorSetpoint(); + } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; +} + +void RoverDifferential::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } +void RoverDifferential::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} + +Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle_body_x -= sign(throttle_body_x) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle_body_x - speed_diff_normalized, + throttle_body_x + speed_diff_normalized); +} + int RoverDifferential::task_spawn(int argc, char *argv[]) { RoverDifferential *instance = new RoverDifferential(); @@ -276,7 +175,7 @@ int RoverDifferential::task_spawn(int argc, char *argv[]) int RoverDifferential::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverDifferential::print_usage(const char *reason) @@ -288,7 +187,7 @@ int RoverDifferential::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Differential controller. +Rover differential module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 8d83519410..9c943627b0 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -39,40 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" -#include "RoverDifferentialControl/RoverDifferentialControl.hpp" - -using namespace time_literals; - -// Constants -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value -static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "DifferentialRateControl/DifferentialRateControl.hpp" +#include "DifferentialAttControl/DifferentialAttControl.hpp" +#include "DifferentialPosVelControl/DifferentialPosVelControl.hpp" class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverDifferential + */ RoverDifferential(); ~RoverDifferential() override = default; @@ -88,50 +82,66 @@ public: bool init(); protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; - // uORB Publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverDifferentialGuidance _rover_differential_guidance{this}; - RoverDifferentialControl _rover_differential_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + DifferentialRateControl _differential_rate_control{this}; + DifferentialAttControl _differential_att_control{this}; + DifferentialPosVelControl _differential_pos_vel_control{this}; // Variables - Vector2f _curr_pos_ned{}; - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - int _nav_state{0}; - bool _armed{false}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode - float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode - Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode - Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_man_yaw_scale, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp deleted file mode 100644 index 7ec57fb176..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ /dev/null @@ -1,174 +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 "RoverDifferentialControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_differential_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void RoverDifferentialControl::updateParams() -{ - ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - pid_set_parameters(&_pid_yaw_rate, - _param_rd_yaw_rate_p.get(), // Proportional gain - _param_rd_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_throttle, - _param_rd_p_gain_speed.get(), // Proportional gain - _param_rd_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_yaw, - _param_rd_p_gain_yaw.get(), // Proportional gain - _param_rd_i_gain_yaw.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit -} - -void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update differential setpoint - _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); - - // Closed loop yaw control (Overrides yaw rate setpoint) - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { - float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw); - _rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward - const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get() / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_thr_yaw_r.get(), - _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); - } - - speed_diff_normalized = math::constrain(speed_diff_normalized + - pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt), - -1.f, 1.f); // Feedback - - } else { // Use normalized setpoint - speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ? - math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f; - } - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), - -1.f, 1.f); - } - - forward_speed_normalized = math::constrain(forward_speed_normalized + pid_calculate(&_pid_throttle, - _rover_differential_setpoint.forward_speed_setpoint, - vehicle_forward_speed, 0, - dt), -1.f, 1.f); // Feedback - - } else { // Use normalized setpoint - forward_speed_normalized = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ? - math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f; - } - - // Publish rover differential status (logging) - rover_differential_status_s rover_differential_status{}; - rover_differential_status.timestamp = _timestamp; - rover_differential_status.actual_speed = vehicle_forward_speed; - rover_differential_status.actual_yaw = vehicle_yaw; - rover_differential_status.desired_yaw_rate = _rover_differential_setpoint.yaw_rate_setpoint; - rover_differential_status.actual_yaw_rate = vehicle_yaw_rate; - rover_differential_status.forward_speed_normalized = forward_speed_normalized; - rover_differential_status.speed_diff_normalized = speed_diff_normalized; - rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - rover_differential_status.pid_throttle_integral = _pid_throttle.integral; - rover_differential_status.pid_yaw_integral = _pid_yaw.integral; - _rover_differential_status_pub.publish(rover_differential_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - -} - -matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, - const float speed_diff_normalized) -{ - float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - forward_speed_normalized -= sign(forward_speed_normalized) * excess; - } - - // Calculate the left and right wheel speeds - return Vector2f(forward_speed_normalized - speed_diff_normalized, - forward_speed_normalized + speed_diff_normalized); -} - -void RoverDifferentialControl::resetControllers() -{ - pid_reset_integral(&_pid_throttle); - pid_reset_integral(&_pid_yaw_rate); - pid_reset_integral(&_pid_yaw); -} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp deleted file mode 100644 index a5eb08135f..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ /dev/null @@ -1,127 +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 - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include - - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialControl. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialControl(ModuleParams *parent); - ~RoverDifferentialControl() = default; - - /** - * @brief Compute motor commands based on setpoints - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * - * @param forward_speed_normalized Normalized forward speed [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. - */ - matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized); - - // uORB subscriptions - uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; - - // Variables - rover_differential_setpoint_s _rover_differential_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - - // Controllers - PID_t _pid_throttle; // The PID controller for the closed loop speed control - PID_t _pid_yaw; // The PID controller for the closed loop yaw control - PID_t _pid_yaw_rate; // The PID controller for the closed loop yaw rate control - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_spd, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_yaw_rate_p, - (ParamFloat) _param_rd_yaw_rate_i, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, - (ParamFloat) _param_rd_p_gain_yaw, - (ParamFloat) _param_rd_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp deleted file mode 100644 index 848a7f513a..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ /dev/null @@ -1,224 +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 "RoverDifferentialGuidance.hpp" - -#include - -using namespace matrix; - -RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_forward_speed = _param_rd_miss_spd_def.get(); - _rover_differential_guidance_status_pub.advertise(); -} - -void RoverDifferentialGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_speed, const int nav_state) -{ - updateSubscriptions(); - - // Catch return to launch - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - math::max(forward_speed, 0.f)); - const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); - - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { - if (_currentState == GuidanceState::STOPPED) { - _currentState = GuidanceState::DRIVING; - } - - if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - _currentState = GuidanceState::SPOT_TURNING; - - } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _currentState = GuidanceState::DRIVING; - } - - } else { // Mission finished or delay command - _currentState = GuidanceState::STOPPED; - } - - // Guidance logic - float desired_forward_speed{0.f}; - - switch (_currentState) { - case GuidanceState::DRIVING: { - // Calculate desired forward speed - desired_forward_speed = _max_forward_speed; - - if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_accel.get(), distance_to_curr_wp, 0.0f); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); - } - } - - } break; - - case GuidanceState::SPOT_TURNING: - if (forward_speed > TURN_MAX_VELOCITY) { - desired_yaw = vehicle_yaw; // Wait for the rover to stop - - } - - break; - - case GuidanceState::STOPPED: - default: - desired_yaw = vehicle_yaw; - break; - - } - - // Publish differential guidance status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - rover_differential_guidance_status_s rover_differential_guidance_status{}; - rover_differential_guidance_status.timestamp = timestamp; - rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - rover_differential_guidance_status.state_machine = (uint8_t) _currentState; - _rover_differential_guidance_status_pub.publish(rover_differential_guidance_status); - - // Publish setpoints - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = desired_yaw; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); -} - -void RoverDifferentialGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } -} - -void RoverDifferentialGuidance::updateWaypoints() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } else { - _curr_wp = Vector2d(0, 0); - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } else { - _prev_wp = _curr_pos; - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } else { - _next_wp = _home_position; - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); - - } else { - _max_forward_speed = _param_rd_miss_spd_def.get(); - } -} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp deleted file mode 100644 index 4b3815247e..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ /dev/null @@ -1,152 +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 - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - SPOT_TURNING, // The vehicle is currently turning on the spot. - DRIVING, // The vehicle is currently driving. - STOPPED // The vehicle is stopped. -}; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialGuidance. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialGuidance(ModuleParams *parent); - ~RoverDifferentialGuidance() = default; - - /** - * @brief Compute and publish attitude and velocity setpoints based on the mission plan. - * @param vehicle_yaw The yaw of the vehicle [rad]. - * @param forward_speed The forward speed of the vehicle [m/s]. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float vehicle_yaw, float forward_speed, int nav_state); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions. - */ - void updateSubscriptions(); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - - // uORB publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; - uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // Waypoints - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - Vector2d _prev_wp{}; - Vector2f _prev_wp_ned{}; - Vector2d _curr_wp{}; - Vector2f _curr_wp_ned{}; - Vector2d _next_wp{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s] - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - - // Constants - static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rd_max_jerk, - (ParamFloat) _param_rd_max_accel, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_miss_spd_def, - (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn - - ) -}; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 3bad343f2e..eca54e798e 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -3,158 +3,17 @@ module_name: Rover Differential parameters: - group: Rover Differential definitions: - RD_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheel to the center of the left wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RD_MAN_YAW_SCALE: - description: - short: Manual yaw rate scale - long: | - In manual mode the setpoint for the yaw rate received from the yaw stick - is scaled by this value. - type: float - min: 0.001 - max: 1 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_SPEED_P: - description: - short: Proportional gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_SPEED_I: - description: - short: Integral gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_YAW_RATE_P: - description: - short: Proportional gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_RATE_I: - description: - short: Integral gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_SPEED: - description: - short: Maximum speed setpoint - long: | - This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_THR_SPD: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_YAW_RATE: - description: - short: Maximum allowed yaw rate for the rover - long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates - in Acro,Stabilized and Position mode. - type: float - unit: deg/s - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 90 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RD_MAX_THR_YAW_R: description: @@ -163,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -171,22 +30,14 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 - - RD_MISS_SPD_DEF: - description: - short: Default forward speed for the rover during auto modes - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 + default: 0 RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving + long: | + This threshold is used for the state machine to switch from turning to driving based on the + error between the desired and actual yaw. type: float unit: rad min: 0.001 @@ -210,3 +61,20 @@ parameters: increment: 0.01 decimal: 3 default: 0.174533 + + RD_MISS_SPD_GAIN: + description: + short: Tuning parameter for the speed reduction during waypoint transition + long: | + The waypoint transition speed is calculated as: + Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) + The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP + interpolated from [0, 180] -> [0, 1]. + Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. + type: float + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index 5c1d662de6..b0729acc02 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -81,7 +81,19 @@ void RoverMecanum::Run() rover_mecanum_setpoint.lateral_speed_setpoint = NAN; rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get(); + + if (_max_yaw_rate > FLT_EPSILON && _param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { + const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.yaw, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float speed_diff = scaled_yaw_rate_input * _param_rm_wheel_track.get() / 2.f; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, + -_param_rm_max_thr_yaw_r.get(), _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); + + } else { + rover_mecanum_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.yaw; + + } + rover_mecanum_setpoint.yaw_setpoint = NAN; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); @@ -100,7 +112,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = NAN; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); } @@ -119,7 +131,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = NAN; if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON @@ -157,9 +169,22 @@ void RoverMecanum::Run() rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = NAN; + // Reset cruise control if the manual input changes + if (_yaw_ctl && (!(fabsf(manual_control_setpoint.throttle - _prev_throttle) > STICK_DEADZONE) + || !(fabsf(manual_control_setpoint.roll - _prev_roll) > STICK_DEADZONE))) { + _yaw_ctl = false; + _prev_throttle = manual_control_setpoint.throttle; + _prev_roll = manual_control_setpoint.roll; + + } else if (!_yaw_ctl) { + _prev_throttle = manual_control_setpoint.throttle; + _prev_roll = manual_control_setpoint.roll; + } + + if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint) < FLT_EPSILON && fabsf(rover_mecanum_setpoint.lateral_speed_setpoint) < FLT_EPSILON)) { // Closed loop yaw rate control @@ -168,12 +193,30 @@ void RoverMecanum::Run() rover_mecanum_setpoint.yaw_setpoint = NAN; _yaw_ctl = false; - } else { // Closed loop yaw control // TODO: Add cruise control + } else { // Cruise control + const Vector3f velocity = Vector3f(rover_mecanum_setpoint.forward_speed_setpoint, + rover_mecanum_setpoint.lateral_speed_setpoint, 0.f); + const float desired_velocity_magnitude = velocity.norm(); + if (!_yaw_ctl) { _desired_yaw = _vehicle_yaw; _yaw_ctl = true; + _pos_ctl_start_position_ned = _curr_pos_ned; + const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); + _pos_ctl_course_direction = Vector2f(pos_ctl_course_direction_local(0), pos_ctl_course_direction_local(1)); + } + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; + const float desired_heading = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, desired_velocity_magnitude); + const float heading_error = matrix::wrap_pi(desired_heading - _vehicle_yaw); + const Vector2f desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); + rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0); + rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; } @@ -195,7 +238,7 @@ void RoverMecanum::Run() rover_mecanum_setpoint.lateral_speed_setpoint = NAN; rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = 0.f; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = 0.f; rover_mecanum_setpoint.yaw_setpoint = NAN; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); break; @@ -240,6 +283,7 @@ void RoverMecanum::updateSubscriptions() if (_vehicle_local_position_sub.updated()) { vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); // Apply threshold to the velocity measurement to cut off measurement noise when standing still diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 40b766c9a4..7c57a1138f 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -39,6 +39,7 @@ #include #include #include +#include // uORB includes #include @@ -52,7 +53,7 @@ #include // Standard libraries -#include +#include #include // Local includes @@ -112,6 +113,7 @@ private: // Instances RoverMecanumGuidance _rover_mecanum_guidance{this}; RoverMecanumControl _rover_mecanum_control{this}; + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library // Variables matrix::Quatf _vehicle_attitude_quaternion{}; @@ -123,11 +125,18 @@ private: int _nav_state{0}; bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in position mode float _desired_yaw{0.f}; // Yaw setpoint for position mode + Vector2f _pos_ctl_start_position_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _curr_pos_ned{}; + float _prev_throttle{0.f}; + float _prev_roll{0.f}; bool _armed{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_rm_man_yaw_scale, - (ParamFloat) _param_rm_max_yaw_rate + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_yaw_r, + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_pp_lookahd_max ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt index 9777ae5d18..61b64980f1 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt +++ b/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(RoverMecanumControl RoverMecanumControl.cpp ) -target_link_libraries(RoverMecanumControl PUBLIC pid) +target_link_libraries(RoverMecanumControl PUBLIC PID) target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index 9f52ad1756..c581236784 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -42,40 +42,38 @@ RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(pa { updateParams(); _rover_mecanum_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_forward_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); } void RoverMecanumControl::updateParams() { ModuleParams::updateParams(); + + _pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + + _pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); + _pid_forward_throttle.setIntegralLimit(1.f); + _pid_forward_throttle.setOutputLimit(1.f); + + _pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); + _pid_lateral_throttle.setIntegralLimit(1.f); + _pid_lateral_throttle.setOutputLimit(1.f); + _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; - pid_set_parameters(&_pid_yaw_rate, - _param_rm_yaw_rate_p.get(), // Proportional gain - _param_rm_yaw_rate_i.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_forward_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_lateral_throttle, - _param_rm_p_gain_speed.get(), // Proportional gain - _param_rm_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - pid_set_parameters(&_pid_yaw, - _param_rm_p_gain_yaw.get(), // Proportional gain - _param_rm_i_gain_yaw.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit + _max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F; + _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + + // Update slew rates + if (_max_yaw_rate > FLT_EPSILON) { + _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); + } + + if (_max_yaw_accel > FLT_EPSILON) { + _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); + } } void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, @@ -91,118 +89,223 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl // Closed loop yaw control if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { - const float heading_error = matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw); - _rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0.f, 0.f, dt); + _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt); + _rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); + _pid_yaw.setSetpoint( + matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - + vehicle_yaw)); // error as setpoint to take care of wrapping + _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); + _rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; } else { - pid_reset_integral(&_pid_yaw); + _pid_yaw.resetIntegral(); + _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); } // Yaw rate control float speed_diff_normalized{0.f}; - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - if (_param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward - const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get(); - speed_diff_normalized = math::interpolate(speed_diff, -_param_rm_max_thr_yaw_r.get(), - _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); - } - - speed_diff_normalized = math::constrain(speed_diff_normalized + - pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt), - -1.f, 1.f); // Feedback + if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, + _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, false); + _rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState(); } else { // Use normalized setpoint - speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f; + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized, + vehicle_yaw_rate, + _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, true); } // Speed control - float forward_throttle{0.f}; - float lateral_throttle{0.f}; + float forward_speed_normalized{0.f}; + float lateral_speed_normalized{0.f}; if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint) && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control - // Closed loop forward speed control - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_throttle = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); + + if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { + + forward_speed_normalized = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); + + lateral_speed_normalized = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); + + const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf( + speed_diff_normalized); + + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); + const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); + forward_speed_normalized *= magnitude * normalization; + lateral_speed_normalized *= magnitude * normalization; + _rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(forward_speed_normalized, -1.f, 1.f, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); + _rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(lateral_speed_normalized, -1.f, 1.f, + -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); + } } - forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint, - vehicle_forward_speed, 0, dt); // Feedback + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint, + vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); + lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint, + vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); + _rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); + _rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState(); - // Closed loop lateral speed control - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - lateral_throttle = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - } - lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint, - vehicle_lateral_speed, 0, dt); // Feedback + } else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) + && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized, + vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); + lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized, + vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - } else { // Use normalized setpoint - forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f; - lateral_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.lateral_speed_setpoint_normalized, -1.f, 1.f) : 0.f; } // Publish rover mecanum status (logging) - rover_mecanum_status_s rover_mecanum_status{}; - rover_mecanum_status.timestamp = _timestamp; - rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; - rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; - rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; - rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; - rover_mecanum_status.measured_yaw = vehicle_yaw; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral; - rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral; - rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral; - _rover_mecanum_status_pub.publish(rover_mecanum_status); + _rover_mecanum_status.timestamp = _timestamp; + _rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; + _rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; + _rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; + _rover_mecanum_status.measured_yaw = vehicle_yaw; + _rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); + _rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); + _rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); + _rover_mecanum_status_pub.publish(_rover_mecanum_status); // Publish to motors actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_throttle, lateral_throttle, + computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); } -matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle, - float speed_diff) +float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, + const float max_thr_yaw_r, + const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, + PID &pid_yaw_rate, const bool normalized) { - // Prioritize ratio between forward and lateral speed over either magnitude - float combined_speed = fabsf(forward_throttle) + fabsf(lateral_throttle); + float slew_rate_normalization{1.f}; - if (combined_speed > 1.f) { - forward_throttle /= combined_speed; - lateral_throttle /= combined_speed; - combined_speed = 1.f; + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; } - // Prioritize yaw rate over forward and lateral speed - const float total_speed = combined_speed + fabsf(speed_diff); + if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); + yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - if (total_speed > 1.f) { - const float excess_velocity = fabsf(total_speed - 1.f); - const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity; - const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity; + } else { + yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); + } - if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) { - forward_throttle = forward_throttle_temp; - lateral_throttle = lateral_throttle_temp; + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (normalized) { + speed_diff_normalized = yaw_rate_with_accel_limit.getState(); + + } else { + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + _pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); + speed_diff_normalized = math::constrain(speed_diff_normalized + + _pid_yaw_rate.update(vehicle_yaw_rate, dt), + -1.f, 1.f); // Feedback + + + } + + return math::constrain(speed_diff_normalized, -1.f, 1.f); + +} + +float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint, + const float vehicle_speed, const float max_thr_spd, SlewRate &speed_setpoint_with_accel_limit, + PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; + } + + // Apply acceleration and deceleration limit + if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) { + if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); + speed_setpoint_with_accel_limit.update(speed_setpoint, dt); } else { - forward_throttle = lateral_throttle = 0.f; + speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); + } + + } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); + speed_setpoint_with_accel_limit.update(speed_setpoint, dt); + + } else { + speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + if (normalized) { + forward_speed_normalized = speed_setpoint_with_accel_limit.getState(); + + } else { // Closed loop speed control + + if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward + forward_speed_normalized = math::interpolate(speed_setpoint_with_accel_limit.getState(), + -max_thr_spd, max_thr_spd, + -1.f, 1.f); + } + + pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback + + } + + return math::constrain(forward_speed_normalized, -1.f, 1.f); + +} + +matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized, + float lateral_speed_normalized, + float speed_diff) +{ + const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(speed_diff); + + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); + const float magnitude = (1.f - fabsf(speed_diff)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); + forward_speed_normalized *= magnitude * normalization; + lateral_speed_normalized *= magnitude * normalization; + } // Calculate motor commands - const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff}; + const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff}; const Matrix input(input_data); const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; const Matrix m(m_data); @@ -213,8 +316,8 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr void RoverMecanumControl::resetControllers() { - pid_reset_integral(&_pid_forward_throttle); - pid_reset_integral(&_pid_lateral_throttle); - pid_reset_integral(&_pid_yaw_rate); - pid_reset_integral(&_pid_yaw); + _pid_forward_throttle.resetIntegral(); + _pid_lateral_throttle.resetIntegral(); + _pid_yaw_rate.resetIntegral(); + _pid_yaw.resetIntegral(); } diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp index 27a084934a..53ae266383 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -43,10 +43,11 @@ #include #include #include - +#include +#include // Standard libraries -#include +#include #include #include #include @@ -89,6 +90,39 @@ protected: void updateParams() override; private: + /** + * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. + * @param vehicle_yaw_rate Measured yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. + * @param wheel_track Wheel track [m]. + * @param dt Time since last update [s]. + * @param yaw_rate_with_accel_limit Yaw rate slew rate. + * @param pid_yaw_rate Yaw rate PID + * @param normalized Indicates if the forward speed setpoint is already normalized. + * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, + float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); + /** + * @brief Compute normalized speed setpoint and apply slew rates. + * to the speed setpoint and doing closed loop speed control if enabled. + * @param speed_setpoint Speed setpoint [m/s]. + * @param vehicle_speed Actual speed [m/s]. + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. + * @param speed_setpoint_with_accel_limit Speed slew rate. + * @param pid_throttle Throttle PID + * @param max_accel Maximum acceleration [m/s^2] + * @param max_decel Maximum deceleration [m/s^2] + * @param dt Time since last update [s]. + * @param normalized Indicates if the speed setpoint is already normalized. + * @return Normalized speed setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd, + SlewRate &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, + float dt, bool normalized); + /** * @brief Turn normalized speed setpoints into normalized motor commands. * @@ -105,30 +139,39 @@ private: // uORB publications uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; + rover_mecanum_status_s _rover_mecanum_status{}; // Variables rover_mecanum_setpoint_s _rover_mecanum_setpoint{}; hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; // Controllers - PID_t _pid_forward_throttle; // PID for the closed loop forward speed control - PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control - PID_t _pid_yaw; // PID for the closed loop yaw control - PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control + PID _pid_forward_throttle; // PID for the closed loop forward speed control + PID _pid_lateral_throttle; // PID for the closed loop lateral speed control + PID _pid_yaw; // PID for the closed loop yaw control + PID _pid_yaw_rate; // PID for the closed loop yaw rate control + SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; + SlewRate _lateral_speed_setpoint_with_accel_limit{0.f}; + SlewRate _yaw_rate_with_accel_limit{0.f}; + SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_spd, + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_spd, + (ParamFloat) _param_rm_max_accel, + (ParamFloat) _param_rm_max_decel, (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_yaw_rate_p, - (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_yaw, - (ParamFloat) _param_rm_i_gain_yaw, - (ParamInt) _param_r_rev + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_rm_max_yaw_accel, + (ParamFloat) _param_rm_yaw_rate_p, + (ParamFloat) _param_rm_yaw_rate_i, + (ParamFloat) _param_rm_p_gain_speed, + (ParamFloat) _param_rm_i_gain_speed, + (ParamFloat) _param_rm_p_gain_yaw, + (ParamFloat) _param_rm_i_gain_yaw, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index 4ffab54919..b72478d746 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -41,7 +41,7 @@ using namespace time_literals; RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _max_velocity_magnitude = _param_rm_miss_spd_def.get(); + _max_velocity_magnitude = _param_rm_max_speed.get(); _rover_mecanum_guidance_status_pub.advertise(); } @@ -110,7 +110,7 @@ void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); } @@ -195,13 +195,9 @@ void RoverMecanumGuidance::updateWaypoints() _waypoint_transition_angle = acosf(cosin); // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > FLT_EPSILON) { - _max_velocity_magnitude = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, - _param_rm_max_speed.get()); - - } else { - _max_velocity_magnitude = _param_rm_miss_spd_def.get(); - } + _max_velocity_magnitude = position_setpoint_triplet.current.cruising_speed > FLT_EPSILON ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, + _param_rm_max_speed.get()) : _param_rm_max_speed.get(); // Waypoint yaw setpoint if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp index 509c2251b4..0970b83646 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -133,7 +133,6 @@ private: (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rm_max_jerk, (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_miss_spd_def, (ParamFloat) _param_rm_max_yaw_rate, (ParamFloat) _param_rm_miss_vel_gain ) diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 92f6600420..8d70ec90f8 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -4,19 +4,6 @@ parameters: - group: Rover Mecanum definitions: - RM_MAN_YAW_SCALE: - description: - short: Manual yaw rate scale - long: | - In Manual mode the setpoint for the yaw rate received from the control stick - is scaled by this value. - type: float - min: 0.01 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RM_WHEEL_TRACK: description: short: Wheel track @@ -42,6 +29,22 @@ parameters: decimal: 2 default: 90 + RM_MAX_YAW_ACCEL: + description: + short: Maximum allowed yaw acceleration for the rover + long: | + This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints + to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. + This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 + RM_MAX_THR_YAW_R: description: short: Yaw rate turning left/right wheels at max speed in opposite directions @@ -173,16 +176,20 @@ parameters: decimal: 2 default: 0.5 - RM_MISS_SPD_DEF: + RM_MAX_DECEL: description: - short: Default rover speed during a mission + short: Maximum deceleration + long: | + Maximum decelaration is used to limit the deceleration of the rover. + Set to -1 to disable, causing the rover to decelerate as fast as possible. + Caution: Disabling the deceleration limit also disables the slow down effect in auto modes. type: float - unit: m/s - min: 0 + unit: m/s^2 + min: -1 max: 100 increment: 0.01 decimal: 2 - default: 1 + default: -1 RM_MISS_VEL_GAIN: description: diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 5e093862e7..ee85e3cc39 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -38,5 +38,5 @@ px4_add_module( RoverPositionControl.hpp DEPENDS l1 - pid + PID ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 928ce13c6e..119c8c18e8 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -92,13 +92,9 @@ void RoverPositionControl::parameters_update(bool force) _gnd_control.set_l1_damping(_param_l1_damping.get()); _gnd_control.set_l1_period(_param_l1_period.get()); - pid_init(&_speed_ctrl, PID_MODE_DERIVATIV_CALC, 0.01f); - pid_set_parameters(&_speed_ctrl, - _param_speed_p.get(), - _param_speed_i.get(), - _param_speed_d.get(), - _param_speed_imax.get(), - _param_gndspeed_max.get()); + _speed_ctrl.setGains(_param_speed_p.get(), _param_speed_i.get(), _param_speed_d.get()); + _speed_ctrl.setIntegralLimit(_param_speed_imax.get()); + _speed_ctrl.setOutputLimit(_param_gndspeed_max.get()); } } @@ -239,12 +235,9 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); - const float x_vel = vel(0); - const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; - // Compute airspeed control out and just scale it as a constant - mission_throttle = _param_throttle_speed_scaler.get() - * pid_calculate(&_speed_ctrl, mission_target_speed, x_vel, x_acc, dt); + _speed_ctrl.setSetpoint(mission_target_speed); + mission_throttle = _param_throttle_speed_scaler.get() * _speed_ctrl.update(vel(0), dt); // Constrain throttle between min and max mission_throttle = math::constrain(mission_throttle, _param_throttle_min.get(), _param_throttle_max.get()); @@ -327,10 +320,8 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2)); - const float x_vel = vel(0); - const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; - - const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); + _speed_ctrl.setSetpoint(desired_speed); + const float control_throttle = _speed_ctrl.update(vel(0), dt); //Constrain maximum throttle to mission throttle _throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle); @@ -392,8 +383,6 @@ RoverPositionControl::Run() vehicle_attitude_poll(); manual_control_setpoint_poll(); - _vehicle_acceleration_sub.update(); - /* update parameters from storage */ parameters_update(); @@ -452,7 +441,6 @@ RoverPositionControl::Run() _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); pos_ctrl_status.acceptance_radius = turn_distance; - pos_ctrl_status.yaw_acceptance = NAN; pos_ctrl_status.timestamp = hrt_absolute_time(); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index ee1c65efa0..d3a26438e2 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -128,8 +128,6 @@ private: uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; - uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**(UINT16_MAX) * 1e-6f}; + static constexpr float DT_MAX{static_cast(UINT32_MAX) * 1e-6f}; /** * Put an item into the integral. @@ -111,7 +111,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) { if (integral_ready()) { integral = _alpha; @@ -200,7 +200,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) { if (Integrator::reset(integral, integral_dt)) { // apply coning corrections diff --git a/src/modules/sensors/sensor_params_flow.c b/src/modules/sensors/sensor_params_flow.c index b8abaab5f6..1082582746 100644 --- a/src/modules/sensors/sensor_params_flow.c +++ b/src/modules/sensors/sensor_params_flow.c @@ -111,3 +111,13 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 8.f); * */ PARAM_DEFINE_FLOAT(SENS_FLOW_RATE, 70.0f); + +/** + * Optical flow scale factor + * + * @min 0.5 + * @max 1.5 + * @decimal 2 + * @group Sensors + */ +PARAM_DEFINE_FLOAT(SENS_FLOW_SCALE, 1.f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..97ceb6db21 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -278,32 +278,15 @@ void Sensors::diff_pres_poll() vehicle_air_data_s air_data{}; _vehicle_air_data_sub.copy(&air_data); - - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } + const float temperature = air_data.ambient_temperature; // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; + float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f }; _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? // accumulate average for publication _diff_pres_timestamp_sum += diff_pres.timestamp_sample; _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _diff_pres_temperature_sum += air_temperature_celsius; _baro_pressure_sum += air_data.baro_pressure_pa; _diff_pres_count++; @@ -313,12 +296,10 @@ void Sensors::diff_pres_poll() const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; // reset _diff_pres_timestamp_sum = 0; _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; _baro_pressure_sum = 0; _diff_pres_count = 0; @@ -354,7 +335,6 @@ void Sensors::diff_pres_poll() airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 911005249c..3a8bcd50bc 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -90,11 +90,7 @@ using namespace sensors; using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f + class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 819eac6c2d..a12634055f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -38,7 +38,6 @@ #include #include - namespace sensors { @@ -46,6 +45,9 @@ using namespace matrix; using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; +static constexpr float DEFAULT_TEMPERATURE_CELSIUS = 15.f; +static constexpr float TEMPERATURE_MIN_CELSIUS = -60.f; +static constexpr float TEMPERATURE_MAX_CELSIUS = 60.f; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), @@ -78,21 +80,23 @@ void VehicleAirData::Stop() } } -void VehicleAirData::AirTemperatureUpdate() +float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, + const hrt_abstime time_now_us) { + // use the temperature from the differential pressure sensor if available + // otherwise use the temperature from the external barometer + // Temperature measurements from internal baros are not used as typically not representative for ambient temperature + float temperature = source == TemperatureSource::EXTERNAL_BARO ? temperature_baro : DEFAULT_TEMPERATURE_CELSIUS; differential_pressure_s differential_pressure; - static constexpr float temperature_min_celsius = -20.f; - static constexpr float temperature_max_celsius = 35.f; - - // update air temperature if data from differential pressure sensor is finite and not exactly 0 - // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight - if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) - && fabsf(differential_pressure.temperature) > FLT_EPSILON) { - - _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, - temperature_max_celsius); + if (_differential_pressure_sub.copy(&differential_pressure) + && time_now_us - differential_pressure.timestamp_sample < 1_s + && PX4_ISFINITE(differential_pressure.temperature)) { + temperature = differential_pressure.temperature; + source = TemperatureSource::AIRSPEED; } + + return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS); } bool VehicleAirData::ParametersUpdate(bool force) @@ -140,7 +144,8 @@ void VehicleAirData::Run() const bool parameter_update = ParametersUpdate(); - AirTemperatureUpdate(); + estimator_status_flags_s estimator_status_flags; + const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); bool updated[MAX_SENSOR_COUNT] {}; @@ -188,9 +193,20 @@ void VehicleAirData::Run() _sensor_sub[uorb_index].registerCallback(); } + if (!_calibration[uorb_index].calibrated()) { + _calibration[uorb_index].set_device_id(report.device_id); + _calibration[uorb_index].ParametersSave(uorb_index); + param_notify_changes(); + } + ParametersUpdate(true); } + if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index + && estimator_status_flags.cs_baro_fault && !_last_status_baro_fault) { + _priority[uorb_index] = 1; // 1 is min priority while still being enabled + } + // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); @@ -212,6 +228,10 @@ void VehicleAirData::Run() } } + if (estimator_status_flags_updated) { + _last_status_baro_fault = estimator_status_flags.cs_baro_fault; + } + // check for the current best sensor int best_index = 0; _voter.get_best(time_now_us, &best_index); @@ -255,23 +275,26 @@ void VehicleAirData::Run() if (publish) { const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; - const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; + const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance]; + TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO : + TemperatureSource::DEFAULT_TEMP; + const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us); const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; out.timestamp_sample = timestamp_sample; out.baro_device_id = _calibration[instance].device_id(); out.baro_alt_meter = altitude; - out.baro_temp_celcius = temperature; + out.ambient_temperature = ambient_temperature; + out.temperature_source = static_cast(temperature_source); out.baro_pressure_pa = pressure_pa; out.rho = air_density; - out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON)); out.calibration_count = _calibration[instance].calibration_count(); out.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index fd28de34f8..1e6555fcdd 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -54,6 +54,7 @@ #include #include #include +#include using namespace time_literals; @@ -72,9 +73,15 @@ public: void PrintStatus(); private: + enum TemperatureSource { + DEFAULT_TEMP = 0, + EXTERNAL_BARO = 1, + AIRSPEED = 2, + }; + void Run() override; - void AirTemperatureUpdate(); + float AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, const hrt_abstime time_now_us); void CheckFailover(const hrt_abstime &time_now_us); bool ParametersUpdate(bool force = false); void UpdateStatus(); @@ -89,6 +96,8 @@ private: uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_baro), 0}, {this, ORB_ID(sensor_baro), 1}, @@ -121,7 +130,7 @@ private: int8_t _selected_sensor_sub_index{-1}; - float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature + bool _last_status_baro_fault{false}; DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index f5b7343496..b8c0a9547e 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -69,9 +69,6 @@ VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, co // schedule conservatively until the actual accel & gyro rates are known _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); #endif - - _notify_clipping = _param_sens_imu_notify_clipping.get(); - // advertise immediately to ensure consistent ordering _vehicle_imu_pub.advertise(); _vehicle_imu_status_pub.advertise(); @@ -127,6 +124,8 @@ bool VehicleIMU::ParametersUpdate(bool force) _accel_calibration.ParametersUpdate(); _gyro_calibration.ParametersUpdate(); + _notify_clipping = _param_sens_imu_notify_clipping.get(); + if (accel_calibration_count != _accel_calibration.calibration_count()) { // if calibration changed reset any existing learned calibration _accel_cal_available = false; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index df90ffdf90..a0ba14bcfd 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -209,6 +209,19 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) { bool parameters_notify = false; + bool external_mag_available = false; + + for (unsigned mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].external() + && _calibration[mag_index].enabled() + && mag_bias_est.valid[mag_index] + && mag_bias_est.stable[mag_index]) { + + external_mag_available = true; + break; + } + } + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { @@ -228,6 +241,11 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); if (_calibration[mag_index].set_offset(offset)) { + if (external_mag_available && !_calibration[mag_index].external()) { + // automatically disable the internal mags as they should not be used for navigation + _calibration[mag_index].disable(); + } + // save parameters with preferred calibration slot to current sensor index _calibration[mag_index].ParametersSave(mag_index); diff --git a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt index ba8761aa00..bf543c0636 100644 --- a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt +++ b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt @@ -36,3 +36,7 @@ px4_add_library(vehicle_optical_flow VehicleOpticalFlow.hpp ) target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index e629095be6..f7d573d927 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -161,7 +161,7 @@ void VehicleOpticalFlow::Run() } Vector3f delta_angle{NAN, NAN, NAN}; - uint16_t delta_angle_dt; + uint32_t delta_angle_dt; if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) { _delta_angle += delta_angle; @@ -226,6 +226,7 @@ void VehicleOpticalFlow::Run() vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample; vehicle_optical_flow.device_id = sensor_optical_flow.device_id; + _flow_integral *= _param_sens_flow_scale.get(); _flow_integral.copyTo(vehicle_optical_flow.pixel_flow); _delta_angle.copyTo(vehicle_optical_flow.delta_angle); diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index fb424ea5af..dd3022c709 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -73,9 +73,12 @@ public: void PrintStatus(); +protected: + void UpdateDistanceSensor(); + int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations + private: void ClearAccumulatedData(); - void UpdateDistanceSensor(); void UpdateSensorGyro(); void Run() override; @@ -117,7 +120,6 @@ private: uint16_t _quality_sum{0}; uint8_t _accumulated_count{0}; - int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations hrt_abstime _last_range_sensor_update{0}; bool _delta_angle_available{false}; @@ -141,7 +143,8 @@ private: (ParamFloat) _param_sens_flow_minhgt, (ParamFloat) _param_sens_flow_maxhgt, (ParamFloat) _param_sens_flow_maxr, - (ParamFloat) _param_sens_flow_rate + (ParamFloat) _param_sens_flow_rate, + (ParamFloat) _param_sens_flow_scale ) }; }; // namespace sensors diff --git a/src/lib/bezier/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt similarity index 90% rename from src/lib/bezier/CMakeLists.txt rename to src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt index f069fff591..e3d89c05a2 100644 --- a/src/lib/bezier/CMakeLists.txt +++ b/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2024 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -18,6 +18,7 @@ # # 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, @@ -31,9 +32,4 @@ # ############################################################################ -px4_add_library(bezier - BezierQuad.cpp - BezierN.cpp -) - -px4_add_unit_gtest(SRC BezierNTest.cpp LINKLIBS bezier) +px4_add_functional_gtest(SRC VehicleOpticalFlowTest.cpp LINKLIBS vehicle_optical_flow uORB sensor_calibration) diff --git a/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp b/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp new file mode 100644 index 0000000000..a97a6ba1ee --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Test for VehicleOpticalFlow + */ + +#include +#include "../VehicleOpticalFlow.hpp" +#include +#include + + +distance_sensor_s createDistanceSensorMessage(uint16_t orientation) +{ + distance_sensor_s message; + message.timestamp = hrt_absolute_time(); + message.min_distance = 1.f; + message.max_distance = 100.f; + message.current_distance = 1.1f; + + message.variance = 0.1f; + message.signal_quality = 100; + message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + message.orientation = orientation; + message.h_fov = math::radians(50.f); + message.v_fov = math::radians(30.f); + return message; + +} + +class VehicleOpticalFlowTest : public ::testing::Test +{ +public: + + class VehicleOpticalFlowTestable : public sensors::VehicleOpticalFlow + { + public: + void UpdateDistanceSensorPublic() + { + VehicleOpticalFlow::UpdateDistanceSensor(); + } + bool IsDistanceSensorSelected() + { + return _distance_sensor_selected >= 0; + + } + }; + + void SetUp() override + { + uORB::Manager::initialize(); + + } + void TearDown() override + { + uORB::Manager::terminate(); + } +}; + + +TEST_F(VehicleOpticalFlowTest, CameraFacingDown) +{ + // GIVEN: message with sensor camera facing down + distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_DOWNWARD_FACING); + orb_advertise(ORB_ID(distance_sensor), &message); + + // WHEN: update distance sensor + VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable; + testable.UpdateDistanceSensorPublic(); + + // THEN: sensor selected + EXPECT_TRUE(testable.IsDistanceSensorSelected()); +} + +TEST_F(VehicleOpticalFlowTest, CameraFacingForward) +{ + // GIVEN: message with sensor camera facing forward + distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_FORWARD_FACING); + orb_advertise(ORB_ID(distance_sensor), &message); + + // WHEN: update distance sensor + VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable; + testable.UpdateDistanceSensorPublic(); + + // THEN: sensor is not selected + EXPECT_FALSE(testable.IsDistanceSensorSelected()); +} diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig index a417092ed9..576834846b 100644 --- a/src/modules/simulation/Kconfig +++ b/src/modules/simulation/Kconfig @@ -9,6 +9,7 @@ menu "Simulation" select MODULES_SIMULATION_SENSOR_BARO_SIM select MODULES_SIMULATION_SENSOR_GPS_SIM select MODULES_SIMULATION_SENSOR_MAG_SIM + select MODULES_SIMULATION_SYSTEM_POWER_SIMULATOR select MODULES_SIMULATION_SIMULATOR_MAVLINK select MODULES_SIMULATION_SIMULATOR_SIH ---help--- diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.hpp b/src/modules/simulation/battery_simulator/BatterySimulator.hpp index a25151b749..1765c63188 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.hpp @@ -74,8 +74,6 @@ private: static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ; - uORB::Publication _battery_pub{ORB_ID(battery_status)}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 358a4e7032..e57c602045 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -32,8 +32,8 @@ ############################################################################ # Find the gz_Transport library -# Look for GZ Ionic, Harmonic, and Garden library options in that order -find_package(gz-transport NAMES gz-transport14 gz-transport13 gz-transport12) +# Look for GZ Ionic or Harmonic +find_package(gz-transport NAMES gz-transport14 gz-transport13) if(gz-transport_FOUND) @@ -61,6 +61,8 @@ if(gz-transport_FOUND) GZMixingInterfaceServo.hpp GZMixingInterfaceWheel.cpp GZMixingInterfaceWheel.hpp + GZGimbal.cpp + GZGimbal.hpp DEPENDS mixer_module px4_work_queue @@ -90,6 +92,7 @@ if(gz-transport_FOUND) lawn aruco rover + walls ) # find corresponding airframes diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 29d19fe111..23684e76c1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -95,12 +95,6 @@ int GZBridge::init() model_pose_v.push_back(0.0); } - // If model position z is less equal than 0, move above floor to prevent floor glitching - if (model_pose_v[2] <= 0.0) { - PX4_INFO("Model position z is less or equal 0.0, moving upwards"); - model_pose_v[2] = 0.5; - } - gz::msgs::Pose *p = req.mutable_pose(); gz::msgs::Vector3d *position = p->mutable_position(); position->set_x(model_pose_v[0]); @@ -141,7 +135,7 @@ int GZBridge::init() // If Gazebo has not been called, wait 2 seconds and try again. else { - PX4_WARN("Service call timed out as Gazebo has not been detected."); + PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); system_usleep(2000000); } } @@ -159,7 +153,7 @@ int GZBridge::init() while (scene_created == false) { if (!callSceneInfoMsgService(scene_info_service)) { - PX4_WARN("Service call timed out as Gazebo has not been detected."); + PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); system_usleep(2000000); } else { @@ -218,17 +212,23 @@ int GZBridge::init() PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str()); } -#if 0 - // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed - std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + - "/link/airspeed_link/sensor/air_speed/air_speed"; + // Distance Sensor(AFBRS50): optional + std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name + + "/link/lidar_sensor_link/sensor/lidar/scan"; - if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + if (!_node.Subscribe(lidar_sensor, &GZBridge::laserScantoLidarSensorCallback, this)) { + PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str()); + } + + // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed + std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; + + if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str()); return PX4_ERROR; } -#endif // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; @@ -262,6 +262,11 @@ int GZBridge::init() return PX4_ERROR; } + if (!_gimbal.init(_world_name, _model_name)) { + PX4_ERR("failed to init gimbal"); + return PX4_ERROR; + } + ScheduleNow(); return OK; } @@ -441,8 +446,8 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) pthread_mutex_unlock(&_node_mutex); } -#if 0 -void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) + +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) { if (hrt_absolute_time() == 0) { return; @@ -467,7 +472,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) pthread_mutex_unlock(&_node_mutex); } -#endif void GZBridge::imuCallback(const gz::msgs::IMU &imu) { @@ -762,10 +766,63 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) pthread_mutex_unlock(&_node_mutex); } +void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) +{ + if (hrt_absolute_time() == 0) { + return; + } + + distance_sensor_s distance_sensor{}; + distance_sensor.timestamp = hrt_absolute_time(); + device::Device::DeviceId id; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.bus = 0; + id.devid_s.address = 0; + id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + distance_sensor.device_id = id.devid; + distance_sensor.min_distance = static_cast(scan.range_min()); + distance_sensor.max_distance = static_cast(scan.range_max()); + distance_sensor.current_distance = static_cast(scan.ranges()[0]); + distance_sensor.variance = 0.0f; + distance_sensor.signal_quality = -1; + distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + + gz::msgs::Quaternion pose_orientation = scan.world_pose().orientation(); + gz::math::Quaterniond q_sensor = gz::math::Quaterniond( + pose_orientation.w(), + pose_orientation.x(), + pose_orientation.y(), + pose_orientation.z()); + + const gz::math::Quaterniond q_left(0.7071068, 0, 0, -0.7071068); + + const gz::math::Quaterniond q_front(0.7071068, 0.7071068, 0, 0); + + const gz::math::Quaterniond q_down(0, 1, 0, 0); + + if (q_sensor.Equal(q_front, 0.03)) { + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + + } else if (q_sensor.Equal(q_down, 0.03)) { + distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + } else if (q_sensor.Equal(q_left, 0.03)) { + distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + + } else { + distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; + distance_sensor.q[0] = q_sensor.W(); + distance_sensor.q[1] = q_sensor.X(); + distance_sensor.q[2] = q_sensor.Y(); + distance_sensor.q[3] = q_sensor.Z(); + } + + _distance_sensor_pub.publish(distance_sensor); +} void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) { - static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each + static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors double angle_min_deg = scan.angle_min() * 180 / M_PI; double angle_step_deg = scan.angle_step() * 180 / M_PI; @@ -856,7 +913,7 @@ bool GZBridge::callEntityFactoryService(const std::string &service, const gz::ms } } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } @@ -869,7 +926,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) gz::msgs::Empty req; gz::msgs::Scene rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!result) { PX4_ERR("Scene Info service call failed."); return false; @@ -879,7 +936,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) } } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } @@ -892,7 +949,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs:: gz::msgs::Boolean rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!rep.data() || !result) { PX4_ERR("String service call failed"); return false; @@ -901,7 +958,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs:: } else { - PX4_ERR("Service call timed out: %s", service.c_str()); + PX4_WARN("Service call timed out: %s", service.c_str()); return false; } @@ -914,7 +971,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V gz::msgs::Boolean rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!rep.data() || !result) { PX4_ERR("String service call failed"); return false; @@ -923,7 +980,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V } else { - PX4_ERR("Service call timed out: %s", service.c_str()); + PX4_WARN("Service call timed out: %s", service.c_str()); return false; } @@ -957,6 +1014,7 @@ void GZBridge::Run() _mixing_interface_esc.stop(); _mixing_interface_servo.stop(); _mixing_interface_wheel.stop(); + _gimbal.stop(); exit_and_cleanup(); return; @@ -973,6 +1031,7 @@ void GZBridge::Run() _mixing_interface_esc.updateParams(); _mixing_interface_servo.updateParams(); _mixing_interface_wheel.updateParams(); + _gimbal.updateParams(); } ScheduleDelayed(10_ms); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index fc2cb3a6e9..27ef424956 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -36,6 +36,7 @@ #include "GZMixingInterfaceESC.hpp" #include "GZMixingInterfaceServo.hpp" #include "GZMixingInterfaceWheel.hpp" +#include "GZGimbal.hpp" #include #include @@ -49,6 +50,8 @@ #include #include #include +#include +#include #include #include #include @@ -66,6 +69,7 @@ #include #include +#include #include #include #include @@ -104,12 +108,13 @@ private: void clockCallback(const gz::msgs::Clock &clock); - // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); + void airspeedCallback(const gz::msgs::AirSpeed &air_speed); void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); void navSatCallback(const gz::msgs::NavSat &nav_sat); + void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan); void laserScanCallback(const gz::msgs::LaserScan &scan); /** @@ -164,7 +169,8 @@ private: // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; @@ -179,6 +185,7 @@ private: GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; + GZGimbal _gimbal{_node, _node_mutex}; px4::atomic _world_time_us{0}; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp new file mode 100644 index 0000000000..976200020e --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -0,0 +1,261 @@ +// #define DEBUG_BUILD +#include "GZGimbal.hpp" + +bool GZGimbal::init(const std::string &world_name, const std::string &model_name) +{ + // Gazebo communication + const std::string gimbal_roll_topic = "/model/" + model_name + "/command/gimbal_roll"; + _gimbal_roll_cmd_publisher = _node.Advertise(gimbal_roll_topic); + + if (!_gimbal_roll_cmd_publisher.Valid()) { + PX4_ERR("failed to advertise %s", gimbal_roll_topic.c_str()); + return false; + } + + const std::string gimbal_pitch_topic = "/model/" + model_name + "/command/gimbal_pitch"; + _gimbal_pitch_cmd_publisher = _node.Advertise(gimbal_pitch_topic); + + if (!_gimbal_pitch_cmd_publisher.Valid()) { + PX4_ERR("failed to advertise %s", gimbal_pitch_topic.c_str()); + return false; + } + + const std::string gimbal_yaw_topic = "/model/" + model_name + "/command/gimbal_yaw"; + _gimbal_yaw_cmd_publisher = _node.Advertise(gimbal_yaw_topic); + + if (!_gimbal_yaw_cmd_publisher.Valid()) { + PX4_ERR("failed to advertise %s", gimbal_yaw_topic.c_str()); + return false; + } + + const std::string gimbal_imu_topic = "/world/" + world_name + "/model/" + model_name + + "/link/camera_link/sensor/camera_imu/imu"; + + if (!_node.Subscribe(gimbal_imu_topic, &GZGimbal::gimbalIMUCallback, this)) { + PX4_ERR("failed to subscribe to %s", gimbal_imu_topic.c_str()); + return false; + } + + // Mount parameters + _mnt_range_roll_handle = param_find("MNT_RANGE_ROLL"); + _mnt_range_pitch_handle = param_find("MNT_RANGE_PITCH"); + _mnt_range_yaw_handle = param_find("MNT_RANGE_YAW"); + _mnt_mode_out_handle = param_find("MNT_MODE_OUT"); + + if (_mnt_range_roll_handle == PARAM_INVALID || + _mnt_range_pitch_handle == PARAM_INVALID || + _mnt_range_yaw_handle == PARAM_INVALID || + _mnt_mode_out_handle == PARAM_INVALID) { + return false; + } + + updateParameters(); + + ScheduleOnInterval(200_ms); // @5Hz + + // Schedule on vehicle command messages + if (!_vehicle_command_sub.registerCallback()) { + return false; + } + + return true; +} + +void GZGimbal::Run() +{ + pthread_mutex_lock(&_node_mutex); + + const hrt_abstime now = hrt_absolute_time(); + const float dt = (now - _last_time_update) / 1e6f; + _last_time_update = now; + + updateParameters(); + + if (pollSetpoint()) { + //TODO handle device flags + publishJointCommand(_gimbal_roll_cmd_publisher, _roll_stp, _roll_rate_stp, _last_roll_stp, _roll_min, _roll_max, dt); + publishJointCommand(_gimbal_pitch_cmd_publisher, _pitch_stp, _pitch_rate_stp, _last_pitch_stp, _pitch_min, _pitch_max, + dt); + publishJointCommand(_gimbal_yaw_cmd_publisher, _yaw_stp, _yaw_rate_stp, _last_yaw_stp, _yaw_min, _yaw_max, dt); + } + + if (_mnt_mode_out == 2) { + // We have a Mavlink gimbal capable of sending messages + publishDeviceInfo(); + publishDeviceAttitude(); + } + + pthread_mutex_unlock(&_node_mutex); +} + +void GZGimbal::stop() +{ + ScheduleClear(); +} + +void GZGimbal::gimbalIMUCallback(const gz::msgs::IMU &IMU_data) +{ + pthread_mutex_lock(&_node_mutex); + + static const matrix::Quatf q_FLU_to_FRD = matrix::Quatf(0.0f, 1.0f, 0.0f, 0.0f); + const matrix::Quatf q_gimbal_FLU = matrix::Quatf(IMU_data.orientation().w(), + IMU_data.orientation().x(), + IMU_data.orientation().y(), + IMU_data.orientation().z()); + _q_gimbal = q_FLU_to_FRD * q_gimbal_FLU * q_FLU_to_FRD.inversed(); + + matrix::Vector3f rate = q_FLU_to_FRD.rotateVector(matrix::Vector3f(IMU_data.angular_velocity().x(), + IMU_data.angular_velocity().y(), + IMU_data.angular_velocity().z())); + + _gimbal_rate[0] = rate(0); + _gimbal_rate[1] = rate(1); + _gimbal_rate[2] = rate(2); + + pthread_mutex_unlock(&_node_mutex); +} + +void GZGimbal::updateParameters() +{ + param_get(_mnt_range_roll_handle, &_mnt_range_roll); + param_get(_mnt_range_pitch_handle, &_mnt_range_pitch); + param_get(_mnt_range_yaw_handle, &_mnt_range_yaw); + param_get(_mnt_mode_out_handle, &_mnt_mode_out); +} + +bool GZGimbal::pollSetpoint() +{ + if (_gimbal_device_set_attitude_sub.updated()) { + gimbal_device_set_attitude_s msg; + + if (_gimbal_device_set_attitude_sub.copy(&msg)) { + const matrix::Eulerf gimbal_att_stp(matrix::Quatf(msg.q)); + _roll_stp = gimbal_att_stp.phi(); + _pitch_stp = gimbal_att_stp.theta(); + _yaw_stp = gimbal_att_stp.psi(); + _roll_rate_stp = msg.angular_velocity_x; + _pitch_rate_stp = msg.angular_velocity_y; + _yaw_rate_stp = msg.angular_velocity_z; + _gimbal_device_flags = msg.flags; + + return true; + } + + } else if (_gimbal_controls_sub.updated()) { + gimbal_controls_s msg; + + if (_gimbal_controls_sub.copy(&msg)) { + // map control inputs from [-1;1] to [min_angle; max_angle] using the range parameters + _roll_stp = math::constrain(math::radians(msg.control[msg.INDEX_ROLL] * _mnt_range_roll / 2), _roll_min, _roll_max); + _pitch_stp = math::constrain(math::radians(msg.control[msg.INDEX_PITCH] * _mnt_range_pitch / 2), _pitch_min, + _pitch_max); + _yaw_stp = math::constrain(math::radians(msg.control[msg.INDEX_YAW] * _mnt_range_yaw / 2), _yaw_min, _yaw_max); + + return true; + } + } + + return false; +} + +void GZGimbal::publishDeviceInfo() +{ + if (_vehicle_command_sub.updated()) { + vehicle_command_s cmd; + _vehicle_command_sub.copy(&cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE && + (uint16_t)cmd.param1 == vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION) { + // Acknowledge the command + vehicle_command_ack_s command_ack{}; + + command_ack.command = cmd.command; + command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + command_ack.timestamp = hrt_absolute_time(); + + _vehicle_command_ack_pub.publish(command_ack); + + // Send the requested message + gimbal_device_information_s device_info{}; + + memcpy(device_info.vendor_name, _vendor_name, sizeof(_vendor_name)); + memcpy(device_info.model_name, _model_name, sizeof(_model_name)); + memcpy(device_info.custom_name, _custom_name, sizeof(_custom_name)); + device_info.firmware_version = _firmware_version; + device_info.hardware_version = _hardware_version; + device_info.uid = _uid; + device_info.cap_flags = _cap_flags; + device_info.custom_cap_flags = _custom_cap_flags; + device_info.roll_min = _roll_min; + device_info.roll_max = _roll_max; + device_info.pitch_min = _pitch_min; + device_info.pitch_max = _pitch_max; + device_info.yaw_min = _yaw_min; + device_info.yaw_max = _yaw_max; + device_info.gimbal_device_id = _gimbal_device_id; + device_info.timestamp = hrt_absolute_time(); + + _gimbal_device_information_pub.publish(device_info); + } + } +} + +void GZGimbal::publishDeviceAttitude() +{ + // TODO handle flags + + gimbal_device_attitude_status_s gimbal_att{}; + + gimbal_att.target_system = 0; // Broadcast + gimbal_att.target_component = 0; // Broadcast + gimbal_att.device_flags = 0; + _q_gimbal.copyTo(gimbal_att.q); + gimbal_att.angular_velocity_x = _gimbal_rate[0]; + gimbal_att.angular_velocity_y = _gimbal_rate[1]; + gimbal_att.angular_velocity_z = _gimbal_rate[2]; + gimbal_att.failure_flags = 0; + gimbal_att.timestamp = hrt_absolute_time(); + + _gimbal_device_attitude_status_pub.publish(gimbal_att); +} + +void GZGimbal::publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp, + float &last_stp, const float min_stp, const float max_stp, const float dt) +{ + gz::msgs::Double msg; + + float new_stp = computeJointSetpoint(att_stp, rate_stp, last_stp, dt); + new_stp = math::constrain(new_stp, min_stp, max_stp); + last_stp = new_stp; + msg.set_data(new_stp); + + publisher.Publish(msg); +} + +float GZGimbal::computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt) +{ + + if (PX4_ISFINITE(rate_stp)) { + const float rate_diff = dt * rate_stp; + const float stp_from_rate = last_stp + rate_diff; + + if (PX4_ISFINITE(att_stp)) { + // We use the attitude rate setpoint but we constrain it by the desired angle + return rate_diff > 0 ? math::min(att_stp, stp_from_rate) : math::max(att_stp, stp_from_rate); + + } else { + // The rate setpoint is valid while the angle one is not + return stp_from_rate; + } + + } else if (PX4_ISFINITE(att_stp)) { + // Only the angle setpoint is valid + return att_stp; + + } else { + // Neither setpoint is valid so we steer the gimbal to the default position (roll = pitch = yaw = 0) + return 0.0f; + } +} diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp new file mode 100644 index 0000000000..662268abd2 --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -0,0 +1,151 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +using namespace time_literals; + +class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams +{ +public: + GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl), + ModuleParams(nullptr), + _node(node), + _node_mutex(node_mutex) + {} + +private: + friend class GZBridge; + + gz::transport::Node &_node; + pthread_mutex_t &_node_mutex; + + uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; + uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)}; + uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)}; + + uORB::Publication _gimbal_device_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Publication _gimbal_device_information_pub{ORB_ID(gimbal_device_information)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + + gz::transport::Node::Publisher _gimbal_roll_cmd_publisher; + gz::transport::Node::Publisher _gimbal_pitch_cmd_publisher; + gz::transport::Node::Publisher _gimbal_yaw_cmd_publisher; + + float _roll_stp = NAN; + float _pitch_stp = NAN; + float _yaw_stp = NAN; + + float _last_roll_stp = 0.0f; + float _last_pitch_stp = 0.0f; + float _last_yaw_stp = 0.0f; + + float _roll_rate_stp = NAN; + float _pitch_rate_stp = NAN; + float _yaw_rate_stp = NAN; + + hrt_abstime _last_time_update; + + // Mount parameters + param_t _mnt_range_pitch_handle = PARAM_INVALID; + param_t _mnt_range_roll_handle = PARAM_INVALID; + param_t _mnt_range_yaw_handle = PARAM_INVALID; + param_t _mnt_mode_out_handle = PARAM_INVALID; + float _mnt_range_pitch = 0.0f; + float _mnt_range_roll = 0.0f; + float _mnt_range_yaw = 0.0f; + int32_t _mnt_mode_out = 0; + + matrix::Quatf _q_gimbal = matrix::Quatf(1.0f, 0.0f, 0.0f, 0.0f); + float _gimbal_rate[3] = {NAN}; + + // Device information attributes + const char _vendor_name[32] = "PX4"; + const char _model_name[32] = "Gazebo Gimbal"; + const char _custom_name[32] = ""; + const uint8_t _firmware_dev_version = 0; + const uint8_t _firmware_patch_version = 0; + const uint8_t _firmware_minor_version = 0; + const uint8_t _firmware_major_version = 1; + const uint32_t _firmware_version = (_firmware_dev_version & 0xff) << 24 | (_firmware_patch_version & 0xff) << 16 | + (_firmware_minor_version & 0xff) << 8 | (_firmware_major_version & 0xff); + const uint8_t _hardware_dev_version = 0; + const uint8_t _hardware_patch_version = 0; + const uint8_t _hardware_minor_version = 0; + const uint8_t _hardware_major_version = 1; + const uint32_t _hardware_version = (_hardware_dev_version & 0xff) << 24 | (_hardware_patch_version & 0xff) << 16 | + (_hardware_minor_version & 0xff) << 8 | (_hardware_major_version & 0xff); + const uint64_t _uid = 0x9a77a55b3c10971f ; + const uint16_t _cap_flags = gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW | + gimbal_device_information_s::GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW; + const uint16_t _custom_cap_flags = 0; + + // This module act as the gimbal driver. In case of a Mavlink compatible gimbal, the driver is aware of + // its mechanical limits. So the values below have to match the characteristics of the simulated gimbal + const float _roll_min = -0.785398f; + const float _roll_max = 0.785398f; + const float _pitch_min = -2.35619f; + const float _pitch_max = 0.785398f; + const float _yaw_min = NAN; // infinite yaw + const float _yaw_max = NAN; // infinite yaw + + const uint8_t _gimbal_device_id = 1; // Gimbal is implemented by the same component: options are 1..6 + uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS + + bool init(const std::string &world_name, const std::string &model_name); + void Run() override; + void stop(); + void gimbalIMUCallback(const gz::msgs::IMU &IMU_data); + void updateParameters(); + /// @brief Poll for new gimbal setpoints either from mavlink gimbal v2 protocol (gimbal_device_set_attitude topic) or from RC inputs (gimbal_controls topic). + /// @return true if a new setpoint has been requested; false otherwise. + bool pollSetpoint(); + /// @brief Respond to the gimbal manager when it requests GIMBAL_DEVICE_INFORMATION messages. + void publishDeviceInfo(); + /// @brief Broadcast gimbal device attitude status message. + void publishDeviceAttitude(); + /// @brief Compute joint position setpoint taking into account both desired position and velocity. Then publish the command using the specified gazebo node. + /// @param publisher Gazebo node that will publish the setpoint + /// @param att_stp desired joint attitude [rad] + /// @param rate_stp desired joint attitude rate [rad/s] + /// @param last_stp last joint attitude setpoint [rad] + /// @param min_stp minimum joint attitude [rad] + /// @param max_stp maximum joint attitude [rad] + /// @param dt time interval since the last computation [s] + static void publishJointCommand(gz::transport::Node::Publisher &publisher, const float att_stp, const float rate_stp, + float &last_stp, + const float min_stp, const float max_stp, const float dt); + /// @brief Compute joint position setpoint taking into account both desired position and velocity. + /// @param att_stp desired joint attitude [rad] + /// @param rate_stp desired joint attitude rate [rad/s] + /// @param last_stp last joint attitude setpoint [rad] + /// @param dt time interval since the last computation [s] + /// @return new joint setpoint + static float computeJointSetpoint(const float att_stp, const float rate_stp, const float last_stp, const float dt); +}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 9c7969b2dc..2fc656074c 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -33,6 +33,74 @@ #include "GZMixingInterfaceServo.hpp" + +float +GZMixingInterfaceServo::get_servo_angle_max(const size_t index) +{ + float angle; + + switch (index) { + case 0: {angle = _servo_max_angle_1.get(); break;} + + case 1: {angle = _servo_max_angle_2.get(); break;} + + case 2: {angle = _servo_max_angle_3.get(); break;} + + case 3: {angle = _servo_max_angle_4.get(); break;} + + case 4: {angle = _servo_max_angle_5.get(); break;} + + case 5: {angle = _servo_max_angle_6.get(); break;} + + case 6: {angle = _servo_max_angle_7.get(); break;} + + case 7: {angle = _servo_max_angle_8.get(); break;} + + default: {angle = NAN; break;} + } + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("max_angle: index out of range, i= %ld, expected i < 8", index); + return NAN; + } + + return math::radians(angle); +} + +float +GZMixingInterfaceServo::get_servo_angle_min(const size_t index) +{ + float angle; + + switch (index) { + case 0: {angle = _servo_min_angle_1.get(); break;} + + case 1: {angle = _servo_min_angle_2.get(); break;} + + case 2: {angle = _servo_min_angle_3.get(); break;} + + case 3: {angle = _servo_min_angle_4.get(); break;} + + case 4: {angle = _servo_min_angle_5.get(); break;} + + case 5: {angle = _servo_min_angle_6.get(); break;} + + case 6: {angle = _servo_min_angle_7.get(); break;} + + case 7: {angle = _servo_min_angle_8.get(); break;} + + default: {angle = NAN; break;} + + } + + if (!PX4_ISFINITE(angle)) { + PX4_ERR("min_angle: index out of range, i= %ld, expected i < 8", index); + return NAN; + } + + return math::radians(angle); +} + bool GZMixingInterfaceServo::init(const std::string &model_name) { // /model/rascal_110_0/servo_2 @@ -46,6 +114,11 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) PX4_ERR("failed to advertise %s", servo_topic.c_str()); return false; } + + double min_val = get_servo_angle_min(i); + double max_val = get_servo_angle_max(i); + _angle_min_rad.push_back(min_val); + _angular_range_rad.push_back(max_val - min_val); } ScheduleNow(); @@ -64,8 +137,9 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA for (auto &servo_pub : _servos_pub) { if (_mixing_output.isFunctionSet(i)) { gz::msgs::Double servo_output; - ///TODO: Normalize output data - double output = (outputs[i] - 500) / 500.0; + + double output_range = _mixing_output.maxValue(i) - _mixing_output.minValue(i); + double output = _angle_min_rad[i] + _angular_range_rad[i] * (outputs[i] - _mixing_output.minValue(i)) / output_range; // std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl; // std::cout << " output: " << output << std::endl; servo_output.set_data(output); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 63345b3aed..180f6cbca2 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -36,6 +36,7 @@ #include #include +#include // GZBridge mixing class for Servos. // It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling @@ -67,10 +68,45 @@ private: void Run() override; + /** + * @brief Get maximum configured angle of servo. + * @param index: servo index + * @return angle_max [rad] + */ + float get_servo_angle_max(const size_t index); + + /** + * @brief Get minimum configured angle of servo. + * @param index: servo index + * @return angle_min [rad] + */ + float get_servo_angle_min(const size_t index); + gz::transport::Node &_node; pthread_mutex_t &_node_mutex; MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; std::vector _servos_pub; + std::vector _angle_min_rad; + std::vector _angular_range_rad; + + DEFINE_PARAMETERS( + (ParamFloat) _servo_max_angle_1, + (ParamFloat) _servo_min_angle_1, + (ParamFloat) _servo_max_angle_2, + (ParamFloat) _servo_min_angle_2, + (ParamFloat) _servo_max_angle_3, + (ParamFloat) _servo_min_angle_3, + (ParamFloat) _servo_max_angle_4, + (ParamFloat) _servo_min_angle_4, + (ParamFloat) _servo_max_angle_5, + (ParamFloat) _servo_min_angle_5, + (ParamFloat) _servo_max_angle_6, + (ParamFloat) _servo_min_angle_6, + (ParamFloat) _servo_max_angle_7, + (ParamFloat) _servo_min_angle_7, + (ParamFloat) _servo_max_angle_8, + (ParamFloat) _servo_min_angle_8 + ) }; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 1b38e13d98..e41aedb1e3 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -1,3 +1,6 @@ +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 + module_name: SIM_GZ actuator_output: show_subgroups_if: 'SIM_GZ_EN>=1' @@ -15,7 +18,7 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } - num_channels: 8 + num_channels: *max_num_servos - param_prefix: SIM_GZ_SV group_label: 'Servos' channel_label: 'Servo' @@ -24,7 +27,7 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } - num_channels: 8 + num_channels: *max_num_servos - param_prefix: SIM_GZ_WH group_label: 'Wheels' channel_label: 'Wheels' @@ -34,3 +37,35 @@ actuator_output: max: { min: 0, max: 200, default: 200 } failsafe: { min: 0, max: 200 } num_channels: 4 + +parameters: + - group: Geometry + definitions: + SIM_GZ_SV_MAXA${i}: + description: + short: Servo ${i} Angle at Maximum + long: | + Defines the angle when the servo is at the maximum. + Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MAXA. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_servos + instance_start: 1 + min: -180.0 + max: 180.0 + default: 45.0 + SIM_GZ_SV_MINA${i}: + description: + short: Servo ${i} Angle at Minimum + long: | + Defines the angle when the servo is at the minimum. + Currently only supported in gz simulation and must be coherent with .sdf file and CA_SV_TL{n}_MINA. + type: float + decimal: 0 + unit: 'deg' + num_instances: *max_num_servos + instance_start: 1 + min: -180.0 + max: 180.0 + default: -45.0 diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/simulation/sensor_agp_sim/CMakeLists.txt similarity index 88% rename from src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt rename to src/modules/simulation/sensor_agp_sim/CMakeLists.txt index 58a5239f8c..e1390068eb 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/simulation/sensor_agp_sim/CMakeLists.txt @@ -31,9 +31,13 @@ # ############################################################################ -px4_add_library(RoverDifferentialControl - RoverDifferentialControl.cpp -) - -target_link_libraries(RoverDifferentialControl PUBLIC pid) -target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +px4_add_module( + MODULE modules__simulation__sensor_agp_sim + MAIN sensor_agp_sim + COMPILE_FLAGS + SRCS + SensorAgpSim.cpp + SensorAgpSim.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/simulation/sensor_agp_sim/Kconfig b/src/modules/simulation/sensor_agp_sim/Kconfig new file mode 100644 index 0000000000..43c6885246 --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SIMULATION_SENSOR_AGP_SIM + bool "sensor_agp_sim" + default n + ---help--- + Enable support for sensor_agp_sim diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp new file mode 100644 index 0000000000..4de5d46c2d --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * 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 "SensorAgpSim.hpp" + +#include +#include +#include + +using namespace matrix; + +SensorAgpSim::SensorAgpSim() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +SensorAgpSim::~SensorAgpSim() +{ + perf_free(_loop_perf); +} + +bool SensorAgpSim::init() +{ + ScheduleOnInterval(500_ms); // 2 Hz + return true; +} + +float SensorAgpSim::generate_wgn() +{ + // generate white Gaussian noise sample with std=1 + // from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / (float)RAND_MAX; + float U2 = (float)rand() / (float)RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); + } + + phase = !phase; + return X; +} + +void SensorAgpSim::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + if (_vehicle_global_position_sub.updated()) { + + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); + + const uint64_t now = gpos.timestamp; + const float dt = (now - _time_last_update) * 1e-6f; + _time_last_update = now; + + if (!(_param_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { + _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); + } + + if (_param_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { + _position_bias += Vector3f(1.5f, -5.f, 0.f) * dt; + _measured_lla += _position_bias; + + } else { + _position_bias.zero(); + } + + const double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 / + CONSTANTS_RADIUS_OF_EARTH); + const double longitude = _measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 / + CONSTANTS_RADIUS_OF_EARTH); + const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f)); + + vehicle_global_position_s sample{}; + + sample.timestamp_sample = gpos.timestamp_sample; + sample.lat = latitude; + sample.lon = longitude; + sample.alt = altitude; + sample.lat_lon_valid = true; + sample.alt_ellipsoid = altitude; + sample.alt_valid = true; + sample.eph = 20.f; + sample.epv = 5.f; + + sample.timestamp = hrt_absolute_time(); + _aux_global_position_pub.publish(sample); + } + + perf_end(_loop_perf); +} + +int SensorAgpSim::task_spawn(int argc, char *argv[]) +{ + SensorAgpSim *instance = new SensorAgpSim(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SensorAgpSim::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SensorAgpSim::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module to simulate auxiliary global position measurements with optional failure modes for SIH simulation. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sensor_agp_sim", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sensor_agp_sim_main(int argc, char *argv[]) +{ + return SensorAgpSim::main(argc, argv); +} diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp new file mode 100644 index 0000000000..9700780adb --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SensorAgpSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SensorAgpSim(); + ~SensorAgpSim() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + enum class FailureMode : int32_t { + Stuck = (1 << 0), + Drift = (1 << 1) + }; + + void Run() override; + + // generate white Gaussian noise sample with std=1 + static float generate_wgn(); + + LatLonAlt _measured_lla{}; + matrix::Vector3f _position_bias{}; + + uint64_t _time_last_update{}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; + + uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sim_agp_fail + ) +}; diff --git a/src/modules/simulation/sensor_agp_sim/parameters.c b/src/modules/simulation/sensor_agp_sim/parameters.c new file mode 100644 index 0000000000..6690255f09 --- /dev/null +++ b/src/modules/simulation/sensor_agp_sim/parameters.c @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +/** + * Simulate Aux Global Position (AGP) + * + * @reboot_required true + * @min 0 + * @max 1 + * @group Sensors + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0); + +/** + * AGP failure mode + * + * Stuck: freeze the measurement to the current location + * Drift: add a linearly growing bias to the sensor data + * + * @group Simulator + * @min 0 + * @max 3 + * @bit 0 Stuck + * @bit 1 Drift + */ +PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0); diff --git a/src/modules/simulation/simulator_mavlink/CMakeLists.txt b/src/modules/simulation/simulator_mavlink/CMakeLists.txt index fc4fa94aa4..f5f39d7106 100644 --- a/src/modules/simulation/simulator_mavlink/CMakeLists.txt +++ b/src/modules/simulation/simulator_mavlink/CMakeLists.txt @@ -40,9 +40,7 @@ px4_add_module( -Wno-address-of-packed-member # TODO: fix in c_library_v2 INCLUDES ${CMAKE_BINARY_DIR}/mavlink - ${CMAKE_BINARY_DIR}/mavlink/development - ${CMAKE_BINARY_DIR}/mavlink/common - ${CMAKE_BINARY_DIR}/mavlink/standard + ${CMAKE_BINARY_DIR}/mavlink/${CONFIG_MAVLINK_DIALECT} SRCS SimulatorMavlink.cpp SimulatorMavlink.hpp diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 20eed499b5..92c5fe73a5 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -388,14 +388,12 @@ void SimulatorMavlink::handle_message(const mavlink_message_t *msg) break; case MAVLINK_MSG_ID_RAW_RPM: - mavlink_raw_rpm_t rpm; - mavlink_msg_raw_rpm_decode(msg, &rpm); - rpm_s rpmmsg{}; - rpmmsg.timestamp = hrt_absolute_time(); - rpmmsg.indicated_frequency_rpm = rpm.frequency; - rpmmsg.estimated_accurancy_rpm = 0; - - _rpm_pub.publish(rpmmsg); + mavlink_raw_rpm_t rpm_mavlink; + mavlink_msg_raw_rpm_decode(msg, &rpm_mavlink); + rpm_s rpm_uorb{}; + rpm_uorb.timestamp = hrt_absolute_time(); + rpm_uorb.rpm_estimate = rpm_mavlink.frequency; + _rpm_pub.publish(rpm_uorb); break; } } @@ -580,7 +578,7 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message double lon = hil_state.lon * 1e-7; if (!_global_local_proj_ref.isInitialized()) { - _global_local_proj_ref.initReference(lat, lon); + _global_local_proj_ref.initReference(lat, lon, timestamp); _global_local_alt0 = hil_state.alt / 1000.f; } @@ -609,7 +607,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message hil_lpos.vxy_max = std::numeric_limits::infinity(); hil_lpos.vz_max = std::numeric_limits::infinity(); hil_lpos.hagl_min = std::numeric_limits::infinity(); - hil_lpos.hagl_max = std::numeric_limits::infinity(); + hil_lpos.hagl_max_z = std::numeric_limits::infinity(); + hil_lpos.hagl_max_xy = std::numeric_limits::infinity(); // always publish ground truth attitude message _lpos_ground_truth_pub.publish(hil_lpos); diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index b67fe546cc..258e18d8b2 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -80,7 +80,6 @@ if(gazebo_FOUND) iris_dual_gps iris_foggy_lidar iris_irlock - iris_obs_avoid iris_depth_camera iris_downward_depth_camera iris_opt_flow diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index f0328024ff..7ae8483c51 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mathlib drivers_accelerometer drivers_gyroscope + geo ) if(PX4_PLATFORM MATCHES "posix") @@ -52,6 +53,7 @@ if(PX4_PLATFORM MATCHES "posix") airplane quadx xvert + standard_vtol ) # find corresponding airframes diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 2ca4987338..2ebf936823 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -68,15 +68,15 @@ void Sih::run() _px4_accel.set_temperature(T1_C); _px4_gyro.set_temperature(T1_C); - parameters_updated(); init_variables(); + parameters_updated(); const hrt_abstime task_start = hrt_absolute_time(); _last_run = task_start; _airspeed_time = task_start; _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), - static_cast(2)); + static_cast(3)); _actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; @@ -216,7 +216,8 @@ void Sih::sensor_step() reconstruct_sensors_signals(now); - if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && now - _airspeed_time >= 50_ms) { + if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) + && now - _airspeed_time >= 50_ms) { _airspeed_time = now; send_airspeed(now); } @@ -241,16 +242,30 @@ void Sih::parameters_updated() _L_PITCH = _sih_l_pitch.get(); _KDV = _sih_kdv.get(); _KDW = _sih_kdw.get(); - _H0 = _sih_h0.get(); - _LAT0 = (double)_sih_lat0.get(); - _LON0 = (double)_sih_lon0.get(); - _COS_LAT0 = cosl((long double)radians(_LAT0)); + if (!_lpos_ref.isInitialized() + || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON) + || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLon()) - _sih_lon0.get()) > FLT_EPSILON) + || (fabsf(_lpos_ref_alt - _sih_h0.get()) > FLT_EPSILON)) { + _lpos_ref.initReference(static_cast(_sih_lat0.get()), static_cast(_sih_lon0.get())); + _lpos_ref_alt = _sih_h0.get(); + + // Reset earth position, velocity and attitude + _lla.setLatitudeDeg(static_cast(_sih_lat0.get())); + _lla.setLongitudeDeg(static_cast(_sih_lon0.get())); + _lla.setAltitude(_lpos_ref_alt); + _p_E = _lla.toEcef(); + + const Dcmf R_E2N = computeRotEcefToNed(_lla); + _R_N2E = R_E2N.transpose(); + _v_E = _R_N2E * _v_N; + + _q_E = Quatf(_R_N2E) * _q; + _q_E.normalize(); + } _MASS = _sih_mass.get(); - _W_I = Vector3f(0.0f, 0.0f, _MASS * CONSTANTS_ONE_G); - _I = diag(Vector3f(_sih_ixx.get(), _sih_iyy.get(), _sih_izz.get())); _I(0, 1) = _I(1, 0) = _sih_ixy.get(); _I(0, 2) = _I(2, 0) = _sih_ixz.get(); @@ -270,9 +285,12 @@ void Sih::init_variables() { srand(1234); // initialize the random seed once before calling generate_wgn() - _p_I = Vector3f(0.0f, 0.0f, 0.0f); - _v_I = Vector3f(0.0f, 0.0f, 0.0f); + _lpos = Vector3f(0.0f, 0.0f, 0.0f); + _v_N = Vector3f(0.0f, 0.0f, 0.0f); + _p_E = Vector3d(Wgs84::equatorial_radius, 0.0, 0.0); + _v_E = Vector3f(0.0f, 0.0f, 0.0f); _q = Quatf(1.0f, 0.0f, 0.0f, 0.0f); + _q_E = Quatf(Eulerf(0.f, -M_PI_2_F, 0.f)); _w_B = Vector3f(0.0f, 0.0f, 0.0f); _u[0] = _u[1] = _u[2] = _u[3] = 0.0f; @@ -285,7 +303,7 @@ void Sih::read_motors(const float dt) if (_actuator_out_sub.update(&actuators_out)) { _last_actuator_output_time = actuators_out.timestamp; - for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals + for (int i = 0; i < NUM_ACTUATORS_MAX; i++) { // saturate the motor signals if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) { _u[i] = actuators_out.output[i]; @@ -304,38 +322,53 @@ void Sih::generate_force_and_torques() _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); - _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft _Ma_B = -_KDW * _w_B; // first order angular damper } else if (_vehicle == VehicleType::FW) { _T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster // _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque _Mt_B = Vector3f(); - generate_fw_aerodynamics(); + generate_fw_aerodynamics(_u[0], _u[1], _u[2], _u[3]); } else if (_vehicle == VehicleType::TS) { _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1])); _Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0])); generate_ts_aerodynamics(); - // _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft + // _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft // _Ma_B = -_KDW * _w_B; // first order angular damper + + } else if (_vehicle == VehicleType::SVTOL) { + + _T_B = Vector3f(_T_MAX * 2 * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), + _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), + _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); + + // thrust 0 because it is already contained in _T_B. in + // equations_of_motion they are all summed into sum_of_forces_E + generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); } } -void Sih::generate_fw_aerodynamics() +void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, + const float throttle_cmd) { - _v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s] - float altitude = _H0 - _p_I(2); - _wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX); - _wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX); - _tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]); - _fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]); - _fuselage.update_aero(_v_B, _w_B, altitude); + const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); + const float &alt = _lla.altitude(); + + _wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX); + _wing_r.update_aero(v_B, _w_B, alt, -roll_cmd * FLAP_MAX); + + _tailplane.update_aero(v_B, _w_B, alt, -pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd); + _fin.update_aero(v_B, _w_B, alt, yaw_cmd * FLAP_MAX, _T_MAX * throttle_cmd); + _fuselage.update_aero(v_B, _w_B, alt); // sum of aerodynamic forces - _Fa_I = _C_IB * (_wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa()) - _KDV - * _v_I; + const Vector3f Fa_B = _wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa() - + _KDV * v_B; + _Fa_E = _q_E.rotateVector(Fa_B); // aerodynamic moments _Ma_B = _wing_l.get_Ma() + _wing_r.get_Ma() + _tailplane.get_Ma() + _fin.get_Ma() + _fuselage.get_Ma() - _KDW * _w_B; @@ -344,12 +377,12 @@ void Sih::generate_fw_aerodynamics() void Sih::generate_ts_aerodynamics() { // velocity in body frame [m/s] - _v_B = _C_IB.transpose() * _v_I; + const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); // the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly) - Vector3f v_ts = _C_BS.transpose() * _v_B; - Vector3f w_ts = _C_BS.transpose() * _w_B; - float altitude = _H0 - _p_I(2); + Vector3f v_ts = _R_S2B.transpose() * v_B; + Vector3f w_ts = _R_S2B.transpose() * _w_B; + float altitude = _lpos_ref_alt - _lpos(2); Vector3f Fa_ts{}; Vector3f Ma_ts{}; @@ -366,68 +399,120 @@ void Sih::generate_ts_aerodynamics() Ma_ts += _ts[i].get_Ma(); } - _Fa_I = _C_IB * _C_BS * Fa_ts - _KDV * _v_I; // sum of aerodynamic forces - _Ma_B = _C_BS * Ma_ts - _KDW * _w_B; // aerodynamic moments + const Vector3f Fa_B = _R_S2B * Fa_ts - _KDV * v_B; // sum of aerodynamic forces + _Fa_E = _q_E.rotateVector(Fa_B); + _Ma_B = _R_S2B * Ma_ts - _KDW * _w_B; // aerodynamic moments +} + +float Sih::computeGravity(const double lat) +{ + // Somigliana formula for gravitational acceleration + const double sin_lat = sin(lat); + const double g = LatLonAlt::Wgs84::gravity_equator * (1.0 + 0.001931851353 * sin_lat * sin_lat) / sqrt( + 1.0 - LatLonAlt::Wgs84::eccentricity2 * sin_lat * sin_lat); + return static_cast(g); } void Sih::equations_of_motion(const float dt) { - _C_IB = matrix::Dcm(_q); // body to inertial transformation + const Vector3f gravity_acceleration_E = Vector3f(_R_N2E.col(2)) * computeGravity( + _lla.latitude_rad()); // gravity along the Down axis + const Vector3f coriolis_acceleration_E = -2.f * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE).cross(_v_E); - // Equations of motion of a rigid body - _p_I_dot = _v_I; // position differential - _v_I_dot = (_W_I + _Fa_I + _C_IB * _T_B) / _MASS; // conservation of linear momentum - // _q_dot = _q.derivative1(_w_B); // attitude differential - _dq = Quatf::expq(0.5f * dt * _w_B); - _w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum + const Vector3f weight_E = _MASS * gravity_acceleration_E; + Vector3f sum_of_forces_E = _Fa_E + _q_E.rotateVector(_T_B) + weight_E; // fake ground, avoid free fall - if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) { - if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { - if (!_grounded) { // if we just hit the floor - // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_I_dot = -_v_I / dt; + const float force_down = Vector3f(_R_N2E.transpose() * sum_of_forces_E)(2); + Vector3f ground_force_E; - } else { - _v_I_dot.setZero(); + if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) { + if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) { + ground_force_E = -sum_of_forces_E; + + if (!_grounded) { + // if we just hit the floor + // compute the force that will stop the vehicle in one time step + ground_force_E += -_v_E / dt * _MASS; } - _v_I.setZero(); - _w_B.setZero(); _grounded = true; } else if (_vehicle == VehicleType::FW) { - if (!_grounded) { // if we just hit the floor - // for the accelerometer, compute the acceleration that will stop the vehicle in one time step - _v_I_dot(2) = -_v_I(2) / dt; + Vector3f down_u = _R_N2E.col(2); + ground_force_E = -down_u * sum_of_forces_E * down_u; - } else { - // we only allow negative acceleration in order to takeoff - _v_I_dot(2) = fminf(_v_I_dot(2), 0.0f); + if (!_grounded) { + // if we just hit the floor + // compute the force that will stop the vehicle in one time step + ground_force_E += down_u * (-_v_N(2) / dt) * _MASS; } - // integration: Euler forward - _p_I = _p_I + _p_I_dot * dt; - _v_I = _v_I + _v_I_dot * dt; - Eulerf RPY = Eulerf(_q); - RPY(0) = 0.0f; // no roll - RPY(1) = radians(0.0f); // pitch slightly up if needed to get some lift - _q = Quatf(RPY); - _w_B.setZero(); _grounded = true; } } else { - // integration: Euler forward - _p_I = _p_I + _p_I_dot * dt; - _v_I = _v_I + _v_I_dot * dt; - _q = _q * _dq; - _q.normalize(); - // integration Runge-Kutta 4 - // rk4_update(_p_I, _v_I, _q, _w_B); - _w_B = constrain(_w_B + _w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); _grounded = false; } + + sum_of_forces_E += ground_force_E; + const Vector3f acceleration_E = sum_of_forces_E / _MASS; + _specific_force_E = acceleration_E - gravity_acceleration_E; + + _v_E_dot = acceleration_E + coriolis_acceleration_E; + + // add fictitious transport rate acceleration as the local navigation frame rotates + // to stay tangent to the ellipsoid + const Vector3f transport_rate = -_lla.computeAngularRateNavFrame(_v_N).cross(_v_N); + _v_N_dot = _R_N2E.transpose() * _v_E_dot + transport_rate; + + // forward Euler velocity intergation + Vector3f v_E_prev = _v_E; + _v_E = _v_E + _v_E_dot * dt; + // trapezoidal position integration + _p_E = _p_E + Vector3d(_v_E + v_E_prev) * 0.5 * static_cast(dt); + + const Quatf dq(AxisAnglef(_w_B * dt)); + + _q_E = _q_E * dq; + _q_E.normalize(); + + const Vector3f w_B_dot = _Im1 * (_Mt_B + _Ma_B - _w_B.cross(_I * _w_B)); // conservation of angular momentum + _w_B = constrain(_w_B + w_B_dot * dt, -6.0f * M_PI_F, 6.0f * M_PI_F); + + ecefToNed(); + + _lpos_ref.project(_lla.latitude_deg(), _lla.longitude_deg(), _lpos(0), _lpos(1)); + _lpos(2) = -(_lla.altitude() - _lpos_ref_alt); +} + +void Sih::ecefToNed() +{ + _lla = LatLonAlt::fromEcef(_p_E); + + const Dcmf C_SE = computeRotEcefToNed(_lla); + _R_N2E = C_SE.transpose(); + + // Transform velocity to NED frame + _v_N = C_SE * _v_E; + _q = Quatf(C_SE) * _q_E; + _q.normalize(); +} + +Dcmf Sih::computeRotEcefToNed(const LatLonAlt &lla) +{ + // Calculate the ECEF to NED coordinate transformation matrix + const double cos_lat = cos(lla.latitude_rad()); + const double sin_lat = sin(lla.latitude_rad()); + const double cos_lon = cos(lla.longitude_rad()); + const double sin_lon = sin(lla.longitude_rad()); + + const float val[] = {(float)(-sin_lat * cos_lon), (float)(-sin_lat * sin_lon), (float)cos_lat, + (float) - sin_lon, (float)cos_lon, 0.f, + (float)(-cos_lat * cos_lon), (float)(-cos_lat * sin_lon), (float) - sin_lat + }; + + return Dcmf(val); } void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) @@ -437,11 +522,28 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us) // In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. // IMU - Vector3f acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f); - Vector3f gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f); + const Dcmf R_E2B(_q_E.inversed()); + Vector3f accel_noise; + Vector3f gyro_noise; + + if (_T_B.longerThan(FLT_EPSILON)) { + accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f); + gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f); + + } else { + // Lower noise when not armed + accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f); + gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f); + } + + Vector3f specific_force_B = R_E2B * _specific_force_E; + Vector3f accel = specific_force_B + accel_noise; + + const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE); + Vector3f gyro = _w_B + earth_spin_rate_B + gyro_noise; // update IMU every iteration - _px4_accel.update(time_now_us, acc(0), acc(1), acc(2)); + _px4_accel.update(time_now_us, accel(0), accel(1), accel(2)); _px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2)); } @@ -450,10 +552,10 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; - // airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter - airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f); + + // regardless of vehicle type, body frame, etc this holds as long as wind=0 + airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); - airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); @@ -468,7 +570,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; distance_sensor_s distance_sensor{}; - //distance_sensor.timestamp_sample = time_now_us; + // distance_sensor.timestamp_sample = time_now_us; distance_sensor.device_id = device_id.devid; distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; @@ -480,7 +582,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) distance_sensor.current_distance = _distance_snsr_override; } else { - distance_sensor.current_distance = -_p_I(2) / _C_IB(2, 2); + distance_sensor.current_distance = -_lpos(2) / _q.dcm_z()(2); if (distance_sensor.current_distance > _distance_snsr_max) { // this is based on lightware lw20 behaviour @@ -525,25 +627,26 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.v_xy_valid = true; local_position.v_z_valid = true; - local_position.x = _p_I(0); - local_position.y = _p_I(1); - local_position.z = _p_I(2); + local_position.x = _lpos(0); + local_position.y = _lpos(1); + local_position.z = _lpos(2); - local_position.vx = _v_I(0); - local_position.vy = _v_I(1); - local_position.vz = _v_I(2); - local_position.z_deriv = _v_I(2); + local_position.vx = _v_N(0); + local_position.vy = _v_N(1); + local_position.vz = _v_N(2); - local_position.ax = _v_I_dot(0); - local_position.ay = _v_I_dot(1); - local_position.az = _v_I_dot(2); + local_position.z_deriv = _v_N(2); + + local_position.ax = _v_N_dot(0); + local_position.ay = _v_N_dot(1); + local_position.az = _v_N_dot(2); local_position.xy_global = true; local_position.z_global = true; local_position.ref_timestamp = _last_run; - local_position.ref_lat = _LAT0; - local_position.ref_lon = _LON0; - local_position.ref_alt = _H0; + local_position.ref_lat = _lpos_ref.getProjectionReferenceLat(); + local_position.ref_lon = _lpos_ref.getProjectionReferenceLon(); + local_position.ref_alt = _lpos_ref_alt; local_position.heading = Eulerf(_q).psi(); local_position.heading_good_for_control = true; @@ -557,11 +660,11 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) // publish global position groundtruth vehicle_global_position_s global_position{}; global_position.timestamp_sample = time_now_us; - global_position.lat = _LAT0 + degrees((double)_p_I(0) / CONSTANTS_RADIUS_OF_EARTH);; - global_position.lon = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0;; - global_position.alt = _H0 - _p_I(2);; + global_position.lat = _lla.latitude_deg(); + global_position.lon = _lla.longitude_deg(); + global_position.alt = _lla.altitude(); global_position.alt_ellipsoid = global_position.alt; - global_position.terrain_alt = -_p_I(2); + global_position.terrain_alt = -_lpos(2); global_position.timestamp = hrt_absolute_time(); _global_position_ground_truth_pub.publish(global_position); } @@ -619,13 +722,16 @@ int Sih::print_status() PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa()))); PX4_INFO("v segment (m/s)"); _ts[4].get_vS().print(); + + } else if (_vehicle == VehicleType::SVTOL) { + PX4_INFO("Running Standard VTOL"); } PX4_INFO("vehicle landed: %d", _grounded); - PX4_INFO("inertial position NED (m)"); - _p_I.print(); - PX4_INFO("inertial velocity NED (m/s)"); - _v_I.print(); + PX4_INFO("local position NED (m)"); + _lpos.print(); + PX4_INFO("local velocity NED (m/s)"); + _v_N.print(); PX4_INFO("attitude roll-pitch-yaw (deg)"); (Eulerf(_q) * 180.0f / M_PI_F).print(); PX4_INFO("angular acceleration roll-pitch-yaw (deg/s)"); @@ -633,8 +739,8 @@ int Sih::print_status() PX4_INFO("actuator signals"); Vector u = Vector(_u); u.transpose().print(); - PX4_INFO("Aerodynamic forces NED inertial (N)"); - _Fa_I.print(); + PX4_INFO("Aerodynamic forces NED (N)"); + (_R_N2E.transpose() * _Fa_E).print(); PX4_INFO("Aerodynamic moments body frame (Nm)"); _Ma_B.print(); PX4_INFO("Thruster moments in body frame (Nm)"); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 8988474e01..51f6e17dae 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -65,6 +65,8 @@ #include // to get the real time #include #include +#include +#include #include #include #include @@ -132,7 +134,7 @@ private: uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; // hard constants - static constexpr uint16_t NB_MOTORS = 6; + static constexpr uint16_t NUM_ACTUATORS_MAX = 9; static constexpr float T1_C = 15.0f; // ground temperature in Celsius static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre @@ -159,9 +161,21 @@ private: void send_airspeed(const hrt_abstime &time_now_us); void send_dist_snsr(const hrt_abstime &time_now_us); void publish_ground_truth(const hrt_abstime &time_now_us); - void generate_fw_aerodynamics(); + void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); void generate_ts_aerodynamics(); void sensor_step(); + static float computeGravity(double lat); + + void ecefToNed(); + static matrix::Dcmf computeRotEcefToNed(const LatLonAlt &lla); + + struct Wgs84 { + static constexpr double equatorial_radius = 6378137.0; + static constexpr double eccentricity = 0.0818191908425; + static constexpr double eccentricity2 = eccentricity * eccentricity; + static constexpr double gravity_equator = 9.7803253359; + }; + #if defined(ENABLE_LOCKSTEP_SCHEDULER) void lockstep_loop(); @@ -185,22 +199,33 @@ private: bool _grounded{true};// whether the vehicle is on the ground matrix::Vector3f _T_B{}; // thrust force in body frame [N] - matrix::Vector3f _Fa_I{}; // aerodynamic force in inertial frame [N] matrix::Vector3f _Mt_B{}; // thruster moments in the body frame [Nm] matrix::Vector3f _Ma_B{}; // aerodynamic moments in the body frame [Nm] - matrix::Vector3f _p_I{}; // inertial position [m] - matrix::Vector3f _v_I{}; // inertial velocity [m/s] - matrix::Vector3f _v_B{}; // body frame velocity [m/s] - matrix::Vector3f _p_I_dot{}; // inertial position differential - matrix::Vector3f _v_I_dot{}; // inertial velocity differential - matrix::Quatf _q{}; // quaternion attitude - matrix::Dcmf _C_IB{}; // body to inertial transformation + matrix::Vector3f _lpos{}; // position in a local tangent-plane frame [m] + matrix::Vector3f _v_N{}; // velocity in local navigation frame (NED, body-fixed) [m/s] + matrix::Vector3f _v_N_dot{}; // time derivative of velocity in local navigation frame [m/s2] + matrix::Quatf _q{}; // quaternion attitude in local navigation frame matrix::Vector3f _w_B{}; // body rates in body frame [rad/s] - matrix::Quatf _dq{}; // quaternion differential - matrix::Vector3f _w_B_dot{}; // body rates differential - float _u[NB_MOTORS] {}; // thruster signals - enum class VehicleType {MC, FW, TS}; + LatLonAlt _lla{}; + + // Quantities in Earth-centered-Earth-fixed coordinates + matrix::Vector3f _Fa_E{}; // aerodynamic force in ECEF frame [N] + matrix::Vector3f _specific_force_E{}; + matrix::Quatf _q_E{}; + matrix::Vector3d _p_E{}; + matrix::Vector3f _v_E{}; + matrix::Vector3f _v_E_dot{}; + matrix::Dcmf _R_N2E; // local navigation to ECEF frame rotation matrix + + float _u[NUM_ACTUATORS_MAX] {}; // thruster signals + + // MC = Multicopter + // FW = Fixed Wing + // TS = Tailsitter VTOL + // SVTOL = Standard VTOL + enum class VehicleType {MC, FW, TS, SVTOL}; + VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing @@ -218,7 +243,7 @@ private: static constexpr const float TS_CM = 0.115f; // longitudinal position of the CM from trailing edge static constexpr const float TS_RP = 0.0625f; // propeller radius [m] static constexpr const float TS_DEF_MAX = math::radians(39.0f); // max deflection - matrix::Dcmf _C_BS = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch + matrix::Dcmf _R_S2B = matrix::Dcmf(matrix::Eulerf(0.0f, math::radians(90.0f), 0.0f)); // segment to body 90 deg pitch AeroSeg _ts[NB_TS_SEG] = { AeroSeg(0.0225f, 0.110f, 0.0f, matrix::Vector3f(0.083f - TS_CM, -0.239f, 0.0f), 0.0f, TS_AR), AeroSeg(0.0383f, 0.125f, 0.0f, matrix::Vector3f(0.094f - TS_CM, -0.208f, 0.0f), 0.0f, TS_AR, 0.063f), @@ -248,9 +273,9 @@ private: // }; // parameters - float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0, _T_TAU; - double _LAT0, _LON0, _COS_LAT0; - matrix::Vector3f _W_I; // weight of the vehicle in inertial frame [N] + MapProjection _lpos_ref{}; + float _lpos_ref_alt; + float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU; matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 16c58430f6..f9e053a5eb 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -332,6 +332,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @value 0 Multicopter * @value 1 Fixed-Wing * @value 2 Tailsitter + * @value 3 Standard VTOL * @reboot_required true * @group Simulation In Hardware */ diff --git a/src/modules/simulation/system_power_simulator/CMakeLists.txt b/src/modules/simulation/system_power_simulator/CMakeLists.txt new file mode 100644 index 0000000000..1168e557cd --- /dev/null +++ b/src/modules/simulation/system_power_simulator/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__simulation__system_power_simulator + MAIN system_power_simulator + COMPILE_FLAGS + SRCS + SystemPowerSimulator.cpp + SystemPowerSimulator.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/simulation/system_power_simulator/Kconfig b/src/modules/simulation/system_power_simulator/Kconfig new file mode 100644 index 0000000000..498e8a989c --- /dev/null +++ b/src/modules/simulation/system_power_simulator/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SIMULATION_SYSTEM_POWER_SIMULATOR + bool "system_power_simulator" + default n + ---help--- + Enable support for system_power_simulator diff --git a/src/modules/simulation/system_power_simulator/SystemPowerSimulator.cpp b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.cpp new file mode 100644 index 0000000000..2c7671c2d2 --- /dev/null +++ b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 "SystemPowerSimulator.hpp" + +SystemPowerSimulator::SystemPowerSimulator() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +SystemPowerSimulator::~SystemPowerSimulator() +{ + perf_free(_loop_perf); +} + +bool SystemPowerSimulator::init() +{ + ScheduleOnInterval(SYSTEM_POWER_SIMLATOR_SAMPLE_INTERVAL_US); + return true; +} + +void SystemPowerSimulator::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + const hrt_abstime now_us = hrt_absolute_time(); + + // system power + system_power_s sim_system_power{}; + + sim_system_power.timestamp = now_us; + sim_system_power.voltage5v_v = 5.f; + sim_system_power.usb_connected = false; + sim_system_power.hipower_5v_oc = false; + sim_system_power.periph_5v_oc = false; + sim_system_power.brick_valid = 1; + + _system_power_pub.publish(sim_system_power); + + perf_end(_loop_perf); +} + +int SystemPowerSimulator::task_spawn(int argc, char *argv[]) +{ + SystemPowerSimulator *instance = new SystemPowerSimulator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SystemPowerSimulator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int SystemPowerSimulator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("system_power_simulation", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int system_power_simulator_main(int argc, char *argv[]) +{ + return SystemPowerSimulator::main(argc, argv); +} diff --git a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.hpp similarity index 55% rename from src/lib/avoidance/ObstacleAvoidance_dummy.hpp rename to src/modules/simulation/system_power_simulator/SystemPowerSimulator.hpp index 7831c919fd..476eb0afcb 100644 --- a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp +++ b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,63 +31,43 @@ * ****************************************************************************/ -/** - * @file ObstacleAvoidance_dummy.hpp - * This is a dummy class to reduce flash space for when obstacle avoidance is not required - * - * @author Julian Kent - */ - #pragma once +#include #include -#include +#include +#include +#include +#include +#include +#include +using namespace time_literals; -#include - - -class ObstacleAvoidance +class SystemPowerSimulator : public ModuleBase, public px4::ScheduledWorkItem { public: - ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor + SystemPowerSimulator(); + ~SystemPowerSimulator() override; + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) - { - notify_dummy(); - }; + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type) - { - notify_dummy(); - }; + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, - const int type) - { - notify_dummy(); - } + bool init(); +private: + void Run() override; - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt) - { - notify_dummy(); - }; + static constexpr uint32_t SYSTEM_POWER_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz + static constexpr uint32_t SYSTEM_POWER_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / SYSTEM_POWER_SIMLATOR_SAMPLE_FREQUENCY_HZ; -protected: + uORB::Publication _system_power_pub{ORB_ID(system_power)}; - bool _logged_error = false; - void notify_dummy() - { - if (!_logged_error) { - PX4_ERR("Dummy avoidance library called!"); - _logged_error = true; - } - } + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; }; diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt new file mode 100644 index 0000000000..88a18e9c01 --- /dev/null +++ b/src/modules/spacecraft/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# Copyright (c) 2019 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_directories(${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_module( + MODULE modules__spacecraft + MAIN spacecraft + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + STACK_MAIN + 3000 + SRCS + SpacecraftHandler.cpp + SpacecraftHandler.hpp + MODULE_CONFIG + module.yaml + DEPENDS + mathlib + px4_work_queue +) diff --git a/src/modules/spacecraft/Kconfig b/src/modules/spacecraft/Kconfig new file mode 100644 index 0000000000..67b77c3b7d --- /dev/null +++ b/src/modules/spacecraft/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SPACECRAFT + bool "SPACECRAFT" + default n + ---help--- + Enable support for spacecraft + +menuconfig USER_SPACECRAFT + bool "spacecraft running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_SPACECRAFT + ---help--- + Put SPACECRAFT in userspace memory diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp new file mode 100644 index 0000000000..92cdf86655 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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. + * + ****************************************************************************/ + +/** + * @file SpacecraftHandler.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "SpacecraftHandler.hpp" + +int SpacecraftHandler::task_spawn(int argc, char *argv[]) +{ + return 0; +} + +int SpacecraftHandler::print_status() +{ + PX4_INFO("Running"); + + return 0; +} + +int SpacecraftHandler::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftHandler::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + This implements control allocation for spacecraft vehicles. + It takes torque and thrust setpoints as inputs and outputs + actuator setpoint messages. + )DESCR_STR" + ); + + PRINT_MODULE_USAGE_NAME("spacecraft", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int spacecraft_main(int argc, char *argv[]); + +int spacecraft_main(int argc, char *argv[]) +{ + return SpacecraftHandler::main(argc, argv); +} diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp new file mode 100644 index 0000000000..8fc954dc71 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.hpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + SpacecraftHandler(); + + virtual ~SpacecraftHandler(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: /**< loop duration performance counter */ + +}; diff --git a/src/modules/spacecraft/module.yaml b/src/modules/spacecraft/module.yaml new file mode 100644 index 0000000000..9dc0d1620b --- /dev/null +++ b/src/modules/spacecraft/module.yaml @@ -0,0 +1,235 @@ +__max_num_mc_motors: &max_num_mc_motors 12 +__max_num_thrusters: &max_num_thrusters 12 +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 + +module_name: Control Allocation + +parameters: + - group: Geometry + definitions: + CA_AIRFRAME: + description: + short: Airframe selection + long: | + Defines which mixer implementation to use. + Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators. + + 'Custom' should only be used if noting else can be used. + type: enum + values: + 0: Multirotor + 1: Fixed-wing + 2: Standard VTOL + 3: Tiltrotor VTOL + 4: Tailsitter VTOL + 5: Rover (Ackermann) + 6: Rover (Differential) + 7: Motors (6DOF) + 8: Multirotor with Tilt + 9: Custom + 10: Helicopter (tail ESC) + 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) + 13: Rover (Mecanum) + 14: Spacecraft 2D + 15: Spacecraft 3D + default: 14 + + CA_METHOD: + description: + short: Control allocation method + long: | + Selects the algorithm and desaturation method. + If set to Automtic, the selection is based on the airframe (CA_AIRFRAME). + type: enum + values: + 0: Pseudo-inverse with output clipping + 1: Pseudo-inverse with metric allocation + 2: Automatic + default: 1 + + CA_R_REV: + description: + short: Bidirectional/Reversible motors + long: | + Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well. + type: bitmask + bit: + 0: Motor 1 + 1: Motor 2 + 2: Motor 3 + 3: Motor 4 + 4: Motor 5 + 5: Motor 6 + 6: Motor 7 + 7: Motor 8 + 8: Motor 9 + 9: Motor 10 + 10: Motor 11 + 11: Motor 12 + default: 0 + + # (SC) Thrusters + CA_THRUSTER_CNT: + description: + short: Total number of thrusters + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + 9: '9' + 10: '10' + 11: '11' + 12: '12' + default: 0 + CA_THRUSTER${i}_PX: + description: + short: Position of thruster ${i} along X body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PY: + description: + short: Position of thruster ${i} along Y body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PZ: + description: + short: Position of thruster ${i} along Z body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AX: + description: + short: Axis of thruster ${i} thrust vector, X body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AY: + description: + short: Axis of thruster ${i} thrust vector, Y body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AZ: + description: + short: Axis of thruster ${i} thrust vector, Z body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: -1.0 + + CA_THRUSTER${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between actuator minimum and maximum) + is the output signal sent to the motor controller. + type: float + decimal: 1 + increment: 1 + num_instances: *max_num_thrusters + min: 0 + max: 100 + default: 6.5 + +# Mixer +mixer: + actuator_types: + motor: + functions: 'Motor' + actuator_testing_values: + min: 0 + max: 1 + default_is_nan: true + DEFAULT: + actuator_testing_values: + min: -1 + max: 1 + default: -1 + + config: + param: CA_AIRFRAME + types: + 14: # Spacecraft 2D + title: 'Spacecraft 2D' + actuators: + - actuator_type: 'motor' + count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true + + 15: # Sapcecraft 3D + title: 'Spacecraft 3D' + actuators: + - actuator_type: 'motor' + count: 'CA_THRUSTER_CNT' + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true diff --git a/src/modules/time_persistor/CMakeLists.txt b/src/modules/time_persistor/CMakeLists.txt new file mode 100644 index 0000000000..add51551b4 --- /dev/null +++ b/src/modules/time_persistor/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__time_persistor + MAIN time_persistor + COMPILE_FLAGS + SRCS + TimePersistor.cpp + TimePersistor.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/time_persistor/Kconfig b/src/modules/time_persistor/Kconfig new file mode 100644 index 0000000000..da75222f49 --- /dev/null +++ b/src/modules/time_persistor/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_TIME_PERSISTOR + bool "time_persistor" + default n + ---help--- + Enable support for time_persistor diff --git a/src/modules/time_persistor/TimePersistor.cpp b/src/modules/time_persistor/TimePersistor.cpp new file mode 100644 index 0000000000..92d310c14c --- /dev/null +++ b/src/modules/time_persistor/TimePersistor.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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 "TimePersistor.hpp" + +#include +#include + +TimePersistor::TimePersistor() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +TimePersistor::~TimePersistor() +{ + fclose(_file); +} + +void TimePersistor::start() +{ + ScheduleOnInterval(1_s); +} + +void TimePersistor::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + if (!_file) { + if (init() != PX4_OK) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + } + + struct timespec ts; + + px4_clock_gettime(CLOCK_REALTIME, &ts); + + if (write_time(ts.tv_sec) != PX4_OK) { + PX4_ERR("error writing RTC to time file"); + exit_and_cleanup(); + return; + } +} + +int TimePersistor::task_spawn(int argc, char *argv[]) +{ + TimePersistor *instance = new TimePersistor(); + + if (!instance) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->start(); + return PX4_OK; +} + +int TimePersistor::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int TimePersistor::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Writes the RTC time cyclically to a file and reloads this value on startup. +This allows monotonic time on systems that only have a software RTC (that is not battery powered). +Explicitly setting the time backwards (e.g. via system_time) is still possible. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("time_persistor", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int time_persistor_main(int argc, char *argv[]) +{ + return TimePersistor::main(argc, argv); +} + +int TimePersistor::init() +{ + if ((_file = fopen(TIME_FILE_PATH, "r+")) == nullptr) { + _file = fopen(TIME_FILE_PATH, "w+"); + } + + if (!_file) { + PX4_ERR("error opening the time file"); + return PX4_ERROR; + } + + time_t t; + + if (read_time(&t) == PX4_OK) { + struct timespec ts; + ts.tv_sec = t; + ts.tv_nsec = 0; + + px4_clock_settime(CLOCK_REALTIME, &ts); + } + + return PX4_OK; +} + +int TimePersistor::read_time(time_t *time) +{ + int status = PX4_OK; + + status |= fseek(_file, 0, SEEK_SET); + status |= fread(time, sizeof(time_t), 1, _file) != 1; + + return status; +} + +int TimePersistor::write_time(const time_t time) +{ + int status = PX4_OK; + + status |= fseek(_file, 0, SEEK_SET); + status |= fwrite(&time, sizeof(time_t), 1, _file) != 1; + status |= fflush(_file); + status |= fsync(fileno(_file)); + + return status; +} diff --git a/src/modules/time_persistor/TimePersistor.hpp b/src/modules/time_persistor/TimePersistor.hpp new file mode 100644 index 0000000000..8aeef085b5 --- /dev/null +++ b/src/modules/time_persistor/TimePersistor.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include + +static constexpr const auto TIME_FILE_PATH = PX4_STORAGEDIR "/time_save.bin"; + +using namespace time_literals; + +class TimePersistor : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + TimePersistor(); + ~TimePersistor() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void Run() override; + + void start(); + +private: + int init(); + int read_time(time_t *time); + int write_time(const time_t time); + + FILE *_file = 0; +}; diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp index d04f21cb11..7559ef4701 100644 --- a/src/modules/uuv_att_control/uuv_att_control.hpp +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index 4d94ac9cd9..192365a98d 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.hpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/uxrce_dds_client/CMakeLists.txt b/src/modules/uxrce_dds_client/CMakeLists.txt index 60c390892b..e5fc481543 100644 --- a/src/modules/uxrce_dds_client/CMakeLists.txt +++ b/src/modules/uxrce_dds_client/CMakeLists.txt @@ -31,11 +31,15 @@ # ############################################################################ -if(${CMAKE_VERSION} VERSION_LESS "3.11") - message(WARNING "skipping uxrce_dds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}") +if(${CMAKE_VERSION} VERSION_LESS_EQUAL "3.15") + message(WARNING "skipping uxrce_dds_client, Micro-XRCE-DDS-Client needs to be fixed to work with CMAKE_VERSION ${CMAKE_VERSION}") else() - px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${CMAKE_CURRENT_LIST_DIR}/Micro-XRCE-DDS-Client") + + set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client) + set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR}) + + px4_add_git_submodule(TARGET git_micro_xrce_dds_client PATH "${microxrceddsclient_src_dir}") # If a toolchain file is given, explicitly define its path when building the Micro-XRCE-DDS-CLient if(CMAKE_TOOLCHAIN_FILE MATCHES "cmake$") @@ -58,9 +62,6 @@ else() set(lib_dir "lib") endif() - set(microxrceddsclient_src_dir ${CMAKE_CURRENT_SOURCE_DIR}/Micro-XRCE-DDS-Client) - set(microxrceddsclient_build_dir ${CMAKE_CURRENT_BINARY_DIR}) - include(ExternalProject) ExternalProject_Add( @@ -114,10 +115,8 @@ else() target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include) - add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py - --topic-msg-dir ${PX4_SOURCE_DIR}/msg --client-outdir ${CMAKE_CURRENT_BINARY_DIR} --dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml --template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em @@ -148,6 +147,8 @@ else() srv_base.cpp srv_base.h DEPENDS + git_micro_xrce_dds_client + libmicroxrceddsclient_project microxrceddsclient libmicroxrceddsclient libmicrocdr diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index 69c8fc58cc..81eadef1bb 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -39,11 +39,31 @@ static constexpr int max_topic_size = 512; static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too large, increase max_topic_size"); @[ end for]@ + + +// SFINAE to use R::MESSAGE_VERSION if it exists, and 0 otherwise +template +class MessageVersionHelper +{ + template + static constexpr uint32_t get(decltype(&C::MESSAGE_VERSION)) { return C::MESSAGE_VERSION; } + template + static constexpr uint32_t get(...) { return 0; } +public: + static constexpr uint32_t m = get(0); +}; + +template +static constexpr uint32_t get_message_version() { + return MessageVersionHelper::m; +} + struct SendSubscription { const struct orb_metadata *orb_meta; uxrObjectId data_writer; const char* dds_type_name; const char* topic; + uint32_t message_version; uint32_t topic_size; UcdrSerializeMethod ucdr_serialize_method; }; @@ -56,6 +76,7 @@ struct SendTopicsSubs { uxr_object_id(0, UXR_INVALID_ID), "@(pub['dds_type'])", "@(pub['topic'])", + get_message_version<@(pub['simple_base_type'])_s>(), ucdr_topic_size_@(pub['simple_base_type'])(), &ucdr_serialize_@(pub['simple_base_type']), }, @@ -101,6 +122,7 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) { // data writer not created yet create_data_writer(session, reliable_out_stream_id, participant_id, static_cast(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].topic, + send_subscriptions[idx].message_version, send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); } @@ -173,7 +195,8 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id @[ for idx, sub in enumerate(subscriptions + subscriptions_multi)]@ { uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal - create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic'])", "@(sub['dds_type'])", queue_depth); + uint32_t message_version = get_message_version<@(sub['simple_base_type'])_s>(); + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic'])", message_version, "@(sub['dds_type'])", queue_depth); } @[ end for]@ diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index e9e67cff25..bbd0d8c631 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -71,8 +71,8 @@ publications: - topic: /fmu/out/vehicle_status type: px4_msgs::msg::VehicleStatus - - topic: /fmu/out/vehicle_trajectory_waypoint_desired - type: px4_msgs::msg::VehicleTrajectoryWaypoint + - topic: /fmu/out/airspeed_validated + type: px4_msgs::msg::AirspeedValidated # Create uORB::Publication subscriptions: @@ -97,6 +97,9 @@ subscriptions: - topic: /fmu/in/config_control_setpoints type: px4_msgs::msg::VehicleControlMode + - topic: /fmu/in/distance_sensor + type: px4_msgs::msg::DistanceSensor + - topic: /fmu/in/manual_control_input type: px4_msgs::msg::ManualControlSetpoint @@ -139,12 +142,6 @@ subscriptions: - topic: /fmu/in/vehicle_command_mode_executor type: px4_msgs::msg::VehicleCommand - - topic: /fmu/in/vehicle_trajectory_bezier - type: px4_msgs::msg::VehicleTrajectoryBezier - - - topic: /fmu/in/vehicle_trajectory_waypoint - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/in/vehicle_thrust_setpoint type: px4_msgs::msg::VehicleThrustSetpoint diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 4052958df8..78e3bf3b93 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -43,9 +43,6 @@ import em import yaml parser = argparse.ArgumentParser() -parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, - help="Topics message, by default using relative path 'msg/'", default="msg") - parser.add_argument("-y", "--dds-topics-file", dest='yaml_file', type=str, help="Setup topics file path, by default using 'dds_topics.yaml'") diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 4e5db21160..773475f1f5 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -105,3 +105,27 @@ parameters: category: System reboot_required: true default: 0 + + UXRCE_DDS_TX_TO: + description: + short: TX rate timeout configuration + long: | + Specifies after how many seconds without sending data the DDS connection is reestablished. + A value less than one disables the TX rate timeout. + type: int32 + category: System + reboot_required: true + default: 3 + unit: s + + UXRCE_DDS_RX_TO: + description: + short: RX rate timeout configuration + long: | + Specifies after how many seconds without receiving data the DDS connection is reestablished. + A value less than one disables the RX rate timeout. + type: int32 + category: System + reboot_required: true + default: -1 + unit: s diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 70886ec795..c2d038b3ed 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -23,29 +23,40 @@ uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) return uxrObjectId{}; } -static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic) +static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic, + uint32_t message_version = 0) { if (topic[0] == '/') { topic++; } + char version[16]; + + if (message_version != 0) { + snprintf(version, sizeof(version), "_v%u", (unsigned)message_version); + version[sizeof(version) - 1] = '\0'; + + } else { + version[0] = '\0'; + } + if (client_namespace != nullptr) { - int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s", client_namespace, topic); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s%s", client_namespace, topic, version); return (ret > 0 && ret < TOPIC_NAME_SIZE); } - int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s", topic); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s%s", topic, version); return (ret > 0 && ret < TOPIC_NAME_SIZE); } static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id, - ORB_ID orb_id, const char *client_namespace, const char *topic, const char *type_name, + ORB_ID orb_id, const char *client_namespace, const char *topic, uint32_t message_version, const char *type_name, uxrObjectId &datawriter_id) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, topic)) { + if (!generate_topic_name(topic_name, client_namespace, topic, message_version)) { PX4_ERR("topic path too long"); return false; } @@ -92,12 +103,13 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic, + uint32_t message_version, const char *type_name, uint16_t queue_depth) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, topic)) { + if (!generate_topic_name(topic_name, client_namespace, topic, message_version)) { PX4_ERR("topic path too long"); return false; } diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 690ae53af1..b550912791 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -209,7 +209,7 @@ void UxrceddsClient::deinit() _comm = nullptr; } -bool UxrceddsClient::setup_session(uxrSession *session) +bool UxrceddsClient::setupSession(uxrSession *session) { _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); _synchronize_timestamps = (_param_uxrce_dds_synct.get() > 0); @@ -222,6 +222,7 @@ bool UxrceddsClient::setup_session(uxrSession *session) } if (!got_response) { + PX4_ERR("got no ping from agent"); return false; } @@ -241,6 +242,8 @@ bool UxrceddsClient::setup_session(uxrSession *session) return false; } + _session_created = true; + // Streams // Reliable for setup, afterwards best-effort to send the data (important: need to create all 4 streams) _reliable_out = uxr_create_output_reliable_stream(session, _output_reliable_stream_buffer, @@ -351,6 +354,7 @@ bool UxrceddsClient::setup_session(uxrSession *session) } if (sync_timeouts > TIMESYNC_MAX_TIMEOUTS) { + PX4_ERR("timeout during time synchronization"); return false; } @@ -375,13 +379,21 @@ bool UxrceddsClient::setup_session(uxrSession *session) return true; } -void UxrceddsClient::delete_session(uxrSession *session) +void UxrceddsClient::deleteSession(uxrSession *session) { delete_repliers(); - uxr_delete_session_retries(session, _connected ? 1 : 0); + + if (_session_created) { + uxr_delete_session_retries(session, _connected ? 1 : 0); + _session_created = false; + } + + if (_subs_initialized) { + _subs->reset(); + _subs_initialized = false; + } _last_payload_tx_rate = 0; - _subs->reset(); _timesync.reset_filter(); } @@ -460,6 +472,20 @@ static void fillMessageFormatResponse(const message_format_request_s &message_fo message_format_response.timestamp = hrt_absolute_time(); } +void UxrceddsClient::calculateTxRxRate() +{ + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_status_update > 1_s) { + float dt = (now - _last_status_update) / 1e6f; + _last_payload_tx_rate = (_subs->num_payload_sent - _last_num_payload_sent) / dt; + _last_payload_rx_rate = (_pubs->num_payload_received - _last_num_payload_received) / dt; + _last_num_payload_sent = _subs->num_payload_sent; + _last_num_payload_received = _pubs->num_payload_received; + _last_status_update = now; + } +} + void UxrceddsClient::handleMessageFormatRequest() { message_format_request_s message_format_request; @@ -471,6 +497,87 @@ void UxrceddsClient::handleMessageFormatRequest() } } +void UxrceddsClient::checkConnectivity(uxrSession *session) +{ + // Reset TX zero counter, when data is sent + if (_last_payload_tx_rate > 0) { + _num_tx_rate_zero = 0; + } + + // Reset RX zero counter, when data is received + if (_last_payload_rx_rate > 0) { + _num_rx_rate_zero = 0; + } + + const hrt_abstime now = hrt_absolute_time(); + + // Start ping and tx/rx rate monitoring, unless we're actively sending & receiving payloads successfully + if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { + _connected = true; + _num_pings_missed = 0; + _last_ping = now; + + } else { + if (hrt_elapsed_time(&_last_ping) > 1_s) { + // Check payload tx rate + if (_last_payload_tx_rate == 0) { + _num_tx_rate_zero++; + } + + // Check payload rx rate + if (_last_payload_rx_rate == 0) { + _num_rx_rate_zero++; + } + + // Check ping + _last_ping = now; + + if (_had_ping_reply) { + _num_pings_missed = 0; + + } else { + ++_num_pings_missed; + } + + int timeout_ms = 1'000; // 1 second + uint8_t attempts = 1; + uxr_ping_agent_session(session, timeout_ms, attempts); + + _had_ping_reply = false; + } + + if (_num_pings_missed >= 3) { + PX4_ERR("No ping response, disconnecting"); + _connected = false; + } + + int32_t tx_timeout = _param_uxrce_dds_tx_to.get(); + int32_t rx_timeout = _param_uxrce_dds_rx_to.get(); + + if (tx_timeout > 0 && _num_tx_rate_zero >= tx_timeout) { + PX4_ERR("Payload TX rate zero for too long, disconnecting"); + _connected = false; + } + + if (rx_timeout > 0 && _num_rx_rate_zero >= rx_timeout) { + PX4_ERR("Payload RX rate zero for too long, disconnecting"); + _connected = false; + } + } +} + +void UxrceddsClient::resetConnectivityCounters() +{ + _last_status_update = hrt_absolute_time(); + _last_ping = hrt_absolute_time(); + _had_ping_reply = false; + _num_pings_missed = 0; + _last_num_payload_sent = 0; + _last_num_payload_received = 0; + _num_tx_rate_zero = 0; + _num_rx_rate_zero = 0; +} + void UxrceddsClient::syncSystemClock(uxrSession *session) { struct timespec ts = {}; @@ -523,8 +630,8 @@ void UxrceddsClient::run() continue; } - if (!setup_session(&session)) { - delete_session(&session); + if (!setupSession(&session)) { + deleteSession(&session); px4_usleep(1'000'000); PX4_ERR("session setup failed, will retry now"); continue; @@ -540,15 +647,11 @@ void UxrceddsClient::run() } hrt_abstime last_sync_session = 0; - hrt_abstime last_status_update = hrt_absolute_time(); - hrt_abstime last_ping = hrt_absolute_time(); - int num_pings_missed = 0; - bool had_ping_reply = false; - uint32_t last_num_payload_sent{}; - uint32_t last_num_payload_received{}; int poll_error_counter = 0; + resetConnectivityCounters(); _subs->init(); + _subs_initialized = true; while (!should_exit() && _connected) { perf_begin(_loop_perf); @@ -616,55 +719,19 @@ void UxrceddsClient::run() // Check for a ping response /* PONG_IN_SESSION_STATUS */ if (session.on_pong_flag == 1) { - had_ping_reply = true; + _had_ping_reply = true; } - const hrt_abstime now = hrt_absolute_time(); + // Calculate the payload tx/rx rate for connectivity monitoring + calculateTxRxRate(); - if (now - last_status_update > 1_s) { - float dt = (now - last_status_update) / 1e6f; - _last_payload_tx_rate = (_subs->num_payload_sent - last_num_payload_sent) / dt; - _last_payload_rx_rate = (_pubs->num_payload_received - last_num_payload_received) / dt; - last_num_payload_sent = _subs->num_payload_sent; - last_num_payload_received = _pubs->num_payload_received; - last_status_update = now; - } - - // Handle ping, unless we're actively sending & receiving payloads successfully - if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { - _connected = true; - num_pings_missed = 0; - last_ping = now; - - } else { - if (hrt_elapsed_time(&last_ping) > 1_s) { - last_ping = now; - - if (had_ping_reply) { - num_pings_missed = 0; - - } else { - ++num_pings_missed; - } - - int timeout_ms = 1'000; // 1 second - uint8_t attempts = 1; - uxr_ping_agent_session(&session, timeout_ms, attempts); - - had_ping_reply = false; - } - - if (num_pings_missed >= 3) { - PX4_INFO("No ping response, disconnecting"); - _connected = false; - } - } + // Check if there is still connectivity with the agent + checkConnectivity(&session); perf_end(_loop_perf); - } - delete_session(&session); + deleteSession(&session); } } diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index d00571d3f9..823e242f9d 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -120,13 +120,17 @@ private: bool init(); void deinit(); - bool setup_session(uxrSession *session); - void delete_session(uxrSession *session); + bool setupSession(uxrSession *session); + void deleteSession(uxrSession *session); bool setBaudrate(int fd, unsigned baud); void handleMessageFormatRequest(); + void calculateTxRxRate(); + void checkConnectivity(uxrSession *session); + void resetConnectivityCounters(); + uORB::Publication _message_format_response_pub{ORB_ID(message_format_response)}; uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)}; @@ -179,11 +183,21 @@ private: uxrCommunication *_comm{nullptr}; int _fd{-1}; + hrt_abstime _last_status_update; + hrt_abstime _last_ping; + bool _had_ping_reply{false}; + int _num_pings_missed{0}; + int32_t _num_tx_rate_zero{0}; + int32_t _num_rx_rate_zero{0}; + uint32_t _last_num_payload_sent{0}; + uint32_t _last_num_payload_received{0}; int _last_payload_tx_rate{}; ///< in B/s int _last_payload_rx_rate{}; ///< in B/s - bool _connected{false}; + bool _connected{false}; + bool _session_created{false}; bool _timesync_converged{false}; + bool _subs_initialized{false}; Timesync _timesync{timesync_status_s::SOURCE_PROTOCOL_DDS}; @@ -195,6 +209,8 @@ private: (ParamInt) _param_uxrce_key, (ParamInt) _param_uxrce_dds_ptcfg, (ParamInt) _param_uxrce_dds_syncc, - (ParamInt) _param_uxrce_dds_synct + (ParamInt) _param_uxrce_dds_synct, + (ParamInt) _param_uxrce_dds_tx_to, + (ParamInt) _param_uxrce_dds_rx_to ) }; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d8d640790e..5422a01898 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -241,10 +241,11 @@ void Standard::update_transition_state() if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. - pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta(); } const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); + q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3a19042579..9a05cf1034 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -295,7 +295,7 @@ void Tiltrotor::update_transition_state() // control backtransition deceleration using pitch. if (_v_control_mode->flag_control_climb_rate_enabled) { - pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = Eulerf(Quatf(_mc_virtual_att_sp->q_d)).theta(); } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 3bcb56e498..3b8114de3f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -354,7 +354,7 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); * Minimum pitch angle during hover. * * Any pitch setpoint below this value is translated to a forward force by the fixed-wing forward actuation if - * VT_FW_TRHUST_EN is set to 1. + * VT_FWD_TRHUST_EN is set. * * @unit deg * @min -10.0 diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 458b3927fb..e7ec2d3e01 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -168,37 +168,6 @@ void VtolType::update_transition_state() _last_thr_in_mc = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; } -float VtolType::update_and_get_backtransition_pitch_sp() -{ - // maximum up or down pitch the controller is allowed to demand - const float pitch_lim = 0.3f; - const Eulerf euler(Quatf(_v_att->q)); - - const float track = atan2f(_local_pos->vy, _local_pos->vx); - const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay; - - // increase the target deceleration setpoint provided to the controller by 20% - // to make overshooting the transition waypoint less likely in the presence of tracking errors - const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f; - - // get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number) - const float accel_error_forward = dec_sp + accel_body_forward; - - const float pitch_sp_new = _accel_to_pitch_integ; - - float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward; - - if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) || - (pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) { - integrator_input = 0.0f; - } - - _accel_to_pitch_integ += integrator_input * _transition_dt; - - // only allow positive (pitch up) pitch setpoint - return math::constrain(pitch_sp_new, 0.f, pitch_lim); -} - bool VtolType::isFrontTransitionCompleted() { bool ret = isFrontTransitionCompletedBase(); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 8f9ae9ef38..737746a78b 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -317,7 +317,6 @@ protected: bool _quadchute_command_treated{false}; - float update_and_get_backtransition_pitch_sp(); bool isFrontTransitionCompleted(); virtual bool isFrontTransitionCompletedBase(); diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig index c3c042df13..3cce22f026 100644 --- a/src/modules/zenoh/Kconfig +++ b/src/modules/zenoh/Kconfig @@ -46,9 +46,6 @@ if MODULES_ZENOH select ZENOH_PUBSUB_VEHICLE_ODOMETRY select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT select ZENOH_PUBSUB_VEHICLE_COMMAND - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - config ZENOH_PUBSUB_ALL bool "All" diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 19e0be635a..ac96b8586f 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -97,10 +97,6 @@ menu "Zenoh publishers/subscribers" bool "collision_constraints" default n - config ZENOH_PUBSUB_COLLISION_REPORT - bool "collision_report" - default n - config ZENOH_PUBSUB_CONFIG_OVERRIDES bool "config_overrides" default n @@ -593,6 +589,10 @@ menu "Zenoh publishers/subscribers" bool "rover_ackermann_guidance_status" default n + config ZENOH_PUBSUB_ROVER_ACKERMANN_SETPOINT + bool "rover_ackermann_setpoint" + default n + config ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS bool "rover_ackermann_status" default n @@ -741,10 +741,6 @@ menu "Zenoh publishers/subscribers" bool "timesync_status" default n - config ZENOH_PUBSUB_TRAJECTORY_BEZIER - bool "trajectory_bezier" - default n - config ZENOH_PUBSUB_TRAJECTORY_SETPOINT bool "trajectory_setpoint" default n @@ -881,14 +877,6 @@ menu "Zenoh publishers/subscribers" bool "vehicle_torque_setpoint" default n - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - bool "vehicle_trajectory_bezier" - default n - - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - bool "vehicle_trajectory_waypoint" - default n - config ZENOH_PUBSUB_VELOCITY_LIMITS bool "velocity_limits" default n @@ -938,7 +926,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_CAN_INTERFACE_STATUS select ZENOH_PUBSUB_CELLULAR_STATUS select ZENOH_PUBSUB_COLLISION_CONSTRAINTS - select ZENOH_PUBSUB_COLLISION_REPORT select ZENOH_PUBSUB_CONFIG_OVERRIDES select ZENOH_PUBSUB_CONTROL_ALLOCATOR_STATUS select ZENOH_PUBSUB_CPULOAD @@ -1062,6 +1049,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REPLY select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REQUEST select ZENOH_PUBSUB_ROVER_ACKERMANN_GUIDANCE_STATUS + select ZENOH_PUBSUB_ROVER_ACKERMANN_SETPOINT select ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS select ZENOH_PUBSUB_ROVER_DIFFERENTIAL_GUIDANCE_STATUS select ZENOH_PUBSUB_ROVER_DIFFERENTIAL_SETPOINT @@ -1099,7 +1087,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_TELEMETRY_STATUS select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS select ZENOH_PUBSUB_TIMESYNC_STATUS - select ZENOH_PUBSUB_TRAJECTORY_BEZIER select ZENOH_PUBSUB_TRAJECTORY_SETPOINT select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_TRANSPONDER_REPORT @@ -1134,8 +1121,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_VEHICLE_STATUS select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_VELOCITY_LIMITS select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS select ZENOH_PUBSUB_WHEEL_ENCODERS diff --git a/src/systemcmds/param/param.cpp b/src/systemcmds/param/param.cpp index f1f355514c..8194cfffa5 100644 --- a/src/systemcmds/param/param.cpp +++ b/src/systemcmds/param/param.cpp @@ -74,13 +74,6 @@ enum class COMPARE_ERROR_LEVEL { SILENT = 1, }; - -#ifdef __PX4_QURT -#define PARAM_PRINT PX4_INFO -#else -#define PARAM_PRINT PX4_INFO_RAW -#endif - static int do_save(const char *param_file_name); static int do_save_default(); static int do_load(const char *param_file_name); @@ -515,10 +508,10 @@ do_save_default() static int do_show(const char *search_string, bool only_changed) { - PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n"); + PX4_INFO_RAW("Symbols: x = used, + = saved, * = unsaved\n"); // also show unused params if we show non-default values only param_foreach(do_show_print, (char *)search_string, only_changed, !only_changed); - PARAM_PRINT("\n %u/%u parameters used.\n", param_count_used(), param_count()); + PX4_INFO_RAW("\n %u/%u parameters used.\n", param_count_used(), param_count()); return 0; } @@ -530,7 +523,7 @@ do_show_for_airframe() int32_t sys_autostart = 0; param_get(param_find("SYS_AUTOSTART"), &sys_autostart); if (sys_autostart != 0) { - PARAM_PRINT("# Make sure to add all params from the current airframe (ID=%" PRId32 ") as well\n", sys_autostart); + PX4_INFO_RAW("# Make sure to add all params from the current airframe (ID=%" PRId32 ") as well\n", sys_autostart); } return 0; } @@ -538,9 +531,9 @@ do_show_for_airframe() static int do_show_all() { - PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n"); + PX4_INFO_RAW("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, nullptr, false, false); - PARAM_PRINT("\n %u parameters total, %u used.\n", param_count(), param_count_used()); + PX4_INFO_RAW("\n %u parameters total, %u used.\n", param_count(), param_count_used()); return 0; } @@ -560,14 +553,14 @@ do_show_quiet(const char *param_name) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &ii)) { - PARAM_PRINT("%ld", (long)ii); + PX4_INFO_RAW("%ld\n", (long)ii); } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &ff)) { - PARAM_PRINT("%4.4f", (double)ff); + PX4_INFO_RAW("%4.4f\n", (double)ff); } break; @@ -589,7 +582,7 @@ do_find(const char *name) return 1; } - PARAM_PRINT("Found param %s at index %i\n", name, (int)ret); + PX4_INFO_RAW("Found param %s at index %i\n", name, (int)ret); return 0; } @@ -614,27 +607,27 @@ do_show_index(const char *index, bool used_index) return 1; } - PARAM_PRINT("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), + PX4_INFO_RAW("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param), param_get_used_index(param), param_get_index(param)); switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &ii)) { - PARAM_PRINT("%ld\n", (long)ii); + PX4_INFO_RAW("%ld\n", (long)ii); } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &ff)) { - PARAM_PRINT("%4.4f\n", (double)ff); + PX4_INFO_RAW("%4.4f\n", (double)ff); } break; default: - PARAM_PRINT("\n", 0 + param_type(param)); + PX4_INFO_RAW("\n", 0 + param_type(param)); } return 0; @@ -684,7 +677,7 @@ do_show_print(void *arg, param_t param) } } - PARAM_PRINT("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '), + PX4_INFO_RAW("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param), param_get_used_index(param), param_get_index(param)); @@ -695,7 +688,7 @@ do_show_print(void *arg, param_t param) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - PARAM_PRINT("%ld\n", (long)i); + PX4_INFO_RAW("%ld\n", (long)i); return; } @@ -703,18 +696,18 @@ do_show_print(void *arg, param_t param) case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { - PARAM_PRINT("%4.4f\n", (double)f); + PX4_INFO_RAW("%4.4f\n", (double)f); return; } break; default: - PARAM_PRINT("\n", 0 + param_type(param)); + PX4_INFO_RAW("\n", 0 + param_type(param)); return; } - PARAM_PRINT("\n", (unsigned long)param); + PX4_INFO_RAW("\n", (unsigned long)param); } static void @@ -739,12 +732,12 @@ do_show_print_for_airframe(void *arg, param_t param) int32_t i; float f; - PARAM_PRINT("param set-default %s ", p_name); + PX4_INFO_RAW("param set-default %s ", p_name); switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - PARAM_PRINT("%ld\n", (long)i); + PX4_INFO_RAW("%ld\n", (long)i); return; } @@ -752,7 +745,7 @@ do_show_print_for_airframe(void *arg, param_t param) case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { - PARAM_PRINT("%4.4f\n", (double)f); + PX4_INFO_RAW("%4.4f\n", (double)f); return; } @@ -762,7 +755,7 @@ do_show_print_for_airframe(void *arg, param_t param) return; } - PARAM_PRINT("\n", (unsigned long)param); + PX4_INFO_RAW("\n", (unsigned long)param); } static int @@ -792,12 +785,12 @@ do_set(const char *name, const char *val, bool fail_on_not_found) int32_t newval = strtol(val, &end, 10); if (i != newval) { - PARAM_PRINT("%c %s: ", + PX4_INFO_RAW("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); - PARAM_PRINT("curr: %ld", (long)i); + PX4_INFO_RAW("curr: %ld", (long)i); param_set(param, &newval); - PARAM_PRINT(" -> new: %ld\n", (long)newval); + PX4_INFO_RAW(" -> new: %ld\n", (long)newval); } } @@ -814,12 +807,12 @@ do_set(const char *name, const char *val, bool fail_on_not_found) if (f != newval) { #pragma GCC diagnostic pop - PARAM_PRINT("%c %s: ", + PX4_INFO_RAW("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); - PARAM_PRINT("curr: %4.4f", (double)f); + PX4_INFO_RAW("curr: %4.4f", (double)f); param_set(param, &newval); - PARAM_PRINT(" -> new: %4.4f\n", (double)newval); + PX4_INFO_RAW(" -> new: %4.4f\n", (double)newval); } } diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 72d99c6708..2beab87943 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -33,7 +33,6 @@ set(srcs test_atomic_bitset.cpp - test_bezierQuad.cpp test_bitset.cpp test_bson.cpp test_dataman.cpp diff --git a/src/systemcmds/tests/test_bezierQuad.cpp b/src/systemcmds/tests/test_bezierQuad.cpp deleted file mode 100644 index 9fc5f7f987..0000000000 --- a/src/systemcmds/tests/test_bezierQuad.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-2019 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. - * - ****************************************************************************/ - -/** - * @file test_bezierQuad.cpp - * Test for Bezier curve computation. - */ - -#include -#include -#include -#include - -#include "../../lib/bezier/BezierQuad.hpp" - -class BezierQuadTest : public UnitTest -{ -public: - virtual bool run_tests(); - -private: - - bool _get_states_from_time(); - bool _get_arc_length(); - bool _set_bez_from_vel(); - - float random(float min, float max); - -}; - - -bool BezierQuadTest::run_tests() -{ - ut_run_test(_get_states_from_time); - ut_run_test(_get_arc_length); - ut_run_test(_set_bez_from_vel); - - return (_tests_failed == 0); -} - -bool BezierQuadTest::_get_states_from_time() -{ - // symmetric around 0 - matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f); - matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f); - matrix::Vector3f pt1(0.5f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bezier::BezierQuad_f bz(pt0, ctrl, pt1); - - matrix::Vector3f pos, vel, acc; - float precision = 0.00001; - - // states at time = 0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 1", vel(1), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), -1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.50f); - - // pos must be equal to ctrl(0) and lower than ctrl(1) - ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision); - ut_assert_true(pos(1) < ctrl(1)); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal -2", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // acceleration - pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f); - ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f); - pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bz.setBezier(pt0, ctrl, pt1, 1.0f); - - // states at time = 0.0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 0", vel(0), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1.0 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 2", vel(0), 2.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.5f); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - return true; - -} - -bool BezierQuadTest::_get_arc_length() -{ - // create random numbers - srand(0); // choose a constant to make it deterministic - - float min = -50.f; - float max = 50.f; - float resolution = 0.1f; - - matrix::Vector3f pt0, pt1, ctrl; - float duration, arc_length, triangle_length, straigth_length; - float T = 100.0f; - - // loop trough different control points 100x and check if arc_length is in the expected range - for (int i = 0; i < 100 ; i++) { - // random bezier point - pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - - // use for each test a new duration - duration = random(0.0f, T); - - // create bezier - bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration); - - // compute arc length, triangle length and straigh length - arc_length = bz.getArcLength(resolution); - triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length(); - straigth_length = (pt1 - pt0).length(); - - // we also compute length from going point to point and add segment - float time_increment = duration / T; - float t = 0.0f + time_increment; - matrix::Vector3f p0 = pt0; - float sum_segments = 0.0f; - - for (int s = 0; s < (int)T; s++) { - matrix::Vector3f nextpt = bz.getPoint(t); - sum_segments = (nextpt - p0).length() + sum_segments; - p0 = bz.getPoint(t); - t = t + time_increment; - } - - // test comparisons - ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length) - && (fabsf(arc_length - sum_segments) < 1.f)); - } - - - return true; -} - -bool BezierQuadTest::_set_bez_from_vel() -{ - // create random numbers - srand(100); // choose a constant to make it deterministic - - float low = -50.0f; - float max = 50.0f; - float precision = 0.001f; - - for (int i = 0; i < 20; i++) { - - // set velocity - matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max)); - float duration = random(0.0f, 100.0f); - - bezier::BezierQuad_f bz;; - bz.setBezFromVel(ctrl, vel0, vel1, duration); - - // get velocity back - matrix::Vector3f v0 = bz.getVelocity(0.0f); - matrix::Vector3f v1 = bz.getVelocity(duration); - ut_compare_float("", vel0(0), v0(0), precision); - ut_compare_float("", vel1(0), v1(0), precision); - - ut_compare_float("", vel0(1), v0(1), precision); - ut_compare_float("", vel1(1), v1(1), precision); - - ut_compare_float("", vel0(2), v0(2), precision); - ut_compare_float("", vel1(2), v1(2), precision); - } - - return true; -} - -float BezierQuadTest::random(float min, float max) -{ - float s = rand() / (float)RAND_MAX; - return (min + s * (max - min)); - -} - -ut_declare_test_c(test_bezierQuad, BezierQuadTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index af25fd074e..3d04ad31ef 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -78,7 +78,6 @@ const struct { #endif /* __PX4_NUTTX */ {"atomic_bitset", test_atomic_bitset, 0}, - {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 0e5a14743c..c42bce07fc 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -44,7 +44,6 @@ __BEGIN_DECLS extern int test_atomic_bitset(int argc, char *argv[]); -extern int test_bezierQuad(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]); diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index b4db038c12..28244869f9 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -139,6 +139,13 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, if (fds[1].revents & POLLIN) { msgs_received++; + if (num_msgs > 1) { + // Clear screen + dprintf(1, "\033[2J\n"); + // Move cursor to home position + dprintf(1, "\033[H"); + } + PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received); int ret = listener_print_topic(id, sub); diff --git a/test/mavros_posix_test_avoidance.test b/test/mavros_posix_test_avoidance.test deleted file mode 100644 index 5c5adeaa4d..0000000000 --- a/test/mavros_posix_test_avoidance.test +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - $(arg pointcloud_topics) - - - - diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index f3ba47f871..a39ca1f3c9 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -96,7 +96,7 @@ void AutopilotTester::wait_until_ready() // Wait until we can arm CHECK(poll_condition_with_timeout( - [this]() { return _telemetry->health().is_armable; }, std::chrono::seconds(20))); + [this]() { return _telemetry->health().is_armable; }, std::chrono::seconds(45))); } void AutopilotTester::store_home() @@ -452,8 +452,29 @@ void AutopilotTester::fly_forward_in_posctl() } CHECK(_manual_control->start_position_control() == ManualControl::Result::Success); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + store_home(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + wait_until_ready(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + arm(); // Climb up for 5 seconds @@ -490,10 +511,37 @@ void AutopilotTester::fly_forward_in_altctl() } CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + store_home(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + wait_until_ready(); + + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + arm(); + // Send something to make sure RC is available. + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { + CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); + } + // Climb up for 5 seconds for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); diff --git a/test/mavsdk_tests/process_helper.py b/test/mavsdk_tests/process_helper.py index 5c8701e39d..ff65e77434 100644 --- a/test/mavsdk_tests/process_helper.py +++ b/test/mavsdk_tests/process_helper.py @@ -155,10 +155,12 @@ class Px4Runner(Runner): debugger: str, verbose: bool, build_dir: str): super().__init__(log_dir, model, case, verbose) self.name = "px4" - self.cmd = os.path.join(workspace_dir, build_dir, "bin/px4") self.cwd = os.path.join(workspace_dir, build_dir, "tmp_mavsdk_tests/rootfs") + self.cmd = "nice" self.args = [ + "--20", + os.path.join(workspace_dir, build_dir, "bin/px4"), os.path.join(workspace_dir, build_dir, "etc"), "-s", "etc/init.d-posix/rcS", @@ -329,8 +331,10 @@ class GzclientRunner(Runner): self.env = dict(os.environ, **{ "GAZEBO_MODEL_PATH": os.path.join(workspace_dir, PX4_GAZEBO_MODELS)}) - self.cmd = "gzclient" - self.args = ["--verbose"] + self.cmd = "nice" + self.args = ["--19", + "gzclient", + "--verbose"] class TestRunner(Runner): @@ -347,7 +351,7 @@ class TestRunner(Runner): self.name = "mavsdk_tests" self.cwd = workspace_dir self.cmd = "nice" - self.args = ["-5", + self.args = ["--18", os.path.join( workspace_dir, build_dir, diff --git a/test/mavsdk_tests/vtol_mission_straight_south.plan b/test/mavsdk_tests/vtol_mission_straight_south.plan index fa6d37472b..1507af13f5 100644 --- a/test/mavsdk_tests/vtol_mission_straight_south.plan +++ b/test/mavsdk_tests/vtol_mission_straight_south.plan @@ -41,7 +41,7 @@ "AltitudeMode": 1, "MISSION_ITEM_ID": "2", "autoContinue": true, - "command": 21, + "command": 85, "doJumpId": 2, "frame": 3, "params": [ diff --git a/test/rostest_avoidance_run.sh b/test/rostest_avoidance_run.sh deleted file mode 100755 index 2fc2be7eb6..0000000000 --- a/test/rostest_avoidance_run.sh +++ /dev/null @@ -1,20 +0,0 @@ -#! /bin/bash - -DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -PX4_SRC_DIR=${DIR}/.. - -source /opt/ros/${ROS_DISTRO:-kinetic}/setup.bash -mkdir -p ${PX4_SRC_DIR}/catkin_ws/src -cd ${PX4_SRC_DIR}/catkin_ws/ -git clone -b 0.3.1 --single-branch --depth 1 https://github.com/PX4/avoidance.git src/avoidance - -catkin init -catkin build local_planner safe_landing_planner --cmake-args -DCMAKE_BUILD_TYPE=Release - -source ${PX4_SRC_DIR}/catkin_ws/devel/setup.bash -source /usr/share/gazebo/setup.sh - -export CATKIN_SETUP_UTIL_ARGS=--extend -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${PX4_SRC_DIR}/catkin_ws/src/avoidance/avoidance/sim/models - -source $DIR/rostest_px4_run.sh "$@"